Coal mine roadway environment-oriented LiDAR and IMU fusion positioning and mapping method
-
摘要: 针对煤矿井下喷浆表面、对称巷道等引起移动机器人自主导航定位与建图失效问题，提出了一种面向煤矿巷道环境的激光雷达(LiDAR)与惯性测量单元（IMU)融合的实时定位与建图方法。首先对原始点云进行分割，利用IMU预积分位姿去除原始点云非线性运动畸变，并对得到的点云进行线、面特征提取。然后将相邻帧的线、面特征进行匹配，在分层位姿估计过程中融合IMU预积分所得到的位姿初值，减少计算迭代次数，提高特征点匹配的精度，解算出当前帧的位姿。最后向因子图中插入局部地图因子、IMU因子、关键帧因子，对位姿进行优化约束，对关键帧与局部地图进行匹配，通过八叉树结构实现地图构建。为验证所提方法的定位性能与建图效果，搭建了Autolabor 、VLP−16 LiDAR和Ellipse−N IMU的实验平台进行验证，并与LeGO−LOAM、LIO−SAM方法进行定性定量对比分析。结果表明：① 在煤矿巷道环境中，面向煤矿巷道环境的LiDAR与IMU融合的实时定位与建图方法三轴方向的绝对定位误差的均值和中值均小于32 cm；对X轴的位姿估计精度最高，其累计误差为1.65 m，位置偏差为2.97 m，建图效果整体良好，建图轨迹未发生漂移；构建的点云地图在完整性和几何结构真实性方面均有着优秀的表现，可以直观反映巷道环境的实际情况，具有良好的鲁棒性。这是因为点云匹配之后进行了分层位姿估计，多因子优化可有效降低全局累计误差，对轨迹精度和地图的一致性提升具有重要作用。② 在楼道走廊环境中，面向煤矿巷道环境的LiDAR与IMU融合的实时定位与建图方法三轴的误差均小于1.01 m，误差均值为5~15 cm，误差范围小，精度高；累计位置偏差仅为1.67 m；完整性与环境匹配均有良好的性能。这是由于通过增加关键帧因子，插入因子图对其新增节点相关变量进行优化，降低了位姿估计漂移，定位与建图精度相对较高。Abstract: The failure of autonomous navigation, positioning and mapping of the mobile robot is caused by the shotcrete surface and symmetrical roadway in coal mine. In order to solve this problem, a real-time positioning and mapping method based on LiDAR and IMU fusion is proposed for the roadway environment in the coal mine. Firstly, the original point cloud is segmented. The IMU pre-integration pose is used to remove the nonlinear motion distortion of the original point cloud. The line and surface feature extraction is carried out on the obtained point cloud. Secondly, the line and surface features of adjacent frames are matched. The initial pose value obtained by IMU pre-integration is fused in the hierarchical pose estimation process. The calculation iteration times are reduced, the matching precision of feature points is improved, and the pose of the current frame is solved. Finally, the local map factor, IMU factor and key frame factor are inserted into the factor graph to optimize and constrain the pose. The key frame is matched with the local map, and the map construction is realized through an octree structure. In order to verify the positioning performance and mapping effect of the proposed method, the experimental platforms of Autolabor, VLP-16 LiDAR and Ellipse-N IMU are built. The qualitative and quantitative comparison between the proposed method and LeGO-LOAM and LIO-SAM methods is carried out. The results show the following points. ① In the coal mine roadway environment, the average and median of the absolute positioning error in the three axes direction of the real-time positioning and mapping method based on LiDAR and IMU fusion are less than 32 cm. The position and attitude estimation precision in the X-axis is the highest, with a cumulative error of 1.65 m and a position deviation of 2.97 m. The overall mapping effect is good, and the mapping track does not drift. The point cloud map constructed has excellent performance in integrity and geometric structure authenticity. The map can directly reflect the actual situation of the roadway environment, and has good robustness. This is because hierarchical pose estimation is performed after point cloud matching. The multi-factor optimization can effectively reduce the global cumulative error, which plays an important role in improving track precision and map consistency. ② In the corridor environment, the three-axis error of the real-time positioning and mapping method based on LiDAR and IMU fusion for the coal mine roadway environment is less than 1.01 m. The average error is 5~15 cm, with small error range and high precision. The accumulated position deviation is only 1.67 m. Integrity and environment matching have good performance. This is because by adding keyframe factors and inserting factor graphs to optimize the related variables of the newly added nodes, the drift of pose estimation is reduced. The positioning and mapping precision is relatively high.
-
0. 引言
煤矿事故发生后，井下存在不确定和危险因素，依靠人工救援成功率低，煤矿井下移动机器人可替代救援人员深入矿井探测灾害现场情况，检查事故现场，并在第一时间将获得的环境信息传输到指挥中心，制定救援策略[1]，以提高救援效率[2]。然而，传统的导线测量[3]、贯通设计、三维激光扫描等技术方案测量速度慢、效率低，严重制约了煤矿井下环境建模，且实施过程中难以保证建模精度[4]。近年来，激光雷达(Light Detection and Ranging，LiDAR)、惯性测量单元（Inertial Measurement Unit，IMU）和全球导航卫星系统(Global Navigation Satellite System，GNSS)[5]的移动激光雷达平台被广泛用于获取三维地理信息数据[6]。煤矿井下环境复杂，无GNSS信号，工况恶劣，地面常规定位与建图技术无法直接应用，因此采用同时定位与地图构建（Simultaneous Localization and Mapping，SLAM）[7]技术辅助煤矿井下移动机器人执行路径规划、自主探索、自主导航任务[8]。基于SLAM技术矿井机器人可以实时准确地构建煤矿井下三维地图，该地图可以灵活、可靠地辅助矿井机器人进行智能导航和避障，并应用于矿井下危险区作业、自动巡检、远程调度等领域，对实现智能化定位导航起着至关重要作用[9-10]。然而，在煤矿井下喷浆表面、对称巷道环境中，单一传感器的SLAM技术已不能满足智能感知精度及可靠性的需求。因此，多源传感器融合的SLAM技术应用在煤矿井下成为了当前学术研究与工程应用的聚焦点。
目前，LiDAR和IMU的融合方法主要分为松耦合和紧耦合2种方法。松耦合类方法[11]分别处理LiDAR和IMU的位姿估计，然后再进行融合。文献[12]提出了利用IMU信息来辅助估计激光里程计，IMU积分状态为激光点云匹配提供位姿初值，该方法缺乏回环抑制漂移误差。紧耦合类方法[13]对LiDAR和IMU的数据进行互相估计与更新。文献[14]将IMU预积分因子、激光里程计因子、GNSS因子和闭环因子进行联合优化，取得了较好的建图效果，但并未考虑煤矿环境对称巷道退化对SLAM结果的影响。文献[15]提出用误差卡尔曼滤波对IMU进行预测，并与LiDAR进行紧融合的方法，该方法用激光雷达高程图和先验数字高程图进行匹配，并用匹配值对状态进行更新，该方法展示了在长距离无GNSS信号场景下的导航能力，但需要先验地图，无法应对无先验地图的场景。文献[16]提出了一种基于因子图的LiDAR和IMU紧耦合算法，通过最小化IMU预积分和LiDAR点云匹配结果的残差来优化LiDAR位姿和IMU偏置，根据IMU的预积分值对LiDAR位姿进行线性插值，去除点云畸变，实验表明该方法可应对LiDAR退化的场景和快速运动的情况，但效率较低，实时性较差，而且基于匀速假设的去畸变策略无法适用于运动变化较快的场景。文献[17-18]提出了一种紧耦合算法，在畸变去除部分，该算法使用高斯过程回归对IMU预积分测量值进行采样，利用采样值对LiDAR位姿进行插值，完成畸变去除，取得了很好效果，但该算法计算效率较低。文献[19]提出了一种MC2SLAM的3D LiDAR和IMU紧耦合里程计算法，该算法使用因子图融合3D LiDAR和IMU，在畸变去除部分，使用一种基于匀速假设的非刚性点云匹配算法，通过最小化点云匹配的残差优化LiDAR位姿，再对优化得到的位姿进行线性插值，但是该算法的特征点选取策略过于简单且存在随机性，精度不够稳定。
为解决发生煤矿事故后救援机器人难以自主导航定位的问题，本文提出了一种面向煤矿巷道环境的LiDAR与IMU融合的实时定位与建图方法，用于探测矿井灾害现场情况，提高煤矿事故救援效率。该方法通过数据预处理、点云扫描匹配和后端因子图优化等多层次的数据融合，降低轨迹漂移，增强救援机器人系统在煤矿巷道环境中快速移动时的鲁棒性，实现高精度定位与建图。
1. SLAM基本框架
SLAM基本框架由预处理、激光里程计、地图构建3个部分组成，如图1所示。首先对原始点云进行分割，利用IMU预积分位姿去除原始点云非线性运动畸变，并对得到的点云进行线、面特征提取。然后将相邻帧的线、面特征进行匹配，在分层位姿估计过程中融合IMU预积分所得到的位姿初值，减少计算迭代次数，提高特征点匹配的精度，解算出当前帧的位姿。最后向因子图中插入局部地图因子、IMU因子、关键帧因子，对位姿进行优化约束，对关键帧与局部地图进行匹配，通过八叉树结构实现地图构建。
2. 基于特征点匹配的相对位姿估计
2.1 预处理
在实际作业环境中，搭载LiDAR的移动机器人不可避免产生非线性运动，导致扫描的激光点云存在非线性运动畸变，因此，利用高频率的IMU状态对激光点进行补偿校正[16]。因为煤矿巷道地面特征点和墙壁特征点对于垂直方向有一定的约束，且平面特征对水平方向的平移和旋转有较好的约束，所以先用基于随机样本一致性(Random Sample Consensus，RANSAC）[20]的快速点云分割算法提取环境中的巷道地面点和墙壁的平面特征。特征提取部分利用深度图计算某一点的曲率，将曲率较大的非地面点标记为线特征点，将曲率较小的非地面点标记为平面特征点[21]。
2.2 激光里程计
在提取特征的前后帧点云中，首先利用KD-tree[22]搜索方法寻找最近邻相似属性的特征点，并将法向和主方向夹角均小于5°且距离小于1 cm的特征点作为同一类特征点。建立点到线和点到面距离最小的约束，进行分层位姿估计，如图2所示。由于煤矿巷道的面特征比线特征多，占据位姿权重较大，数据可信度高，因此，选择源点云面特征和目标点云面特征点去对应匹配位姿，确定垂向平移量、横滚角、俯仰角，得出一个较准确的位姿。再根据源点云线特征和目标点云线特征匹配确定侧向平移量、前向平移量和航向角，从而达到整体优化位姿精度。
3. 多因子联合图优化及地图构建
3.1 关键帧因子提取
为确保本文算法在煤矿井下实时运行，在前端里程计中部署了关键帧选取策略，
$ {T}_{1}—{T}_{6} $ 为连续帧，$ \Delta {T}$ 为连续帧$ {T}_{1}—{T}_{6} $ 之间的位姿变化，选择$ {T}_{1} $ 和$ {T}_{6} $ 为关键帧，如图3所示。关键帧的选取可极大地提高计算效率，确保点云能够快速匹配。选取关键帧的标准是给欧氏距离
$\Delta S $ 、关键帧旋转矩阵$ \Delta {\boldsymbol{R}} $ 、时间间隔$ \Delta t $ 设定一个阈值，任意满足其一，则当前第$ k $ 帧点云被选取为候选关键帧。选择关键帧时，必须减少匹配错误帧和冗余关键帧，以达到节省计算量的目的。在保证精度的前提下减少数据冗余，以提高运算效率。因此，基于实时点云关键帧选择标准，将第1帧点云作为判断准则开始选择关键帧。连续关键帧的相对变换位姿为$$ \Delta {T_{k - 1,k}} = T_{k - 1}^{ - 1}{T_k} = \left[ \begin{gathered} \Delta {\boldsymbol{q}}{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \Delta {\boldsymbol{{S}}} \\ 0{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} 1 \\ \end{gathered} \right] $$ (1) $$ \Delta {\boldsymbol{R}}=\arccos \left(\frac{\operatorname{trace}(\Delta {\boldsymbol{q}})-1}{2}\right) $$ (2) $$ \|\Delta S\|=\sqrt{\Delta d^2+\Delta e^2+\Delta f^2} $$ (3) $$ \Delta t=t_k-t_{k-1} $$ (4) 式中：
$ \Delta {T_{k - 1,k}} $ 为第$ k - 1 $ 帧到第k帧关键帧的位姿变化；$ T_{k-1} $ 为第$ k - 1 $ 帧的关键帧；$ T_{{k}} $ 为第$ k $ 帧的关键帧；$ \Delta {\boldsymbol{q}} $ 为第$ k - 1 $ 帧到第$ k $ 帧关键帧的旋转矩阵；$ \Delta d $ ，$ \Delta e $ ，$ \Delta f $ 分别为在X轴、Y轴、Z轴的位移变化；$ {t_k} $ 为第$ k $ 帧关键帧的时间戳。在确定关键帧后，再以初始相对变换位姿优化点云配准，最终变换的位姿应用于位姿和位姿图中位置之间的约束。
3.2 多因子位姿优化及地图构建
假设LiDAR第i（i=1,2，…，j，j为关键帧总次数）帧关键帧的扫描起始时刻为
$ {t_i} $ ，扫描得到的全部点云为$ P_{i} $ ；IMU在$ \left[t_{i}, t_{j}\right] $ 内采集的数据为I$ _{(i, j)} $ 。系统在$ t_{i} $ 时刻的姿态、位置、速度和IMU零点偏置为$$ x_{i}=\left[{\boldsymbol{R}}_{i}, t_{i}, v_{i}, b_{i}\right] $$ (5) 式中：xi为关键帧在
$ t_{i} $ 时刻的状态；Ri为李群表示的旋转矩阵；vi为速度；bi为零点偏差。设在
$ t_{k} $ 时刻的所有关键帧为$ \mathcal{K}_{k} $ ，其对应的状态量$\chi_{k}=\left\{x_{i}\right\}_{i \in \mathcal{K}_{k}}$ ，其对应的所有观测量$ \mathcal{Z}_{k}=\left\{P_{i,} I_{(i, j)}\right\}_{(i, j) \in \mathcal{K}_{k}} $ 。在给定观测量
$ \mathcal{Z}_{k} $ 和先验信息$ p\left(\chi_{0}\right) $ 的条件下，预测关键帧$ \mathcal{K}_{k} $ 对应的初始状态量$ \chi_{0} $ 的后验概率问题为状态估计问题。$$ p\left(\chi_k \mid \mathcal{Z}_k\right) \propto p\left(\chi_0\right) p\left(\mathcal{Z}_k \mid \chi_k\right)=p\left(\chi_0\right) \prod_{(i, j) \in \mathcal{K}_k} p\left(P_i, I_{(i, j)} \mid \chi_k\right) $$ (6) $$ \begin{split} & p\left(\chi_0\right) \prod_{(i, j) \in \mathcal{K}_k} p\left(P_i, I_{(i, j)} \mid \chi_k\right)= \\& p\left(\chi_0\right) \prod_{(i, j) \in \mathcal{K}_k} p\left(I_{(i, j)} \mid x_i, x_j\right) \prod_{i \in \mathcal{K}_k} p\left(P_i \mid x_i\right) \end{split}$$ (7) 变量因子最大后验概率为
$$ \begin{array}{*{20}{c}} \hat{\chi}_k=\arg \min \left(-\ln p\left(\chi_k \mid \mathcal{Z}_k\right)\right)= \\ \arg \min _{\chi_t}\left( \left\|r_0\right\|_{{{{\boldsymbol{\varGamma}}}}_0}^2 + \displaystyle \sum_{(i, j) \in \mathcal{K}_t}\left\|r_{I(i, j)}\right\|_{{{{\boldsymbol{\varGamma}}}}_{L_{i, 0}}}^2 + \displaystyle \sum_{(i, j) \in \mathcal{K}_t}\left\|r_{P_{i, j}}\right\|_{{{{\boldsymbol{\varGamma}}}}_{S_{i, 0}}}^2 + \displaystyle \sum_{i \in \mathcal{K}_{\mathrm{t}}}\left\|r_{P_i}\right\|_{{{{\boldsymbol{\varGamma}}}}_{s_i}}^2 \right) \end{array} $$ (8) 式中：
${\chi}_t $ 为t时刻的状态量；$ r $ 为观测模型与实际观测的残差，是关于状态量$ \chi_{k} $ 的函数；$\mathcal{K}_t $ 为t时刻的关键帧；${\boldsymbol{\varGamma }}$ 为对应的协方差矩阵。根据IMU动力学模型，采用离散化积分方法对加速度和角速度在IMU采样间隔时间
$ \Delta t $ 内积分，将高频输出的加速度和角速度观测量转换为状态量间的位姿变换，构成关键帧状态量之间的约束因子。$$ \Delta {\boldsymbol{R}}_{i j}={\boldsymbol{R}}_i^{\mathrm{T}} {\boldsymbol{R}}_j=\prod_{k=i}^{j-1} \exp \left(\left(\left(\widetilde{\omega_k}-b_k^g-\eta^g\right) \cdot \Delta t\right)^{\wedge}\right) $$ (9) $$ \Delta v_{i j}={\boldsymbol{R}}_i^{\mathrm{T}}\left(v_j-v_i-g \cdot \Delta t_{i j}\right)=\sum_{k=i}^{j-1} \Delta {\boldsymbol{R}}_{i k}\left(\widetilde{a_k}-b_k^a-\eta^a\right) \cdot \Delta t $$ (10) $$\begin{split} \Delta s_{i j}=&{\boldsymbol{R}}_i^{\mathrm{T}}\Bigg(s_j-s_i-v_i \Delta t_{i j}-\frac{1}{2} \sum_{k=i}^{j-1} g(\Delta t)^2\Bigg)= \\& \sum_{k=i}^{j-1}\left(\Delta v_{i k} \Delta t+\frac{1}{2} \Delta {\boldsymbol{R}}_{i k}\left(\widetilde{a_k}-b_k^a-\eta^a\right)(\Delta t)^2\right) \end{split}$$ (11) 式中：
$ \Delta \boldsymbol{R}_{ij} $ ，$ \Delta v_{i j} $ ，$ \Delta s_{i j} $ 分别为在$ \Delta t $ 内第$ i $ ~$ j $ 帧关键帧之间的旋转矩阵、速度、位移；Rj为第j帧关键帧的旋转矩阵；vi，vj分别为第i，j帧关键帧的速度；$\Delta \boldsymbol{R}_{i k} $ 为$\Delta t $ 内第i～k帧关键帧之间的旋转矩阵；$\widetilde{a_k}$ 、$\widetilde{\omega_k}$ 分别为第$ k $ 帧关键帧的角速度、加速度；$ b_{k}^{a} $ ，$ b_{k}^{g} $ 分别为IMU的零偏加速度与零偏角速度；si，sj分别为第i，j帧关键帧的位移；g为重力加速度；$ \eta $ 为观测噪声。假设关键帧在状态
$ x_{i} $ 与$ {x}_{j} $ 的IMU零点偏置保持不变，则可由式（9）−式（11）得预积分观测模型。$$ \Delta \widetilde{{\boldsymbol{R}}}_{i j}={\boldsymbol{R}}_i^{\mathrm{T}} {\boldsymbol{R}}_j \exp \left(\delta \phi_{i j}^{\wedge}\right) $$ (12) $$ \Delta \widetilde{v_{i j}}={\boldsymbol{R}}_i^{\mathrm{T}}\left(v_j-v_i-g \cdot \Delta t_{i j}\right)+\delta v_{i j} $$ (13) $$ \Delta \widetilde{s_{i j}}={\boldsymbol{R}}_i^{\mathrm{T}}\left(s_j-s_i-v_i \Delta t_{i j}-\frac{1}{2} g \Delta t_{i j}{ }^2\right)+\delta s_{i j} $$ (14) 式中
$ \delta \phi_{i j}^{\wedge} $ ，$ \delta v_{i j} $ ，$ \delta_{s_{ij}} $ 均为随机变量，其协方差矩阵${{\boldsymbol{\varGamma }}}_{ij}$ 由误差传播定律推导。由式（12）−式（14）可推导出残差项
$ {{r}}_{I(i, j)} $ 。$$ {{r}}_{I(i, j)}=\left[{{r}}_{\Delta {\boldsymbol{R}}_{i j}}^{\mathrm{T}} \;\; {{r}}_{\Delta v_{i j}}^{\mathrm{T}} \;\; {{r}}_{\Delta t_{i j}}^{\mathrm{T}}\right]^{\mathrm{T}} $$ (15) 多因子图优化模块是在系统中维护一个全局因子图，因子图结构如图4所示。设给定初始状态为
$ {x}_{0} $ ，采用数据预处理模块计算相邻关键帧之间的IMU预积分，并向因子图插入IMU预积分因子$ {{r}}_{I(i, j)} $ 。$$ {{r}}_{I_{i, j,}}=\left(x_i \ominus x_j\right) \ominus \int I_{(i, j)} {\rm{d}} t $$ (16) 式中
$\ominus $ 为李群中的位姿变换的逆运算。特征点云与局部地图配准，优化激光里程计估计的位姿
$\overline{Q}_{(i, j)}$ ，地图构建模块向因子图插入地图配准因子${r_{p_i}} $ 。$$ r_{p_i}=\left(x_i \ominus m_j\right) \ominus \overline{Q}_{(i, j)} $$ (17) 式中mj为第j帧的关键帧局部地图。
在因子图中插入关键帧因子
$ {r_{{p_{\left( {i,j} \right)}}}} $ 。$$ r_{p_{(i, j)}}=\left(x_i \ominus x_j\right) \ominus \overline{Q}_{(i, j)} $$ (18) 当提取一个连续帧作为关键帧时，根据连续关键帧的位姿估计，将关键帧点云匹配进行地图的更新。首先利用线、面特征匹配求得LiDAR坐标中连续关键帧之间的位姿，然后通过坐标变换更新关键帧位姿，并将其点云转换到全局坐标系中，最后通过八叉树结构对点云图进行更新。
4. 实验结果和分析
4.1 实验设备及数据介绍
为了测试本文所提方法性能，自主搭建Autolabor 、VLP-16 LiDAR和Ellipse-N IMU的实验平台，如图5所示。本次实验中LiDAR频率为10 Hz，IMU频率为200 Hz，2个传感器采用硬同步方式进行时间对齐。煤矿井下移动机器人可进行原地旋转，最大旋转角速度为0.523 5 rad/s，最大位移速度为1.5 m/s。移动机器人系统采用C++实现，在机器人操作系统（Robot operation system，ROS)中运行。利用全站仪记录移动机器人经过参考点时的绝对位置，定量分析移动机器人位姿精度。实验所采集的数据分别有煤矿巷道和楼道数据，具体信息见表1。
表 1 实测数据Table 1. Measured data数据集 地物
类型移动
目标轨迹
长度/m高差/cm 最大位移
速度/（m·s−1）煤矿巷道A 防护装置 无 279 15 0.8 楼道走廊B 室内建筑 少量 221 10 1.5 4.2 实验结果分析
为验证本文所提方法的定位性能与建图效果，在煤矿巷道A和楼道走廊B中选取起点和终点相同的闭环环境，定量计算各方案的累计位姿误差，定性评价各方案的建图效果及起点和终点的激光里程计轨迹闭合情况。
为了评价不同方法的绝对定位精度，对煤矿巷道场景A所示的10个控制点（A0−A9）和楼道走廊场景B所示的6个控制点（B0−B5）的坐标真值（图6）进行测量。分别与目前主流的LeGO−LOAM，LIO−SAM方法进行对比。通过全站仪测量真值，并记录移动机器人在对应控制点位置停止区间时刻的起点和终点，将其时间段内的位置估计结果求取均值，作为当前方法在该点的观测值。全站仪实测数据作为判断移动机器人定位精度的依据。
![]()
图 6 场景A（参考点A0—A9）和场景B（参考点B0—B5）的坐标真值Figure 6. Coordinate ture value of scenario A (reference points A0-A9) and scenario B (reference points B0-B5)4.2.1 定位精度分析
为分析移动机器人在运动过程中的定位精度，给移动机器人粘贴标志点，分别在场景A（A0—A9）、场景B（B0—B5）处，使用全站仪测量其真值，并对3种方法绝对定位误差分布效果进行分析，如图7所示。在场景A中可看出，LeGO−LOAM方法定位精度较差，在X方向上最大绝对定位误差为300 cm；LIO−SAM方法定位精度虽高于LeGO−LOAM方法，但在三轴方向的绝对定位误差在50 cm附近；本文方法在三轴方向的绝对定位误差的均值和中值均小于32 cm。这是因为点云匹配之后进行了分层位姿估计，多因子优化可有效降低全局累计误差，对轨迹精度和地图的一致性的提升具有重要作用。在场景B中可看出，LeGO−LOAM方法在Z轴的误差均值为50~55 cm，X、Y轴误差均值为30~35 cm；LIO−SAM方法三轴误差均值为15~25 cm；本文方法三轴的误差均值为5~15 cm，误差范围小，精度高。
![]()
图 7 场景A、B的绝对定位误差分布Figure 7. Absolute positioning error distribution of scenarios A and B3种方法的累计误差见表2。在场景A中可看出，LeGO−LOAM方法在狭小的煤矿巷道点云匹配发生退化，造成三轴的累计误差很大，位置偏差达到49.16 m，使得建图效果发生了漂移；本文方法和LIO−SAM方法的定位精度相对较高，而本文方法对X轴的位姿估计精度最高，其累计误差为1.65 m，位置偏差为2.97 m，从三轴或累计误差来看，本文方法建图效果整体良好，建图轨迹未发生漂移。在场景B中可看出，LeGO−LOAM方法在X、Y轴的累计误差均小于1.5 m，Z轴误差达到了2.33 m，位置累计偏差为2.95 m；LIO−SAM方法在X、Y轴的累计误差分别为2.21，3.15 m，Z轴误差仅为1.05 m，位置偏差为3.99 m；本文方法在X、Y、Z轴的误差均小于1.01 m，累计位置偏差仅为1.67 m。综合分析得出，在较少特征的楼道中，LeGO−LOAM方法与LIO−SAM方法点云匹配时发生退化，导致X、Y、Z轴的平移误差较大，点云模型相差较大，建图定位精度较差。本文方法应用于楼道退化场景依旧有较强的鲁棒性，定位精度高，有良好的建图效果。
表 2 3种方法的累计误差Table 2. Cumulative error of there methods场景 方法 ΔX/m ΔY/m ΔZ/m 位置
偏差/mA LeGO−LOAM 13.10 16.56 44.39 49.16 LIO−SAM 2.10 1.88 3.04 4.15 本文方法 1.65 1.75 1.74 2.97 B LeGO−LOAM 1.42 1.13 2.33 2.95 LIO−SAM 2.21 3.15 1.05 3.99 本文方法 0.94 0.94 1.01 1.67 4.2.2 建图效果分析
为了进一步说明3种方法在煤矿巷道和楼道定位的建图效果，分别在煤矿巷道与楼道环境进行验证，建图结果与轨迹如图8和图9所示。
![]()
图 8 煤矿巷道场景A 中3种方法建图结果与轨迹Figure 8. Mapping results and tracks of three methods in coal mine roadway scenarios A![]()
图 9 楼道走廊场景B中3种方法建图结果与轨迹Figure 9. Mapping results and tracks of three methods in corridor scenarios B由图8可看出，LeGO−LOAM方法由于点云误匹配出现大的漂移现象，建图失效；LIO−SAM方法可以构建出大致轮廓的点云地图，但由于Z轴上出现整体明显漂移，导致移动机器人所生成的点云地图结构变形且不完整；本文方法构建的点云地图，在完整性和几何结构真实性方面均有着优秀的表现，可以直观地反映巷道环境的实际情况，在煤矿井下环境具有良好的鲁棒性。
由图9可看出，采用LeGO−LOAM方法进行建图时，轨迹重复，整体发生偏移，移动机器人在回到终点时，轨迹无法闭合，建图失效；采用LIO−SAM方法进行建图时，随着激光里程计里程的增加，由于部分场景相似，导致建立了错误的回环约束，发生漂移，以致点云地图构建失败；采用本文方法进行建图时，地图完整性与环境匹配均有良好的性能，这是由于通过增加关键帧因子，插入因子图对其新增节点相关变量进行优化，降低了位姿估计漂移，定位与建图精度相对较高。
5. 结论
（1） 提出了一种LiDAR与IMU融合的实时定位与建图方法。该方法首先采用面特征匹配估计位姿，然后进行线特征匹配，实现激光序列帧的相对位姿估计，提高点云匹配精度，最后在因子图优化与地图构建过程中插入关键帧因子、IMU预积分因子、局部地图配准因子，对LiDAR和IMU数据进行整体优化，提高了实时定位与建图的精度，降低了煤矿环境下SLAM的累计误差。
（2） 该方法在自采数据集上分别进行了定量定性分析，并与现有的LeGO−LOAM，LIO−SAM方法进行比较。结果表明：① 在煤矿巷道环境中，本文方法三轴的绝对定位误差的均值和中值均小于32 cm；X轴的位姿估计精度最高，其累计误差为1.65 m，位置偏差为2.97 m，建图效果整体良好，建图轨迹未发生漂移；构建的点云地图，在完整性和几何结构真实性方面均有着优秀的表现，可以直观地反映巷道环境的实际情况，具有良好的鲁棒性。② 在楼道走廊环境中，本文方法三轴的误差均小于1.01 m，误差均值为5~15 cm，误差范围小，精度高；累计位置偏差仅为1.67 m；地图完整性与环境匹配均有良好的性能。
-
![]()
图 6 场景A（参考点A0—A9）和场景B（参考点B0—B5）的坐标真值
Figure 6. Coordinate ture value of scenario A (reference points A0-A9) and scenario B (reference points B0-B5)
![]()
图 7 场景A、B的绝对定位误差分布
Figure 7. Absolute positioning error distribution of scenarios A and B
![]()
图 8 煤矿巷道场景A 中3种方法建图结果与轨迹
Figure 8. Mapping results and tracks of three methods in coal mine roadway scenarios A
![]()
图 9 楼道走廊场景B中3种方法建图结果与轨迹
Figure 9. Mapping results and tracks of three methods in corridor scenarios B
表 1 实测数据
Table 1 Measured data
数据集 地物
类型移动
目标轨迹
长度/m高差/cm 最大位移
速度/（m·s−1）煤矿巷道A 防护装置 无 279 15 0.8 楼道走廊B 室内建筑 少量 221 10 1.5 
下载: 导出CSV
表 2 3种方法的累计误差
Table 2 Cumulative error of there methods
场景 方法 ΔX/m ΔY/m ΔZ/m 位置
偏差/mA LeGO−LOAM 13.10 16.56 44.39 49.16 LIO−SAM 2.10 1.88 3.04 4.15 本文方法 1.65 1.75 1.74 2.97 B LeGO−LOAM 1.42 1.13 2.33 2.95 LIO−SAM 2.21 3.15 1.05 3.99 本文方法 0.94 0.94 1.01 1.67
下载: 导出CSV
-
[1] 文虎,刘洋,郑学召,等. 矿山救援机器人群设计[J]. 工矿自动化,2019,45(9):34-39. DOI: 10.13272/j.issn.1671-251x.17476 WEN Hu,LIU Yang,ZHENG Xuezhao,et al. Design of mine rescue robot group[J]. Industry and Mine Automation,2019,45(9):34-39. DOI: 10.13272/j.issn.1671-251x.17476
[2] 翟国栋,任聪,王帅,等. 多尺度特征融合的煤矿救援机器人目标检测模型[J]. 工矿自动化,2020,46(11):54-58. DOI: 10.13272/j.issn.1671-251x.2020050033 ZHAI Guodong,REN Cong,WANG Shuai,et al. Object detection model of coal mine rescue robot based on multi-scale feature fusion[J]. Industry and Mine Automation,2020,46(11):54-58. DOI: 10.13272/j.issn.1671-251x.2020050033
[3] 孙金礼,陈杰. 煤矿井下巷道贯通测量精度分析及技术方法[J]. 煤炭科学技术,2010,38(6):112-114,66. DOI: 10.13199/j.cst.2010.06.72.sunjl.025 SUN Jinli,CHEN Jie. Analysis and technical method of linkage survey accruacy for underground mine roadway[J]. Coal Science and Technology,2010,38(6):112-114,66. DOI: 10.13199/j.cst.2010.06.72.sunjl.025
[4] 胡建胜. 煤矿全站仪导线测量误差分析及技术措施研究[J]. 能源技术与管理,2018,43(4):170-172. DOI: 10.3969/j.issn.1672-9943.2018.04.070 HU Jiansheng. Study on error analysis and technical measures of total station traverse measurement in coal mine[J]. Energy Technology and Management,2018,43(4):170-172. DOI: 10.3969/j.issn.1672-9943.2018.04.070
[5] LI D,YAO Yuan,SHAO Zhenfeng,et al. From digital earth to smart earth[J]. Chinese Science Bulletin,2014,59(8):722-733. DOI: 10.1007/s11434-013-0100-x
[6] 蒋萍. LiDAR/IMU组合导航定位算法研究[D]. 南昌: 南昌大学, 2021. JIANG Ping. Research on LiDAR/IMU integrated navigation and positioning algorithm[D]. Nanchang: Nanchang University, 2021.
[7] CHEN Chi,YANG Bisheng,TIAN Mao,et al. Automatic registration of vehicle-borne mobile mapping laser point cloud and sequent panoramas[J]. Acta Geodaetica et Cartographica Sinica,2018,47(2):215.
[8] 陈先中,刘荣杰,张森,等. 煤矿地下毫米波雷达点云成像与环境地图导航研究进展[J]. 煤炭学报,2020,45(6):2182-2192. DOI: 10.13225/j.cnki.jccs.zn20.0316 CHEN Xianzhong,LIU Rongjie,ZHANG Sen,et al. Development of millimeter wave radar imaging and SLAM in underground coal mine environment[J]. Journal of China Coal Society,2020,45(6):2182-2192. DOI: 10.13225/j.cnki.jccs.zn20.0316
[9] DEBEUNNE C,VIVET D. A review of visual-LiDAR fusion based simultaneous localization and mapping[J]. Sensors(Basel,Switzerland),2020,20(7):2068.
[10] CADENA C,CARLONE L,CARRILLO H,et al. Past,present,and future of simultaneous localization and mapping:toward the robust-perception age[J]. IEEE Transactions on Robotics,2016,32(6):1309-1332. DOI: 10.1109/TRO.2016.2624754
[11] 种一帆,冀杰,宫铭钱,等. 半直接法与IMU融合的双目视觉里程计[J]. 西南师范大学学报(自然科学版),2021,46(2):112-120. CHONG Yifan,JI Jie,GONG Mingqian,et al. A stereo visual odometry aided by IMU based on semi-direct method[J]. Journal of Southwest China Normal University(Natural Science Edition),2021,46(2):112-120.
[12] ZHANG Ji,SINGH S. Low-drift and real-time lidar odometry and mapping[J]. Autonomous Robots,2017,41(2):401-416. DOI: 10.1007/s10514-016-9548-2
[13] 周治国,曹江微,邸顺帆. 3D激光雷达SLAM算法综述[J]. 仪器仪表学报,2021,42(9):13-27. DOI: 10.19650/j.cnki.cjsi.J2107897 ZHOU Zhiguo,CAO Jiangwei,DI Shunfan. Overview of 3D Lidar SLAM algorithms[J]. Chinese Journal of Scientific Instrument,2021,42(9):13-27. DOI: 10.19650/j.cnki.cjsi.J2107897
[14] SHAN T X, 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）, Las Vegas, 2021: 5135-5142.
[15] HEMANN G, SINGH S, KAESS M. Long-range GPS-denied aerial inertial navigation with LIDAR localization[C]. IEEE/RSJ International Conference on Inetlligent Robots & Systems, Daejeon, 2016: 1659-1666.
[16] YE Haoyang, CHEN Yuying, LIU Ming, et al. Tightly coupled 3d lidar inertial odometry and mapping[C]. International Conference on Robotics and Automation（ICRA）, Montreal, 2019: 3144-3150.
[17] GENTIL C L,VIDAL-CALLLEJJA T,HUANG Shoudong. IN2LAAMA:inertial lidar localization autocalibration and mapping[J]. IEEE Transactions on Robotics,2021,37(1):275-290. DOI: 10.1109/TRO.2020.3018641
[18] KULKARNI M, DHARMADHIKARI M, TRANZATTO M, et al. Autonomous teamed exploration of subterranean environments using legged and aerial robots[C]. International Conference on Robotics and Automatioan（ICRA）, Philadelphia, 2022: 3306-3313.
[19] FRANK N, TILMAN K, ROBERT K, et al. Mc2SLAM: real-time inertial lidar odometry using two-scan motion compensation[C]. German Conference on Pattern Recognition, Cham, 2018: 60-72.
[20] REN Zhuli,WANG Liguan,BI Lin. Robust GICP-based 3D LiDAR SLAM for underground mining environment[J]. Sensors (Basel Switzerland),2019,19(13):2915. DOI: 10.3390/s19132915
[21] 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, 2019: 4758-4765.
[22] ZHAO Yue,ZHANG Jian,FU Chiwing,et al. KD-Box:line-segment-based KD-tree for interactive exploration of large-scale time-series data[J]. IEEE Transactions on Visualization and Computer Graphics,2021,28(1):890-900.
-
期刊类型引用(11)
1. 马庆禄，汪军豪，张杰，邹政. 激光雷达与惯性测量单元同步融合下的园区三维建图. 光学精密工程. 2024(03): 422-434 . 
百度学术
2. 周波，唐桂彬. 基于遥感GIS信息融合的测绘机器人滑模跟踪控制系统. 计算机测量与控制. 2024(06): 125-132 . 百度学术
3. 高毅楠，姚顽强，蔺小虎，郑俊良，马柏林，冯玮，高康洲. 煤矿井下多重约束的视觉SLAM关键帧选取方法. 煤炭学报. 2024(S1): 472-482 . 百度学术
4. 倪志平，卢光云. 激光雷达实时采集数据的车辆智能定位方法研究. 激光杂志. 2024(09): 218-222 . 百度学术
5. 覃学宁，钟辉，岳志伟，何瑞冠，冷泉，李飞. 基于轨迹重构与贝叶斯推理的空中机器人灯塔距离测绘技术. 计算机测量与控制. 2024(10): 291-297 . 百度学术
6. 胡青松，李敬雯，张元生，李世银，孙彦景. 面向矿井无人驾驶的IMU与激光雷达融合SLAM技术. 工矿自动化. 2024(10): 21-28 . 本站查看
7. 马亮，高亮，廉博翔，张琦，蔺小虎，姜之跃. 基于已知点约束的高精度煤矿巷道三维点云建模方法. 工矿自动化. 2024(11): 78-83+151 . 本站查看
8. 马宏伟，苏浩，薛旭升，李超，郭逸风，王星，周文剑，崔闻达，喻祖坤，成佳帅. 煤矿井下移动机器人激光标靶定位方法研究. 煤炭科学技术. 2024(11): 60-73 . 百度学术
9. 景宁波，马宪民，郭卫，秦学斌. 改进动态半径的矿井激光雷达点云滤波算法. 西安科技大学学报. 2023(02): 406-413 . 百度学术
10. 高海跃，王凯，王保兵，王丹丹. 基于全局点云地图的煤矿井下无人机定位方法. 工矿自动化. 2023(08): 81-87+133 . 本站查看
11. 王媛媛. 基于多传感信息融合的学前教育机器人导航定位研究. 自动化与仪器仪表. 2023(09): 233-236 . 百度学术
其他类型引用(1)
计量
- 文章访问数: 354
- HTML全文浏览量: 35
- PDF下载量: 64
- 被引次数: 12
