面向矿井无人驾驶的IMU与激光雷达融合SLAM技术

关键词：无人驾驶；同时定位与地图构建；SLAM；激光雷达；惯性测量单元；环境信息辅助；因子图优化

中图分类号：TD67 文献标志码：A

0引言

智能化、无人化运输是提高井工煤矿运输安全性和效率的重要保障[1]。近年来，矿井无人驾驶技术研究主要集中在环境感知、路径规划、自主导航等方面，其中环境感知和自主导航是实现无人驾驶的关键[2]。同时定位与地图构建（SimultaneousLocalization and Mapping，SLAM）技术可使移动机器人在未知环境中实时定位，同时构建环境地图[3]，是无人驾驶、灾后救援等领域的核心技术[4]。然而，矿井生产环境恶劣，地形复杂、巷道狭窄、光照条件差、信号衰减大等因素增加了SLAM 技术在井下应用的难度。

SLAM 技术常用传感器包括激光雷达、摄像头（单目、立体、RGB−D、鱼眼）等[5]。视觉传感器提供的信息更丰富，但对环境照明条件非常敏感，在黑暗区域或纹理较弱的场景中可能无法工作。激光雷达作为一种主动式传感器，不受环境照明的影响，具有分辨率高、抗干扰能力强等优点，常被用作SLAM 首选传感器[6]。但激光雷达存在运动失真、低频更新、稀疏点云等问题。与激光雷达不同，惯性测量单元（Inertial Measurement Unit，IMU）不受结构特征或环境剧烈变化的影响，可在短时间内提供高精度、高频率的姿态估计，但其累计误差随时间漂移[7]。

IMU 与激光雷达融合[8-10]是实现复杂环境中SLAM 技术的重要方法。二者的融合方式主要有松耦合和紧耦合。松耦合是将IMU 提供的运动估计结果作为先验信息，用于辅助激光雷达的SLAM 过程；紧耦合是用IMU 和激光雷达数据互相估计与更新。针对矿井巷道特殊环境，文献[11]提出了IMU 与激光雷达融合的SLAM 方法，通过选取关键帧、采用因子图优化方式对激光雷达和IMU 数据进行整体优化，降低了SLAM 累计误差，但未充分利用巷道环境信息的辅助作用。文献[12]采用回环检测、扫描匹配、IMU 预积分分别构建约束因子图来优化约束因子，完成激光雷达和IMU 的紧耦合，但在狭长巷道内可能不存在机器人的回环，此时定位与建图效果会受到影响。文献[13]在前端加入卡尔曼滤波算法，后端使用位姿图优化算法，实现了激光雷达和IMU耦合，并引入巷道的地面约束，但只考虑地面约束对定位精度和鲁棒性的提高有限。

本文提出一种巷道环境特征辅助的IMU 与激光雷达融合SLAM 算法，用于矿井无人驾驶的高精度定位。该算法由IMU 提供运动状态的实时观测数据，通过激光雷达获取环境中的点云信息，进而提取侧壁和地面信息，结合巷道环境特征来提高SLAM精确性和鲁棒性。

1IMU与激光雷达融合SLAM算法框架

巷道环境特征辅助的IMU 与激光雷达融合SLAM 算法框架如图1 所示。对激光雷达获取的原始点云数据进行去畸变处理，以消除传感器运动而引入的误差；对IMU 观测数据进行预积分，为激光雷达点云匹配提供初值。选取关键帧构成局部地图，对局部地图中的点云进行平面特征提取，通过点云匹配算法与已知局部地图进行匹配，以确定相邻关键帧之间的位姿变换关系。通过激光雷达里程计约束、IMU 预积分和巷道环境约束共同进行因子图优化，限制惯导零偏等参数的漂移，优化位姿变化，实现对无人驾驶车辆运动和环境的精确估计。

2 IMU与激光雷达融合SLAM算法关键技术

3算法性能测试

利用Gazebo 仿真平台对本文算法的性能进行仿真验证，在涵洞和车库2 个场景进行实测验证。

3.1仿真巷道性能测试

在Gazebo 软件中搭建一个长200 m、宽5 m、高3m的巷道模型，其包括直道、弯道、岔口等复杂情况。在巷道中部署一个ScoutV2 轮式机器人模型代替无人驾驶小车，其配备Velodyne VLP−16 激光雷达和9 轴IMU 传感器，用于采集仿真环境数据。部分仿真场景如图4 所示。

小车在平坦地面和侧壁处凸起地面上行驶时采集的点云如图5所示。

