文|正经的烧杯

编辑|正经的烧杯

«——【·前言·】——»

近年来明确强调增强机器人在非结构化地下狭窄空间的导航定位和多维信息感知能力，精准鲁棒的位姿自我感知则是地下机器人自主作业的前提，受限于地下环境全球定位系统信号拒止和狭长的作业空间。

同步定位及建图在地下机器人应用中已成为最活跃和具挑战性研究方向之一，因此在未知、复杂、退化的地下环境中进行精准鲁棒的SLAM仍具挑战性。

已有的地面SLAM技术主要基于视觉和激光雷达传感器部署，视觉感知在地下更容易受到光照和遮挡的影响，而激光雷达作为一种主动传感器，测距精度高，不易受环境干扰，在复杂地下工程空间中的应用逐渐增多。

某公司基于激光雷达的移动目标定位一般通过计算给定连续点云帧之间的运动来恢复定位目标的六自由度位姿，而连续点云帧之间的点云匹配则主要通过最近点迭代及其变体、正态分布变换方法实现。

针对地下巷道退化环境机器人的精准定位问题，本文在上述研究的基础上提出了一种鲁棒、精准的SLAM方法。

通过基于强度特征辅助的点云特征匹配实现退化环境中稀疏点云的几何特征增强，进而采用级联优化方式由粗略到精确的恢复运动，在地下退化环境中保证机器人的精确位姿和轨迹估计。

«——【·系统框架·】——»

针对地下退化巷道环境，基于激光雷达和惯性测量单元构建了高效、精准、鲁棒的SLAM系统，如图1所示。

系统主要由惯性里程计和激光雷达-惯性里程计子系统组成，惯性里程计通过预积分的方式提供高频自主定位信息以修正激光雷达点云数据畸变，并为激光雷达传感器运动提供先验估计。

在激光雷达-惯性里程计模块中，通过强度特征辅助增强点云几何特征提取的有效性，同时基于退化检测进行帧-局部地图间的特征匹配以输出激光雷达-惯性里程计。

所提SLAM子系统之间的级联图优化过程一方面通过融合闭环因子和IMU预积分因子实现机器人位姿的优化估计，同时激光雷达-惯性里程计因子亦为惯性里程计的状态更新提供位姿约束。具体地惯性里程计与激光雷达-惯性里程计因子图及耦合方式如图2所示。

«——【·惯性里程计·】——»

IMU状态传递与预积分：从IMU获取的加速度计测量信息和陀螺仪测量信息ωt可以表示为：

其中R表示从世界坐标系到IMU坐标系的旋转矩阵，at(real)为加速度真实值，bta表示加速度偏置，na为加速度计测量噪声，ωt(real)为角速度真实值，b表示陀螺仪偏置，n负为陀螺仪测量噪声。

根据i时刻的状态以及在时间间隔[i,j]内的IMU测量值，可以通过预积分方法计算得出j时刻的速度vj、位置pj以及方向qWBj：

其中Δt是时间间隔，qWBi为用四元数表示的i时刻从IMU坐标系到世界坐标系的旋转矢量。其中积分项称为预积分量，参考坐标系为IMU坐标系，记作{αij,βij,γij}，积分结果是j时刻相对于i时刻的运动量。

基于上述IMU预积分处理，首先可以矫正激光雷达数据帧在起始时刻i与终止时刻j存在的激光点云畸变，还可以直接将IMU预积分量通过因子图优化方式得到相邻激光数据帧之间的位姿关系。根据预积分方法得到的IMU预积分因子为：

其中zu表示连续两帧激光雷达i、j之间的预积分测量值；X表示待优化状态节点，惯性里程计的估计状态包括位置p、速度v、方向R、加速度计偏置ba和陀螺仪偏置bo。

惯性里程计因子图优化融合IMU预积分因子和激光雷达-惯性里程计先验因子，并通过定期重置保证惯性里程计的高频状态估计。

如图2所示，因子图中的每个节点都与上一个状态相关联，两个相邻节点之间的双元边表示IMU预积分因子以及激光雷达-惯性里程计因子。

