基于激光惯性融合的煤矿井下移动机器人SLAM算法

杨 林，马宏伟，王 岩

(1.西安科技大学 机械工程学院，陕西 西安 710054； 2.陕西省矿山机电装备智能监测重点实验室，陕西 西安 710054)

同步定位和建图(SLAM)是实现煤矿井下机器人自主导航的基础，而机器人利用各种传感器，在三维空间中进行6DOF状态估计，是实现高性能同步定位和建图(SLAM)的关键。虽然基于激光雷达的方法可实时获得三维空间的精确场景测距信息，且对环境照明条件的不变性。但是，由于激光雷达测量存在运动失真、低频更新和点云稀疏的问题，这些特性决定了纯激光雷达不利于机器人处理激进的移动或重复的结构，如煤矿巷道或狭窄走廊。值得注意的是，激光雷达的缺陷可以通过融合IMU来弥补。与激光雷达不同，IMU不受结构特征和环境的剧烈变化影响，并可以提供高频率和短时间的高精度位姿估计，但由于IMU传感器的噪声和偏置，以及误差累积会随着时间的积累而漂移。为了克服独立传感器的这些缺点，可采用多传感器融合的方法来提高状态估计的可靠性。

多传感器融合的相关工作，根据不同的融合框架被分为：松耦合和紧耦合。基于松耦合的融合方法通常分别处理各传感器输入的测量，并对其测量结果进行加权融合，以推断机器人的运动状态。LOAM和LeGO-LOAM是松耦合系统，IMU测量通常用于减轻无特征环境中激光退化的问题，只以IMU计算出的姿态作为激光雷达扫描配准的初始估计值和运动失真校正的先验，而不作为全局优化的约束条件。为了提高自主机器人的定位精度，在LIO松耦合框架中融入轮速计，利用轮速计测量的速度和距离建立了EKF的观测方程。Cartographer是基于图优化实现了激光雷达、IMU、里程计和GNSS的融合，灵活处理扫描匹配和IMU联合约束以及其他观测约束。一般来说，松耦合的方法计算效率高，实现也相对灵活。然而，由于忽略了IMU内部状态的修正，它们很容易受到噪声的干扰。与松耦合方法不同，紧耦合的方法通常会将激光雷达的原始特征点与IMU数据相融合，表现出较高的鲁棒性和准确性，并可分为基于优化和基于滤波的方法。对于基于优化的方法，LIO-mapping引入了IMU预积分，最小化IMU和激光雷达测量的残差，并且可以通过回环检测来纠正长时间的漂移，取得了较好的精度，而该算法的实时应用则较耗时。作为目前最先进的算法之一，LIO-SAM通过引入激光雷达关键帧的滑动窗口来限制计算复杂度，利用因子图进行联合IMU和激光雷达约束优化，以获得更高的精度。它是一种基于因子图的融合框架，适用于多传感器融合。而基于滤波的方法，经常使用扩展卡尔曼滤波(EKF)。LINS提出了一种以机器人为中心的激光雷达惯性估计器，通过使用误差状态迭代卡尔曼滤波器。该方法虽然具有较高的精度，计算效率高，但在长期导航过程中也存在漂移问题。为了减少线性化导致的估计误差对系统带来的影响，R-LINS，FAST-LIO和FAST-LIO2都采用了迭代卡尔曼滤波器。与R-LINS和FAST-LIO相比，FAST-LIO2提出了一种新的数据结构ikd-Tree来保存地图，它可以在不提取特征的情况下直接将原始点注册到地图上，使算法的工作速度更快、更准确。

尽管三维激光雷达性能优越，但结合煤矿井下的特殊环境存在空间非结构化、环境复杂、光照不均匀、空间狭窄等实际情况，此类场景给激光SLAM带来了新的挑战：① 激光雷达在环境中识别到的几何特征，通常是用数学方法人工定义的特征(例如点、线、面)，当机器人在无几何特征、环境相似、对称结构的环境中运行时，基于激光雷达的方案很容易特征退化。② 激光雷达测量到的数据会在某一维度上呈现高度的相似，主要原因是煤矿巷道环境中的侧壁，只能提供横向定位参考，而不能提供纵向定位参考。在这种情况下，侧壁的特征匹配很容易导致错误的姿态约束估计。③ 重复的激光特征会导致纵向上的误导性约束和姿态漂移，以致点云配准收敛至错误结果，定位性能会快速劣化或完全失效。严重地影响了SLAM整体的性能，也限制了机器人在此类特殊环境下自主导航的能力。

