露天煤矿环境下基于LiDAR/IMU 的紧耦合SLAM 算法研究

马宝良 ，崔丽珍 ，李敏超 ，张清宇

（内蒙古科技大学 信息工程学院, 内蒙古 包头 014010）

0 引 言

“双碳”目标下[1]，煤炭依旧为我国主体能源，为响应国家生态文明建设和实现高质量发展，加快推进绿色智能煤矿建设尤为重要[2-3]。煤炭开采环境复杂、条件恶劣，大量挖掘运输设备相互交叉运行在矿场中，操作人员工作强度大、危险系数高，安全问题难以保障[4]。因此，实现开采机械自动化、无人驾驶矿车智能化在煤矿开采中的应用是建设智能矿山的首要举措[5-6]。同时定位与地图构建是实现采矿设备自动化和采矿环境数字化的一种关键技术，该技术能够让机器人在未知的环境中，完成定位、建图和路径规划[7]。

早期的SLAM 使用单一的激光雷达进行定位，在非结构化或复杂恶劣的环境下运行时会造成点云地图稀疏的问题。如REN[8]等在连续帧之间、连续关键帧之间和回环检测帧之间使用GICP[9]配准方法并使用了随机抽样一致性（Random Sample Consensus,RANSAC）对煤矿巷道点云进行了分割，并将分割得到的巷道平面作为了观测约束，实现了在结构化的煤矿巷道环境低漂移定位与建图。惯性测量单元能够不受环境影响，实现高频高精度位姿估计，但其在长时间工作状态下会导致自身误差累积无法精确估计，因此将LiDAR 和IMU 进行融合能够实现在复杂恶劣环境下长时间高精度定位[10]。目前，国内外学者在LiDAR/IMU 融合方面已经进行了大量研究，主要分为松耦合和紧耦合2 大类[11]。对LiDAR 和IMU 的测量数据分别处理后进行加权融合确定运动状态的方法被称为松耦合，如ZHANG等[12]提出了激光里程计与建图（LiDAR Odometry and Mapping，LOAM），该算法使用IMU 计算的姿态作为LiDAR 扫描配准的初始值，而未将其作为全局优化的约束条件。后续对松耦合系统的研究大多延续了LOAM 算法框架，如SHAN 等[13]在LOAM 算法的基础上增加了地面优化，对具有相同类别的特征点进行匹配，提高了融合计算效率，使得特征匹配更稳定。XUE 等[14]在LeGO-LOAM 基础上通过整合扫描上下文回环算法降低了点云维数，提高了在煤矿井下模拟场景下的实时性和鲁棒性。

松耦合算法仅停留在数据结果层面分析，无法解决退化场景问题，为此，紧耦合算法使用图优化或者滤波的方式将LiDAR 与IMU 观测数据融合，充分考虑了两者间的内在约束，系统的鲁棒性和准确性得到提升。YE 等提出的LIO-Mapping[15]使用VINS-Mono[16]中的优化过程来最小化IMU 残差和LiDAR 测量误差，并以一种旋转约束的方法来细化最终的地图，但由于该约束的计算复杂性，系统难以实时运行。SHAN 等[17]提出了基于图优化的LIOSAM 使用划窗的方式来完成帧到局部子图的扫描配准，关键帧的选择以及对旧帧的边缘化有效降低了计算复杂性，提高了系统运行速度。QIN 等基于滤波的LINS[18]提出了一种以机器人为中心，以误差状态进行迭代卡尔曼滤波的融合算法。YANG 等[19]利用扰动模型对煤矿长巷道退化环境进行了检测和补偿，IMU 预积分用于补偿旋转状态退化，LiDAR 与IMU 融合产生的新姿态用于补偿平移状态退化，提高了煤矿巷道退化场景的鲁棒性。XU 等在FAST-LIO[20]中提出了一个新的计算卡尔曼增益公式，显著提高了计算效率。在此基础之上的FASTLIO2[21]提出了一种新的数据结构ikd-tree[22]，能够高效动态地对数据结构进行划分，实现增量更新地图，极大地提高了地图管理效率。

