基于全局点云地图的煤矿井下无人机定位方法

高海跃 王凯 王保兵 王丹丹

摘要：即時定位与建图（SLAM）技术应用于煤矿井下无人机自主定位时，由于采用特征点构建地图，易出现退化问题，导致定位不准确，且因其以机体作为参考坐标系，无法实现全局定位。针对该问题，提出了一种基于全局点云地图的煤矿井下无人机定位方法。以 Fast?LIO2算法作为激光 SLAM算法，获得无人机位姿估计；采用迭代最近邻算法，对获取的激光雷达实时点云和全局点云地图进行两步匹配，实现无人机位姿校正；针对因点云数量过多导致点云匹配速度无法保证定位实时性的问题，设计了基于时间的位姿输出策略，提高了无人机位姿数据输出频率。在1000 m煤矿井下巷道中测试无人机定位方法的 SLAM精度和位姿校正效果，结果表明：在长距离巷道环境中，Fast?LIO2算法的定位累计误差小于1 m，在600 m 以上范围内小于0.3 m，明显小于 LOAM?Livox算法和 LIO?Livox算法；Fast?LIO2算法输出的位姿估计经校正算法校正后，飞行路径全部位于全局点云地图中，验证了位姿校正算法有效；单次 SLAM算法运行耗时14.83 ms，单次位姿校正耗时883 ms，位姿数据输出频率为10 Hz，满足无人机定位实时性要求。

关键词：无人机定位；即时定位与建图；激光雷达；惯性测量单元；全局点云地图；位姿校正中图分类号： TD67    文献标志码： A

Positioning method for underground unmanned aerial vehicles in coal mines based on global point cloud map

GAO Haiyue， WANG Kai， WANG Baobing， WANG Dandan

（CCTEG Beijing Tianma Intelligent Control Technology Co.， Ltd.， Beijing 101399， China）

Abstract： When simultaneous localization and mapping （SLAM） technology is applied to autonomous positioning of unmanned aerial vehicles in coal mines， the use of feature points to construct maps can easily lead to degradation issues， resulting in inaccurate positioning. Moreover， due to its use of the body as a reference coordinate system， global positioning cannot be achieved. In order to solve the problems， a positioning method for underground unmanned aerial vehicles （UAV） in coal mines based on global point cloud map is proposed. The method uses Fast-LIO2 algorithm as the lidar SLAM algorithm to obtain UAV position and attitude estimation. An iterative nearest-neighbor algorithm is used for two-step matching of the acquired real-time lidar point cloud and the global point cloud map to achieve UAV position and attitude correction. To address the issue of point cloud matching speed not ensuring real-time positioning due to the excessive number of point clouds， a time- based position and attitude output strategy is designed to increase the frequency of outputting UAV position and attitude data. The SLAM precision and position and attitude correction effect of the UAV positioning method are tested in a 1000 m underground coal mine roadway. The results show that in long-distance roadway environments， the cumulative positioning error of the Fast-LIO2 algorithm is less than 1 m， and is less than 0.3 min the range of 600 m or more， which is significantly smaller than the cumulative positioning errors of LOAM- Livox algorithm and LIO-Livox algorithm. The position and attitude estimation output by the Fast-LIO2 algorithm has been corrected by the correction algorithm， and all flight paths are located in the global point cloud map， verifying the effectiveness of the position and attitude correction algorithm. The time consumption of single SLAM algorithm operation is 14.83 ms， the one of single position and attitude correction is 883 ms， and the output frequency of position and attitude data is 10 Hz， meeting the real-time requirements of UAV positioning.

Key words： UAV positioning; simultaneous localization and mapping; lidar; inertial measurement unit; global point cloud map; position and attitude correction

0 引言