综上所述，机器人在煤矿井下如何保证SLAM系统的估计精度、实时性、累计误差和鲁棒性之间的权衡，仍然是LIO面临的一大难题。因此，本文提出了一种以LiDAR和IMU两种传感器作为输入信息源基于紧耦合的激光惯性融合SLAM算法，并围绕其展开研究工作具有重要意义。

1 系统框架

考虑到系统接收来自3D LiDAR、IMU传感器的数据。目标是机器人进行实时6DOF的状态估计，并建立全局一致的地图。本文所提出的算法系统框架主要由前端迭代卡尔曼滤波和后端位姿图优化两部分组成，如图1所示。在前端将传感器数据预处理，分别建立IMU预测模型和激光雷达观测模型，构建迭代卡尔曼滤波器，将机器人先验位姿经过预测和观测的状态传播过程，使其状态更新后的姿态更加准确，进而循环迭代得到基于紧耦合的激光惯性里程计。在后端使用关键帧判断当前位姿是否被加入到全局优化，使用回环检测判断当前位置是否被重新访问，这可以表述成一个最大后验(MAP)的问题。此外，再优化框架中添加了回环检测和地面约束，优化相邻关键帧之间的相对位姿，减少累积漂移，保持全局一致，提高定位精度。最后，在全局优化后输出运动轨迹、全局地图、状态估计等信息。

图1 系统框架

2 数据预处理

2.1 IMU预积分

根据IMU预积分算法。为了避免再传播，而将世界坐标系从中分离开，只计算IMU系下通过连续两帧和+1的位置、速度和姿态，可以表示为

(1)

(2)

2.2 运动补偿

由于激光雷达传感器内部的旋转机制，导致激光雷达测量不同步，因此容易产生当前帧点云失真的情况。考虑了激光雷达的自身以恒定的角速度和线速度进行匀速运动。所以，获取了每个点的时间戳信息，并使用线性插值操作，得到每个时刻对应的姿态变换矩阵，通过使用IMU测量来进行当前帧点云的运动补偿。

(3)

(4)

2.3 特征提取

本文的特征提取和匹配方法与LOAM中引入的方法相似：首先使用平滑度评价变量来选择边缘点和平面点，然后选择其他关键帧中相应的边缘线点和平面点，并用于构造激光雷达观测模型的残差。当一帧新的激光雷达点云数据经过运动补偿后，接下来进行特征提取。首先通过计算局部平滑度，可以提取边缘和平面特征。此外，点的反射率也被用作一个额外的行列式。如果一个点的反射率与相邻的阈值不同，则也将其视为另一个边点。根据点的曲率来计算平面光滑度作为提取当前帧特征信息的指标。则定义评估局部曲面的平滑度为

(5)

可以将特征点划分为两大类：平面点和边缘点。然后，根据值对扫描中的点进行排序，选择最大点作为边缘点，即在三维空间中处于尖锐边缘上的点，其和周围点的大小差距较大，曲率较高，平滑度较高。选择最小值点作为平面点，即在三维空间中处于平滑平面上的点，其和周围点的大小差距不大，曲率较低，平滑度较低。提取的边缘线和平面在第帧激光雷达扫描的特征分别表示为：和。则组成一个在第帧所有提取的特征的集合，可以表示为={,}。

2.4 提取地面约束

根据文献[9]可知，当地面机器人在不同的空间区域工作时，添加地面约束可以为关键帧节点轴位移提供额外的约束条件，同时通过使用更多的约束信息来提高姿态估计的精度来减少累积误差。建立地面约束的关键是准确地提取道路平面，如何花最短的时间来完成平面分割是一个关键问题，并选择一种迭代次数少、抗噪声能力强的鲁棒估计方法——随机样本共识(RANSAC)。根据RANSAC的基本原理，从点云的每一帧中任意选择3个点，得到一个平面。常用的平面方程为：++=，其中一个++=1，>0，(，，)是平面法向量，为激光雷达传感器到平面的距离。这4个参数可以确定一个平面。

