CN113970330B - 车载多传感器融合定位方法、计算机设备及存储介质 - Google Patents
车载多传感器融合定位方法、计算机设备及存储介质 Download PDFInfo
- Publication number
- CN113970330B CN113970330B CN202111579011.6A CN202111579011A CN113970330B CN 113970330 B CN113970330 B CN 113970330B CN 202111579011 A CN202111579011 A CN 202111579011A CN 113970330 B CN113970330 B CN 113970330B
- Authority
- CN
- China
- Prior art keywords
- data
- imu
- odo
- mileage
- laser radar
- 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.)
- Active
Links
- 230000004927 fusion Effects 0.000 title claims abstract description 115
- 238000000034 method Methods 0.000 title claims abstract description 69
- 238000005457 optimization Methods 0.000 claims abstract description 57
- 238000004364 calculation method Methods 0.000 claims abstract description 28
- 230000008569 process Effects 0.000 claims description 15
- 239000011159 matrix material Substances 0.000 claims description 14
- 238000004590 computer program Methods 0.000 claims description 13
- 230000006870 function Effects 0.000 claims description 12
- 238000000605 extraction Methods 0.000 claims description 6
- 230000004807 localization Effects 0.000 claims description 4
- 238000007499 fusion processing Methods 0.000 abstract description 5
- 230000002829 reductive effect Effects 0.000 abstract description 5
- 238000005295 random walk Methods 0.000 description 22
- 230000001133 acceleration Effects 0.000 description 15
- 238000003745 diagnosis Methods 0.000 description 14
- 238000004422 calculation algorithm Methods 0.000 description 13
- 238000010586 diagram Methods 0.000 description 10
- 230000010354 integration Effects 0.000 description 7
- 238000005259 measurement Methods 0.000 description 6
- 238000001514 detection method Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- 230000002401 inhibitory effect Effects 0.000 description 3
- 230000001360 synchronised effect Effects 0.000 description 3
- 239000006185 dispersion Substances 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 230000010365 information processing Effects 0.000 description 2
- 238000013508 migration Methods 0.000 description 2
- 238000012544 monitoring process Methods 0.000 description 2
- 238000013139 quantization Methods 0.000 description 2
- 238000010845 search algorithm Methods 0.000 description 2
- 238000012546 transfer Methods 0.000 description 2
- 230000000670 limiting effect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
- 239000013598 vector Substances 0.000 description 1
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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本申请提供一种车载多传感器融合定位方法、计算机设备及存储介质，车载多传感器融合定位方法包括获取IMU数据、ODO数据、激光雷达数据以及组合导航数据；对IMU里程数据和IMU&ODO融合里程数据进行迭代优化；根据优化后的IMU里程数据、优化后的IMU&ODO融合里程数据以及激光雷达数据获取激光雷达位姿数据；通过滑窗进行联合优化融合得到最终定位结果。本申请技术方案相对简单的坐标对齐之后直接单独使用积分里程噪声更小，通过修正后的IMU数据在后续积分里程精度可以得到保证，保证了滑窗约束优化融合过程初始节点位姿精度，降低了优化迭代周期，提高了计算速度，同时提高了计算精度，系统稳定性高，提升了车辆的安全性。
Description
技术领域
本发明涉及车载定位技术领域，尤其涉及一种车载多传感器融合定位方法、计算机设备及存储介质。
背景技术
目前，关于轨道车辆中的定位技术，通过sil4安全完整性认证的定位技术主要依赖于轨旁设备，例如应答器、计轴等设备系统，为了减少轨旁设备，通常使用基于GNSS和轮速融合定位方法，该定位方法将多个传感器的信息进行融合定位，融合方式为采用卡尔曼滤波、粒子滤波等技术进行融合计算。这种融合方式很难提高定位的精度，同时定位系统的稳定性差，导致不能完全发挥各个传感器的优势。
发明内容
本发明实施例提供一种车载多传感器融合定位方法、计算机设备及存储介质，以解决现有技术采用的融合定位方法很难提高定位的精度以及定位系统的稳定性差的问题。
本申请第一方面提供一种车载多传感器融合定位方法，包括：
获取IMU数据、ODO数据、激光雷达数据以及组合导航数据，并将所述IMU数据和所述ODO数据进行积分得到IMU里程数据和IMU&ODO融合里程数据；
根据所述激光雷达数据或者所述组合导航数据中的GNSS数据获取积分约束，并根据前一周期输出的最终定位结果获取先验约束，根据所述积分约束对所述IMU里程数据进行迭代优化以及根据所述先验约束对所述IMU&ODO融合里程数据进行迭代优化；
根据优化后的IMU里程数据、优化后的IMU&ODO融合里程数据以及所述激光雷达数据获取激光雷达位姿数据；
将所述组合导航数据、所述优化后的IMU&ODO融合里程数据、所述激光雷达位姿数据通过滑窗残差公式进行联合优化融合得到最终定位结果。
本申请第二方面提供一种计算机设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如本发明第一方面所述方法的步骤。
本申请第三方面提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如本发明第一方面所述方法的步骤。
本申请提供一种车载多传感器融合定位方法、计算机设备及存储介质，车载多传感器融合定位方法包括获取IMU数据、ODO数据、激光雷达数据以及组合导航数据，并获取IMU里程数据和IMU&ODO融合里程数据；根据积分约束对IMU里程数据进行迭代优化以及根据先验约束对IMU&ODO融合里程数据进行迭代优化；根据优化后的IMU里程数据、优化后的IMU&ODO融合里程数据以及激光雷达数据获取激光雷达位姿数据；将组合导航数据、优化后的IMU&ODO融合里程数据、激光雷达位姿数据通过滑窗残差公式进行联合优化融合得到最终定位结果。本申请技术方案将IMU数据和ODO数据进行融合，由于ODO数据不具有随机游走噪声，ODO数据同样对IMU数据的随机游走噪声有抑制作用，相对简单的坐标对齐之后直接单独使用积分里程噪声更小。通过积分约束和先验约束修正IMU的随机游走参数，修正后的IMU数据在后续积分里程精度可以得到保证，保证了滑窗约束优化融合过程初始节点位姿精度，降低了优化迭代周期，提高了计算速度，同时提高了计算精度，系统稳定性高，提升了车辆的安全性。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对本发明实施例的描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1是本发明一实施例中一种车载多传感器融合定位方法中的安全计算平台的结构示意图；
图2是本发明一实施例中一种车载多传感器融合定位方法的流程图；
图3是本发明一实施例中一种车载多传感器融合定位方法中步骤S101中的IMU数据安全判断示意图；
图4是本发明一实施例中一种车载多传感器融合定位方法中步骤S101中的ODO数据安全判断示意图；
图5是本发明一实施例中一种车载多传感器融合定位方法中步骤S101中的组合导航数据安全判断示意图；
图6是本发明一实施例中一种车载多传感器融合定位方法中步骤S101中的流程图；
图7是本发明一实施例中一种车载多传感器融合定位方法中的车辆轨迹示意图；
图8是本发明一实施例中一种车载多传感器融合定位方法中的IMU&ODO坐标系分布以及转换关系示意图；
图9是本发明一实施例中一种车载多传感器融合定位方法中步骤S102中的流程图；
图10是本发明一实施例中一种车载多传感器融合定位方法中步骤S102中的流程图；
图11是本发明一实施例中一种车载多传感器融合定位方法中步骤S103中的流程图；
图12是本发明一实施例中一种车载多传感器融合定位方法中步骤S103中的另一流程图；
图13是本发明一实施例中一种车载多传感器融合定位方法中步骤S103中的点云特征信息示意图；
图14是本发明一实施例中一种车载多传感器融合定位方法中步骤S104中的流程图；
图15是本发明一实施例中一种车载多传感器融合定位方法中步骤S104中的滑窗约束优化融合示意图；
图16是本发明一实施例中一种车载多传感器融合定位方法中的融合定位系统信息处理关系图；
图17是本发明一实施例中一种车载多传感器融合定位方法中的融合定位系统信息处理关系图；
图18是本发明一实施例中计算机设备的一示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本申请实施例提供一种车载多传感器融合定位方法，可应用于轨道车辆中的控制系统中，如图1所示，控制系统包括数据采集传感器和车载安全计算平台，其中，数据采集传感器包括激光雷达Lidar、轮速传感器ODO、惯性传感器IMU、组合导航。其中组合导航设备包括惯性传感器IMU、GNSS天线、RTK天线、组合导航处理单元；安全计算平台上搭载了多传感器融合定位模块软件系统以及地图数据，以根据多传感器采集的信息进行融合定位。
在一实施例中，如图2所示，提供一种车载多传感器融合定位方法，包括步骤S101、步骤S102、步骤S103以及步骤S104，具体步骤如下：
步骤S101.获取IMU数据、ODO数据、激光雷达数据以及组合导航数据，并将IMU数据和ODO数据进行积分得到IMU里程数据和IMU&ODO里程融合数据。
其中，IMU数据的获取是通过惯性传感器（IMU，Inertial Measurement Unit）进行采集，作为一种实施方式，如图3所示，获取IMU数据的过程包括对IMU数据进行安全判断，具体过程为通过两组惯性传感器得到两组IMU数据，对两组数据中的加速度和角速度发散诊断，如果诊断安全，则计算加速度和角速度均值，其中，对加速度和角速度发散诊断是指判断两组数据的差值是否在预设误差内，在预设误差内则视为判断安全。
其中，ODO数据的获取是通过轮速传感器进行采集，作为一种实施方式，如图4所示，获取ODO数据的过程包括对ODO数据进行安全判断，具体过程为通过轮速传感器得到ODO数据，对两组ODO数据中的速度和加速度发散诊断，如果诊断安全，则计算速度和加速度均值，其中，对速度和加速度发散诊断是指判断两组数据的差值是否在预设误差内，在预设误差内则视为判断安全。
其中，组合导航数据的获取是通过组合导航设备包括IMU、GNSS天线、RTK天线、组合导航处理单元进行采集，作为一种实施方式，如图5所示，获取组合导航数据的过程包括对组合导航数据进行安全判断，具体过程为通过组合导航设备得到两组组合导航数据，组合导航A位置坐标和速度以及组合导航B位置坐标和速度，对两组组合导航数据中的位置坐标和速度发散诊断，如果诊断安全，则计算位置坐标和速度均值，其中，对位置坐标和速度发散诊断是指判断两组数据的差值是否在预设误差内，在预设误差内则视为判断安全。
其中，如图6所示，将IMU数据和ODO数据进行积分得到IMU里程数据和IMU&ODO里程融合数据，包括：
步骤S110. 对IMU数据进行积分得到IMU里程数据。
其中，由于IMU数据频率较高，需要对IMU数据进行积分使IMU数据和ODO数据同步，IMU数据包括加速度计数据、陀螺仪数据以及航向角，通过对加速度积分得到速度与时间相乘以及对航向角进行积分得到里程数据。
步骤S111.根据IMU数据对ODO数据进行积分得到IMU&ODO融合里程数据。
其中，仅根据ODO数据不能进行积分，ODO数据需要IMU提供的航向角、俯仰角以及横滚角进行积分得到IMU&ODO融合里程数据。
具体的，如图7和图8所示，车辆ODO坐标系下线速度和角速度转换如下：
其中
是左右轮半径，
是左右轮角速度，d是左右轮距；
基于IMU局部坐标系{I}下通过动力学方程（3）进行积分里程，IMU&ODO融合积分动力学方程如下：
其中，
为K+1时刻IMU&ODO的状态量，
为K时刻IMU&ODO的状态量，
为状态转移模型矩阵，
为量测噪声向量，Gk观测矩阵，默认为单位矩阵；
例如：
={Pxyz，Vxyz，Rrpy，
}
A=
B=
C=
Pxyz表示位置坐标状态，Vxyz表示速度状态，Rrpy表述k时刻IMU方向状态，dt表示时间增量，
表示加速度信息，
表示角速度信息，
表示加速度随机游走，
表示角速度随机游走。
预积分的雅克比矩阵传递公式如下：
协方差P传递公式：
其中
和
分别为k以及k+1时刻的雅克比矩阵，
和
分别为k以及k+1时刻的协方差矩阵，Q为噪声信号N的协方差矩阵。
通过上述公式预积分计算得到IMU&ODO融合里程数据。
步骤S102.根据激光雷达数据或者组合导航数据中的GNSS数据获取积分约束，并根据前一周期输出的最终定位结果获取先验约束，根据积分约束对IMU里程数据进行迭代优化以及根据先验约束对IMU&ODO融合里程数据进行迭代优化。
其中，由于IMU&ODO的数据频率远高于雷达Lidar和GNSS的频率，所以分别在两帧Lidar数据或者GNSS数据之间进行里程积分转换成里程数据，两帧之间的积分量为IMU里程数据的约束，最近一个周期的结果可以作为IMU&ODO融合里程数据的先验约束，两个约束可以计算车辆位姿状态误差。
其中，如图9所示，根据积分约束对IMU里程数据进行迭代优化，包括：
步骤S120.将激光雷达数据或者组合导航数据中的GNSS数据与IMU里程数据进行对比得到车辆位姿状态误差。
步骤S121.将车辆位姿状态误差加入到对IMU里程数据的计算过程中对IMU里程数据进行迭代优化。
其中，激光雷达数据或者组合导航数据中的GNSS数据可以视为准确值，建立激光雷达数据或者组合导航数据中的GNSS数据与IMU里程数据以及随机游走的迭代优化，通过迭代优化可以获得当前帧随机游走变化量（车辆位姿状态误差），根据机游走变化量对IMU里程数据的计算过程中对IMU里程数据进行迭代优化，具体在计算位置值和速度值时其中加速度值替换为加速度值减去加速度随机游走，角速度值替换为角速度值减去角速度随机游走。
其中，如图10所示，根据先验约束对IMU&ODO融合里程数据进行迭代优化，包括：
步骤S122.将前一周期输出的最终定位结果与IMU&ODO融合里程数据进行对比得到车辆位姿状态误差。
步骤S123.将车辆位姿状态误差加入到对IMU&ODO融合里程数据的计算过程中对IMU&ODO融合里程数据进行迭代优化。
其中，由于两帧之间IMU的bias（随机游走）真实情况是有变化的，建立车辆位姿状态误差关于两帧的位姿、速度以及bias的迭代优化。通过迭代优化可以获得当前帧随机游走变化量（车辆位姿状态误差），通过上述公式（4）可以在迭代优化过程计算更新随机游走变化的积分里程，基于优化后的随机游走和IMU&ODO融合里程数据重新计算先验约束关键帧时间点以后的IMU&ODO融合里程数据增量，再加上先验约束关键帧位姿可以得到优化后的IMU&ODO融合里程数据。
步骤S103.根据优化后的IMU里程数据、优化后的IMU&ODO融合里程数据以及激光雷达数据获取激光雷达位姿数据。
其中，本步骤包括两种实施方式，一种实施方式是采用激光雷达定位算法A点云特征帧最近邻搜索算法获取激光雷达位姿数据，一种实施方式采用激光雷达定位算法B点云特征帧与局部地图匹配算法取激光雷达位姿数据。
作为一种实施方式，如图11所示，根据优化后的IMU里程数据、优化后的IMU&ODO融合里程数据以及激光雷达数据获取激光雷达位姿数据，包括：
步骤S131.获取激光雷达数据，基于IMU里程数据对激光雷达数据进行去畸变计算，并对激光雷达数据进行特征点提取得到激光雷达数据特征点。
步骤S132.根据点云特征和IMU&ODO融合里程数据提取局部特征地图，将提取的特征点在高维空间量化，并在高位空间搜索局部特征地图对应的最近邻特征点，得到最近邻关键帧位姿。
其中，本实施方式采用点云特征帧最近邻搜索算法获取激光雷达位姿数据，获取激光雷达数据为通过激光雷达设备获取点云数据，根据IMU里程数据中的位姿数据对激光雷达数据进行去畸变计算，得到预测的激光雷达位姿，对激光雷达数据进行特征点提取计算得到激光雷达数据特征点，根据IMU&ODO融合里程数据在点云特征所对应的地图中提取IMU&ODO融合里程数据对应的局部特征地图，将局部特征地图进行高维空间量化得到特征点集合，在特征点集合中搜索激光雷达数据特征点对应的最近邻特征点，得到最近邻关键帧位姿。
作为一种实施方式，如图12所示，根据优化后的IMU里程数据、优化后的IMU&ODO融合里程数据以及激光雷达数据获取激光雷达位姿数据，包括：
步骤S133.获取激光雷达数据，基于IMU里程数据对激光雷达数据进行去畸变计算，并对激光雷达数据进行特征点提取得到激光雷达数据特征点。
步骤S134.根据点云特征和IMU&ODO融合里程数据提取局部特征地图，将激光雷达数据特征点匹配到局部特征地图中，根据匹配结果得到激光雷达位姿数据。
其中，本实施方式采用点云特征帧与局部地图匹配算法获取激光雷达位姿数据，获取激光雷达数据为通过激光雷达设备获取点云数据，根据IMU里程数据中的位姿数据对激光雷达数据进行去畸变计算，得到预测的激光雷达位姿，对激光雷达数据进行特征点提取计算得到激光雷达数据特征点，根据IMU&ODO融合里程数据在点云特征所对应的地图中提取IMU&ODO融合里程数据对应的局部特征地图，将激光雷达数据特征点匹配到局部特征地图中，根据匹配结果得到激光雷达位姿数据。
其中，激光雷达定位算法A和激光雷达定位算法B相互配合使用，激光雷达定位算法A可以用于检测激光雷达定位算法B，例如，如图13所示，激光雷达定位算法A得到的特征点为局部特征地图中的某个特征点，检测激光雷达定位算法B可以跟精确的得到具体位置为在该特征点的前后某个位置，通过安全判断检测激光雷达定位算法B得到的是否在激光雷达定位算法A的阈值范围内，在阈值范围内，则判定激光雷达定位算法B通过安全判断，输出激光雷达定位算法B的定位结果。
步骤S104.将组合导航数据、优化后的IMU&ODO融合里程数据、激光雷达位姿数据通过滑窗进行联合优化融合得到最终定位结果。
其中，激光雷达位姿数据和组合导航数据包括位姿数据以及噪声协方差数据，通过滑窗的方式进行联合优化融合，联合当前周期的以及前n个周期的历史数据创建联合残差函数，进行联合优化迭代求解n+1周期的状态节点的状态信息（位置姿态为Xk，速度为Vk），例如，如图15所示，设置滑窗可以融合5个周期，当前周期为5，根据第1、2、3、4、5周期的数据获取最终定位结果，当前周期为6，根据第2、3、4、5、6周期的数据获取最终定位结果，以此类推，得到每个周期的定位结果。
其中，如图14所示，将组合导航数据、优化后的IMU&ODO融合里程数据、激光雷达位姿数据通过滑窗进行联合优化融合得到最终定位结果，包括：
步骤S141.根据当前周期的组合导航数据、优化后的IMU&ODO融合里程数据、激光雷达位姿数据以及前n个周期的历史数据获取滑窗残差公式以及联合优化迭代残差函数。
步骤S142. 根据滑窗残差公式以及联合优化迭代残差函数计算最终定位结果。
其中，滑窗残差公式为：
dt表示两节点时间差，
表示激光雷达数据协方差矩阵，
表示组合导航数据协方差矩阵，
表示IMU&ODO融合里程数据协方差矩阵，
为第K周期的位姿，
为第K-n周期的位姿，
为第K周期的速度，
为第K-n周期的速度，I为单位矩阵，
为第K-n周期的激光雷达数据，
为第K-n周期的组合导航数据；
联合优化迭代残差函数为：
其中各个传感器噪声对应协方差矩阵作为各个残差项的权重信息，可以自适应动态滑框内残差函数。例如，当激光雷达噪声
增大时，对应第一项优化残差权重降低。当组合导航噪声
降低时（定位精度高），对应第二项的优化残差权重增大，对应优化变量受组合导航定位影响较多。
此外，滑框内n+1个周期的传感器信息都参与了公式6残差函数方程的计算，会影响n+1个节点优化后的定位状态，通过残差函数方程第三项约束了n+1个节点定位状态，在前n个状态精度保证的前提下，第n+1的状态误差可以控制在线性化的动力学模型误差范围内，另外该残差项也起到了多周期之间的传感器数据联动求解。
进一步的，在步骤S104之前还包括对组合导航数据、优化后的IMU&ODO融合里程数据、激光雷达位姿数据进行安全判断。
具体的，组合导航数据、优化后的IMU&ODO融合里程数据、激光雷达位姿数据均在同一坐标系下，判断组合导航数据、优化后的IMU&ODO融合里程数据、激光雷达位姿数据之间的差值是否在误差范围内，在误差范围内则通过安全判断。
如图16和图17所示，下面通过具体的例子对本申请技术方案进行具体说明：
组合导航设备A进行设备状态检测输出第一组组合导航数据，组合导航设备B进行设备状态检测输出第二组组合导航数据，对第一组组合导航数据和第二组组合导航数据进行安全判断，检测两组数据差距是否在允许的误差范围内，通过安全判断时输出组合导航数据。
激光雷达设备进行设备状态检测输出点云数据，安全计算平台中存储有点云特征地图信息。
IMU设备A进行设备状态检测输出第一组IMU数据，IMU设备B进行设备状态检测输出第二组IMU数据，对第一组IMU数据和第二组IMU数据进行安全判断，检测两组数据差距是否在允许的误差范围内，通过安全判断时输出IMU数据。
ODO设备A进行设备状态检测输出第一组ODO数据，ODO设备B进行设备状态检测输出第二组ODO数据，对第一组ODO数据和第二组ODO数据进行安全判断，检测两组数据差距是否在允许的误差范围内，通过安全判断时输出ODO数据。
将IMU数据加速度积分得到速度与时间相乘以及对航向角进行积分得到里程数据得到IMU里程数据。
结合上述公式（1）至（5），根据IMU提供的航向角、俯仰角以及横滚角对ODO数据进行积分得到IMU&ODO融合里程数据。
对IMU里程数据和IMU&ODO融合里程数据进行IMU&ODO安全判断，当两个数据存在一致性时，通过判断输出两个数据。
对IMU里程数据和IMU&ODO融合里程数据进行融合积分，获取两帧雷达数据，两帧之间的积分量为IMU里程数据的约束。滑窗约束优化融合输出的最近一个周期的结果可以作为IMU&ODO融合里程数据的先验约束，两个约束可以计算残差，由于两帧之间IMU的bias（随机游走）真实情况是有变化的，建立IMU&ODO融合里程数据状态量残差关于两帧的位姿、速度以及随机游走的迭代优化。通过迭代优化可以获得当前帧随机游走变化量，根据当前帧随机游走变化量优化IMU里程数据，以及通过公式（4）可以在迭代优化过程计算更新随bias变化的积分里程。基于优化后的bias和IMU&ODO融合里程数据重新计算先验约束关键帧时间点以后的融合积分里程增量，在加上先验约束关键帧位姿可以得到优化后的IMU&ODO融合里程数据。
根据IMU里程数据中的位姿数据对点云数据进行去偏移计算，假设采集激光雷达数据的过程中激光雷达是线性运动的，然后根据开始S1帧采集时车辆的位姿和结束S1帧时的位姿进行线性插值，实现去偏移计算。得到预测的激光雷达位姿，对激光雷达数据进行特征点提取计算得到特征点，根据IMU&ODO融合里程数据在点云特征中提取IMU&ODO融合里程数据对应的局部特征地图，将局部特征地图进行高维空间量化得到特征点集合，在特征点集合中搜索激光雷达数据特征点对应的最近邻特征点，得到最近邻关键帧位姿。以及将激光雷达数据特征点匹配到局部特征地图中，根据匹配结果得到激光雷达位姿数据。判断激光雷达位姿数据是否在最近邻关键帧位姿的安全阈值范围内，在安全范围内输出激光雷达位姿数据。
判断组合导航数据、优化后的IMU&ODO融合里程数据、激光雷达位姿数据之间的差值是否在误差范围内，在误差范围内则通过安全判断。
将组合导航数据、优化后的IMU&ODO融合里程数据、激光雷达位姿数据通过滑窗残差公式（6）和公式（7）进行联合优化融合得到最终定位结果。
本实施方式针对每个使用到的传感器进行安全诊断以及融合结果的安全诊断保证了计算全流程的安全完整性监测，相对于现有方案，安全性更高；本实施方式所有融合计算都是基于安全诊断结果良好的情况下进行，保证最终定位结果的安全性完整性，降低失效率。
本实施方式IMU&ODO与组合导航以及激光雷达定位结果融合后的结果作为先验约束，反馈到IMU&ODO融合优化约束，从而修正IMU的随机游走参数，修正后的IMU数据在后续积分里程精度可以得到保证，保证了滑窗约束优化融合过程初始节点位姿精度。降低了优化迭代周期，提高了计算速度。
本实施方式IMU与ODO融合过程，ODO不具有随机游走噪声，ODO同样对IMU的随机游走噪声有抑制作用，相对简单的坐标对齐之后直接单独使用积分里程噪声更小。
本申请提供一种车载多传感器融合定位方法，车载多传感器融合定位方法包括获取IMU数据、ODO数据、激光雷达数据以及组合导航数据，并获取IMU里程数据和IMU&ODO融合里程数据；根据积分约束对IMU里程数据进行迭代优化以及根据先验约束对IMU&ODO融合里程数据进行迭代优化；根据优化后的IMU里程数据、优化后的IMU&ODO融合里程数据以及激光雷达数据获取激光雷达位姿数据；将组合导航数据、优化后的IMU&ODO融合里程数据、激光雷达位姿数据通过滑窗残差公式进行联合优化融合得到最终定位结果。本申请技术方案将IMU数据和ODO数据进行融合，由于ODO数据不具有随机游走噪声，ODO数据同样对IMU数据的随机游走噪声有抑制作用，相对简单的坐标对齐之后直接单独使用积分里程噪声更小。通过积分约束和先验约束修正IMU的随机游走参数，修正后的IMU数据在后续积分里程精度可以得到保证，保证了滑窗约束优化融合过程初始节点位姿精度，降低了优化迭代周期，提高了计算速度，同时提高了计算精度，系统稳定性高。
在一个实施例中，提供了一种计算机设备，该计算机设备可以是服务器，其内部结构图可以如图18所示。该计算机设备包括通过系统总线连接的处理器、存储器、网络接口和数据库。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统、计算机程序和数据库。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的数据库用于存储上述实施例的车辆客流监测方法中所使用到的数据。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种车载多传感器融合定位方法。
在一个实施例中，提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行计算机程序时实现上述实施例中的车载多传感器融合定位方法。
在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现上述实施例中的车载多传感器融合定位方法。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器（ROM）、可编程ROM（PROM）、电可编程ROM（EPROM）、电可擦除可编程ROM（EEPROM）或闪存。易失性存储器可包括随机存取存储器（RAM）或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM（SRAM）、动态RAM（DRAM）、同步DRAM（SDRAM）、双数据率SDRAM（DDRSDRAM）、增强型SDRAM（ESDRAM）、同步链路（Synchlink） DRAM（SLDRAM）、存储器总线（Rambus）直接RAM（RDRAM）、直接存储器总线动态RAM（DRDRAM）、以及存储器总线动态RAM（RDRAM）等。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。
以上所述实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围，均应包含在本发明的保护范围之内。
Claims (6)
1.一种车载多传感器融合定位方法，其特征在于，包括：
获取IMU数据、ODO数据、激光雷达数据以及组合导航数据，并将所述IMU数据和所述ODO数据进行积分得到IMU里程数据和IMU&ODO融合里程数据；
根据所述激光雷达数据或者所述组合导航数据中的GNSS数据获取积分约束，并根据前一周期输出的最终定位结果获取先验约束，根据所述积分约束对所述IMU里程数据进行迭代优化以及根据所述先验约束对所述IMU&ODO融合里程数据进行迭代优化；
根据优化后的IMU里程数据、优化后的IMU&ODO融合里程数据以及所述激光雷达数据获取激光雷达位姿数据；
将所述组合导航数据、所述优化后的IMU&ODO融合里程数据、所述激光雷达位姿数据通过滑窗进行联合优化融合得到最终定位结果；
所述根据所述积分约束对所述IMU里程数据进行迭代优化，包括：
将所述激光雷达数据或者所述组合导航数据中的GNSS数据与所述IMU里程数据进行对比得到车辆位姿状态误差；
将所述车辆位姿状态误差加入到对IMU里程数据的计算过程中，以对IMU里程数据进行迭代优化；
所述根据所述先验约束对所述IMU&ODO融合里程数据进行迭代优化，包括：
将前一周期输出的最终定位结果与所述IMU&ODO融合里程数据进行对比得到车辆位姿状态误差；
将所述车辆位姿状态误差加入到对IMU&ODO融合里程数据的计算过程中，以对IMU&ODO融合里程数据进行迭代优化；
所述将所述组合导航数据、所述优化后的IMU&ODO融合里程数据、所述激光雷达位姿数据通过滑窗进行联合优化融合得到最终定位结果，包括：
根据当前周期的组合导航数据、优化后的IMU&ODO融合里程数据、激光雷达位姿数据以及前n个周期的历史数据获取滑窗残差公式以及联合优化迭代残差函数；
根据所述滑窗残差公式以及所述联合优化迭代残差函数计算最终定位结果；
所述滑窗残差公式为：
其中，dt表示两节点时间差，cov(Lk-n)表示激光雷达数据协方差矩阵，cov(Gk-n)表示组合导航数据协方差矩阵，cov(Xk-n)表示IMU&ODO融合里程数据协方差矩阵，Xk为第K周期的位姿，Xk-n为第K-n周期的位姿，Vk为第K周期的速度，Vk-n为第K-n周期的速度，I为单位矩阵，Lk-n为第K-n周期的激光雷达数据，Gk-n为第K-n周期的组合导航数据；
所述联合优化迭代残差函数为：
min{f([Xk-n...Xk]，[Vk-n...Vk])}。
2.如权利要求1所述的车载多传感器融合定位方法，其特征在于，所述将所述IMU数据和所述ODO数据进行积分得到IMU里程数据和IMU&ODO里程融合数据，包括：
对所述IMU数据进行积分得到IMU里程数据；
根据IMU数据对ODO数据进行积分得到IMU&ODO融合里程数据。
3.如权利要求1所述的车载多传感器融合定位方法，其特征在于，所述根据优化后的IMU里程数据、优化后的IMU&ODO融合里程数据以及所述激光雷达数据获取激光雷达位姿数据，包括：
获取激光雷达数据，基于所述IMU里程数据对激光雷达数据进行去畸变计算，并对激光雷达数据进行特征点提取得到激光雷达数据特征点；
根据点云特征和IMU&ODO融合里程数据提取局部特征地图，将提取的特征点在高维空间量化，并在高位空间搜索所述激光雷达数据特征点对应的最近邻特征点，得到最近邻关键帧位姿。
4.如权利要求1所述的车载多传感器融合定位方法，其特征在于，所述根据优化后的IMU里程数据、优化后的IMU&ODO融合里程数据以及所述激光雷达数据获取激光雷达位姿数据，包括：
获取激光雷达数据，基于所述IMU里程数据对激光雷达数据进行去畸变计算，并对激光雷达数据进行特征点提取得到激光雷达数据特征点；
根据点云特征和IMU&ODO融合里程数据提取局部特征地图，将激光雷达数据特征点匹配到局部特征地图中，根据匹配结果得到激光雷达位姿数据。
5.一种计算机设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至4任一项所述方法的步骤。
6.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至4任一项所述方法的步骤。
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111579011.6A CN113970330B (zh) | 2021-12-22 | 2021-12-22 | 车载多传感器融合定位方法、计算机设备及存储介质 |
| PCT/CN2022/140863 WO2023116797A2 (zh) | 2021-12-22 | 2022-12-22 | 车载多传感器融合定位方法、计算机设备及存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111579011.6A CN113970330B (zh) | 2021-12-22 | 2021-12-22 | 车载多传感器融合定位方法、计算机设备及存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113970330A CN113970330A (zh) | 2022-01-25 |
| CN113970330B true CN113970330B (zh) | 2022-04-19 |
Family
ID=79590749
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111579011.6A Active CN113970330B (zh) | 2021-12-22 | 2021-12-22 | 车载多传感器融合定位方法、计算机设备及存储介质 |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN113970330B (zh) |
| WO (1) | WO2023116797A2 (zh) |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113970330B (zh) * | 2021-12-22 | 2022-04-19 | 比亚迪股份有限公司 | 车载多传感器融合定位方法、计算机设备及存储介质 |
| CN115790618B (zh) * | 2022-11-03 | 2023-09-01 | 中科天极(新疆)空天信息有限公司 | 基于激光雷达的slam定位方法、系统及存储介质 |
| CN117473455B (zh) * | 2023-12-27 | 2024-03-29 | 合众新能源汽车股份有限公司 | 多源定位数据的融合方法、装置和电子设备 |
| CN117848332B (zh) * | 2024-03-07 | 2024-05-03 | 北京理工大学前沿技术研究院 | 一种车载多源融合高精度定位系统的imu噪声消除方法 |
| CN118334568B (zh) * | 2024-06-13 | 2024-09-06 | 广汽埃安新能源汽车股份有限公司 | 一种位姿构建方法、装置、电子设备及存储介质 |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6459990B1 (en) * | 1999-09-23 | 2002-10-01 | American Gnc Corporation | Self-contained positioning method and system thereof for water and land vehicles |
| CN110906923A (zh) * | 2019-11-28 | 2020-03-24 | 重庆长安汽车股份有限公司 | 车载多传感器紧耦合融合定位方法、系统、存储介质及车辆 |
Family Cites Families (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6564148B2 (en) * | 2001-03-06 | 2003-05-13 | Honeywell International | Integrated inertial VMS navigation with inertial odometer correction |
| US8174568B2 (en) * | 2006-12-01 | 2012-05-08 | Sri International | Unified framework for precise vision-aided navigation |
| US9482536B2 (en) * | 2012-05-31 | 2016-11-01 | International Electronic Machines Corp. | Pose estimation |
| CA2985263A1 (en) * | 2015-05-06 | 2016-11-10 | Crown Equipment Corporation | Industrial vehicle comprising tag reader and reader module |
| CN107402012A (zh) * | 2016-05-20 | 2017-11-28 | 北京自动化控制设备研究所 | 一种车辆的组合导航方法 |
| US10837773B2 (en) * | 2016-12-30 | 2020-11-17 | DeepMap Inc. | Detection of vertical structures based on LiDAR scanner data for high-definition maps for autonomous vehicles |
| EP3358303B1 (en) * | 2017-02-07 | 2021-09-01 | HERE Global B.V. | An apparatus and associated methods for use in updating map data |
| CN112744120B (zh) * | 2019-10-31 | 2023-04-07 | 比亚迪股份有限公司 | 估算电动车辆续驶里程的方法、云端服务器、车辆和介质 |
| CN111156984B (zh) * | 2019-12-18 | 2022-12-09 | 东南大学 | 一种面向动态场景的单目视觉惯性slam方法 |
| CN113819905B (zh) * | 2020-06-19 | 2024-07-12 | 北京图森未来科技有限公司 | 一种基于多传感器融合的里程计方法及装置 |
| CN111949943B (zh) * | 2020-07-24 | 2022-08-30 | 北京航空航天大学 | 一种面向高级自动驾驶的v2x和激光点云配准的车辆融合定位方法 |
| CN112634451B (zh) * | 2021-01-11 | 2022-08-23 | 福州大学 | 一种融合多传感器的室外大场景三维建图方法 |
| CN113311452B (zh) * | 2021-05-26 | 2022-12-30 | 上海新纪元机器人有限公司 | 一种基于多传感器的定位方法及其系统 |
| CN113432600B (zh) * | 2021-06-09 | 2022-08-16 | 北京科技大学 | 基于多信息源的机器人即时定位与地图构建方法及系统 |
| CN113405549B (zh) * | 2021-06-17 | 2023-11-14 | 中寰卫星导航通信有限公司 | 车辆定位方法、组件、电子设备及存储介质 |
| CN113970330B (zh) * | 2021-12-22 | 2022-04-19 | 比亚迪股份有限公司 | 车载多传感器融合定位方法、计算机设备及存储介质 |
-
2021
- 2021-12-22 CN CN202111579011.6A patent/CN113970330B/zh active Active
-
2022
- 2022-12-22 WO PCT/CN2022/140863 patent/WO2023116797A2/zh unknown
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6459990B1 (en) * | 1999-09-23 | 2002-10-01 | American Gnc Corporation | Self-contained positioning method and system thereof for water and land vehicles |
| CN110906923A (zh) * | 2019-11-28 | 2020-03-24 | 重庆长安汽车股份有限公司 | 车载多传感器紧耦合融合定位方法、系统、存储介质及车辆 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113970330A (zh) | 2022-01-25 |
| WO2023116797A2 (zh) | 2023-06-29 |
| WO2023116797A3 (zh) | 2023-08-17 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113970330B (zh) | 车载多传感器融合定位方法、计算机设备及存储介质 | |
| US11002859B1 (en) | Intelligent vehicle positioning method based on feature point calibration | |
| CN111947671B (zh) | 用于定位的方法、装置、计算设备和计算机可读存储介质 | |
| US10860035B2 (en) | Travel history storage method, method for producing travel path model, method for estimating local position, and travel history storage device | |
| US20150046020A1 (en) | Orientation Model for a Sensor System | |
| KR20140093961A (ko) | 센서 시스템의 데이터의 정확성을 독립적으로 평가하기 위한 센서 시스템 | |
| US10267638B2 (en) | Method and system for adapting a navigation system | |
| WO2023071442A1 (zh) | 一种数据处理方法和装置 | |
| Xia et al. | Advancing estimation accuracy of sideslip angle by fusing vehicle kinematics and dynamics information with fuzzy logic | |
| CN103868523A (zh) | 获取车辆的偏航率传感器的偏差的系统和方法 | |
| CN103796885A (zh) | 用于评估车辆的轮胎参数的方法 | |
| EP3155374A1 (de) | Verfahren und system zur initialisierung eines sensorfusionssystems | |
| CN112406965B (zh) | 一种提高列车定位安全性的方法及系统 | |
| US20220063642A1 (en) | Method for Determining an Integrity Range | |
| US20170122770A1 (en) | Method and system for providing dynamic error values of dynamic measured values in real time | |
| EP3155454B1 (de) | Verfahren und system zur anpassung eines navigationssystems | |
| CN113933858A (zh) | 定位传感器的异常检测方法、装置及终端设备 | |
| DE102014211178A1 (de) | Verfahren und System zur Korrektur von Messdaten eines ersten Sensorsystems | |
| CN116279410A (zh) | 航向误差补偿方法、系统、装置和存储介质 | |
| CN115900796A (zh) | 定位诊断方法、装置、设备及存储介质 | |
| CN114882119A (zh) | 相机外参标定方法、装置、计算机设备和存储介质 | |
| CN115046546A (zh) | 一种基于车道线识别的自动驾驶汽车定位系统及方法 | |
| Shen et al. | An integrated localization system with fault detection, isolation and recovery for autonomous vehicles | |
| Huang et al. | Lane following system for a mobile robot using information from vision and odometry | |
| US20230147739A1 (en) | Automatic detection of lidar to vehicle alignment state using localization data |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |