一种自动驾驶车辆的多传感器融合定位建图方法

1.本发明属于激光雷达、惯性测量单元与轮速编码器数据融合的技术领域，特别涉及一种自动驾驶车辆的多传感器融合定位建图方法。


背景技术：

2.在无特征环境下，如隧道、矿井、地下管道等场景，传统slam算法面临着较大的挑战。由于这些环境中缺乏典型的特征点，导致传统的基于特征点的slam算法难以实现高精度的定位与建图。此外，低照度环境也给激光雷达测量带来困难，影响slam算法的性能。目前在自动驾驶领域针对这类场景的车辆定位问题的解决方案有通过大量消耗人力物力的基础设施部署的方法以及采用价格昂贵的高精度传感器的方案，但是其定位精度还是无法满足此类场景下的车辆定位建图问题。
3.由于激光雷达的测量不受光照条件和天气条件的影响，并且能够获得周围环境准确的距离信息，从原理上保证基于激光雷达的定位算法的精度，因此以激光雷达为核心传感器的定位与建图技术成为了绝大多数自动驾驶公司的方案。由于使用纯激光雷达的定位与建图方案无法解决环境退化、采样频率低等问题，所以该方案定位的鲁棒性、精度以及输出频率都无法满足自动驾驶面对各种复杂工况时的定位需求(cn202210285283.3)。
4.目前，imu是激光雷达融合方案中最常用的辅助传感器，一方面它可以提供高频率的三轴加速度和三轴角速度，这可以弥补激光雷达采集频率较低的问题，另一方面imu的测量不受光照条件，纹理结构，天气条件等外部环境的影响，可以增加激光和imu融合slam算法的鲁棒性。激光雷达和imu的融合方式有松耦合和紧耦合两种。基于激光雷达的松耦合方法通常是分别处理激光雷达和imu的测量结果，并且之后对其结果进行加权融合。松耦合的方法把激光雷达和imu的数据分开单独处理，数据之间没有进行相互更新，尽管这种方法的计算效率比较高，但是理论上的精度不如紧耦合的算法，所以，在现有技术中没有一种适用于车辆在无特征隧道场景自动驾驶的既经济又能够精准定位的方法。


技术实现要素：

