煤矿移动机器人LiDAR/IMU紧耦合SLAM方法

李猛钢，胡而已，朱华

（1.中国矿业大学 机电工程学院，江苏 徐州 221116；2.江苏省矿山智能采掘装备协同创新中心，江苏 徐州 221008；3.中国科学院沈阳自动化研究所 机器人学国家重点实验室，辽宁 沈阳 110016；4.应急管理部 信息研究院，北京 100029）

0 引言

煤炭是我国的主要能源和重要战略资源。目前我国煤矿智能化水平较低，井下作业人员仍然较多、安全风险大，加快煤炭企业对煤矿机器人的研发和应用是破解当前安全生产矛盾、实现减人增效的重要途径和现实需求。对于各类煤矿移动机器人，精确定位与地图构建是其推广应用亟待实现的共性关键技术[1]。同步定位与地图构建（Simultaneous Localization and Mapping，SLAM）具有在定位的同时实时重构场景模型的功能，为辅助驾驶、自主导航等提供定位建图功能，近10 a来得到了长足的发展并已逐渐走向工业应用领域。

在煤矿井下SLAM方法方面，国外较早开展了相关研究，但主要采用手持设备[2]、移动小车[3]、探测机器人[4]、矿车[5]搭载激光雷达及惯性测量单元（Inertial Measurement Unit，IMU）等传感器进行环境建模。D.Tardioli等[6]分析了其所在课题组多年来在地下隧道机器人领域开展的相关研究进展，主要探讨了基于激光雷达、视觉相机、里程计等传感器的SLAM方法在隧道环境的应用。美国国防部高级研究计划局（Defense Advanced Research Projects Agency，DARPA）于2018−2021年举办的SubT赛事面向城市地下空间、隧道和天然洞穴环境下的机器人应用，相关研究针对光照变化[7]、场景退化[8]及巷道特征利用[9]等地下场景SLAM中的典型问题提出了新的解决方案。国内方面，马宏伟等[10]提出了基于深度相机进行井下定位建图实现移动机器人自主导航的方案。陈先中等[11]提出了一种煤矿地下毫米波雷达点云成像、结合深度学习处理稀疏点云的SLAM方法构想。杨健健等[12]研究了基于Hector−SLAM的掘进机器人巷道环境感知方法。Li Menggang等[13]提出了一种基于激光雷达的NDT（正态分布变换）−Graph SLAM方法，利用平面约束减轻地图漂移，用于煤矿救援机器人的高效建图任务。高士岗等[14]在国家能源神东煤炭榆家梁煤矿初步应用了搭载Exscan激光扫描仪的巡检机器人，基于SLAM技术构建了动态数字化工作面，三维点云模型实测精度为0.2 m。总体来看，针对煤矿机器人SLAM的研究集中于解决激光雷达、相机、毫米波雷达等传感器在特定场景、特定条件下的初步应用和性能改善问题，对井下多种条件耦合的复杂环境长期定位建模技术仍有待进一步攻关。

在三维激光SLAM研究方面，以迭代最近点（Iterative Closest Point，ICP）为代表的扫描匹配方法研究较早[15]。6D SLAM[16]的前端利用大量原始点云进行ICP配准，后端使用全局松弛化，实现了非实时的高精地图构建。与使用全部点云不同，为了减少计算量、提高算法精度，另一类基于特征的方法被提出。Zhang Ji等[17]提出了一种激光里程计与建图（LiDAR Odometry and Mapping,LOAM）方法，通过计算曲率分割边点和面点，构建点−线和点−面优化问题，是目前诸多激光里程计采用的方法，但由于缺乏回环检测，LOAM方法在长距离应用中仍然有较大的漂移。Shan Tixiao等[18]基于LOAM方法提出LeGO−LOAM（Light Weight and Ground−optimized LOAM）方法，采用了与LOAM方法同样的特征提取方法，通过深度图像分割剔除地面的干扰。J.Weingartent等[19]和A.J.B.Trevor等[20]均提出构建平面特征进行数据关联，后端分别基于扩展卡尔曼滤波（Extended Kalman Filter，EKF）和图优化构建SLAM。此类方法必须要求环境中有大量可提取的平面，形成稀疏平面特征地图。

