A three-dimensional reconstruction method of coal mine tunnel fused with LiDAR and IMU
-
摘要:
针对煤矿掘进巷道存在典型非结构化、特征退化、大尺度环境，巷道三维重建易出现位姿估计精度低、累计漂移误差大等问题，提出了一种激光雷达和惯导（Inertial Measurement Unit，IMU）融合的煤矿掘进巷道三维重建方法。该方法通过迭代卡尔曼滤波将激光雷达观测模型的残差函数和IMU预测模型的先验状态偏差紧耦合，经状态更新得到更为精确的后验状态，提升了退化环境下的位姿估计精度。为降低巷道三维模型重建过程中的累计漂移误差，提出基于体素化广义迭代最近点（Voxelized Generalized ICP，VGICP）的回环检测算法，以基于体素的单分布到多分布的方式进行配准，完成对回环帧的选取及精确匹配，实现回环帧的全局位姿校正，有效降低煤矿巷道三维重建的累计漂移误差。相比于A-LOAM、LEGO-LOAM、LINS算法，所提算法在位姿估计精度和全局一致性方面显著提升。公开数据集实验结果表明：所提算法的RPE和APE均方根误差分别为
0.2718 和0.5008 ，与其他算法相比分别降低了53.14%、50.97%、48.31%，和50.41%、47.99%、47.49%。开展了3种模拟巷道场景三维重建实验，结果表明所提算法构建的室内长廊模型各区域在长度、宽度和高度方向的误差均在1.2%以内；所提算法构建的煤矿巷道三维模型与真实巷道空间分布一致，总体距离退化误差仅为2.46%，较其他3种算法重建性能分别提升了66.12%、65.30%、70.43%。在煤矿主体实验室掘进巷道进行三维重建实验，结果表明三维重建结果在长度、宽度和高度方向的平均误差百分比分别为0.47%、0.75%和0.67%，可以实现掘进巷道三维精确建模。Abstract:In view of the problems of typical unstructured, degraded characteristics and large-scale environment in coal mine tunnel, and the 3D reconstruction of coal mine tunnel is prone to low pose estimation accuracy and large cumulative drift error, a 3D reconstruction method of coal mine tunnel fused with lidar and inertial measurement unit (IMU) is proposed. In this method, the residual function of the lidar observation model and the prior state deviation of the IMU prediction model are tightly coupled through iterative kalman filter, and a more accurate posteriori state is obtained through state update, which provides reliable pose estimation for the degraded environment. In order to reduce the cumulative drift error in the process of tunnel 3D model reconstruction, a loopback detection algorithm based on Voxelized Generalized ICP (VGICP) is proposed. It is registered in a voxel-based single to multi-distribution mode, so as to complete the selection and accurate matching of loopback keyframes, realize the global pose correction of loopback keyframes, and effectively reduce the cumulative drift error of 3D reconstruction of coal mine tunnel. Compared with the A-LOAM and LEGO-LOAM algorithms, the proposed algorithm has significantly improved the accuracy and global consistency of pose estimation. Experimental results on public datasets show that the root mean square errors of RPE and APE of the proposed algorithm are
0.2718 and0.5008 , respectively, which are reduced by 53.14%, 50.97%, 48.31%, and 50.41%, 47.99% 47.49% respectively compared with other algorithms. Two real-world 3D reconstruction experiments are carried out. The results show that the error percentage of each area of the indoor corridor model constructed by the proposed algorithm in the directions of length, width and height was within 1.2%. The three-dimensional model of coal mine tunnel constructed by the proposed algorithm is consistent with the spatial distribution of the real coal mine tunnel. The overall distance degradation error is only 2.46%, which is 66.12%, 65.30% and 70.43% higher than that of the other three algorithms, respectively. Three-dimensional reconstruction experiments are carried out in the roadway excavation in the main laboratory of the coal mine. The results show that the average error percentages in the length, width and height directions are 0.47%, 0.75% and 0.67%, respectively. It can realize the accurate three-dimensional modeling of the tunnel.-
Keywords:
- coal mine tunnel /
- 3D reconstruction /
- lidar /
- inertial navigation /
- iterative kalman filter /
- loopback detection
-
0. 引 言
国家发展改革委等八部委联合印发《关于加快煤矿智能化发展的指导意见》，为煤矿智能化掘进的创新发展指明了方向[1-2]。国家能源局等印发《煤矿智能化建设指南（2021年版）》，要求对掘进巷道质量进行实时监测[3]，避免巷道超欠挖引发施工进度慢、人员安全严重受威胁等问题。精确的模型构建是实现掘进巷道质量检测的重要前提。井下掘进巷道具有长距离、大跨度、非结构化等特点，要实现精确模型构建具有较大的难度。随着三维扫描技术的发展，连续大范围场景的三维重建成为可能。因此，本文研究煤矿掘进巷道三维重建对巷道成形质量监测具有重要意义。
在煤矿掘进巷道三维重建方面，胡兴涛等[4]提出使用同步定位与建图技术对掘进工作面环境感知进行研究。三维激光雷达可以提供丰富的环境结构信息，但其点云数据离散稀疏，这使得在煤矿纹理较少的环境中进行特征提取变得棘手。若雷达感应点很少，容易导致姿态不受约束[5-6]。而IMU不受结构特征和环境变化的影响，短时间内能够提供精确姿态信息，但会随着误差累计发生漂移。因此多传感器融合已成为三维重建的新趋势。LOAM算法[7]以IMU辅助计算雷达里程计，对雷达和IMU测量进行解耦，以IMU为整个系统的先验。LeGO-LOAM算法[8]利用加速度公式对IMU数据进行点云失真校正提供先验姿态，但并不能有效地消除IMU本身的测量偏差。YANG L等[9]提出了一种LIDAR-IMU融合算法，构建了激光惯性里程计，使用IMU对机器人的位姿预测，实现了高鲁棒性的六自由度状态估计。YANG等[10]集成激光雷达和惯性测量单元检测和补偿退化场景，将IMU预积分信息投影到平面特征补偿旋转状态退化，通过雷达和IMU融合的后验姿态补偿平移状态退化。刘峰等[11]利用IMU获取连续高频的位姿信息，利用雷达获取密集准确的点云数据，通过二者融合实现了巷道环境的三维空间映射。LIO-SAM算法[12]利用平滑和映射策略将激光雷达数据和惯性测量单元数据进行耦合，但对数据的关联性要求较高，错误的匹配会导致整个模型重建失败。马庆禄等[13]对Fast-LIO2算法进行改进，通过引入GTSAM增强激光雷达与IMU的融合精度，形成激光雷达与IMU的耦合建图系统，提高了园区三维重建的精度。XU等[14]提出了一种快速、稳健的三维重建算法，通过扩展卡尔曼滤波算法融合传感器，将当前帧点云与局部地图直接配准，有效地提高了点云配准的实时性和准确性。张福斌等[15]提出了一种基于非线性优化的多传感器融合算法，可以较好地修正与估计MEMS IMU位置信息，提高了惯性系统运动的位置精度。综上所述，国内外学者基于LOAM等算法展开的研究均取得了一定进展，但煤矿掘进巷道低纹理、少特征导致的低精度位姿估计问题使激光雷达三维重建仍面临严峻挑战。此类环境空间特征退化，三维重建方法会出现错误的点云配准以致姿态漂移，影响重建精度；仅凭结构复杂、不规整的巷道侧壁特征无法提供纵向位置参考，导致三维重建模型距离退化。
随着巷道距离的增加，三维重建会产生累计漂移误差。对于该问题，目前国内外学者主要采用回环检测算法解决。任好玲等[16]采用正态分布变换对关键帧与相邻关键帧进行帧间匹配，用最优坐标变换矩阵对当前关键帧的全局位姿进行校正。REN等[17-18]基于GICP（Generalized Iterative Closest Point）对连续帧和连续关键帧配准，实现了井下巷道轻量级、高效的回环检测。JI等[19]使用形状描述符来描述分割后的点云，通过与当前获得的点云进行匹配来进行回环检测。JIANG等[20]通过平面法向约束检验符合约束条件的候选回环帧，通过改进迭代最近点进行验证，能够实现快速可靠的闭环性能。徐晓苏等[21]用kd-tree加速搜索候选回环帧，基于残差项加权的特征匹配算法获取回环帧间的位姿变换。马宝良等[22]通过设定最小距离阈值筛选满足条件的关键帧，使用GICP对新帧与待匹配帧进行配准获得置信分数完成回环检测 。薛光辉等[23]利用SegMatch算法改进LeGO-LOAM回环检测模块，使用ICP算法进行全局图优化，能够实现更高精度的点云地图构建和位姿估计。李芳威等[24]将Lazy Decision算法引入到Cartographer算法进行优化，达到煤矿井下环境的使用要求。邱佳月等[25]利用区域高度差作为Scan Context描述符，对当前帧与历史帧进行相似性匹配，实现快速精确的回环检测。郝宇等[26]将两级滤波引入Cartographer回环检测，根据最大回环兼容子集法选择正确的闭环回路。综上所述，现有算法采用传统ICP（Iterative Closest Point）或NDT（Normal Distributions Transform）等进行回环检测为学者们提供了研究参考。但在煤矿掘进巷道环境回环检测更容易陷入局部最优值，匹配迭代精度低；相似的巷道环境结构会导致错误的回环匹配以致三维重建结果缺乏全局一致性。
针对以上问题，笔者提出了一种激光雷达和IMU融合的煤矿巷道三维重建方法。通过迭代卡尔曼滤波实现激光雷达观测状态和IMU预测状态紧耦合，提高掘进巷道退化环境中位姿估计精度；采用VGICP（Voxelized Generalized ICP）模块建立帧间数据关联，以基于体素的单分布到多分布的方式进行匹配，提高回环帧选取准确性，提升回环帧匹配的精确度，抑制掘进巷道模型重建的累积误差。最后，利用公开数据集和自采数据集分别进行验证。
1. 煤矿巷道三维重建总体方案
1.1 坐标系定义
煤矿巷道三维重建的前提是求解各坐标系间的变换矩阵，所以要定义与声明相关坐标系。本文涉及4类坐标系：巷道坐标系、掘进机机身坐标系、激光雷达坐标系以及IMU坐标系，如图1所示。
巷道坐标系$ {O_W}{X_W}{Y_W}{Z_W} $（也称世界坐标系）是全局坐标系，记作$W$系。将掘进机初始位置作为坐标原点$ {O_W} $，$ {X_W} $轴指向巷道右侧，表示巷道宽度方向，$ {Y_W} $轴与巷道中轴线平行并指向巷道掘进方向，$ {Z_W} $轴垂直于巷道地面向上，表示巷道高度方向。
掘进机机身坐标系$ {O_b}{X_b}{Y_b}{Z_b} $，以惯导实际安装位置为原点$ {O_b} $，$ {X_b} $轴水平向右与机身侧面垂直，$ {Y_b} $轴沿机身纵轴线指向前进方向、$ {Z_b} $轴垂直于掘进机顶板向上。通过计算从IMU坐标系到掘进机机身坐标系的转换矩阵，将掘进机机身坐标系和IMU坐标系处理为同一坐标系。转换过程表示为：
$$ \left[ {\begin{array}{*{20}{c}} X \\ Y \\ Z \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {\cos\; \theta }&{ - \sin\; \theta }&0 \\ {\sin\; \theta }&{\cos\; \theta }&0 \\ 0&0&1 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{X_0}} \\ {{Y_0}} \\ {{Z_0}} \end{array}} \right] $$ (1) 式中：$\theta $为IMU坐标系和巷道坐标系（掘进机初始位置坐标系）之间的夹角。
激光雷达坐标系$ {O_L}{X_L}{Y_L}{Z_L} $，以激光雷达重心为原点$ {O_L} $，$ {X_L} $轴水平向右与机身侧面垂直，$ {Y_L} $轴沿机身纵轴线指向掘进方向、$ {Z_L} $轴垂直于掘进机顶板向上。
1.2 多传感器融合的巷道三维重建方案
本文提出的基于激光雷达和IMU融合的巷道三维重建总体方案如图2所示。该方案由数据采集模块、数据预处理模块、位姿估计模块和巷道三维模型构建与误差校正模块4个模块组成。
![]()
图 2 煤矿巷道三维重建总体方案Figure 2. The overall scheme of three-dimensional reconstruction of coal mine1）数据采集模块：激光雷达和惯导分别安装在掘进机机身上，激光雷达扫描巷道环境获取三维点云数据，惯导获取掘进机机身位姿信息。
2）数据预处理模块：高频的IMU进行反向传播对巷道畸变点云进行运动补偿，同时进行预积分。
3）位姿估计模块：对预处理的巷道点云数据提取边缘特征和平面特征，基于特征匹配算法构建残差函数。根据IMU预积分结果进行前向传播，获得当前时刻掘进机机身的先验状态。通过迭代卡尔曼滤波融合残差函数和先验状态偏差得到精确的后验位姿信息。
4）巷道三维模型构建与误差校正模块：以帧到局部模型的匹配方式构建巷道全局巷道三维模型。最后加入回环检测，将发生回环的两处位置关系作为约束反馈给巷道全局模型，消除重建过程的累计误差。
2. 煤矿巷道三维重建方法
2.1 基于激光雷达的巷道特征提取
设 激光雷达坐标系$L$下，第$k$帧点云${P_k}$存在边缘特征点和平面特征点，取当前点及其左右各5个点进行曲率计算，根据曲率大小区分边缘点和平面点。设$i$为${P_k}$中的一个点，$i \in {P_k}$，$S$为点云${P_k}$中点$i$所在行的连续点集，则相应点的曲率$c$可表示为：
$$ c = \frac{1}{{\left| S \right|\left| {X_{k,i}^L} \right|}} \times \left\| {\sum\limits_{j \in S,j \notin i} {\left( {X_{k,i}^L - X_{k,j}^L} \right)} } \right\| $$ (2) 其中，$\left| S \right|$为10，$X_{k,i}^L$和$X_{k,j}^L$分别为激光雷达坐标系下第$k$帧数据中的第i、j个点的坐标。根据式(2)计算各点云的曲率并排序，将曲率最大的$m$个点作为边缘点${F_e}$，将曲率最小的$n$个点作为平面点${F_p}$，则第$K$帧所有特征集合为${F_K} = \left\{ {F_e^k,F_p^k} \right\}$。
2.2 紧耦合的巷道三维模型位姿估计方法
2.2.1 状态定义
在IMU坐标系下，系统$k$时刻到$k + 1$时刻的转换可表示为
$$ {\boldsymbol{x}}_{{b_{k + 1}}}^{{b_k}} = [{\boldsymbol{r}}_{{b_{k + 1}}}^{{b_k}},{\boldsymbol{v}}_{{b_{k + 1}}}^{{b_k}},{\boldsymbol{q}}_{{b_{k + 1}}}^{{b_k}},{{\boldsymbol{b}}_a},{{\boldsymbol{b}}_g},{{\boldsymbol{g}}^{{b_k}}}] $$ (3) 式中：$ {\boldsymbol{r}},{\boldsymbol{v}},{\boldsymbol{q}},{{\boldsymbol{b}}_a},{{\boldsymbol{b}}_g},{\boldsymbol{g}} $分别表示位置、速度、姿态角、加速度偏差、陀螺仪偏差和重力向量。
本文采用误差状态估计求解${\boldsymbol{x}}_{{b_{k + 1}}}^{{b_k}}$，将误差状态$\delta {\boldsymbol{x}}$定义为：
$$ \delta {\boldsymbol{x}} = [\delta {\boldsymbol{r}},\delta {\boldsymbol{v}},\delta {\boldsymbol{q}},\delta {{\boldsymbol{b}}_a},\delta {{\boldsymbol{b}}_g},\delta {\boldsymbol{g}}] $$ (4) 对误差状态$\delta {\boldsymbol{x}}$和先验状态$ {\boldsymbol{\bar x}}_{{b_{k + 1}}}^{{b_k}} $执行“$ \oplus $”运算得到系统后验状态：
$$ {\boldsymbol{x}}_{{b_{k + 1}}}^{{b_k}} = {\boldsymbol{\bar x}}_{{b_{k + 1}}}^{{b_k}} \oplus \delta {\boldsymbol{x}} = \left[ {\begin{array}{*{20}{c}} {{\boldsymbol{\bar r}}_{{b_{k + 1}}}^{{b_k}} + \delta {\boldsymbol{r}}} \\ {{\boldsymbol{\bar v}}_{{b_{k + 1}}}^{{b_k}} + \delta {\boldsymbol{v}}} \\ {{\boldsymbol{\bar q}}_{{b_{k + 1}}}^{{b_k}} \otimes \exp (\delta {\boldsymbol{\theta }})} \\ {{{{\boldsymbol{\bar b}}}_a} + \delta {{\boldsymbol{b}}_a}} \\ {{{{\boldsymbol{\bar b}}}_g} + \delta {{\boldsymbol{b}}_g}} \\ {{{{\boldsymbol{\bar g}}}^{{b_k}}} + \delta {\boldsymbol{g}}} \end{array}} \right] $$ (5) 式中：$ \otimes $为四元数乘积；$\exp \left( \cdot \right)$为欧拉角转四元数。
2.2.2 IMU预测模型
IMU误差状态的线性化连续时间运动状态方程表示为：
$$ \delta {\boldsymbol{\hat x}}(t) = {{\boldsymbol{F}}_t}\delta {\boldsymbol{x}}(t) + {{\boldsymbol{G}}_t}{\boldsymbol{w}} $$ (6) 式中：${\boldsymbol{w}} = {\left[ {{\boldsymbol{n}}_a^T,{\boldsymbol{n}}_g^T,{\boldsymbol{n}}_{{b_a}}^T,{\boldsymbol{n}}_{{b_g}}^T} \right]^T}$，${\boldsymbol{w}}$为系统噪声向量；${{\boldsymbol{F}}_t}$为$t$时刻误差转移矩阵；${{\boldsymbol{G}}_t}$为$t$时刻噪声雅克比矩阵。按一阶泰勒展开为
$$ \boldsymbol{F}_t=\left[\begin{array}{cccccc} \mathbf{0}_{3 \times 3} & \boldsymbol{I}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & -\boldsymbol{R}_t^{b_k}\left[\hat{\boldsymbol{a}}_t\right]_{\times} & -\boldsymbol{R}_t^{b_k} & \mathbf{0}_{3 \times 3} & -\boldsymbol{I}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & -\left[\hat{\boldsymbol{\omega}}_t\right]_{\times} & \mathbf{0}_{3 \times 3} & -\boldsymbol{ I}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \end{array}\right] $$ (7) $$ \boldsymbol{G}_t=\left[\begin{array}{cccc} \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ -\boldsymbol{R}_t^{\boldsymbol{b}_k} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & -\boldsymbol{I}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \boldsymbol{I}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \boldsymbol{I}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \end{array}\right] $$ (8) 式中：${{\boldsymbol{I}}_{3 \times 3}}$为3乘3的单位矩阵，${{\boldsymbol{0}}_{3 \times 3}}$为3乘3的零矩阵；$ {\left[ \cdot \right]_ \times } \in {R^{3 \times 3}} $为将三维向量转换为斜对称矩阵；$ {{\boldsymbol{\hat a}}_{\boldsymbol{t}}} $和$ \hat{\boldsymbol{\omega}}_t $为$ t $时刻加速度和角速度。
对式(6)进行离散化：
$$ \delta {{\boldsymbol{x}}_{{t_i}}} = \left( {{\boldsymbol{I}} + {{\boldsymbol{F}}_{{t_i}}}{\text{\Delta }}t} \right)\delta {{\boldsymbol{x}}_{{t_i} - 1}} $$ (9) $$ \begin{gathered} {\boldsymbol{P}}_{{t_i}}^{{b_k}} = \left( {{\boldsymbol{I}} + {{\boldsymbol{F}}_{{t_i} - 1}}{\text{\Delta }}t} \right){\boldsymbol{P}}_{{t_i} - 1}^{{b_k}}{\left( {{\boldsymbol{I}} + {{\boldsymbol{F}}_{{t_i} - 1}}{\text{\Delta }}t} \right)^T}+ \\ \left( {{{\boldsymbol{G}}_{{t_i} - 1}}{\text{\Delta }}t} \right){\boldsymbol{Q}}{\left( {{{\boldsymbol{G}}_{{t_i} - 1}}{\text{\Delta }}t} \right)^T} \\ \end{gathered} $$ (10) 式中：${\boldsymbol{P}}$和${\boldsymbol{Q}}$分别为$\delta x$和$w$的协方差矩阵，可对IMU标定获得。
2.2.3 激光雷达观测模型
对提取的巷道边缘特征点和平面特征点分别进行点线匹配和点面匹配构建残差方程即可得到观测模型方程。在雷达坐标系下第$k + 1$帧点云中的第$i$个特征点的观测模型方程可表示为：
$${\begin{array}{*{20}{c}} {f_i}\left( {{\boldsymbol{x}}_{{b_{k + 1}}}^{{b_k}}} \right) =\\ \left\{ {\begin{array}{*{20}{l}} {\dfrac{{\left| {\left( {\hat p_i^{{L_k}} - p_j^{{L_k}}} \right) \times \left( {p_i^{{L_k}} - p_l^{{L_k}}} \right)} \right|}}{{\left| {p_j^{{L_k}} - p_l^{{L_k}}} \right|}},p_i^{{L_{k + 1}}} \in {F_e}} \\ {\dfrac{{\left| {{{\left( {\hat p_i^{{L_k}} - p_j^{{L_k}}} \right)}^T}\left[ {\left( {p_j^{{L_k}} - p_l^{{L_k}}} \right) \times \left( {p_j^{{L_k}} - p_m^{{L_k}}} \right)} \right]} \right|}}{{\left| {\left( {p_j^{{L_k}} - p_l^{{L_k}}} \right) \times \left( {p_j^{{L_k}} - p_m^{{L_k}}} \right)} \right|}},p_i^{{L_{k + 1}}} \in {F_p}} \end{array}} \right.\end{array}} $$ (11) 若$ p_i^{{L_{k + 1}}} $为边缘特征点，$ {f_i}\left( {{\boldsymbol{x}}_{{b_{k + 1}}}^{{b_k}}} \right) $表示点$ \hat p_i^{{L_k}} $与$ p_j^{{L_k}} $和$ p_l^{{L_k}} $两点所在直线之间的欧式距离；若$ p_i^{{L_{k + 1}}} $为平面特征点，$ {f_i}\left( {{\boldsymbol{x}}_{{b_{k + 1}}}^{{b_k}}} \right) $表示点$ \hat p_i^{{L_k}} $与$ p_j^{{L_k}} $、$ p_l^{{L_k}} $和$ p_m^{{L_k}} $三点所在平面之间的欧式距离。
$ \hat p_i^{{L_k}} $可表示为：
$$ \hat p_i^{{L_k}} = {\boldsymbol{R}}_L^{{b^T}}\left( {{\boldsymbol{R}}_{{b_{k + 1}}}^{{b_k}}\left( {{\boldsymbol{R}}_L^bp_i^{{L_{k + 1}}} + {\boldsymbol{p}}_L^b} \right) + {\boldsymbol{p}}_{{b_{k + 1}}}^{{b_k}} - {\boldsymbol{p}}_L^b} \right) $$ (12) 式中：$ \hat p_i^{{L_k}} $表示点$ p_i^{{L_{k + 1}}} $从第$k + 1$帧转换到第$k$帧的坐标变换；$ {\boldsymbol{R}}_L^b $和$ {\boldsymbol{p}}_L^b $为IMU与激光雷达间的旋转矩阵和平移矩阵。
2.2.4 状态更新
迭代卡尔曼滤波中，状态更新可以与优化问题联系起来。基于IMU观测模型的先验状态偏差和激光观测模型的残差函数，非线性目标函数可表示为：
$$ \Upsilon (x) = {\min _{\delta x}}{\left\| {\delta {\boldsymbol{x}}} \right\|_{P{{(k)}^{ - 1}}}} + {\left\| {f({\boldsymbol{\bar x}}_{{b_k}}^{{b_{k + 1}}} \oplus \delta {\boldsymbol{x}})} \right\|_{{{({{\boldsymbol{J}}_k}{{\boldsymbol{M}}_k}{\boldsymbol{J}}_k^T)}^{ - 1}}}} $$ (13) 式中：${{\boldsymbol{J}}_k}$和${{\boldsymbol{M}}_k}$分别为测量噪声的雅可比矩阵和协方差矩阵，$\left\| \cdot \right\|$为马氏范数。
对式(13)迭代更新的卡尔曼增益方程和后验状态的均值表示为：
$$ {{\boldsymbol{K}}_{k,j}} = {{\boldsymbol{P}}_k}{\boldsymbol{H}}_{k,j}^{\text{T}}{({{\boldsymbol{H}}_{k,j}}{{\boldsymbol{P}}_k}{\boldsymbol{H}}_{k,j}^{\text{T}} + {{\boldsymbol{J}}_{k,j}}{{\boldsymbol{M}}_k}{\boldsymbol{J}}_{k,j}^{\text{T}})^{ - 1}} $$ (14) $$ \delta {{\boldsymbol{x}}_{_{j + 1}}} = \delta {{\boldsymbol{x}}_j} + {{\boldsymbol{K}}_{k,j}}[{{\boldsymbol{H}}_{k,j}}\delta {{\boldsymbol{x}}_{_j}} - f({\boldsymbol{\bar x}}_{{b_k}}^{{b_{k + 1}}} \oplus \delta {{\boldsymbol{x}}_{j + 1}})] $$ (15) 式中：${\boldsymbol{H}}_{k,j}^{}$为$f({\boldsymbol{\bar x}}_{{b_k}}^{{b_{k + 1}}} \oplus \delta {{\boldsymbol{x}}_{j + 1}})$的雅克比矩阵。在迭代过程中当$f({\boldsymbol{\bar x}}_{{b_k}}^{{b_{k + 1}}})$小于阈值时，通过式( 16)进行更新${{\boldsymbol{P}}_k}$：
$$ {{\boldsymbol{P}}_{k + 1}} = ({\boldsymbol{I}} - {{\boldsymbol{K}}_{k,n}}{{\boldsymbol{H}}_{k,n}}){{\boldsymbol{P}}_k}{({\boldsymbol{I}} - {{\boldsymbol{K}}_{k,n}}{{\boldsymbol{H}}_{k,n}})^{\text{T}}} + {{\boldsymbol{K}}_{k,n}}{{\boldsymbol{M}}_k}{\boldsymbol{K}}_{k,n}^{\text{T}} $$ (16) 通过不断迭代可得到最佳后验状态$ {\boldsymbol{x}}_{{b_{k + 1}}}^{{b_k}} $，将此位姿信息从掘进机机身坐标系转换到巷道坐标系即可得到全局姿态信息$ {\boldsymbol{x}}_W^{{b_{k + 1}}} $：
$$ {\boldsymbol{x}}_W^{{b_{k + 1}}} = \left[ {\begin{array}{*{20}{c}} {{\boldsymbol{p}}_W^{{b_{k + 1}}}} \\ {{\boldsymbol{q}}_W^{{b_{k + 1}}}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{\boldsymbol{R}}_{{b_k}}^{{b_{k + 1}}}({\boldsymbol{p}}_W^{{b_k}} - {\boldsymbol{p}}_{{b_{k + 1}}}^{{b_k}})} \\ {{\boldsymbol{q}}_{{b_k}}^{{b_{k + 1}}} \otimes {\boldsymbol{q}}_W^{{b_k}}} \end{array}} \right] $$ (17) 2.3 巷道三维模型重建方法
根据点云帧间相对变换矩阵可更新当前点云的巷道模型，即在巷道坐标系下得到当前帧的坐标信息。将第$k$帧点按照式(18)转换到巷道坐标系：
$$ {\boldsymbol{T}}_k^W = {\boldsymbol{T}}_{k - 1}^W{\boldsymbol{T}}_k^L $$ (18) 其中，${\boldsymbol{T}}_k^L$为相邻帧的位姿变换，${\boldsymbol{T}}_{k - 1}^W$为第$k - 1$帧到巷道坐标系的变换矩阵。转换后的所有点云组成局部模型${M_j}$，其中包含边缘特征集合$M_j^e$和平面特征集合$M_j^p$，即${M_j} = \left\{ {M_j^e \cup M_j^p} \right\}$。最后，对转换点云帧的边缘特征和平面特征进行下采样，消除同一体素单元中的冗余特征。最后，以线面特征匹配方法将当前帧点云与巷道局部模型点云进行匹配得到全局一致的巷道三维模型，并通过八叉树结构进行更新。
3. 基于VGICP回环检测的巷道模型累计误差校正方法
基于紧耦合的位姿估计方法，巷道重建可以达到良好的局部精度，但当前点云的位姿估计是由上一帧点云位姿计算所得，每次匹配都会产生误差，即使在局部的重建过程中误差很小，但误差会由每帧进行传递下去，最终导致累计误差越来越大，所以煤矿巷道大尺度场景环境重建效果依然很差。
回环检测算法可以对煤矿环境中的重复场景进行判断，建立当前场景与历史场景的全局约束，校正轨迹漂移。传统回环检测中使用ICP进行回环验证，若两个关键帧的相对姿态变换越大，则更容易陷入局部最优值，GICP精度高但计算代价高，而NDT速度快、收敛域大但精度稍显不足。针对该问题，本文改进了传统回环检测方法，提出了一种基于VGICP的回环检测算法。该算法考虑源点分布与最近邻体素内所有分布之间的关系，以基于体素的单分布到多分布的方式进行匹配，使用体素化建立回环数据关联，提升回环检测精度，算法流程如图3所示。
基于VGICP的回环检测算法原理如下:
1）回环候选帧选取。若每一帧点云都用来检测回环，会造成极大的计算量。关键帧不仅可以减少待优化的帧数，并且具有代表性，关键帧与其附近帧的特性相似，少量的关键帧即可代表大量原始帧表达出所含信息。回环检测算法首先判断当前帧点云是否为关键帧，再利用kd树在历史关键帧集合中寻找与当前关键帧相邻的特征点云，并判断相邻关键帧与当前关键帧的时间间隔是否大于设定的阈值，大于则选择当前关键帧为回环候选帧。反之进入下一回环线程。
2）基于VGICP进行回环帧选取。使用VGICP求解回环候选帧和相邻关键帧之间的坐标变换矩阵。假设回环候选帧点云$P = \left\{ {{p_1},{p_2} ,\cdots ,{p_M}} \right\}$和相邻关键帧$Q = \left\{ {{q_1},{q_2} ,\cdots ,{q_M}} \right\}$中的点均服从高斯分布且独立，即
$$ {p_i} \sim \mathcal{N}\left( {{{\hat p}_i},{\boldsymbol{C}}_i^P} \right) $$ (19) $$ {q_i} \sim \mathcal{N}\left( {{{\hat q}_i},{\boldsymbol{C}}_i^Q} \right) $$ (20) 式中：${\hat p_i}$、${\hat q_i}$和${\boldsymbol{C}}_i^P$、${\boldsymbol{C}}_i^Q$分别表示${p_i}$、${q_i}$的均值和邻域协方差矩阵。定义近邻距离之和$\hat d_i^{'}$满足：
$$ \hat d_i^{'} = \sum\limits_j {\left( {{{\hat q}_j} - {\boldsymbol{T}}{{\hat p}_i}} \right)} $$ (21) 其中，${q_j}$为$Q$中与$ {\boldsymbol{T}}{p_i}$距离在$r$以内的点：
$$ \left\{ {{q_j}|\left\| {{\boldsymbol{T}}{p_i} - {q_j}} \right\| < r} \right\} $$ (22) $\hat d_i^{'}$的分布表示为：
$$ \hat d_i^{'} \sim \mathcal{N}\left( {{\mu _i},{{\boldsymbol{C}}_i}} \right) $$ (23) $$ {\mu _i} = \sum\limits_j {\left( {{{\hat q}_j} - {\boldsymbol{T}}{{\hat p}_i}} \right)} $$ (24) $$ {{\boldsymbol{C}}_i} = \sum\limits_j {\left( {{\boldsymbol{C}}_j^Q + {\boldsymbol{TC}}_i^p{{\boldsymbol{T}}^{\text{T}}}} \right)} $$ (25) 当变换正确时，所有近邻点与$T{p_i}$的距离应当趋近于0，即近邻距离和$\hat d_i^{'}$的数学期望为0：
$$ {\mu _i} = \sum\limits_j {\left( {{{\hat q}_j} - {\boldsymbol{T}}{{\hat p}_i}} \right)} = 0 $$ (26) 于是，通过最大化式(23)的似然函数即可求得坐标变换矩阵${T^ * }$:
$$ \begin{array}{l}{{\boldsymbol{T}}}^{\ast }= \mathrm{arg}\underset{T}{\mathrm{min}}\displaystyle\sum _{i}\left[{\left(\displaystyle\sum _{j}\left({\hat{q}}_{j}-T{\hat{p}}_{i}\right)\right)}^{\text{T}}{\left(\displaystyle\sum _{j}\left({C}_{j}^{Q}+T{C}_{i}^{P}{T}^{\text{T}}\right)\right)}^{-1}\left(\displaystyle\sum _{j}\left({\hat{q}}_{j}-T{\hat{p}}_{i}\right)\right)\right]\end{array} $$ (27) 简化得：
$$ \begin{array}{l}{{\boldsymbol{T}}}^{\ast }= \mathrm{arg}\underset{r}{\mathrm{min}}\displaystyle\sum _{i}\left[{N}_{j}{\left(\dfrac{\displaystyle\sum _{j}{\hat{q}}_{j}}{{N}_{j}}-T{\hat{p}}_{i}\right)}^{\text{T}}{\left(\dfrac{\displaystyle\sum _{j}{C}_{j}^{Q}}{{N}_{j}}+T{C}_{i}^{P}{T}^{\text{T}}\right)}^{-1}\left(\dfrac{\displaystyle\sum _{j}{\hat{q}}_{j}}{{N}_{j}}-T{\hat{p}}_{i}\right)\right]\end{array} $$ (28) 式中：${N_j}$为${p_j}$近邻点的总数。
以回环候选帧和相邻关键帧间的坐标变换作为判断标准，以剔除空间上距离较远的回环候选帧。设距离阈值${d_{{\mathrm{th}}}}$，计算回环候选帧和相邻关键帧的欧式距离：
$$ d = \sqrt {s_x^2 + s_y^2 + s_z^2} $$ (29) 式中：${s_x}$、${s_y}$、${s_z}$分别为回环候选帧和相邻关键帧在$x$、$y$、z三轴的平移分量，回环候选帧作为回环帧的条件为：$d \leqslant {d_{{\mathrm{th}}}}$。
3）位姿优化。根据相邻关键帧与巷道坐标系的坐标变换矩阵，将回环帧和相邻关键帧的坐标变换矩阵${{\boldsymbol{T}}^ * }$反馈给巷道全局模型进行位姿校正。
4. 实验验证与结果分析
4.1 公开数据集实验验证
首先使用开源数据集M2DGR对本文算法与主流算法—基于单激光雷达的A-LOAM算法、基于松耦合和ICP回环检测的LEGO-LOAM方法、其他方式的紧耦合LINS方法进行测试。该数据集的hall序列为室内走廊场景，场景墙壁光滑、特征单一，与煤矿掘进巷道环境相似，具有一定的挑战性[27]，故本次实验选择hall_04序列验证本文所提算法的有效性。
图4所示为各算法运动轨迹与真实轨迹的对比结果。图5为各算法的三轴偏移量。结果显示，A-LOAM算法起始位置偏差较小，随后逐渐接近真实轨迹，但由于缺少回环检测，无法抑制误差累计，在终点位置偏差较大。LEGO-LOAM算法整体与真实轨迹基本一致，但在转弯处位姿发生漂移，部分区域无法准确检测回环存在明显漂移。LINS算法产生的轨迹除部分区域外，与真实轨迹存在细微差别，但z轴方向轨迹偏移量大于其他算法。相比之下，本文算法有效抑制了位姿漂移，回环检测算法准确性高，在转弯和回环区域表现显著优于其他算法，三轴偏移量均最小，与真实轨迹最为接近。
为进一步定量分析各算法的误差，采用EVO评估工具通过计算相对位姿误差（RPE, Relative Pose Error）评估各算法局部位姿估计精度，通过计算绝对轨迹误差（APE, Absolute Pose Error）评估各算法全局一致性。RPE仅比较运动姿态增量，APE计算整个轨迹的统计误差。
表1所示为各算法在5个衡量指标下的RPE和APE统计结果。结果表明，本文算法RPE和APE各项指标均最小。其中RPE均方根误差为
0.2718 ，与A-LOAM、LEGO-LOAM、LINS相比分别降低了53.14%、50.97%、48.31%，局部精度优于其他算法。APE均方根误差为0.5008 ，与A-LOAM、LEGO-LOAM、LINS相比分别降低了50.41%、47.99%、47.49%。相比之下，本文算法的运动轨迹误差较小，位姿估计精度更高，全局一致性更好，验证了本文算法的有效性。表 1 各算法轨迹误差统计Table 1. Trajectory error statistics of each algorithm算法 A-LOAM LEGO-LOAM LINS OURS RPE 最大值 2.7500 2.5402 2.3449 1.0224 平均值 0.3815 0.3715 0.3517 0.2197 最小值 0.0023 0.0027 0.0112 0.0067 均方根误差 0.5800 0.5543 0.5258 0.2718 标准差 0.4368 0.4262 0.3909 0.1599 APE 最大值 1.9418 1.8644 1.8088 1.5932 平均值 0.9199 0.8655 0.8555 0.4276 最小值 0.1587 0.1382 0.1308 0.0020 均方根误差 1.0099 0.9628 0.9537 0.5008 标准差 0.4416 0.4167 0.4004 0.2610 4.2 模拟巷道三维重建实验验证
4.2.1 实验场景及平台
本次试验旨在充分验证所提算法在煤矿巷道环境中的抗特征退化、低飘移和回环检测等性能，在少特征、重退化、大尺度场景——室内长廊和模拟煤矿巷道工作面完成。图6a是一自由度退化结构，实验环境特征单一重复，极易产生距离退化导致重建效果降低，可视为煤矿掘进巷道特殊的退化场景。图6b是“口字型”回环场景，主体结构包括四条长直巷道，更符合掘进巷道形态。
实验所用设备如图7所示，包括Velodyne16线三维激光雷达，10 Hz采样频率下水平分辨率为0.2°，垂直视野±15°，测量范围可达100 m；采样频率为200 Hz的超核惯性测量单元；配置为Intel Core i5-8300H和NVIDIA GeForce的机载计算机，主频2.3 GHz，内存16 G；团队轮式移动机器人。实验所用传感器均进行了自身校准及标定处理。使用各算法对2种实际场景进行模型重建对比，各算法均由C++编译执行，并在ROS环境中运行，各算法配置与公开数据集验证实验配置相同。
4.2.2 室内长廊场景三维重建实验
移动机器人根据路径A→B→C→B→D→E→F在室内长廊中行走采集环境数据，如图8所示。图9为室内长廊场景重建结果。为评估各种算法对室内长廊场景三维重建的具体表现，对重建效果进行定性定量分析。将整个场景分为5个区域，模型尺寸为各区域内十次采样的均值。通过测量墙角、门框等标志性参考物坐标，计算室内长廊的实际尺寸。A-LOAM、LINS算法所建模型在高度方向上误差漂移较大，只统计长、宽方向上的误差。LEGO-LOAM算法整体重建结果明显失败，模型相关尺寸无法准确计算，不进行误差统计。表2为室内长廊三维模型重建误差统计，“−”为漂移太大无法准确测量的模型尺寸。
表 2 室内长廊三维模型重建误差统计Table 2. Statistics of the reconstruction error of the 3D model of the indoor corridor测量区域 A-LOAM LINS OURS 长度 宽度 长度 宽度 长度 宽度 高度 AB段 模型尺寸/mm 18717 — 19176 2132 19629 2119 2324 实际尺寸/mm 19700 2100 19700 2100 19700 2100 2300 误差值/mm −983 — −524 32 −71 19 24 误差/% 4.99 — 2.66 1.52 0.36 0.90 1.04 BC段 模型尺寸/mm 10504 2265 10577 2251 10447 2236 2479 实际尺寸/mm 10400 2215 10400 2215 10400 2215 2450 误差值/mm 104 50 177 36 47 21 29 误差/% 1.00 2.26 1.70 1.63 0.45 0.95 1.18 BD段 模型尺寸/mm 8828 3877 — 4005 8736 3973 2476 实际尺寸/mm 8700 3940 8700 3940 8700 3940 2450 误差值/mm 128 −63 — 65 36 33 26 误差/% 1.47 1.60 — 1.65 0.41 0.84 1.06 DE段 模型尺寸/mm 18746 — 19296 2263 21175 2231 2473 实际尺寸/mm 21250 2215 21250 2215 21250 2215 2450 误差值/mm − 2504 — −1954 48 −75 16 23 误差/% 11.78 — 9.20 2.17 0.35 0.72 0.94 EF段 模型尺寸/mm 12996 2286 13005 2256 12952 2235 2478 实际尺寸/mm 12900 2215 12900 2215 12900 2215 2450 误差值/mm 96 71 105 41 52 20 28 误差/% 0.74 3.21 0.81 1.85 0.40 0.90 1.14 据图9和表2分析，对于A-LOAM算法，在AB、DE区域，宽度方向部分点云数据严重偏离整体模型，其余区域宽度平均误差百分比为2.36%。这主要是因为长廊两侧宽度相等，仅依赖几何特征点进行模型构建很容易导致匹配失败。在长度方向（移动机器人行驶方向），纯激光里程计已发生失效，无法进行准确位姿估计，平均误差百分比为4.00%，且缺乏回环检测导致误差无法修正。对于LEGO-LOAM算法，移动机器人在B处转弯时仅依赖于分割和聚类后的地面点云特征进行横向约束，导致不正确的位姿估计，使得模型在该处发生变形。选取错误的地面特征导致长度误差较大，在BF段激光里程计发生严重失效，运动轨迹缩短，且回环检测性能在此场景中退化，累积误差增大。对于LINS算法，模型误差整体小于A-LOAM所建模型，但在B处发生错误匹配，该处长度方向重影较大。本文算法构建的室内长廊模型各区域在长度、宽度和高度方向的误差百分比均在1.2%以内。使用该算法显著减少了位姿估计的漂移，回环检测在此类非结构化的场景下表现仍良好。本文算法在大尺度退化场景的重建效果最优。
4.2.3 模拟煤矿巷道工作面场景三维重建实验
图10为模拟煤矿巷道工作面场景轮廓图，移动机器人沿路径P0→P1→P2→P3→P4行走采集数据，其中P0和P4分别为移动机器人起点和终点位置。图11展示了各算法相应的三维重建效果。表3为各算法总距离退化误差统计结果，根据式(30)计算总距离误差百分比，评估模型重建性能。
![]()
图 10 模拟煤矿巷道工作面场景轮廓Figure 10. Contour diagram of simulated coal mine tunnel working face scene$$ A=\frac{V_{\mathrm{c}}-V_{\mathrm{z}}}{V_{\mathrm{z}}}\times 100\% $$ (30) 式中：A为误差；$V_{\mathrm{c}} $为模型测量值；$V_{\mathrm{z}} $为真实值。
由图11和表3分析，A-LOAM算法整体效果良好，但由于相似性旋转和平移的影响，在重建过程中局部区域出现了偏移和重影，模型轮廓不清晰。LeGO-LOAM算法受环境相似点云干扰的影响，和环境特征点减少的影响，损失了局部精度。少量的地面点云特征无法对环境模型进行约束，基于ICP回环检测匹配不准确性导致无法完全校正累积误差，整体模型扭曲。LINS算法在z轴方向误差经过P1位置后极具增大，终点位置无法检测到回环帧进行误差消除。以上三种算法所建模型均在机器人行驶方向上存在距离退化现象，原因为此场景缺少环境特征信息，算法在行驶方向出现明显漂移，相比实际环境出现较大偏差。相比之下，本文算法采用迭代卡尔曼滤波将激光雷达和IMU进行紧耦合，位姿估计表现优秀，三维重建无明显失真、无重影。基于VGICP的回环检测算法有效建立了精确的回环约束，显著减少了高程误差累积，确保煤矿巷道三维模型与真实巷道空间分布一致，总体距离退化误差仅为2.46%，较其他三种算法重建性能分别提升了66.12%、65.30%、70.43%。
![]()
图 11 模拟煤矿巷道工作面场景三维重建结果Figure 11. Reconstruction results of simulated coal mine tunnel working face scene表 3 各算法距离退化误差统计Table 3. Distance degradation error statistics of each algorithm方法 P0-P1/m P1-P2/m P2-P3/m P3-P4/m 总距离/m 误差/% 真实值 44.00 48.00 47.00 45.00 184.00 − A-LOAM 41.30 45.77 40.84 42.74 170.65 7.26 LEGO-LOAM 40.33 43.14 44.88 42.61 170.96 7.09 LINS 42.52 43.90 40.31 41.96 168.69 8.32 OURS 43.46 46.91 45.98 43.13 179.48 2.46 4.3 掘进巷道场景三维重建实验
为进一步证实本文研究的实用性，在煤矿主体实验室掘进巷道场景进行实验验证，如图12所示。
在煤矿主体实验室掘进巷道场景搭建实验平台，如图13所示。实验主要设备包括煤矿掘进机、激光雷达、IMU和计算机。IMU与激光雷达安装于具有一定高度的支架上，该支架固定在掘进机机身。实验过程中保证激光雷达、IMU与计算机接通能够实时采集掘进机机身后端巷道点云信息。
利用本文方法对所采数据进行三维重建，结果如图14所示。根据图14可知，所建模型清晰，无重影、无变形。对巷道模型长度、宽度和高度进行测量，并与真实值对比，结果如表4所示。长度、宽度和高度方向的平均误差百分比分别为0.47%、0.75%和0.67%，表明本文方法可以实现掘进巷道三维精确建模。
表 4 掘进巷道场景三维重建误差Table 4. Reconstruction error of the tunnel scene参数 实际尺寸/m 模型尺寸/m 误差/m 误差/% 长度 23.42 23.31 0.11 0.47 宽度 4.00 3.97 0.03 0.75 高度 3.00 2.98 0.02 0.67 5. 结 论
1）针对煤矿掘进巷道非结构退化环境中激光雷达位姿估计精度低的问题，本文提出了一种激光雷达和IMU紧耦合的位姿估计方法，公开数据集实验结果表明：所提算法的RPE均方根误差为
0.2718 ，与A-LOAM、LEGO-LOAM、LINS算法相比分别降低了53.14%、50.97%、48.31%，实现了高精度和鲁棒性的位姿估计。2）针对巷道模型三维重建时累计误差大的问题，本文提出了一种基于VGICP的回环检测算法，公开数据集实验结果表明：所提算法的APE均方根误差为
0.5008 ，与A-LOAM、LEGO-LOAM、LINS算法相比分别降低了50.41%、47.99%、47.49%，显著提升了回环检测精度和有效抑制了三维重建漂移误差的累计。3）为了验证本文方法的有效性及先进性，开展了三种模拟巷道场景三维重建实验，结果表明：所提算法构建的室内长廊模型各区域在长度、宽度和高度方向的相对误差均在1.2%以内；所提算法构建的煤矿巷道三维模型与真实巷道空间分布一致，总体距离退化误差仅为2.46%，较其他三种算法重建性能分别提升了66.12%、65.30%、70.43%。
4）为了验证本文方法的实用性，在煤矿主体实验室掘进巷道进行三维重建实验，结果表明三维重建结果在长度、宽度和高度方向的平均误差百分比分别为0.47%、0.75%和0.67%，可以实现掘进巷道三维精确建模。
-
![]()
图 2 煤矿巷道三维重建总体方案
Figure 2. The overall scheme of three-dimensional reconstruction of coal mine
![]()
图 10 模拟煤矿巷道工作面场景轮廓
Figure 10. Contour diagram of simulated coal mine tunnel working face scene
![]()
图 11 模拟煤矿巷道工作面场景三维重建结果
Figure 11. Reconstruction results of simulated coal mine tunnel working face scene
表 1 各算法轨迹误差统计
Table 1 Trajectory error statistics of each algorithm
算法 A-LOAM LEGO-LOAM LINS OURS RPE 最大值 2.7500 2.5402 2.3449 1.0224 平均值 0.3815 0.3715 0.3517 0.2197 最小值 0.0023 0.0027 0.0112 0.0067 均方根误差 0.5800 0.5543 0.5258 0.2718 标准差 0.4368 0.4262 0.3909 0.1599 APE 最大值 1.9418 1.8644 1.8088 1.5932 平均值 0.9199 0.8655 0.8555 0.4276 最小值 0.1587 0.1382 0.1308 0.0020 均方根误差 1.0099 0.9628 0.9537 0.5008 标准差 0.4416 0.4167 0.4004 0.2610 
下载: 导出CSV
表 2 室内长廊三维模型重建误差统计
Table 2 Statistics of the reconstruction error of the 3D model of the indoor corridor
测量区域 A-LOAM LINS OURS 长度 宽度 长度 宽度 长度 宽度 高度 AB段 模型尺寸/mm 18717 — 19176 2132 19629 2119 2324 实际尺寸/mm 19700 2100 19700 2100 19700 2100 2300 误差值/mm −983 — −524 32 −71 19 24 误差/% 4.99 — 2.66 1.52 0.36 0.90 1.04 BC段 模型尺寸/mm 10504 2265 10577 2251 10447 2236 2479 实际尺寸/mm 10400 2215 10400 2215 10400 2215 2450 误差值/mm 104 50 177 36 47 21 29 误差/% 1.00 2.26 1.70 1.63 0.45 0.95 1.18 BD段 模型尺寸/mm 8828 3877 — 4005 8736 3973 2476 实际尺寸/mm 8700 3940 8700 3940 8700 3940 2450 误差值/mm 128 −63 — 65 36 33 26 误差/% 1.47 1.60 — 1.65 0.41 0.84 1.06 DE段 模型尺寸/mm 18746 — 19296 2263 21175 2231 2473 实际尺寸/mm 21250 2215 21250 2215 21250 2215 2450 误差值/mm − 2504 — −1954 48 −75 16 23 误差/% 11.78 — 9.20 2.17 0.35 0.72 0.94 EF段 模型尺寸/mm 12996 2286 13005 2256 12952 2235 2478 实际尺寸/mm 12900 2215 12900 2215 12900 2215 2450 误差值/mm 96 71 105 41 52 20 28 误差/% 0.74 3.21 0.81 1.85 0.40 0.90 1.14
下载: 导出CSV
表 3 各算法距离退化误差统计
Table 3 Distance degradation error statistics of each algorithm
方法 P0-P1/m P1-P2/m P2-P3/m P3-P4/m 总距离/m 误差/% 真实值 44.00 48.00 47.00 45.00 184.00 − A-LOAM 41.30 45.77 40.84 42.74 170.65 7.26 LEGO-LOAM 40.33 43.14 44.88 42.61 170.96 7.09 LINS 42.52 43.90 40.31 41.96 168.69 8.32 OURS 43.46 46.91 45.98 43.13 179.48 2.46
下载: 导出CSV
表 4 掘进巷道场景三维重建误差
Table 4 Reconstruction error of the tunnel scene
参数 实际尺寸/m 模型尺寸/m 误差/m 误差/% 长度 23.42 23.31 0.11 0.47 宽度 4.00 3.97 0.03 0.75 高度 3.00 2.98 0.02 0.67
下载: 导出CSV
-
[1] 王国法,王虹,任怀伟,等. 智慧煤矿2025情景目标和发展路径[J]. 煤炭学报,2018,43(2):295−305. WANG Guofa,WANG Hong,REN Huaiwei,et al. 2025 scenarios and development path of intelligent coal mine[J]. Journal of China Coal Society,2018,43(2):295−305.
[2] 毛君,杨润坤,谢苗,等. 煤矿智能快速掘进关键技术研究现状及展望[J]. 煤炭学报,2024,49(2):1214−1229. MAO Jun,YANG Runkun,XIE Miao,et al. Research status and prospects of key technologies for intelligent rapid excavation in coal mines[J]. Journal of China Coal Society,2024,49(2):1214−1229.
[3] 王国法. 《煤矿智能化建设指南(2021年版)》解读:从编写组视角进行解读[J]. 智能矿山,2021,2(4):2−9. [4] 胡兴涛,朱涛,苏继敏,等. 煤矿巷道智能化掘进感知关键技术[J]. 煤炭学报,2021,46(7):2123−2135. HU Xingtao,ZHU Tao,SU Jimin,et al. Key technology of intelligent drivage perception in coal mine roadway[J]. Journal of China Coal Society,2021,46(7):2123−2135.
[5] 马宏伟,王岩,杨林. 煤矿井下移动机器人深度视觉自主导航研究[J]. 煤炭学报,2020,45(6):2193−2206. MA Hongwei,WANG Yan,YANG Lin. Research on depth vision based mobile robot autonomous navigation in underground coal mine[J]. Journal of China Coal Society,2020,45(6):2193−2206.
[6] 杨林,马宏伟,王岩. 基于激光惯性融合的煤矿井下移动机器人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.
[7] ZHANG J,SINGH S. LOAM:Lidar odometry and mapping in real-time[C]//Robotics:Science and Systems X. Robotics:Science and Systems Foundation,2014.
[8] SHAN T X,ENGLOT B. LeGO-LOAM:Lightweight and ground-optimized lidar odometry and mapping on variable terrain[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Piscataway,NJ:IEEE,2018:4758−4765.
[9] YANG L,MA H W,WANG Y,et al. A tightly coupled LiDAR-inertial SLAM for perceptually degraded scenes[J]. Sensors,2022,22(8):3063. doi: 10.3390/s22083063
[10] YANG X,LIN X H,YAO W Q,et al. A robust LiDAR SLAM method for underground coal mine robot with degenerated scene compensation[J]. Remote Sensing,2023,15(1):186.
[11] 刘峰,王宏伟,刘宇. 基于多传感融合的巷道三维空间映射[J]. 煤炭学报,2024,49(9):4019−4026 LIU Feng,WANG Hongwei,LIU Yu. Research on 3D Spatial Mapping of Road ways Based on Multi-sensor Fusion[J]. Journal of Coal Industry,2024,49(9):4019−4026
[12] 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.
[13] 马庆禄,汪军豪,张杰,等. 激光雷达与惯性测量单元同步融合下的园区三维建图[J]. 光学 精密工程,2024,32(3):422−434. doi: 10.37188/OPE.20243203.0422 MA Qinglu,WANG Junhao,ZHANG Jie,et al. Three-dimensional mapping of park based on synchronous fusion of lidar and inertial measurement unit[J]. Optics and Precision Engineering,2024,32(3):422−434. doi: 10.37188/OPE.20243203.0422
[14] XU W,CAI Y X,HE D J,et al. FAST-LIO2:Fast direct LiDAR-inertial odometry[J]. IEEE Transactions on Robotics,2022,38(4):2053−2073. doi: 10.1109/TRO.2022.3141876
[15] 张福斌,王凯,廖伟飞,等. 激光雷达/MEMS IMU/里程计紧组合导航算法[J]. 仪器仪表学报,2022,43(7):139−148. ZHANG Fubin,WANG Kai,LIAO Weifei,et al. Lidar/MEMS IMU/Odometer integrated tightly navigation algorithm[J]. Chinese Journal of Scientific Instrument,2022,43(7):139−148.
[16] 任好玲,吴江东,林添良,等. 基于多传感器紧耦合的工程机械定位与建图系统[J]. 机械工程学报,2023,59(24):323−333. doi: 10.3901/JME.2023.24.323 REN Haoling,WU Jiangdong,LIN Tianliang,et al. Construction machinery localization and mapping system based on multi-sensor tight coupling[J]. Journal of Mechanical Engineering,2023,59(24):323−333. doi: 10.3901/JME.2023.24.323
[17] 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
[18] 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
[19] JI X L,ZUO L,ZHANG C H,et al. LLOAM:LiDAR odometry and mapping with loop-closure detection based correction[C]//2019 IEEE International Conference on Mechatronics and Automation (ICMA). Piscataway,NJ:IEEE,2019:2475−2480.
[20] JIANG J W,WANG J K,WANG P,et al. LiPMatch:LiDAR point cloud plane based loop-closure[J]. IEEE Robotics and Automation Letters,2020,5(4):6861−6868. doi: 10.1109/LRA.2020.3021374
[21] 徐晓苏,李诺,姚逸卿. 基于快速回环检测的室外环境下激光雷达SLAM算法[J]. 中国惯性技术学报,2022,30(6):716−722. XU Xiaosu,LI Nuo,YAO Yiqing. Lidar SLAM algorithm in outdoor environment based on fast loop detection[J]. Journal of Chinese Inertial Technology,2022,30(6):716−722.
[22] 马宝良,崔丽珍,李敏超,等. 露天煤矿环境下基于LiDAR/IMU的紧耦合SLAM算法研究[J]. 煤炭科学技术,2024,52(3):236−244. doi: 10.12438/cst.2023-0538 MA Baoliang,CUI Lizhen,LI Minchao,et al. Study on tightly coupled LiDAR-Inertial SLAM for open pit coal mine environment[J]. Coal Science and Technology,2024,52(3):236−244. doi: 10.12438/cst.2023-0538
[23] 薛光辉,李瑞雪,张钲昊,等. 基于激光雷达的煤矿井底车场地图融合构建方法研究[J]. 煤炭科学技术,2023,51(8):219−227. XUE Guanghui,LI Ruixue,ZHANG Zhenghao,et al. Lidar based map construction fusion method for underground coal mine shaft bottom[J]. Coal Science and Technology,2023,51(8):219−227.
[24] 李芳威,鲍久圣,王陈,等. 基于LD改进Cartographer建图算法的无人驾驶无轨胶轮车井下SLAM自主导航方法及试验[J/OL]. 煤炭学报,1−12[2025−02−18]. https://doi.org/10.13225/j.cnki.jccs.2023.0731. LI Fangwei,BAO Jiusheng,WANG Chen,et al. Unmanned trackless rubber wheeler based on LD improved Cartographer mapping algorithm Underground SLAM autonomous navigation method and test[J/OL]. Journal of Coal Industry,1−12[2025−02−18]. https://doi.org/10.13225/j.cnki.jccs.2023.0731.
[25] 邱佳月,赖际舟,方玮,等. 一种基于动态剔除和场景匹配的Robust SLAM方法[J]. 仪器仪表学报,2022,43(3):249−257. QIU Jiayue,LAI Jizhou,FANG Wei,et al. A Robust SLAM method based on eliminating dynamic points and matching scenes[J]. Chinese Journal of Scientific Instrument,2022,43(3):249−257.
[26] 郝宇,张亿,黄磊,等. 基于改进图优化的移动机器人二维激光SLAM算法研究[J/OL]. 激光与光电子学进展,1−13[2025−02−18]. http://kns.cnki.net/kcms/detail/31.1690.TN.20240517.1421.004.html. HAO Yu,ZHANG Yi,HUANG Lei,et al. Research on Mobile Robot 2D Laser SLAM Algorithm Based on Improved Graph Optimization [J/OL]. Laser & Optoelectronics Progress,1−13[2025−02−18]. http://kns.cnki.net/kcms/detail/31.1690.TN.20240517.1421.004.html.
[27] 薛光辉,张钲昊,张桂艺,等. 煤矿井下点云特征提取和配准算法改进与激光SLAM研究[J/OL]. 煤炭科学技术,1−12[2025−02−18]. http://kns.cnki.net/kcms/detail/11.2402.TD.20240722.1557.003.html. XU Guanghui,ZHANG Zhenghao,ZHANG Guiyi,et al. Improvement of Point Cloud Feature Extraction and Alignment Algorithms and LiDAR SLAM in Coal Mine Underground [J/OL]. Coal Science and Technology, 1−12[2025−02−18]. http://kns.cnki.net/kcms/detail/11.2402.TD.20240722.1557.003.html.
计量
- 文章访问数: 87
- HTML全文浏览量: 14
- PDF下载量: 34
