CN115265551A - 一种无人驾驶复杂场景下的多传感器融合定位方法和系统 - Google Patents

一种无人驾驶复杂场景下的多传感器融合定位方法和系统 Download PDF

Info

Publication number
CN115265551A
CN115265551A CN202110993243.XA CN202110993243A CN115265551A CN 115265551 A CN115265551 A CN 115265551A CN 202110993243 A CN202110993243 A CN 202110993243A CN 115265551 A CN115265551 A CN 115265551A
Authority
CN
China
Prior art keywords
positioning
fault detection
positioning information
navigation satellite
obtained based
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202110993243.XA
Other languages
English (en)
Inventor
魏超
张婷
钟思维
余杰
朱耿霆
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Institute of Technology BIT
Advanced Technology Research Institute of Beijing Institute of Technology
Original Assignee
Beijing Institute of Technology BIT
Advanced Technology Research Institute of Beijing Institute of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Institute of Technology BIT, Advanced Technology Research Institute of Beijing Institute of Technology filed Critical Beijing Institute of Technology BIT
Priority to CN202110993243.XA priority Critical patent/CN115265551A/zh
Publication of CN115265551A publication Critical patent/CN115265551A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3407—Route searching; Route guidance specially adapted for specific applications
    • G01C21/343—Calculating itineraries, i.e. routes leading from a starting point to a series of categorical destinations using a global route restraint, round trips, touristic trips
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/485—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an optical system or imaging system
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明涉及一种无人驾驶复杂场景下的多传感器融合定位方法和系统。该多传感器融合定位方法，通过采用因子图优化方法，即采用最大后验概率估计将由定位信息构建的因子图模型转化为非线性问题后，对非线性问题进行求解得到无人车定位结果的方式，对传感器定位模型的观测数据进行融合，能够在增强无人驾驶复杂场景下多传感器融合定位鲁棒性的同时，有效提高无人驾驶车辆定位精度和速度估计精度。

Description