近年来，无人机在煤矿灾后救援[1-2]、井下探测[3]、设备巡检[4]等领域得到了研究和应用。精确定位是无人机开展各项应用的重要前提。目前民用无人机定位和导航依赖全球导航卫星系统（Global Navigation Satellite System，GNSS）实现。而煤矿井下无法接收到 GNSS 信号，这对无人机在井下的应用造成了很大困难。近年来，随着即时定位与建图（Simultaneous Localization and Mapping，SLAM）技术的发展，无人机已经能在限定条件的无 GNSS 环境中实现自动巡航飞行，如文献[5-7]将 SLAM 技术应用于煤矿井下环境中的无人机定位，其中文献[7]采用激光雷达实时建图（Lidar Odometry and Mapping in Real-time，LOAM）算法[8-9]，在精确定位基础上实现了无人机自主避障。为提高 SLAM 算法性能，文献[10]采用回环检测方法优化了 SLAM建图效果；文献[11]引入基于关键帧的滑动窗口及因子图，提高了 SLAM定位精度；文献[12]采用卡尔曼滤波器实现惯性测量单元（Inertial Measurement Unit， IMU）数据和激光点云数据的紧耦合，进一步提高了 SLAM 定位精度。上述研究可在一定程度上提升基于 SLAM 算法的煤矿井下移动平台自主定位效果[13-16]。但井下巷道是一类典型的缺乏结构约束的环境，基于特征点云匹配的方法易出现退化问题，导致定位不准确。另外，SLAM 算法以机体作为参考坐标系，无法得到全局坐标信息，但实际应用中需要全局坐标信息，用于提供不同架次飞行中获取的具有一致坐标信息的任务数据。鉴此，本文提出一种煤矿井下无人机定位方法，结合激光 SLAM 和基于全局点云地图的位姿校正，实现了无人机在井下巷道较长范围内的稳定定位。

1 井下无人机定位方法架构

煤矿井下无人机定位方法主要包括前端激光 SLAM 算法和后端基于点云地图的位姿校正算法，如图1所示。 SLAM 处理激光雷达点云数据和 IMU 输出的角速度、加速度数据，通过激光 SLAM 算法得到里程计信息，即无人机当前位姿估计。基于点云地图的位姿校正算法使用点云地图确定井下全局坐标系，对无人机的位姿估计误差进行校正，并基于当前点云帧和点云地图计算无人机的全局位姿矩阵。

2 激光 SLAM 算法

以 LOAM 为代表的传统激光 SLAM 算法常在原始点云中提取边缘特征点和平面特征点，且使用特征点构建地图，提高了运算效率，但在非结构化场景中定位精度较差。 Fast?LIO2算法[17-18]采用 IMU预积分方法在点云帧采集时间内进行状态传播，使用传播后的状态校正点云失真，进而使用点云构造残差，通过迭代扩展卡尔曼滤波器（Iterated Extended Kalman Filter，IEKF）将激光雷达特征点云和 IMU 预积分状态进行融合。该算法使用原始点云直接计算局部地图，因此在非结构化或复杂场景中依然保持较高的定位精度；采用新的卡尔曼增益计算公式进行滤波，使计算量依赖于状态量的维度而不是观测量的维度，在保证计算精度的同时，降低了 IEKF 的运算复杂度；在近邻点搜索及地图构建过程中使用ikd?tree 算法，进一步降低了算法运行时间。因此，采用 Fast?LIO2算法作为激光 SLAM 算法，计算井下无人机位姿估计，流程如图2所示。需要说明的是，本节提到的点云地图是激光 SLAM 算法维护的点云地图，与本文使用的全局点云地图不同。

3 基于点云地图的位姿校正算法

3.1 两步匹配算法

为了解决 SLAM 应用于长廊形巷道易导致无人机位姿漂移的问题，提出了两步匹配算法，依靠激光扫描点云和全局点云地图进行匹配，进而实现无人机位姿校正，具体流程如图3所示。

