基于集成式因子图优化的煤矿巷道移动机器人三维地图构建

邹筱瑜，黄鑫淼，王忠宾，房东圣，潘杰，司垒

（1.中国矿业大学 机电工程学院，江苏 徐州 221116；2.中国矿业大学 江苏省矿山机电装备高校重点实验室，江苏 徐州 221116）

0 引言

煤矿井下同步定位与三维地图构建技术可为井下移动机器人提供定位先验信息、本体路径规划与自主避障全局导航地图、执行机构作业目标位置信息等[1]。精确的地图构建是实现煤矿巷道移动机器人自主行走、导航避障、作业施工的关键技术[2-3]。因此，研究煤矿巷道三维地图构建方法具有重要意义。

同步定位与地图构建（Simultaneous Localization and Mapping，SLAM）是一种广泛应用于移动机器人、无人驾驶等领域的关键技术，可对处于未知环境的移动机器人进行姿态估计和定位。基于激光雷达的三维SLAM算法可以实现图像、物理模型等信息的融合，从而提高机器人操作系统的定位精度[4]。但是，三维点云数据量巨大，导致计算开销较大。Zhang Ji等[5]提出了实时激光雷达测距与建图（Lidar Odometry and Mapping in Real-time,LOAM）算法，开创性地引入视觉SLAM 中常用的特征点提取思想，大大减少了前端点云配准的时间和计算开销。基于图优化框架的激光2D/3D−SLAM算法−Cartographer，W.Hess等[6]采用基于子图构建全局地图的思想，利用非线性优化库实现了图优化框架在激光SLAM 领域的应用尝试。Shan Tixiao等[7]提出了轻量级及地面优化激光雷达测距与建图（Lightweight and Ground-Optimized LOAM,LeGO−LOAM）算法，该算法基于LOAM 思想，进一步简化了特征点提取，通过剔去地面点来减少计算量，并通过2步优化分别计算6个自由度，达到了与LOAM相近的性能且实时性更强。Shan Tixiao等[8]在文献[7]的基础上提出了基于平滑建图的雷达惯性里程计（Lidar Inertial Odometry via Smoothing and Mapping，LIO−SAM）算法，通过激光雷达−惯性测量单元（Inertial Measurement Unit，IMU）−里程计紧耦合的方式增强了算法的鲁棒性。目前大部分的三维激光SLAM算法通常采用前端构建和后端优化的策略，且多数前端构建都采取了迭代最近点（Iterative Closest Point,ICP）算法或其衍生算法[9]。ICP算法可满足大多数三维点云配准要求，其运行速度和精度主要取决于给定的初始变换估计、点云大小和初始位置精度[10]。Chen Hui等[11]提出了2步ICP算法，先利用ICP算法对初始点云数据进行粗配准，将得到的变换矩阵作为初始变换再次应用到ICP算法中，进行精细配准，提高了点云配准效率。为了克服上述ICP算法存在的问题，He Shijun等[12]提出了一种基于主成分分析的ICP方法，通过变换数据的维度为ICP提供良好的初始值，然而，当点云中存在许多噪声点时，该方法很难获得较好的初始值。事实上，ICP在处理大规模点云配准和特征点较少的点云时会出现拟合时间过长、运动估计不准的问题，在井下长直巷道三维地图构建中往往会面临激光里程计失效情况。近年来，正态分布变换 （Normal-Distributions Transform, NDT）算法逐渐引起了研究者们的关注[13]。NDT算法使用标准的优化技术来确定点云的最佳配准，基于概率分布，不使用对应点的特征来计算点云的配准，对点云数据的适用能力强，但是其精度通常低于ICP算法。K.Koide[14]等提出了基于便携式三维激光雷达的长期和广域人行为测量系统，采用了 NDT配准的接口。

针对井下特殊环境的激光SLAM 算法，Li Menggang等[15]提出了融合NDT激光里程计约束、环境中的平面特征约束及回环检测约束的NDT−graph−SLAM，解决了井下移动机器人高效定位与地图构建的问题。Xu Jiachang等[16]结合复杂的煤矿环境，融合无人直升机IMU、激光探测与测距（LiDAR）系统和深度相机等，提出了基于概率膜计算的煤矿巡逻无人机SLAM方法。

上述方法为巷道环境定位与建图提供了可行的解决方案，但针对煤矿长直巷道移动机器人的三维地图构建还存在如下问题：①目前的研究主要聚焦于多传感融合建图方法，较少关注激光SLAM算法建图精度的提升。②长直巷道环境退化严重，缺少特征点，传统激光点云匹配算法和激光里程计易失效，导致煤矿巷道建图精度受限。

