A Multi-level Data Fusion Localization Algorithm for SLAM
ZHANG Hongkai1, CHEN Niansheng1, DAI Zuoxiao2, FAN Guangyu1
1. School of Electronic Information, Shanghai DianJi University, Shanghai 201306, China; 2. Shanghai Institute of Technical Physics, Chinese Academy of Sciences, Shanghai 200040, China
Abstract：There exist some problems in 2D laser SLAM (simultaneous localization and mapping) based robot navigation, such as the large calculation amount in laser point cloud matching, the poor effect of trajectory closure, the large cumulative error of pose, and the insufficient use of data observed by sensors in each link. To solve these problems, a multi-level sensor data fusion method for real-time positioning and mapping is proposed, named Multilevel-SLAM. Firstly, coordinates are transformed based on the preintegration results of IMU (inertial measurement unit) data in data pre-processing, to register the laser point cloud. Features of laser point cloud are sampled to reduce the computation of point cloud matching. Secondly, the robot pose is obtained by combining IMU and LiDAR observations with unscented Kalman filter algorithm, to improve the loop closure detection effect. Finally, laser point cloud registration constraints, closed-loop constraints and IMU pre-integration constraints are added to back-end optimization of SLAM, to provide constraint registration for the pose node estimation in global map and to realize multi-level data fusion. In the experiment, performances of Karto-SLAM, Cartographer, and Multilevel-SLAM algorithms are tested and compared on LiDAR-IMU open datasets. The positioning accuracy is consistently kept within 5 cm by Multilevel-SLAM algorithm, while there are positioning deviations to different degrees by the contrast methods. Experimental results show that Multilevel-SLAM algorithm effectively improves the effect of trajectory closure at the closing point of the loop, and has a lower positioning error without any significant increment of computation.
[1] Durrant-Whyte H, Bailey T. Simultaneous localization and mapping:Part I[J]. IEEE Robotics & Automation Magazine, 2006, 13(2):99-110. [2] 王任栋,李华,赵凯,等. 基于核密度估计的城市动态密集场景激光雷达定位[J].光学学报,2019,39(5):341-350.Wang R D, Li H, Zhao K, et al. Robust localization based on kernel density estimation in dynamic diverse city scenes using Lidar[J]. Acta Optica Sinica, 2019, 39(5):341-350. [3] Wang Z L, Chen Y, Mei Y, et al. IMU-assisted 2D SLAM method for low-texture and dynamic environments[J]. Applied Sciences, 2018, 8(12). DOI:10.3390/app8122534. [4] Besl P J, 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. [5] Pomerleau F, Colas F, Siegwart R, et al. Comparing ICP variants on real-world data sets[J]. Autonomous Robots, 2013, 34:133-148. [6] Tiar R, Lakrouf M, Azouaoui O. FAST ICP-SLAM for a bi-steerable mobile robot in large environments[C]//IEEE International Workshop of Electronics, Control, Measurement, Signals and Their Application to Mechatronics. Piscataway, USA:IEEE, 2015. DOI:10.1109/ECMSM.2015.7208683. [7] Zheng Z Y, Li Y. LIDAR data registration for unmanned ground vehicle based on improved ICP algorithm[C]//7th International Conference on Intelligent Human-Machine Systems and Cybernetics. Piscataway, USA:IEEE, 2015:554-558. [8] Grisetti G, Stachniss C, Burgard W. Improved techniques for grid mapping with Rao-Blackwellized particle filters[J]. IEEE Transactions on Robotics, 2007, 23(1):34-46. [9] Liu T, Du H W. A camera-IMU system extrinsic parameter calibration method[C]//IEEE 2nd Information Technology, Networking, Electronic and Automation Control Conference. Piscataway, USA:IEEE, 2017:1063-1066. [10] Win N N, Kida K, Ko M, et al. A novel particle filter based SLAM algorithm for lunar navigation and exploration[C]//4th International Conference on Robotics and Automation Engineering. Piscataway, USA:IEEE, 2019:74-78. [11] Ullah S, Song B W, Chen W D. EMoVI-SLAM:Embedded monocular visual inertial SLAM with scale update for large scale mapping and localization[C]//27th IEEE International Symposium on Robot and Human Interactive Communication. Piscataway, USA:IEEE, 2018:880-885. [12] Xu X B, Luo M Z, Tan Z Y, et al. Improved ICP matching algorithm based on laser radar and IMU[C]//5th IEEE International Conference on Cloud Computing and Intelligence Systems. Piscataway, USA:IEEE, 2018:517-520. [13] Hang Y J, Liu J Y, Li R B, et al. Optimization method of MEMS IMU/LADAR integrated navigation system based on compressed-EKF[C]//IEEE/ION Position, Location and Navigation Symposium. Piscataway, USA:IEEE, 2014:115-120. [14] Ji X L, Zuo L, Zhang C H, et al. LLOAM:LiDAR odometry and mapping with loop-closure detection based correction[C]//IEEE International Conference on Mechatronics and Automation. Piscataway, USA:IEEE, 2019:2475-2480. [15] Wang Z Y, Zhang J H, Chen S Y, et al. Robust high accuracy visual-inertial-laser SLAM system[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, USA:IEEE, 2019:6636-6641. [16] Zhen W K, Scherer S. Estimating the localizability in tunnel-like environments using LiDAR and UWB[C]//International Conference on Mechatronics and Automation. Piscataway, USA:IEEE, 2019:4903-4908. [17] Kaijaluoto R, Hyyppa A. Precise indoor localization for mobile laser scanner[J]. The International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, 2015, XL-4/W5:1-6. [18] Hess W, Kohler D, Rapp H, et al. Real-time loop closure in 2D LIDAR SLAM[C]//IEEE International Conference on Robotics and Automation. Piscataway, USA:IEEE, 2016:1271-1278. [19] Chen M X, Yang S W, Yi X D, et al. Real-time 3D mapping using a 2D laser scanner and IMU-aided visual SLAM[C]//IEEE International Conference on Real-time Computing and Robotics. Piscataway, USA:IEEE, 2017:297-302. [20] Geneva P, Maley J, Huang G Q. An efficient Schmidt-EKF for 3D visual-inertial SLAM[C]//IEEE/CVF Conference on Computer Vision and Pattern Recognition. Piscataway, USA:IEEE, 2019:12097-12107. [21] Maset E, Arrigoni F, Fusiello A. Practical and efficient multi-view matching[C]//IEEE International Conference on Computer Vision. Piscataway, USA:IEEE, 2017:4578-4586. [22] Zhang M, Xu X Y, Chen Y M, et al. A lightweight and accurate localization algorithm using multiple inertial measurement units[J]. IEEE Robotics and Automation Letters, 2020, 5(2):1508-1515. [23] Hartley R, Ghaffari M, Eustice R M, et al. Contact-aided invariant extended Kalman filtering for robot state estimation[J]. International Journal of Robotics Research, 2020, 39(4):402-430. [24] Liu W L, Wu S T, Wen Y M, et al. Integrated autonomous relative navigation method based on vision and IMU data fusion[J]. IEEE Access, 2020, 8:51114-51128. [25] Smith R C, Cheeseman P. On the representation and estimation of spatial uncertainty[J]. International Journal of Robotics Research, 1986, 5(4):56-68. [26] Merwe R V D, Wan E A. The square-root unscented Kalman filter for state and parameter estimation[C]//IEEE International Conference on Acoustics, Speech, and Signal Processing. Piscataway, USA:IEEE, 2001:3461-3464. [27] Thrun S. Probabilistic robotics[J]. Communications of the ACM, 2002, 45(3):3-4. [28] Konolige K, Grisetti G, Kümmerle R, et al. Efficient sparse pose adjustment for 2D mapping[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, USA:IEEE, 2010:22-29. [29] Cartographer. Public data:2D Cartographer backpack——Deutsches museum[DB/OL]. (2021-03-20)[2020-05-06]. https://google-cartographer-ros.readthedocs.io/en/latest/data.html. [30] Mason J, Marthi B. An object-based semantic world model for long-term change detection and semantic querying[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, USA:IEEE, 2012:3851-3858.