尽管当前SLAM 解决方案众多，但是在露天煤矿环境下使用时的表现却不尽如人意。该环境存在大量斜坡、碎石以及开采后产生的不规则单侧石壁，算法会随运行距离增长产生累计漂移问题，而且大多数SLAM 算法需要提取边缘和平面等几何特征进行配准，均采用LOAM 框架对海量点云进行多次降采样，导致配准结果不准确，影响定位精度以及建图效果。另外，在这样的环境下会出现分段式的相似环境，当无法检测到新的特征时LiDAR 会出现退化现象，一旦配准失败且经过IMU 积分发散，SLAM的定位性能将迅速下降，导致建图失败。综上所述，SLAM 如何在露天煤矿环境下保持系统的鲁棒性和准确性仍是一大难题。为此，提出了一种基于滤波与图优化相结合的紧耦合SLAM 算法，前端使用迭代扩展卡尔曼滤波（Iterater Extended Kalman Filter,IEKF）来实现激光雷达与IMU 数据的融合，后端分别设计了雷达相对位姿因子和回环检测因子来完成全局优化，增强了在露天煤矿环境下对颠簸不平路况的鲁棒性，实现了复杂环境下精确的定位建图，为煤矿设备智能化提供了一种关键技术支撑。

1 系统框架

所提出的算法主要由前端迭代扩展卡尔曼滤波和后端因子图优化两部分组成。该算法使用LiDAR 和IMU 两种传感器作为数据输入信息源。前端使用紧耦合的迭代扩展卡尔曼滤波器将LiDAR 特征点与IMU 数据相融合，使用后向传播来补偿LiDAR 运动失真，经过运动补偿后与该时刻的特征点来构造残差，完成状态更新。后端分别设计了雷达相对位姿因子和回环检测因子，相对位姿因子作用于全局提供新的关位姿与局部子图内关键帧位姿之间的约束，并对当前帧与局部子图进行匹配；回环检测因子同样作用于全局，检测是否与历史位姿重合并对相邻关键帧进行调整来，完成全局优化。最后输出优化后的机器人轨迹，位姿以及全局点云地图，系统结构如图1 所示。

图1 系统结构Fig.1 System framework

2 系统模型描述

使用了文献[23]中定义封装了两个算子⊞和⊟，⊞：S×Rn→S 及其逆⊟：S×S →Rn，其中 S为维数为n的流形（S=S O(3) ）。IMU 坐标系记为I，一般假设IMU 与LiDAR 以外参ITL=(IRL,I pL)刚性连接在一起，则IMU 运动学模型如下：

其中，Δt为IMU 采样时间间隔；f为函数；x为状态；u为输入；w为噪声；定义如下：

3 激光惯性里程计模型构建

使用IEKF 来估计式(2) 中的状态。假设LiDAR 在时刻的扫描最优状态估计值是，则系统的随机误差状态向量为

3.1 状态传播

IMU 测量时，按照式(2)将过程噪声设置为零进行传播得到下式：

传播协方差以迭代误差动态模型来处理，定义如下[19]：

传播一直进行到tk这一帧结束，传播状态以及协方差分别记为和，将白噪声w的协方差定义为Q，则传播协方差可以通过下式计算得出：

当点云累积时间间隔达到tk时，将新产生的特征点集与式(9)中的传播状态和协方差进行融合来产生最优的状态。对于采集一帧点云时间内产生的时间偏差所造成的运动畸变，可以用高于IMU 测量值的后向传播进行补偿。后向传播可以在采样时间ρj以及扫描结束时间tk之间，产生一个相对位姿。该相对位姿可以将局部测量投影到扫描结束时的测量位置：

3.2 状态更新

xk先验分布由前向传播得到：

其中，K为卡尔曼增益，随着状态更新，每个特征点（）将会投影到全局地图中，实现地图更新。

4 全局因子图优化

理论上前端的状态更新后已经可以获得完整的激光雷达惯性里程计，但在实际试验过程中，上述得到的定位与建图结果随着系统运行会产生偏移，并且这种偏移是不可逆的。这个问题在露天煤矿环境下显得尤为突出，图优化的方式可以非常简洁方便的描述各个误差项之间的关联，因此本文后端使用因子图来进行全局位姿图优化，通过对残差模型进行分析，最终目的就是最小化一个残差项。构建了雷达相对位姿和回环检测两个因子来实现全局位姿优化。