5.本发明对现有的自动驾驶车辆在无gps且无边缘特征场景下车辆的定位建图方法的技术问题，提出一种经济且定位精度满足自动驾驶车辆导航需求的多传感器融合定位方法，涉及一种基于激光雷达、惯性测量单元(imu)和轮式编码器融合的在低照度、无特征隧道环境中的同时定位与建图(slam)算法，可实现在无gps信号、低照度、无特征隧道环境中提高地面车辆的精准鲁棒定位的精准鲁棒定位；
6.本发明至少通过如下技术方案之一实现。
7.一种自动驾驶车辆的多传感器融合定位建图方法，包括以下步骤：
8.步骤1、对车载传感器进行标定；
9.步骤2、采集标定的传感器接收的车辆运动过程中实际场景的激光雷达点云数据、imu的位姿数据、轮速计车轮速度数据，并存储在缓存区中；
10.步骤3、利用组合导航系统测量的姿态角初始化imu的重力方向以消除不同地理位置重力的影响，降低姿态估计误差；
11.步骤4、构建imu预积分模型和车轮预积分模型，以获取imu预积分和车速预积分；
12.步骤5、激光点云预处理：对每一帧点云进行体素滤波，再结合imu的里程计信息对点云去除畸变；
13.步骤6、点云配准与建立局部地图，以得到前端里程计位姿；
14.步骤7、当步骤6中的点云配准获得的位姿变化超过阈值时，提取关键帧，并且一个激光扫描的最后一帧也作为一个关键帧；
15.步骤8、根据激光雷达获取的三维点云，将点云的空间结构特征用二维特征图表示，并在检测到回环时构建回环检测因子；
16.步骤9、构建因子图优化模型，将回环检测因子放入因子图优化模型中进行融合定位。
17.进一步地，步骤4包括构建两个激光雷达关键帧之间的imu预积分因子，预积分项为δr
imu
、δv
imu
、δp
imu
，其中δr
imu
为imu旋转预积分、δv
imu
为imu速度预积分，δp
imu
为imu位置预积分；
18.利用二自由度车辆模型求解车辆的纵向速度、侧向速度以及横摆角速度构建车轮预积分因子，车轮预积分因子包括车速预积分项δr
vehicle
、δp
vehicle
，其中δr
vehicle
为车辆旋转预积分，δp
vehicle
为车辆位置预积分。
19.进一步地，步骤4中，获取imu预积分和车速预积分具体为：
20.imu预积分：
[0021][0022]
车速预积分：
[0023][0024]
其中是旋转的预积分测量，δφ
ij
是旋转噪声；是速度的预积分测量，δv
ij
速度噪声；是位置的预积分测量，δp
ij
是位置噪声，ri、rj分别为i、j时刻到世界坐标系下的旋转矩阵，δr
ij
为i、j时刻的旋转矩阵，vi、vj分别是i、j时刻的速度，δt
ij
为i、j之间的时间差，δp
ij
表示i、j时刻的位置变化量，pi代表i时刻的位置，pj代表j时刻的位置。
[0025]
进一步地，激光点云预处理过程中具体为：采用体素滤波算法对原始激光点进行滤波，将3d点云划分出空间体素格子，在每个体素内，用重心点代替体素内的其他数据点，以提高点云配准的准确率。
[0026]
进一步地，点云配准与建立局部地图包括：对提取的关键帧，利用ikd-tree结构的地图结构搜索当前点云帧周围的历史关键帧的集合，然后结合每一个历史关键帧上的全局
位姿信息，构建由ikd-tree数据结构进行维护的局部地图和位姿变换关系。
[0027]
进一步地，在构建ikd-tree结构的过程，首先将整个地图分成多个体素，将每个体素内点云的ikd-tree结构建立起来，每个ikd-tree对应一个体素；
[0028]
用iekf进行点云配准与数据融合过程中的残差计算，计算当前点云帧和局部地图之间的位姿变换关系，首先将局部地图中的特征点提取出来，并根据其相对位置构建特征点地图，在接收到新的点云帧后，使用点云配准算法将当前点云帧与特征点地图进行匹配，得到初始位姿估计，基于特征点对位姿进行进一步的优化，以获得更准确的位姿估计。
[0029]
进一步地，步骤7中，通过检查位姿变化和时间间隔提取关键帧；
[0030]
位姿变化：当相邻激光帧之间的位移或旋转超过预定阈值时，将当前帧作为关键帧；
[0031]
时间间隔：设定最小时间间隔，确保相邻关键帧之间至少存在时间差，以提高运行效率。
[0032]
进一步地，通过基于scan-context描述符的描述符提取方法，将点云的空间结构特征用二维特征图表示。
[0033]
进一步地，步骤8中，将imu预积分因子与激光里程计因子、车轮预积分因子以及回环检测因子建模为最大后验概率问题，每一个状态节点对应一个激光雷达关键帧，imu预积分和车速预积分别为两个关键帧之间提供约束，回环因子则为两个回环点之间的关键帧提供约束，以形成整个多传感器融合目标函数和因子图。
[0034]
进一步地，因子图优化模型的优化目标函数为：
[0035][0036]
其中为预积分因子残差项，为激光里程计因子残差项，为回环因子残差项，为车速预积分因子残差项，e
ij
代表残差，i
pre
代表imu的预积分，i、j代表不同时刻，od代表odometry为激光里程计，l代表回环，lo代表车速里程计。
[0037]
与现有的技术相比，本发明的有益效果为：
[0038]
本发明通过结合激光雷达、轮式编码器和imu数据，提高算法在特征缺失环境下的鲁棒性和可靠性。即使在激光雷达受到环境噪声影响的情况下，也能保持较高的定位和建图精度。不仅在普通的多特征场景下实现了高精度的定位建图，而且在无边缘特征的隧道场景中，即使没有gps信号，也能保证车辆的定位精度。本发明采用的都是低成本的16线激光雷达和低精度的imu以及轮速计，在保证成本优势的前提下实现了隧道场景下车辆的高精度定位。该方法非常有助于自动驾驶技术在应用场景上的扩展和补充。
附图说明
[0039]
图1为本发明实施例一种自动驾驶车辆的多传感器融合定位建图方法的流程图；
[0040]
图2为本发明实施例n200imu标定结果的allan方差图；
[0041]
图3为本发明实施例体素滤波示意图；
[0042]
图4为本发明实施例中的位姿估计示意图；
[0043]
图5为本发明实施例中在地下停车场实施例中所建点云图。
具体实施方式
[0044]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围；
[0045]
本实施例公开的一种自动驾驶车辆的多传感器融合定位建图方法，用于在低照度、无特征隧道环境中提高地面车辆的自主导航性能，算法流程如图1所示，具体包括以下步骤：
[0046]
步骤1、传感器标定；
[0047]
首先进行数据采集，在进行数据采集之前，需要对车载传感器进行标定，其中包括imu内参标定和多传感器联合标定。车体坐标系定位imu传感器坐标系，对imu内参标定，进行的内参标定主要针对高斯白噪声(white noise)和偏置(bias，随机游走)这两个误差来源，内参标定的方法使用imu_utils工具进行标定，标定后通过绘制allan方差图(如图2)来判断标定结果是否准确，测量值和预测值拟合好则标定结果精确，并通过在线联合标定的方式标定好imu和激光雷达的外参。
[0048]
步骤2、数据采集
[0049]
采集的数据包括传感器在工作过程中实时的接收车辆运动过程中实际场景的激光雷达点云数据、imu的位姿数据、轮速计车轮速度数据。采集的数据存储在算法设计的缓存区中。
[0050]
在本实施例中，需要在静止状态下采集imu的原始数据。数据采集时，将imu安装在稳定的平台上，避免受到外部环境的影响。数据采集时长通常要大于120分钟，以保证获得足够的数据用于标定。通常采集时间加长会使得标定结果在一定程度上更加精确。
[0051]
在本实施例中设置在汽车上的激光雷达和在激光雷达正下方的imu分别采集汽车的实时信息，其中激光雷达采用的是vlp-16激光雷达，imu采用的是n200型号的十轴imu，轮速里程计采用车辆原厂的轮速里程计。其中激光雷达采集点云数据，imu车辆的航向信息，航向信息包括车辆的航向角和航向角的角速度，车辆轮速编码器采集车辆的速度信息。
[0052]
步骤3、定位初始化，初始化imu的重力方向，利用组合导航系统测量的姿态角来初始化imu的重力方向以消除不同地理位置重力的影响，降低姿态估计误差；
[0053]
步骤4、构建imu预积分模型和车轮预积分模型，二者将作为算法后端基于因子图优化的两个优化因子。
[0054]
构建两个激光雷达关键帧之间的imu预积分因子，预积分项为δr
imu
、δv
imu
、δp
imu
，其中δr
imu
为imu旋转预积分、δv
imu
为imu速度预积分，δp
imu
为imu位置预积分。
[0055]
利用二自由度车辆模型求解车辆的纵向速度、侧向速度以及横摆角速度，然后构建车速预积分项δr
vehicle
、δp
vehicle
，其中δr
vehicle
为车辆旋转预积分，δp
vehicle
为车辆位置预积分。
[0056]
作为一种优选的实施例，采用的惯性测量单元为100hz，采用周期为0.01s，激光雷
达的扫描频率为10hz，在一个激光关键帧之间有大量的imu测量值，实时对所有imu测量值进行积分计算，计算量很大。
[0057]
故通过在连续的激光帧之间构建imu预积分的形式采集两个关键帧之间的运动信息，得到imu预积分模型：
[0058][0059]
利用二自由度车辆模型求解车辆的纵向速度、侧向速度以及横摆角速度，然后构建车速预积分模型：
[0060][0061]
其中是旋转的预积分测量，δφ
ij
是旋转噪声；是速度的预积分测量，δv
ij
速度噪声；是位置的预积分测量，δp
ij
是位置噪声，ri、rj分别为i、j索引值下imu到世界坐标系下的旋转矩阵，δr
ij
为i、j时刻的旋转矩阵，vi、vj分别是i、j时刻的速度，δt
ij
为i、j之间的时间差，δp
ij
表示i、j时刻的位置变化量，pi代表i时刻的位置，pj代表j时刻的位置。
[0062]
步骤5、激光点云预处理。
[0063]
在本实施例中激光雷达采集的原始点云数据，通常包含的数据量比较大，并且点云数据不可避免的出现噪声点、离群点。这些都会增大点云配准计算复杂度和准确率。因此，通过对点云滤波，可以降低点云中激光点的数量，从而提高计算效率。与此同时，为了保证点云配准的准确率，滤波后的点云应尽可能保留原始点云的几何特征。对于输入的激光雷达原始点云。
[0064]
作为一种优选的实施例，在本实施例中以50hz频率积累点云原始数据，作为激光雷达的一个扫描，然后对输入的每一帧点云进行体素滤波，如图3为本实施例中的滤波示意图，体素是三维空间中一个小的立方体。在每个体素内，用重心点代替体素内的其他数据点。这样既能减少点云数量，减少计算的复杂度，又可以保存点云的几何特征，保证后续点云配准的准确率。对原始激光点进行滤波，将3d点云划分出空间体素格子，体素是三维空间中一个小的立方体。在每个体素内，用重心点代替体素内的其他数据点。这样既能减少点云数量，减少计算的复杂度，保证后续点云配准的准确率。然后结合imu的里程计信息将连续激光帧中的多个imu位姿信息转换到同一时刻坐标系下，对点云去除因激光雷达运动而产生的运动畸变。
[0065]
步骤6、进行点云配准与建立局部地图。
[0066]
点云配准与建立局部地图包括：对提取的关键帧，利用ikd-tree(增量k维树)结构的地图结构搜索当前点云帧周围的历史关键帧的集合，然后结合每一个历史关键帧上的全局位姿信息，构建由ikd-tree数据结构进行维护的局部地图和位姿变换关系。在构建ikd-tree的过程中，首先将整个地图分成多个体素(非上述体素滤波中的体素，此体素为储存点
云地图的一种数据结构)，每个体素内的点云数据称为
″
体素内点云
″
。然后，将每个体素内点云的ikd-tree结构建立起来，以便后续查询。这样，整个地图就被分成多个ikd-tree，每个ikd-tree对应一个体素，用iekf(迭代扩展卡尔曼滤波)进行点云配准与数据融合过程中的残差计算，计算当前点云帧和局部地图之间的位姿变换关系，具体地，首先将局部地图中的特征点提取出来，并根据其相对位置构建特征点地图。然后，在接收到新的点云帧后，使用点云配准算法将当前点云帧与特征点地图进行匹配，得到它们之间的初始位姿估计。接着，使用基于特征点的优化方法对位姿进行进一步的优化，以获得更准确的位姿估计。在本实施例中点云配准与建立局部地图可以看作是一个位姿估计的过程。
[0067]
如图4所示，在这个过程中在一个激光scan内包括imu里程计的前向传播以及通过imu的积分结果进行向后传播的过程。
[0068]
前向传播：
[0069]
x
i+1
＝xi+(δtf(xi,ui,wi))
[0070]
其中xi代表imu的状态量，i为时刻，δt为imu的采样间隔，ui为输入，wi为噪声。
[0071]
后向传播：
[0072]
x
j-1
＝xj+(-δtf(xj,uj,0))
[0073]
其中xj代表imu的状态量，j为时刻，δt为imu的采样间隔，uj为输入。
[0074]
步骤7、提取关键帧，以获得的关键帧的位姿构成的激光里程计因子。提取历史关键帧的策略主要是通过检查位姿变化和时间间隔。在具体实现中，采用了以下方法来挑选关键帧：
[0075]
位姿变化：当相邻激光帧之间的位移或旋转超过预定阈值时，将当前帧作为关键帧。这样做的目的是确保关键帧之间有足够的场景变化，使得后续的优化过程更加稳定和有效。
[0076]
时间间隔：为了限制关键帧的数量，可以设定一个最小时间间隔，确保相邻关键帧之间至少存在一定的时间差。这有助于降低计算量并提高算法的运行效率。
[0077]
通过这种策略挑选关键帧，本发明提出算法能够在保持较高精度的同时，降低计算复杂度和内存需求。
[0078]
步骤8、回环检测。根据激光雷达获取的三维点云，通过基于scan-context描述符的描述符提取方法，将点云的空间结构特征用一种简单的二维特征图表示，提高了回环检测的效率和精度，并在检测到回环时构建回环检测因子。在本实施例中通过检测当前扫描得到的scan-context和储存在全局地图中的scan-context数据库进行匹配检测是否回环。
[0079]
步骤9、后端优化。在本实施例中的后端因子图优化中，将步骤4的imu预积分因子、步骤8的车轮预积分因子以及回环检测因子、步骤7中获得的关键帧的位姿构成的激光里程计因子放入因子图优化模型中构建优化目标函数最后求得优化后得位姿。
[0080][0081]
其中为预积分因子残差项，为激光里程计因子残差项，为回环因子残差项，为车速与积分呢因子残差项，e
ij
代表残差。其中i
pre
代表imu的预积分，i，j代表不同时刻，od代表odometry为激光里程计，l代表回环，lo代表车速里程计。
[0082]
本发明的自动驾驶车辆的多传感器融合定位建图方法，采用激光雷达、惯性测量单元和轮速里程计三种传感器，并通过扩展卡尔曼滤波将三种传感器数据进行融合，实现了厘米级的定位精度。在获取激光雷达、imu和轮速里程计的原始数据后，先对积累激光雷达扫面的一个scan，同时imu进行里程计积分和连续激光关键帧之间的预积分，轮速里程计同时进行车速预积分；当三种传感器的初始数据都进行上述处理后，imu预积分和车速预积分作为后端因子图优化的优化因子。激光里程计部分通过imu的里程计积分进行运动去畸变，同时检测全局scan-context数据库和当前scan-context的相似度判断是否回环；通过迭代扩展卡尔曼滤波方法将局部地图中的历史关键帧和当前帧进行点云配准与数据融合过程中的残差计算，得到前端里程计位姿。最后将四种因子介入到因子图优化模型中进行融合定位求解。
[0083]
本发明不仅在普通的多特征场景下实现了高精度的定位建图，而且在无边缘特征的隧道场景中，即使没有gps信号，也能保证车辆的定位精度。本发明采用的都是低成本的16线激光雷达和低精度的imu以及轮速计，在保证成本优势的前提下实现了隧道场景下车辆的高精度定位，能够在无gps信号且低照度、无特征隧道环境中提高地面车辆的自主导航性能。
[0084]
实现一种自动驾驶车辆的多传感器融合定位建图方法的系统，包括硬件传感器模块1、同步控制器模块2、计算模块3。
[0085]
所述件传感器模块1包括车载激光雷达、imu、轮速里程计三种传感器；
[0086]
所述同步控制器模块2用于实现不同传感器采集数据的时空同步问题；
[0087]
所述计算模块3基于硬件传感器模块1中采集到的各传感器数据，通过本发明的自动驾车车辆定位建图方法实现即使没有gps信号场景下的自动驾驶车辆的高精度定位功能，通过结合激光雷达、轮式编码器和imu数据，提高算法在特征缺失环境下的鲁棒性和可靠性。即使在激光雷达受到环境噪声影响的情况下，也能保持较高的定位和建图精度。
[0088]
在本实施例中，车辆采取基于16线激光雷达、经济型imu以及轮速计的传感器融合方案实现在无gps信号无边缘特征场景下的车辆定位；所述的种自动驾驶车辆的多传感器融合定位建图方法实现了自动驾驶车辆在没有gps信号的情况下高精度的定位建图，如图5所示。
[0089]
以上公开的本发明优选实施例只是用于帮助阐述本发明。优选实施例并没有详尽叙述所有的细节，也不限制该发明仅为所述的具体实施方式。显然，根据本说明书的内容，可作很多的修改和变化。本说明书选取并具体描述这些实施例，是为了更好地解释本发明的原理和实际应用，从而使所属技术领域技术人员能很好地理解和利用本发明。本发明仅受权利要求书及其全部范围和等效物的限制。