1）采用离线方式扫描并建立巷道全局点云地图Pmap，其包含无人机的全部作业场景。设 SLAM 坐标系为 G，全局点云地图坐标系为M，激光雷达坐标系为 L，当前时刻tk扫描的激光点云为Pk（L）。将激光点云Pk（L）和全局点云地图Pmap进行第1次体素降采样，以降低点云数量和密度，提高后续匹配速度。具体过程：将原始点云空间按边长d1分割为小正方体（即体素），对每个体素中的点做平均，得到均值点，最终只保留均值点。令第1次体素降采样后的激光点云和全局点云地图分别为Pk（′）L，Pm（′）ap。

2）通过 Fast –LIO2算法获取位姿变换矩阵 Tk，将 Pk（′）L按 Tk 进行变换，得到点云 Pk（′）M 。对 Pk（′）M 和 Pm（′）ap进行点云粗匹配，得到位姿变换矩阵 Tk（′）。点云匹配采用迭代最近点（Iterative Closest Point，ICP）算法，具体过程在3.2节给出。

3）对激光点云Pk（L）和全局点云地图Pmap进行第2次體素降采样，边长为d2，d2＜d1，得到点云Pk（′′）L 和全局点云地图 Pm（′′）ap。将 Pk（′′）L按TkTk（′）进行变换，得到点云Pk（′′）M。对 Pk（′′）M 和 Pm（′′）ap进行点云精匹配，得到位姿变换矩阵 Tk（′′）。最终得到全局位姿变换矩阵 Tk =

3.2 基于 ICP 的点云匹配方法

设待匹配的 2组点云分别为{s1（L），s2（L），··· ， sn（L）}，{s1（M），s2（M），··· ， sn（M）}，si（L）为从激光雷达获取的第i个点云，si（M）为全局点云地图中si（L）的最近邻点云，i=1，2，…，n，n 为待匹配点云个数。则 ICP 目标公式为

式中：E（R′; x′）为代价函数；R′为旋转矩阵；x′为平移向量。

设sM，sL分别为全局点云地图和激光雷达点云的质心，令A = sM ? R′sL，则E（R′; x′）可进一步表示为"si（M）? R′si（L）? x′+ A? A"=

由此将求解2组点云的位姿变换问题转换为求使tr（R′ Z）最大的R′。

對 Z 进行奇异值分解（Singular Value Decomposition，SVD），得

式中：Σ为由 SVD得到的奇异值构成的对角矩阵； U，V为Σ对应的正交矩阵。

取R′= VUT，将R′代入式（5）得到平移向量x′，进而得到全局点云地图和当前时刻激光点云的全局位姿变换矩阵Tk。

3.3 基于时间的位姿输出策略

由于点云匹配耗费的时间和点的数量呈正相关，当点数量过多时，点云匹配算法的运行速度无法保证定位数据的实时性。对此，设计了基于时间的位姿输出策略，在满足位姿精度要求的前提下，保证位姿的输出频率，具体流程如下。

1）设定点云匹配算法的运行频率f，单次匹配算法运行时间 t应满足t＞1/f 。

2）获取激光 SLAM 算法得到的位姿变换矩阵 Tk 。

3）通过点云匹配得到当前时刻全局位姿变换矩阵Tk，则 SLAM 坐标系到点云地图坐标系的位姿变换矩阵Tk（G）?M = Tk Tk。

4）设下一次运行点云匹配算法的时刻为 tm，将tk到 tm 之间所有的位姿变换矩阵{Tk+1; Tk+2;···; Tm }全部按 Tk（G）?M进行位姿变换，得到全局位姿变换矩阵{T（?）k+1; T（?）k+2;···; T（?）m }。之后转步骤3）。

需要注意的是，定位系统初始运行时全局位姿变换矩阵的初始经验值 T（?）1需人为给出。

位姿变换算法伪代码如下。

4 试验验证

4.1 硬件架构

试验用无人机硬件包括无人机平台、机载计算机、三维激光雷达，如图4所示。无人机平台包括飞行控制器及动力系统，采用 X 型四旋翼布局，如图5所示。无人机平台对称轴距为600 mm，最大起飞质量为4 kg，搭载Livox–AVIA雷达采集环境点云数据和 IMU 数据，通过网口将点云数据和 IMU 数据发送至khadas–vim3机载计算机。机载计算机对接收数据进行定位程序处理，计算无人机当前位姿，并实时规划无人机飞行轨迹，将当前位姿和飞行路径发送至飞行控制器，实现无人机精准定位及位置控制。

