基于分布优化配准的实时激光SLAM算法

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

摘要/Abstract

摘要：

激光雷达在扫描周围环境时会产生部分杂乱且稀疏的点云，该类点云会在配准过程中产生过大的分布拟合误差和关联距离，进而影响配准算法的精度及同步定位与建图（simultaneous localization and mapping，SLAM）的效果。针对以上问题，提出了一种基于分布优化配准的实时激光SLAM算法。设计了一个特征谱滤波器，该滤波器以归一化最小特征值为滤波对象，去除不符合设定分布的点云以减小分布拟合误差；提出了一个点云配准损失函数，对源点云和目标点云构成的联合协方差矩阵和误差项进行复合归一化，以减小关联距离过大的点在迭代求解过程中的干扰；设计了一个SLAM算法框架，该框架包含前端里程计、回环检测和后端优化等环节，兼容纯激光建图和激光/惯性融合建图，进而保证建图的精确性和一致性，并提高了算法的适应性。在公开数据集上进行了多组实验，实验结果表明，相较于现有SLAM算法，所提算法在精度和速度指标方面均具有较大优势。

关键词: 激光雷达, 点云配准, 广义迭代最近邻, 分布优化, 同步定位与建图

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)

中图分类号:

TP242.6

李维刚,余楚翔,王永强等 . 基于分布优化配准的实时激光SLAM算法[J]. 系统仿真学报, 2024, 36(11): 2566-2577.

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

图/表 12

图1

图2

图3

表1

表2

表3

表4

图4

图5

图6

图7

图8

参考文献 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]	王海超, 尹勇, 景乾峰, 丛琳. 基于3D激光雷达的水面无人艇靠泊参数估计[J]. 系统仿真学报, 2024, 36(8): 1737-1748.
[2]	张元, 韩浩宇, 韩燮, 付嘉旭. 基于改进的协方差矩阵描述子的点云配准方法[J]. 系统仿真学报, 2023, 35(5): 979-986.
[3]	毛雪松, 雷闰龙, 黄少伟, 毛雪涛. 道路环境中激光雷达互干扰的仿真研究[J]. 系统仿真学报, 2022, 34(4): 891-900.
[4]	张艳国, 李擎. 基于惯性测量单元的激光雷达点云融合方法[J]. 系统仿真学报, 2018, 30(11): 4334-4339.
[5]	张航, 陈彬, 薛含章, 朱正秋, 王戎骁. 基于无人机和LIDAR的三维场景建模研究[J]. 系统仿真学报, 2017, 29(9): 1914-1920.
[6]	胡凌燕, 曹禄, 熊鹏文, 辛勇, 谢泽坤. 基于RGB-D图像的三维同步定位与建图研究[J]. 系统仿真学报, 2017, 29(11): 2840-2846.
[7]	夏文泽, 韩绍坤, 曹京亚. 条纹管成像激光雷达仿真技术研究[J]. 系统仿真学报, 2016, 28(12): 3027-3033.
[8]	付成群, 律秀原, 王勇, 王怀晓. 机载激光雷达海洋探测的模拟研究[J]. 系统仿真学报, 2015, 27(5): 1038-1043.
[9]	余文利, 周明全, 税午阳, 武仲科. 基于曲率的点云自动配准方法[J]. 系统仿真学报, 2015, 27(10): 2374-2379.