Study on tightly coupled LiDAR-Inertial SLAM for open pit coal mine environment
-
摘要:
随着人工智能和无人驾驶等相关学科的快速发展，煤矿装备的智能化和无人化成为了新的趋势。智能设备的应用将大幅提高煤矿作业的生产力以及人员安全性。露天煤矿地形复杂，与城市环境相比无明显的几何特征，具有分段相似性，利用现有以激光雷达为主的同时定位与建图（Simultaneous Localization and Mapping，SLAM）方案在该环境下易出现定位漂移和建图误差较大等现象。针对上述问题，提出了一种基于激光雷达（Light Detection and Ranging，LiDAR）和惯导（Inertial Measurement Unit，IMU）紧耦合的SLAM算法，该算法使用LiDAR和IMU两种传感器作为数据输入，对数据进行预处理，前端利用迭代扩展卡尔曼滤波器将预处理后的LiDAR特征点与IMU数据相融合，并使用后向传播来矫正雷达运动畸变，后端利用雷达相对位姿因子将LiDAR帧间配准结果作为约束因子与回环因子共同完成全局因子图优化。利用开源数据集和露天煤矿实地数据集验证了算法的鲁棒性和精确性。试验结果表明在城市结构化环境中文中所提算法与当前激光SLAM算法精度保持一致，而针对长达两千多米的露天煤矿实地环境，所提算法较FAST-LIO2、LIO-SAM紧耦合算法在定位精度上分别提高了46.00%和23.15%，且具有更高的鲁棒性。
Abstract:With the rapid development of artificial intelligence and unmanned and other related disciplines, the intelligence and unmanned of coal mining equipment has become a new trend. The application of intelligent equipment will greatly improve the productivity of coal mine operations as well as personnel safety. In this environment, the existing LIDAR-based Simultaneous localization and mapping (SLAM) solution is prone to positioning drift and large mapping errors. To address these problems, a tightly coupled SLAM algorithm based on LiDAR (Light Detection and Ranging) and IMU (Inertial Measurement Unit) is proposed, which uses both LiDAR and IMU sensors as data inputs.The front-end uses an iterative extended Kalman filter to fuse the pre-processed LiDAR feature points with the IMU data and uses backward propagation to correct the radar motion distortion, the back-end uses the LiDAR relative positional factor to use the LiDAR inter-frame alignment results as a constraint factor together with the loopback factor to complete the global factor map optimization. The robustness and accuracy of the algorithm are verified using open source dataset and open pit coal mine field dataset. The experimental results show that the accuracy of the proposed algorithm is consistent with the current LiDAR SLAM algorithm in the urban structured environment, while the proposed algorithm improves the localization accuracy by 46.00% and 23.15% with higher robustness than the FAST-LIO2 and LIO-SAM tightly coupled algorithms for the open pit coal mine field environment of more than
2000 meters long, respectively.-
Keywords:
- open-pit coal mine /
- simultaneous localization and mapping /
- LiDAR /
- IMU /
- factor graph
-
0. 引 言
“双碳”目标下[1]，煤炭依旧为我国主体能源，为响应国家生态文明建设和实现高质量发展，加快推进绿色智能煤矿建设尤为重要[2-3]。煤炭开采环境复杂、条件恶劣，大量挖掘运输设备相互交叉运行在矿场中，操作人员工作强度大、危险系数高，安全问题难以保障[4]。因此，实现开采机械自动化、无人驾驶矿车智能化在煤矿开采中的应用是建设智能矿山的首要举措[5-6]。同时定位与地图构建是实现采矿设备自动化和采矿环境数字化的一种关键技术，该技术能够让机器人在未知的环境中，完成定位、建图和路径规划[7]。
早期的SLAM使用单一的激光雷达进行定位，在非结构化或复杂恶劣的环境下运行时会造成点云地图稀疏的问题。如REN[8]等在连续帧之间、连续关键帧之间和回环检测帧之间使用GICP[9]配准方法并使用了随机抽样一致性（Random Sample Consensus,RANSAC）对煤矿巷道点云进行了分割，并将分割得到的巷道平面作为了观测约束，实现了在结构化的煤矿巷道环境低漂移定位与建图。惯性测量单元能够不受环境影响，实现高频高精度位姿估计，但其在长时间工作状态下会导致自身误差累积无法精确估计，因此将LiDAR和IMU进行融合能够实现在复杂恶劣环境下长时间高精度定位[10]。目前，国内外学者在LiDAR/IMU融合方面已经进行了大量研究，主要分为松耦合和紧耦合2大类[11]。对LiDAR和IMU的测量数据分别处理后进行加权融合确定运动状态的方法被称为松耦合，如ZHANG等[12]提出了激光里程计与建图（LiDAR Odometry and Mapping，LOAM），该算法使用IMU计算的姿态作为LiDAR扫描配准的初始值，而未将其作为全局优化的约束条件。后续对松耦合系统的研究大多延续了LOAM算法框架，如SHAN 等[13]在LOAM算法的基础上增加了地面优化，对具有相同类别的特征点进行匹配，提高了融合计算效率，使得特征匹配更稳定。XUE等[14]在LeGO-LOAM基础上通过整合扫描上下文回环算法降低了点云维数，提高了在煤矿井下模拟场景下的实时性和鲁棒性。
松耦合算法仅停留在数据结果层面分析，无法解决退化场景问题，为此，紧耦合算法使用图优化或者滤波的方式将LiDAR与IMU观测数据融合，充分考虑了两者间的内在约束，系统的鲁棒性和准确性得到提升。YE等提出的LIO-Mapping[15]使用VINS-Mono[16]中的优化过程来最小化IMU残差和LiDAR测量误差，并以一种旋转约束的方法来细化最终的地图，但由于该约束的计算复杂性，系统难以实时运行。SHAN等[17]提出了基于图优化的LIO-SAM使用划窗的方式来完成帧到局部子图的扫描配准，关键帧的选择以及对旧帧的边缘化有效降低了计算复杂性，提高了系统运行速度。QIN等基于滤波的LINS[18]提出了一种以机器人为中心，以误差状态进行迭代卡尔曼滤波的融合算法。YANG等[19]利用扰动模型对煤矿长巷道退化环境进行了检测和补偿，IMU预积分用于补偿旋转状态退化，LiDAR与IMU融合产生的新姿态用于补偿平移状态退化，提高了煤矿巷道退化场景的鲁棒性。XU等在FAST-LIO[20]中提出了一个新的计算卡尔曼增益公式，显著提高了计算效率。在此基础之上的FAST-LIO2[21]提出了一种新的数据结构ikd-tree[22]，能够高效动态地对数据结构进行划分，实现增量更新地图，极大地提高了地图管理效率。
尽管当前SLAM解决方案众多，但是在露天煤矿环境下使用时的表现却不尽如人意。该环境存在大量斜坡、碎石以及开采后产生的不规则单侧石壁，算法会随运行距离增长产生累计漂移问题，而且大多数SLAM算法需要提取边缘和平面等几何特征进行配准，均采用LOAM框架对海量点云进行多次降采样，导致配准结果不准确，影响定位精度以及建图效果。另外，在这样的环境下会出现分段式的相似环境，当无法检测到新的特征时LiDAR会出现退化现象，一旦配准失败且经过IMU积分发散，SLAM的定位性能将迅速下降，导致建图失败。综上所述，SLAM如何在露天煤矿环境下保持系统的鲁棒性和准确性仍是一大难题。为此，提出了一种基于滤波与图优化相结合的紧耦合SLAM算法，前端使用迭代扩展卡尔曼滤波（Iterater Extended Kalman Filter,IEKF）来实现激光雷达与IMU数据的融合，后端分别设计了雷达相对位姿因子和回环检测因子来完成全局优化，增强了在露天煤矿环境下对颠簸不平路况的鲁棒性，实现了复杂环境下精确的定位建图，为煤矿设备智能化提供了一种关键技术支撑。
1. 系统框架
所提出的算法主要由前端迭代扩展卡尔曼滤波和后端因子图优化两部分组成。该算法使用LiDAR和IMU两种传感器作为数据输入信息源。前端使用紧耦合的迭代扩展卡尔曼滤波器将LiDAR特征点与IMU数据相融合，使用后向传播来补偿LiDAR运动失真，经过运动补偿后与该时刻的特征点来构造残差，完成状态更新。后端分别设计了雷达相对位姿因子和回环检测因子，相对位姿因子作用于全局提供新的关位姿与局部子图内关键帧位姿之间的约束，并对当前帧与局部子图进行匹配；回环检测因子同样作用于全局，检测是否与历史位姿重合并对相邻关键帧进行调整来，完成全局优化。最后输出优化后的机器人轨迹，位姿以及全局点云地图，系统结构如图1所示。
2. 系统模型描述
使用了文献[23]中定义封装了两个算子⊞和⊟，⊞：$\mathcal{S} \times {\mathbb{R}^n} \to \mathcal{S}$及其逆⊟：$\mathcal{S} \times \mathcal{S} \to {\mathbb{R}^n}$，其中$\mathcal{S}$为维数为n的流形（$\mathcal{S} = SO(3)$）。IMU坐标系记为${I}$，一般假设IMU与LiDAR以外参$^I{{\boldsymbol{T}}_L} = {(^I}{{\boldsymbol{R}}_L}{,^I}{{\boldsymbol{p}}_L})$刚性连接在一起，则IMU运动学模型如下：
$$ \begin{array}{l}{}^{G}{{\boldsymbol{p}}}_{I}=^{G}{{\boldsymbol{v}}}_{I}{,}^{G}{\dot{{\boldsymbol{v}}}}_{I}=^{G}{{\boldsymbol{R}}}_{I}\left({{\boldsymbol{a}}}_{m}-{{\boldsymbol{b}}}_{a}-{{\boldsymbol{n}}}_{a}\right)+^{G}{\boldsymbol{g}}{\text{，}}^{G}\dot{{\boldsymbol{g}}}=0\\ {}^{G}{\dot{{\boldsymbol{R}}}}_{I}=^{G}{{\boldsymbol{R}}}_{I}{\lfloor {{\boldsymbol{\omega}} }_{m}-{{\boldsymbol{b}}}_{\omega }-{{\boldsymbol{n}}}_{\omega }\rfloor }_{\wedge }\text{，}{\dot{{\boldsymbol{b}}}}_{a}={{\boldsymbol{n}}}_{ba},{\dot{{\boldsymbol{b}}}}_{\omega }={n}_{{\boldsymbol{b}}\omega }\end{array} $$ (1) 其中，$ ^G{{\boldsymbol{p}}_I} $和$ ^G{{\boldsymbol{R}}_I} $分别为IMU在机器人坐标系（记为$ G $）中的位置和姿态；$ ^G{\boldsymbol{g}} $为未知重力向量；$ {{\boldsymbol{a}}_m} $和$ {{\boldsymbol{\omega }}_m} $为IMU测得的加速度与角速度；$ {{\boldsymbol{n}}_{\boldsymbol{a}}} $和$ {{\boldsymbol{n}}_{\boldsymbol{\omega }}} $为IMU测量值的白噪声；$ {{\boldsymbol{b}}_{\boldsymbol{a}}} $和$ {{\boldsymbol{b}}_{\boldsymbol{\omega }}} $建模为高斯噪声$ {{\boldsymbol{n}}_{{\boldsymbol{ba}}}} $和$ {{\boldsymbol{n}}_{{\boldsymbol{b\omega }}}} $的一阶马尔科夫过程，符号${\left\lfloor \alpha \right\rfloor _ \wedge }$为向量${\boldsymbol{\alpha }}\in {\mathbb{R}^3}$的斜对称矩阵。离散化上述模型可以得到：
$$ {{\boldsymbol{x}}_{i + 1}} = {{\boldsymbol{x}}_i} \boxplus \left[ {\Delta t{\boldsymbol{f}}({{\boldsymbol{x}}_i},{{\boldsymbol{u}}_i},{{\boldsymbol{w}}_i})} \right] $$ (2) 其中，$\Delta t$为IMU采样时间间隔；${\boldsymbol{f}}$为函数；${\boldsymbol{x}}$为状态；${\boldsymbol{u}}$为输入；${\boldsymbol{w}}$为噪声；定义如下：
$$ \boldsymbol{f}\left(\boldsymbol{x}_i, \boldsymbol{u}_i, \boldsymbol{w}_i\right)=\left[\begin{array}{c} \boldsymbol{\omega}_{m_i}-\boldsymbol{b}_{\boldsymbol{\omega}_i}-\boldsymbol{n}_{\boldsymbol{\omega}_i} \\ { }^G \boldsymbol{v}_{I_i} \\ \boldsymbol{R}_{I_i}\left(\boldsymbol{a}_{m_i}-\boldsymbol{b}_{\boldsymbol{a}_i}-\boldsymbol{n}_{\boldsymbol{a}_i}\right)+{ }^G \boldsymbol{g}_i \\ \boldsymbol{n}_{\boldsymbol{b \omega}_i} \\ \boldsymbol{n}_{\boldsymbol{b a}_i} \\ \boldsymbol{0}_{3 \times 1} \end{array}\right] $$ (3) $$ \begin{aligned} & \boldsymbol{x} \doteq\left[\begin{array}{llllll} { }^G \boldsymbol{R}_{\mathrm{I}}^{\mathrm{T}} & { }^G \boldsymbol{p}_{\mathrm{I}}^{\mathrm{T}} & { }^G \boldsymbol{v}_{\mathrm{I}}^{\mathrm{T}} & \boldsymbol{b}_{\boldsymbol{\omega}}^{\mathrm{T}} & \boldsymbol{b}_{\boldsymbol{a}}^{\mathrm{T}} & { }^G \boldsymbol{g}^{\mathrm{T}} \end{array}\right]^{\mathrm{T}} \in \mathcal{S} \\ & \boldsymbol{u} \doteq\left[\begin{array}{llll} \boldsymbol{\omega}_{{m}}^{\mathrm{T}} & \boldsymbol{a}_{{m}}^{\mathrm{T}} \end{array}\right]^{\mathrm{T}} \\ & \boldsymbol{w} \doteq\left[\begin{array}{llll} \boldsymbol{n}_{\boldsymbol{\omega}}^{\mathrm{T}} & \boldsymbol{n}_{\boldsymbol{a}}^{\mathrm{T}} & \boldsymbol{n}_{\boldsymbol{b \omega}}^{\mathrm{T}} & \boldsymbol{n}_{{ba}}^{\mathrm{T}} \end{array}\right]^{\mathrm{T}} \end{aligned} $$ (4) 3. 激光惯性里程计模型构建
使用IEKF来估计式(2)中的状态。假设LiDAR在$t_{k{-1}}$时刻的扫描最优状态估计值是${{\boldsymbol{\bar x}}_{k - 1}}$，则系统的随机误差状态向量为
$$ {{\boldsymbol{\tilde x}}_{k - 1}} \doteq {{\boldsymbol{x}}_{k - 1}} \boxminus {{\boldsymbol{\bar x}}_{k - 1}} = {\left[ {\begin{array}{*{20}{l}} {\delta {{\boldsymbol{\theta }}^{\mathrm{T}}}}&{^G{\boldsymbol{\tilde p}}_{\mathrm{I}}^{\mathrm{T}}}&{{}^G{\boldsymbol{\tilde v}}_{\mathrm{I}}^{\mathrm{T}}}&{{\boldsymbol{\tilde b}}_{\boldsymbol{\omega }}^{\mathrm{T}}}&{{\boldsymbol{\tilde b}}_{\boldsymbol{a}}^{\mathrm{T}}}&{^G{{{\boldsymbol{\tilde g}}}^{\mathrm{T}}}} \end{array}} \right]^{\mathrm{T}}} $$ (5) 其中，$ \delta {\boldsymbol{\theta }} = {\text{log}}{(^G}\overline {\boldsymbol{R}} _I^{TG}{{\boldsymbol{R}}_I}) $为姿态误差，其他项为标准加性误差[24]，姿态误差$ \delta {\boldsymbol{\theta }} $的定义直观地描述了真实姿态与估计姿态之间的偏差，使得姿态不确定性可以简洁的使用3$ \times $3的协方差矩阵$\mathbb{E}[ \delta \theta \delta {\theta ^{\mathrm{T}}}]$表示。
3.1 状态传播
IMU测量时，按照式(2)将过程噪声设置为零进行传播得到下式：
$$ {\widehat{{\boldsymbol{x}}}}_{i+1}={\widehat{{\boldsymbol{x}}}}_{i}\boxplus\left(\Delta t{\boldsymbol{f}}\left({\widehat{{\boldsymbol{x}}}}_{i},{{\boldsymbol{u}}}_{i},0\right)\right)\text{，}{\widehat{{\boldsymbol{x}}}}_{0}={\overline{{\boldsymbol{x}}}}_{k-1} $$ (6) 传播协方差以迭代误差动态模型来处理，定义如下[19]：
$$ \begin{split} {{{\boldsymbol{\tilde x}}}_{i + 1}} & ={{\boldsymbol{x}}_{i + 1}} \boxminus {{{\boldsymbol{\hat x}}}_{i + 1}} \\ & {\text{ }} = \left( {{{\boldsymbol{x}}_i} \boxplus \Delta t{\boldsymbol{f}}\left( {{{\boldsymbol{x}}_i},{{\boldsymbol{u}}_i},{{\boldsymbol{w}}_i}} \right)} \right) \boxminus \left( {{{{\boldsymbol{\hat x}}}_i} \boxplus \Delta t{\boldsymbol{f}}\left( {{{{\boldsymbol{\hat x}}}_i},{{\boldsymbol{u}}_i},{\boldsymbol{0}}} \right)} \right) \\ & {\text{ }} \simeq {{\boldsymbol{F}}_{{\boldsymbol{\tilde x}}}}{{{\boldsymbol{\tilde x}}}_i} + {{\boldsymbol{F}}_{\boldsymbol{w}}}{{\boldsymbol{w}}_i} \\ \end{split} $$ (7) 其中，$ {{\boldsymbol{F}}_{{\boldsymbol{\tilde x}}}} $和$ {{\boldsymbol{F}}_{\boldsymbol{w}}} $分别为$ {{\boldsymbol{\tilde x}}_i} $和$ {{\boldsymbol{w}}_i} $的雅克比矩阵，定义如下：
$${ {{\boldsymbol{F}}_{\widetilde {\boldsymbol{x}}}} = \left[ {\begin{array}{*{20}{c}} {{Exp} \left( { - {{{{\hat {\boldsymbol{\omega}} }}}_i}\Delta t} \right)} & {\boldsymbol{0}} & {\boldsymbol{0}} & { - {\boldsymbol{A}}{{\left( {{{{{\hat {\boldsymbol{\omega}} }}}_i}\Delta t} \right)}^{\rm{T}}}\Delta t} & 0 & 0 \\ 0 & {{\text{ }}{\boldsymbol{I}}} & {{\boldsymbol{I}}{\text{ }}\Delta t} & 0 & 0 & 0 \\ {{ - ^G}{{{{\hat {\boldsymbol{R}}}}}_{{I_i}}}{{\left\lfloor {{{{{\hat {\boldsymbol{a}}}}}_i}} \right\rfloor }_ \wedge }\Delta t} & 0 & {\boldsymbol{I}} & 0 & {{ - ^G}{{{{\hat {\boldsymbol{R}}}}}_{{I_i}}}\Delta t} & {{\boldsymbol{I}}\Delta t} \\ 0 & 0 & 0 & {\boldsymbol{I}} & 0 & 0 \\ 0 & 0 & 0 & 0 & {\boldsymbol{I}} & 0 \\ 0 & 0 & 0 & 0 & 0 & {\boldsymbol{I}} \end{array}} \right] }$$ (8) $$ {{\boldsymbol{F}}_{\boldsymbol{w}}} = \left[ {\begin{array}{*{20}{c}} { - {\boldsymbol{A}}{{\left( {{{{{\hat {\boldsymbol{\omega}} }}}_i}\Delta t} \right)}^{\rm{T}}}\Delta t}&0&0&0 \\ 0&0&0&0 \\ 0&{{ - ^G}{{{{\hat {\boldsymbol{R}}}}}_{{I_i}}}\Delta t}&0&0 \\ 0&0&{{\boldsymbol{I}}\Delta t}&0 \\ 0&0&0&{{\boldsymbol{I}}\Delta t} \\ 0&0&0&0 \end{array}} \right] $$ (9) 其中，${\hat \omega _i} = {\omega _{{m_i}}} - {{\boldsymbol{\hat b}}_{{{\boldsymbol{\omega }}_i}}}$，${{\boldsymbol{\hat a}}_i} = {{\boldsymbol{a}}_{{m_i}}} - {{\boldsymbol{\hat b}}_{{{\boldsymbol{a}}_i}}}$
传播一直进行到${{{t}}_{{k}}}$这一帧结束，传播状态以及协方差分别记为$ {{\boldsymbol{\hat x}}_k} $和${{\boldsymbol{\hat P}}_k}$，将白噪声$ {\boldsymbol{w}} $的协方差定义为$ {\boldsymbol{Q}} $，则传播协方差${{\boldsymbol{\hat P}}_i}$可以通过下式计算得出：
$$ {\widehat{{\boldsymbol{P}}}}_{i+1}={{\boldsymbol{F}}}_{\tilde{x}}{\widehat{{\boldsymbol{P}}}}_{i}{{\boldsymbol{F}}}_{\tilde{x}}^{{\mathrm{T}}}+{{\boldsymbol{F}}}_{w}{\boldsymbol{Q}}{{\boldsymbol{F}}}_{w}^{{\mathrm{T}}}\text{，}{\widehat{{\boldsymbol{P}}}}_{0}={\overline{{\boldsymbol{P}}}}_{k-1} $$ (10) 当点云累积时间间隔达到${t_k}$时，将新产生的特征点集与式(9)中的传播状态和协方差进行融合来产生最优的状态。对于采集一帧点云时间内产生的时间偏差所造成的运动畸变，可以用高于IMU测量值的后向传播进行补偿。后向传播可以在采样时间$ {\rho _j} $以及扫描结束时间${t_k}$之间，产生一个相对位姿$ ^{{I_k}}{\mathop {\boldsymbol{T}}\limits^ \vee _{{I_j}}} = {(^{{I_k}}}{\mathop {\boldsymbol{R}}\limits^ \vee _{{I_j}}}{,^{{I_k}}}{\mathop {\boldsymbol{p}}\limits^ \vee _{{I_j}}}) $。该相对位姿可以将局部测量${}^{{L_j}}{{\boldsymbol{p}}_{{f_j}}}$投影到扫描结束时的测量位置：
$$ { }^{L_k} \boldsymbol{p}_{f_j}={ }^I \boldsymbol{T}_L^{-1 I_k} \stackrel{\rightharpoonup}{\boldsymbol{T}}_{I_j}{ }^I \boldsymbol{T}_L^{L_j} \boldsymbol{p}_{f_j} $$ (11) 其中，$ {}^I{\boldsymbol{T}}_L^{} $为LiDAR和IMU之间外参，可通过标定获得，特征点集$ ^{{L_k}}{{\boldsymbol{p}}_{{f_j}}} $可以用来构造残差，将残差定义为特征点全局点云坐标${}^G{\boldsymbol{\hat p}}_{{f_j}}^\kappa $与地图中最近的平面点或者边缘点的距离：
$$ {\boldsymbol{z}}_j^\kappa = {{\boldsymbol{G}}_j}({}^{{G}}\hat p_{{f_j}}^\kappa - {}^G{{\boldsymbol{q}}_j}) $$ (12) 3.2 状态更新
$ \boldsymbol{x}_{k} $先验分布由前向传播得到：
$$ {{\boldsymbol{x}}_k} \boxminus {{\boldsymbol{\hat x}}_k} = ({\boldsymbol{\hat x}}_k^\kappa \boxplus {\boldsymbol{x}}_k^\kappa ) \boxminus {{\boldsymbol{x}}_k} = {\boldsymbol{\hat x}}_k^\kappa \boxminus {{\boldsymbol{x}}_k} + {{\boldsymbol{J}}^\kappa }{\boldsymbol{\tilde x}}_k^\kappa $$ (13) $$ {\boldsymbol{\hat x}}_k^{\kappa + 1} = {\boldsymbol{\hat x}}_k^\kappa \boxplus \left[ { - {\boldsymbol{Kz}}_k^\kappa - ({\boldsymbol{I}} - {\boldsymbol{KH}}){{({{\boldsymbol{J}}^\kappa })}^{ - 1}}({\boldsymbol{\hat x}}_k^\kappa \boxminus {{{\boldsymbol{\hat x}}}_k})} \right] $$ (14) 利用式(13)更新后的估计值${\boldsymbol{\hat x}}_k^{\kappa + 1}$来计算残差，并重复该过程，直到$ \Vert {\widehat{{\boldsymbol{x}}}}_{k}^{\kappa +1}\boxminus {\widehat{{\boldsymbol{x}}}}_{k}^{\kappa }\Vert < \varepsilon $，收敛后最优估计状态和协方差为：
$$ {{\boldsymbol{\bar x}}_k} = {\boldsymbol{\hat x}}_k^{\kappa + 1},{\overline {\boldsymbol{P}} _k} = ({\boldsymbol{I}} - {\boldsymbol{KH}}){\boldsymbol{P}} $$ (15) $$ {\boldsymbol{K}} = {\left( {{{\boldsymbol{H}}^{\rm{T}}}{{\boldsymbol{R}}^{ - 1}}{\boldsymbol{H}} + {{\boldsymbol{P}}^{ - 1}}} \right)^{ - 1}}{{\boldsymbol{H}}^{\rm{T}}}{{\boldsymbol{R}}^{ - 1}} $$ (16) 其中，${\boldsymbol{K}}$为卡尔曼增益，随着状态更新，每个特征点（$ { }^{5} \boldsymbol{P}_{f j} $）将会投影到全局地图中，实现地图更新。
4. 全局因子图优化
理论上前端的状态更新后已经可以获得完整的激光雷达惯性里程计，但在实际试验过程中，上述得到的定位与建图结果随着系统运行会产生偏移，并且这种偏移是不可逆的。这个问题在露天煤矿环境下显得尤为突出，图优化的方式可以非常简洁方便的描述各个误差项之间的关联，因此本文后端使用因子图来进行全局位姿图优化，通过对残差模型进行分析，最终目的就是最小化一个残差项。构建了雷达相对位姿和回环检测两个因子来实现全局位姿优化。
1）雷达相对位姿因子。在前端激光雷达惯性里程计中采用了帧到子图的扫描配准方式，并且通过滑动窗口的方式来创建固定关键帧数量的子图。机械式雷达的运行频率一般为10 Hz，相邻帧之间变化量极小但却需要消耗海量的计算资源，将每一帧都添加到因子图中进行优化显然是低效的，因此本文采用了选取关键帧的策略。关键帧的选取主要遵循两个阈值指标进行确立：①最小平移阈值d当系统接收到的当前帧与上一帧的最小平移距离超过d时选取当前帧为关键帧，这样可以避免设备移动缓慢甚至暂停时产生大量冗余激光帧。②最小旋转角$\alpha $，当接受到的当前帧与上一帧的姿态旋转角度变化值大于$\alpha $时选取该帧为关键帧，由于机械雷达扫描覆盖范围较大，该阈值的设定可避免设备在颠簸路面的微量转动提取到太多的激光点云数据。满足上述任一指标即可确立当前帧为关键帧，利用关键帧来维护一个固定滑窗，即局部子图。接着对新到来的关键帧进行特征提取，利用该帧以及局部子图来构建以下代价函数求解位姿变换关系$ \overline{\boldsymbol{T}}_{k+1} $：
$$ f{\text{(}}{{\boldsymbol{\bar T}}_{k + 1}}{\text{)}} = {\boldsymbol{d}},{\boldsymbol{d}} = \left[ {\begin{array}{*{20}{c}} {{d_{\rm{e}}}} \\ {{d_{\rm{p}}}} \end{array}} \right] $$ (17) ${d_{\rm{e}}}$和${d_{\rm{p}}}$分别为新一帧提取到的边缘特征和平面特征与其在局部子图中相邻帧提取到对应特征之间的距离，以此构建相邻位姿间的优化目标：
$$ {\phi _L}({\boldsymbol{x}}) = \frac{1}{2}\left\| {{r_L}({{\boldsymbol{x}}_i},{{\boldsymbol{x}}_j})} \right\|_\Sigma ^2 $$ (18) 其中，${\boldsymbol{x}}$为待优化量；${{\boldsymbol{x}}_i}$和${{\boldsymbol{x}}_j}$分别为第$ \hat{t} $帧和相邻第$ j $帧对应运因子动状态。
2）回环检测。SLAM系统运行时，各个传感器误差会不断积累，这一误差难以消除并且会逐渐增大，导致定位结果漂移。回环检测因子的使用对于纠正漂移误差以及构建全局一致的地图具有重要作用[25]。此外，在露天煤矿环境下往来矿用卡车基本会走固定路线进行运输，重复运输的路线使得检测到回环的几率大幅增加，因此本文将回环检测因子加入因子图，与雷达相对位姿因子共同进行对全局位姿的优化。
当新的关键帧确立后，首先设定最小距离阈值，对因子图内的全部历史关键帧进行搜索，找到与新关键帧在欧氏距离上满足最小阈值的关键帧，设其为待匹配帧。使用GICP对新帧与对应待匹配帧进行配准，通过式(19)计算出最近点均方根的最小距离获得置信分数，同时与设定的最小距离阈值进行比较，确认该帧是否为回环关键帧，确定后可利用式(20)计算出该位姿的相对变换并添加到因子图中。该步骤需要严格设置最小距离阈值，一旦回环检测匹配到错误的关键帧就会导致地图崩溃。
$$ {\rm{score}} = \frac{1}{N}\sum\limits_{k = 1}^N {\sqrt {\Delta {x^2} + \Delta {y^2} + \Delta {{\textit{z}}^2}} } $$ (19) $$ \Delta {{\boldsymbol{T}}_{k,k + 1}} = {\boldsymbol{T}}_k^{ - 1}{{\boldsymbol{T}}_{k + 1}} $$ (20) 5. 试验和讨论
为验证文中算法的有效性以及鲁棒性，我们分别在公共开源数据集以及露天煤矿实测数据集上进行了试验。开源数据集选择了M2DGR[26]：一个带有全套传感器的地面机器人对上海交通大学校园内不同场景采集的大规模数据集，所有传感器都经过良好的外部校准和时间同步。露天煤矿实测数据集使用的数据采集设备如图2所示，该设备主要搭载传感器为速腾聚创RS-LiDAR-16型激光雷达，10 Hz工作模式下水平分辨率为0.2°，垂直分辨率为2°；超核电子CH110型9轴IMU，设置采样频率为400 Hz；北斗星通C200卫星接收机；机载计算机为NVIDIA Jetson AGX Xavier，8核ARM处理器，主频2.26 GHz，内存32 G。提出的SLAM算法均使用C++实现，运行于Ubuntu20.04，ROS版本为noetic。
5.1 开源数据集试验
使用开源数据集对目前主流激光SLAM算法：A-LOAM、LeGO-LOAM、FAST-LIO2、LIO-SAM与本文所提算法进行了测试，通过计算最终绝对位姿误差(Absolute Position Error，APE)来计算各个算法的定位精度。所有算法均在相同配置环境下使用，运行设备为Intel(R) i9-10850K 3.60 GHz。分别选取了3个不同场景下采集的数据street_08、door_02和gate_02序列进行试验，使用EVO[27]作为最终结果评估工具。其中street08序列为户外环境，采集过程中存在大量连续转弯，图3所示为各个算法在该序列上的定位轨迹对比结果。
![]()
图 3 street_08序列各算法轨迹对比Figure 3. Trajectory comparison of each algorithm for the street_08 sequence从图3中可以看出A-LOAM、LeGO-LOAM表现较差，运行一段时间后开始出现明显漂移，LIO-SAM同样存在该问题，但绕行一周后成功检测到了回环，纠正了累计漂移，仍取得较好的结果，FAST-LIO2对新的数据结构的应用可以在不提取特征的情况下可以快速直接地将原始点配准到地图上，在短距离结构化的环境下，精度较高。算法所设计的相对位姿因子有效抑制了IMU累计误差，同时后期检测到了回环，表现良好。各算法在所选序列试验结果的APE见表1，其中加粗项表示该项测试中误差项最小的结果，对应箱型图如图4所示。
表 1 开源数据集APE误差Table 1. Open source dataset APE error数据集 RMSE/m A-LOAM LeGO-LOAM FAST-LIO2 LIO-SAM OURS door_02 0.1861 2.0724 0.2801 0.1849 0.1855 gate_02 0.3305 0.3171 0.3204 0.3264 0.3167 street_08 3.3376 1.1327 0.1982 0.1708 0.1179 5.2 露天矿山实测数据集试验
为进一步验证算法的有效性以及鲁棒性，对内蒙古哈尔乌素露天煤矿进行了数据采集，实地环境如图5所示。该环境道路崎岖不平，沿途布满山石碎块，呈现分段相似性，且结构复杂无明显的几何特征。在采集的数据集中选取了两个序列进行试验分析，各算法配置均与开源数据集试验配置相同。
序列1全长
2024 m，为实时观测到矿山数据误差累积情况，对序列1进行了分段切片处理，分别截取为500、1000 和2000 m。图6分别展示了不同算法在两处不同地点的建图效果。从图6建图效果中能够看出LeGO-LOAM和FAST-LIO2存在明显的漂移现象，虽完成了最终建图，但是误差较大。A-LOAM与LIO-SAM无直观地漂移现象，但累计定位误差较大。本文所提出的算法完整地构建出了高精度点云地图，对碎石岩壁结构建图清晰，扫描到的矿卡轮廓直观无重影，具有较高的一致性。针对露天煤矿环境体现出了较高的鲁棒性。
序列2全长2 120 m，各算法运行出的定位轨迹如图7所示。A-LOAM和LeGO-LOAM属于单一激光雷达建图算法，缺少IMU的辅助出现了较大的累计误差，尤其是轻量化LiDAR里程计LeGO-LOAM严重依赖地面特征提取，在露天煤矿这样的环境下，大量斜坡的存在导致该算法严重失效，状态估计出现了较大误差。FAST-LIO2默认不使用特征提取的方法，降采样之后直接将原始点云配准到地图上来体现全局一致性，但是在碎石岩壁与颠簸道路环境中，原始点云经过降采样后呈现高度一致性，导致几处相似场景出现了较大的漂移现象。
为避免这些问题，提出的算法将激光雷达帧间配准结果作为约束因子在后端与回环因子进行联合优化，进一步提升了SLAM系统的鲁棒性，同时提高了定位建图精度。各算法在所选序列试验结果的APE见表2，其中加粗项表示该项测试中误差项最小的结果，其对应箱型图如图8所示。
表 2 露天煤矿数据集APE误差Table 2. open pit coal mine dataset APE error数据集 RMSE/m A-LOAM LeGO-LOAM FAST-LIO2 LIO-SAM OURS 序列1-500 m 1.9001 12.7723 12.8621 2.7005 0.8394 序列1- 1000 m7.3956 12.7723 20.6426 3.3946 1.4519 序列1- 2000 m10.1301 31.3826 24.5531 4.1940 3.1644 序列2- 2120 m6.9211 51.4630 10.1340 7.1204 5.4720 6. 结 论
1）提出了一种针对露天煤矿复杂环境下具有高精度高鲁棒性的LiDAR/IMU紧耦合SLAM算法，前端使用紧耦合的迭代扩展卡尔曼将LiDAR特征点与IMU数据融合，后端使用因子图来接收关键帧位姿状态，将激光雷达帧间配准结果作为约束因子并与回环检测因子共同完成全局优化。
2）利用该算法在开源数据集M2DGR的3个不同场景和露天煤矿实地环境进行了试验测试，结果表明，算法在开源数据集的城市化环境下精度表现与当前的SOAT激光SLAM算法保持一致，在长达两千多米的露天煤矿实地环境下所提算法较FAST-LIO2、LIO-SAM紧耦合算法在定位精度上分别提高了46.00%和23.15%，提高了复杂环境下的定位精度与鲁棒性。
-
![]()
图 3 street_08序列各算法轨迹对比
Figure 3. Trajectory comparison of each algorithm for the street_08 sequence
表 1 开源数据集APE误差
Table 1 Open source dataset APE error
数据集 RMSE/m A-LOAM LeGO-LOAM FAST-LIO2 LIO-SAM OURS door_02 0.1861 2.0724 0.2801 0.1849 0.1855 gate_02 0.3305 0.3171 0.3204 0.3264 0.3167 street_08 3.3376 1.1327 0.1982 0.1708 0.1179 
下载: 导出CSV
表 2 露天煤矿数据集APE误差
Table 2 open pit coal mine dataset APE error
数据集 RMSE/m A-LOAM LeGO-LOAM FAST-LIO2 LIO-SAM OURS 序列1-500 m 1.9001 12.7723 12.8621 2.7005 0.8394 序列1- 1000 m7.3956 12.7723 20.6426 3.3946 1.4519 序列1- 2000 m10.1301 31.3826 24.5531 4.1940 3.1644 序列2- 2120 m6.9211 51.4630 10.1340 7.1204 5.4720
下载: 导出CSV
-
[1] 王国法,任世华,庞义辉,等. 煤炭工业“十三五”发展成效与 “双碳”目标实施路径[J]. 煤炭科学技术,2021,49(9):1−8. WANG Guofa,REN Shihua,PANG Yihui,et al. Development achievements of China’s coal industry during the 13th Five-Year Plan period and implementation path of “dual carbon” target[J]. Coal Science and Technology,2021,49(9):1−8.
[2] 王 猛,马如英,代旭光,等. 煤矿区碳排放的确认和低碳绿色发展途径研究[J]. 煤田地质与勘探,2021,49(5):63−69. WANG Meng,MA Ruying,DAI Xuguang,et al. Confirmation of carbon emissions in coal mining areas and research on low–carbon green development path[J]. Coal Geology & Exploration,2021,49(5):63−69.
[3] 王国法. 煤矿智能化最新技术进展与问题探讨[J]. 煤炭科学技术,2022,50(1):1−27. WANG Guofa. New technological progress of coal mine intelligence and its problems[J]. Coal Sci. Technol,2022,50(1):1−27.
[4] 赵 浩. 露天煤矿高质量安全发展形势分析与对策措施[J]. 煤矿安全,2022,53(7):251−256. ZHAO Hao. Analysis of development situation and countermeasures of high quality safety in open -pit coal mines[J]. Safety in Coal Mines,2022,53(7):251−256.
[5] 葛世荣,胡而已,李允旺. 煤矿机器人技术新进展及新方向[J]. 煤炭学报,2023,48(1):54−73. GE Shirong,HU Eryi,LI Yunwang. New progress and direction of robot technology in coal mine[J]. Journal of China Coal Society,2023,48(1):54−73.
[6] XUE G,LI R,LIU S,et al. Research on Underground Coal Mine Map Construction Method Based on LeGO-LOAM Improved Algorithm[J]. Energies,2022,15(17):6256. doi: 10.3390/en15176256
[7] 于海旭,杜志勇,魏志丹,等. 我国矿区无人驾驶技术现状与发展趋势分析[J]. 工矿自动化,2022,48(S2):82−87. YU Haixu,DU Zhiyong,WEI Zhidan,et al. Analysis on the current situation and development trend ofunmanned driving technology in mining areas in China[J]. Journal of Minc Automation,2022,48(S2):82−87.
[8] REN Z,WANG L,BI L. Robust GICP-based 3D LiDAR SLAM for underground mining environment[J]. Sensors,2019,19(13):2915. doi: 10.3390/s19132915
[9] SEGAL A,HAEHNEL D,THRUN S. Generalized-icp[C]//Robotics:science and systems. 2009,2(4):435.
[10] XU X,ZHANG L,YANG J,et al. A review of multi-sensor fusion slam systems based on 3D LIDAR[J]. Remote Sensing,2022,14(12):2835. doi: 10.3390/rs14122835
[11] 薛光辉,李瑞雪,张钲昊,等. 基于3D激光雷达的SLAM算法研究现状与发展趋势[J]. 信息与控制,2023,52(1):18−36. XUE Guanghui,LI Ruixue,ZHANG Zhenghao, et al. State-of-the-art and Tendency of SLAM AlgorithmsBased on 3D LiDAR[J]. Information and Control. 2023,52(1):18−36.
[12] ZHANG J,SINGH S. LOAM:Lidar odometry and mapping in real-time[C]//Robotics:Science and Systems. 2014,2(9):1−9.
[13] SHAN T,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). IEEE,2018:4758−4765.
[14] XUE G,WEI J,LI R,et al. LeGO-LOAM-SC:An Improved Simultaneous Localization and Mapping Method Fusing LeGO-LOAM and Scan Context for Underground Coalmine[J]. Sensors,2022,22(2):520. doi: 10.3390/s22020520
[15] YE H,CHEN Y,LIU M. Tightly coupled 3d lidar inertial odometry and mapping[C]//2019 International Conference on Robotics and Automation (ICRA). IEEE,2019:3144−3150.
[16] QIN T,LI P,SHEN S. Vins-mono:A robust and versatile monocular visual-inertial state estimator[J]. IEEE Transactions on Robotics,2018,34(4):1004−1020. doi: 10.1109/TRO.2018.2853729
[17] SHAN T,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). IEEE,2020:5135−5142.
[18] QIN C,YE H,PRANATA C E, et al. Lins:A lidar-inertial state estimator for robust and efficient navigation[C]//2020 IEEE international conference on robotics and automation (ICRA). IEEE,2020:8899−8906.
[19] YANG X,LIN X,YAO W,et al. A Robust LiDAR SLAM Method for Underground Coal Mine Robot with Degenerated Scene Compensation[J]. Remote Sensing,2022,15(1):186. doi: 10.3390/rs15010186
[20] XU W,ZHANG F. Fast-lio:A fast,robust lidar-inertial odometry package by tightly-coupled iterated kalman filter[J]. IEEE Robotics and Automation Letters,2021,6(2):3317−3324. doi: 10.1109/LRA.2021.3064227
[21] XU W,CAI Y,HE D,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
[22] CAI Y,XU W,ZHANG F. ikd-tree:An incremental kd tree for robotic applications[J]. arXiv preprint arXiv:2102.10808.
[23] HERTZBERG C,WAGNER R,FRESE U,et al. Integrating generic sensor fusion algorithms with sound state representations through encapsulation of manifolds[J]. Information Fusion,2013,14(1):57−77. doi: 10.1016/j.inffus.2011.08.003
[24] XU W,HE D,CAI Y,et al. Robots’ state estimation and observability analysis based on statistical motion models[J]. IEEE Transactions on Control Systems Technology,2022,30( 5):2030−2045. doi: 10.1109/TCST.2021.3133080
[25] 薛光辉,李瑞雪,张钲昊,等. 基于激光雷达的煤矿井底车场地图融合构建方法研究[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.
[26] 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,2021,7(2):2266−2273.
-
期刊类型引用(8)
1. 黄高翔，许国庆，姚强，杨旭，陈星艮，李洪涛. 小断面隧洞聚能水压光面爆破试验. 爆破. 2025(01): 89-96 . 
百度学术
2. 田家璇，张昌锁，李泽，廖霖. 新型聚能管设计及工程应用. 爆破. 2025(01): 159-165+198 . 百度学术
3. 王永法. 防隔水煤柱切顶卸压定向爆破参数设计及工程应用. 煤炭工程. 2024(02): 1-9 . 百度学术
4. 叶斯波拉提·叶买. 隧道工程聚能爆破围岩裂纹演化机理研究. 工程机械与维修. 2024(06): 158-160 . 百度学术
5. 杨帅，刘泽功，常帅，张健玉，傅师贵，乔国栋，张鑫. 地应力作用下聚能爆破煤体损伤特征试验研究. 采矿与安全工程学报. 2024(05): 1078-1090 . 百度学术
6. 李旭东. 高地应力隧道聚能爆破裂纹扩展规律研究. 铁道建筑技术. 2024(10): 27-31 . 百度学术
7. 朱凯，张学民，龙立敦，武朝光，周贤舜，万正，伍福寿. C型开口聚能管爆破机理与隧道光爆优化效果研究. 中南大学学报(自然科学版). 2024(10): 3849-3863 . 百度学术
8. 柴亚博，罗宁，张浩浩，李鹏龙，周嘉楠，廖禹成. 甲烷燃爆载荷下页岩裂缝网络发育特征与定量分析. 煤炭学报. 2023(12): 4308-4321 . 百度学术
其他类型引用(6)
计量
- 文章访问数: 134
- HTML全文浏览量: 36
- PDF下载量: 47
- 被引次数: 14