由于ICP算法在处理大规模点云配准和特征点较少的点云时会出现拟合时间过长、运动估计不准等问题，在井下长直巷道三维地图构建中常常会面临机器人在运动而激光里程计认为没有运动的情况，导致激光里程计失效。而 NDT算法对点云数据的适用性强，对数据特征点不敏感，且计算速度快[17]，井下长直巷道几何特征少但点云分布特征明显，在三维地图构建中可利用 NDT 算法对ICP算法进行补充，但NDT算法精度通常低于ICP算法。因此，可在构建激光里程计约束因子时，集成ICP特征点配准方法和 NDT相似度配准方法，提升建图效果。本文针对煤矿巷道移动机器人建图中面临的上述问题，提出了一种基于集成式因子图优化的煤矿巷道移动机器人三维地图构建（ICP and NDT Ensemble SLAM，INE−SLAM）方法，集成ICP和NDT算法进行因子图优化，以提高煤矿巷道移动机器人激光SLAM建图精度。设计了INE−SLAM的前端点云配准模块和基于滤波、图优化的后端优化方法，构建了INE−SLAM的因子图优化模型，分析了ICP相对位姿因子、NDT相对位姿因子的特性。利用公开数据集、模拟煤矿巷道点云数据集开展了实验，验证了INE−SLAM 方法的有效性和优越性。

1 相关理论

1.1 ICP算法原理

ICP算法被广泛应用于点云匹配中，常用于解决2个点云集合之间的转换关系[10-12,18]。

假设激光雷达在井下探测到2组点云，分别用{Pi}和{Qi} 表示点云集合，其中{Qi}为参考帧，{ Pi}为当前帧的点云，共有 N 个点。需要计算从{Pi}到{Qi}的变化参数，即旋转和位移。如果变换参数是准确的，那么点云{Pi}中的每一个点 pi经过变换后应该与点云{Qi} 中的点qi完全重合，即 qi=hpi+t（h为旋转矩阵，t为位移矩阵）。但是，由于井下噪声的影响，不能使各个点都完全一致。因此，定义目标函数为

使目标函数最小的h和t即为所求变换参数。

1.2 NDT算法原理

NDT算法与ICP算法一样是点云配准的经典算法之一。该算法基本思路为构造多维变量正态分布，若变换参数为2个点云最优匹配时，变换点概率密度达到最大值。为此，通过优化找到使概率密度总和达到最大值的变换参数，其核心思想：将点云离散化，如果是二维平面，则离散成方格，如果是三维空间，则离散成立方体，使取样的点云被分割成若干个格子，从而便于描述其局部特征，如点云局部的形状（直线、平面或球体）、方向（平面法向、直线方向）等[13-16,19]。

2 INE−SLAM方法

2.1 INE−SLAM方法框架

煤矿井巷高精度定位和地图构建是自主移动机器人亟需解决的关键技术问题。为解决井下长直巷道特征点缺失、激光里程计失效的难题，本文提出了一种INE−SLAM方法。构建激光里程计约束因子时，集成ICP特征点配准方法和 NDT相似度配准方法，以降低激光里程计累计误差，并通过因子图优化得到一致性较高的三维地图。该方法可提升基于激光雷达的煤矿巷道三维地图构建的精度及鲁棒性。

INE−SLAM方法框架如图1所示，分为前端构建和后端优化2个部分，包括点云预处理模块、特征点匹配模块、相似度匹配模块和位姿图优化模块。其中，特征点匹配模块和相似度匹配模块分别通过2种不同的点云配准方法，构建2种不同的约束因子提供给后端优化。

图1 INE−SLAM方法框架Fig.1 Framwork of ICPand NDT ensemble SLAM method

2.2 点云预处理

由于三维激光雷达传回的每一帧数据数量巨大，尤其是井下巷道空间狭小，由于物体反射会产生很多无效数据，如果直接对所有点云数据进行计算，将耗费大量的计算资源甚至出现数据报错。点云预处理模块首先通过滤波、下采样2步操作滤除点云的噪声点，在不改变点云性质的情况下减少计算量。然后通过点云矫正算法对雷达运动产生的运动畸变进行矫正。最后将预处理后的点云数据输出给特征点匹配模块和相似度匹配模块。点云预处理相关操作可参考文献[20-21]。综合考虑井下巷道的环境特点和激光雷达测量噪声，本文主要需要滤除与被观测物体有显著偏差的离群点，利用统计滤波器能很好地解决离群点问题。