1）雷达相对位姿因子。在前端激光雷达惯性里程计中采用了帧到子图的扫描配准方式，并且通过滑动窗口的方式来创建固定关键帧数量的子图。机械式雷达的运行频率一般为10 Hz，相邻帧之间变化量极小但却需要消耗海量的计算资源，将每一帧都添加到因子图中进行优化显然是低效的，因此本文采用了选取关键帧的策略。关键帧的选取主要遵循两个阈值指标进行确立：①最小平移阈值d当系统接收到的当前帧与上一帧的最小平移距离超过d时选取当前帧为关键帧，这样可以避免设备移动缓慢甚至暂停时产生大量冗余激光帧。②最小旋转角 α，当接受到的当前帧与上一帧的姿态旋转角度变化值大于 α时选取该帧为关键帧，由于机械雷达扫描覆盖范围较大，该阈值的设定可避免设备在颠簸路面的微量转动提取到太多的激光点云数据。满足上述任一指标即可确立当前帧为关键帧，利用关键帧来维护一个固定滑窗，即局部子图。接着对新到来的关键帧进行特征提取，利用该帧以及局部子图来构建以下代价函数求解位姿变换关系：

de和dp分别为新一帧提取到的边缘特征和平面特征与其在局部子图中相邻帧提取到对应特征之间的距离，以此构建相邻位姿间的优化目标：

其中，x为待优化量；和分别为第帧和相邻第j帧对应运因子动状态。

2）回环检测。SLAM 系统运行时，各个传感器误差会不断积累，这一误差难以消除并且会逐渐增大，导致定位结果漂移。回环检测因子的使用对于纠正漂移误差以及构建全局一致的地图具有重要作用[25]。此外，在露天煤矿环境下往来矿用卡车基本会走固定路线进行运输，重复运输的路线使得检测到回环的几率大幅增加，因此本文将回环检测因子加入因子图，与雷达相对位姿因子共同进行对全局位姿的优化。

当新的关键帧确立后，首先设定最小距离阈值，对因子图内的全部历史关键帧进行搜索，找到与新关键帧在欧氏距离上满足最小阈值的关键帧，设其为待匹配帧。使用GICP 对新帧与对应待匹配帧进行配准，通过式(19)计算出最近点均方根的最小距离获得置信分数，同时与设定的最小距离阈值进行比较，确认该帧是否为回环关键帧，确定后可利用式(20)计算出该位姿的相对变换并添加到因子图中。该步骤需要严格设置最小距离阈值，一旦回环检测匹配到错误的关键帧就会导致地图崩溃。

5 试验和讨论

为验证文中算法的有效性以及鲁棒性，我们分别在公共开源数据集以及露天煤矿实测数据集上进行了试验。开源数据集选择了M2DGR[26]：一个带有全套传感器的地面机器人对上海交通大学校园内不同场景采集的大规模数据集，所有传感器都经过良好的外部校准和时间同步。露天煤矿实测数据集使用的数据采集设备如图2 所示，该设备主要搭载传感器为速腾聚创RS-LiDAR-16 型激光雷达，10 Hz工作模式下水平分辨率为0.2°，垂直分辨率为2°；超核电子CH110 型9 轴IMU，设置采样频率为400 Hz；北斗星通C200 卫星接收机；机载计算机为NVIDIA Jetson AGX Xavier，8 核ARM 处理器，主频2.26 GHz，内存32 G。提出的SLAM 算法均使用C++实现，运行于Ubuntu20.04，ROS 版本为noetic。

图2 数据采集设备Fig.2 Data collection equipment

5.1 开源数据集试验

使用开源数据集对目前主流激光SLAM 算法：A-LOAM、LeGO-LOAM、FAST-LIO2、LIO-SAM 与本文所提算法进行了测试，通过计算最终绝对位姿误差(Absolute Position Error，APE)来计算各个算法的定位精度。所有算法均在相同配置环境下使用，运行设备为Intel(R) i9-10850K 3.60 GHz。分别选取了3 个不同场景下采集的数据street_08、door_02和gate_02 序列进行试验，使用EVO[27]作为最终结果评估工具。其中street08 序列为户外环境，采集过程中存在大量连续转弯，图3 所示为各个算法在该序列上的定位轨迹对比结果。

图3 street_08 序列各算法轨迹对比Fig.3 Trajectory comparison of each algorithm for the street_08 sequence