由于激光雷达-惯性里程计的全局参考是基于移动机器人的第一个位姿，因此使用激光雷达-惯性里程计提供激光雷达帧的绝对位姿构造状态节点的先验观测约束。

而对于每一个新增帧，惯性里程计的优化过程可表示为最小化IMU预积分因子和激光雷达-惯性里程计先验因子组成的优化问题：

«——【·激光雷达-惯性里程计·】——»

激光雷达所采集的原始点云数据通常包含无法提供几何约束的点云以及大量的噪声点云，严重影响特征提取和点云匹配的精度、可靠性和效率。

因此本文采用图像分割聚类的方法以减少计算的时间消耗，根据距离图像上点的曲率来计算平面光滑度作为提取当前帧的几何特征信息的指标。曲率较大的点被归类为边缘特征，曲率较小的点被归类为平面特征。点i的曲率计算公式：

其中S代表在i周围，并且和i在同一条扫描线上的点，ri和rj分别代表点i和j的距离值。以上获得的点云几何特征将通过特征匹配来构造连续帧之间的约束关系。

然而点云几何特征十分依赖环境的几何特征，在地下巷道、廊道等环境，容易出现退化现象，考虑到激光雷达获取场景目标点三维坐标信息的同时还记录了回波强度信息，场景中不同物体的点云映射具有不同的强度信息。

通常激光雷达点云的强度信息I与物体表面反射率p、激光入射角度C、三维点的测量距离r有如下关系：

强度信息可以有效弥补几何特征较少带来的匹配误差。然而由于激光强度信息波段较少，不同种类的强度数值较为接近，强度过大的点和过小的点均会影响测距精度，极易在强度剧烈变化或者不变的结构中发生误判。

因此本文通过点i附近的强度差η判断其和提取点云群体之间的强度特征，若相邻点之间的反射强度差较大，将其认为是强度特征：

其中S代表在i点周围且和i点在同一条扫描线上的点，Ii和Ij分别代表点i和j的强度值，由于强度特征表示不同反射率物体的轮廓分布，其点云分布更加符合边缘线的几何特征，因此可以将其认为是“线特征”，从而辅助提取地下退化环境中的壁面几何特征。

如图3（a）所示，地下巷道退化环境单帧原始点云中，由于巷道环境特征退化，难以直接提取出充足的几何特征。

图3（b）所示稀疏的边缘线特征进一步降低了特征匹配的可靠性。而图3（c）所示巷道壁面上存在部分凸起的标志牌等典型几何结构，其点云帧的强度信息较为丰富，通过强度特征的辅助有效增加了线、面等几何特征数量（图3（d））。

«——【·实验结果分析·】——»

KITTI数据集实验：选取不同的实验方法程序分别读取数据集中的连续IMU数据和激光雷达点云数据，不同的方法程序完成定位和建图后输出移动目标的轨迹位置和周围环境的地图结果。

对于SLAM方法建图性能的评价方法主要分为定性评价和定量评价两类，其中定量评价主要是认为移动目标的轨迹精度是对地图重建精度的间接度量，因此通常采用移动目标的定位结果指标间接的评估SLAM方法的建图性能。

而定性评价方法则认为轨迹精度并不能够代表地图的重构性能，因此本文在建图结果中给出了不同方法的建图表现，而在定位结果中也给出了定量的精度指标，具体的实验结果如图4、图5和表1所示。

从图4中的建图表现可以看出，FAST-LIO由于不包含闭环检测，出现了明显的地图重影现象，也缺失了较多的结构细节。

LEGO-LOAM构建的地图虽然由于引入了闭环检测而优于FAST-LIO，但对点数较少的聚类特征点进行了删除，损失了一定的精度，导致地图虚化和部分重叠。

LIO-SAM与本文方法所构建的点云地图具有较好的整体精度和细节表现，但LIO-SAM方法输出的稠密地图因累积误差而产生结构细节的重叠和边缘区域地图的重影，而本文方法采用了更稀疏的点云特征和退化检测方法，在地图的整体精度和细节方面综合表现更优。

图5展示了不同方法在KITTI数据集上的定位结果，可以看出本文方法在直线和转弯阶段更接近于真值，提供了更高的定位精度和全局一致性。

