多分辨率去动点的IMU激光雷达定位方法*

杨智宇，陆佳红，杜 力，鞠 浩

(重庆工商大学 制造装备机构设计与控制重庆市重点实验室，重庆 400067)

多模态传感器融合测量在各个领域都起着至关重要的作用。在自动驾驶领域，利用复杂三维立体空间环境信息精准构建三维地图是自动驾驶汽车感知系统的重要基础。同步定位与建图(simultaneous localization and mapping，SLAM)[1]是三维建图中的主流算法。环境感知主要依赖于激光雷达(light detection and ranging，LiDAR)、相机和惯性测量单元(inertial measurement unit，IMU)等传感器实现。虽然基于视觉的SLAM可运用于特征信息缺失的场景识别，但由于初始化、光照和对范围的敏感性使得其在单独用于自主导航系统时并不可靠[2]。相反地，利用LiDAR的方法可以在很大程度上避免光照的影响，并且LiDAR地图相比于通用的视觉特征点加描述子地图有更好的稳定性。

LOAM(LiDAR odometry and mapping in real-time)[3]是激光SLAM领域最经典、应用最广泛的方法。然而,LOAM将点云数据直接存储于全局体素地图，无法执行回环检测以修正漂移。LeGO-LOAM(lightweight and ground-optimized LiDAR odometry and mapping on variable terrain)[4]利用边缘点和平面点建立相邻2次扫描关系，优化后得到位姿变换矩阵，但帧间匹配算法中未能考虑空间移动物体特征点对特征提取和匹配的影响，导致位姿估计精度降低。

事实上，LiDAR在空旷环境等会出现退化现象。通常利用提高精度构建的复杂算法避免环境信息缺失，但却牺牲系统实时性能，最终导致建图累积误差过大的问题[5]。融合IMU能够弥补单LiDAR长时间定位精度和鲁棒性能差的缺陷。然而，两者数据类型不同和复杂的环境信息成为融合SLAM算法的难点[6]。如何降低动态环境中由关键帧匹配误差导致的定位和建图累积误差成为当前LiDAR SLAM的研究热点[7,8]。

针对上述问题，本文创新性地引入了地面平面拟合算法分割地面和利用多分辨率的区域图像去除移动物体的方法，提出了一种高精度自适应多分辨率IMU三维LiDAR SLAM算法。利用地面平面拟合方法分割地面点云，并用自适应多分辨率区域图像的方法去除动态物体特征点，同时构建静态空间约束，用于提高帧间匹配精度，从而提高位姿估计精度。为验证该方法的可行性，在自主搭建的车载平台上，进行了模拟小车和实车测试。

算法整体分为4大部分：

1)数据预处理

预处理包括：计算IMU预积分量及畸变矫正、计算点云去畸变和聚类、实现地面分割。提取IMU前后帧加速度和角速度数据，作为IMU预积分的数据输入计算IMU预积分。在融合算法中，利用LiDAR和IMU之间的外参标定进行迭代优化，并确定旋转变换矩阵。

2)特征提取与匹配

计算曲率标记点云角点、平面点。利用地面平面拟合方法分割地面点和非地面点，高精度处理地面存在坡度情况。提取静态点云线特征构建点云特征匹配估计位姿静态点云约束，增强LiDAR位姿估计精度。

3)位姿估计

通过IMU原始数据计算增量数据，由IMU预积分计算得到的每一时刻IMU增量位姿，并且利用最近一帧LiDAR里程计位姿构建当前时刻IMU里程计位姿。将当前时刻IMU里程计位姿赋给激光里程计，得到当前时刻LiDAR里程计的位姿。因此，构建匹配关系和因子图优化后的位姿共同作用得到当前帧LiDAR位姿。

4)因子图优化[9]

在优化环节，利用优化因子包含LiDAR里程计因子、IMU预积分因子和回环因子的联合优化方案来得到车体的全局一致的位姿。在因子图中输入数据位姿、速度、偏置等数据，根据理论计算模型优化位姿。

算法整体流程框图如图1所示。

图1 算法概述

2.1 点云特征提取

利用前后5个点，计算点距离差之和的平方值作为曲率的度量，依据平滑度区分角点和平面点。曲率计算公式如下

(1)

2.2 平面拟合地面分割

采用平面模型来拟合当前地面。通常，现实生活中地面并非完全平整，而且在测量距离较远时,LiDAR会存在一定的测量噪声，单一的平面模型并不能完整拟合现实复杂的非结构路况[10]。采用平面模型拟合当前地面可处理存在一定坡度的地面情况，实现高精度的地面分割。将空间平面沿车头分为若干个子平面，然后对子平面进行地面平面拟合算法,得到处理陡坡的地面分割方法。构建初始平面模型，如下

ax+by+cz+d=0

(2)

即

nTx=-d

(3)

式中x=[x,y,z]T，代表空间平面坐标系；
n=[a,b,c]T，通过初始点集协方差C∈R3×3矩阵来求解；
d为平面模型常数项。采用种子点集S∈R3作为初始点集，其协方差矩阵为

(4)

计算点云中每个点到平面的正交投影的距离，将该值与设定的阈值比较，当高度小于设定的阈值，则该点属于地面点；
反之，则为非地面点。经分类后将地面点作为下一次迭代的种子点集，迭代优化。

2.3 IMU预积分因子

IMU预积分是基于相邻两帧点云之间的IMU积分测量值计算及其动态更新。通过IMU原始数据计算增量数据，由IMU预积分计算得到每一时刻IMU增量位姿。具体通过获得每周期位置、速度、姿态(position,velocity,quaternion, PVQ)增量的测量值，对照其他通过非IMU方式获得的PVQ增量估计值，进而获得PVQ增量的残差[11]。

