A GPS-Laser-IMU Fusion Mapping Algorithm Based on Iterated Kalman Filter

doi:10.12141/j.issn.1000-565X.220667

Abstract

Abstract:

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.

Key words: laser SLAM （simultaneous localization and mapping）, multi-sensor fusion, iterated Kalman filter, gravity optimization

CLC Number:

TP242

CONG Ming, WEN Xu, WANG Minghao, et al. A GPS-Laser-IMU Fusion Mapping Algorithm Based on Iterated Kalman Filter[J]. Journal of South China University of Technology(Natural Science Edition), 2024, 52(3): 75-83.

Figures/Tables 12

Fig.1

Fig.2

Fig.3

Fig.4

Fig.5

Table 1

Fig.6

Table 2

Table 3

Fig.7

Fig.8

Table 4

References 24

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.

算法	均方根误差		均方根误差
本文算法-NoGrav	0.12	LIO-SAM-GPS	0.19
本文算法	0.089	FAST-LIO2	2.37
本文算法-NoGPS	0.44	LeGO-LOAM	0.53

算法	均方根误差/m
算法	Street_03	Street_08
本文算法-NoGPS	0.13	0.22
LIO-SAM	0.14	0.25
FAST-LIO2	0.18	0.23
LeGO-LOAM	0.12	0.20

算法	最大平移误差/m
算法	Street_03	Street_08
本文算法-NoGPS	0.31	0.52
LIO-SAM	0.42	0.84
FAST-LIO2	0.47	0.58
LeGO-LOAM	0.33	0.47

算法	起始平移误差		起始平移误差
本文算法-NoGrav	0.18	LIO-SAM-GPS	0.25
本文算法	0.11	FAST-LIO2	—
本文算法-NoGPS	—	LeGO-LOAM	—