由于机器人所经过的地面不平整，不能以固定平面为节点构造姿态图进行约束。为了保证所构建的约束与实际情况相一致，提出了基于子图的地面提取方法。根据构建的局部点云图，提取真实的平面参数为：=[,,,]。为了提高地面提取的效率，取当前位置作为原点，并在激光雷达测量距离半径范围内的子图搜索点云。根据当前时刻的三维激光雷达姿态，利用式(6)和(7)将地面参数转换到激光雷达坐标系：

(6)

(7)

式中，为提取巷道平面参数；[,]为激光雷达在时刻的位姿；为激光雷达在时刻的旋转；为激光雷达在时刻的平移；，，分别为平面方程++=，轴、轴、轴坐标；′为在时刻的平移后，激光雷达传感器到平面的距离。

可以得到，姿态节点与地平面节点之间的误差,计算可表示为式(8)。

(8)

(9)

3 前端：紧耦合激光惯性里程计

3.1 状态定义

(10)

式中，⊗为四元数乘积；exp(·)为欧拉角转四元数。

3.2 运动方程的定义

将线性化连续时间下的运动方程定义为

(11)

(12)

(13)

+1=(+Δ)

(14)

(15)

式中，为的协方差矩阵；为的协方差矩阵，通过传感器校准离线获得。

3.3 观测方程的定义

观测模型主要由激光雷达对点云所提取特征的匹配获得，可以定义为

(16)

式中，为(·)的测量噪声；为测量噪声的协方差矩阵；(·)为一个由点到边对或点到平面对计算出的堆叠残差向量。

(17)

(18)

3.4 状态更新

本文采用的是迭代卡尔曼滤波(IEKF)，状态更新可以抽象为一个优化问题。考虑误差状态是由先验状态的偏差和观测模型的残差两部分组成，因此可以将误差状态更新问题变成一个求解先验状态偏差最小和观测模型相对姿态解算偏差最小的联合优化问题。可以表示为

(19)

式中，‖·‖为范数。

因此，迭代更新的卡尔曼增益方程可以表示为式(20)和后验状态的均值可以表示为式(21)。

(20)

(21)

(22)

3.5 状态合成

(23)

4 后端：位姿图优化

4.1 初始化

4.2 关键帧选取

为了保持有限数量的姿态进行估计，在后端部署了关键帧的选取策略。关键帧的选取可以极大地提高计算效率，特别是它可以确保算法能够在较大的环境地图上实时运行。选择关键帧时，必须减少匹配错误和冗余关键帧，以达到节省计算量的目的，同时稀疏的关键帧会增加帧间观测的不确定性，导致地图质量下降。基于实时点云的关键帧选取标准，通常第一帧点云被用作关键帧，其余的则根据连续帧之间的相对位姿变化来判断，设第-1关键帧的姿态为-1和第关键帧的姿态为，可使用式(24)获得连续关键帧的相对位姿变化：

(24)

其中，Δ为连续2个关键帧之间变化的旋转；Δ为连续2关键帧之间变化的平移；Δ为连续2关键帧之间变换的时间。

(25)

(26)

Δ=--1

(27)

当以上3个标准中的任何一个超过设定的阈值时，当前第帧点云选取为关键帧。2个关键帧之间的其他激光雷达帧则被丢弃。以这种方式添加关键帧不仅可以实现地图密度和内存消耗之间的平衡，而且在空间中均匀分布，这适用于实时非线性优化。

4.3 地图构建

(28)

4.4 回环检测