激光雷达穿透回来的原始点云数据通常比较稠密，尤其在井下巷道，竖直墙壁为主要特征，过多重复的点云给后续配准工作的计算带来挑战。针对该问题，提出了一种体素滤波和体素格化处理相结合的方法。

机械旋转式扫描激光雷达运动时不可避免地存在点云扭曲，为了避免这一现象的发生，必须对扭曲进行补偿，以得到完整的点云数据。在煤矿巷道这类作业低速低位移场景中，以激光里程计常速运动假设为前提，采用各点相应时间戳对激光点云进行插值还原，该方法效率非常高，效果良好。

2.3 特征点匹配

特征点匹配模块采用与LOAM 系列算法相似的思想，对点云的曲率进行计算，提取出曲率大的角点和曲率较小的平面点，利用PL−ICP（Point-to-Line ICP）和PP−ICP（Point-to-Plane ICP）算法，估计移动机器人的运动并计算出变换矩阵及需要构建的约束因子参数。相似度匹配模块采用基于概率的 NDT算法，通过划分体素块，计算各个体素块的均值和协方差矩阵，利用点云的分布情况计算运动变换，进而计算约束因子的参数。特征点匹配模块和相似度匹配模块输出的关键帧携带移动机器人的位姿信息，与约束因子信息一起输入位姿图优化模块，通过时间帧对齐，相互约束，相互补充。为提取出当前点云中的线特征及面特征，利用类似LOAM[5]和LeGO−LOAM[7]算法的特征提取方法，对局部区域点云进行曲率计算。激光雷达在某个扫描周期 K 内获得的点云记为 PK，这期间对应的雷达坐标系定义为LK，点云中第i个点 在LK下的位置坐标可表示为XKL,i。S 为点云 PK中点 pi所在行中点组成的连续点集，此处设置|S |为10，即点 pi两侧各有5个点。可以设计以下公式来评价点 pi所在局部表面的曲率：

采用上述曲率计算方法可对点云排序并找到曲率最小的m个点，作为平面点和曲率最大的n个点，即边缘点。

激光雷达频率为10 Hz，若利用每一个雷达扫描帧来匹配特征和在因子图上加入相对雷达因子，存在多余帧带来极少新变换信息、造成巨大计算量等问题，不利于实时工作。针对此问题，设定最小平移距离阈值和最小旋转角度阈值，其中一个阈值满足条件，则当前雷达帧为新关键帧。在相邻关键帧间去除雷达扫描帧，将有助于降低后续优化时的内存消耗及地图构建时的存储消耗，进而保持因子图的稀疏性，便于进行实时非线性优化。

特征匹配主要依据上述特征点，利用ICP 算法对2次连续扫描间的相对移动进行匹配估计。2次扫描间的变换通过点与边、点与平面间扫描匹配完成，即要从上一帧特征点集中找出相应特征关系。可通过文献[9]、文献[18]中的方法，迭代计算出2帧数据间的位姿变换矩阵为第k帧的位姿），得到各帧的相对位姿，并提取关键帧的位姿作为后续输出到图优化模块的约束因子。

2.4 相似度匹配

将点云预处理后的第1帧数据作为参考帧，输入 NDT算法作为初值，并作为第1帧关键帧。对点云进行体素分割，分成A个体素块，并对每个体素块中的点云计算均值 µ和协方差矩阵 Σ：

式中Xa为第a个体素块中的点云数据。

将最近一帧的点云数据投影到参考帧上，并对这一帧点云数据计算均值µ和协方差矩阵Σ，根据式（1）得到2帧点云之间变换矩阵[h| t]。由此可得当前帧f的位姿：

式中 ∆Tk−1,f为当前帧f和前一个关键帧k−1之间的位姿变换矩阵。

假定移动机器人运动状态改变为低速匀速状态，可将前次相对变换用作NDT配准初值以达到快速计算收敛的目的。当移动机器人当前帧相对于前一个关键帧位姿变化超过设定阈值时，就会造成NDT 不收敛，前一个关键帧及相对变换就会用来计算当前输入点云的里程。当前帧f和前一个关键帧k−1之间的位姿变换矩阵可表示如下：

式中：∆h为 旋转变化；∆t为位移变化。

与特征点匹配的关键帧提取相似，设置3个阈值来减少冗余关键帧，包括最小平移距离阈值、最小旋转角度阈值、最小运行时间阈值，只要满足任意一个阈值就将当前帧输出为关键帧加入之后的因子图优化。

2.5 因子图模型构建

