3D map construction of coal mine roadway mobile robot based on integrated factor graph optimization
-
摘要: 煤矿井下移动机器人作业精度严重依赖于同步定位与建图 (SLAM) 技术的准确性。井下长直巷道存在特征缺失、光照条件差等问题，导致激光里程计和视觉里程计易失效，因而限制了传统SLAM方法在煤矿巷道的有效应用，且目前SLAM方法的研究主要聚焦于多传感融合建图方法，较少关注激光 SLAM 方法建图精度的提升。针对上述问题，面向移动机器人在煤矿巷道的建图需求，提出了一种基于集成式因子图优化的煤矿巷道移动机器人三维地图构建方法，采用前端构建和后端优化的策略，设计了前端点云配准模块和基于滤波、图优化的后端构建方法，使建图结果更准确、适应性更强。针对煤矿长直巷道环境退化导致三维激光点云配准精度低的问题，融合迭代最近点 (ICP) 和正态分布变换 (NDT)算法，兼顾点云几何特征和概率分布特征，设计了集成式前端点云配准模块，实现了点云的精确配准。针对三维激光 SLAM 后端优化问题，研究了基于位姿图和因子图优化的后端构建方法，构建了集成 ICP和NDT 相对位姿因子的因子图优化模型，以准确估计移动机器人位姿。分别利用公开数据集 KITTI和模拟巷道点云数据集对三维地图构建方法在不同工况下的性能进行了实验验证。公开数据集 KITTI上的实验结果表明：在全局一致性上，该方法与传统基于特征点匹配的A−LOAM方法和基于平面分割及特征点提取的LeGO−LOAM方法具有相似的性能，在建图局部精度上优于其他2种方法。模拟巷道点云数据集上的实验结果表明：该方法具有显著优势，通过因子图优化，可得到一致性较高的三维地图，提升了煤矿巷道三维地图构建的精度及鲁棒性，解决了井下长直巷道特征点缺失、激光里程计失效的难题。Abstract: The working precision of mobile robots in coal mines seriously depends on the accuracy of simultaneous localization and mapping (SLAM) technology. There are some problems such as feature missing and poor lighting conditions in long and straight underground roadway. The problems lead to the failure of the laser odometer and visual odometer. The result limits the effective application of traditional SLAM method in coal mine roadway. At present, the research of the SLAM method mainly focuses on the multi-sensor fusion mapping method. There is a lack of research on the improvement of the mapping precision of the laser SLAM method. In order to solve the above problems, facing the mapping requirements of mobile robot in coal mine roadway, a 3D map construction method of coal mine roadway mobile robot based on integrated factor graph optimization is proposed. The method adopts the strategy of front-end construction and back-end optimization. The method designs a front-end point cloud registration module and a back-end construction method based on filtering and graph optimization. Therefore, the mapping result is more accurate and adaptable. The environmental degradation in coal mine long and straight roadway leads to the low registration precision of 3D laser point cloud. In order to solve the above problem, integrating iterative closest point (ICP) and normal-distributions transform (NDT) algorithms, taking into account the geometric characteristics and probability distribution characteristics of point clouds, an integrated front-end point cloud registration module is designed, which realizes the accurate registration of point clouds. Inview of the back-end optimization problem of 3D laser SLAM, the back-end construction method based on pose map and factor map optimization is studied. The factor map optimization model integrating ICP and NDT relative pose factors is constructed to accurately estimate the pose of the mobile robot. The performance of the proposed method of 3D map construction under different working conditions is verified by using the open dataset KITTI and the simulated roadway point cloud dataset. The experimental results on the open dataset KITTI show the following points. In terms of global consistency, this method has similar performance with the traditional A-LOAM method based on feature point matching and the LeGO-LOAM method based on plane segmentation and feature point extraction. It is superior to the other two methods in the local precision of mapping. The experimental results on the simulated roadway point cloud dataset show the following points. This method has significant advantages, through factor map optimization, a 3D map with high consistency can be obtained. The precision and robustness of 3D map construction of coal mine roadway are improved. The problems of the feature point missing and laser odometer failure in long straight underground roadway are solved.
-
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组点云，分别用 {
$ {\boldsymbol{P}}_{i} $ }和{$ {\boldsymbol{Q}}_{i} $ } 表示点云集合，其中 {$ {\boldsymbol{Q}}_{i} $ } 为参考帧，{$ {\boldsymbol{P}}_{i} $ } 为当前帧的点云，共有 N 个点。需要计算从 {$ {\boldsymbol{P}}_{i} $ } 到 {$ {\boldsymbol{Q}}_{i} $ } 的变化参数，即旋转和位移。如果变换参数是准确的，那么点云 {$ {\boldsymbol{P}}_{i} $ } 中的每一个点$ {\boldsymbol{p}}_{i} $ 经过变换后应该与点云 {$ {\boldsymbol{Q}}_{i} $ } 中的点${\boldsymbol{q}}_i $ 完全重合，即$ {\boldsymbol{q}}_{i}={\boldsymbol{h}\boldsymbol{p}}_{i}+\boldsymbol{t} $ （$ \boldsymbol{h} $ 为旋转矩阵，$ \boldsymbol{t} $ 为位移矩阵）。但是，由于井下噪声的影响，不能使各个点都完全一致。因此，定义目标函数为$$ F=\sum _{i=1}^{N}{\left|{\boldsymbol{q}}_{i}-({\boldsymbol{h}\boldsymbol{p}}_{i}+\boldsymbol{t})\right|}^{2} $$ (1) 使目标函数最小的
$ \boldsymbol{h} $ 和$ \boldsymbol{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种不同的约束因子提供给后端优化。
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 下的位置坐标可表示为
$ {X}_{K,i}^{L} $ 。S 为点云 PK 中点 pi 所在行中点组成的连续点集，此处设置 |S | 为 10，即点 pi 两侧各有 5 个点。可以设计以下公式来评价点 pi 所在局部表面的曲率：$$ c=\frac{1}{\left|S\right| \left\|{X}_{K,i}^{L}\right\|} \times \left\|\sum _{j\in S,j\ne i}\left({X}_{K,i}^{L}-{X}_{K,j}^{L}\right)\right\| $$ (2) 采用上述曲率计算方法可对点云排序并找到曲率最小的 m 个点，作为平面点和曲率最大的 n 个点，即边缘点。
激光雷达频率为 10 Hz，若利用每一个雷达扫描帧来匹配特征和在因子图上加入相对雷达因子，存在多余帧带来极少新变换信息、造成巨大计算量等问题，不利于实时工作。针对此问题，设定最小平移距离阈值和最小旋转角度阈值，其中一个阈值满足条件，则当前雷达帧为新关键帧。在相邻关键帧间去除雷达扫描帧，将有助于降低后续优化时的内存消耗及地图构建时的存储消耗，进而保持因子图的稀疏性，便于进行实时非线性优化。
特征匹配主要依据上述特征点，利用 ICP 算法对2次连续扫描间的相对移动进行匹配估计。2次扫描间的变换通过点与边、点与平面间扫描匹配完成，即要从上一帧特征点集中找出相应特征关系。可通过文献[9]、文献[18]中的方法，迭代计算出2帧数据间的位姿变换矩阵
$ {\Delta }{\boldsymbol{T}}_{k-1,k}={\boldsymbol{T}}_{k-1}^{-1}{\boldsymbol{T}}_{k}^{} $ ($ {\boldsymbol{T}}_{k}^{} $ 为第$ k $ 帧的位姿)，得到各帧的相对位姿，并提取关键帧的位姿作为后续输出到图优化模块的约束因子。2.4 相似度匹配
将点云预处理后的第1帧数据作为参考帧，输入 NDT 算法作为初值，并作为第1帧关键帧。对点云进行体素分割，分成A个体素块，并对每个体素块中的点云计算均值
$ \boldsymbol{\mu } $ 和协方差矩阵$ \boldsymbol{\varSigma } $ ：$$ \boldsymbol{\mu }=\frac{1}{A}\sum _{a=1}^A{\boldsymbol{X}}_{a} $$ (3) $$ \boldsymbol{\varSigma }=\frac{1}{A}\sum _{a=1}^A{({\boldsymbol{X}}_{\mu }-\boldsymbol{\mu })({\boldsymbol{X}}_{a }-\boldsymbol{\mu })}^{\mathrm{T}} $$ (4) 式中
$ {\boldsymbol{X}}_{a} $ 为第a个体素块中的点云数据。将最近一帧的点云数据投影到参考帧上，并对这一帧点云数据计算均值
$ \boldsymbol{\mu } $ 和协方差矩阵$ \boldsymbol{\varSigma } $ ，根据式(1)得到2帧点云之间变换矩阵 [$ \boldsymbol{h} $ |$ \boldsymbol{t} $ ]。由此可得当前帧f的位姿：$$ {\boldsymbol{T}}_{f}={\boldsymbol{T}}_{k-1}{\Delta }{\boldsymbol{T}}_{k-1,f} $$ (5) 式中
$ {\Delta }{\boldsymbol{T}}_{k-1,f} $ 为当前帧f和前一个关键帧k−1之间的位姿变换矩阵。假定移动机器人运动状态改变为低速匀速状态，可将前次相对变换用作NDT 配准初值以达到快速计算收敛的目的。当移动机器人当前帧相对于前一个关键帧位姿变化超过设定阈值时，就会造成 NDT 不收敛，前一个关键帧及相对变换就会用来计算当前输入点云的里程。当前帧f和前一个关键帧k−1之间的位姿变换矩阵可表示如下：
$$ {\Delta }{\boldsymbol{T}}_{k-1,f}={\boldsymbol{T}}_{k-1}^{-1}{\boldsymbol{T}}_{f}=\left[ \begin{array}{cc}\mathrm{\Delta }\boldsymbol{h}& \mathrm{\Delta }\boldsymbol{t}\\ {\bf{0}}& {\bf{1}}\end{array} \right] $$ (6) 式中：
$ {\Delta }\boldsymbol{h} $ 为旋转变化；$ {\Delta }\boldsymbol{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所示。
煤矿巷道多方法集成激光里程计的无约束优化目标函数定义为
$$ \widehat{\boldsymbol{\chi }}=\mathrm{a}\mathrm{r}\mathrm{g}\underset{\boldsymbol{\chi }}{\mathrm{max}}\left\{{\phi }_{\mathrm{p}\mathrm{r}\mathrm{i}\mathrm{o}\mathrm{r}}\left(\boldsymbol{\chi }\right)+{\phi }_{{\rm{ICP}}}\left(\boldsymbol{\chi }\right)+{\phi }_{{\rm{NDT}}}\left(\boldsymbol{\chi }\right)\right\} $$ (7) 式中：
$ \boldsymbol{\chi } $ 为待优化的位姿参数；$ {\phi }_{\mathrm{p}\mathrm{r}\mathrm{i}\mathrm{o}\mathrm{r}}\left(\boldsymbol{\chi }\right) $ 为先验因子的代价项；$ {\phi }_{{\rm{ICP}}}\left(\boldsymbol{\chi }\right) $ 为使用ICP配准的代价项；$ {\phi }_{{\rm{NDT}}}\left(\boldsymbol{\chi }\right) $ 为使用NDT配准的代价项。$$ {\phi }_{\mathrm{p}\mathrm{r}\mathrm{i}\mathrm{o}\mathrm{r}}\left(\boldsymbol{\chi }\right)=\frac{1}{2}{||{{r}}_{{\rm{prior}}}-{\boldsymbol{H}}_{{\rm{prior}}}\boldsymbol{\chi }||}^{2} $$ (8) $$ {\phi }_{{\rm{ICP}}}\left(\boldsymbol{\chi }\right)=\frac{1}{2}\sum _{k\in \{w+1,\cdots,u,v\}}{||{{r}}_{{\rm{ICP}}}({{\boldsymbol{z}}}_{{k+1}}^{{k}},\boldsymbol{\chi })||}_{{{\boldsymbol{\varOmega }}}_{{k+1}}^{{k}}}^{2} $$ (9) $$ {\phi }_{{\rm{NDT}}}\left(\boldsymbol{\chi }\right)=\frac{1}{2}\sum _{k\in \{w+1,\cdots,u,v\}}{||{{r}}_{{\rm{NDT}}}({{\boldsymbol{ z'}}}_{{k+1}}^{{k}},\boldsymbol{\chi })||}_{{{ \boldsymbol{ \varOmega' }}}_{{k+1}}^{{k}}}^{2} $$ (10) 式中：
$ {{r}}_{{\rm{prior}}} $ 和$ {\boldsymbol{H}}_{{\rm{prior}}} $ 分别为先验因子的残差和海森矩阵；$ w $ 为先验帧的索引；u和v为关键帧；$ {{r}}_{{\rm{ICP}}} $ 为ICP配准的残差；${{\boldsymbol{ z}}}_{{k+1}}^{{k}}$ 为ICP观测向量；$ {{\boldsymbol{\varOmega }}}_{{k+1}}^{{k}} $ 和${{\boldsymbol{ \varOmega' }}}_{{k+1}}^{{k}}$ 分别为ICP和NDT方法所得前后2帧的协方差矩阵；$ {{{r}}}_{{\rm{NDT}}} $ 为NDT配准的残差；${\boldsymbol{z'}}_{{{k}+1}}^{{{k}}}$ 为NDT观测向量。通过 ICP 配准方法得到2帧之间的位姿相对变换，可以构建 ICP相对位姿因子，提供给新的关键帧与已优化的关键帧位姿之间的约束。构建的ICP 相对位姿之间的优化目标项为
$$ {\phi }_{{\rm{ICP}}}\left({\boldsymbol{x}}\right)=\frac{1}{2}{||{{r}}_{{\rm{ICP}}}({\boldsymbol{x}}_{u},{\boldsymbol{x}}_{v})||}_{{{ \boldsymbol{\varSigma } _T}}}^{2} $$ (11) 式中： x 为优化变量；
$ {\boldsymbol{x}}_{u} $ 为关键帧 u 的索引对应的运动状态；$ {\boldsymbol{x}}_{v} $ 为关键帧 v 的索引对应的运动状态；${\boldsymbol{\varSigma}}_{T} $ 为相对位姿变换协方差矩阵，${\boldsymbol{\varSigma}}_{T} $ ∈ R6×6，R为旋转矩阵。在多方法集成过程中，状态变量是基于移动机器人本体坐标系。因此，转换到移动机器人本体坐标系下的相对位姿观测为
$$ {\boldsymbol{T}}_{v}^{u}={\boldsymbol{T}}_{B}^{L}{\boldsymbol{T}}_{{B}_{u}}^{O-1}{\boldsymbol{T}}_{{B}_{v}}^{O}{\boldsymbol{T}}_{B}^{L-1} $$ (12) 式中：
$ {\boldsymbol{T}}_{B}^{L} $ 为移动机器人本体坐标系到雷达坐标系O的外参变换；$ {\boldsymbol{T}}_{{B}_{u}}^{O},{\boldsymbol{T}}_{{B}_{v}}^{O} $ 为关键帧 u 和 v 对应的激光惯性里程计坐标系下的待估计位姿，$ {\boldsymbol{T}}_{{B}_{u}}^{O},{\boldsymbol{T}}_{{B}_{v}}^{O}\in SE\left(3\right)$ （三维欧氏空间的数学表示）。与 ICP 相对位姿因子构建方法类似，通过 NDT 相似度配准方法得到的2帧之间的位姿相对变换，可以构建 NDT 相对位姿因子，提供给新关键帧与已优化关键帧位姿之间的约束。构建的NDT相对位姿之间的优化目标项为
$$ {\phi }_{{\rm{NDT}}}\left({\boldsymbol{x}}\right)=\frac{1}{2}{||{{r}}_{{\rm{NDT}}}({\boldsymbol{x}}_{u},{\boldsymbol{x}}_{v})||}_{{\boldsymbol{\varSigma } _{T}}}^{2} $$ (13) 3. 方法验证
为验证INE−SLAM 方法的有效性，将在公开数据集和模拟煤矿巷道点云数据集中开展实验。
3.1 KITTI 数据集实验
采用目前广泛使用的 KITTI数据集[27]进行实验，以便于方法对比与复现。将本文提出的 INE−SLAM方法分别与基于特征点匹配的 A−LOAM方法、基于平面分割及特征点提取的 LeGO−LOAM 方法进行对比。基于 KITTI 里程计数据集的原始数据序列 00，01，05，09进行了对比实验，并与数据集提供的真实位姿数据进行对比。3种方法的轨迹对比如图3所示。
从图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。
从图4、图5、表1、表2可看出：INE−SLAM方法在 RPE 指标上有良好的发挥, 在控制RPE的最大值上表现最好；从建图的全局一致性上，通过 APE 指标可看出，INE−SLAM方法还有一定不足，能与 A−LOAM 和 LeGO−LOAM 方法达到相近的水平。这是因为在多方法集合的因子图中，NDT 相对位姿因子是基于概率模型，相较于 ICP 相对位姿因子存在更大的误差，在进行基于信息矩阵的融合时，由于误差的存在，会使整体优化在全局一致性上产生持续的误差累计。
表 1 APE对比Table 1. Absolute pose error comparison序列 方法 APE/m 最大值 平均值 中位数 最小值 均方根误差 和方差 标准偏差 A−LOAM 7.708074 2.014421 1.705119 0.291645 2.465952 27558.715930 1.422330 00 LeGO−LOAM 8.557403 4.429299 4.409769 0.351308 4.866022 107309.456000 2.014814 INE−SLAM 17.083902 7.400451 7.140376 0.808981 8.020236 291517.204500 3.091523 A−LOAM 35.109976 16.999973 18.074131 0.728562 18.893051 391928.228800 8.243076 01 LeGO−LOAM 290.476267 201.358024 224.237243 113.022321 210.274942 48548675.320000 60.584632 INE−SLAM 297.638420 203.560376 226.902531 112.478793 212.970114 49801183.740000 62.605453 A−LOAM 8.140921 1.971348 1.592937 0.228409 2.373178 15527.353030 1.321272 05 LeGO−LOAM 9.238267 2.139186 1.827800 0.482764 2.519107 17495.642090 1.330332 INE−SLAM 8.649879 2.256443 1.996284 0.472615 2.514627 17433.470720 1.109870 A−LOAM 3.285654 1.331722 1.012147 0.242015 1.545941 3795.214464 0.785143 09 LeGO−LOAM 6.063340 2.108868 1.971362 0.292446 2.343031 8717.794097 1.021015 INE−SLAM 7.315510 2.622855 2.616781 0.112678 2.840584 12813.436660 1.090663 表 2 RPE对比Table 2. Relative pose error comparison序列 方法 RPE/m 最大值 平均值 中位数 最小值 均方根误差 和方差 标准偏差 A−LOAM 1.896651 1.154897 1.200041 0.003222 1.215805 6697.636647 0.379992 00 LeGO−LOAM 0.972212 0.061612 0.047549 0.003754 0.076631 26.607482 0.045567 INE−SLAM 0.514520 0.092929 0.078107 0.004084 0.109881 54.707031 0.058635 A−LOAM 3.885951 3.132486 3.553323 1.028025 3.218495 11363.507790 0.739085 01 LeGO−LOAM 2.699271 0.517959 0.169555 0.016541 0.831417 758.306416 0.650364 INE−SLAM 2.604537 0.517871 0.170409 0.020407 0.831250 758.001303 0.650220 A−LOAM 1.716019 1.123840 1.217291 0.000759 1.196405 3944.896806 0.410327 05 LeGO−LOAM 0.942990 0.053947 0.046318 0.002914 0.063128 10.983231 0.032786 INE−SLAM 0.883468 0.056378 0.047659 0.003852 0.065977 11.996595 0.034271 A−LOAM 2.197394 1.505677 1.491705 0.323694 1.550710 3816.262265 0.370999 09 LeGO−LOAM 0.334782 0.063748 0.057174 0.005735 0.070704 7.933544 0.030582 INE−SLAM 0.315042 0.064674 0.058037 0.008362 0.071649 8.146901 0.030836 由实验结果可见， INE−SLAM方法在全局一致性上与LeGO−LOAM方法相似，在建图局部精度上优于A−LOAM和LeGO−LOAM方法。
3.2 模拟巷道实验
为了验证所提出的 INE−SLAM 方法在煤矿井下巷道应用场景的性能，在模拟巷道进行了实验，如图6所示。
采用履带式移动机器人平台进行实验，搭载 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方法在模拟巷道的建图效果Figure 7. Mapping effect of ICP and NDT ensemble SLAM in simulated roadwayINE−SLAM方法的建图细节如图9所示，其中 图9(a)的点云尺寸放大如图9(b)所示，可以更好地显现巷道内的门型轨道架。巷道进入弧形弯道前收窄部分的细节如图10所示，可看出收窄部分的墙体明显，薄厚均匀，没有因激光里程计失效导致重影。
![]()
图 8 LeGO−LOAM 方法在模拟巷道的建图结果Figure 8. Mapping result of lightweight and ground-optimized LOAM in simulated roadway![]()
图 10 巷道收窄部分INE−SLAM 方法的建图细节Figure 10. Mapping details of ICP and NDT ensemble SLAM in narrowing roadway4. 结论
（1） 面向煤矿长直巷道特征点稀少导致激光里程计易失效的问题，提出了基于集成式因子图优化的煤矿巷道移动机器人三维地图构建方法。针对激光里程计在煤矿井下建图时由于时间产生的漂移误差，导致全局一致性差的问题，构建了 ICP 相对位姿因子和 NDT 相对位姿因子，共同作为约束输入因子图优化，能够使地图的全局一致性相对较高。在公开数据集和模拟巷道点云数据集中对该方法的性能进行了实验验证，结果表明：该方法可解决几何特征少，但点云分布特征明显的长直巷道的激光SLAM建图问题，与传统A−LOAM、LeGO−LOAM 方法相比，在室外场景中性能指标类似，但实时性强；在巷道场景下具有显著优势，通过因子图优化，可得到一致性较高的三维地图，提升了煤矿巷道三维地图构建的精度及鲁棒性。
（2） 该方法目前主要聚焦于通过提升局部位姿估计精度来实现煤矿巷道的地图构建，后续工作将研究如何提升煤矿巷道移动机器人的定位精度。
-
![]()
图 7 INE−SLAM方法在模拟巷道的建图效果
Figure 7. Mapping effect of ICP and NDT ensemble SLAM in simulated roadway
![]()
图 8 LeGO−LOAM 方法在模拟巷道的建图结果
Figure 8. Mapping result of lightweight and ground-optimized LOAM in simulated roadway
![]()
图 10 巷道收窄部分INE−SLAM 方法的建图细节
Figure 10. Mapping details of ICP and NDT ensemble SLAM in narrowing roadway
表 1 APE对比
Table 1 Absolute pose error comparison
序列 方法 APE/m 最大值 平均值 中位数 最小值 均方根误差 和方差 标准偏差 A−LOAM 7.708074 2.014421 1.705119 0.291645 2.465952 27558.715930 1.422330 00 LeGO−LOAM 8.557403 4.429299 4.409769 0.351308 4.866022 107309.456000 2.014814 INE−SLAM 17.083902 7.400451 7.140376 0.808981 8.020236 291517.204500 3.091523 A−LOAM 35.109976 16.999973 18.074131 0.728562 18.893051 391928.228800 8.243076 01 LeGO−LOAM 290.476267 201.358024 224.237243 113.022321 210.274942 48548675.320000 60.584632 INE−SLAM 297.638420 203.560376 226.902531 112.478793 212.970114 49801183.740000 62.605453 A−LOAM 8.140921 1.971348 1.592937 0.228409 2.373178 15527.353030 1.321272 05 LeGO−LOAM 9.238267 2.139186 1.827800 0.482764 2.519107 17495.642090 1.330332 INE−SLAM 8.649879 2.256443 1.996284 0.472615 2.514627 17433.470720 1.109870 A−LOAM 3.285654 1.331722 1.012147 0.242015 1.545941 3795.214464 0.785143 09 LeGO−LOAM 6.063340 2.108868 1.971362 0.292446 2.343031 8717.794097 1.021015 INE−SLAM 7.315510 2.622855 2.616781 0.112678 2.840584 12813.436660 1.090663 
下载: 导出CSV
表 2 RPE对比
Table 2 Relative pose error comparison
序列 方法 RPE/m 最大值 平均值 中位数 最小值 均方根误差 和方差 标准偏差 A−LOAM 1.896651 1.154897 1.200041 0.003222 1.215805 6697.636647 0.379992 00 LeGO−LOAM 0.972212 0.061612 0.047549 0.003754 0.076631 26.607482 0.045567 INE−SLAM 0.514520 0.092929 0.078107 0.004084 0.109881 54.707031 0.058635 A−LOAM 3.885951 3.132486 3.553323 1.028025 3.218495 11363.507790 0.739085 01 LeGO−LOAM 2.699271 0.517959 0.169555 0.016541 0.831417 758.306416 0.650364 INE−SLAM 2.604537 0.517871 0.170409 0.020407 0.831250 758.001303 0.650220 A−LOAM 1.716019 1.123840 1.217291 0.000759 1.196405 3944.896806 0.410327 05 LeGO−LOAM 0.942990 0.053947 0.046318 0.002914 0.063128 10.983231 0.032786 INE−SLAM 0.883468 0.056378 0.047659 0.003852 0.065977 11.996595 0.034271 A−LOAM 2.197394 1.505677 1.491705 0.323694 1.550710 3816.262265 0.370999 09 LeGO−LOAM 0.334782 0.063748 0.057174 0.005735 0.070704 7.933544 0.030582 INE−SLAM 0.315042 0.064674 0.058037 0.008362 0.071649 8.146901 0.030836
下载: 导出CSV
-
[1] 杨林,马宏伟,王岩,等. 煤矿巡检机器人同步定位与 地图构建方法研究[J]. 工矿自动化,2019,45(9):18-24. YANG Lin,MA Hongwei,WANG Yan,et al. Research on method of simultaneous localization and mapping of coal mine inspection robot[J]. Industry and Mine Automation,2019,45(9):18-24.
[2] 孙继平,江嬴. 矿井车辆无人驾驶关键技术研究[J]. 工矿自动化,2022,48(5):1-5,31. DOI: 10.13272/j.issn.1671-251x.17947 SUN Jiping,JIANG Ying. Research on key technologies of mine unmanned vehicle[J]. Journal of Mine Automation,2022,48(5):1-5,31. DOI: 10.13272/j.issn.1671-251x.17947
[3] 潘祥生,陈晓晶. 矿用智能巡检机器人关键技术研究[J]. 工矿自动化,2020,46(10):43-48. DOI: 10.13272/j.issn.1671-251x.2020080042 PAN Xiangsheng,CHEN Xiaojing. Research on key technologies of mine-used intelligent inspection robot[J]. Industry and Mine Automation,2020,46(10):43-48. DOI: 10.13272/j.issn.1671-251x.2020080042
[4] 周治国,曹江微,邸顺帆. 3D 激光雷达 SLAM 算法综述[J]. 仪器仪表学报,2021,42(9):13-27. ZHOU Zhiguo,CAO Jiangwei,DI Shunfan. Overview of 3D Lidar SLAM algorithms[J]. Chinese Journal of Scientific Instrument,2021,42(9):13-27.
[5] ZHANG Ji, SINGH S. LOAM: lidar odometry and mapping in real-time[C]. Robotics: Science and Systems Conference, Berkeley, 2014. DOI: 10.15607/RSS. 2014.X.007.
[6] HESS W, KOHLER D, RAPP H, et al. Real-time loop closure in 2D LIDAR SLAM[C]. IEEE International Conference on Robotics and Automation （ICRA） , Stockholm, 2016: 1271-1278.
[7] SHAN Tixiao, ENGLOT B. LeGO-LOAM: lightweight and ground-optimized lidar odometry and mapping on variable terrain[C]. IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）, Madrid, 2018: 4758-4765.
[8] SHAN Tixiao, ENGLOT B, MEYERS D, et al. LIO-SAM: tightly-coupled lidar inertial odometry via smoothing and mapping[C]. IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）, Madrid, 2020: 3106-3111.
[9] BESL P J,MCKAY N D. A method for registration of 3D shapes[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence,1992,14(2):239-256. DOI: 10.1109/34.121791
[10] LI Qingde ,GRIFFITHS J G. Iterative closest geometric objects registering[J]. Computers & Mathematics with Applications,2000,40(10/11):1171-1188.
[11] CHEN Hui, BHANU B. Contour matching for 3D ear recognition[C]. 7th IEEE Workshop on Applications of Computer Vision, Breckenridge, 2005: 123-128.
[12] HE Shijun, ZHAO Shiting, BAI Fan, et al. A method for spatial data registration based on PCA-ICP algorithm[C]. 3rd International Conference on Advanced Measurement and Test, Xiamen, 2013: 1063-1066.
[13] BIBER P, STRASSER W. The normal distributions transform: a new approach to laser scan matching[C]. IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）, Las Vegas, 2003: 2743-2748.
[14] KOIDE K,MIURA J,MENEGATTI E. A portable three-dimensional LIDAR-based system for long-term and wide-area people behavior measurement[J]. International Journal of Advanced Robotic Systems,2019,16(2):1729-8806.
[15] LI Menggang,ZHU Hua,YOU Shaoze,et al. Efficient laser-based 3D SLAM for coal mine rescue robots[J]. IEEE Access,2019,7:14124-14138. DOI: 10.1109/ACCESS.2018.2889304
[16] XU Jiachang,HUANG Yourui,ZHAO Ruijuan,et al. Probabilistic membrane computing-based SLAM for patrol UAVs in coal mines[J]. Journal of Sensors,2021:1-11. DOI: 10.1155/2021/7610126.
[17] 王佳婧,王晓南,郑顺义,等. 三维点云初始配准方法的比较分析[J]. 测绘科学,2018,43(2):16-23. DOI: 10.16251/j.cnki.1009-2307.2018.02.004 WANG Jiajing,WANG Xiaonan,ZHENG Shunyi,et al. Comparison and analysis of initial registration methods of 3D point cloud[J]. Science of Surveying and Mapping,2018,43(2):16-23. DOI: 10.16251/j.cnki.1009-2307.2018.02.004
[18] 李猛钢. 煤矿救援机器人导航系统研究[D]. 徐州: 中国矿业大学, 2017. LI Menggang. Research on navigation system of coal mine rescue robot [D]. Xuzhou: China University of Mining and Technology, 2017.
[19] SCHULZ C, ZELL A. Real-time graph-based SLAM with occupancy normal distributions transforms [C]. IEEE International Conference on Robotics and Automation （ICRA）, Paris, 2020: 3105-3111.
[20] PARK C, MOGHADAM P, KIM S, et al. Elastic LiDAR fusion: dense map-centric continuous-time SLAM[C]. IEEE International Conference on Robotics and Automation （ICRA）, Brisbane, 2018: 1-25.
[21] QIN Chao, YE Haoyang, PRANATA C E, et al. LINS: a lidar-inertial state estimator for robust and efficient navigation[C]. IEEE International Conference on Robotics and Automation （ICRA）, Paris, 2020: 8899-8906.
[22] KSCHISCHANG F,FREY B,LOELIGER H. Factor graphs and the sum-product algorithm[J]. IEEE Transactions on Information Theory,2001,47(2):498-498. DOI: 10.1109/18.910572
[23] KUMMERLE R, GRISETTI G, STRASDAT H, et al. G2o: a general framework for graph optimization[C]. IEEE International Conference on Robotics and Automation, Shanghai, 2011: 3607-3613.
[24] KAESS M,DELLAERT F. iSAM:incremental smoothing and mapping[J]. IEEE Transactions on Robotics,2008,24(6):1365-1378.
[25] KAESS M, JOHANNSSON H, ROBERTS R, et al. iSAM2: incremental smoothing and mapping with fluid relinearization and incremental variable reordering[C]. IEEE International Conference on Robotics and Automation, Shanghai, 2011. DOI: 10.1109/ICRA.2011.5979641.
[26] DELLAERT F. Factor graphs and GTSAM: a hands-on introduction [EB/OL]. （2016-01-13） [2022-09-16]. https://www.docin.com/p-1444792601.html.
[27] GEIGER A,LENZ P,STILLER C,et al. Vision meets robotics:the KITTI dataset[J]. The International Journal of Robotics Research,2013,32(11):1231-1237. DOI: 10.1177/0278364913491297
计量
- 文章访问数:
- HTML全文浏览量:
- PDF下载量:
