收稿日期: 2022-10-14
网络出版日期: 2023-10-27
基金资助
国家自然科学基金资助项目(62173064);装备预研教育部联合基金资助项目(8091B022119)
A GPS-Laser-IMU Fusion Mapping Algorithm Based on Iterated Kalman Filter
Received date: 2022-10-14
Online published: 2023-10-27
Supported by
the National Natural Science Foundation of China(62173064); the Joint Fund of the Ministry of
Education for Equipment Pre-research(8091B022119)
在当前机器人导航和环境感知领域,室外大尺度场景下的三维激光SLAM一直是一个挑战性问题。由于GPS信号在某些环境下的不稳定性和激光SLAM的误差累积特性,传统算法在大尺度场景下表现不佳。针对室外大尺度场景下三维激光SLAM(同步定位和地图构建)存在的误差累积严重问题,本文提出了一种基于迭代卡尔曼滤波器的GPS-激光-IMU融合建图算法。该算法通过利用惯性测量单元(IMU)数据对机器人状态进行预测,同时以激光和全球定位系统(GPS)数据作为观测,更新机器人状态,推导出观测方程和雅可比矩阵,显著提高了建图的精度和鲁棒性。里程计中融合GPS数据的绝对位置信息以解决长时间运行中的误差累积问题。在特征稀疏的环境中,由于约束不足可能导致算法崩溃,GPS数据的引入可以提高系统的鲁棒性。此外,重力对于IMU数据预测机器人状态起到关键的作用。虽然重力是三维向量,但在不发生区域变化的情况下,其模长是不变的,因此被视为二自由度向量。通过将重力的优化转化为旋转矩阵群上的优化,成功避免了重力过参数化的问题,提高了算法的精度。在室外场景下与其他算法进行了性能测试对比并且验证了在大尺度场景下的鲁棒性和精度,结果表明:本文算法的均方根误差为0.089 m,与其他算法相比降低了54%。
关键词: 激光SLAM(同步定位和地图构建); 多传感器融合; 迭代卡尔曼滤波器; 重力优化
丛明, 温旭, 王明昊, 等 . 基于迭代卡尔曼滤波器的GPS-激光-IMU融合建图算法[J]. 华南理工大学学报(自然科学版), 2024 , 52(3) : 75 -83 . DOI: 10.12141/j.issn.1000-565X.220667
3D laser SLAM in outdoor large-scale scenes has been a challenging problem in the current field of robot navigation and environment sensing. Due to the instability of GPS signals in some environments and the error accumulation property of laser SLAM, traditional algorithms perform poorly in large-scale scenes. In order to solve the problems of error accumulation of 3D laser SLAM (simultaneous localization and mapping) in outdoor large-scale scenes, this paper proposed a GPS-Laser-IMU fusion mapping algorithm based on iterated Kalman filter, which improves the mapping accuracy and robustness. The algorithm used IMU to predict the robot state, while laser and GPS data were used as observations to update the robot state. The measurement equations and the related Jacobian matrix were derived. Integrating the absolute position information of GPS data in odometer can solve the problem of error accumulation in long-time operation. In the environment with sparse features, the algorithm may collapse due to insufficient constraints. The introduction of GPS can improve the robustness of the system. Additionally, gravity plays a crucial role in the prediction of robot state. Although gravity is a three-dimensional vector, the module length of gravity remains constant when the region does not change, so gravity was treated as a two-degree-of-freedom vector. Transforming gravity optimization into optimization on SO(3) successfully avoids over-parameterization issues, thereby improving precision. Its performance was compared with that of other algorithms in outdoor environments and the robustness and accuracy were verified in large scale scenes. The results show that the root mean square error of this algorithm is 0.089 m, which is 54% lower than that of other algorithms.
| 1 | DURRANT-WHYTE H, BAILEY T .Simultaneous localization and mapping:part I[J].IEEE Robotics & Automation Magazine,2006,13(2):99-110. |
| 2 | BESL P J, MCKAY N D .A method for registration of 3D shapes[J].IEEE Transactions on Pattern Analycis & Machine Intelligence,1992,14(2):239-256. |
| 3 | SEGAL A, HAEHNEL D, THRUN S .Generalized-ICP[C]∥Proceedings of Robotics:Science and Systems.Seattle:MIT Press,2009:435-442. |
| 4 | ZHANG J, SINGH S .Visual-lidar odometry and mapping:low-drift,robust,and fast[C]∥Proceedings of the IEEE International Conference on Robotics and Automation.Seattle:IEEE,2015:2174-2181. |
| 5 | SHAN T, ENGLOT B . Shan T,et al .LeGO-LOAM:lightweight and ground-optimized lidar odometry and mapping on variable terrain[C]∥Proceedings of 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).Madrid:IEEE,2019:4758-4765. |
| 6 | SHAN T, ENGLOT B, MEYERS D,et al .LIO-SAM:tightly-coupled lidar inertial odometry via smoothing and mapping[C]∥Proceedings of 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).Las Vegas:IEEE,2020:5135-5142. |
| 7 | WANG H, WANG C, CHEN C L,et al .F-loam:fast lidar odometry and mapping[C]∥Proceedings of 2021 IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS).Prague:IEEE,2021:4390-4396. |
| 8 | LIN J, ZHANG F .Loam livox:a fast,robust,high-precision lidar odometry and mapping package for lidars of small fov[C]∥Proceedings of 2020 IEEE International Conference on Robotics and Automation (ICRA).[S.l.]:Virtual Conference,2020:3126-3131. |
| 9 | LYNEN S, ACHTELIK M W, WEISS S,et al .Robust and modular multi-sensor fusion approach applied to mav navigation[C]∥Proceedings of 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems.Tokyo:IEEE,2013:3923-3929. |
| 10 | ZHEN W, ZENG S, SOBERER S .Robust localization and localizability estimation with a rotating laser scanner[C]∥Proceedings of 2017 IEEE International Conference on Robotics and Automation (ICRA).Singapore:IEEE,2017:6240-6245. |
| 11 | GAO Y, LIU S, MOHAMED A,et al .INS/GPS/LiDAR integrated navigation system for urban and indoor environments using hybrid scan matching algorithm[J].Sensors,2015,15(9):23286-23302. |
| 12 | YE H, CHEN Y, LIU M .Tightly coupled 3d lidar inertial odometry and mapping[C]∥Proceedings of IEEE International Conference on Robotics and Automation.Montreal:IEEE,2019:3144-3150. |
| 13 | FORSTER C, CARLONE L, DELLAERT F,et al .On-manifold preintegration for real-time visual-inertial odometry[J].IEEE Transactions on Robotics,2017,33(1):1-21. |
| 14 | KAESS M, JOHANNSSON H, ROBERTS R,et al .iSAM2:incremental smoothing and mapping using the bayes tree[J].The International Journal of Robotics Research,2012,31(2):216-235. |
| 15 | 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. |
| 16 | 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. |
| 17 | HE D, XU W, ZHANG F .Kalman filters on differentiable manifolds[J].arXiv preprint arXiv:2102.03804,2021. |
| 18 | CAI Y, XU W, ZHANG F .ikd-Tre:an incremental kd tree for robotic applications[J].arXiv preprint arXiv:2102.10808,2021. |
| 19 | KIM G, KIM A .Scan context:egocentric spatial descriptor for place recognition within 3d point cloud map[C]∥Proceedings of 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).Madrid:IEEE,2018:4802-4809. |
| 20 | WANG H, WANG C, XIE L .Intensity scan context:coding intensity and geometry relations for loop closure detection[C]∥Proceedings of 2020 IEEE International Conference on Robotics and Automation (ICRA).[S.l.]:Virtual Conference,2020:2095-2101. |
| 21 | GUO J, BORGES P V K, PARK C,et al .Local descriptor for robust place recognition using lidar intensity[J].IEEE Robotics and Automation Letters,2019,4(2):1470-1477. |
| 22 | SOLA J .Quaternion kinematics for the error-state Kalman filter[J].arXiv preprint arXiv:1711.02508,2017. |
| 23 | MOORE T, STOUCH D .A generalized extended kalman filter implementation for the robot operating system[C]∥Proceedings of the 13th International Conference on Intelligent Autonomous Systems (IAS).Padua:IEEE,2016:335-348. |
| 24 | 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. |
/
| 〈 |
|
〉 |