Positioning method for underground unmanned aerial vehicles in coal mines based on global point cloud map
-
摘要: 即时定位与建图 （SLAM）技术应用于煤矿井下无人机自主定位时，由于采用特征点构建地图，易出现退化问题，导致定位不准确，且因其以机体作为参考坐标系，无法实现全局定位。针对该问题，提出了一种基于全局点云地图的煤矿井下无人机定位方法。以Fast−LIO2算法作为激光SLAM算法，获得无人机位姿估计；采用迭代最近邻算法，对获取的激光雷达实时点云和全局点云地图进行两步匹配，实现无人机位姿校正；针对因点云数量过多导致点云匹配速度无法保证定位实时性的问题，设计了基于时间的位姿输出策略，提高了无人机位姿数据输出频率。在1 000 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，满足无人机定位实时性要求。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 1 000 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 m in 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.
-
图 1 煤矿井下无人机定位方法架构
Figure 1. Structure of unmanned aerial vehicles (UAV) positioning method in underground coal mine
图 2 激光SLAM算法的无人机位姿估计流程
Figure 2. Flow of UAV position and attitude estimation of lidar simultaneous localization and mapping (SLAM) algorithm
图 9 位姿校正前后无人机飞行路径对比
Figure 9. Comparison of UAV flight routes before and after position and attitude correction
算法1：位姿变换算法 输入：点云地图$ {{{\boldsymbol{P}}}_{{\text{map}}}} $， 当前时刻的扫描点云${{\boldsymbol{P}}}_k^{{L}}$， SLAM位姿变换矩阵${\overline {\boldsymbol{T}}_k}$， 当前时刻${t_k}$， 上一次运行位姿校正程序的时间${t_j}$， SLAM坐标系到全局点云地图坐标系的位姿变换矩阵${{\boldsymbol{T}}_k ^{{{G - M}}}}$。输出：当前时刻的全局位姿变换矩阵${{\boldsymbol{T}}_k}$。 1 预测位姿变换矩阵${\hat {\boldsymbol{T}}_k} = {{\boldsymbol{T}}_k^{G - M}}{{\boldsymbol{T}}_k}$； 2 If ${t_k} - {t_j} ＞ 1/f$ then 3 将${{{\boldsymbol{P}}}_{{\text{map}}}}$和${{\boldsymbol{P}}}_k^{{L}}$点云降采样，得到$ {{{\boldsymbol{P}}}_{ {\text{map}}}^\prime} $和${{\boldsymbol{P}}}_k^{ \prime {L}}$，将${{\boldsymbol{P}}}_k^{ \prime {L}}$按${\hat {\boldsymbol{T}}_k}$变换为${{\boldsymbol{P}}}_k^{ \prime {M}}$； 4 使用ICP算法对$ {{{\boldsymbol{P}}}_{ {\text{map}}}^ \prime} $和${{\boldsymbol{P}}}_k^{ \prime{M}}$进行粗匹配，得到${\overline {\boldsymbol{T }}_k^\prime}$； 5 将${M}$和${{\boldsymbol{P}}}_k^{{L}}$点云降采样，得到$ {{{\boldsymbol{P}}}_{ {\text{map}}}^{\prime\prime}} $和${{\boldsymbol{P}}}_k^{ \prime \prime {L}}$，将${{\boldsymbol{P}}}_k^{ \prime \prime{L}}$按${\hat {\boldsymbol{T}}_k}{\overline {\boldsymbol{T}}{}_k^{\prime }}$变换为${{\boldsymbol{P}}}_k^{ \prime \prime{M}}$； 6 使用ICP算法对$ {{{\boldsymbol{P}}} _{ {\text{map}}}^{\prime\prime}} $和${{\boldsymbol{P}}}_k^{ \prime \prime{M}}$进行精匹配，得到${\overline {\boldsymbol{T}}_k^{\prime \prime}}$； 7 全局位姿变换矩阵${{\boldsymbol{T}}_k}{\text{ = }}{\hat {\boldsymbol{T}}_k}{\overline {\boldsymbol{T}}_k^{\prime }}{\overline {\boldsymbol{T}}{}_k^{\prime \prime}}$； 8 ${\boldsymbol{T}}_k^{{{G } -{ M}}} = {{\boldsymbol{T}}_k}{\overline {\boldsymbol{T}}_k}$ 9 else 10 ${{\boldsymbol{T}}_k}{\text{ = }}{\hat {\boldsymbol{T}}_k}$ 11 final 12 return ${{\boldsymbol{T}}_k}$ 
下载: 导出CSV
表 1 不同SLAM算法定位误差对比
Table 1. Positioning error comparison of different SLAM algorithms
试验条件 误差/m 100 m处 200 m处 300 m处 400 m处 500 m处 600 m处 700 m处 800 m处 900 m处 1 000 m处 从标记a点起飞 LOAM−Livox 0.16 −0.31 0.20 −0.25 −6.18 −11.90 −27.84 −60.76 −62.10 −63.82 LIO−Livox −0.04 −0.41 −0.50 −1.15 −2.08 −2.36 −2.25 −3.31 −5.47 −7.28 本文算法 0.27 0.10 0.38 0.36 −0.18 −0.02 0.35 0.38 0.27 −0.76 从标记200 m处起飞 LOAM−Livox − − 0.69 0.32 −5.27 −11.17 −26.51 −62.41 −63.91 −65.91 LIO−Livox − − 0.64 0.05 −0.04 −1.20 −1.02 −1.82 −4.78 −6.80 本文算法 − − 0.29 0.27 −0.28 −0.12 0.25 0.29 0.18 −0.86 从标记400 m处起飞 LOAM−Livox − − − − −9.32 −13.84 −28.72 −64.62 −66.22 −68.52 LIO−Livox − − − − −1.50 −1.97 −2.03 −3.08 −7.15 −9.30 本文算法 − − − − −0.28 −0.40 −0.03 0.01 −0.10 −0.58 从标记600 m处起飞 LOAM−Livox − − − − − − −17.48 −50.84 −51.72 −53.58 LIO−Livox − − − − − − −0.19 −1.61 −4.72 −6.76 本文算法 − − − − − − −0.13 0.02 −0.13 −0.55
下载: 导出CSV
表 2 位姿校正前后标记点坐标
Table 2. Coordinate of label points before and after position and attitude correction
位置 校正前坐标/m 校正后坐标/m X Y Z X Y Z 100 m处 100.193 −3.810 −0.672 99.967 −0.326 3.497 200 m处 199.877 −7.530 −5.523 200.106 −0.513 2.860 300 m处 299.978 −11.356 −10.658 300.008 −0.856 2.036 400 m处 399.728 −14.856 −16.975 400.102 −0.845 0.071 500 m处 499.157 −18.536 −17.829 500.070 −1.053 3.640 600 m处 599.201 −22.067 −21.024 600.035 −1.172 4.899 700 m处 699.390 −24.916 −26.742 700.184 −0.703 3.668 800 m处 799.140 −28.294 −34.422 800.051 −0.821 0.595 900 m处 898.886 −30.942 −39.222 900.065 −0.124 0.395 1000 m处 997.655 −34.066 −44.636 998.930 −0.014 −0.082
下载: 导出CSV
表 3 算法单个步骤单次运行耗时
Table 3. Time consumption of single operation in single step of the algorithm
步骤 运行频率/Hz 运行耗时/ms SLAM 预处理 10 0.05 位姿估计 10 14.35 建图 10 0.43 位姿校正 0.05 883
下载: 导出CSV
-
[1] 郑学召,童鑫,张铎,等. 矿井危险区域多旋翼侦测无人机关键技术探讨[J]. 工矿自动化,2020,46(12):48-56. doi: 10.13272/j.issn.1671-251x.17653ZHENG 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. doi: 10.13272/j.issn.1671-251x.17653 [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. doi: 10.3969/j.issn.1003-0107.2017.12.015XIA Shuangshuang,YIN Lijie. Research on SLAM location algorithm of downhole UAV[J]. Electronics Quality,2017(12):56-61,66. doi: 10.3969/j.issn.1003-0107.2017.12.015 [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. doi: 10.1007/s10514-016-9548-2 [10] 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, 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. doi: 10.13225/j.cnki.jccs.2022.0506YANG 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. doi: 10.13225/j.cnki.jccs.2022.0506 [14] 邹筱瑜,黄鑫淼,王忠宾,等. 基于集成式因子图优化的煤矿巷道移动机器人三维地图构建[J]. 工矿自动化,2022,48(12):57-67,92. doi: 10.13272/j.issn.1671-251x.2022100041ZOU 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. doi: 10.13272/j.issn.1671-251x.2022100041 [15] 李猛钢,胡而已,朱华. 煤矿移动机器人LiDAR/IMU紧耦合SLAM方法[J]. 工矿自动化,2022,48(12):68-78. doi: 10.13272/j.issn.1671-251x.2022100061LI 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. doi: 10.13272/j.issn.1671-251x.2022100061 [16] 马艾强,姚顽强,蔺小虎,等. 面向煤矿巷道环境的LiDAR与IMU融合定位与建图方法[J]. 工矿自动化,2022,48(12):49-56. doi: 10.13272/j.issn.1671-251x.2022070007MA Aiqiang,YAO Wanqiang,LIN Xiaohu,et al. Coal mine roadway environment-oriented LiDAR and IMU fusion positioning and mapping method[J]. Journal of Mine Automation,2022,48(12):49-56. doi: 10.13272/j.issn.1671-251x.2022070007 [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. doi: 10.1109/LRA.2021.3064227 [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. doi: 10.1109/TRO.2022.3141876 [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 Livox LiDAR[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. doi: 10.1109/34.121791 -
点击查看大图
图(9) / 表(4)
计量
- 文章访问数: 643
- HTML全文浏览量: 53
- PDF下载量: 34
- 被引次数: 0
