面向煤矿巷道环境的LiDAR与IMU融合定位与建图方法

马艾强，姚顽强，蔺小虎，张联队，郑俊良，武谋达，杨鑫

（1.西安科技大学 测绘科学与技术学院，陕西 西安 710054；2.陕西彬长矿业集团有限公司，陕西 咸阳 712000）

0 引言

煤矿事故发生后，井下存在不确定和危险因素，依靠人工救援成功率低，煤矿井下移动机器人可替代救援人员深入矿井探测灾害现场情况，检查事故现场，并在第一时间将获得的环境信息传输到指挥中心，制定救援策略[1]，以提高救援效率[2]。然而，传统的导线测量[3]、贯通设计、三维激光扫描等技术方案测量速度慢、效率低，严重制约了煤矿井下环境建模，且实施过程中难以保证建模精度[4]。近年来，激光雷达（Light Detection and Ranging，LiDAR）、惯性测量单元（Inertial Measurement Unit，IMU）和全球导航 卫 星 系 统（Global Navigation Satellite System，GNSS）[5]的移动激光雷达平台被广泛用于获取三维地理信息数据[6]。煤矿井下环境复杂，无GNSS信号，工况恶劣，地面常规定位与建图技术无法直接应用，因此采用同时定位与地图构建（Simultaneous Localization and Mapping，SLAM）[7]技术辅助煤矿井下移动机器人执行路径规划、自主探索、自主导航任务[8]。基于SLAM技术矿井机器人可以实时准确地构建煤矿井下三维地图，该地图可以灵活、可靠地辅助矿井机器人进行智能导航和避障，并应用于矿井下危险区作业、自动巡检、远程调度等领域，对实现智能化定位导航起着至关重要作用[9-10]。然而，在煤矿井下喷浆表面、对称巷道环境中，单一传感器的SLAM技术已不能满足智能感知精度及可靠性的需求。因此，多源传感器融合的SLAM技术应用在煤矿井下成为了当前学术研究与工程应用的聚焦点。

目前，LiDAR和IMU的融合方法主要分为松耦合和紧耦合2种方法。松耦合类方法[11]分别处理LiDAR和IMU的位姿估计，然后再进行融合。文献[12]提出了利用IMU信息来辅助估计激光里程计，IMU积分状态为激光点云匹配提供位姿初值，该方法缺乏回环抑制漂移误差。紧耦合类方法[13]对LiDAR和IMU的数据进行互相估计与更新。文献[14]将IMU预积分因子、激光里程计因子、GNSS因子和闭环因子进行联合优化，取得了较好的建图效果，但并未考虑煤矿环境对称巷道退化对SLAM结果的影响。文献[15]提出用误差卡尔曼滤波对IMU进行预测，并与LiDAR进行紧融合的方法，该方法用激光雷达高程图和先验数字高程图进行匹配，并用匹配值对状态进行更新，该方法展示了在长距离无GNSS信号场景下的导航能力，但需要先验地图，无法应对无先验地图的场景。文献[16]提出了一种基于因子图的LiDAR和IMU紧耦合算法，通过最小化IMU预积分和LiDAR点云匹配结果的残差来优化LiDAR位姿和IMU偏置，根据IMU的预积分值对LiDAR位姿进行线性插值，去除点云畸变，实验表明该方法可应对LiDAR退化的场景和快速运动的情况，但效率较低，实时性较差，而且基于匀速假设的去畸变策略无法适用于运动变化较快的场景。文献[17-18]提出了一种紧耦合算法，在畸变去除部分，该算法使用高斯过程回归对IMU预积分测量值进行采样，利用采样值对LiDAR位姿进行插值，完成畸变去除，取得了很好效果，但该算法计算效率较低。文献[19]提出了一种MC2SLAM的3D LiDAR和IMU紧耦合里程计算法，该算法使用因子图融合3D LiDAR和IMU，在畸变去除部分，使用一种基于匀速假设的非刚性点云匹配算法，通过最小化点云匹配的残差优化LiDAR位姿，再对优化得到的位姿进行线性插值，但是该算法的特征点选取策略过于简单且存在随机性，精度不够稳定。

为解决发生煤矿事故后救援机器人难以自主导航定位的问题，本文提出了一种面向煤矿巷道环境的LiDAR与IMU融合的实时定位与建图方法，用于探测矿井灾害现场情况，提高煤矿事故救援效率。该方法通过数据预处理、点云扫描匹配和后端因子图优化等多层次的数据融合，降低轨迹漂移，增强救援机器人系统在煤矿巷道环境中快速移动时的鲁棒性，实现高精度定位与建图。