为了减小传感器的累积误差，添加了回环检测，当机器人在运行过程中检测到历史关键帧，将结合历史关键帧中的所有相关位姿进行全局优化。在执行循环检测时，首先将当前的实时关键帧与历史关键帧之间进行比较，然后选择候选回环帧。满足以下条件：① 当前关键帧的索引大于历史关键帧的索引；② 当前关键帧与历史关键帧的轨迹距离之差大于设定的阈值；③ 当前关键帧与历史关键帧之间的相对平移距离小于设定的阈值。最后，通过比较候选回环帧和当前已注册的关键帧之间，选择最高得分的候选回环帧。将关键帧和候选回环帧作为节点添加到SLAM图优化中，边缘约束为点云配准得到的相对姿态。

算法1 回环检测算法Input:Pm and Tm from the current frame point cloudOutput:TICP1:if(Tm,Pm).isKeyframe() then2: if DT≠ϕ or DP≠ϕ then3: T'm← KDtree.RadiusSearch(Tm,DT,r);4: M ← registerPointCloud(DT,DP);5: if T'm≠0 then6: TICP← ComputeICP(Pm,M,Tm);7: end if8: end if9: T'm← 010:DT=DT∪Tm11:DP=DP∪Pm12:end if

只有获得当前帧的点云在世界坐标中的当前姿态，可以将和反馈给全局优化进行状态校正。状态校正可以表示为(29)进行修改，通过求解以下代价函数：

(29)

4.5 全局优化

为了有效地解决大规模同步定位和建图问题，人们通常采用集束调整(BA)的方法，即利用传感器姿态和空间点进行的图优化。但是，随着机器人运行的轨迹将持续增长，BA方法会使系统随地图规模的持续增加而计算效率降低。针对这一问题，所采用的位姿图优化只将其作为位姿估计的约束条件，不再对特征点的位姿估计进行优化。并基于位姿图优化理论，计算出关键帧姿态、地面约束和回环检测约束，构建出基于多约束因子的位姿图优化代价函数。

姿态图优化问题可以通过标准的优化方法有效地解决，如高斯-牛顿或黎文堡马夸特(LM)算法。它已经集成到优化库的通用图优化(G2O)。目标是估计机器人6DOF的状态估计，并同时建立一个全局一致的地图。假设这些测量误差符合零均值高斯分布时，求解最大后验(MAP)问题等价于最小化负对数似然，负对数似然可以写成马氏距离平方的形式：

(30)

5 实验和讨论

对本文所提出的激光惯性融合SLAM算法性能进行了一系列的定量和定性分析实验，并与其他最先进的激光SLAM方法进行比较。并且，所有的方法都是在相同的条件下进行测试的。使用的硬件平台是带有传感器和机载计算机的轮式移动机器人平台，如图2所示。此处实验使用的传感器包含Velodyne VLP-16激光雷达和hipnuc-CH110的IMU，该激光雷达的采样频率为10 Hz，而IMU的采样频率为200 Hz。机载计算机是一台英特尔酷睿i7，具有2.7 GHz主频，八核和16 G内存。所有的算法都在C++中实现，并使用ROS的medoic版本在Ubuntu18.04系统中执行。

图2 轮式移动机器人

由于煤矿井下无法获取真值作比较，为了验证本文所提出方法的性能(精度、绝对误差和鲁棒性)，同时保证实验的合理性。首先使用了M2DGR公共数据集的hall_05序列进行定量分析。注意，所有的算法都在没有回环的情况下进行了测试。为了验证所提出方法的精度和绝对误差，分别进行了不同算法与真值之间轨迹误差(平移和旋转)的对比和绝对位姿误差(平移和旋转)的对比。为了测试4种不同算法所得到轨迹的全局一致性，进行了在SE(3)下同时考虑旋转和平移的绝对位姿误差分析及箱型图。其次，为了验证所提出的方法在不同煤矿井下环境，是否具有良好的鲁棒性。用笔者的试验平台记录了3组自采数据集进行定性分析，并将该方法与a_loam，lego_loam和lio_sam算法做比较，通过不同算法之间的横向对比和不同场景之间的纵向对比，验证了所提出算法具有良好的鲁棒性。此外，还设计了消融实验分别验证了添加地面约束和回环约束对整个优化结果的影响。最后，针对主要模块进行了运行时间分析，验证了其可以满足系统实时性的需求。

5.1 定量分析