在激光−惯性融合定位建图方面，目前主要分为松耦合和紧耦合2种方式。松耦合通过构建惯性导航独立进行状态传播，利用激光进行观测更新。紧耦合方法近年来逐渐得到关注。Ye Haoyang等[21]提出了一种雷达惯性里程计与建图（Lidar Inertial Odometry and Mapping，LIO−mapping）方法，使用面特征构建相邻帧之间的点−面约束，同时构建IMU预积分约束，基于因子图联合优化所有帧间观测，在特征较多的大场景条件下难以满足实时运行需求。Shan Tixiao等[22]提出了一种基于平滑建图的雷达惯性里程计（Lidar Inertial Odometry via Smoothing and Mapping，LIO−SAM）方法，前端同样采用LOAM进行特征提取，后端利用因子图框架融合IMU，取得了较好的效果。除了优化方案，滤波方法也被用于激光惯性传感器融合中。Qin Chao等[23]提出的用于鲁棒和高效导航的雷达惯性状态估计器（Lidar−Inertial State Estimator for Robust and Efficient Navigation，LINS）和Xu Wei等[24]提出的快速雷达惯性里程计（Fast Lidar Inertial Odometry,Fast−LIO）系列方法，基于迭代扩展卡尔曼滤波（Iterative Extended Kalman Filter，IEKF）实现激光和IMU紧耦合，在观测更新时采用了类似优化方法的迭代过程，但没有使用历史数据，在复杂场景中的精度和鲁棒性较差。杨林等[25]提出了激光惯性融合SLAM方法，前端利用IEKF设计了激光惯性里程计，通过构建关键帧在后端采用位姿图优化进行联合优化，降低了系统累计误差。

煤矿井下机器人SLAM是当前研究热点，但针对提高激光SLAM在井下复杂条件下的精度、鲁棒性的研究仍然不足；传统激光SLAM方法在此类环境下存在累计误差迅速增大、旋转过程鲁棒性差、特征关联错误率高等问题；现有激光−惯性融合的定位建图紧耦合融合机制仍需进一步探讨，以期进一步提高激光−惯性融合SLAM方法对煤矿井下复杂环境的适应能力。为此，本文提出了一种煤矿机器人LiDAR/IMU紧耦合SLAM方法（Lidar−Inertial SLAM，LI−SLAM方法）。基于点−线和点−面扫描匹配构建激光相对位姿约束，融合IMU预积分因子约束和回环因子约束，利用因子图优化实现了LiDAR/IMU紧耦合的SLAM。构建了雷达相对位姿因子约束模型，解析推导了雷达相对位姿约束的残差、雅可比和协方差矩阵；基于因子图模型，构建了激光相对位姿约束与IMU预积分约束的紧耦合数据融合机制，提升了对煤矿井下颠簸路面、机器人剧烈旋转等复杂工况的适应能力。该方法可帮助煤矿移动机器人在井下复杂地形与环境工况中实现鲁棒精确的定位建图，为煤矿移动机器人自主导航提供关键技术支撑。

1 LI−SLAM框架

LI−SLAM框架如图1所示。激光约束构建阶段包括5个步骤：①雷达数据预处理：利用IMU的短时状态估计传播运动状态，校正时段内采集的雷达线扫运动畸变，进行区域滤波等预处理过程。②特征提取：提取点云中的边和面特征。③关键帧与子图构建：提取关键帧并构建局部子图。④ 特征关联：利用特征关联和扫描匹配计算相对位姿变换。⑤雷达相对位姿因子构建：基于帧到子图的扫描匹配建立局部雷达相对位姿因子。完成激光约束构建后，执行激光−惯性紧耦合融合机制：与同时段内惯性预积分因子在滑动窗口中进行联合优化，实现状态估计，利用雷达相对位姿因子限制惯导零偏等参数的漂移，实现紧耦合的联合状态估计，同时利用子图关键帧构建全局地图。迭代执行上述过程，可实现在线激光−惯性紧耦合SLAM。