1 SLAM基本框架

SLAM基本框架由预处理、激光里程计、地图构建3个部分组成，如图1所示。首先对原始点云进行分割，利用IMU预积分位姿去除原始点云非线性运动畸变，并对得到的点云进行线、面特征提取。然后将相邻帧的线、面特征进行匹配，在分层位姿估计过程中融合IMU预积分所得到的位姿初值，减少计算迭代次数，提高特征点匹配的精度，解算出当前帧的位姿。最后向因子图中插入局部地图因子、IMU因子、关键帧因子，对位姿进行优化约束，对关键帧与局部地图进行匹配，通过八叉树结构实现地图构建。

图1 SLAM基本框架Fig.1 SLAM basic framework

2 基于特征点匹配的相对位姿估计

2.1 预处理

在实际作业环境中，搭载LiDAR的移动机器人不可避免产生非线性运动，导致扫描的激光点云存在非线性运动畸变，因此，利用高频率的IMU状态对激光点进行补偿校正[16]。因为煤矿巷道地面特征点和墙壁特征点对于垂直方向有一定的约束，且平面特征对水平方向的平移和旋转有较好的约束，所以先用基于随机样本一致性（Random Sample Consensus，RANSAC）[20]的快速点云分割算法提取环境中的巷道地面点和墙壁的平面特征。特征提取部分利用深度图计算某一点的曲率，将曲率较大的非地面点标记为线特征点，将曲率较小的非地面点标记为平面特征点[21]。

2.2 激光里程计

在提取特征的前后帧点云中，首先利用KDtree[22]搜索方法寻找最近邻相似属性的特征点，并将法向和主方向夹角均小于5°且距离小于1 cm的特征点作为同一类特征点。建立点到线和点到面距离最小的约束，进行分层位姿估计，如图2所示。由于煤矿巷道的面特征比线特征多，占据位姿权重较大，数据可信度高，因此，选择源点云面特征和目标点云面特征点去对应匹配位姿，确定垂向平移量、横滚角、俯仰角，得出一个较准确的位姿。再根据源点云线特征和目标点云线特征匹配确定侧向平移量、前向平移量和航向角，从而达到整体优化位姿精度。

图2 分层位姿估计Fig.2 Hierarchical pose estimation

3 多因子联合图优化及地图构建

3.1 关键帧因子提取

为确保本文算法在煤矿井下实时运行，在前端里程计中部署了关键帧选取策略，T1—T6为连续帧，∆T为连续帧T1—T6之间的位姿变化，选择T1和T6为关键帧，如图3所示。

图3 关键帧选取Fig.3 Key frames selection

关键帧的选取可极大地提高计算效率，确保点云能够快速匹配。选取关键帧的标准是给欧氏距离∆S 、关键帧旋转矩阵∆R、时间间隔∆t设定一个阈值，任意满足其一，则当前第k帧点云被选取为候选关键帧。选择关键帧时，必须减少匹配错误帧和冗余关键帧，以达到节省计算量的目的。在保证精度的前提下减少数据冗余，以提高运算效率。因此，基于实时点云关键帧选择标准，将第1帧点云作为判断准则开始选择关键帧。连续关键帧的相对变换位姿为

式中：∆Tk−1,k为第 k−1帧到第k帧关键帧的位姿变化；Tk−1为 第k−1帧 的关键帧；Tk为第 k 帧的关键帧；∆q为第 k−1帧 到第 k 帧关键帧的旋转矩阵；∆d，∆e，∆f分别为在X轴、Y轴、Z轴的位移变化；tk为第k帧关键帧的时间戳。

在确定关键帧后，再以初始相对变换位姿优化点云配准，最终变换的位姿应用于位姿和位姿图中位置之间的约束。

3.2 多因子位姿优化及地图构建

假设LiDAR第i（i=1,2，…，j，j为关键帧总次数）帧关键帧的扫描起始时刻为ti，扫描得到的全部点云为Pi；IMU在内采集的数据为I(i,j)。系统在ti时刻的姿态、位置、速度和IMU零点偏置为式中：xi为关键帧在ti时刻的状态；Ri为李群表示的旋转矩阵；vi为速度；bi为零点偏差。

设在tk时刻的所有关键帧为Kk，其对应的状态量其对应的所有观测量

在给定观测量Zk和先验信息 p(χ0)的条件下，预测关键帧Kk对 应的初始状态量χ0的后验概率问题为状态估计问题。

变量因子最大后验概率为

