Real-time Lidar SLAM Algorithm Based on Distribution Optimal Registration

doi:10.16182/j.issn1004731x.joss.23-0887

Abstract

Abstract:

When scanning the surrounding environment, a lidar will generate some cluttered and sparse point cloud, which will cause excessive distribution fitting errors and correlation distances in the registration process, thus affecting the accuracy of the registration algorithm and the effect of simultaneous localization and mapping (SLAM). To address this problem, a real-time lidar SLAM algorithm based on distribution optimal registration is proposed. An eigenspectrum filter is designed, which takes the normalized minimum eigenvalue as the filtering object to filter out the points that do not match the set distribution in order to reduce the distribution fitting error. Secondly, a point cloud registration loss function is proposed for the compound normalization of the joint covariance matrix and error terms composed of the source and target point clouds to reduce the interference caused by points with excessive correlation distance in the iterative solution process. A SLAM algorithm framework is designed which contains front-end odometry, loop-closure detection, and back-end optimization. It is compatible with pure lidar mapping and lidar/inertial fusion mapping, thus ensuring the accuracy and consistency of the mapping and improving the adaptability of the algorithm. Several sets of experiments are conducted on public datasets. The experimental results show that the proposed algorithm has great advantages in terms of accuracy and speed compared with the existing state-of-the-art SLAM algorithms.

Key words: lidar, point cloud registration, generalized iterative closest point, distribution optimal, simultaneous localization and mapping(SLAM)

CLC Number:

TP242.6

Li Weigang, Yu Chuxiang, Wang Yongqiang, Zou Shaofeng. Real-time Lidar SLAM Algorithm Based on Distribution Optimal Registration[J]. Journal of System Simulation, 2024, 36(11): 2566-2577.

Figures/Tables 12

Fig. 1

Fig. 2

Fig. 3

Table 1

Table 2

Table 3

Table 4

Fig. 4

Fig. 5

Fig. 6

Fig. 7

Fig. 8

References 32

1	Durrant-Whyte H, Bailey T. Simultaneous Localization and Mapping: Part I[J]. IEEE Robotics & Automation Magazine, 2006, 13(2): 99-110.
2	Geiger Andreas, Lenz Philip, Urtasun R. Are We Ready for Autonomous Driving? The KITTI Vision Benchmark Suite[C]//2012 IEEE Conference on Computer Vision and Pattern Recognition. Piscataway: IEEE, 2012: 3354-3361.
3	焦嵩鸣, 姚鑫, 丁辉, 等. 适应于环境空间变化的激光雷达SLAM建图方法[J]. 系统仿真学报, 2023, 35(8): 1788-1798.
	Jiao Songming, Yao Xin, Ding Hui, et al. Lidar SLAM Mapping Method Adapted to Environmental Spatial Changes[J]. Journal of System Simulation, 2023, 35(8): 1788-1798.
4	Brock E, Huang Chengxuan, Wu Dalei, et al. Lidar-based Real-time Mapping for Digital Twin Development[C]//2021 IEEE International Conference on Multimedia and Expo (ICME). Piscataway: IEEE, 2021: 1-6.
5	Singandhupe A, La H M. A Review of SLAM Techniques and Security in Autonomous Driving[C]//2019 Third IEEE International Conference on Robotic Computing (IRC). Piscataway: IEEE, 2019: 602-607.
6	李维刚, 钟正, 王永强, 等. 基于时间距离-熵减策略的同步定位与地图构建算法[J]. 信息与控制, 2023, 52(5): 660-668, 688.
	Li Weigang, Zhong Zheng, Wang Yongqiang, et al. Synchronous Localization and Mapping Algorithm Based on Time Distance-entropy Reduction Strategy[J]. Information and Control, 2023, 52(5): 660-668, 688.
7	黄鹤, 佟国峰, 夏亮, 等. SLAM技术及其在测绘领域中的应用[J]. 测绘通报, 2018(3): 18-24.
	Huang He, Tong Guofeng, Xia Liang, et al. SLAM Technology and Its Application in Surveying and Mapping[J]. Bulletin of Surveying and Mapping, 2018(3): 18-24.