图1 LI−SLAM方法框架Fig.1 Framework of the LI-SLAM method

定义惯导坐标系B，雷达坐标系L，激光惯性里程计坐标系O。LI−SLAM的因子图模型如图2所示。待估计的变量xOBi是优化窗口内所有的IMU相对于局部坐标系下的位置p、速度v、姿态q、加速度零偏ba（a为加速度）、角速度零偏bω（ω为角速度）及从惯导到雷达坐标系间的外参TBL。从i，i+1，…，j时刻所有的待估计变量xOBi，xOBi+1，…， xOBj组 成的集合为χ：

图2 因子图模型Fig.2 Factor graph model

优化窗中第i时刻对应的状态变量和外参分别表示为

采用子图进行扫描匹配过程。在局部优化过程中，仅滑动窗口以内的各类因子参与优化过程，窗口外的状态利用边缘化技术固定。基于因子图模型的优化目标函数为

2 激光约束构建

2.1 雷达数据预处理

点云畸变在机械式激光雷达高速运动问题中是必须解决的问题。现有手段通常基于匀速运动假设，利用线性插值重新部署激光点云的位置。这种方法对于剧烈的非线性运动适应性差。本文使用高频IMU进行状态传播，以恢复每个点的运动。设定第i时刻和第j时刻对应的雷达观测帧为Si和Sj。对第Si帧与第Sj帧之间的IMU观测进行积分，作为激光扫描匹配的初值。相邻采样时刻间认为加速度和角速度未发生变化，加速度、角速度零偏是固定值。IMU加速度观测值为，角速度为从t（t=i，i+1，…， j）时刻传播的状态包括位置、速度方向。

式中：vWBt为t时刻世界坐标系下的速度；gO为里程计坐标系下的重力加速度；和为t时刻相对于世界坐标系和里程计坐标系的旋转矩阵。

2.2 特征提取

采用LOAM[17]的特征提取方法设计点云粗糙度指标作为线特征和面特征的区分方式。设定某个雷达扫描周期K内点云为PK，点云PK中2个点(PA,PU)∈PK，在雷达坐标系下的位置坐标可以表示为()。s为点云PK中点PA所在的行中的点组成的连续点集，则点PA所在局部曲面的粗糙度为

式中A，U为点云中的2个点对应的序号。

根据以下规则提取特征：①边缘点：取粗糙度C最大的某些个点排序即为曲率较大的边缘点。② 平面点：取粗糙度C最大的某些个点排序即为曲率较小的平面点。③为了使点云特征在雷达坐标系下的各个区域分布尽可能均匀，将每个扫描帧等分为若干区域，每个区域提取2个边点和4个平面点。

2.3 关键帧与子图构建

为处理高频激光观测数据，采用关键帧进行特征匹配和雷达约束的构建。预设最小平移距离阈值、最小旋转角度阈值，若达到其中一个条件，则设置当前帧为关键帧。关键帧位姿变化计算公式可以参考文献[13]。

在前端匹配中，采用帧到子图的方式进行扫描匹配，组合固定数量的关键帧形成子图。设关键帧k的边和面特征集合为Fek和Fhk（e，h分别表示边和面，下同），n个关键帧组成的特征集合为{Fk−n,Fk−n+1···,Fk},Fk=Fek∪Fhk。利用各关键帧的变换可以将特征转换到局部里程计坐标系O下。转换后，拼接关键帧，组成局部子图Mk，其中包含变换到里程计坐标系后的边和面两类特征， Mk={Mke∪Mkh}。采用下采样方法剔除同一个体素栅格中的重复特征，避免冗余的特征匹配。Mke和Mkh的下采样精度根据环境特点、计算量按照经验设置，此处设为0.1 m和0.2 m，子图内关键帧数量为n=20。

2.4 帧到子图的特征关联与扫描匹配