因子图是一种概率图模型，是一种无向图。首先由F.Kschischang[22]提出并用于解决SLAM 问题。因子图有2种节点，分别为表示优化变量的变量节点和表示因子的因子节点。

从概率图理论来看，基于因子图的SLAM可以利用最大后验估计（Maximum a Posterior，MAP）进行求解[23]。因子图的优化与位姿图优化类似，都可以使用稀疏QR分解、Schur分解或Cholesky 分解进行求解。不同的是，M.Kaess等[24-25]提出的iSAM（Incremental Smoothing and Mapping）中对因子图的优化进行了全新解读，认为可以增量式地处理优化因子图。传统图优化要面临的问题：当移动机器人运动后，位姿图中得到新的节点和边时，就要计算所有的节点来更新变量，计算量较大。文献[24-25]提出的方法能够增量优化因子图，只需计算新节点影响到的节点，减少了不必要的计算量，加速了优化流程。在iSAM 的基础上，F.Dellaert[26]建立了GTSAM （Georgia Tech Smoothing and Mapping）优化库，方便其他研究人员使用。基于上述因子图优化模型目标函数的构建方法，可以构建多方法集成式因子图。构建方法如图2所示。

图2 集成式因子图构建Fig.2 Construction of integrated factor graph

煤矿巷道多方法集成激光里程计的无约束优化目标函数定义为

式中：χ为待优化的位姿参数；ϕpriorχ为先验因子的代价项；ϕICP(χ)为 使用ICP配准的代价项；ϕNDT(χ)为使用NDT配准的代价项。

式中：rprior和Hprior分别为先验因子的残差和海森矩阵；w 为先验帧的索引；u和v为关键帧；rICP为ICP配准的残差；为ICP观测向量；和分别为ICP和NDT方法所得前后2帧的协方差矩阵；为NDT配准的残差； z′kk+1为NDT观测向量。

通过ICP配准方法得到2帧之间的位姿相对变换，可以构建ICP相对位姿因子，提供给新的关键帧与已优化的关键帧位姿之间的约束。构建的ICP相对位姿之间的优化目标项为

式中： x 为优化变量；xu为关键帧u的索引对应的运动状态；xv为关键帧v 的索引对应的运动状态；ΣT为相对位姿变换协方差矩阵，ΣT∈ R6×6，R为旋转矩阵。

在多方法集成过程中，状态变量是基于移动机器人本体坐标系。因此，转换到移动机器人本体坐标系下的相对位姿观测为

式中：TBL为 移动机器人本体坐标系到雷达坐标系O的外参变换；为关键帧u和v 对应的激光惯性里程计坐标系下的待估计位姿，∈S E(3)（三维欧氏空间的数学表示）。

与ICP相对位姿因子构建方法类似，通过 NDT相似度配准方法得到的2帧之间的位姿相对变换，可以构建 NDT相对位姿因子，提供给新关键帧与已优化关键帧位姿之间的约束。构建的NDT相对位姿之间的优化目标项为

3 方法验证

为验证INE−SLAM 方法的有效性，将在公开数据集和模拟煤矿巷道点云数据集中开展实验。

3.1 KITTI数据集实验

采用目前广泛使用的KITTI数据集[27]进行实验，以便于方法对比与复现。将本文提出的INE−SLAM方法分别与基于特征点匹配的A−LOAM方法、基于平面分割及特征点提取的LeGO−LOAM 方法进行对比。基于KITTI 里程计数据集的原始数据序列00，01，05，09进行了对比实验，并与数据集提供的真实位姿数据进行对比。3种方法的轨迹对比如图3所示。

图3 KITTI 数据集上实验结果对比Fig.3 Comparison of experimental resultson KITTI dataset

从图3可看出，在序列00，05，09数据集上，各种方法的轨迹和真值相差不大，而在序列01数据集上，3种方法产生了一定差异。在00，05，09数据集上，INE−SLAM方法位姿估计精度与传统的A−LOAM、LeGO−LOAM 方法类似，证明了其可行性，在01数据集上的表现说明INE−SLAM方法在室外场景中的位姿估计精度可能存在不足。

进一步使用轨迹评估工具EVO对3种方法的性能进行定量分析评估。采用的评价指标为绝对位姿误差（Absolute Pose Error,APE）和相对位姿误差（Relative Pose Error, RPE）。APE能反映建图的全局一致性，RPE能反映建图的局部精度和细节还原程度。

3种方法在4个数据集上的APE和RPE对比结果如图4、图5所示，见表1、表2。

表1 APE对比Table 1 Absolute pose error comparison

表2 RPE对比Table 2 Relative pose error comparison