技术特征：
1.一种自动驾驶车辆的多传感器融合定位建图方法，其特征在于，包括以下步骤：步骤1、对车载传感器进行标定；步骤2、采集标定的传感器接收的车辆运动过程中实际场景的激光雷达点云数据、imu的位姿数据、轮速计车轮速度数据，并存储在缓存区中；步骤3、利用组合导航系统测量的姿态角初始化imu的重力方向以消除不同地理位置重力的影响，降低姿态估计误差；步骤4、构建imu预积分模型和车轮预积分模型，以获取imu预积分和车速预积分；步骤5、激光点云预处理：对每一帧点云进行体素滤波，再结合imu的里程计信息对点云去除畸变；步骤6、点云配准与建立局部地图，以得到前端里程计位姿；步骤7、当步骤6中的点云配准获得的位姿变化超过阈值时，提取关键帧，并且一个激光扫描的最后一帧也作为一个关键帧；步骤8、根据激光雷达获取的三维点云，将点云的空间结构特征用二维特征图表示，并在检测到回环时构建回环检测因子；步骤9、构建因子图优化模型，将回环检测因子放入因子图优化模型中进行融合定位。2.根据权利要求1所述的自动驾驶车辆的多传感器融合定位建图方法，其特征在于：步骤4包括构建两个激光雷达关键帧之间的imu预积分因子，预积分项为δr
imu
、δv
imu
、δp
imu
，其中δr
imu
为imu旋转预积分、δv
imu
为imu速度预积分，δp
imu
为imu位置预积分；利用二自由度车辆模型求解车辆的纵向速度、侧向速度以及横摆角速度构建车轮预积分因子，车轮预积分因子包括车速预积分项δr
vehicle
、δp
vehicle
，其中δr
vehicle
为车辆旋转预积分，δp
vehicle
为车辆位置预积分。3.根据权利要求1所述的自动驾驶车辆的多传感器融合定位建图方法，其特征在于：步骤4中，获取imu预积分和车速预积分具体为：imu预积分：车速预积分：其中是旋转的预积分测量，δφ
ij
是旋转噪声；是速度的预积分测量，δv
ij
速度噪声；是位置的预积分测量，δp
ij
是位置噪声，r
i
、r
j
分别为i、j时刻到世界坐标系下的旋转矩阵，δr
ij
为i、j时刻的旋转矩阵，v
i
、v
j
分别是i、j时刻的速度，δt
ij
为i、j之间的时间差，δp
ij
表示i、j时刻的位置变化量，p
i
代表i时刻的位置，p
j
代表j时刻的位置。4.根据权利要求1所述的自动驾驶车辆的多传感器融合定位建图方法，其特征在于：激
光点云预处理过程中具体为：采用体素滤波算法对原始激光点进行滤波，将3d点云划分出空间体素格子，在每个体素内，用重心点代替体素内的其他数据点，以提高点云配准的准确率。5.根据权利要求1所述的自动驾驶车辆的多传感器融合定位建图方法，其特征在于：点云配准与建立局部地图包括：对提取的关键帧，利用ikd-tree结构的地图结构搜索当前点云帧周围的历史关键帧的集合，然后结合每一个历史关键帧上的全局位姿信息，构建由ikd-tree数据结构进行维护的局部地图和位姿变换关系。6.根据权利要求5所述的自动驾驶车辆的多传感器融合定位建图方法，其特征在于：在构建ikd-tree结构的过程，首先将整个地图分成多个体素，将每个体素内点云的ikd-tree结构建立起来，每个ikd-tree对应一个体素；用iekf进行点云配准与数据融合过程中的残差计算，计算当前点云帧和局部地图之间的位姿变换关系，首先将局部地图中的特征点提取出来，并根据其相对位置构建特征点地图，在接收到新的点云帧后，使用点云配准算法将当前点云帧与特征点地图进行匹配，得到初始位姿估计，基于特征点对位姿进行进一步的优化，以获得更准确的位姿估计。7.根据权利要求1所述的自动驾驶车辆的多传感器融合定位建图方法，其特征在于：步骤7中，通过检查位姿变化和时间间隔提取关键帧；位姿变化：当相邻激光帧之间的位移或旋转超过预定阈值时，将当前帧作为关键帧；时间间隔：设定最小时间间隔，确保相邻关键帧之间至少存在时间差，以提高运行效率。8.根据权利要求1所述的自动驾驶车辆的多传感器融合定位建图方法，其特征在于：通过基于scan-context描述符的描述符提取方法，将点云的空间结构特征用二维特征图表示。9.根据权利要求1～8任一项所述的自动驾驶车辆的多传感器融合定位建图方法，其特征在于：步骤8中，将imu预积分因子与激光里程计因子、车轮预积分因子以及回环检测因子建模为最大后验概率问题，每一个状态节点对应一个激光雷达关键帧，imu预积分和车速预积分别为两个关键帧之间提供约束，回环因子则为两个回环点之间的关键帧提供约束，以形成整个多传感器融合目标函数和因子图。10.根据权利要求1所述的自动驾驶车辆的多传感器融合定位建图方法，其特征在于：因子图优化模型的优化目标函数为：其中为预积分因子残差项，为激光里程计因子残差项，为回环因子残差项，为车速预积分因子残差项，e
ij
代表残差，i
pre
代表imu的预积分，i、j代表不同时刻，od代表odometry为激光里程计，l代表回环，lo代表车速里程计。

技术总结
本发明公开了一种自动驾驶车辆的多传感器融合定位建图方法，包括以下步骤：在获取激光雷达、IMU和轮速里程计的原始数据后，先对积累激光雷达扫面的一个scan，同时IMU进行里程计积分和连续激光关键帧之间的预积分，轮速里程计同时进行车速预积分；当三种传感器的初始数据都进行处理后，IMU预积分和车速预积分作为后端因子图优化的优化因子；激光里程计部分通过IMU的里程计积分进行运动去畸变，同时检测是否回环；通过iEKF方法将局部地图中的历史关键帧和当前帧进行点云配准与数据融合过程中的残差计算，得到前端里程计位姿，最后将因子介入到因子图优化模型中进行融合定位求解。子介入到因子图优化模型中进行融合定位求解。子介入到因子图优化模型中进行融合定位求解。


技术研发人员：熊模英 兰凤崇 陈吉清
受保护的技术使用者：华南理工大学
技术研发日：2023.04.18
技术公布日：2023/8/21