图3 2类特征点数据关联方法Fig.3 Data association method for two kinds of features

至此，构建点到线的距离de：

构建点到面的距离dh：

式中：Xkh+1,I为关键帧k+1对应的面特征I的坐标；为关键帧k对应的面特征A，U，V对应的坐标。

利用帧与子图中的点到线的距离de与点到面的距离dh，可以组成距离矩阵d，基于扫描匹配方法，可以建立以下代价函数 f来求解帧到子图的最优位姿变换关系

利用列文伯格马切尔特（LM）方法可以构建以下优化方程来求解帧间位姿的变换：

式中：J为雅可比矩阵，J=∂f/∂Tk+1；D为系数矩阵；µ为信赖区域半径。

迭代推导出最优估计：

式中λ为阻尼因子。

采用以上列文伯格马切尔特优化方法收敛后可以得到当前位姿估计值Tk+1=。位姿Tk和Tk+1之间的相对位姿变换为

2.5 相对位姿因子构建

利用雷达相对位姿构建关键帧位姿之间的约束关系，在滑窗中联合惯性预积分一起参与优化。相邻位姿间的优化目标项为

式中：x为待优化状态变量；rL为相对位姿因子残差；xi和xj分别为i和j时刻对应的运动状态；Σ为约束不确定度的协方差矩阵， Σ∈R6×6，R为旋转矩阵。由于估计过程是基于IMU坐标系，相对位姿观测需要转换到LiDAR坐标系：

2.5.1 残差计算

从i时刻关键帧到j时刻关键帧之间的位姿变换的观测和期望之间的残差可以表示为

式中rL(χ)为 雷达坐标系下相对于观测zBBij 和待估计变量χ的残差，为从j时刻到i时刻IMU坐标系下的观测。

推导可得相对位姿因子残差的旋转部分rθ和平移部分rξ：

2.5.2 雅可比矩阵计算

（1）向量空间：平移残差向量在向量空间参数块的雅可比矩阵为

（2）流形空间：平移与旋转残差向量在流形空间参数块的雅可比为

2.5.3 协方差计算

由于前端配准采用了扫描到子图的方法，因此可通过计算所有匹配成功特征点来计算整体扫描匹配的协方差。计算当前扫描帧c的特征点u的坐标Xuc在子图w中对应特征点u的坐标Xuw：

所有匹配成功的雷达特征计算的协方差矩阵H为

定义噪声矩阵Λ为

式中σx，σy，σz为x，y，z方向的的噪声西格玛值，受到2个点Xuw和Xuc噪声的影响，参考激光雷达的噪声参数，设置噪声典型值为5 cm。

协方差矩阵对应的信息矩阵Ω为

利用式（22）从初始时刻的06×6矩阵开始迭代计算信息矩阵，随着运动距离增加，信息矩阵的不确定度逐渐增长。

3 IMU预积分约束构建

IMU预积分作为一种高频惯性观测数据处理方法，在视觉惯性里程计、激光惯性里程计中逐渐得到应用，其核心思想是将相邻关键帧之间的大量惯性观测进行集成，在关键帧的局部惯性坐标系下开始积分，获得独立的相对运动约束，与其他约束联合优化，实现完整的状态估计。预积分原理如图4所示，横坐标代表时间戳。预积分过程就是将关键帧So、Si和Sj之间的高频IMU观测独立建立为IMU运动约束。本文采用视觉惯性导航系统（Visual Inertial Navigation System,VINS）的预积分方法对IMU进行积分约束，预积分观测方程、残差、雅可比矩阵和协方差矩阵推导参考文献[26]。

图4 预积分原理Fig.4 Principleof pre-integration

4 回环检测约束构建

回环检测优化对于获得全局一致的地图有很大作用。本文采用文献[13]提出的回环检测方法，通过以下3个步骤实现回环检测与约束构建。

（1）里程判断：当新的关键帧建立后，首先搜索因子图并找到与新关键帧位姿在欧氏距离上接近的关键帧。小于设定距离阈值的历史关键帧将被作为回环候选关键帧。