图4 APE结果对比Fig.4 Result comparison of absolute pose error

图5 RPE结果对比Fig.5 Result comparison of relativepose error

从图4、图5、表1、表2可看出：INE−SLAM方法在 RPE 指标上有良好的发挥, 在控制RPE的最大值上表现最好；从建图的全局一致性上，通过 APE指标可看出，INE−SLAM方法还有一定不足，能与A−LOAM 和 LeGO−LOAM 方法达到相近的水平。这是因为在多方法集合的因子图中，NDT 相对位姿因子是基于概率模型，相较于 ICP 相对位姿因子存在更大的误差，在进行基于信息矩阵的融合时，由于误差的存在，会使整体优化在全局一致性上产生持续的误差累计。

由实验结果可见， INE−SLAM方法在全局一致性上与LeGO−LOAM方法相似，在建图局部精度上优于A−LOAM和LeGO−LOAM方法。

3 .2 模拟巷道实验

为了验证所提出的 INE−SLAM 方法在煤矿井下巷道应用场景的性能，在模拟巷道进行了实验，如图6所示。

图 6 模拟巷道实验现场Fig. 6 Experiment site in simulated roadway

采用履带式移动机器人平台进行实验，搭载Ouster OS−1 128 线三维激光雷达采集数据，平台移动速度约为1 m/s，探测器为移动扫描。通过离线建图方式将INE−SLAM 方法运行在 Intel Core i7−8750H CPU（2.20 GHz，6核）、16 GB内存便携式计算机上。INE−SLAM方法在模拟巷道的建图效果如图7所示，同时利用LeGO−LOAM方法对模拟巷道场景进行建图，移动机器人在巷道转弯处行走约50 m时的建图结果如图8所示。从图7可看出，INE−SLAM方法解决了LeGO−LOAM 方法因只采用特征点匹配的前端配准方法，导致在如井下长直巷道这类特征点退化的特殊环境中激光里程计失效，无法有效构建三维点云地图的问题。由于采用了多方法集成的点云配准，使用基于概率分布的 NDT算法构建的相对位姿因子能够更加有效地判断出移动机器人在相似环境中的运动，而基于特征匹配的ICP算法能够使得三维建图在细节上足够准确。从图8可看出，LeGO−LOAM方法建图的 z 轴数据即可定量分析出移动机器人在运动方向的位移不超过6 m，远小于移动机器人实际行走距离。由于LeGO−LOAM方法只提取极少的特征点作匹配，在模拟巷道重复特征多、特征退化的情况下，激光里程计认为移动机器人在原地不动，多帧点云匹配失败，最终导致建图失败。

图7 INE−SLAM方法在模拟巷道的建图效果Fig.7 Mapping effect of ICPand NDTensemble SLAM in simulated roadway

图8 LeGO−LOAM方法在模拟巷道的建图结果Fig.8 Mapping result of lightweight and ground-optimized LOAM in simulated roadway

INE−SLAM方法的建图细节如图9所示，其中图9（a）的点云尺寸放大如图9（b）所示，可以更好地显现巷道内的门型轨道架。巷道进入弧形弯道前收窄部分的细节如图10所示，可看出收窄部分的墙体明显，薄厚均匀，没有因激光里程计失效导致重影。

图9 INE−SLAM 方法的建图细节Fig.9 Mapping details of ICP and NDT ensemble SLAM

图10 巷道收窄部分INE−SLAM方法的建图细节Fig.10 Mapping details of ICPand NDT ensemble SLAM in narrowing roadway

4 结论

（1）面向煤矿长直巷道特征点稀少导致激光里程计易失效的问题，提出了基于集成式因子图优化的煤矿巷道移动机器人三维地图构建方法。针对激光里程计在煤矿井下建图时由于时间产生的漂移误差，导致全局一致性差的问题，构建了ICP 相对位姿因子和 NDT相对位姿因子，共同作为约束输入因子图优化，能够使地图的全局一致性相对较高。在公开数据集和模拟巷道点云数据集中对该方法的性能进行了实验验证，结果表明：该方法可解决几何特征少，但点云分布特征明显的长直巷道的激光SLAM建图问题，与传统A−LOAM、LeGO−LOAM 方法相比，在室外场景中性能指标类似，但实时性强；在巷道场景下具有显著优势，通过因子图优化，可得到一致性较高的三维地图，提升了煤矿巷道三维地图构建的精度及鲁棒性。

（2）该方法目前主要聚焦于通过提升局部位姿估计精度来实现煤矿巷道的地图构建，后续工作将研究如何提升煤矿巷道移动机器人的定位精度。