4.2 SLAM 精度试验对比

受井下环境条件的限制，难以布置测量设备得到无人机飞行位置的坐标真值，因此直接评估定位精度成本和代价非常高，也不是本文重点。巷道是典型的长廊形结构，由于其在长度方向上缺乏明显的结构特征，所以激光 SLAM 容易出现定位漂移。为了分析和验证本文算法在长廊形巷道中的定位精度，设计以下试验过程。

以某矿辅助运输大巷为试验环境，试验巷道长度为1000 m，巷道较平直，无明显转弯，无照明，无大型设备。标记无人机起飞位置 a 和降落位置 b，用100 m 钢卷尺从位置 a 开始，沿巷道中线每100 m测量标记1个位置点，记为{a1，a2，··· ， a9，b}。无人机从位置 a 起飞至距地面1 m 高度时记录时间戳 t0，控制无人机沿巷道平飞。飞行过程中记录激光雷达点云扫描数据、IMU 原始数据，以及到达标记位置和终点的时间戳{t0，t1，··· ， t9，tb }。采用不同的 SLAM 算法处理飞行过程中记录的数据，主要步骤如下：

1）记录 SLAM 算法在{t0，t1，··· ， t9，tb }时刻得到的位姿数据{T0，T1，··· ， T9，Tb }。

2）依次计算{t1，t2，··· ， t9，tb }时刻的里程计数据与t0时刻里程计数据差值d ={d1，d2，··· ， d9，db }。

3）计算 d 和各标记点真实里程的误差。

为验证本文算法在巷道环境中定位的稳定性，同时消除试验设置的特殊性，分别从试验巷道200，400，600 m 处作为起点，以 b 点为终点进行试验，记录途经标记点的真实里程误差。试验场景如图6 所示。

采用 LOAM?Livox算法[19]、LIO?Livox算法[20]和本文算法进行里程精度对比。 LOAM?Livox算法仅使用激光点云作为输入，是 LOAM 算法针对Livox激光雷达的改进版本；LIO?Livox是一种激光点云和 IMU 数据紧耦合的算法。

3种 SLAM 算法的建图效果如图7所示，在4组试验中的定位误差见表1，误差曲线如图8所示。可看出3种算法的定位误差总体上随飞行距离不断累计，在标记 a 点起飞试验100 m标记处、标记200 m 处起飞试验400 m 和500 m标记处，LIO?Livox算法的误差较小，但在600 m 以上范围内本文算法的累计误差明显小于 LOAM?Livox算法和 LIO?Livox算法，且在长距离定位场景中误差绝对值始终小于1 m，验证了本文算法在长距离巷道环境中具有较高的定位精度。特别指出的是，在大于600 m距离的试验巷道场景中，本文算法的定位误差始终小于0.3 m，满足实际应用要求。

4.3 位姿校正试验

由于 SLAM 算法定位以起飞时刻的位姿作为定位坐标系，导致每次飞行中使用的坐标系不同。为验证提出的位姿校正算法的有效性，设计了以下试验：使用 Fast?LIO2算法建立巷道的全局点云地图并保存，使用该地图确定井下全局坐标系；将无人机起飞位置偏转一定角度，重新沿巷道飞行，记录 Fast?LIO2算法输出的位置数据和本文位姿校正算法输出的位置数据。试验结果如图9所示，蓝色为校正前的路径，红色为校正后的路径。可看出校正前的路径偏离至全局点云地图之外，校正后的路径全部位于全局点云地图中，验证了算法的有效性。10个标记点坐标见表2。

4.4 算法复杂度分析和实时性试验

