Improvement of point cloud feature extraction and alignment algorithms and lidar slam in coal mine underground
-
摘要:
激光SLAM在狭长局促的煤矿井下非结构化环境中面临退化问题的挑战，在特征很少或者特征复杂的情况下，位姿估计不准确，导致建图失真甚至失败。针对煤矿井下非结构退化环境中激光点云特征提取与配准困难的问题，提出了一种融合FPFH和ICP算法的二阶段点云特征提取及配准的方法，首先构建源点云和目标点云的kd-tree结构，利用统计滤波和体素滤波降低点云数量，提取点云表面法线并计算关键点云的快速点特征直方图特征描述子，再利用采样一致性初始配准算法进行粗配准，然后采用ICP算法进行精配准，以提升点云配准的准确性和位姿估计的精度；改进了LIO-SAM算法的特征提取和配准算法以及后端回环因子图优化算法，提升其关键局部特征识别与配准能力，利用Scan Context全局描述符闭环因子改善其闭环检测性能，以获得精确的全局一致性地图。利用M2DGR公开数据集测试了改进算法的性能，开展了2种煤矿井下典型场景的模拟SLAM试验，验证改进算法在特征提取和配准方面的有效性。测试和试验结果表明：与传统的LIO-SAM算法相比，改进算法的位姿估计和点云配准精度更高，相对位姿误差的平均误差改善了6.52%，绝对位姿误差的最大误差降低了18.84%，构建的地图无明显失真，建图误差均在1%以内，可实现非结构化退化环境的高精度全局一致性地图构建。
Abstract:LiDAR SLAM faces challenges in the narrow and confined unstructured environment of underground coal mines, where inaccurate point cloud pose estimation due to few or complex features can result in distortion or even map construction failure. To address the difficulties in LiDAR point cloud feature extraction and registration in this degraded environment, a two-stage method integrating FPFH and ICP algorithms is proposed. Initially, the method constructs kd-tree structures for the source and target point clouds, reduces point cloud numbers through statistical and voxel filtering, extracts point cloud surface normal, and computes fast point feature histogram descriptors for key points. Subsequently, a coarse registration is performed using the sampling consistency initial registration algorithm, followed by fine registration using the ICP algorithm to enhance point cloud registration accuracy and pose estimation precision. Furthermore, enhancements are made to the feature extraction and registration algorithm of the LIO-SAM, along with the optimization algorithm of the back-end loopback factor, to improve key local feature identification and registration capabilities. The addition of the Scan Context global descriptor loop factor enhances loop detection accuracy for consistent global mapping. Experimental testing on the M2DGR public dataset and SLAM experiments in simulated coal mine scenarios demonstrate the effectiveness of the improved algorithm in feature extraction and registration of the point clouds. Compared to the traditional LIO-SAM algorithm, the improved algorithm showcases higher accuracy in pose estimation and point cloud registration, with a 6.52% improvement in average relative position error and an 18.84% reduction in maximum absolute position error. The resulting maps exhibit no obvious distortion and mapping errors are within 1%, allowing for the construction of high-precision consistent global maps in unstructured and degraded environments.
-
Keywords:
- coal mine underground /
- SLAM /
- LIO-SAM /
- unstructured environment /
- point cloud registration
-
0. 引 言
煤矿智能化建设目前正处于初级阶段，大力应用和发展煤矿机器人等智能装备已成为国家煤炭能源发展战略和行业共识。煤矿机器人在煤矿井下巷道狭长封闭空间中的定位与导航面临着极大的挑战[1]，即时定位与建图（Simultaneous Localization and Mapping，SLAM）是其亟待解决的核心关键技术之一[2]。激光雷达具有不受粉尘遮挡以及光照变化影响的特点，可以实时获得精确的三维空间的距离信息，适用于煤矿井下低光照、高粉尘环境定位与建图。LIO-SAM[3]算法为具有代表性的激光IMU紧耦合SLAM算法，具有较好的精度和实时性。煤矿井下为典型的半结构化或非结构化环境，GPS拒止，巷道两帮环境相似，难以获得准确的特征匹配以及位姿估计，制约了其建图和定位效果。
点云特征提取通过点云帧处理，获得其有效的特征点以用于后续点云匹配，目前主要有单点特征提取和局部特征提取方法。单点特征方法主要有基于曲率的方法和基于主成分分析（Principal Component Analysis，PCA）方法。LOAM[4]算法即采用基于点云曲率的方法区分边缘特征点和平面特征点。GUO等[5]和CHENG等[6]利用PCA算法提取激光点云特征值来表征点云特征。点云局部特征方法是用SHOT[7]、B-SHOT[8]等局部描述子来表征激光点云帧中关键点邻域的几何特征，常用的关键点检测算法有3DSift[9]、3DHarris[10]、3D-SURF[11]、ISSs[12]等。煤矿井下巷道壁面点云具有复杂纹理结构，单点特征无法获得稳定有效的点云特征，需采用点云局部特征来提升建图匹配精度。
点云配准是利用两帧点云的特征进行匹配，获得其位姿转换关系，有粗配准和精配准之分。粗配准是较为粗糙的配准，以获得完全未知情况下两帧点云间的简单位姿关系。随机采样一致性（Random Sample Consensus，RANSAC）配准算法[13]通过随机抽样估计模型参数迭代寻优的配准算法，效率较低。LOAM系列算法基于平面和边缘特征，采用两步LM（Levenberg-Marquardt）算法提升配准精度。基于RANSAC粗配准搭配精配准的二阶段配准方法[14]考虑了两帧点云的表面法线和曲率多个因素，精度较高。精配准主要有基于正态分布变换[15]（Normal Distributions Transform，NDT）和基于迭代最近点[16]（Iterative Closest Point，ICP）或其变体的配准算法[17-18]。NDT算法是基于概率的点云配准算法，速度快但精度不高。ICP算法以欧氏距离误差最小作为目标函数，通过迭代的方式寻找最佳变换关系，配准精度高。
闭环检测是SLAM算法保持状态估计的一致性和收敛性，减少机器人位姿估计累积误差的方法。M3RSM根据两帧激光数据的几何相似性来判断闭环[19]。LIO-SAM利用ICP算法结合欧氏距离进行闭环检测，但准确率不高[3]。GLAROT-3D利用平移和旋转不变的3D全局描述符检测闭环[20]。Seed则将分割点云拓扑信息编码到全局描述符中，利用全局描述符检测闭环[21]。SegMatch[22]和SegMap[23]针对点云数据稀疏问题，将一个点云聚为多个片段并提取片段特征，使用KNN（K-Nearest Neighbor）算法识别闭环。Scan Context方法则利用存储高度值的二维矩阵数据作为描述符，并采用两阶段搜索算法检测闭环[24]。
针对煤矿井下环境中特征模糊、环境复杂的问题，RAVAL等[25]通过在激光雷达的数据中结合惯性测量单元（Inertial Measurement Unit，IMU）并引入了统计离群点算法，减少了环境噪声对系统的影响和累计误差。上海交通大学DONG等[26]提出了一种适用于巷道环境的点云编码和快速关键帧匹配算法，以解决巷道环境特征难提取的问题。上海交通大学HUANG等[27]针对煤矿井下退化环境中点云特征稀疏问题，利用PCA方法对点云进行分割进而提取稳定的平面和线性特征。REN等[28-29]研究了煤矿井下的3D激光SLAM算法，将基于广义迭代最近点（GICP，Generalized Iterative Closest Point）的连续帧点云配准和连续关键帧之间的点云配准相结合，并将巷道平面作为节点约束，设计了基于规则和GICP的高效闭环检测。邹筱瑜等[30]为解决井下退化环境导致特征点缺失、配准精度低的问题，提出一种融合ICP和NDT的精配准方法。薛光辉等[31]针对煤矿井下地图构建问题，提出SegMatch描述子改进LEGO-LOAM回环检测模块，提高煤矿井下的建图精度。韩超等[32]利用特征向量与中心点坐标系尺度分布关系构建全局描述子，提升后端闭环系统的运算速度和准确性。杨林等[33]在SLAM后端引入关键帧和地面约束的图优化降低系统累计误差，提升机器人定位建图精度。
针对煤矿井下非结构退化环境的激光SLAM算法，国内外学者已开展了较多研究，但在点云特征提取、点云配准和闭环检测方面仍存在精度不高等问题。为实现煤矿井下环境精确鲁棒的地图构建以及位姿估计，笔者基于LIO-SAM算法框架，提出了一种融合基于快速点特征直方图（Fast Point Feature Histograms，FPFH）特征和ICP算法的二阶段匹配算法改进LIO-SAM的特征提取和匹配算法，利用Scan Context闭环因子加入图优化过程改进算法，加强闭环检测的有效性和准确性，以增强SLAM算法在煤矿井下环境建图定位的准确性以及鲁棒性，并利用公开数据集和自备数据集分别进行试验验证，以为煤矿井下机器人的定位与导航提供新思路。
1. 点云特征提取和配准方法
基于特征的点云配准的大致流程为：① 点云的关键点提取；② 计算关键点的特征描述符；③ 基于最近邻匹配获得初始的对应关系；④ 通过RICP算法计算最终的变换关系。
1.1 点云特征提取
点云关键特征点提取能够提高点云配准的精度和效率。相较于其他几何特征，表面法向量能够描述被检测物体的表面细节信息，因此笔者采用直接估计法求取表面法向量。但仅利用单一的点云表面法向量区分点云的效果有限，因此引入具有较好的特征描述性及鲁棒性的点特征直方图（Point Feature Histograms，PFH）[34]以及快速点特征直方图（Fast PFH，FPFH）[35]局部描述子来提取点云特征。
PFH描述子首先通过法线估计得到点云表面变化，然后计算每个点的特征直方图，再通过比较PFH的相似度以确定对应点关系，以完成对应点的配准。图1表示了计算查询点pq（位于球形区域中心的红点）FPH的影响区域，查询点pq与其邻域半径r的所有k近邻点相互连接在一个网络中，计算该邻域内所有两点之间的关系得到PFH描述子。为计算查询点对ps、pt及其邻域点的法线ns、nt之间的相对偏差，定义查询点ps固接的局部坐标系如图2所示，其中坐标轴U由法线ns方向确定，是ps点的表面法向量；v垂直于ns和矢量[pt-ps]，W轴由右手法则确定。
![]()
图 2 查询点ps固接的局部坐标系示意Figure 2. Schematic diagram of local coordinate system fixed on querying point pS则ns、nt之间的相对偏差可用$ \alpha $、$ \phi $、$ \theta $表示，计算式如下：
$$ \alpha = {\boldsymbol{v}} {{\boldsymbol{n}}_t} $$ (1) $$ \phi = {\boldsymbol{u}} \frac{{{p_t} - {p_s}}}{{{{\left\| {{p_t} - {p_s}} \right\|}_2}}} $$ (2) $$ \theta = \arctan \left( {{\boldsymbol{w}} {{\boldsymbol{n}}_t},{\boldsymbol{u}} {{\boldsymbol{n}}_t}} \right) $$ (3) 式中：${\left\| {{p_t} - {p_s}} \right\|_2}$为查询点对pt与ps欧式距离。 u、v和w为3个坐标轴U、 V和W的单位矢量。
则查询点对ps、pt的位置和法线12个参数就被简化为$ \alpha $、$ \phi $、$ \theta $ 3个特征参数。然后将3个特征参数等分为若干个子区间，统计落在每个区间内点云数，就得到了点云的PFH特征。
PFH具有旋转不变性，鲁棒性好，但计算复杂性高，计算效率低。FPFH在PFH的基础上，通过计算查询点pq的k邻域中各个点的简化点特征直方图（Simplified Point Feature Histograms，SPFH）（仅利用图3中红线连接的对应点对），然后将所有的SPFH加权得到的FFPH特征，在保持描述子识别特性的同时，降低了算法复杂度，计算效率更高。计算式为
$$ \mathrm{F}\mathrm{P}\mathrm{F}\mathrm{H}\left({p}_{q}\right)=\mathrm{S}\mathrm{P}\mathrm{F}\mathrm{H}\left({p}_{q}\right)+\frac{1}{k}\displaystyle\sum_{m=1}^{k}\frac{1}{{\omega }_{km}}\mathrm{S}\mathrm{P}\mathrm{F}\mathrm{H}\left({p}_{km}\right) $$ (4) 式中：权重ωm为查询点pq与其近邻点pm之间的距离；pm为查询点pq的第m个近邻点。
1.2 点云配准
首先利用基于FPFH特征描述子的粗配准获得源点云和目标点云的旋转和平移关系初始估计值，再利用基于ICP算法的精配准通过迭代使误差最小化以完成点云配准。为提高计算速度，在点云配准过程中对点云进行滤波及下采样以精简数据量，并利用kd-tree构建数据结构。点云配准流程如图4所示。
粗配准阶段，首先建立源点云和目标点云的kd-tree数据结构，利用基于k近邻点计算的统计滤波算法去除离群点，再经过基于体素网格的下采样降低数据量，以保持点云形状的同时提高点云配准速度；然后，估计点云表面法线并计算关键点的FPFH特征描述子；最后，利用采样一致性初始配准算法（Sample Consensus Initial Alignment, SAC-IA）进行点云的粗配准，获得源点云和目标点云旋转和平移的初始位姿参数。
精配准阶段，首先根据粗配准获得的转换矩阵，限定参数剔除错误点对，并构建目标函数，即两点间的欧氏距离；计算迭代误差，当迭代误差满足设定的阈值或达到迭代次数时，停止迭代并输出变换矩阵，完成点云精配准。
1.3 性能验证
为验证本文提出的点云特征提取与配准算法的效果，从非结构退化模拟环境SLAM试验中获取的激光点云数据中，分别选取了大平移、小旋转和大旋转、小平移的激光点云进行了点云配准试验，并与3D Shape Context特征描述子（3D SC）、NDT算法和PFH特征描述子的配准方法进行了对比。
图5为不同配准算法对大平移、小旋转的两帧激光点云进行配准的结果，图中用不同颜色来区分隶属于不同帧的激光点云数据。从图5中可以看出，各个算法对点云平移变化的配准结果都普遍较好。为准确量化不同配准算法的配准效果，统计了其配准误差与所用的时间，结果见表1。由表1可知，NDT算法配准的所用时间最短，其次是本文算法，然后是PFH算法与3D SC算法；本文算法的旋转误差最小，PFH算法次之，NDT算法与3D SC算法最大；NDT算法平移误差最小，本文算法与PFH算法次之，最后是3D SC算法。本文采用的算法对旋转的配准具有优势。
![]()
图 5 不同配准算法对大平移、小旋转激光点云的配准结果Figure 5. Registration results of different algorithms for LiDAR point clouds with large translation and small rotation表 1 不同配准算法对大平移、小旋转的点云配准效果对比Table 1. Comparison of registration effects of different algorithms on point clouds with large translation and small rotation算法 时间/s 旋转误差/（°） 平移误差/mm X轴 Y轴 Z轴 总旋转 X轴 Y轴 Z轴 总平移 3D SC 24.724 5.807×10−5 2.18×10−4 −0.783 0.784 50.092 −0.292 −0.197 50.102 NDT 0.237 0.003 0.012 −0.784 0.784 0.085 −0.193 −0.106 0.236 PFH 4.390 7.712×10−9 3.127×10−9 −0.783 0.783 2.500 −0.300 −0.200 2.526 本文算法 2.294 4.359×10−9 4.893×10−9 0.783 0.783 2.500 −0.300 −1.999 2.526 图6为不同配准算法对大旋转，小平移的两帧激光点云的配准结果。从图6中看出，各个算法对大旋转情况下的配准效果差异较大，本文提出的算法效果最好。表2列出了不同算法对大旋转、小平移激光点云的配准误差与配准计算时间。由表2可知，NDT算法配准计算时间最短，其次是本文算法，3D SC算法最长；本文算法的旋转误差最小，为0.716，其次是PFH算法，再次是NDT算法与3D SC算法；NDT算法平移误差最小，其次是3D SC算法与本文算法，最大是PFH算法。
![]()
图 6 不同配准算法对大旋转、小平移两帧点云的配准结果Figure 6. Registration results of two frames of point clouds with large rotation and small translation表 2 不同算法对大旋转、小平移的点云配准时间与误差Table 2. Point cloud registration time and error of different algorithms for large rotation and small translation算法 时间/s 旋转误差/（°） 平移误差/mm X轴 Y轴 Z轴 总旋转 X轴 Y轴 Z轴 总平移 3D SC 25.191 0.010 0.009 −0.780 0.789 1.626 0.606 −0.136 1.741 NDT 0.236 0.015 0.007 −0.782 0.782 0.019 −0.289 −0.190 0.346 PFH 5.861 0.008 0.022 −0.733 0.733 11.303 1.228 0.167 11.371 本文算法 3.082 0.003 0.019 0.716 0.716 2.074 0.559 0.016 2.148 综上所述，本文算法在2个配准过程中的旋转误差都是最小的，在旋转配准上有优势，平移误差与3D SC相当，在配准计算用时上较3D SC缩短了1个数量级，但平移误差和配准用时上较NDT算法要逊色。
2. 基于改进特征匹配算法的LIO-SAM算法
2.1 LIO-SAM算法
LIO-SAM算法一种具有代表性的激光惯性SLAM算法框架，其基本框架如图7所示。其前端利用与LeGO-LOAM相同的特征配准模块，通过基于局部平滑度的计算提取当前帧和匹配帧的边缘和曲面特征，并将当前点云中的点与地图中的边缘和曲面进行匹配；后端则采用因子图优化，添加了IMU预积分因子、激光惯性里程计因子、GPS因子以及闭环因子，以获得更精确的位姿估计和地图构建。
![]()
图 7 本文构建的融合IMU数据的激光SLAM算法框架Figure 7. LIDAR SLAM algorithm framework of IMU data fusion in this paper2.2 改进LIO-SAM算法框架
在LIO-SAM算法的基础上，融合前文的研究成果，前端采用基于FPFH特征描述子的粗配准方法及基于ICP算法的精配准的方案，后端继承基于因子图优化的紧耦合方案，构建了融合IMU数据的激光SLAM算法框架如图7所示。改进后的LIO-SAM算法实现了多传感器融合，信息互补，提高了整体精度，能够得到精度更高的点云地图。算法前端实现激光点云的去畸变，后端以因子图的形式实现紧耦合，激光雷达数据与IMU数据的紧耦合可以改善狭长空间环境的退化问题。
前端使用基于FPFH特征描述子的粗配准方法与基于ICP算法的精配准构建地图，改进原算法中基于曲率的方法的特征提取和基于特征点的配准方法，提升激光雷达对周围环境的特征点识别，识别环境中的局部特征点，并通过两次配准提升配准精度，从而增强煤矿井下机器人的位姿估计精度。后端继承了因子图优化方法，将因子图节点中的回环因子改为Scan Context回环因子，利用Scan Context全局描述符进行闭环检测，将得到的空间约束以闭环因子的形式加入至图优化中，以获得更精确的全局一致性地图。
2.3 算法流程
2.3.1 前端特征提取与配准
由于环境噪声或设备精度的影响，3D激光雷达获取的点云存在“离群点”，会造成点云配准不准确，降低配准效率[36]。本文采用基于k近邻点计算的统计滤波算法对点云数据进行降噪处理，以减少离群点数量。离群点滤波效果如图8所示，由图可见部分离群点已被去除。
![]()
图 8 离群点滤波前后效果对比示意Figure 8. Diagram comparing the effect before and after outlier filtering基于体素网格下采样是指将点云构建成体素网格，将体素网格基于哈希函数计算的重心来近似点云数据，并通过控制体素网格的大小控制下采样的比重。该方法计算量较小，也能够保留完整的点云局部特征信息。效果如图9所示，经过体素滤波后，点云原有的分布没有改变但密度下降，点云数量从3 431 547降至357 886，降低了89.57%。
然后，利用FPFH特征描述子提取点云特征。再利用两步配准方法对源点云到目标点云进行配准，获得使所有对应点的距离误差为最小的变换矩阵，作为激光里程计因子，用于后续位姿优化。
2.3.2 后端因子图优化
1）IMU预积分因子。因子图模型包括变量节点和因子节点。IMU预积分因子根据IMU误差模型及测量数据可解得位置、速度和姿态，描述关键帧之间的运动，通过增加因子图变量节点之间的约束进行优化。设i时刻IMU的姿态、速度和位置分别为Ri、vi和pi，利用i时刻至j时刻IMU的所有测量值，可更新得到j时刻的姿态Rj、速度vj和位置pj，其计算式为
$$ {R}_{j}={R}_{i}\prod \limits_{c=i}^{j-1}\mathrm{e}\mathrm{x}\mathrm{p}\left[\left({\widehat{\omega }}_{c}-{b}_{c}^{\omega }-{n}_{c}^{\mathrm{\omega }\mathrm{d}}\right)\Delta t\right] $$ $$ {v}_{j}={v}_{i}+{g}\Delta {t}_{ij}+\displaystyle\sum _{c=i}^{j-1}{R}_{c}\left({\widehat{a}}_{c}-{b}_{c}^{a}-{n}_{c}^{{a}\mathrm{d}}\right)\Delta t $$ $$ {p}_{j}={p}_{i}+\displaystyle\sum _{c=i}^{j-1}\left[{v}_{c}\Delta t+\frac{1}{2}\mathrm{g}\Delta {t}^{2}+\frac{1}{2}{R}_{c}\left({\widehat{a}}_{c}-{b}_{c}^{a}-{n}_{c}^{{a}\mathrm{d}}\right)\Delta {t}^{2}\right] $$ (5) 式中：$ {\widehat{\omega }}_{c} $和$ {\widehat{a}}_{c} $分别为c时刻角速度和加速度的测量值；$ {b}_{c}^{\omega } $和$ {b}_{c}^{a} $分别为时刻c角速度和加速度的测量偏差；$ {n}_{c}^{\omega {\mathrm{d}}} $为c时刻的角速度测量噪声，rad/s；$ {n}_{c}^{a{\mathrm{d}}} $为c时刻线加速度的测量噪声，m/s2；$ \Delta t $为采样时间间隔；g为重力加速度；$ {v}_{c} $和$ {R}_{c} $分别为c时刻的位置和速度；$ \Delta {t}_{ij}\triangleq \displaystyle\sum _{c=i}^{j-1}\Delta t $。
通过分解离散化运动方程的测量值，则得到的预积分因子如式（6）所示，利用预积分因子构建的残差方程则如式（7）所示。
$$ \left\{\begin{array}{l} \Delta {R}_{ij}=\displaystyle\prod _{c=i}^{j-1}\exp\left[\left({\widehat{\omega }}_{c}-{b}_{c}^{\omega }-{n}_{c}^{\omega {\mathrm{d}}}\right)\Delta t\right]\\ \Delta {v}_{ij}=\displaystyle\sum _{c=i}^{j-1}\Delta {{\boldsymbol{R}}}_{ic}\left({\widehat{a}}_{c}-{b}_{c}^{a}-{n}_{c}^{a{\mathrm{d}}}\right)\Delta t\\ \Delta {p}_{ij}=\displaystyle\sum _{c=i}^{j-1}\left[\Delta {v}_{ic}\Delta t+\frac{1}{2}\Delta {{\boldsymbol{R}}}_{ic}\left({\widehat{a}}_{c}-{b}_{c}^{a}-{n}_{c}^{a{\mathrm{d}}}\right)\Delta {t}^{2}\right] \end{array}\right. $$ (6) $$ {\boldsymbol{r}}=\left[\begin{array}{l}{r}_{p}\\ {r}_{v}\\ {r}_{\theta }\\ {r}_{b}^{a}\\ {r}_{b}^{\omega }\end{array}\right]= \left[\begin{array}{l}{{\boldsymbol{R}}}_{i}^{{\mathrm{T}}}\left({p}_{j}-{p}_{i}-\dfrac{1}{2}\Delta {t}_{ij}^{2}-{v}_{i}\Delta {t}_{ij}\right)-\displaystyle\sum _{c=i}^{j-1}\left(\Delta {v}_{ic}\Delta t+\dfrac{1}{2}\Delta {{\boldsymbol{R}}}_{ic}\left({\widehat{a}}_{c}-{b}_{c}^{a}\right)\Delta {t}^{2}\right)\\ {{\boldsymbol{R}}}_{i}^{{\mathrm{T}}}\left({v}_{j}-{v}_{i}-g\Delta {t}_{ij}\right)-\displaystyle\sum _{c=i}^{j}\Delta {{\boldsymbol{R}}}_{ic}\left({\widehat{a}}_{c}-{b}_{c}^{a}\right)\Delta t\\ {\log}\left({\left\{\displaystyle\prod _{c=i}^{j-1}{\mathrm{exp}}\left[\left({\widehat{\omega }}_{c}-{b}_{c}^{\omega }\right)\Delta t\right]\right\}}^{{\mathrm{T}}}{{\boldsymbol{R}}}_{i}^{{\mathrm{T}} }{{\boldsymbol{R}}}_{j}\right)\\ {b}_{j}^{a}-{b}_{i}^{a}\\ {b}_{j}^{\omega }-{b}_{i}^{\omega }\end{array}\right] $$ (7) 式中：$ {{\boldsymbol{R}}}_{i}^{{\mathrm{T}}} $为i时刻旋转矩阵的转置；$ {\Delta v}_{ic} $为i时刻到c时刻速度的变化量；$ {\Delta {\boldsymbol{R}}}_{ic} $为i时刻到c时刻旋转矩阵的变化量；$ {b}_{j}^{\omega }、{b}_{i}^{\omega }和b_{j}^{a}、{b}_{i}^{a} $分别为i和j时刻IMU角速度和加速度的测量偏差；rp为位置残差；rv为速度残差；$r_{\theta} $为姿态残差；$r_b^a $、$r_b^{\omega } $分别为IMU加速度和角速度bias残差。
后端图优化中，激光雷达数据与IMU数据以因子形式加入至图优化中，预积分因子添加了更多里程计的约束，通过IMU的位姿数据与雷达点云数据时空上的融合，从而实现测量数据的紧耦合，提高结果的准确性。
2）激光里程计因子。构建激光雷达里程计因子步骤如下：① 对激光点云预处理，构建kd-tree结构，计算关键特征点，提取关键特征帧；② 基于几何特征描述的粗配准结合ICP精配准，进行关键帧和地图的特征配准；③ 通过因子图优化计算最终的相对位姿，并进行地图更新。
直接利用点云构建地图会影响后端优化的效果，因此在构建地图时，将关键点特征描述子的点云添加至局部地图进行优化。利用关键帧进行地图构建，能够提高算法实时性，保存局部地图信息。
关键帧的特征提取方法与上文的特征提取方法相同，IMU数据结合激光数据去畸变，并通kd-tree结构查找特征点进行配准，最终将位姿变换加入至地图更新中。
3）回环因子。Scan Context是2018年Giseop Kim和Ayoung Kim提出的算法，将三维扫描得到的点云编码为一个二维矩阵作为描述符，用于三维激光雷达扫描数据的位置识别。其框架如图10所示，首先将局部关键点附近的点云的几何形状编码为图像，然后采用融合相似度评分和最近邻搜索两阶段搜索算法以实现高效、鲁棒的检测闭环。
基于Scan Context的闭环因子构建流程：首先对点云处理提取点云描述符，并通过特征点选取关键帧，利用ICP算法比较关键帧与相邻帧得到相似度，然后在因子图中添加约束，基于Scan Context的闭环节点，通过增加闭环约束进行全局优化，得到全局一致性的地图。
3. 试验结果与分析
为验证本文算法的有效性，选择M2DGR公共数据集[37]的hall_05序列进行测试，主要从建图轨迹长度、绝对位姿误差和相对位姿误差等指标进行算法的效果衡量和定量分析。同时，利用搭载激光雷达和IMU的试验平台开展了不同模拟场景下的SLAM试验，验证算法的有效性和和鲁棒性。
3.1 公开数据集测试结果与分析
M2DGR是由上海交大针对地面机器人导航采集的SLAM数据集，涵盖了电梯、街道、房间、大厅等多个室内外具有挑战性的场景，具有丰富的传感信息，包括视觉、LiDAR、IMU、GNSS、事件、热红外图像等。
该数据集中的室内场景hall为室内走廊场景，与煤矿井下水泵房、变电站的环境具有一定的相似性，故选择M2DGR公开数据集的hall_05序列对本文算法进行测试，采用EVO工具对比分析了LIO-SAM算法和本文改进后算法的绝对位姿误差（Absolute Pose Error，APE）和相对位姿误差（Relative Pose Error，RPE），结果如图11所示。
![]()
图 11 LIO-SAM算法与本文算法的APE和RPEFigure 11. APE and RPE of LIO-SAM algorithm and algorithm proposed in this paper图12显示了LIO-SAM算法和改进算法的相对位姿误差效果，颜色的深浅代表了误差的大小。由图12可知，LIO-SAM算法在弯道和重叠区域位姿估计存在不稳定的现象，而本文算法在相应位置表现更加平稳。
![]()
图 12 LIO-SAM算法与本文算法的RPE可视化效果Figure 12. RPE visualization effect of LIO-SAM algorithm and algorithm proposed in this paper表3为利用hall_05序列数据集测试LIO-SAM和本文算法时的APE和RPE统计结果。从表3可以看出，与LIO-SAM算法相比，本文算法的APE和RPE的最大误差、最小误差、平均误差均有改善，其中APE的平均误差改善了6.52%，RPE的最大误差降低了18.84%。本文算法估计得到的hall_05序列轨迹长度与实际长度仅相差了2.434 m，较改进前LIO-SAM算法提升了13.8%，表明本文算法估计的位姿更准确。
表 3 利用hall_05序列测试LIO-SAM和本文算法时的APE和RPE统计结果Table 3. APE and RPE statistical results when testing LIO-SAM and our algorithm using the hall_05 sequence dataset评价指标 LIO-SAM 本文算法 APE RPE APE RPE 最大误差 2.395 3 10.408 7 2.391 5 8.447 3 最小误差 0.028 1 0.001 4 0.026 2 0.001 3 平均误差 1.019 5 1.219 2 0.953 0 1.323 4 标准差 0.372 5 1.486 1 0.372 5 1.438 1 均方误差 1.022 3 1.922 3 1.023 2 1.954 4 综上所述，在M2DGR公开数据集hall_05序列测试中，本文算法相较于改进前的LIO-SAM算法具有更好的位姿估计和建图效果，估计精度更高，验证了改进算法的有效性。
3.2 模拟环境试验与结果分析
为验证本文算法在煤矿井下狭长巷道环境中的定位与建图效果，搭建了试验平台，开展了狭长走廊（模拟煤矿井下狭长巷道的“长廊”效应）和回形走廊（模拟煤矿井底车场等场景的回环效应）的环境测试试验。这2种场景与煤矿井下环境特点相似，如图13所示，特征相对较少且单一，易使机器人产生错误的位姿估计，对激光点云的特征提取和配准具有较高的挑战性。
试验平台采用履带式机器人底盘，搭载有16线的激光雷达RS-LiDAR-16和六轴的IMU HI229-6，如图14所示。激光雷达和IMU的采样频率分别为10 Hz和500 Hz。上位机采用工控机，硬件配置为8 GB内存、Intel Core i3-4100M和NVIDIA GeForce 940M，操作系统为Ubuntu 18.04+ROS melodic。
1）狭长走廊。利用本文算法对采集的狭长走廊激光点云和IMU数据进行了地图构建，如图15所示。
由图15可知，本文算法可较好的构建出狭长巷道环境地图，没有发生角度倾斜或者是建图不成功的情况。对构建的狭长走廊点云地图中关键尺寸进行了测量，并于用激光测距仪测得的尺寸进行了对比，结果见表4。由表4可知，长度和宽度方向上的绝对误差分别为0.175、0.042 m或相对误差分别为0.49%与0.57%，表明构建的地图精度较高。
表 4 狭长走廊建图尺寸及误差Table 4. Build dimensions and errors for Narrow corridor走廊 实际尺寸/m 建图尺寸/m 建图误差/m 相对误差/% 长度 36.086 35.911 0.175 0.49 宽度 2.114 2.102 0.012 0.57 2）回形走廊。回形走廊与煤矿井底车场特征较相似，同为封闭狭长、两帮结构特征相似的退化环境，难以为机器人位姿估计提供准确的特征信息，容易产生错误匹配，导致定位错误、建图失真甚至失败。
对比了LOAM、LEGO-LOAM、LIO-SAM和本文算法对回形走廊试验的建图效果，如图16所示。由图16可以看出，LOAM、LEGO-LOAM算法构建的地图出现了严重的失真，LIO-SAM算法效果要好一些，但已出现了明显的偏移现象，本文算法构建地图没有出现明显偏差，这是因为本文算法在LIO-SAM传统算法的基础上改进了点云特征提取算法，采用融合FPFH特征描述子的粗配准与ICP精配准算法能够准确识别点云的关键特征点并实现较高精度的点云配准，引入的scan context闭环因子也提升了闭环检测的性能，提升了位姿估计精度，从而可构建出准确的地图。
![]()
图 16 典型SLAM算法和本文算法对回型走廊建图效果Figure 16. Mapping effect of circular corridor by typical SLAM algorithm and our algorithm提取了本文算法构建的地图的关键尺寸并与激光测距仪的测量结果进行了对比，结果见表5。由表可知，本文算法构建的地图在横向、纵向和轨迹长度上的相对误差均在1%以内，表明本文算法在退化环境中构建的地图具有较高的精度。
表 5 本文算法构建的回形走廊地图关键尺寸及其误差Table 5. Key dimensions and errors of circular corridor map constructed by improved algorithm指标 实际尺寸/m 建图尺寸/m 建图误差/m 相对误差/% 横向长度 35.504 35.441 0.162 0.45 纵向长度 1.509 1.499 0.010 0.65 轨迹长度 27.506 27.324 0.182 0.66 4. 结 论
1）针对煤矿井下非结构退化环境点云特征提取和配准难的问题，提出一种融合FPFH和ICP的二阶段点云配准算法，首先构建源点云和目标点云的kd-tree数据结构，利用统计滤波和体素网格下采样降低点云数据量，估计点云表面法线并计算关键点的FPFH特征描述子，再利用SAC-IA算法进行点云粗配准，然后利用ICP迭代算法完成点云精配准，性能验证试验结果表明该算法在大平移、小旋转场景和小平移、大旋转场景下的旋转误差最小，可实现较高精度的点云配准。
2）在LIO-SAM算法框架基础上，前端改进了点云特征提取和配准算法，以提升非结构退化环境的关键局部特征点识别和点云配准能力，增强煤矿机器人位姿估计精度；后端改进了回环因子图优化方法，利用Scan Context全局描述符因子进行闭环检测，以获得更精确的全局一致性地图。利用M2DGR公开数据集hall_05序列对其性能进行了测试，结果表明改进后的算法具有更好的位姿估计和建图效果。
3）为验证本文算法应用在煤矿移动机器人上的可行性，利用搭载LiDAR和IMU的试验平台开展了狭长走廊和回形走廊2种煤矿井下模拟场景的SLAM试验，对比分析了典型SLAM算法和本文算法的建图效果，结果表明本文算法构建的地图没有出现明显的偏差，误差在1%以内，可构建出高精度的全局一致性地图。同时研究也发现本文提出的激光点云特征提取与配准算法在平移误差和用时上较NDT算法要逊色，改进后的LIO-SAM算法尚需要煤矿井下真实场景的试验验证，有待后续的研究。
-
![]()
图 2 查询点ps固接的局部坐标系示意
Figure 2. Schematic diagram of local coordinate system fixed on querying point pS
![]()
图 5 不同配准算法对大平移、小旋转激光点云的配准结果
Figure 5. Registration results of different algorithms for LiDAR point clouds with large translation and small rotation
![]()
图 6 不同配准算法对大旋转、小平移两帧点云的配准结果
Figure 6. Registration results of two frames of point clouds with large rotation and small translation
![]()
图 7 本文构建的融合IMU数据的激光SLAM算法框架
Figure 7. LIDAR SLAM algorithm framework of IMU data fusion in this paper
![]()
图 8 离群点滤波前后效果对比示意
Figure 8. Diagram comparing the effect before and after outlier filtering
![]()
图 11 LIO-SAM算法与本文算法的APE和RPE
Figure 11. APE and RPE of LIO-SAM algorithm and algorithm proposed in this paper
![]()
图 12 LIO-SAM算法与本文算法的RPE可视化效果
Figure 12. RPE visualization effect of LIO-SAM algorithm and algorithm proposed in this paper
![]()
图 16 典型SLAM算法和本文算法对回型走廊建图效果
Figure 16. Mapping effect of circular corridor by typical SLAM algorithm and our algorithm
表 1 不同配准算法对大平移、小旋转的点云配准效果对比
Table 1 Comparison of registration effects of different algorithms on point clouds with large translation and small rotation
算法 时间/s 旋转误差/（°） 平移误差/mm X轴 Y轴 Z轴 总旋转 X轴 Y轴 Z轴 总平移 3D SC 24.724 5.807×10−5 2.18×10−4 −0.783 0.784 50.092 −0.292 −0.197 50.102 NDT 0.237 0.003 0.012 −0.784 0.784 0.085 −0.193 −0.106 0.236 PFH 4.390 7.712×10−9 3.127×10−9 −0.783 0.783 2.500 −0.300 −0.200 2.526 本文算法 2.294 4.359×10−9 4.893×10−9 0.783 0.783 2.500 −0.300 −1.999 2.526 
下载: 导出CSV
表 2 不同算法对大旋转、小平移的点云配准时间与误差
Table 2 Point cloud registration time and error of different algorithms for large rotation and small translation
算法 时间/s 旋转误差/（°） 平移误差/mm X轴 Y轴 Z轴 总旋转 X轴 Y轴 Z轴 总平移 3D SC 25.191 0.010 0.009 −0.780 0.789 1.626 0.606 −0.136 1.741 NDT 0.236 0.015 0.007 −0.782 0.782 0.019 −0.289 −0.190 0.346 PFH 5.861 0.008 0.022 −0.733 0.733 11.303 1.228 0.167 11.371 本文算法 3.082 0.003 0.019 0.716 0.716 2.074 0.559 0.016 2.148
下载: 导出CSV
表 3 利用hall_05序列测试LIO-SAM和本文算法时的APE和RPE统计结果
Table 3 APE and RPE statistical results when testing LIO-SAM and our algorithm using the hall_05 sequence dataset
评价指标 LIO-SAM 本文算法 APE RPE APE RPE 最大误差 2.395 3 10.408 7 2.391 5 8.447 3 最小误差 0.028 1 0.001 4 0.026 2 0.001 3 平均误差 1.019 5 1.219 2 0.953 0 1.323 4 标准差 0.372 5 1.486 1 0.372 5 1.438 1 均方误差 1.022 3 1.922 3 1.023 2 1.954 4
下载: 导出CSV
表 4 狭长走廊建图尺寸及误差
Table 4 Build dimensions and errors for Narrow corridor
走廊 实际尺寸/m 建图尺寸/m 建图误差/m 相对误差/% 长度 36.086 35.911 0.175 0.49 宽度 2.114 2.102 0.012 0.57
下载: 导出CSV
表 5 本文算法构建的回形走廊地图关键尺寸及其误差
Table 5 Key dimensions and errors of circular corridor map constructed by improved algorithm
指标 实际尺寸/m 建图尺寸/m 建图误差/m 相对误差/% 横向长度 35.504 35.441 0.162 0.45 纵向长度 1.509 1.499 0.010 0.65 轨迹长度 27.506 27.324 0.182 0.66
下载: 导出CSV
-
[1] 葛世荣,胡而已,裴文良. 煤矿机器人体系及关键技术[J]. 煤炭学报,2020,45(1):455−463. GE Shirong,HU Eryi,PEI Wenliang. Classification system and key technology of coal mine robot[J]. Journal of China Coal Society,2020,45(1):455−463.
[2] 薛光辉,李瑞雪,张钲昊,等. 基于3D激光雷达的SLAM算法研究现状与发展趋势[J]. 信息与控制,2023,52(1):18−36. XUE Guanghui,LI Ruixue,ZHANG Zhenghao,et al. State-of-the-art and tendency of SLAM algorithms based on 3D LiDAR[J]. Information and Control,2023,52(1):18−36.
[3] SHAN T X,ENGLOT B,MEYERS D,et al. LIO-SAM:Tightly-coupled lidar inertial odometry via smoothing and mapping[C]//2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Piscataway,NJ:IEEE,2020:5135−5142.
[4] ZHANG J,SINGH S. LOAM:Lidar odometry and mapping in real-time [C/OL]//California:Robotics:Science and Systems. 2014,2(9):1−9[2022−12−20]. http://www.roboticsproceedings.org/rss10/p07.pdf.
[5] GUO S Y,RONG Z,WANG S,et al. A LiDAR SLAM with PCA-based feature extraction and two-stage matching[J]. IEEE Transactions on Instrumentation and Measurement,2022,71:8501711.
[6] CHENG D Y,ZHANG J C,ZHAO D J,et al. Automatic extraction of indoor structural information from point clouds[J]. Remote Sensing,2021,13(23):4930. doi: 10.3390/rs13234930
[7] SALTI S,TOMBARI F,DI STEFANO L. SHOT:Unique signatures of histograms for surface and texture description[J]. Computer Vision and Image Understanding,2014,125:251−264. doi: 10.1016/j.cviu.2014.04.011
[8] PRAKHYA S M,LIU B B,LIN W S. B-SHOT:A binary feature descriptor for fast and efficient keypoint matching on 3D point clouds[C]//2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Piscataway,NJ:IEEE,2015:1929−1934.
[9] SCOVANNER P,ALI S,SHAH M. A 3-dimensional sift descriptor and its application to action recognition[C]//Proceedings of the 15th ACM International Conference on Multimedia. New York:ACM,2007:357−360.
[10] SIPIRAN I,BUSTOS B. A robust 3D interest points detector based on harris operator[C]//Eurographics Workshop on 3D Object Retrieval,Norrköping:The Eurographics Association,2010:7−14.
[11] KNOPP J,PRASAD M,WILLEMS G,et al. Hough transform and 3D SURF for robust three dimensional classification[M]//Computer vision–ECCV 2010. Berlin:Springer Berlin Heidelberg,2010:589−602.
[12] ZHONG Y. Intrinsic shape signatures:A shape descriptor for 3D object recognition[C]//2009 IEEE 12th International Conference on Computer Vision Workshops,ICCV Workshops. Piscataway,NJ:IEEE,2009:689−696.
[13] DERPANIS K G. Overview of the RANSAC Algorithm[J]. Image Rochester NY,2010,4(1):2−3.
[14] 陈学伟,朱耀麟,武桐,等. 基于SAC-IA和改进ICP算法的点云配准技术[J]. 西安工程大学学报,2017,31(3):395−401. CHEN Xuewei,ZHU Yaolin,WU Tong,et al. The point cloud registration technology based on SAC-IA and improved ICP[J]. Journal of Xi’an Polytechnic University,2017,31(3):395−401.
[15] BIBER P,STRASSER W. The normal distributions transform:A new approach to laser scan matching[C]//Proceedings 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003). Piscataway,NJ:IEEE,2003:2743−2748.
[16] RUSINKIEWICZ S,LEVOY M. Efficient variants of the ICP algorithm[C]//Proceedings Third International Conference on 3-D Digital Imaging and Modeling. Piscataway,NJ:IEEE,2001:145−152.
[17] BESL P J,MCKAY N D. Method for registration of 3-D shapes [C]//Sensor fusion IV:Control paradigms and data structures. Piscataway,USA:Spie,1992,1611:586−606.
[18] CHEN Y,MEDIONI G. Object modelling by registration of multiple range images[J]. Image and Vision Computing,1992,10(3):145−155. doi: 10.1016/0262-8856(92)90066-C
[19] OLSON E. M3RSM:Many-to-many multi-resolution scan matching[C]//2015 IEEE International Conference on Robotics and Automation (ICRA). Piscataway,NJ:IEEE,2015:5815−5821.
[20] RIZZINI D L. Place recognition of 3D landmarks based on geometric relations[C]//2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Piscataway,NJ:IEEE,2017:648−654.
[21] FAN Y F,HE Y C,TAN U X. Seed:A segmentation-based egocentric 3D point cloud descriptor for loop closure detection[C]//2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Piscataway,NJ:IEEE,2020:5158−5163.
[22] DUBÉ R,DUGAS D,STUMM E,et al. SegMatch:Segment based place recognition in 3D point clouds[C]//2017 IEEE International Conference on Robotics and Automation (ICRA). Piscataway,NJ:IEEE,2017:5266−5272.
[23] DUBÉ R,CRAMARIUC A,DUGAS D,et al. SegMap:3D segment mapping using data-driven descriptors[EB/OL]. 2018:1804.09557[2022−12−20]. https://arxiv.org/abs/1804.09557v2.
[24] KIM G,KIM A. Scan context:Egocentric spatial descriptor for place recognition within 3D point cloud map[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Piscataway,NJ:IEEE,2018:4802−4809.
[25] RAVAL S,BANERJEE B P,KUMAR SINGH S,et al. A preliminary investigation of mobile mapping technology for underground mining[C]//2019 IEEE International Geoscience and Remote Sensing Symposium. Piscataway,NJ:IEEE,2019:6071−6074.
[26] DONG L F,CHEN W D,WANG J C. Efficient feature extraction and localizability based matching for lidar SLAM[C]//2021 IEEE International Conference on Robotics and Biomimetics (ROBIO). Piscataway,NJ:IEEE,2021:820−825.
[27] HUANG Z Y,LU J G. A new laser-based loop detection method for laneway environment 3D mapping[C]//2021 China Automation Congress (CAC). Piscataway,NJ:IEEE,2021:3219−3223.
[28] REN Z L,WANG L G,BI L. Robust GICP-based 3D LiDAR SLAM for underground mining environment[J]. Sensors,2019,19(13):2915. doi: 10.3390/s19132915
[29] REN Z L,WANG L G. Accurate real-time localization estimation in underground mine environments based on a distance-weight map (DWM)[J]. Sensors,2022,22(4):1463. doi: 10.3390/s22041463
[30] 邹筱瑜,黄鑫淼,王忠宾,等. 基于集成式因子图优化的煤矿巷道移动机器人三维地图构建[J]. 工矿自动化,2022,48(12):57−67,92. ZOU Xiaoyu,HUANG Xinmiao,WANG Zhongbin,et al. 3D map construction of coal mine roadway mobile robot based on integrated factor graph optimization[J]. Journal of Mine Automation,2022,48(12):57−67,92.
[31] 薛光辉,李瑞雪,张钲昊,等. 基于激光雷达的煤矿井底车场地图融合构建方法研究[J]. 煤炭科学技术,2023,51(8):219−227. XUE Guanghui,LI Ruixue,ZHANG Zhenghao,et al. Lidar based map construction fution method for underground coal mine shaft bottom[J]. Coal Science and Technology,2023,51(8):219−227.
[32] 韩超,陈敏,黄宇昊,等. 基于全局特征描述子的激光 SLAM 回环检测方法[J]. 上海交通大学学报,2022,56(10):1379. HAN Chao,CHEN Min,HUANG Yuhao,et al. Loop closure detection method of laser slam based on global feature descriptor[J]. Journal of Shanghai Jiao Tong University,2022,56(10):1379.
[33] 杨林,马宏伟,王岩. 基于激光惯性融合的煤矿井下移动机器人SLAM算法[J]. 煤炭学报,2022,47(9):3523−3534. YANG Lin,MA Hongwei,WANG Yan. LiDAR-Inertial SLAM for mobile robot in underground coal mine[J]. Journal of China Coal Society,2022,47(9):3523−3534.
[34] RUSU R B,BLODOW N,MARTON Z C,et al. Aligning point cloud views using persistent feature histograms[C]//2008 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway,NJ:IEEE,2008:3384−3391.
[35] RUSU R B,BLODOW N,BEETZ M. Fast point feature histograms (FPFH) for 3D registration[C]//2009 IEEE International Conference on Robotics and Automation. Piscataway,NJ:IEEE,2009:3212−3217.
[36] 周玉,朱文豪,房倩,等. 基于聚类的离群点检测方法研究综述[J]. 计算机工程与应用,2021,57(12):37−45. doi: 10.3778/j.issn.1002-8331.2102-0167 ZHOU Yu,ZHU Wenhao,FANG Qian,et al. Survey of outlier detection methods based on clustering[J]. Computer Engineering and Applications,2021,57(12):37−45. doi: 10.3778/j.issn.1002-8331.2102-0167
[37] YIN J,LI A,LI T,et al. M2DGR:A multi-sensor and multi-scenario SLAM dataset for ground robots[J]. IEEE Robotics and Automation Letters,2022,7(2):2266−2273. doi: 10.1109/LRA.2021.3138527
计量
- 文章访问数: 92
- HTML全文浏览量: 8
- PDF下载量: 25