进一步采用绝对轨迹误差对图5中不同方法在Z轴方向的定位误差值进行统计分析，结果如表1所示。

由图5和表1可见，LEGO-LOAM仅采用单一激光传感器方案，均方根误差达到2.17m，FAST-LIO由于没有闭环检测，Z轴均方根误差积累到2.76m，LIO-SAM则通过检测多个闭环点来不断修正积累误差，使RMSE保持在1.42m。

本文采用级联优化过程紧耦合惯性传感器和激光雷达数据，并及时修正积累误差，将其降低到1.25m，相比LIO-SAM方法降低了12.3%。

现场实验在中国矿业大学瓦斯煤尘爆炸实验室进行，如图6(a)所示，实验环境光线灰暗，特征稀疏，是典型的地下退化环境。实验设备布置如图6(b)所示，其中红外动捕系统通过布置在机器人上的Marker点捕获机器人的真实运动轨迹，作为定位结果的地面真值。

为了验证所提方法在地下退化环境中的建图鲁棒性，在地下实验场景中分别选取了巷道场景和廊道场景进行对比分析，其中巷道场景为一条拱形实验巷道，巷道壁面处有一些警告标识牌以及通风管道，如图7(a)所示。

而廊道场景则是一条长直地下走廊，截面呈类矩形，壁面仅存在一些等宽的棱柱，如图8(a)所示。

两种地下实验场景均存在严重的感知退化现象，对传统SLAM方法的鲁棒性和精度构成巨大挑战。图7(b)~图7(d)和图8(b)~图8(d)分别为不同方法在巷道和廊道实验场景中的点云地图构建结果。

从图中可以看出，FAST-LIO点云更加稠密，但是出现了一些重影和模糊现象。LIO-SAM虽然能较好的还原巷道和廊道的整体结构，但是由于点云特征过于稀疏，细节结构缺失较多。

而本文方法由于点云强度特征的辅助有效增加了点云特征数量，对巷道和廊道中的通风管道、金属围栏、标识牌、棱柱的还原更为准确清晰。而LEGO-LOAM在开环的地下退化环境中局部地图扭曲严重，无法完成完整的地图构建。

图9和图10分别为不同方法在巷道实验中的轨迹和定位结果。可以看出，本文方法在X和Y方向上的定位轨迹和误差明显优于对比方法，尤其在转弯阶段，所提方法可以更精准的跟踪机器人的位姿变化。

进一步地，不同方法在地下巷道场景中的定位误差统计结果如表2所示，由于该巷道环境几何特征稀疏，FAST-LIO、LIO-SAM方法的RMSE分别在0.042m和0.044m左右。

而本文方法在地下巷道等退化环境中将定位结果的均方根误差值降低到0.030m。相比于对照方法，定位精度分别提升28.57%和31.82%。

进一步对比KITTI数据集实验和现场巷道实验结果可知，LIO-SAM虽然在KITTI数据集的地面道路等特征显著的场景中提供了高于FAST-LIO的定位精度。

但在地下巷道退化场景中，地图精细程度和定位精度均出现了较为明显的损失，而本文方法在KITTI数据集和现场巷道实验中均有更优的表现。

«——【·结语·】——»

针对地下巷道等退化环境中自主机器人状态估计不完备的问题，提出一种基于级联优化和强度特征辅助的激光雷达-惯性融合SLAM框架和方法。

该方法组合惯性里程计和激光雷达-惯性里程计两个优化过程完成状态估计，惯性里程计可以为激光雷达-惯性里程计提供一个先验运动估计，激光雷达-惯性里程计可为惯性里程计提供位姿约束来更新惯性偏置，降低积累误差。

为提高激光雷达点云的特征提取与匹配有效性，利用强度特征辅助提取点云线、面特征，并通过检测退化方向来优化特征匹配，进一步提升系统在地下巷道退化环境中的位姿估计精度和点云地图的全局一致性。

通过KITTI数据集和巷道实验对所提方法的有效性进行验证，结果表明，所提SLAM方法在建图鲁棒性和定位精度方面均有明显优势，可为煤矿、地下工程和管道等退化场景的自主移动机器人提供稳定精确的状态估计和环境描述。