8	Cadena Cesar, Carlone L, Carrillo Henry, et al. Past, Present, and Future of Simultaneous Localization and Mapping: Toward the Robust-perception Age[J]. IEEE Transactions on Robotics, 2016, 32(6): 1309-1332.
9	Zhang Ji, Singh S. Low-drift and Real-time Lidar Odometry and Mapping[J]. Autonomous Robots, 2017, 41(2): 401-416.
10	Shan Tixiao, Englot B. LeGO-LOAM: Lightweight and Ground-optimized Lidar Odometry and Mapping on Variable Terrain[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Piscataway: IEEE, 2018: 4758-4765.
11	Wang Han, Wang Chen, Chen Chunlin, et al. F-LOAM: Fast LiDAR Odometry and Mapping[C]//2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Piscataway: IEEE, 2021: 4390-4396.
12	Cedric Le Gentil, Vidal-Calleja Teresa, Huang Shoudong. IN2LAAMA: Inertial Lidar Localization Autocalibration and Mapping[J]. IEEE Transactions on Robotics, 2021, 37(1): 275-290.
13	Shan Tixiao, Englot B, Meyers D, et al. LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping[C]//2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Piscataway: IEEE, 2020: 5135-5142.
14	Pan Yue, Xiao Pengchuan, He Yujie, et al. MULLS: Versatile LiDAR SLAM via Multi-metric Linear Least Square[C]//2021 IEEE International Conference on Robotics and Automation (ICRA). Piscataway: IEEE, 2021: 11633-11640.
15	Zhou Pengwei, Guo Xuexun, Pei Xiaofei, et al. T-LOAM: Truncated Least Squares LiDAR-only Odometry and Mapping in Real Time[J]. IEEE Transactions on Geoscience and Remote Sensing, 2022, 60: 1-13.
16	Dellenbach Pierre, Jean Emmanuel Deschaud, Jacquet Bastien, et al. CT-ICP: Real-time Elastic LiDAR Odometry with Loop Closure[C]//2022 International Conference on Robotics and Automation (ICRA). Piscataway: IEEE, 2022: 5580-5586.
17	Chang Yun, Ebadi K, Denniston C E, et al. LAMP 2.0: A Robust Multi-robot SLAM System for Operation in Challenging Large-scale Underground Environments[J]. IEEE Robotics and Automation Letters, 2022, 7(4): 9175-9182.
18	Segal A V, Haehnel D, Thrun S. Generalized-ICP[C]//Proceedings of Robotics: Science and Systems. [S.l.:s.n.], 2009: 21.
19	Koide Kenji, Yokozuka Masashi, Oishi Shuji, et al. Voxelized GICP for Fast and Accurate 3D Point Cloud Registration[C]//2021 IEEE International Conference on Robotics and Automation (ICRA). Piscataway: IEEE, 2021: 11054-11059.
20	Vlaminck Michiel, Luong Hiep, Philips Wilfried. Surface-based GICP[C]//2018 15th Conference on Computer and Robot Vision (CRV). Piscataway: IEEE, 2018: 262-268.
21	Servos James, Waslander Steven L. Multi-channel Generalized-ICP: A Robust Framework for Multi-channel Scan Registration[J]. Robotics and Autonomous Systems, 2017, 87: 247-257.
22	Ren Zhuli, Wang Liguan, Bi Lin. Robust GICP-Based 3D LiDAR SLAM for Underground Mining Environment[J]. Sensors, 2019, 19(13): 2915.
23	Chen K, Lopez B T, Agha-mohammadi A A, et al. Direct LiDAR Odometry: Fast Localization with Dense Point Clouds[J]. IEEE Robotics and Automation Letters, 2022, 7(2): 2000-2007.
24	Yokozuka Masashi, Koide Kenji, Oishi Shuji, et al. LiTAMIN: LiDAR-based Tracking and Mapping by Stabilized ICP for Geometry Approximation with Normal Distributions[C]//2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Piscataway: IEEE, 2020: 5143-5150.
25	Yokozuka Masashi, Koide Kenji, Oishi Shuji, et al. LiTAMIN2: Ultra Light LiDAR-based SLAM Using Geometric Approximation Applied with KL-divergence[C]//2021 IEEE International Conference on Robotics and Automation (ICRA). Piscataway: IEEE, 2021: 11619-11625.
26	Wang Jikai, Xu Meng, Foroughi Farzin, et al. FasterGICP: Acceptance-rejection Sampling Based 3D Lidar Odometry[J]. IEEE Robotics and Automation Letters, 2022, 7(1): 255-262.
27	Blanco J L, Rai P K. Nanoflann: A C++ Header-only Fork of FLANN, a Library for Nearest Neighbor (NN) with KD-trees[EB/OL]. [2023-01-18]. .
28	Xu Wei, Cai Yixi, He Dongjiao, et al. FAST-LIO2: Fast Direct LiDAR-inertial Odometry[J]. IEEE Transactions on Robotics, 2022, 38(4): 2053-2073.
29	Kim Giseop, Kim Ayoung. Scan Context: Egocentric Spatial Descriptor for Place Recognition Within 3D Point Cloud Map[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Piscataway: IEEE, 2018: 4802-4809.
30	Dellaert F, Kaess M. Factor Graphs for Robot Perception[M]. Hanover: Now Publishers Inc., 2017: 1-139.
31	Kim Giseop, Yeong Sang Park, Cho Younghun, et al. MulRan: Multimodal Range Dataset for Urban Place Recognition[C]//2020 IEEE International Conference on Robotics and Automation (ICRA). Piscataway: IEEE, 2020: 6246-6253.
32	Liu Xiyuan, Liu Zheng, Kong Fanze, et al. Large-scale LiDAR Consistent Mapping Using Hierarchical LiDAR Bundle Adjustment[J]. IEEE Robotics and Automation Letters, 2023, 8(3): 1523-1530.

数据集	RMSE/m			FPS/(帧/s)
数据集	Fast-GICP	Faster-GICP	DO-GICP(Proposed)	Fast-GICP	Faster-GICP	DO-GICP(Proposed)
Avg	6.26	6.34	6.08	66.62	38.31	72.31
KITTI-00	6.96	10.14	6.97	81.23	45.18	89.49
KITTI-01	23.67	24.21	23.74	39.38	31.73	46.26
KITTI-02	17.69	15.24	16.62	78.67	43.52	82.13
KITTI-03	2.74	1.91	2.66	54.58	34.54	60.52
KITTI-04	0.93	0.53	0.83	49.26	30.52	59.71
KITTI-05	3.90	4.04	3.46	76.86	41.79	83.78
KITTI-06	1.42	1.27	1.41	51.23	31.72	57.79
KITTI-07	0.76	1.04	0.74	82.3	46.58	89.39
KITTI-08	6.46	6.39	6.25	63.98	36.84	67.73
KITTI-09	2.26	2.76	2.20	64.10	34.93	66.37
KITTI-10	2.05	2.28	1.97	91.24	43.93	92.30

数据集	Fast-GICP^[9]	A (Proposed)	B (Proposed)	DO-GICP (Proposed)
Avg	6.26	6.22	6.23	6.08
KITTI-00	6.96	6.91	6.97	6.97
KITTI-01	23.67	23.58	23.66	23.74
KITTI-02	17.69	17.61	17.72	16.62
KITTI-03	2.74	2.69	2.75	2.66
KITTI-04	0.93	0.89	0.93	0.83
KITTI-05	3.90	3.91	3.55	3.46
KITTI-06	1.42	1.42	1.41	1.41
KITTI-07	0.76	0.76	0.74	0.74
KITTI-08	6.46	6.42	6.51	6.25
KITTI-09	2.26	2.25	2.29	2.20
KITTI-10	2.05	2.00	2.02	1.97

数据集	LOAM^[9]	LEGO-LOAM^[10]	LEGO- LOAM(SC)	LITAMIN2^[25]	Proposed
KITTI-00	19.4	1.9	1.8	5.8	1.7
KITTI-01	21	285.7	285.4	15.9	1.7
KITTI-02	111.6	30.0	28.4	10.7	5.2
KITTI-03	1.0	1.1	1.1	0.8	1.1
KITTI-04	0.5	0.5	0.5	0.7	0.5
KITTI-05	4.6	1.1	1.1	2.4	1.1
KITTI-06	1.1	0.9	0.9	0.9	0.9
KITTI-07	1.3	0.5	0.5	0.6	0.4
KITTI-08	6.7	4.7	5.0	2.5	3.3
KITTI-09	5.3	7.4	6.7	2.1	1.8
KITTI-10	1.9	2.5	2.6	1.0	1.8
Avg	15.8	30.5	30.3	3.9	3.3
LOOP	×	√	√	×	√

数据集		LIO- SAM^[13]	LIO- SAM(SC)	LEGO- LOAM(SC)	LS- BA^[32]	Proposed
Kaist-01	Std	1.58	1.51	2.68	—	0.74
Kaist-01	RMSE	3.73	3.55	5.27	3.36	2.17
Kaist-02	Std	4.52	1.95	3.10	—	0.93
Kaist-02	RMSE	7.2	3.76	5.38	3.75	2.65
Dcc-01	Std	15.54	1.93	2.25	—	1.70
Dcc-01	RMSE	34.82	5.75	6.69	5.20	5.09
Dcc-02	Std	12.09	1.32	1.62	—	1.10
Dcc-02	RMSE	27.22	3.55	4.44	3.22	3.05
RS-01	Std	48.91	4.44	12.11	—	4.08
RS-01	RMSE	87.30	9.62	24.57	8.92	6.69
RS-02	Std	56.13	6.51	15.52	—	2.71
RS-02	RMSE	114.18	13.59	32.37	7.94	5.66
LOOP		√		√	√	√

[1]	Wang Haichao, Yin Yong, Jing Qianfeng, Cong Lin. Estimation of the Berthing Parameter of Unmanned Surface Vessels Based on 3D LiDAR [J]. Journal of System Simulation, 2024, 36(8): 1737-1748.
[2]	Yuan Zhang, Haoyu Han, Xie Han, Jiaxu Fu. Point Cloud Registration Method Based on Improved Covariance Matrix Descriptor [J]. Journal of System Simulation, 2023, 35(5): 979-986.
[3]	Xuesong Mao, Runlong Lei, Shaowei Huang, Xuetao Mao. Simulation on Mutual Interference of Laser Radar in Road Environments [J]. Journal of System Simulation, 2022, 34(4): 891-900.
[4]	Sun Shuifa, Li Zhun, Xia Kun, Shi Yunfei, Yang Jiquan, Dong Fangmin. Variable Scale Point Cloud Registration Algorithm [J]. Journal of System Simulation, 2018, 30(7): 2465-2474.
[5]	Zhang Yanguo, Li Qing. Multi-frame Fusion Method for Point Cloud of LiDAR Based on IMU [J]. Journal of System Simulation, 2018, 30(11): 4334-4339.
[6]	Zhang Hang, Chen Bin, Xue Hanzhang, Zhu Zhengqiu, Wang Rongxiao. Research on 3D Scene Modeling Based on UAV and LIDAR [J]. Journal of System Simulation, 2017, 29(9): 1914-1920.
[7]	Song Liang, Li Zhi, Ma Xingrui. LIDAR-Based Relative Position and Attitude Filtering for Unknown Targets [J]. Journal of System Simulation, 2017, 29(5): 1103-1111.
[8]	Xia Wenze, Han Shaokun, Cao Jingya. Simulation Technology Research on Streak Tube Imaging Lidar [J]. Journal of System Simulation, 2016, 28(12): 3027-3033.
[9]	Fu Chengqun, Lü Xiuyuan, Wang Yong, Wang Huaixiao. Simulation Research on Airborne Lidar Bathymetry System [J]. Journal of System Simulation, 2015, 27(5): 1038-1043.