Fast?LIO2算法复杂度分析详见文献[17-18]，算法中主要包括 IEKF 算法和ikd?tree 算法。传统的 IEKF 算法的时间复杂度和观测量相关，设观测量维度为 r，时间复杂度为O（r2），而 Fast?LIO2算法采用新的卡尔曼增益计算公式，时间复杂度和状态量维度直接相关，在保证运算精度的同时，降低了时间复杂度。ikd?tree 算法主要实现地图的增量操作、地图重建和 K 近邻搜索，设ikd?tree 的尺寸为Ntree，则增量操作的时间复杂度为O（log2 Ntree ），地图重建的时间复杂度为 O（Ntree ），K 近邻搜索的时间复杂度为 O（Ntree log2 Ntree ）。

ICP 算法的时间复杂度依赖于源点云和目标点云的大小，设Nlidar为激光雷达某一点云帧中的三维点数，Nmap 为全局点云地图中三维点数，则 ICP 算法单次迭代的时间复杂度为O（NlidarNmap ），详细分析见文献[21]。

机载计算机 CPU 为 Amlogic?A311D（ARM 架构、4核2.2 GHz+双核1.8 GHz），激光雷达数据帧为10 Hz，根据实测，位姿校正算法每20 s运行1次即可满足要求。4.2节和4.3节的试验过程中，记录各步骤运行的平均时间，结果见表3。单次 SLAM 算法运行耗时14.83 ms，单次位姿校正耗时883 ms。位姿校正算法和激光 SLAM 算法分别在2个线程中运行，最终整体计算输出的位姿数据频率为10 Hz，而低速无人机位置控制频率不低于5 Hz 即可满足实时性，因此本文方法能够满足实时性要求。

5 结论

1）提出的井下无人机定位方法采用基于 IEKF 的激光雷达和 IMU 融合 SLAM 获得无人机位姿估计，基于点云地图和 ICP点云匹配对无人机位姿进行校正，从而得到无人机井下全局位姿数据。

2）在1000 m井下巷道环境中开展无人机定位试验，比较了 LOAM?Livox算法、LIO?Livox算法和本文算法在相同数据集上的结果差异，验证了本文方法具有更高的定位精度和稳定性。

参考文献（References）：

[1] 郑学召，童鑫，张铎，等.矿井危险区域多旋翼侦测无人机关键技术探讨[J].工矿自动化，2020，46（12）：48-56.

ZHENG Xuezhao，TONG Xin，ZHANG Duo，et al. Discussion on key technologies of multi-rotor detection UAVs in mine dangerous area [J]. Industry and Mine Automation，2020，46（12）：48-56.

[2] 呂文红，夏双双，魏博文，等.基于改进A*算法的灾后井下无人机航迹规划[J].工矿自动化，2018，44（5）：85-90.

LYU Wenhong，XIA Shuangshuang，WEI Bowen，et al. Route planning of unmanned aerial vehicle in post- disaster  underground  based  on  improved  A* algorithm[J]. Industry and Mine Automation，2018，44（5）：85-90.

[3] 张铎，吴佩利，郑学召，等.矿井侦测无人机研究现状与发展趋势[J].工矿自动化，2020，46（7）：76-81.

ZHANG Duo，WU Peili，ZHENG Xuezhao，et al. Research status and development trend of mine detection unmanned aerial vehicle[J]. Industry and Mine Automation，2020，46（7）：76-81.

[4] 李标.基于无人机技术的煤矿带式输送机巡检方案[J].煤矿安全，2020，51（7）：128-131.

LI Biao. Inspection scheme of coal mine belt conveyor based on UAV technology[J]. Safety in Coal Mines，2020，51（7）：128-131.

[5] 王岩，马宏伟，王星，等.基于迭代最近点的井下无人机实时位姿估计[J].工矿自动化，2019，45（9）：25-29.

WANG Yan，MA Hongwei，WANG Xing，et al. Real- time pose estimation of underground unmanned aerial vehicle based on ICP method[J]. Industry and Mine Automation，2019，45（9）：25-29.

[6] 夏双双，殷立杰.煤矿井下无人机SLAM定位算法研究[J].电子质量，2017（12）：56-61，66.

