[发明专利]一种自动驾驶车辆的多传感器融合定位建图方法

权利要求书

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数据结构进行维护的局部地图和位姿变换关系。