式中：χt为t时刻的状态量；r为观测模型与实际观测的残差，是关于状态量χk的函数；Kt为t时刻的关键帧；Γ为对应的协方差矩阵。

根据IMU动力学模型，采用离散化积分方法对加速度和角速度在IMU采样间隔时间∆t内积分，将高频输出的加速度和角速度观测量转换为状态量间的位姿变换，构成关键帧状态量之间的约束因子。

式中：∆Rij，∆vij，∆sij分 别为在∆t 内第i ～j帧关键帧之间的旋转矩阵、速度、位移；Rj为第j帧关键帧的旋转矩阵；vi，vj分别为第i，j帧关键帧的速度；∆Rik为∆t 内第i～k帧关键帧之间的旋转矩阵；分别为第k帧关键帧的角速度、加速度；bak， bgk分别为IMU的零偏加速度与零偏角速度；si，sj分别为第i，j帧关键帧的位移；g为重力加速度；η为观测噪声。

假设关键帧在状态xi与xj的IMU零点偏置保持不变，则可由式（9）−式（11）得预积分观测模型。

由式（12）−式（14）可推导出残差项 rI(i,j)。

多因子图优化模块是在系统中维护一个全局因子图，因子图结构如图4所示。设给定初始状态为x0，采用数据预处理模块计算相邻关键帧之间的IMU预积分，并向因子图插入IMU预积分因子rI(i,j)。

图4 多因子图优化Fig.4 Multi-factor graph optimization

式中mj为第j帧的关键帧局部地图。

在因子图中插入关键帧因子rp(i,j)。

当提取一个连续帧作为关键帧时，根据连续关键帧的位姿估计，将关键帧点云匹配进行地图的更新。首先利用线、面特征匹配求得LiDAR坐标中连续关键帧之间的位姿，然后通过坐标变换更新关键帧位姿，并将其点云转换到全局坐标系中，最后通过八叉树结构对点云图进行更新。

4 实验结果和分析

4.1 实验设备及数据介绍

为了测试本文所提方法性能，自主搭建Autolabor 、VLP-16 LiDAR和Ellipse-N IMU的实验平台，如图5所示。本次实验中LiDAR频率为10 Hz，IMU频率为200 Hz，2个传感器采用硬同步方式进行时间对齐。煤矿井下移动机器人可进行原地旋转，最大旋转角速度为0.523 5 rad/s，最大位移速度为1.5 m/s。移动机器人系统采用C++实现，在机器人操作系统（Robot operation system，ROS）中运行。利用全站仪记录移动机器人经过参考点时的绝对位置，定量分析移动机器人位姿精度。实验所采集的数据分别有煤矿巷道和楼道数据，具体信息见表1。

图5 实验平台Fig.5 Experimental platform

表1 实测数据Table1 Measured data

4.2 实验结果分析

为验证本文所提方法的定位性能与建图效果，在煤矿巷道A和楼道走廊B中选取起点和终点相同的闭环环境，定量计算各方案的累计位姿误差，定性评价各方案的建图效果及起点和终点的激光里程计轨迹闭合情况。

为了评价不同方法的绝对定位精度，对煤矿巷道场景A所示的10个控制点（A0−A9）和楼道走廊场景B所示的6个控制点（B0−B5）的坐标真值（图6）进行测量。分别与目前主流的LeGO−LOAM，LIO−SAM方法进行对比。通过全站仪测量真值，并记录移动机器人在对应控制点位置停止区间时刻的起点和终点，将其时间段内的位置估计结果求取均值，作为当前方法在该点的观测值。全站仪实测数据作为判断移动机器人定位精度的依据。

图6 场景A（参考点A0−A9）和场景B（参考点B0−B5）的坐标真值Fig.6 Coordinate ture value of scenario A （reference points A0-A9）and scenario B（reference points B0-B5）

4.2.1 定位精度分析

为分析移动机器人在运动过程中的定位精度，给移动机器人粘贴标志点，分别在场景A（A0−A9）、场景B（B0−B5）处，使用全站仪测量其真值，并对3种方法绝对定位误差分布效果进行分析，如图7所示。在场景A中可看出，LeGO−LOAM方法定位精度较差，在X方向上最大绝对定位误差为300 cm；LIO−SAM方法定位精度虽高于LeGO−LOAM方法，但在三轴方向的绝对定位误差在50 cm附近；本文方法在三轴方向的绝对定位误差的均值和中值均小于32 cm。这是因为点云匹配之后进行了分层位姿估计，多因子优化可有效降低全局累计误差，对轨迹精度和地图的一致性的提升具有重要作用。在场景B中可看出，LeGO−LOAM方法在Z轴的误差均值为50～55 cm，X、Y轴误差均值为30～35 cm；LIO−SAM方法三轴误差均值为15～25 cm；本文方法三轴的误差均值为5～15 cm，误差范围小，精度高。