为了评估所提出方法的精度和误差状态，选择使用M2DGR公共数据集的hall_05序列进行不同算法之间的测试。可以看出，aloam所产生的轨迹与真值之间差别较大，lego_loam和lio_sam所产生的轨迹与真值之间除部分区域有细微差别外，其他部分基本相同，提出的算法所产生的轨迹最接近真值。为了探究不同算法之间的差别，本文采用EVO工具对其进行定量分析，不同算法所得到运动轨迹和真值之间的对比如图3所示，平移误差如图4所示，旋转误差如图5所示。

图3 不同算法所得到运动轨迹和真值之间的对比

图4 不同算法所产生轨迹平移误差与真值之间的比较

图5 不同算法所产生轨迹旋转误差与真值之间的比较

为了测试4种不同算法所得到轨迹的全局一致性，分别统计了M2DGR公共数据集的hall_05序列运行4种不同算法，不同轨迹和真值之间的绝对位姿误差统计，见表3。以下所得到数据中最好的结果以粗体显示。在SE(3)下分析了同时考虑旋转和平移误差的绝对位姿误差如图6所示，其对应的箱型图如图7所示。

图6 同时考虑平移和旋转的绝对位姿误差

图7 绝对位姿误差箱型

表3 4种不同算法与真值之间绝对位姿误差统计

统计结果表明，在M2DGR公共数据集hall_05序列上进行不同算法之间的测试，所提出的算法相对于其他3种算法而言，具有更小的绝对位姿误差。因此，所提出算法状态估计的精度更优。

5.2 定性分析

为了验证本文所提出算法在煤矿井下的实际表现，分别进行了3次相同配置不同环境下的测试，3个自采数据集包括煤矿巷道长廊场景1、煤矿巷道水泵房和配电站场景2、煤矿巷道工作面场景3，并将本文所提出算法分别与aloam，lego_loam和lio_sam方法进行了分析与比较，其在当前实验环境所构建的点云地图，如图8所示。

根据图8所示，4种不同的激光SLAM算法分别构建煤矿井下3种不同场景的点云地图，图8(a)为典型的“长廊”结构，图8(b)包含丰富的场景变化存在回环结构，图8(c)为典型的“口字型”回环结构。由于3种测试场景皆为煤矿井下感知退化环境，其点云相对特征较少，因此给测试算法带来了很大的挑战。对于4种不同的算法来说，aloam在3种场景下也有不错的表现，但在图8(a)场景下由于出现连续的相似旋转和平移，导致点云产生了少量的误匹配。lego_loam在图8(a)和(b)两种场景下，环境的大致轮廓可以构建出完整点云地图，但由于轴上出现了明显的漂移，导致所生成点云地图的结构变形和不完整。在图8(c)场景下，由于口字型场景中存在相似结构，导致原始点云数据的提取的特征受到了大量的相似点云干扰，而使整个SLAM系统失败。lio_sam在图8(a),(b)和(c)三种场景下，可以在短时间内构建出周围环境的几何结构，但随着激光里程计里程的增加，由于部分场景相似，导致建立了错误的回环约束，或发生轴漂移，以致点云地图构建失败。而本文所提出的方法在3种场景下，所构建的点云地图在完整性和几何结构真实性方面均有着优秀的表现。由此可见本文，所提出的算法在煤矿井下环境，具有良好的鲁棒性。

图8 当前实验环境采用4种SLAM方法构建的的点云地图

总之，4种不同算法在相同环境下所构建出不同效果的点云地图，是由于自身的优缺点所导致。a_loam的方案采用匀速运动模型假设，利用IMU数据来进行点云的畸变校正，系统实时性好，但是没有闭环检测模块，导致它在大规模测试中也存在漂移。Lego_loam的方法是loam的优化版本，对激光雷达扫描的原始数据帧进行了分割和聚类，将地面点云进行了标记，通过两步LM优化来提高状态估计的精度。但是由于其过于依赖点面点云的提取，一旦地面点云提取失败，会导致状态估计误差的增加。Lio_sam是lego-loam的扩展版本，通过不断加入各种因子，联合来求解最小二乘的问题。但是回环检测对正确数据的关联性要求较高，错误的匹配会导致整个SLAM系统的失败。本文所提出的方法，融合了上述算法的优点。首先，前端分别采用了loam中的匀速模型假设和lego_loam中提取地面特征的思想，但是通过RANSAC的方法进行点云地面的提取，改进了LINS中的卡尔曼滤波模型，提出了一种迭代卡尔曼滤波模型，消除错误匹配引起的误差，以实现紧耦合的激光惯性里程计。其次，后端借鉴了lio_sam中因子图优化的思想，通过关键帧的选取策略，添加地面约束和回环检测约束，构建位姿图优化的代价函数，通过求解最大后验的问题，进一步提高状态估计的精度。