一种无人驾驶复杂场景下的多传感器融合定位方法和系统
技术领域
本发明涉及无人驾驶定位领域，特别是涉及一种无人驾驶复杂场景下的多传感器融合定位方法和系统。
背景技术
随着经济社会的快速发展，全世界的汽车保有量逐年增加，人们对交通安全和出行舒适性提出了更高的要求。无人驾驶是在传统车辆的基础上赋予其生命和智慧，让其能向人类一样智能识别路况，安全自主的行驶到目的地，给驾乘人员带来安全和舒心，车辆的智能化、网联化发展也是国际汽车工业发展的新动力，也是经济社会发展的新驱动力。
车辆的自主定位是实现无人驾驶的一个重要环节，对于无人驾驶的环境感知、动态规划、智能决策和运动控制环节都有着重要作用。只有车辆精确确定自身位置、姿态和运动状态之后，才能实现一系列的无人驾驶功能。但在无人驾驶的复杂场景下，传感器受到环境干扰，自身精度降低，且传感器定位模型输出的定位信息误差也会增大，导致无人车定位系统难以长期输出高精度的定位信息、姿态信息和速度信息。
现有技术中，无人驾驶多传感器融合定位方法多以滤波方法进行多传感器融合。如中国专利公布号为CN110631593A，公布日为2019.12.31，公开了一种用于自动驾驶场景的多传感器融合定位方法，该方法采用低成本传感器结合矢量地图通过改进的粒子滤波算法实现车道级定位，能保证定位精度，同时还能输出频率可调的高频定位信息，其能为环境感知和车身控制提供基准数据；又如中国专利公布号为CN108692701A，公布日为2018.10.23，公开了一种基于粒子滤波器的移动机器人多传感器融合定位方法，该方法使用每个传感器的观察模型和当前时刻传感器测量数据更新粒子滤波器中粒子的权值，最后以权值最大的粒子的位姿作为机器人当前时刻的定位数据，大大提高了定位的准确性；又如中国专利公布号为CN107289948A，公布日为2017.10.24，公开了一种基于多传感器融合的无人机导航系统及方法，该发明公开了一种基于多传感器融合的无人机导航系统，还公开了相应的基于多传感器融合的无人机导航方法，利用扩展卡尔曼滤波融合相应模块获取的数据信息获得无人机当前姿态和位置，适用于无人机精确导航。但上述方法均使用滤波的方法进行多传感器融合，也未考虑在无人驾驶复杂场景下多传感器定位系统传感器故障和传感器定位误差问题，在无人驾驶真实工况下由于环境干扰，基于滤波的多传感器融合定位方法的定位精度、姿态估计精度有限，且鲁棒性较差。
因此，本领域亟需提供一种具有强鲁棒性的无人驾驶复杂场景下的多传感器融合定位方法，以有效提高无人驾驶车辆定位精度和速度估计精度。
发明内容
本发明的目的是提供一种具有强鲁棒性的无人驾驶复杂场景下的多传感器融合定位方法，能够有效提高无人驾驶车辆定位精度和速度估计精度。
为实现上述目的，本发明提供了如下方案：
一种无人驾驶复杂场景下的多传感器融合定位方法，包括：
根据定位信息构建因子图模型；所述定位信息包括：基于惯性测量单元预积分模型得到的定位信息、基于全球导航卫星系统定位模型得到的定位信息和/或基于激光雷达里程计得到的定位信息；
利用最大后验概率估计将所述因子图模型转化为非线性问题；
求解所述非线性问题得到无人车的定位结果；所述定位结果包括：定位信息、姿态信息和速度信息。
优选地，所述根据定位信息构建因子图模型，具体包括：
采用故障检测方法获取故障检测指标；
根据所述故障检测指标判断全球导航卫星系统是否发生故障，得到判断结果；
当所述判断结果为全球导航卫星系统发生故障时，根据所述基于惯性测量单元预积分模型得到的定位信息和所述基于激光雷达里程计得到的定位信息构建因子图模型；
当所述判断结果为全球导航卫星系统未发生故障时，根据所述基于惯性测量单元预积分模型得到的定位信息、所述基于全球导航卫星系统定位模型得到的定位信息和所述基于激光雷达里程计得到的定位信息构建因子图模型。
优选地，所述采用故障检测方法获取故障检测指标，具体包括：
利用观测数据的残差序列和观测数据的残差协方差矩阵构造故障检测函数；所述观测数据的残差序列和所述观测数据的残差协方差矩阵均基于无人车的预设观测数据确定；
对所述故障检测函数进行归一化处理，得到归一化处理结果；
对所述归一化处理结果进行加权处理得到所述故障检测指标。
优选地，所述根据所述故障检测指标判断全球导航卫星系统是否发生故障，得到判断结果，具体包括：
当所述故障检测指标小于1时，判定所述全球导航卫星系统的工作正常；
当所述故障检测指标大于等于1时，判定所述全球导航卫星系统发生故障，此时，所述故障检测指标置为1。
优选地，还包括：
利用上一时刻激光雷达里程计的噪声均值和噪声协方差矩阵修正当前时刻的激光雷达里程计的观测噪声及观测噪声的误差协方差矩阵；
利用自适应滤波确定修正后的激光雷达里程计的观测噪声的均值和观测噪声的协方差矩阵。
根据本发明提供的具体实施例，本发明公开了以下技术效果：
本发明提供的无人驾驶复杂场景下的多传感器融合定位方法，通过采用因子图优化方法，即采用最大后验概率估计将由定位信息构建的因子图模型转化为非线性问题后，对非线性问题进行求解得到无人车定位结果的方式，对传感器定位模型的观测数据进行融合，能够在增强无人驾驶复杂场景下多传感器融合定位鲁棒性的同时，有效提高无人驾驶车辆定位精度和速度估计精度。
此外，对应于上述提供的无人驾驶复杂场景下的多传感器融合定位方法，本发明还提供了以下实施系统：
一种无人驾驶复杂场景下的多传感器融合定位系统，包括：
因子图模型构建模块，用于根据定位信息构建因子图模型；所述定位信息包括：基于惯性测量单元预积分模型得到的定位信息、基于全球导航卫星系统定位模型得到的定位信息和/或基于激光雷达里程计得到的定位信息；
非线性问题转化模块，用于利用最大后验概率估计将所述因子图模型转化为非线性问题；
定位结果确定模块，用于求解所述非线性问题得到无人车的定位结果；所述定位结果包括：定位信息、姿态信息和速度信息。
优选地，所述因子图模型构建模块包括：
故障检测指标获取单元，用于采用故障检测方法获取故障检测指标；
判断单元，用于根据所述故障检测指标判断全球导航卫星系统是否发生故障，得到判断结果；
第一因子图模型构建单元，用于当所述判断结果为全球导航卫星系统发生故障时，根据所述基于惯性测量单元预积分模型得到的定位信息和所述基于激光雷达里程计得到的定位信息构建因子图模型；
第二因子图模型构建单元，用于当所述判断结果为全球导航卫星系统未发生故障时，根据所述基于惯性测量单元预积分模型得到的定位信息、所述基于全球导航卫星系统定位模型得到的定位信息和所述基于激光雷达里程计得到的定位信息构建因子图模型。
优选地，所述故障检测指标获取单元包括：
故障检测函数构建子单元，用于利用观测数据的残差序列和观测数据的残差协方差矩阵构造故障检测函数；所述观测数据的残差序列和所述观测数据的残差协方差矩阵均基于无人车的预设观测数据确定；
归一化处理子单元，用于对所述故障检测函数进行归一化处理，得到归一化处理结果；
故障检测指标确定子单元，用于对所述归一化处理结果进行加权处理得到所述故障检测指标。
优选地，所述判断单元包括：
第一判断子单元，用于当所述故障检测指标小于1时，判定所述全球导航卫星系统的工作正常；
第二判断子单元，用于当所述故障检测指标大于等于1时，判定所述全球导航卫星系统发生故障，此时，所述故障检测指标置为1。
优选地，还包括：
修正模块，用于利用上一时刻激光雷达里程计的噪声均值和噪声协方差矩阵修正当前时刻的激光雷达里程计的观测噪声及观测噪声的误差协方差矩阵；
噪声均值-协方差矩阵确定模块，用于利用自适应滤波确定修正后的激光雷达里程计的观测噪声的均值和观测噪声的协方差矩阵。
因本发明提供的无人驾驶复杂场景下的多传感器融合定位系统实现的技术效果与上述提供的无人驾驶复杂场景下的多传感器融合定位方法实现的技术效果相同，故在此不再进行赘述。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明提供的无人驾驶复杂场景下的多传感器融合定位方法的流程图；
图2为本发明实施例提供的局部因子图；
图3为本发明实施例提供的激光雷达里程计误差修正流程图；
图4为本发明实施例提供的无人驾驶复杂场景下的多传感器融合定位方法的整体工作流程框架图；
图5为本发明提供的无人驾驶复杂场景下的多传感器融合定位系统的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的目的是提供一种具有强鲁棒性的无人驾驶复杂场景下的多传感器融合定位方法，能够有效提高无人驾驶车辆定位精度。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
如图1所示，本发明提供的无人驾驶复杂场景下的多传感器融合定位方法，包括：
步骤100：根据定位信息构建因子图模型。定位信息包括：基于惯性测量单元预积分模型得到的定位信息、基于全球导航卫星系统定位模型得到的定位信息和/或基于激光雷达里程计得到的定位信息。例如，利用三种传感器(惯性测量单元、全球导航卫星系统和激光雷达里程计)获取的定位信息搭建相应的因子图，如图2所示，该因子图包括因子节点和状态变量节点。其中，状态变量节点包括无人车位姿、运动速度和惯性测量单元的零偏，表示为[p,v,b]T。图2中只考虑了四个时刻的状态变量节点和因子节点，其中imu因子节点是惯性测量单元子定位系统及其误差模型转换得到的二元因子、gnss_p因子节点是全球导航卫星系统子定位系统及其误差模型转换得到的一元因子，gnss_v因子节点是全球导航卫星系统输出的速度信息转换得到的一元因子，lo因子节点是激光雷达子定位系统及其误差模型转换得到的一元因子，bias因子节点是惯性测量单元零偏和零偏噪声转换得到的二元因子。
步骤101：利用最大后验概率估计将因子图模型转化为非线性问题。例如，采用公式
Figure BDA0003231119290000071
得到最大后验概率的全局函数XMAP，通过公式
Figure BDA0003231119290000072
得到传感器观测数据zi与对应状态变量Xi的非标准后验概率
Figure BDA0003231119290000073
将公式
Figure BDA0003231119290000074
带入公式
Figure BDA0003231119290000075
中并取负对数得到非线性最小二乘问题。该非线性最小二乘问题为：
Figure BDA0003231119290000076
其中，hi(Xi)为状态变量Xi和传感器观测量zi之间的观测函数，
Figure BDA0003231119290000077
为观测量zi的平方马氏范数。
步骤102：求解非线性问题得到无人车的定位结果。定位结果包括：定位信息、姿态信息和速度信息。
基于上述过程，本发明可以将因子图优化过程输出的无人车位姿信息及其误差协方差矩阵作为自适应滤波噪声估计和故障检测的数据基础，以用于当前帧激光雷达里程计噪声估计和下一帧全球导航卫星系统的故障检测。基于此，在采用定位信息进行因子图模型的构建过程中，还需要对全球导航卫星系统的故障进行检测，这一检测过程包括：
步骤1000：采用故障检测方法获取故障检测指标。该步骤的实施过程优选为：
①利用观测数据的残差序列和观测数据的残差协方差矩阵构造故障检测函数。观测数据的残差序列和观测数据的残差协方差矩阵均基于无人车的预设观测数据确定。其中，构造得到的故障检测函数服从自由度为3的卡方分布为：
ek＝Zk-HkXk,k-1
Figure BDA0003231119290000081
Figure BDA0003231119290000082
式中，Zk为全球导航卫星系统定位模型的观测值。Hk为观测矩阵。ek为预测残差序列。Xk,k-1为当前时刻的状态预测值。Pk,k-1为预测协方差矩阵。Rk为量测噪声协方差矩阵。Pe,k为残差序列的协方差矩阵。
②对故障检测函数进行归一化处理，得到归一化处理结果。其具体实施过程为：设置TD1和TD2两级阈值，进行分级故障检测，TD1为根据χ2检验法中的显著水平即误警率确定的阈值。当故障检测指标满足λk＜TD1时，可以将全球导航卫星系统系统状态判断为无故障。当λk≥TD1时，为了降低误警现象对多传感器融合定位系统的影响，设置了二级故障检测阈值TD2。在二级故障检测中，当λk＞TD2时，直接判定全球导航卫星系统出现了故障。当TD1≤λk≤TD2时，故障检测法出现误警的概率较大，将λk映射到区间[0,1]，进行归一化处理得到
Figure BDA0003231119290000083
Figure BDA0003231119290000084
③对归一化处理结果进行加权处理得到故障检测指标。考虑到更加快速的识别全球导航卫星系统故障，对当前待检测目标值和上一次的检测目标值进行加权处理，只有当连续的故障检测指标λk小于解除故障阈值时才能判定全球导航卫星系统故障解除。加权处理方法如下式所示：
Figure BDA0003231119290000085
其中，α1和α2为加权系数，代表当前检测指标和前一时刻的检测指标在故障检测函数中所占的比重。
步骤1001：根据故障检测指标判断全球导航卫星系统是否发生故障，得到判断结果。
步骤1002：当判断结果为全球导航卫星系统发生故障时，根据基于惯性测量单元预积分模型得到的定位信息和基于激光雷达里程计得到的定位信息构建因子图模型。
步骤1003：当判断结果为全球导航卫星系统未发生故障时，根据基于惯性测量单元预积分模型得到的定位信息、基于全球导航卫星系统定位模型得到的定位信息和基于激光雷达里程计得到的定位信息构建因子图模型。
例如，当故障检测指标小于1时，判定全球导航卫星系统的工作正常。当故障检测指标大于等于1时，判定全球导航卫星系统发生故障，此时，故障检测指标置为1。在全球导航卫星系统子系统判定为故障后，只有当检验值小于解除故障阈值ε时，才能解除故障报警，即λk＜ε。
进一步，为了提高鲁棒性，本发明上述提供的无人驾驶复杂场景下的多传感器融合定位方法，还包括：
利用上一时刻激光雷达里程计的噪声均值和噪声协方差矩阵修正当前时刻的激光雷达里程计的观测噪声及观测噪声的误差协方差矩阵。
利用自适应滤波确定修正后的激光雷达里程计的观测噪声的均值和观测噪声的协方差矩阵。
在本发明中，主要是运用自适应滤波方法对多传感器定位系统中的激光雷达子定位系统的观测值进行噪声估计，自适应滤波噪声估计方法会输出当前时刻激光雷达子定位系统观测值的噪声均值和噪声协方差，其中噪声均值反映了激光雷达里程计的误差。如图3所示，自适应滤波噪声估计流程为：
①利用当前时刻的激光雷达子定位系统的观测值zk和因子图优化输出的当前时刻无人车位姿信息
Figure BDA0003231119290000101
计算激光雷达子定位系统的噪声均值。
②构建激光雷达子定位系统观测值的残差ek。
③利用残差ek和因子图优化输出的当前时刻无人车位姿的误差协方差矩阵
Figure BDA0003231119290000102
计算激光雷达子定位系统的噪声协方差矩阵。
其中，噪声估计过程采用的公式为：
Figure BDA0003231119290000103
Figure BDA0003231119290000104
Figure BDA0003231119290000105
式中，ek激光雷达里程计观测值的残差。Hk为激光雷达里程计的量测矩阵。
Figure BDA0003231119290000106
为当前时刻的无人车位姿估计结果。
Figure BDA0003231119290000107
为当前时刻的无人车位姿估计的误差协方差矩阵。
Figure BDA0003231119290000108
为前一时刻激光雷达里程计观测噪声均值的估计结果。
Figure BDA0003231119290000109
为前一时刻激光雷达里程计观测噪声协方差矩阵的估计结果。
综上，本发明提供的无人驾驶复杂场景下的多传感器融合定位方法的整体工作流程框架如图4所示。
此外，对应于上述提供的无人驾驶复杂场景下的多传感器融合定位方法，本发明还提供了一种无人驾驶复杂场景下的多传感器融合定位系统，如图5所示，该多传感器融合定位系统包括：因子图模型构建模块1、非线性问题转化模块2和定位结果确定模块3。
因子图模型构建模块1用于根据定位信息构建因子图模型。定位信息包括：基于惯性测量单元预积分模型得到的定位信息、基于全球导航卫星系统定位模型得到的定位信息和/或基于激光雷达里程计得到的定位信息。
非线性问题转化模块2用于利用最大后验概率估计将因子图模型转化为非线性问题。
定位结果确定模块3用于求解非线性问题得到无人车的定位结果。定位结果包括：定位信息、姿态信息和速度信息。
进一步，为了提高无人车定位的准确性，本发明上述提供的因子图模型构建模块1还可以设置有：故障检测指标获取单元、判断单元、第一因子图模型构建单元和第二因子图模型构建单元。
故障检测指标获取单元用于采用故障检测方法获取故障检测指标。
判断单元用于根据故障检测指标判断全球导航卫星系统是否发生故障，得到判断结果。
第一因子图模型构建单元用于当判断结果为全球导航卫星系统发生故障时，根据基于惯性测量单元预积分模型得到的定位信息和基于激光雷达里程计得到的定位信息构建因子图模型。
第二因子图模型构建单元用于当判断结果为全球导航卫星系统未发生故障时，根据基于惯性测量单元预积分模型得到的定位信息、基于全球导航卫星系统定位模型得到的定位信息和基于激光雷达里程计得到的定位信息构建因子图模型。
其中，为了提高鲁棒性，上述采用的故障检测指标获取单元可以进一步包括：故障检测函数构建子单元、归一化处理子单元和故障检测指标确定子单元。
故障检测函数构建子单元用于利用观测数据的残差序列和观测数据的残差协方差矩阵构造故障检测函数。观测数据的残差序列和观测数据的残差协方差矩阵均基于无人车的预设观测数据确定。
归一化处理子单元用于对故障检测函数进行归一化处理，得到归一化处理结果。
故障检测指标确定子单元用于对归一化处理结果进行加权处理得到故障检测指标。
进一步，也是为了提高鲁棒性，本发明上述采用的判断单元可以包括：第一判断子单元和第二判断子单元。
其中，第一判断子单元用于当故障检测指标小于1时，判定全球导航卫星系统的工作正常。
第二判断子单元用于当故障检测指标大于等于1时，判定全球导航卫星系统发生故障，此时，故障检测指标置为1。
进一步，为了在提高鲁棒性的同时，提高定位准确性，本发明上述提供的无人驾驶复杂场景下的多传感器融合定位系统还可以包括：修正模块和噪声均值-协方差矩阵确定模块。
其中，修正模块用于利用上一时刻激光雷达里程计的噪声均值和噪声协方差矩阵修正当前时刻的激光雷达里程计的观测噪声及观测噪声的误差协方差矩阵。
噪声均值-协方差矩阵确定模块用于利用自适应滤波确定修正后的激光雷达里程计的观测噪声的均值和观测噪声的协方差矩阵。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种无人驾驶复杂场景下的多传感器融合定位方法，其特征在于，包括：
根据定位信息构建因子图模型；所述定位信息包括：基于惯性测量单元预积分模型得到的定位信息、基于全球导航卫星系统定位模型得到的定位信息和/或基于激光雷达里程计得到的定位信息；
利用最大后验概率估计将所述因子图模型转化为非线性问题；
求解所述非线性问题得到无人车的定位结果；所述定位结果包括：定位信息、姿态信息和速度信息。
2.根据权利要求1所述的无人驾驶复杂场景下的多传感器融合定位方法，其特征在于，所述根据定位信息构建因子图模型，具体包括：
采用故障检测方法获取故障检测指标；
根据所述故障检测指标判断全球导航卫星系统是否发生故障，得到判断结果；
当所述判断结果为全球导航卫星系统发生故障时，根据所述基于惯性测量单元预积分模型得到的定位信息和所述基于激光雷达里程计得到的定位信息构建因子图模型；
当所述判断结果为全球导航卫星系统未发生故障时，根据所述基于惯性测量单元预积分模型得到的定位信息、所述基于全球导航卫星系统定位模型得到的定位信息和所述基于激光雷达里程计得到的定位信息构建因子图模型。
3.根据权利要求2所述的无人驾驶复杂场景下的多传感器融合定位方法，其特征在于，所述采用故障检测方法获取故障检测指标，具体包括：
利用观测数据的残差序列和观测数据的残差协方差矩阵构造故障检测函数；所述观测数据的残差序列和所述观测数据的残差协方差矩阵均基于无人车的预设观测数据确定；
对所述故障检测函数进行归一化处理，得到归一化处理结果；
对所述归一化处理结果进行加权处理得到所述故障检测指标。
4.根据权利要求3所述的无人驾驶复杂场景下的多传感器融合定位方法，其特征在于，所述根据所述故障检测指标判断全球导航卫星系统是否发生故障，得到判断结果，具体包括：
当所述故障检测指标小于1时，判定所述全球导航卫星系统的工作正常；
当所述故障检测指标大于等于1时，判定所述全球导航卫星系统发生故障，此时，所述故障检测指标置为1。
5.根据权利要求2所述的无人驾驶复杂场景下的多传感器融合定位方法，其特征在于，还包括：
利用上一时刻激光雷达里程计的噪声均值和噪声协方差矩阵修正当前时刻的激光雷达里程计的观测噪声及观测噪声的误差协方差矩阵；
利用自适应滤波确定修正后的激光雷达里程计的观测噪声的均值和观测噪声的协方差矩阵。
6.一种无人驾驶复杂场景下的多传感器融合定位系统，其特征在于，包括：
因子图模型构建模块，用于根据定位信息构建因子图模型；所述定位信息包括：基于惯性测量单元预积分模型得到的定位信息、基于全球导航卫星系统定位模型得到的定位信息和/或基于激光雷达里程计得到的定位信息；
非线性问题转化模块，用于利用最大后验概率估计将所述因子图模型转化为非线性问题；
定位结果确定模块，用于求解所述非线性问题得到无人车的定位结果；所述定位结果包括：定位信息、姿态信息和速度信息。
7.根据权利要求6所述的无人驾驶复杂场景下的多传感器融合定位系统，其特征在于，所述因子图模型构建模块包括：
故障检测指标获取单元，用于采用故障检测方法获取故障检测指标；
判断单元，用于根据所述故障检测指标判断全球导航卫星系统是否发生故障，得到判断结果；
第一因子图模型构建单元，用于当所述判断结果为全球导航卫星系统发生故障时，根据所述基于惯性测量单元预积分模型得到的定位信息和所述基于激光雷达里程计得到的定位信息构建因子图模型；
第二因子图模型构建单元，用于当所述判断结果为全球导航卫星系统未发生故障时，根据所述基于惯性测量单元预积分模型得到的定位信息、所述基于全球导航卫星系统定位模型得到的定位信息和所述基于激光雷达里程计得到的定位信息构建因子图模型。
8.根据权利要求7所述的无人驾驶复杂场景下的多传感器融合定位系统，其特征在于，所述故障检测指标获取单元包括：
故障检测函数构建子单元，用于利用观测数据的残差序列和观测数据的残差协方差矩阵构造故障检测函数；所述观测数据的残差序列和所述观测数据的残差协方差矩阵均基于无人车的预设观测数据确定；
归一化处理子单元，用于对所述故障检测函数进行归一化处理，得到归一化处理结果；
故障检测指标确定子单元，用于对所述归一化处理结果进行加权处理得到所述故障检测指标。
9.根据权利要求8所述的无人驾驶复杂场景下的多传感器融合定位方法，其特征在于，所述判断单元包括：
第一判断子单元，用于当所述故障检测指标小于1时，判定所述全球导航卫星系统的工作正常；
第二判断子单元，用于当所述故障检测指标大于等于1时，判定所述全球导航卫星系统发生故障，此时，所述故障检测指标置为1。
10.根据权利要求7所述的无人驾驶复杂场景下的多传感器融合定位系统，其特征在于，还包括：
修正模块，用于利用上一时刻激光雷达里程计的噪声均值和噪声协方差矩阵修正当前时刻的激光雷达里程计的观测噪声及观测噪声的误差协方差矩阵；
噪声均值-协方差矩阵确定模块，用于利用自适应滤波确定修正后的激光雷达里程计的观测噪声的均值和观测噪声的协方差矩阵。
CN202110993243.XA 2021-08-26 2021-08-26 一种无人驾驶复杂场景下的多传感器融合定位方法和系统 Pending CN115265551A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110993243.XA CN115265551A (zh) 2021-08-26 2021-08-26 一种无人驾驶复杂场景下的多传感器融合定位方法和系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110993243.XA CN115265551A (zh) 2021-08-26 2021-08-26 一种无人驾驶复杂场景下的多传感器融合定位方法和系统

