LiDAR/IMU tightly-coupled SLAM method for coal mine mobile robot
-
摘要: 煤矿井下机器人同步定位与地图构建(SLAM)是当前研究热点,但针对提高激光SLAM在井下复杂条件下精度、鲁棒性的研究仍然不足;传统激光SLAM方法在井下复杂环境下存在累计误差迅速增大、旋转过程鲁棒性差、特征关联错误率高等问题;现有激光−惯性融合的定位建图紧耦合融合机制仍需进一步提高对煤矿井下复杂环境的适应能力。针对上述问题,提出了一种煤矿机器人LiDAR(激光雷达)/IMU(惯性测量单元)紧耦合SLAM方法(LI−SLAM方法)。首先利用IMU观测信息预测点云运动状态并进行有效补偿,减少由于剧烈振动、快速旋转等恶劣运动工况导致的点云畸变;然后提取雷达点云的边线与平面特征,基于点−线和点−面扫描匹配构建激光相对位姿约束,并在向量空间与流形空间解析推导了约束的残差、雅可比矩阵、协方差矩阵构建过程;最后通过构建雷达相对位姿约束因子、IMU预积分约束因子、回环检测约束因子,基于因子图优化方法完成LiDAR/IMU紧耦合,实现井下复杂环境下煤矿移动机器人的定位与地图构建。为了验证LI−SLAM方法在颠簸路面、复杂场景的精度与鲁棒性,基于煤矿轮式移动机器人平台,在野外、地下车库环境下进行了试验,在晋能集团塔山煤矿开展了工业性试验,并与当前最优的激光里程计与建图(LOAM)方法、激光雷达惯性状态估计(LINS)方法、雷达惯性里程计与建图(LIO−mapping)方法进行了对比。在野外颠簸路面的试验结果表明:LI−SLAM方法和LOAM方法的地图一致性最好,与真实路线基本吻合,LI−SLAM方法对旋转有更佳的适应能力,距离误差最小;LIO−mapping方法无法实时运行,在0.5倍速下可以获得完整轨迹,但在初始运动阶段出现了较大程度的方向偏移,初始化过程容易失败;LINS方法由于仅利用了最新的观测信息,在复杂地形下出现了漂移。地下车库环境下的试验结果表明:与LOAM方法、LINS方法、LIO−mapping方法相比,LI−SLAM方法具有较高的建模精度,局部精细化程度更高,运动轨迹更平滑。煤矿井下现场工业性试验结果表明:LI−SLAM方法在各类地形环境中均可以稳定、在线运行,满足鲁棒性、实时性需求;在煤矿移动机器人行驶巷道直线距离为273 m时,分析30组距离结果,平均误差小于15 cm,具有较高的定位和建模精度,基本满足煤矿移动机器人的定位建模精度需求,对于煤矿井下复杂环境下的移动机器人精确定位与地图构建有更好的适用性。Abstract: SLAM (Simultaneous Localization and Mapping) of the underground robot is a hot research topic at present. But the research on improving the precision and robustness of laser SLAM in underground complicated conditions is still insufficient. The traditional laser SLAM method has the problems of rapidly increasing cumulative error, poor robustness of the rotation process and high error rate of feature correlation under complex underground environment. The existing laser inertial fusion location mapping tightly-coupled fusion mechanism still needs to further improve the adaptability to the complex environment in coal mines. In order to solve the above problems, a LiDAR (lidar)/IMU (inertial measurement unit) tightly-coupled SLAM (LI-SLAM) method for coal mine robot is proposed. Firstly, the IMU observation information is used to predict the point cloud motion state and make effective compensation to reduce the point cloud distortion caused by severe vibration, rapid rotation and other severe motion conditions. Secondly, the edge and plane features of the radar point cloud are extracted. The laser relative pose constraints are constructed based on point line and point surface scanning matching. In vector space and manifold space, the construction process of residual, Jacobian matrix and covariance matrix of constraints is derived analytically. Finally, the LiDAR/IMU tight coupling is completed based on the factor graph optimization method by constructing the radar relative pose constraint factor, IMU pre-integration constraint factor and loopback detection constraint factor. The localization and map construction of the mine mobile robot in the complex underground environment is realized. In order to verify the precision and robustness of the LI-SLAM method in the bumpy road and complex scenario, experiments are carried out in the field and underground garage environment based on the platform of wheeled mobile robot in the coal mine. The industrial experiments are carried out in Tashan Coal Mine of Jinneng Group. The results are compared with the current optimal LiDAR odometry and mapping (LOAM) method, lidar-inertial state estimator (LINS) method and lidar inertial odometry and mapping (LIO-mapping) method. The test results in field bumpy road show the following points. The map consistency of the LI-SLAM method and the LOAM method is the best, which is basically consistent with the real route. The LI-SLAM method has better adaptability to rotation, and the distance error is the minimum. The LIO-mapping method cannot run in real time. The method can obtain complete trajectory at 0.5 times. However, in the initial motion phase, there is a large degree of direction deviation, and the initialization process is easy to fail. Because LINS only uses the latest observation information, it drifts under complex terrain. The test results in underground garage environment show the following points. Compared with the LOAM method, LINS method and LIO-mapping method, the LI-SLAM method has higher modeling precision. The local refinement is higher, and the motion trajectory is smoother. The industrial test results in underground coal mines show the following points. The LI-SLAM method can operate stably and online in various terrain environments. The result meets the requirements of robustness and real-time. When the straight-line distance of the roadway where the coal mine mobile robot drives on is 273 m, 30 groups of distance results are analyzed, and the average error is less than 15 cm. It has high positioning and modeling precision. It basically meets the positioning and modeling precision requirements of coal mine mobile robots. It has better applicability for precise positioning and mapping of mobile robot in the complex environment of the coal mine.
-
表 1 不同方法起点、终点间距离与真值的误差
Table 1. The distance and truth value error between the starting point and the end point of different methods
试验场地 行程/m 时间/s 误差/m LIO−mapping×0.5 LINS LOAM LI−SLAM 1 336 854 8.867 − 4.809 0.233 2 197 477 2.788 − 0.9859 0.4 3 963 1433 29.217 − 15.904 17.204 表 2 野外试验时LI−SLAM方法中各模块耗时均值与最大值
Table 2. The average and maximum time consumption of each module of LI-SLAM method in field tests
ms 模块 耗时均值 耗时最大值 去畸变 1.889 7.466 特征提取 2.658 13.332 回环检测 85.820 915.839 地图构建 49.322 169.238 优化 0.153 156.012 表 3 不同方法起点、终点间距离的误差
Table 3. The distance error between the starting point and the end point of different methods
行程/m 时间/s 误差/m LIO−mappinsg LINS LOAM LI−SLAM 168.9 549.7 1.227 0.181 0.070 0.126 表 4 地下车库试验时LI−SLAM方法中各模块耗时均值与最大值
Table 4. The average and maximum time consumption of each module in LI-SLAM method in underground garage tests
ms 模块 耗时均值 耗时最大值 去畸变 2.319 10.885 特征提取 3.380 9.034 回环检测 44.092 359.736 地图构建 28.115 171.516 优化 0.078 79.906 -
[1] 葛世荣,胡而已,裴文良. 煤矿机器人体系及关键技术[J]. 煤炭学报,2020,45(1):455-463. doi: 10.13225/j.cnki.jccs.YG19.1478GE Shirong,HU Eryi,PEI Wenliang. Classification system and key technology of coal mine robot[J]. Journal of China Coal Society,2020,45(1):455-463. doi: 10.13225/j.cnki.jccs.YG19.1478 [2] BOSSE M,ZLOT R,FLICK P. Zebedee:design of a spring-mounted 3-D range sensor with application to mobile mapping[J]. IEEE Transaction on Robot,2012,28(5):1104-1119. doi: 10.1109/TRO.2012.2200990 [3] HUBER D F, VANDAPEL N. Automatic 3D underground mine mapping[C]. Proceedings of Field and Service Robotics, Berlin, 2003: 497-506. [4] THRUN S,THAYER S,WHITTAKER W,et al. Autonomous exploration and mapping of abandoned mines[J]. IEEE Robotics & Automation Magazine,2004,11(4):79-91. [5] ZLOT R,BOSSE M. Efficient large-scale three-dimensional mobile mapping for underground mines[J]. Journal of Field Robotics,2014,31(5):758-779. doi: 10.1002/rob.21504 [6] TARDIOLI D,RIAZUELO L,SICIGNANO D,et al. Ground robotics in tunnels:keys and lessons learned after 10 years of research and experiments[J]. Journal of Field Robotics,2019,36(6):1074-1101. doi: 10.1002/rob.21871 [7] KASPER M, MCGUIRE S, HECKMAN C. A benchmark for visual-inertial odometry systems employing onboard illumination[C]. IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS), Macau, 2019: 5256-5263. [8] EBADI K, CHANGE Y, PALIERI M, et al. LAMP: large-scale autonomous mapping and positioning for exploration of perceptually-degraded subterranean environments[C]. IEEE International Conference on Robotics and Automation (ICRA), Paris, 2020: 80-86. [9] WISTH D,CAMURRI M,DAS S,et al. Unified multi-modal landmark tracking for tightly coupled lidar-visual-inertial odometry[J]. IEEE Robotics and Automation Letters,2021,6(2):1004-1011. doi: 10.1109/LRA.2021.3056380 [10] 马宏伟,王岩,杨林. 煤矿井下移动机器人深度视觉自主导航研究[J]. 煤炭学报,2020,45(6):2193-2206. doi: 10.13225/j.cnki.jccs.zn20.0214MA Hongwei,WANG Yan,YANG Lin. Research on depth vision based mobile robot autonomous navigation in underground coal mine[J]. Journal of China Coal Society,2020,45(6):2193-2206. doi: 10.13225/j.cnki.jccs.zn20.0214 [11] 陈先中,刘荣杰,张森,等. 煤矿地下毫米波雷达点云成像与环境地图导航研究进展[J]. 煤炭学报,2020,45(6):2182-2192. doi: 10.13225/j.cnki.jccs.zn20.0316CHEN 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 [12] 杨健健,张强,吴淼,等. 巷道智能化掘进的自主感知及调控技术研究进展[J]. 煤炭学报,2020,45(6):2045-2055. doi: 10.13225/j.cnki.jccs.zn20.0287YANG Jianjian,ZHANG Qiang,WU Miao,et al. Research progress of autonomous perception and control technology for intelligent heading[J]. Journal of China Coal Society,2020,45(6):2045-2055. doi: 10.13225/j.cnki.jccs.zn20.0287 [13] LI Menggang,ZHU Hua,YOU Shaoze,et al. Efficient laser-based 3D SLAM for coal mine rescue robots[J]. IEEE Access,2019(7):14124-14138. [14] 高士岗,高登彦,欧阳一博,等. 中薄煤层智能开采技术及其装备[J]. 煤炭学报,2020,45(6):1997-2007. doi: 10.13225/j.cnki.jccs.zn20.0246GAO Shigang,GAO Dengyan,OUYANG Yibo,et al. Intelligent mining technology and its equipment for medium thickness thin seam[J]. Journal of China Coal Society,2020,45(6):1997-2007. doi: 10.13225/j.cnki.jccs.zn20.0246 [15] POMERLEAU F,COLAS F,SIEGWART R. A review of point cloud registration algorithms for mobile robotics[J]. Foundations and Trends in Robotics,2015,5(4-1):1-104. [16] NÜCHTER A,LINGEMANN K,HERTZBERG J,et al. 6D SLAM—3D mapping outdoor environments[J]. Journal of Field Robotics,2007,24(8/9):699-722. [17] ZHANG Ji, SINGH S. LOAM: lidar odometry and mapping in real-time[C]. Robotics: Science and Systems Conference, California, 2014. DOI: 10.15607/RSS.2014. X.007. [18] 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, 2018: 4758-4765. [19] WEINGARTEN J, SIEGWART R. 3D SLAM using planar segments[C]. IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS), Beijing, 2006: 3062-3067. [20] TREVOR A J B, ROGERS J G, CHRISTENSEN H I. Planar surface SLAM with 3D and 2D sensors[C]. IEEE International Conference on Robotics and Automation(ICRA), Saint Paul, 2012: 3041-3048. [21] YE Haoyang, CHEN Yuying, LIU Ming. Tightly coupled 3D lidar inertial odometry and mapping[C]. IEEE International Conference on Robotics and Automation (ICRA), Montreal, 2019: 3144-3150. [22] SHAN Tixiao, 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, 2020: 5135-5142. [23] QIN Chao, 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 (ICRA), Paris, 2020: 8899-8906. [24] 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 [25] 杨林,马宏伟,王岩. 煤矿井下移动机器人基于激光惯性的融合 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. [26] QIN Tong,LI Peiliang,SHEN Shaojie. 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 [27] 朱华,由韶泽. 新型煤矿救援机器人研发与试验[J]. 煤炭学报,2020,45(6):2170-2181. doi: 10.13225/j.cnki.jccs.zn20.0352ZHU Hua,YOU Shaoze. Research and experiment of a new type of coal mine rescue robot[J]. Journal of China Coal Society,2020,45(6):2170-2181. doi: 10.13225/j.cnki.jccs.zn20.0352