（2）形貌相似度判断：利用ICP扫描匹配方法匹配当前关键帧的点云，与回环候选关键帧前后设定的某些帧构成局部子图。利用最近点对的均方根最小距离计算拟合率，与设定阈值比较来确定是否为真实回环关键帧。

（3）回环因子构建：利用以上双重判断选择出回环关键帧后，通过式（12）计算相对变换因子，并将相对变换因子添加到全局因子图中。

5 实验分析

5.1 野外颠簸路面试验

为了验证LI−SLAM方法在颠簸路面和复杂场景的精度与鲁棒性，基于轮式移动机器人试验平台在野外开展了试验。机器人搭载VLP−16 LiDAR，MTi−G710 IMU。轮式移动机器人搭载的运算平台配置为i7−8700k，32G内存。基于轮式移动机器人的试验平台和3个试验场地如图5所示。

图5 轮式移动机器人试验平台及野外试验场地Fig.5 Wheeled mobile robot test platform and field testsenvironment

5.1.1 精度分析

为了分析LI−SLAM方法的优越性，将其与当前最优的激光算法LOAM方法、LINS方法、LIO−mapping方法进行了对比。几种方法的轨迹对比及其对齐到卫星地图上的结果如图6−图8所示。可看出：在所有试验场地中，LI−SLAM方法和LOAM方法的地图一致性最好，与真实路线基本吻合，LI−SLAM方法对旋转有更佳的适应能力。LIO−mapping方法无法实时运行，在0.5倍速下可以获得完整轨迹，但在初始运动阶段出现了较大程度的方向偏移，初始化过程容易失败。LINS方法由于仅利用了最新的观测信息，在复杂地形下出现了漂移。

图6 试验场地1中各种方法的运行轨迹Fig.6 Trajectoriesfor different methods in different test scenario 1

图8 试验场地3中各种方法的运行轨迹Fig.8 Trajectoriesfor different methods in different test scenario 3

不同方法在机器人起点、终点间测量距离与真值的误差见表1，测量距离通过图上的坐标差获得，距离真值可以通过全站仪等测量和计算获得。从表1可看出：LI−SLAM方法在试验场地1、试验场地2中误差最小，在试验场地3中与LOAM方法的误差接近，均明显优于其他方法。

图7 试验场地2中各种方法的运行轨迹Fig.7 Trajectoriesfor different methods in different test scenario 2

表1 不同方法起点、终点间距离与真值的误差Table 1 Thedistance and truth valueerror between thestarting point and the end point of different methods

5.1.2 运算速度分析

LI−SLAM方法各个模块耗时的均值和最大值见表2，可看出回环检测模块耗时最长，其次是建图模块。实际应用中，回环检测和地图构建模块本身不需要实时，可以根据环境和需求人为设定。其余模块均满足10 Hz雷达采样频率的实时运行需求。

表2 野外试验时LI−SLAM方法中各模块耗时均值与最大值Table 2 The averageand maximum time consumption of each moduleof LI-SLAM method in field tests ms

5.1.3 回环检测效果分析

在试验场地1中机器人起始和终止位置发生的回环优化前后的效果如图9所示。图9（a）中回环尚未发生，在位置2的树木和道路出现明显的模糊，图9（b）中回环优化后树木辨别更加清晰，获得了全局一致性的地图，证明了回环检测的有效性。

图9 回环优化效果Fig.9 Effect of loop optimization

5.2 地下车库少结构环境试验

为了验证LI−SLAM方法在少结构的井底车场等环境的应用效果，采用自主研发的CUMTV−C煤矿救援机器人平台[27]进行了地下车库的模拟试验，如图10所示，在机器人上方水平和倾斜30°各部署1个16线激光雷达，分别用于定位和构建稠密地图。机器人搭载Xsens MTi−G−710 IMU，用于惯性数据测量。

图10 地下车库环境及机器人平台Fig.10 Underground garage environment and robot platform

