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

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

【技术实现步骤摘要】
一种自动驾驶车辆的多传感器融合定位建图方法


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

技术介绍

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

技术实现思路

[0005]本专利技术对现有的自动驾驶车辆在无GPS且无边缘特征场景下车辆的定位建图方法的技术问题，提出一种经济且定位精度满足自动驾驶车辆导航需求的多传感器融合定位方法，涉及一种基于激光雷达、惯性测量单元(IMU)和轮式编码器融合的在低照度、无特征隧道环境中的同时定位与建图(SLAM)算法，可实现在无GPS信号、低照度、无特征隧道环境中提高地面车辆的精准鲁棒定位的精准鲁棒定位；
[0006]本专利技术至少通过如下技术方案之一实现。
[0007]一种自动驾驶车辆的多传感器融合定位建图方法，包括以下步骤：
[0008]步骤1、对车载传感器进行标定；
[0009]步骤2、采集标定的传感器接收的车辆运动过程中实际场景的激光雷达点云数据、IMU的位姿数据、轮速计车轮速度数据，并存储在缓存区中；
[0010]步骤3、利用组合导航系统测量的姿态角初始化IMU的重力方向以消除不同地理位置重力的影响，降低姿态估计误差；
[0011]步骤4、构建IMU预积分模型和车轮预积分模型，以获取IMU预积分和车速预积分；
[0012]步骤5、激光点云预处理：对每一帧点云进行体素滤波，再结合IMU的里程计信息对点云去除畸变；
[0013]步骤6、点云配准与建立局部地图，以得到前端里程计位姿；
[0014]步骤7、当步骤6中的点云配准获得的位姿变化超过阈值时，提取关键帧，并且一个激光扫描的最后一帧也作为一个关键帧；
[0015]步骤8、根据激光雷达获取的三维点云，将点云的空间结构特征用二维特征图表示，并在检测到回环时构建回环检测因子；
[0016]步骤9、构建因子图优化模型，将回环检测因子放入因子图优化模型中进行融合定位。
[0017]进一步地，步骤4包括构建两个激光雷达关键帧之间的IMU预积分因子，预积分项为ΔR
imu
、Δv
imu
、Δp
imu
，其中ΔR
imu
为IMU旋转预积分、Δv
imu
为IMU速度预积分，Δp
imu
为IMU位置预积分；
[0018]利用二自由度车辆模型求解车辆的纵向速度、侧向速度以及横摆角速度构建车轮预积分因子，车轮预积分因子包括车速预积分项ΔR
vehicle
、Δp
vehicle
，其中ΔR
vehicle
为车辆旋转预积分，Δp
vehicle
为车辆位置预积分。
[0019]进一步地，步骤4中，获取IMU预积分和车速预积分具体为：
[0020]IMU预积分：
[0021][0022]车速预积分：
[0023][0024]其中是旋转的预积分测量，δφ
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时刻的位置。
[0025]进一步地，激光点云预处理过程中具体为：采用体素滤波算法对原始激光点进行滤波，将3D点云划分出空间体素格子，在每个体素内，用重心点代替体素内的其他数据点，以提高点云配准的准确率。
[0026]进一步地，点云配准与建立局部地图包括：对提取的关键帧，利用ikd
‑
tree结构的地图结构搜索当前点云帧周围的历史关键帧的集合，然后结合每一个历史关键帧上的全局
位姿信息，构建由ikd
‑
tree数据结构进行维护的局部地图和位姿变换关系。
[0027]进一步地，在构建ikd
‑
tree结构的过程，首先将整个地图分成多个体素，将每个体素内点云的ikd
‑
tree结构建立起来，每个ikd
‑
tree对应一个体素；
[0028]用iEKF进行点云配准与数据融合过程中的残差计算，计算当前点云帧和局部地图之间的位姿变换关系，首先将局部地图中的特征点提取出来，并根据其相对位置构建特征点地图，在接收到新的点云帧后，使用点云配准算法将当前点云帧与特征点地图进行匹配，得到初始位姿估计，基于特征点对位姿进行进一步的优化，以获得更准确的位姿估计。
[0029]进一步地，步骤7中，通过检查位姿变化和时间间隔提取关键帧；
[0030]位姿变化：当相邻激光帧之间的位移或旋转超过预定阈值时，将当前帧作为关键帧；
[0031]时间间隔：设定本文档来自技高网...

【技术保护点】

【技术特征摘要】
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时刻的位...

【专利技术属性】
技术研发人员：熊模英，兰凤崇，陈吉清，
申请(专利权)人：华南理工大学，
类型：发明
国别省市：