图7 场景A、B的绝对定位误差分布Fig.7 Absolute positioning error distribution of scenarios A and B

3种方法的累计误差见表2。在场景A中可看出，LeGO−LOAM方法在狭小的煤矿巷道点云匹配发生退化，造成三轴的累计误差很大，位置偏差达到49.16 m，使得建图效果发生了漂移；本文方法和LIO−SAM方法的定位精度相对较高，而本文方法对X轴的位姿估计精度最高，其累计误差为1.65 m，位置偏差为2.97 m，从三轴或累计误差来看，本文方法建图效果整体良好，建图轨迹未发生漂移。在场景B中可看出，LeGO−LOAM方法在X、Y轴的累计误差均小于1.5 m，Z轴误差达到了2.33 m，位置累计偏差为2.95 m；LIO−SAM方法在X、Y轴的累计误差分别为2.21，3.15 m，Z轴误差仅为1.05 m，位置偏差为3.99 m；本文方法在X、Y、Z轴的误差均小于1.01 m，累计位置偏差仅为1.67 m。综合分析得出，在较少特征的楼道中，LeGO−LOAM方法与LIO−SAM方法点云匹配时发生退化，导致X、Y、Z轴的平移误差较大，点云模型相差较大，建图定位精度较差。本文方法应用于楼道退化场景依旧有较强的鲁棒性，定位精度高，有良好的建图效果。

表2 3种方法的累计误差Table 2 Cumulative error of there methods

4.2.2 建图效果分析

为了进一步说明3种方法在煤矿巷道和楼道定位的建图效果，分别在煤矿巷道与楼道环境进行验证，建图结果与轨迹如图8和图9所示。

图8 煤矿巷道场景A 中3种方法建图结果与轨迹Fig.8 Mapping resultsand tracksof three methods in coal mine roadway scenarios A

图9 楼道走廊场景B中3种方法建图结果与轨迹Fig.9 Mapping results and tracks of three methods in corridor scenarios B

由图8可看出，LeGO−LOAM方法由于点云误匹配出现大的漂移现象，建图失效；LIO−SAM方法可以构建出大致轮廓的点云地图，但由于Z轴上出现整体明显漂移，导致移动机器人所生成的点云地图结构变形且不完整；本文方法构建的点云地图，在完整性和几何结构真实性方面均有着优秀的表现，可以直观地反映巷道环境的实际情况，在煤矿井下环境具有良好的鲁棒性。

由图9可看出，采用LeGO−LOAM方法进行建图时，轨迹重复，整体发生偏移，移动机器人在回到终点时，轨迹无法闭合，建图失效；采用LIO−SAM方法进行建图时，随着激光里程计里程的增加，由于部分场景相似，导致建立了错误的回环约束，发生漂移，以致点云地图构建失败；采用本文方法进行建图时，地图完整性与环境匹配均有良好的性能，这是由于通过增加关键帧因子，插入因子图对其新增节点相关变量进行优化，降低了位姿估计漂移，定位与建图精度相对较高。

5 结论

（1）提出了一种LiDAR与IMU融合的实时定位与建图方法。该方法首先采用面特征匹配估计位姿，然后进行线特征匹配，实现激光序列帧的相对位姿估计，提高点云匹配精度，最后在因子图优化与地图构建过程中插入关键帧因子、IMU预积分因子、局部地图配准因子，对LiDAR和IMU数据进行整体优化，提高了实时定位与建图的精度，降低了煤矿环境下SLAM的累计误差。

（2）该方法在自采数据集上分别进行了定量定性分析，并与现有的LeGO−LOAM，LIO−SAM方法进行比较。结果表明：①在煤矿巷道环境中，本文方法三轴的绝对定位误差的均值和中值均小于32 cm；X轴的位姿估计精度最高，其累计误差为1.65 m，位置偏差为2.97 m，建图效果整体良好，建图轨迹未发生漂移；构建的点云地图，在完整性和几何结构真实性方面均有着优秀的表现，可以直观地反映巷道环境的实际情况，具有良好的鲁棒性。②在楼道走廊环境中，本文方法三轴的误差均小于1.01 m，误差均值为5～15 cm，误差范围小，精度高；累计位置偏差仅为1.67 m；地图完整性与环境匹配均有良好的性能。