5.3 消融实验

为了进一步分析本文所提出算法的定位和建图效果。分别分析了该算法的2个后端优化模块的影响，地面约束和回环检测。

5.3.1 地面约束的影响

通过实验分析，系统中后端优化的地面约束对建图结果有很大的影响。如图9所示。图9(a)没有在后端优化中添加地面约束，所构造的点云地图无法进行校正，使其向一定的角度倾斜。图9(b)在后端优化中添加了地面约束，修正了定位和建图过程中的轴漂移，最后构建了一个全局一致的点云地图。

图9 地面约束对定位和建图的影响

5.3.2 回环检测的影响

通过实验分析，系统中后端优化的回环检测对地图的构造有很大的影响，如图10所示。图10(a)在后端优化中无回环约束，这导致了构建的点云图有较大的偏差，以致右上角部分区域出现部分地图重影，且初始位置和最终位置发生漂移。图10(b)在后端优化中添加了回环约束。对全局地图的累积误差进行了修正，最后构造了一个全局一致的点云地图。

图10 回环检测对定位和建图的影响

5.4 运行时间分析

为了探索所提出算法的实时性能，分别在自采数据集和M2DGR公共数据集上进行了实验，详见表4。由表4可知，由于自采数据集来自VLP-16数据，而M2DGR数据集来自HDL-32数据。因此，对于激光SLAM系统来说，在采样频率相同的情况下，处理M2DGR数据集各个模块则需要更长的时间。针对自采数据进行分析，对于前端激光点云和IMU数据预处理，每帧的平均时间仅为10 ms左右，整个过程没有较大的偏差。对于激光惯性里程计，扫描匹配大约需要3 ms，最长需要10 ms迭代匹配才能收敛。构建局部地图的构建大约需要2 ms。由于回环检测需要循环判断，也需要在回环帧之间进行基于ICP的配准，进行姿态优化，平均回环检测时间约35 ms，全局优化的平均时间约13 ms，可以快速满足收敛条件。

表4 各主要模块平均运行时间统计

结合系统框架(图1)可知，首先将200 Hz工作频率的IMU预积分后和10 Hz工作频率的LiDAR进行紧耦合，通过紧耦合后同步输出10 Hz的激光惯性里程计到后端优化模块，而构建的LiDAR-IMU融合SLAM系统前端和后端为2个独立的线程进行工作，因此前后端各模块的每一帧数据处理时间之和只要小于100 ms(10 Hz)，即可满足系统实时性的要求。

6 结 论

(1)提出了一种针对煤矿井下环境的移动机器人基于激光惯性融合的SLAM算法。通过前端构建迭代卡尔曼滤波器和后端位姿图优化的方案，增强了SLAM系统针对此类非结构化场景的鲁棒性。通过在优化框架中添加了回环检测和地面约束，优化了相邻关键帧之间的相对位姿，减小了传感器的累计误差，提高了系统的精度，确保了全局地图一致性。

(2)该算法在公共数据集和自采数据集上分别进行了定量和定性的分析，并与现有的激光SLAM算法进行比较。结果表明，该算法在此类特殊环境中能够实现稳健的6DOF状态估计和全局一致的同步定位与建图，并且在全局优化后输出运动轨迹、全局地图、状态估计等信息。

(3)该算法分别从地面约束和回环检测2个方面进行消融实验，分析了各自对整个系统的影响，解释了该算法的合理性。并分别在不同数据集上统计分析了系统前端和后端各主要模块的运行时间，证明了该算法可以满足实时性的要求。