Publications (1)

Publication Number Publication Date
CN115265551A true CN115265551A (zh) 2022-11-01

Family

ID=83758871

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110993243.XA Pending CN115265551A (zh) 2021-08-26 2021-08-26 一种无人驾驶复杂场景下的多传感器融合定位方法和系统

Country Status (1)

Country Link
CN (1) CN115265551A (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116222588A (zh) * 2023-05-08 2023-06-06 睿羿科技(山东)有限公司 一种gps与车载里程计融合的定位方法
CN117406259A (zh) * 2023-12-14 2024-01-16 江西北斗云智慧科技有限公司 一种基于北斗的智慧工地车辆定位方法及系统
CN117451043A (zh) * 2023-12-25 2024-01-26 武汉大学 一种数-模混合估计的多源融合定位方法及系统

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116222588A (zh) * 2023-05-08 2023-06-06 睿羿科技(山东)有限公司 一种gps与车载里程计融合的定位方法
CN116222588B (zh) * 2023-05-08 2023-08-04 睿羿科技(山东)有限公司 一种gps与车载里程计融合的定位方法
CN117406259A (zh) * 2023-12-14 2024-01-16 江西北斗云智慧科技有限公司 一种基于北斗的智慧工地车辆定位方法及系统
CN117406259B (zh) * 2023-12-14 2024-03-22 江西北斗云智慧科技有限公司 一种基于北斗的智慧工地车辆定位方法及系统
CN117451043A (zh) * 2023-12-25 2024-01-26 武汉大学 一种数-模混合估计的多源融合定位方法及系统
CN117451043B (zh) * 2023-12-25 2024-03-15 武汉大学 一种数-模混合估计的多源融合定位方法及系统

Similar Documents

Publication Publication Date Title
CN115265551A (zh) 一种无人驾驶复杂场景下的多传感器融合定位方法和系统
US10976410B1 (en) Generating data using radar observation model based on machine learning
CN113945206B (zh) 一种基于多传感器融合的定位方法及装置
CN112154455B (zh) 数据处理方法、设备和可移动平台
CN106840179A (zh) 一种基于多传感器信息融合的智能车定位方法
CN109975839B (zh) 一种车辆卫星定位数据的联合滤波优化方法
CN112505737B (zh) 一种gnss/ins组合导航方法
CN109752725A (zh) 一种低速商用机器人、定位导航方法及定位导航系统
CN108387236B (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN112180361B (zh) 一种基于动态联邦滤波的车载雷达目标跟踪方法
CN111750854A (zh) 车辆定位方法、装置、系统及存储介质
CN112987061B (zh) 一种基于gps和激光雷达模糊融合定位方法
JP7047576B2 (ja) 地図作成装置
CN112731371A (zh) 一种激光雷达与视觉融合的集成化目标跟踪系统及方法
CN111257853B (zh) 一种基于imu预积分的自动驾驶系统激光雷达在线标定方法
CN114200945A (zh) 一种移动机器人的安全控制方法
CN117260757A (zh) 一种基于巡检数据的机器人巡检系统
Parra-Tsunekawa et al. A kalman-filtering-based approach for improving terrain mapping in off-road autonomous vehicles
CN112229392B (zh) 一种高冗余的室内煤场导航方法及系统
CN113029173A (zh) 车辆导航方法及装置
CN105629953A (zh) 基于车辆辅助驾驶的杆体识别系统
CN113237482B (zh) 一种基于因子图的城市峡谷环境下车辆鲁棒定位方法
Wei et al. Multi-sensor environmental perception and adaptive cruise control of intelligent vehicles using kalman filter
CN115046546A (zh) 一种基于车道线识别的自动驾驶汽车定位系统及方法
CN111326002A (zh) 一种自动驾驶汽车环境感知的预测方法、装置及系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination