基于运动预测的改进激光里程计

秦正高向川陈政康录一凡屈凌波

郑州大学电气与信息工程学院2. 河南省先进移动通信与应用工程研究中心3. 郑州大学电子材料与系统国际联合研究中心4. 郑州大学化学学院

1 引言

随着自动驾驶与机器人导航等领域的发展，同时定位与建图（SLAM）算法成为了无人驾驶技术的研究热点。根据使用传感器的不同可将SLAM分为视觉SLAM[1-2]与激光SLAM[3]，其中视觉 SLAM 采用视觉传感器，易受光照等外部因素影响，鲁棒性不高；而基于激光雷达的激光 SLAM 不受光照等环境影响，在实际工程中应用更为广泛。激光 SLAM 可分为前端激光里程计、后端建图两部分,其中前端激光里程计通过点云匹配的位姿变换估计运动轨迹[4]，后端根据运动轨迹完成全局地图构建[5]，因此激光里程计的精度决定了 SLAM 精度。在高速大范围建图场景中，激光雷达随着载体运动产生振荡，点云匹配不可避免地被外部环境影响，在不使用其他传感器进行互补性误差纠正情况下，某帧点云匹配的误差会造成位姿估计偏差累积传播，从而导致运动轨迹与地图漂移。目前处理该问题的主流方法为增设 IMU等传感器进行数据融合提高 SLAM 精度[6]，但是多传感器的外参标定需要消耗更多算力，同时会增加硬件成本[7]。因此仅使用激光雷达完成 SLAM 不仅节省成本而且更易于对不同无人车的适配，如何在不增设传感器的条件下减小误差便成为一项重点研究内容。

点云匹配精度决定激光里程计的精度[8]，点云匹配算法可分为特征法与直接法[9]。常用的特征法包括基于法向对齐径向特征的 NARF[10]，快速点特征直方图 FPFH[11]，基于线面特征的 LOAM[12]和利用移动最小二乘平面的 IMLS[13]等。直接法则以迭代最近点 ICP[14-15]和NDT[16-17]为代表。相比于直接法，特征法更加依赖环境信息，因此鲁棒性较差且计算量大。直接法中的 ICP 通常需要结合特征构成匹配点对计算的旋转矩阵进行匹配，对点云位置要求苛刻，而 NDT 利用点云信息的分布进行匹配，并引入非线性优化法迭代求解相对位姿。综上，NDT 在降低数据规模的同时保留特性信息，效率高、鲁棒性强，这也使得基于 NDT的激光里程计成为主流。然而，基于 NDT 的激光里程计中使用广泛的 hdl_graph_slam 算法[18]，在实际环境下运动轨迹与实际值仍然存在较大偏差。文[19]在 hdl_graph_slam 算法的基础上将当前帧相对前一帧的变化量作为点云匹配的运动预测值输入至 NDT 进行迭代，对运动轨迹漂移问题抑制甚微。针对运动轨迹漂移的问题，本文在单传感器的条件下，基于 NDT点云匹配算法对激光里程计进行改进，降低运动轨迹漂移的同时能够在多种场景快速配置。

本文所做工作如下：（1）基于匀速平滑运动假设，提出了一种连续运动预测算法为点云匹配提供更准确的初始迭代值，抑制了因某帧点云匹配误差产生的累积传播导致的运动轨迹漂移；（2）选取关键帧组成局部地图，在点云匹配时将帧间匹配转换为帧与局部地图匹配，进一步提升了匹配精度。

2 基本原理

激光里程计通过点云匹配估计载体的运动轨迹，点云匹配的准确度决定运动轨迹的准确度。本节介绍基于 NDT 的激光里程计原理，将激光里程计分为点云匹配和位姿预测与转换两部分，分别对 NDT 点云匹配算法和本文位姿转换矩阵的定义与运算进行介绍。

2.1 原始激光里程计

原始激光里程计可分为点云匹配，位姿预测与转换两部分。基于 NDT 的激光里程计，激光点云数据经预处理后输入 NDT 进行点云匹配，原始激光点云数据存在冗余，直接进行匹配会消耗大量的运算资源，为了提高运算效率需对数据进行降采样，在滤除冗余数据的同时保留数据特征信息。点云降采样通常选用点云库 PCL 中的体素滤波 VoxelGrid[20]，点云数据经预处理后输入激光里程计，通过点云匹配迭代至最大收敛次数后输出相对位姿变换矩阵，然后利用当前帧相对上一帧的相对位姿变换预测下一帧的位姿，经位姿推算后将两帧点云的预测变换矩阵输入 NDT 进行新一轮匹配，最终输出运动轨迹。基于 NDT 的激光里程计框架如图 1 所示。