XIA Shuangshuang，YIN Lijie. Research on SLAM location algorithm of downhole UAV[J]. Electronics Quality，2017（12）：56-61，66.

[7] 江传龙，黄宇昊，韩超，等.井下巡检无人机系统设计及定位与避障技术[J].机械设计与研究，2021，37（4）：38-42，48.

JIANG Chuanlong，HUANG Yuhao，HAN Chao，et al. Design of underground inspection UAV system and studyof positioning and obstacle avoidance[J]. Machine Design & Research，2021，37（4）：38-42，48.

[8] ZHANG Ji， SINGH S. Visual-lidar odometry and mapping： low-drift， robust， and fast[C]. IEEE International Conference on Robotics and Automation， Piscataway，2015：2174-2181.

[9] ZHANG Ji，SINGH S. Low-drift and real-time lidar odometry and mapping[J]. Autonomous Robots，2017，41（2）：401-416.

[10] SHAN Tixiao，ENGLOT B. LeGO-LOAM：lightweightand ground-optimized lidar odometry and mapping on variable terrain[C]. IEEE/RSJ International Conference on Intelligent Robots and Systems，Piscataway，2018：4758-4765.

[11] SHAN Tixiao，ENGLOT B，MEYERS D，et al. LIO- SAM： tightly-coupled lidar inertial odometry via smoothing and mapping[C]. IEEE International Conference on Intelligent Robots and Systems，Las Vegas，2020：5135-5142.

[12] CHAO Qin，YE Haoyang，PRANATA C E，et al. LINS：a lidar-inertial state estimator for robust and efficient navigation[C]. IEEE International Conference on Robotics  and  Automation， Piscataway， 2020：8899-8906.

[13] 杨林，马宏伟，王岩.基于激光惯性融合的煤矿井下移动机器人 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.

[14] 鄒筱瑜，黄鑫淼，王忠宾，等.基于集成式因子图优化的煤矿巷道移动机器人三维地图构建[J].工矿自动化，2022，48（12）：57-67，92.

ZOU Xiaoyu，HUANG Xinmiao，WANG Zhongbin， et al.3D map construction of coal mine roadway mobile robot based on integrated factor graph optimization[J]. Journal of Mine Automation，2022，48（12）：57-67，92.

[15] 李猛钢，胡而已，朱华.煤矿移动机器人LiDAR/IMU紧耦合SLAM方法[J].工矿自动化，2022，48（12）：68-78.

LI Menggang，HU Eryi，ZHU Hua. LiDAR/IMU tightly- coupled SLAM method for coal mine mobile robot[J]. Journal of Mine Automation，2022，48（12）：68-78.

[16] 马艾强，姚顽强，蔺小虎，等.面向煤矿巷道环境的 LiDAR与IMU融合定位与建图方法[J].工矿自动化，2022，48（12）：49-56.

MA Aiqiang，YAO Wanqiang，LIN Xiaohu，et al. Coal mine roadway environment-oriented LiDAR and IMU fusion positioning and mapping method[J]. Journal ofMine Automation，2022，48（12）：49-56.

[17] XU Wei， ZHANG Fu. 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.

[18] XU Wei，CAI Yixi，HE Dongjiao，et al. FAST-LIO2：fast  direct  LiDAR-Inertial  odometry[J]. IEEE Transactions on Robotics，2022，38（4）：2053-2073.

[19] LIN Jiarong，ZHANG Fu. LOAM Livox：a fast，robust， high-precision LiDAR odometry and mapping package for LiDARs of small FoV[C]. IEEE International Conference on Robotics and Automation，Paris，2020：3126-3131.

[20] GitHub-Livox-SDK/LIO-Livox：a robust LiDAR-inertial odometry for LivoxLiDAR[EB/OL]. [2022-12-22]. https：//github.com/Livox-SDK/LIO-Livox.

[21] BESL P，MCKAY N D. A method for registration of 3-D shapes[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence，1992，14（2）：239-256.