从图3 中可以看出A-LOAM、LeGO-LOAM 表现较差，运行一段时间后开始出现明显漂移，LIOSAM 同样存在该问题，但绕行一周后成功检测到了回环，纠正了累计漂移，仍取得较好的结果，FASTLIO2 对新的数据结构的应用可以在不提取特征的情况下可以快速直接地将原始点配准到地图上，在短距离结构化的环境下，精度较高。算法所设计的相对位姿因子有效抑制了IMU 累计误差，同时后期检测到了回环，表现良好。各算法在所选序列试验结果的APE 见表1，其中加粗项表示该项测试中误差项最小的结果，对应箱型图如图4 所示。

表1 开源数据集APE 误差Table 1 Open source dataset APE error

图4 开源数据集APE 误差数据集箱型图Fig.4 Open source dataset APE box plot

5.2 露天矿山实测数据集试验

为进一步验证算法的有效性以及鲁棒性，对内蒙古哈尔乌素露天煤矿进行了数据采集，实地环境如图5 所示。该环境道路崎岖不平，沿途布满山石碎块，呈现分段相似性，且结构复杂无明显的几何特征。在采集的数据集中选取了两个序列进行试验分析，各算法配置均与开源数据集试验配置相同。

图5 露天矿山环境Fig.5 Open pit mine environmental

序列1 全长2 024 m，为实时观测到矿山数据误差累积情况，对序列1 进行了分段切片处理，分别截取为500、1 000 和2 000 m。图6 分别展示了不同算法在两处不同地点的建图效果。

图6 序列1 建图效果Fig.6 Sequence 1 mapping results

从图6 建图效果中能够看出LeGO-LOAM 和FAST-LIO2 存在明显的漂移现象，虽完成了最终建图，但是误差较大。A-LOAM 与LIO-SAM 无直观地漂移现象，但累计定位误差较大。本文所提出的算法完整地构建出了高精度点云地图，对碎石岩壁结构建图清晰，扫描到的矿卡轮廓直观无重影，具有较高的一致性。针对露天煤矿环境体现出了较高的鲁棒性。

序列2 全长2 120 m，各算法运行出的定位轨迹如图7 所示。A-LOAM 和LeGO-LOAM 属于单一激光雷达建图算法，缺少IMU 的辅助出现了较大的累计误差，尤其是轻量化LiDAR 里程计LeGOLOAM 严重依赖地面特征提取，在露天煤矿这样的环境下，大量斜坡的存在导致该算法严重失效，状态估计出现了较大误差。FAST-LIO2 默认不使用特征提取的方法，降采样之后直接将原始点云配准到地图上来体现全局一致性，但是在碎石岩壁与颠簸道路环境中，原始点云经过降采样后呈现高度一致性，导致几处相似场景出现了较大的漂移现象。

图7 序列2 轨迹对比Fig.7 Sequence 2 trajectory comparison result

为避免这些问题，提出的算法将激光雷达帧间配准结果作为约束因子在后端与回环因子进行联合优化，进一步提升了SLAM 系统的鲁棒性，同时提高了定位建图精度。各算法在所选序列试验结果的APE 见表2，其中加粗项表示该项测试中误差项最小的结果，其对应箱型图如图8 所示。

表2 露天煤矿数据集APE 误差Table 2 open pit coal mine dataset APE error

图8 露天煤矿数据集APE 误差箱线图Fig.8 Open pit coal mine dataset APE box plot

6 结 论

1）提出了一种针对露天煤矿复杂环境下具有高精度高鲁棒性的LiDAR/IMU 紧耦合SLAM 算法，前端使用紧耦合的迭代扩展卡尔曼将LiDAR 特征点与IMU 数据融合，后端使用因子图来接收关键帧位姿状态，将激光雷达帧间配准结果作为约束因子并与回环检测因子共同完成全局优化。

2）利用该算法在开源数据集M2DGR 的3 个不同场景和露天煤矿实地环境进行了试验测试，结果表明，算法在开源数据集的城市化环境下精度表现与当前的SOAT 激光SLAM 算法保持一致，在长达两千多米的露天煤矿实地环境下所提算法较FASTLIO2、LIO-SAM 紧耦合算法在定位精度上分别提高了46.00%和23.15%，提高了复杂环境下的定位精度与鲁棒性。