Ming LU, Kedi ZHONG, Jun ZHANG, Yuansheng LIU, Jiansuo YANG

A factor graph optimization mapping based on normal distributions transform

This paper aims to achieve highly accurate mapping results and real time pose estimation of autonomous vehicle by using the normal distribution transform (NDT) algoritm. A factor graph optimization algorithm (FGO-NDT) is proposed to address the poor real-time performance and pose drift errors of the NDT algorithm. Smooth point cloud data are obtained by multisensor calibration and data preprocessing. NDT registration is then used for lidar odometry and feature matching. The global navigation satellite system (GNSS) data and loop detection results are added to the factor graph framework as the pose constraint factors to optimize the pose trajectory and eliminate the pose drift error generated during mapping. In addition, a sliding window method is used for map registration to extract a local map to shorten the map loading time. Thus, the real-time performance of creating point cloud maps of large scenes is significantly improved. Several experiments are conducted in different environments to verify the accuracy and performance of the FGO-NDT. The experimental results demonstrate that the proposed method can eliminate the pose estimation error caused by drift, improve the local structure, and reduce and root mean square error.

PDF

___

[1] Stoyanov T, Saarinen J, Andreasson H. Normal distributions transform occupancy map fusion: Simultaneous mapping and tracking in large scale dynamic environments. IEEE/RSJ International Conference on Intelligent Robots and Systems 2013; 4702-4708.
[2] Saarinen JP, Andreasson H, Stoyanov T. 3D normal distributions transform occupancy maps: An efficient representation for mapping in dynamic environments. The International Journal of Robotics Research 2013; 32 (14): 1627-1644.
[3] Shi X, Peng J, Li J. The iterative closest point registration algorithm based on the normal distribution transformation. Procedia Computer Science 2019; 147: 181-190.
[4] Lee J, Lee K, Yoo A. Design and Implementation of Edge-Fog-Cloud System through HD Map Generation from LiDAR Data of Autonomous Vehicles. Electronics 2020; 12 (9): 2084.
[5] C Ulas H, Temeltas. A 3D Scan Matching Method BasedOn Multi-Layered Normal Distribution Transform. IFAC Proceedings Volumes 2011; 44 (1): 11602–11607.
[6] Schulz C, Zell A. Real-Time Graph-Based SLAM with Occupancy Normal Distributions Transforms. IEEE International Conference on Robotics and Automation (ICRA) 2020: 3106-3111.
[7] Zaganidis A, Zerntev A, Duckett T. Semantically assisted loop closure in SLAM using NDT histograms. IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) 2019; 4562-4568.
[8] Zhao C, Zhou Z, Adolfsson D. Large-Scale 3D Point Cloud Localisation using the Normal Distribution Transform Representation. IEEE 2021; 4.
[9] Hong H, Yu H, Lee BH. Regeneration of normal distributions transform for target lattice based on fusion of truncated gaussian components. IEEE Robotics and Automation Letters 2019; 4 (2): 684-691.
[10] Wen W, Hsu LT, Zhang G. Performance analysis of NDT-based graph SLAM for autonomous vehicle in diverse typical driving scenarios of Hong Kong. Sensors 2018; 18 (11): 3928.
[11] Zhang J, Singh S. Low-drift and real-time lidar odometry and mapping. Autonomous Robots 2017; 41 (2): 401-416.
[12] Shan T, Englot B. Lego-loam: Lightweight and ground-optimized lidar odometry and mapping on variable terrain. IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) 2018; 4758-4765.
[13] Shan T, Englot B, Meyers D, Wang W, Ratti C et al. Lio-sam: Tightly-coupled lidar inertial odometry via smoothing and mapping. IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) 2020; 5135—5142.
[14] Wang R, Xu Y, Sotelo MA. A robust registration method for autonomous driving pose estimation in urban dynamic environment using LiDAR. Electronics 2019; 8 (11): 43.
[15] Yokozuka M, Koide K, Oishi S. LiTAMIN2: Ultra Light LiDAR-based SLAM using Geometric Approximation applied with KL-Divergence. arXiv preprint arXiv:2103.00784, 2021,5.
[16] Lee HC, Park SK, Choi JS. PSO-FastSLAM: An improved FastSLAM framework using particle swarm optimization. IEEE International Conference on Systems 2009; 2763-2768.
[17] Lei X, Feng B, Wang G. A novel fastslam framework based on 2d lidar for autonomous mobile robot. Electronics 2020; 9 (4): 695.
[18] Bailey T, Nieto J, Guivant J. Consistency of the EKF-SLAM algorithm. IEEE/RSJ International Conference on Intelligent Robots and Systems 2006; 3562-3568.
[19] Tang J, Chen Y, Niu X. LiDAR scan matching aided inertial navigation system in GNSS-denied environments. Sensors 2015; 15 (7): 16710-16728.
[20] Niu X, Yu T, Tang J. An online solution of LiDAR scan matching aided inertial navigation system for indoor mobile mapping. Mobile Information Systems 2017; 2017.
[21] Lynen S, Achtelik MW, Weiss S. A robust and modular multi-sensor fusion approach applied to mav navigation. IEEE/RSJ international conference on intelligent robots and systems 2013; 3923-3929.
[22] Zhou L, Wang Y, Liu Y. A Tightly-Coupled Positioning System of Online Calibrated RGB-D Camera and Wheel Odometry Based on SE (2) Plane Constraints. Electronics 2021; 10 (8): 970.
[23] Li K, Li M, Hanebeck UD. Towards high-performance solid-state-lidar-inertial odometry and mapping. IEEE Robotics and Automation Letters 2021; 6 (3): 5167-5174.
[24] Yang JC, Lin CJ, You BY. RTLIO: real-time LiDAR-inertial odometry and mapping for UAVs. Sensors 2021; 21 (12): 3955.
[25] Zuo X, Geneva P, Lee W. LIC-Fusion: LiDAR-inertial-camera odometry. IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) 2019; 5848-5854.
[26] Zuo X, Yang Y, Geneva P. LIC-Fusion 2.0: LiDAR-inertial-camera odometry with sliding-Window plane-feature tracking. IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) 2020; 5112-5119.
[27] D Wisth, M Camurri, S Das, M Fallon. Unified multi-modal landmark tracking for tightly coupled LiDAR-visualinertial odometry. IEEE Robotics and Automation Letters 2021; 6 (2): 1004-1011.
[28] Stott PH.The UTM grid reference system. IA. The Journal of the Society for Industrial Archeology 1977; 3: 1-14.
[29] Quigley M, Conley K, Gerkey B. ROS: an open-source robot operating system. ICRA workshop on open source software 2009; 3 (3.2): 5.
[30] A Geiger, P Lenz, R Urtasun. Are we ready for autonomous driving? The KITTI vision benchmark suite. IEEE Conference on Computer Vision and Pattern Recognition 2012; 3354-3361.