根据IMU角速度和加速度测量值，可以算出车体姿态、速度和位置增量

(5)

(6)

(7)

上述表达式即为PVQ增量测量值(含IMU测量值和偏差估计值)与真值之间的关系。提供一种IMU预积分约束，利用IMU零偏实现系统对于位姿计算估计优化。

2.4 区域图像去动态点

本文通过构建多分辨率的区域图像使用投影的区域图像的可见性进行移动点识别。首先，使用Fk+1表示当前关键帧，Mk表示对应的子地图，该子地图由Fk+1周围的滑动窗口创建。将此刻LiDAR点云分成2个互斥的子集，动态点集D和静态点集S。因此，当前时刻子地图和关键帧具体表示为

(·)D∩(·)S=∅

(8)

(9)

(10)

τ=γdist(p)

(11)

系统可将自适应阈值τ设为阈值内的最大值，可有效避免由于错误的分辨率预测而将静态点当成动态点。

2.5 LiDAR里程计因子

如前所述，基于IMU的预积分因子和激光里程计因子，可获得比较准确的状态估计和建图。

本文借助于测试工具EVO进行算法性能测试实验，主要通过精度指标RPE以及ATE对算法性能评估。实验室硬件设备为具有16GBRAM的Intel®CoreTMi7—1070处理器和NVIDIA GeForcen RTK 2060图形处理器的台式机，系统环境为基于Ubuntu18.04的机器人操作系统(robot operation system,ROS)。

使用上述所用设备对校区新北区域场地展开实地算法性能检测实验。分别使用LOAM算法、A-LOAM算法、LeGO-LOAM算法与本文算法进行解析比较。

3.1 硬件平台搭建

融合SLAM系统主要围绕LiDAR和IMU两大传感器展开理论研究和实验测试。LiDAR坐标系和IMU坐标系都使用左手坐标系(右X，前Y，上Z)，其坐标轴对应关系如图2所示。

图2 LiDAR和IMU的坐标轴指向

IMU通过串口将6轴信息传输到台式电脑主机。LiDAR通过统一有线数据端口将点云数据传输给主机进行数据传输。

3.2 数据测试与分析

在实车验证时，首先,需要提前进行内LiDAR和惯导的外参标定。本文使用ROS节点录制LiDAR和惯导联合标定数据包，利用瑞士苏黎世联邦理工大学自动驾驶实验室贡献的标定工具进行外参标定[14]。

其次，编译和运行相关文件。录制相关节点的数据包，利用可视化工具RVIZ实现点云数据可视化,利用pcl工具保存pcd地图，便于实时应用和增强现实存储和处理点云数据集。

3.2.1 KITTI官方数据集验证

通过在不同的官方数据集下，使用本文算法与多种定位和建图方法，计算并比较回到初始原点后的相对平移误差的大小验证算法的精度，如表1所示。

表1 各种方法回到原点后的相对平移误差 m

利用包含LiDAR和惯性里程计的3个官方的数据集West,Outdoor,Walking，多层次，多场景的全面测试在同一数据集下不同建图方法在回到原点的相对平移误差。由表1得知，利用本文算法实时定位建图，回到原点后的相对偏移误差与LeGO-LOAM相比分别减小77.7 %,54.7 %,88.7 %，大幅度提升定位精确度。

3.2.2 实车数据集验证

利用实验室室内和校园场景地图的建立和可视化证明本文算法的鲁棒性和精度;用不同场景和对比其他算法证明本文算法的鲁棒性和精度。

1)自建室内数据集

在室内环境中，构建三维点云地图，如图3所示，从地面平整、漂移、墙面贴合度来看是比较好的。

图3 室内点云建图

2)自建校园数据集

图4为校园高德卫星图和各种方法和本文方法的点云建图结果。可以看出，后两者都有很好的建图效果，且轨迹都与真实轨迹相同。实验结果表明：在大地平整及特征信息丰富的路况下，后两者均有较好的算法效果能够满足实际应用中的精度需求。

图4 重庆工商大学新北区域点云结果

将点云地图通过pcl工具转换为PCD地图，如图5所示。利用实车运行本文算法绕南岸校区新北区域指定范围1圈得到运行轨迹如图5(c)所示。

图5 新北区域点云PCD地图及点云完整轨迹

然后，根据华策组合导航GPS定位得到相应路线的真实轨迹，利用开源LeGO-LOAM算法和本文算法得到的轨迹分别进行比较，如图6所示。

图6 算法轨迹对比

可从3个轨迹对比看出，利用本文算法得到的轨迹图与真实轨迹更接近，定位精度效果更好，并通过表2中与绝对轨迹误差对比得到进一步验证。

表2 绝对轨迹误差

本文实验结果分析表明，使用地面平面拟合方法去地面，使用投影区域图的可见性进行移动点识别并去除动态物体。与传统算法LeGO-LOAM相比，误差的均方根误差减小51.9 %，标准差减少79.8 %，通过提高点云匹配精度从而提高定位的精度。

本文利用地面平面拟合方法分割并标记地面点云，并用区域图的方法去除动态物体特征点，提高帧间匹配精度。通过构建静态空间约束进行位姿估计，提高定位精度。建立帧到局部地图的匹配代替传统的帧到全局地图匹配，提高了帧图匹配的效率。利用因子图联合优化方案来得到车体的全局一致位姿，提高位姿估计精确度。通过官方数据集和校园环境下实车验证表明：相比于传统算法，本文算法计算绝对轨迹误差标准差减小79.8 %，具有更高的精度和鲁棒性，能够有效减少移动物体点云对匹配精度的影响，提高系统定位精度。