通过巷道侧壁信息提取方法对采集的点云进行处理，结果如图6所示。

为验证本文算法的准确性和鲁棒性，将其与应用广泛的LeGO−LOAM[19]， LIO−SAM 算法进行对比。3种算法规划的三维轨迹如图7 所示。可看出本文算法规划的轨迹与真实轨迹的偏差较小， 而LeGO−LOAM算法和LIO−SAM 算法规划的轨迹出现了不同程度的偏差，尤其在运动幅度较大和较长时间后，偏差更明显。

3种算法规划的二维轨迹如图8 所示。可看出本文算法在x，y 轴上的规划轨迹几乎与真实轨迹重合，且预测的巷道长度与真实值差别不大，但LeGO−LOAM 算法和LIO−SAM 算法在定位上有较大偏差。在z 轴方向，LeGO−LOAM，LIO−SAM 算法的定位值漂移较大，特别是1 000 s 后（运动较长时间），漂移更加显著。而本文算法的定位结果虽然也不可避免地存在漂移、误差累计情况，但明显小于其他2 种算法，验证了本文算法在垂直方向上具有强鲁棒性。

3 种算法的绝对轨迹误差（Absolute TrajectoryError， ATE） 和相对轨迹误差（Relative Pose Error，RPE）如图9 所示，具体值见表1、表2。

从图9、表1、表2 可看出，LeGO−LOAM 算法定位误差明显，且误差波动更大，表明其在巷道环境中可靠性较低；LIO−SAM 算法性能优于LeGO−LOAM算法，但定位误差仍有波动，且累计误差随时间增长而增大；本文算法的ATE 和RPE 最低且最稳定，定位精度、稳定性和鲁棒性最优。

3.2真实环境性能测试

在一条长约600 m、宽6 m 的复杂巷道（包含上下坡、大拐角等场景）中对本文算法进行测试，定性分析建图效果。测试场景如图10 所示。

实验平台采用Autolabor 机器人代替无人驾驶车辆，如图11所示。机器人配备hipnuc−CH104M 9 轴IMU 和Velodyne 16线激光雷达。雷达、IMU 的采样频率分别为10，200 Hz。平台搭载的计算机为IntelCore i7，8 个内核，16 GiB 内存。算法用C++编写，用ROS 的melodic 版本在Ubuntu 18.04 系统上运行[20]。

采用LeGO−LOAM 算法、LIO−SAM 算法和本文算法进行SLAM 测试，结果如图12 所示。可看出使用LeGO−LOAM 算法时，巷道边缘呈模糊状态，并伴随大量杂乱点云，表明该算法在巷道环境中具有局限性。LIO−SAM 算法针对长巷道边缘的对齐精度较低。本文算法在巷道边缘建图方面表现优异，即使在大拐角处也能保持良好效果，证明了其在复杂环境中的适应性和有效性。

在与矿井巷道具有相似性的2 个代表性场景中对本文算法进行实测。① 涵洞场景，如图13（a）所示。该场景具有地面坑洼、侧壁粗糙等特点，用于测试算法在不平整地面和复杂壁面环境中的表现。② 地下车库场景，如图13（b）所示。该场景存在较多的车辆和其他障碍物，用于测试算法在复杂动态环境中的定位与建图能力。

在涵洞和地下车库测试场景中分别进行多次实验，在不同路径和不同障碍物分布情况下，本文算法的SLAM 结果如图14 所示。可看出本文算法在涵洞和地下车库场景中均表现出良好的适应性和鲁棒性。在涵洞场景中，小车在地面坑洼和侧壁粗糙条件下能准确定位并建图，且侧壁为一条直线，没有出现弯曲、扭曲效果。在地下车库较多障碍物的情况下，本文算法能实现准确定位和建图，效果良好，未出现拖尾、漂移现象。

4结论

1） 巷道环境特征辅助的IMU 与激光雷达融合SLAM 算法通过IMU 预积分校正、侧壁和地面平面信息提取、因子图优化等，实现复杂矿井环境的高精度定位与建图。

2） 仿真及真实环境测试结果表明，该算法的定位精度和鲁棒性较常用的LeGO−LOAM、LIO−SAM算法有明显提升，在复杂巷道、涵洞、地下车库场景中均具有较好的建图效果，未出现漂移和拖尾现象，定位精度和鲁棒性较高。

3） 今后将进一步提升算法的计算效率，并对SLAM 框架进行优化扩展，以适应更复杂的环境和任务需求。