LI−SLAM方法的建模效果如图11所示，绿色到红色渐变的球形表示机器人的关键帧位姿，球形之间连线为相对位姿约束与回环约束。从图11可看出，LI−SLAM方法具有较好的建模精度，局部精细化程度高。

图11 LI−SLAM方法在地下车库建模效果Fig.11 Modeling resultsof LI−SLAM method in underground garage

不同方法的运动轨迹如图12所示，由于地面较为平坦，LINS、LOAM、LI−SLAM方法均获得了平滑的运动轨迹，LIO−Mapping方法在初始化时出现了偏移，表明该方法在缺乏足够运动激励时难以获得鲁棒的状态估计。

图12 不同方法轨迹对比Fig.12 Comparison of trajectoriesof different methods

不同方法起点、终点间距离的误差见表3，可看出LOAM和LI−SLAM方法的误差最小，证明了紧耦合方法可以提升定位精度。

表3 不同方法起点、终点间距离的误差Table3 The distance error between the starting point and theend point of different methods

LI−SLAM方法各模块耗时均值与最大值见表4，可以看出：去畸变、特征提取、优化模块均可满足10 Hz运行条件，回环检测与地图构建模块无高频运行需求。

表4 地下车库试验时LI−SLAM方法中各模块耗时均值与最大值Table4 Theaverage and maximum time consumption of each module in LI-SLAM method in underground garage tests ms

5.3 煤矿井下现场工业性测试

2020年11月2日，在晋能控股山西煤业股份有限公司塔山煤矿开展了煤矿救援机器人工业性验证试验，如图13所示。采用自主研发的CUMTV−C煤矿救援机器人平台进行试验[27]，机器人实时运行LI−SLAM方法，通过无线信号传输（带宽大于120 Mbit/s）实时显示构建的地图，用于人员遥控和自主导航。

图13 煤矿井下现场试验Fig.13 Field testsin underground coal mine

机器人在行走过程遇到了各类复杂工况，包括大量水汽、斜坡、岔口、颠簸路面等，如图14所示。搭载LI−SLAM方法的煤矿救援机器人在各类地形环境中均可稳定运行，说明LI−SLAM方法满足鲁棒性、实时性需求。

图14 机器人在井下各类复杂环境中运行情况Fig.14 Running process of the robot in several complex environment in underground coal mine

LI−SLAM方法实际运行中的一组建模结果如图15所示。机器人行驶巷道直线距离为273 m，将地图上的距离作为测量值，利用全站仪测量获得人工标志点的定位结果之间的距离作为真值进行对比，分析30组距离结果表明，平均误差小于15 cm，具有较高的定位和建模精度，基本满足煤矿移动机器人的定位建模精度需求。

图15 煤矿井下建模效果Fig.15 Modeling resultsin underground coal mine

6 结论

（1）提出了一种基于因子图优化框架的LiDAR和IMU紧耦合SLAM（LI−SLAM）方法，基于点−线和点−面扫描匹配构建激光相对位姿约束，融合IMU预积分因子约束和回环因子约束，利用因子图优化实现了LiDAR/IMU紧耦合的SLAM。

（2）详细分析了激光约束的构建过程，设计了点云去畸变、特征提取、关键帧与子图构建、特征关联策略，解析推导了雷达相对位姿因子的残差、雅可比与协方差矩阵。

（3）设计了雷达相对位姿因子、惯性预积分因子、回环检测因子的代价函数，构建了激光惯性里程计因子图紧耦合模型，并设计了无约束优化目标函数。

（4）在野外颠簸路面、地下车库进行了大量测试，在真实煤矿井下现场进行了工业性试验。结果表明：与LOAM方法、LINS方法、LIO−mapping方法相比，LI−SLAM方法在精度、鲁棒性方面表现更佳，对于颠簸、少结构、水汽等复杂煤矿井下环境的移动机器人精确定位与地图构建有更好的适用性，具有较高的定位和建模精度，基本满足煤矿移动机器人的定位与建模精度需求，对井下各类移动机器人的定位建图与自主导航具有较强的指导意义和参考价值。