3D LiDAR motion distortion algorithm for open-pit coal mine
-
摘要:
近年来，随着我国煤矿业的快速发展，智能化技术的运用越来越广泛。其中，露天煤矿环境的精确定位导航技术研发显得尤为重要。同步定位和地图构建(Simultaneous Localization and Mapping，SLAM)作为无人驾驶的关键技术，在露天煤矿中的应用面临诸多挑战。由于露天煤矿道路周围环境特征点较少，且环境退化严重，SLAM技术需要根据稀疏的特征点进行定位和地图构建，难度较大。此外，由于斜坡和道路不平，传感器易产生抖动，导致机器人运行时的运动畸变问题。针对这些问题，文中提出了一种新的解决方案。首先，对传感器外部参数进行重新标定，采用惯导和激光雷达融合的方式，以增强数据的一致性和准确性。在此基础上，采用全特征点匹配方式，直接对激光雷达采集的数据进行点云降采样提取。通过在算法前端对预处理后的激光点云数据添加迭代最近点(Iterative Closest Point，ICP)匹配提取出关键帧点云X，再结合惯导数据对点云信息进行畸变校正形成点云P，再次通过迭代最近点配准X和P。此外，后端采用因子图加入了回环检测提高约束的方法，进一步提高算法在露天煤矿环境下的定位精度和建图效果。试验结果表明，文中所提算法具有较高的定位精度和完整的建图效果，未产生明显的畸变。侧壁纹理清晰，具有一定的鲁棒性，有效提高了在露天煤矿环境下的鲁棒性和精度。
Abstract:In recent years, the coal mining industry in China has been experiencing rapid growth, resulting in an increasing adoption of intelligent technologies. Among these advancements, precise positioning and navigation technology for open-pit coal mining environments have become crucial. However, Simultaneous Localization and Mapping (SLAM), a key technology for unmanned driving, is currently facing significant challenges in open-pit coal mines. Limited environmental feature points and environmental degradation have necessitated SLAM to rely solely on sparse feature points for localization and mapping, thus increasing its complexity. Furthermore, sensor jitter caused by slopes and uneven roads can lead to motion distortion during robot operation. To address these challenges, a novel solution is proposed in this paper. Firstly, the external parameters of the sensors are being recalibrated. Secondly, the integration of inertial guidance and LiDAR is being utilized to improve data consistency and accuracy. This approach aims to enhance the performance of SLAM in open-pit coal mines, improving localization accuracy and mapping effectiveness. Building upon this foundation, our approach is leveraging full feature point matching to directly down sample and extract the point cloud from the LiDAR data. To enrich the preprocessed laser point cloud data, Iterative Closest Point (ICP) matching is being incorporated at the front-end of the algorithm, facilitating the extraction of the key-frame point cloud X. Subsequently, this data is being integrated with inertial guidance information to correct aberrations in the point cloud, leading to the formation of the refined point cloud P. ICP matching is once again being employed to align X and P. Furthermore, a factor graph is being incorporated into our back-end to enhance loopback detection, strengthening constraints and further improving localization accuracy and mapping effectiveness in open-pit coal mine environments. Experimental results demonstrate the high localization precision and undistorted map building capabilities of our proposed algorithm. Notably, the sidewall texture remains clear, exhibiting a certain degree of robustness, effectively enhancing both robustness and accuracy in open-pit coal mine settings.
-
0. 引 言
智慧露天煤矿的建设是我国煤矿发展的新目标[1]，中国安全生产科学研究院的张瑞新等[2]提出了智慧露天煤矿建设的总体框架为“1个基础平台+4个业务系统”的模式，其中1个基础平台是指露天煤矿基础支撑智慧化系统，平台的建设需要对露天矿地形地貌和运行设备进行统筹管理和优化，平台对露天煤矿的地形地貌运行设备的定位精度提出了高要求。针对此类需求，同时定位和对周围环境的地图的构建技术发挥了特有的优势[3]。SLAM为指在机器人自主移动的同时，利用传感器从环境中获取信息，建立环境的地图，并确定机器人在地图中的位置。SLAM技术已经成为机器人领域中最受关注和研究的核心问题之一，它在包括自动驾驶、无人机、人工智能等方面都有广泛的应用，同时SLAM技术也是无人驾驶领域解决矿山未知环境定位问题的关键技术，在露天煤矿环境下矿车通过搭载多种传感器提取并组合位置环境信息，在移动的同时完成对环境地图的构建并不断地对自身的位姿进行修正。目前为了加强鲁棒性和定位建图的精度，主流的SLAM技术采用多传感器进行融合的方式，按照传感器的类型分为以视觉为主的视觉SLAM[4]和以激光雷达为主的激光雷达SLAM[5]两种。
由于露天煤矿环境下粉尘高、光照不稳定、纹理弱的特点，视觉很难提取稳定的特征点。相比之下在此环境下选取3D激光雷达(Light Detection and Ranging，LiDAR)具有一定优势。在露天煤矿下的SLAM技术有2种方案，一种是激光雷达与惯导(Inertial Measurement Unit，IMU)融合的SLAM，另一种是激光雷达与卫星定位中实时动态(Real-Time Kinematic，RTK)载波相位差分技术融合的SLAM技术[6]。RTK技术是一种“有源定位”，容易受到卫星和其他基站源的影响，无法从算法上克服，IMU为一种“无源定位”，可以摆脱源对其定位的影响[7-8]。在成本方面，IMU设备相较于RTK设备的价格更加便宜，RTK设备需要外露在车顶上方，在矿卡装载过程中容易出现煤块碰撞设备的情况，由此造成RTK设备损坏率和后期维护成本较高。因此笔者在露天煤矿SLAM技术研究中采用激光雷达和IMU融合的方式。
对于激光雷达和IMU融合的SLAM技术已经有许多开源的SLAM解决方案。2014年，ZHANG J等[9]首先提出激光雷达测程和测绘LOAM方案，在激光雷达的连续扫描中改变了特征提取的方式，分为了角点和面点，迭代估计自我运动并发布建图结果，再通过点云匹配对LiDAR位姿进行优化。2016年，秦彤发布了A–LOAM开源方案，算法中采用了Google开发的非线性优化库Ceres Solver进行残差计算，优化了LOAM的整体代码结构。
多源传感器融合开源方案主要可以分为松耦合和紧耦合里程计2种。松耦合方式指激光雷达和IMU在整个系统中作为独立的子系统进行操作，将传感器的计算结果进行融合估计。SHAN等[10]提出了LeGO–LOAM模型，此方案相比于LOAM对地面环境假设为平坦的地面环境，从每次的LiDAR扫描中提取了地面特征和非地面特征，使特征提取方式更轻量级，提高了特征点的提取效率。紧耦合方式是指将激光雷达和IMU的原始观测数据先融合后进行联合处理，考虑它们之间相互影响的内在约束关系。SHAN等[11]提出了LIO–SAM算法框架，该算法通过平滑映射窗口，依赖特征匹配提供里程计因子，该因子同时结合IMU预积分来实现紧密耦合激光雷达惯性里程计，利用IMU预积分消除了激光点云的倾斜，后端利用因子图对不同传感器的数据进行了融合，实现了移动机器人轨迹估计和地图构建。王铉彬等[12]提出的紧耦合多传感器的三维激光SLAM方法，实现了高精度的全局位姿图优化。马宝良和张清宇等[13-14]提出一种露天煤矿环境下的LiDAR和IMU的紧耦合算法，前端通过迭代扩展卡尔曼滤波器的方式将2者数据融合，利用后向传播矫正激光雷达运动畸变，在后端加入雷达帧间配准因子图完成全局优化，相较于LIO–SAM算法在精度上提高了23.15%。
由于矿山环境复杂多变[15]，除机器人采集激光点数据时由于激光雷达扫描频率较低所产生的固有差值所造成的畸变和采集设备不合理得标定而造成的漂移外，地面颠簸、螺旋式地形、表面倾斜等复杂地形也会产生运动畸变，同时相较于城市道路，露天煤矿几何特征较简单，采用提取特征的方式容易出现环境退化的问题，严重影响建图效果和定位精度。REN等[16]关于矿山环境提出一种GICP方法，通过优化激光雷达的约束条件，实现了点云的连续帧和关键帧之间的注册关系。薛光辉等[17]提出了适用于煤矿的LeGO–LOAM–SC方法，利用ICP将约束添加到图优化中，弥补了煤矿下点云地图漂移的问题。杨琳[18]在构造IMU预积分误差函数及点云配准误差函数的基础上，利用高斯牛顿解优化激光雷达测程帧之间的约束关系，更好地估计移动机器人的位置和姿态。
矿山环境相似路段多，仅依靠激光雷达采集的数据对特征进行分类约束并不占优势，同时多斜面使得角点和面点有时无法匹配到新的特征点[19]。另一方面出于安全考虑，各种车辆在露天煤矿环境下的行驶速度在30～40 km/h间，但是长时间的运行会造成IMU的累计误差和漂移增大。慢速也会使得要处理的每帧数据增多，先进行特征分类匹配不利于及时响应。针对这些问题，笔者首先对设备进行了适配露天煤矿环境的参数标定，然后使用全特征匹配，前端通过IMU数据对LiDAR数据进行畸变矫正融合，在帧间匹配中2次加入迭代最近点进行直接配准，第1次是在3D激光雷达原始数据初次矫正畸变后，第2次是3D激光雷达降采样后形成新的关键帧。后端利用因子图和回环检测优化建图效果，提高了建图的鲁棒性及定位精度。
1. 系统框架
1.1 三维激光雷达运动畸变的产生
一般激光雷达驱动封装一帧数据时，默认一帧激光数据的所有激光点是在同一个时刻和同一个位姿下采集的。机器人在静止状态下，每个激光点都处在不同的基准位姿；在移动过程中，不同时刻发出的激光点位置不同，当三维激光雷达扫描频率(5～10 Hz)较低时，机器人运动带来的点云误差是不可忽略的，就会产生运动畸变，而且露天煤矿的颠簸路况使得运动畸变更加严重[20]。
针对激光雷达运动畸变问题，对各传感器进行参数标定，通过在算法的前端对预处理后的激光点云数据添加ICP点云匹配算法，利用IMU对点云信息进行畸变校正，有效减少由于机器人运动所产生的运动畸变，降低建图抖动失真问题，能够在矿山复杂环境实现更加清晰准确的建图和定位效果。
1.2 SLAM系统框架
本文算法的整体框架如图1所示，该系统从LiDAR和IMU传感器接收数据，对LiDAR原始数据采用体素滤波器对点云数据进行降采样，利用滑动窗口方法来创建一个最近LiDAR扫描的点云图，再分3次处理雷达降采样后的数据，其中两次应用IMU原始数据。第1次采用LiDAR数据结合IMU数据偏移纠正进行畸变矫正形成点云P，同时对IMU进行预积分形成IMU因子。第2次对LiDAR降采样数据进行ICP配准，形成关键帧点云X，然后将P和X进行ICP配准，形成LiDAR因子。通过这2次的ICP配准，加强了对环境点云的约束。第3次结合LiDAR和IMU数据形成里程计因子。将LiDAR因子、IMU因子和里程计因子融入到因子图中实现状态更新，在后端采用Scan Context技术[17]加入回环检测，可有效纠正累计漂移完成建图。
2. 迭代最近点算法
经典的激光SLAM常使用基于点的配准方法，ICP类算法进行帧间点云配准，主要算法包括ICP[21]、GICP[22]、PL–ICP[23]等。
在针对露天煤矿退化的特征环境，为了充分发挥ICP的优势，提高匹配精度，摈弃了特征的划分，采用了2次匹配，先进行一次粗配准，再进行一次细配准，控制迭代次数，在降低计算量的同时提高计算精度。将激光惯性里程计因子与ICP点云匹配方法结合，可以最大化的减少激光雷达运动畸变[24]。具体做法为
1)根据初始位姿，提取最近的一帧激光点云对应IMU里程计位姿，同时提取当前时刻IMU位姿；
2)计算这段时间里IMU增量位姿变换，包括旋转增量和平移增量；
3)当前时刻IMU的位姿=最近的一帧激光点云位姿×IMU增量位姿变换，当前时刻激光点与第1个激光点的位姿变换,使每一时刻激光点位置变换到第1个激光点坐标系下，进行运动补偿；
4)保存并发布当前帧激光运动畸变校正之后的点云信息，包括点云数据、初始位姿、姿态角、有效点云数据；
5)添加ICP点云匹配算法处理校正后的点云信息。
ICP算法的具体方案是给定2个点云集合$X$和$P$，$X$为参考帧的点云；$P$为当前帧的点云：
$$ \left\{ {\begin{array}{*{20}{l}} {X = \left\{ {{x_1},{x_2}, \cdots ,{x_{{N_x}}}} \right\}} \\ {P = \left\{ {{p_1},{p_2}, \cdots ,{p_{{N_p}}}} \right\}} \end{array}} \right. $$ (1) 其中，${x_i}$和${p_i}$为点云的坐标；${N_x}$和${N_p}$为点云的数量。所求的条件矩阵如式(2)所示。
$$ E\left( {{\boldsymbol{R,t}}} \right) = \frac{1}{{{N_p}}}\sum\limits_{i = 1}^{{N_p}} {\left\| {{x_i} - {{\boldsymbol{R}}_{pi}} - {\boldsymbol{t}}} \right\|} $$ (2) 为了得到旋转矩阵${\boldsymbol{R}}$和平移向量${\boldsymbol{t}}$，使$E\left( {{\boldsymbol{R,t}}} \right)$最小，计算过程如式(3)和式(4)所示。
$$ {U_x} = \frac{1}{{{N_x}}}\sum\limits_{i = 1}^{{N_x}} {{x_i}} $$ (3) $$ {U_p} = \frac{1}{{{N_p}}}\sum\limits_{i = 1}^{{N_p}} {{p_i}} $$ (4) 其中，${U_x}$和${U_p}$分别为点云集合$X$和$P$的几何中心。去中心化为
$$ {X_i} = {x_i} - {U_x} $$ (5) $$ {P_i} = {p_i} - {U_p} $$ (6) 要使$E\left( {{\boldsymbol{R,t}}} \right)$取最小值，${\boldsymbol{t}} = {U_x} - {\boldsymbol{R}} {U_p}$，则：
$$ E\left( {{\boldsymbol{R,t}}} \right) = \frac{1}{{{N_p}}}\sum\limits_{i = 1}^{{X_p}} {\left\{ {x_i^{'{\mathrm{T}}}x_i^{'} + p_i^{'{\mathrm{T}}}p_i^{'}} \right\} + \frac{1}{{{N_p}}}\sum\limits_{i = 1}^{{N_p}} {\left\{ { - 2x_i^{'{\mathrm{T}}}Rp_i^{'}} \right\}} } $$ (7) 要使$E\left( {{\boldsymbol{R,t}}} \right)$取最小值，$\dfrac{1}{{{N_p}}}\displaystyle\sum_{i = 1}^{{N_p}} {\left\{ {x_i^{'{\mathrm{T}}}{\boldsymbol{R}}p_i^{'}} \right\}} $应取最大值，则：
$$ \frac{1}{{{N_p}}}\sum\limits_{i = 1}^{{N_p}} {x_i^{'{\mathrm{T}}}{\boldsymbol{R}}p_i^{'}} = \frac{1}{{{N_p}}}{t_r}\left( {\sum\limits_{i = 1}^{{N_p}} {x_i^{'{\mathrm{T}}}{\boldsymbol{R}}p_i^{'}x_i^{'{\mathrm{T}}}} } \right) = \frac{1}{{{N_p}}}{t_r}\left( {{\boldsymbol{R}}H} \right) $$ (8) 令$H =\displaystyle \sum_{i = 1}^{{N_p}} {p_i^{'}x_i^{'{\mathrm{T}}} = U\sum {{V^{\mathrm{T}}}} } $，当${\boldsymbol{R }}= V{U^{\mathrm{T}}}$时，${t_r}\left( {{\boldsymbol{R}}H} \right)$取得最大值，则$E\left( {{\boldsymbol{R,t}}} \right)$取得最小值。
3. 激光惯性里程计
针对三维激光雷达所产生的运动畸变，在运动非连续性假设下，通过扫描关键帧和样条分割来提高点云匹配精度，然后添加样条约束，基于改进ICP算法进行帧与地图匹配，有效抑制运动轨迹的漂移[25]。原理是在处理器上提取出激光雷达数据和里程计积分数据，处理两者之间的时间同步问题和进行位姿插值计算。通过已知数据：
1)当前帧激光雷达数据的开始和结束时间${t_{\mathrm{s}}}$，${t_{\mathrm{e}}}$。
2) 2个激光束间的间隔时间$\Delta t$。
3)里程计的数据按照时间顺序存储在一个队列中，队首时间最早。
4)最早的里程计数据的时间>${t_{\mathrm{s}}}$。
5)最晚的里程计数据的时间<${t_{\mathrm{e}}}$。
求解以下过程包括：
1)当前激光雷达数据帧中每一个激光点对应的激光雷达坐标系的机器人位姿，即求解$\left\{ {{t_{\mathrm{s}}},{t_{{\mathrm{s}} + \Delta t}},\cdots,{t_{\mathrm{e}}}} \right\}$时刻的机器人位姿。
2)根据求解的位姿把每一个激光点都转化到同一个坐标系下，需要2次转化。
3)重新封装一帧激光数据，并发布出去。
针对三维激光雷达运动所产生的运动畸变，本文通过激光惯性里程计，利用当前激光帧的起止时刻间的IMU数据计算旋转增量，IMU里程计数据计算平移增量，进而对该帧激光每一时刻的激光点进行运动畸变校正，利用相对于激光帧起始时刻的位姿增量，变换当前激光点到起始时刻激光点的坐标系下，实现校正。同时用IMU数据的姿态角作IMU里程计数据的位姿，对当前帧的激光位姿进行粗略初始化。
具体方法是以当前低频激光里程计为基准，找到与它最近的一个IMU里程计和时间最新的一个IMU里程计，用这2个IMU里程计求出位姿的变化量，再作用到LiDAR位姿上，得到一个高频的里程计。其中激光里程计因子如式（9）所示。
$$ M_i^p{\mathbf{ = }}'F_i^pU'F_{i - 1}^pU\cdots U'F_{i - n}^p $$ (9) 而图优化因子发布2个里程计信息，一个生成此处高频的激光里程计，作为激光里程计的输出。另一个里程计信息与高频的IMU数据进行预积分加上图优化生成高频的IMU里程计，也就是IMU位姿的输出，其中IMU预积分因子为推导如下。
IMU从时刻$t$积分到$t + \Delta t$的公式为
$$ {{\boldsymbol{V}}_{t + \Delta t}} = {{\boldsymbol{V}}_t} + g\Delta t + {R_t}\left( {\mathop {{a_t}}\limits^ \wedge - b_t^a - n_t^a} \right)\Delta t $$ (10) $$ {{\boldsymbol{P}}_{t + \Delta t}} = {{\boldsymbol{P}}_t} + {{\boldsymbol{V}}_t}\Delta t + \frac{1}{2}g\Delta {t^2} + \frac{1}{2}{R_t}\left( {\mathop {{a_t}}\limits^ \wedge - b_t^a - n_t^a} \right)\Delta {t^2} $$ (11) $$ {R_{t + \Delta t}} = {R_t}\exp \left[ {\left( {\mathop {{\omega _t}}\limits^ \wedge - b_t^\omega - n_t^\omega } \right)\Delta t} \right] $$ (12) 其中，${{\boldsymbol{V}}_{t + \Delta t}}$为后一时刻的速度；${{\boldsymbol{P}}_{t + \Delta t}}$为后一时刻位置；${R_{t + \Delta t}}$为后一时刻旋转。速度位置和旋转的预积分测量值为
$$ \Delta {{\boldsymbol{V}}_{ij}} = R_i^{\mathrm{T}}\left( {{{\boldsymbol{V}}_j} - {{\boldsymbol{V}}_i} - g\Delta {t_{ij}}} \right) $$ (13) $$ \Delta {p_{ij}} = R_i^{\mathrm{T}}\left( {{p_j} - {p_i} - {{\boldsymbol{V}}_i}\Delta {t_{ij}} - \frac{1}{2}g\Delta t_{ij}^2} \right) $$ (14) $$ \Delta {R_{ij}} = R_i^{\mathrm{T}}{R_j} $$ (15) 4. 试验分析
4.1 试验设备
本文算法验证分别采用上海交通大学提供的开源数据集M2DGR[26]中的户外street和gate系列数据集和哈尔乌素煤矿采集的真实露天煤矿数据集。M2DGR采集设备如图2a所示，该设备搭载了Velodyne-VLP–32C 3D激光雷达和HandsfreeA9九轴IMU机器人。
哈尔乌素露天煤矿实测数据集使用的数据采集设备如图2b所示，该设备搭载速腾聚创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，Ubuntu18.04系统，编程语言为C++。
为了解决由于机器人运动产生的激光雷达运动畸变和建图的抖动失真问题，采用Lidar_align的方法[27]进行激光雷达与惯导的外参标定，通过多次迭代选取收敛结果最小的参数。IMU的内参未标定会造成建图的各种漂移现象，严重影响定位精度，甚至导致无法建图。本文采用IMU_utils方法[28]进行IMU内参标定，使用Allan方差标定随机游走噪声和高斯白噪声。
4.2 开源数据集试验
考虑到运动畸变的产生原因，为了验证运动畸变去除效果，选取巡检机器人行驶至拐弯处抖动较为剧烈的一段数据。本文算法实现过程中前端摒弃了特征分类提取，设置体素滤波器尺寸统一为0.4 m的立方体，进行降采样。为了提高效率和精度，加入了2次ICP点云配准，分成粗配准和细配准2个阶段，粗配准可快速找到2个点云之间的对应关系，为细配准提供一个良好的初始位置。本文根据实际需求在粗配准上的迭代次数为60，收敛无效最小阈值为10–6，拟合度阈值为10–6。在粗配准的基础上，设置精配准的迭代次数为80，收敛无效最小阈值为10–6，拟合度阈值为10–6，进一步精确2个点云之间的对应关系。回环检测部分加入了Scan Context技术，设置体素滤波器为0.5 m的立方体。
对添加和未添加ICP算法进行建图，结果如图3所示。未添加ICP算法处理过的点云建图会产生模糊和点/线的缺失，图3a中放大部分楼梯台阶和柱子可以明显看出线条缺失模糊，不能够显示出周围环境具体特征，添加ICP点云匹配算法后的建图效果如图3b所示，具有更加清晰的轮廓和建图细节。
由表1可知，在本文算法中未添加ICP点云匹配算法的均方根误差(Root Mean Square Error，ERMS)为3.260 m，添加ICP点云匹配算法后的ERMS为1.121 m，这是由于添加ICP点云匹配算法后，在迭代过程中减少了运动畸变所产生的定位误差，因此本文算法在开源数据集上获得了更高的定位精度。
表 1 添加和未添加ICP算法Table 1. With and without the ICP algorithmICP算法 ERMS/m 未添加 3.260 添加 1.121 为了进一步验证本文所提算法对于激光雷达运动畸变去除的效果，本文对目前流行的SLAM算法A–LOAM、LeGO–LOAM、LIO–SAM和本文提出的算法(Ours)在开源数据集M2DGR中进行轨迹定位误差分析对比。分别选取M2DGR数据集中gate_03、street_05、street_04、street_02四个序列，对应的行进路程长度分别为248、420、840、1 484 m。验证分析使用绝对位姿误差(Absolute Position Error，EAP)中的ERMS对算法的有效性进行评估。
street_04序列的真值轨迹和4种算法的轨迹对比结果如图4所示。A–LOAM算法初始定位精准，拟合度较高，但是随着路程的增加，IMU漂移逐渐增加，鲁棒性较差。LeGO–LOAM、LIO–SAM和本文所提算法轨迹相差较小，这是由于开源数据集城市道路几何特征丰富、路程距离短，使得算法的误差累积小，造成轨迹对比效果不能由图4中明显区分。为了从数据上验证本文算法的效果，采用车载全球导航卫星系统RTK的真值和各个算法定位轨迹做了绝对误差箱线图分析，如图5所示。由图5可以明显看出，在4个数据序列上，本文所提算法的EAP均小于其他3个算法，从侧面验证了图4中本文算法的轨迹和真值拟合度最高。
![]()
图 4 street_04序列真值轨迹和各算法轨迹对比Figure 4. Trajectory comparison of each algorithm for the street_04 sequence试验中为了量化轨迹定位误差，本文选择EVO[29]对4种算法进行EAP分析，结果见表2。在gate_03序列短距离情况下，各算法EAP区别较小。随着距离增加，误差逐渐增大，在序列street_04序列时，其他3种算法误差均达到了米级，而本文算法的EAP为0.989 m。纵向对比其他3种算法，在每一个序列中本文算法误差均最小，表现出一定的鲁棒性。
表 2 开源数据EAPTable 2. EAP in open source datam 数据集 EAP 距离/m A–LOAM LeGO–LOAM LIO–SAM Ours gate_03 248 0.263 0.113 0.143 0.117 street_05 420 0.728 0.710 1.297 0.371 street_04 840 3.869 1.093 1.457 0.989 street_02 1 484 24.183 3.278 5.624 3.084 4.3 实测露天煤矿数据集试验
为了验证本文算法在实际露天煤矿环境下的应用效果，对内蒙古哈尔乌素露天煤矿数据进行实地采集，哈尔乌素露天煤矿环境和道路如图6所示。
进一步为了验证试验的科学性和有效性，将煤矿整体长2 000 m距离的数据从同一初始位置分别截取为500、1 000、1 700、2 000 m四个不同长度序列。图7展示了在2 000 m序列中，车载RTK真值轨迹和不同算法轨迹对比。通过图7可以看出，本文算法在露天煤矿环境下定位轨迹与真值轨迹的拟合度最高。另一方面图8的EAP箱线图可以看出本文算法在距离不同序列上，相较于其他3种算法的EAP最小，所提算法具有更高的精度和鲁棒性，更加适合露天矿的定位与建图。
![]()
图 7 2 000 m序列真值轨迹和算法轨迹对比Figure 7. Trajectory comparison of each algorithm for the 2 000 mEVO对4种算法进行EAP分析，对4个算法和4个序列共进行了16组试验，EAP结果见表3。A–LOAM算法依靠曲率划分特征进行配准，在500 m距离时EAP仅为0.663 m，优于其他3种算法，随着距离的增加，A–LOAM算法精度快速下降，因露天煤矿环境的退化，使得提取特征困难，存在对于低纹理区域等特定场景的处理能力不足。LeGO–LOAM算法由于斜面碎石造成车辆抖动时会对地面点提取不准确，从而影响算法整体性能，所以在露天煤矿500～2 000 m距离上EAP表现均较差。LIO–SAM算法由于在露天煤矿初始化过程中容易受到周围环境特征的影响，需要较长时间收敛到正确的状态，引起系统的不稳定，造成后续长距离的建图精度有所下降。本文算法在长距离露天煤矿道路EAP更小，在2 000 m距离时的EAP仅为4.527 m，定位轨迹更加精确，具有一定的鲁棒性。
表 3 露天煤矿EAPTable 3. EAP in open-pit coal minem 距离/m EAP A–LOAM LeGO–LOAM LIO–SAM Ours 500 0.663 4.213 1.641 1.000 1 000 2.274 7.382 2.559 1.184 1 700 17.539 39.363 3.650 1.309 2 000 57.204 69.166 6.205 4.527 对露天煤矿环境实现建图结果如图9所示，由图9可以看出A–LOAM算法对露天煤矿道路上边缘部分建图产生了缺失，LeGO–LOAM算法缺失更加严重，同时在道路拐弯处，地面部分也发生了畸变，LIO–SAM算法整体建图完整，对于露天煤矿侧壁细节较为模糊，纹理不够清晰，而本文算法对露天煤矿整体建图效果较为完整，侧壁纹理清晰，定位精度高，没有产生明显的畸变，具有一定的鲁棒性。
5. 结 论
本文对三维激光雷达运动畸变算法进行了研究分析，对激光点云摒弃了特征提取的方式，加入了ICP配准，提出一种针对露天煤矿环境下的SLAM算法。融合了三维激光雷达和6轴IMU传感器数据，有效解决露天煤矿因环境特征退化、路况崎岖颠簸产生的畸变问题。前端使用全点云特征配准方式，两次加入了ICP配准，使环境特征数据不因退化而缺失，结合IMU数据形成雷达IMU里程计。后端利用的因子图融合了激光雷达因子、IMU因子、里程计因子，同时加入回环检测完成全局优化，加强对点云的约束，实现在露天煤矿下具有一定鲁棒性的算法。
利用本文算法在M2DGR数据集和哈尔乌素露天煤矿进行试验测试，结果表明：
1）二次添加ICP可以有效提高定位精度，同时可优化建图中的运动畸变。
2）在开源城市环境下，本文算法定位精度略优于LIO–SAM算法，建图效果于LIO–SAM保持一致。
3）在哈尔乌素露天煤矿下，随着距离的增加，本文算法定位误差均最小，优于其他3种算法，而且露天煤矿建图效果完整，侧壁纹理清晰可辨，未出现明显的畸变现象，具有一定鲁棒性。在一定程度上解决SLAM算法在露天煤矿环境下的建图失真和抖动漂移问题。
-
![]()
图 4 street_04序列真值轨迹和各算法轨迹对比
Figure 4. Trajectory comparison of each algorithm for the street_04 sequence
![]()
图 7 2 000 m序列真值轨迹和算法轨迹对比
Figure 7. Trajectory comparison of each algorithm for the 2 000 m
表 1 添加和未添加ICP算法
Table 1 With and without the ICP algorithm
ICP算法 ERMS/m 未添加 3.260 添加 1.121 
下载: 导出CSV
表 2 开源数据EAP
Table 2 EAP in open source data
m 数据集 EAP 距离/m A–LOAM LeGO–LOAM LIO–SAM Ours gate_03 248 0.263 0.113 0.143 0.117 street_05 420 0.728 0.710 1.297 0.371 street_04 840 3.869 1.093 1.457 0.989 street_02 1 484 24.183 3.278 5.624 3.084
下载: 导出CSV
表 3 露天煤矿EAP
Table 3 EAP in open-pit coal mine
m 距离/m EAP A–LOAM LeGO–LOAM LIO–SAM Ours 500 0.663 4.213 1.641 1.000 1 000 2.274 7.382 2.559 1.184 1 700 17.539 39.363 3.650 1.309 2 000 57.204 69.166 6.205 4.527
下载: 导出CSV
-
[1] 李浩荡,佘长超,周永利,等. 我国露天煤矿开采技术综述及展望[J]. 煤炭科学技术,2019,47(10):24−35. LI Haodang,SHE Changchao,ZHOU Yongli,et al. Summary and prospect of open-pit coal mining technology in China[J]. Coal Science and Technology,2019,47(10):24−35.
[2] 张瑞新,毛善君,赵红泽,等. 智慧露天矿山建设基本框架及体系设计[J]. 煤炭科学技术,2019,47(10):1−23. ZHANG Ruixin,MAO Shanjun,ZHAO Hongze,et al. Framework and structure design of system construction for intelligent open-pit mine[J]. Coal Science and Technology,2019,47(10):1−23.
[3] 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−2861.
[4] CHOU C C,CHOU C F. Efficient and accurate tightly-coupled visual-lidar SLAM[J]. IEEE Transactions on Intelligent Transportation Systems,2022,23(9):14509−14523.
[5] 王金科,左星星,赵祥瑞,等. 多源融合SLAM的现状与挑战[J]. 中国图象图形学报,2022,27(2):368−389. WANG Jinke,ZUO Xingxing,ZHAO Xiangrui,et al. Review of multi-source fusion SLAM:Current status and challenges[J]. Journal of Image and Graphics,2022,27(2):368−389.
[6] LI W Y,LIU G,CUI X W,et al. Feature-aided RTK/LiDAR/INS integrated positioning system with parallel filters in the ambiguity-position-joint domain for urban environments[J]. Remote Sensing,2021,13(10):2013.
[7] LI X X,YU H,WANG X B,et al. FGO-GIL:Factor graph optimization-based GNSS RTK/INS/LiDAR tightly coupled integration for precise and continuous navigation[J]. IEEE Sensors Journal,2023,23(13):14534−14548.
[8] LI X X,WANG S W,LI S Y,et al. Enhancing RTK performance in urban environments by tightly integrating INS and LiDAR[J]. IEEE Transactions on Vehicular Technology,2023,72(8):9845−9856. doi: 10.1109/TVT.2023.3257874
[9] ZHANG J,SINGH S. Loam:Lidar odometry and mapping in real-time[C]// Robotics:Science and Systems,2014,2(9):1−9.
[10] 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.
[11] 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.
[12] 王铉彬,李星星,廖健驰,等. 基于图优化的紧耦合双目视觉/惯性/激光雷达SLAM方法[J]. 测绘学报,2022,51(8):1744−1756. WANG Xuanbin,LI Xingxing,LIAO Jianchi,et al. Tightly-coupled stereo visual-inertial-LiDAR SLAM based on graph optimization[J]. Acta Geodaetica et Cartographica Sinica,2022,51(8):1744−1756.
[13] 张清宇,崔丽珍,李敏超,等. 倾斜地面3D点云快速分割算法[J]. 无线电工程,2024,54(2):447−456. ZHANG Qingyu,CUI Lizhen,LI Minchao,et al. A fast segmentation algorithm for 3D point cloud on inclined ground[J]. Radio Engineering,2024,54(2):447−456.
[14] 马宝良,崔丽珍,李敏超,等. 露天煤矿环境下基于LiDAR/IMU的紧耦合SLAM算法研究[J]. 煤炭科学技术,2024,52(3):236−244. MA Baoliang,CUI Lizhen,LI Minchao,et al. Tightly coupled LiDAR-Inertial SLAM for open pit coal mine environment[J]. Coal Science and Technology,2024,52(3):236−244.
[15] 王忠鑫,辛凤阳,宋波,等. 论露天煤矿智能化建设总体设计[J]. 煤炭科学技术,2022,50(2):37−46. WANG Zhongxin,XIN Fengyang,SONG Bo,et al. Overall design of intelligent construction in open pit coal mines[J]. Coal Science and Technology,2022,50(2):37−46.
[16] 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
[17] XUE G H,WEI J B,LI R X,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
[18] YANG L,MA H W,NIE Z,et al. 3D LiDAR point cloud registration based on IMU preintegration in coal mine roadways[J]. Sensors,2023,23(7):3473.
[19] 邓鹏,罗静. 复杂环境下机器人多传感器融合定位方法[J]. 电子测量与仪器学报,2023,37(12):48−57. DENG Peng,LUO Jing. Robot multi-sensor fusion localization method in complex environment[J]. Journal of Electronic Measurement and Instrumentation,2023,37(12):48−57.
[20] 蒲文浩,刘锡祥,陈昊,等. 多传感器融合的激光雷达点云矫正与定位方法[J]. 激光与光电子学进展,2023,60(24):3788/LOP230762. PU Wenhao,LIU Xixiang,CHEN Hao,et al. LiDAR point cloud correction and location based on multisensor fusion[J]. Laser & Optoelectronics Progress,2023,60(24):3788/LOP230762.
[21] CENSI A. An ICP variant using a point-to-line metric[C]//2008 IEEE International Conference on Robotics and Automation. Piscataway,NJ:IEEE,2008:19−25.
[22] LI X Y,LI X,LI Z Y,et al. Construction of a 3D distribution network environment based on multi-source data fusion[J]. Journal of Physics:Conference Series,2023,2614(1):012011. doi: 10.1088/1742-6596/2614/1/012011
[23] WU H W,ZHU L C,CHENG J D,et al. Research on motion distortion optimization algorithm of laser SLAM[J]. Journal of Physics:Conference Series,2022,2330(1):012013. doi: 10.1088/1742-6596/2330/1/012013
[24] 王雪,李登峰,黄杉杉,等. 激光雷达运动畸变去除的算法设计[J]. 自动化仪表,2021,42(5):89−91. WANG Xue,LI Dengfeng,HUANG Shanshan,et al. Design of algorithm for removal of lidar motion distortion[J]. Process Automation Instrumentation,2021,42(5):89−91.
[25] 饶启鹏,凌铭,王鑫,等. 基于连续时间样条约束的改进激光里程计[J]. 激光与光电子学进展,2023,60(22):248−255. RAO Qipeng,LING Ming,WANG Xin,et al. Improved lidar odometry based on continuous-time spline constraints[J]. Laser & Optoelectronics Progress,2023,60(22):248−255.
[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,2022,7(2):2266−2273.
[27] Ethz-asl. Lidar_align:A simple method for finding the extrinsic calibration between a 3D lidar and a 6-dof pose sensor[EB/OL]. (2018−03−26) [2024−03−27]. https://github.com/ethz-asl/lidar_align.2019.
[28] GAO Wenliang. Imu_utils:A ROS package tool to analyze the IMU performance[EB/OL]. (2018−03−30)[2024−03−27]. https://github. com/gaowenliang/imu_utils. 2018.
[29] GRUPP M. Evo:Python package for the evaluation of odometry and SLAM[EB/OL]. (2017−07−25) [2024−03−27]. https://github.com/MichaelGrupp/evo.2017.
计量
- 文章访问数: 33
- HTML全文浏览量: 7
- PDF下载量: 31
