基于多传感器融合的紧耦合SLAM系统

doi:10.19562/j.chinasae.qcgc.2022.03.006

摘要/Abstract

摘要：

同时建图与定位（SLAM）是自动驾驶功能重要的组成部分，现有算法以激光或视觉惯性里程计为主，未充分利用多模态传感器各自的优势，对特征缺失的场景鲁棒性不足。针对此问题，本文中提出了一种采用激光雷达、摄像头和惯性测量单元（IMU）的多传感器紧耦合SLAM系统。首先它改善了激光雷达点云特征提取和平面拟合的方案，提升了利用点云对视觉特征点深度信息优化的效率和精度。其次提出的紧耦合状态估计框架通过在视觉惯性系统中直接添加激光雷达里程计约束，在不增加算法复杂度的前提下提升了系统的稳定性和精度。最后由粗到精的视觉-激光雷达耦合回环框架进一步降低了系统的长时累计漂移。在开源数据集KITTI上进行大量测试验证的结果表明，与其它常用的算法相比，所提出的算法具有较高的精度和环境适应能力。另外在基于自主搭建的自动驾驶汽车测试平台进行的实车试验还证明本算法可适应长时间大场景的工作环境。

关键词: 自动驾驶, 状态估计, 同时建图与定位, 多传感器融合, 回环检测

Abstract:

Simultaneous localization and mapping （SLAM） is an essential part of autonomous vehicles. Its existing algorithms are mainly based on lidar or visual-inertial odometer， which do not make full use of the respective advantages of multi-mode sensors， leading to insufficient robustness to featureless scenes. In view of these， a multi-sensor tightly coupled SLAM system using lidar， camera and inertial measurement unit is proposed in this paper. Firstly， the system improves the schemes of the feature extraction of lidar point cloud and plane fitting and enhances the efficiency and accuracy of the depth information optimization of visual feature points by using lidar point cloud. Secondly， the tightly coupled state estimation framework proposed directly adds lidar odometer constraints onto visual inertial system， so enhancing the system stability and accuracy without increasing the complexity of algorithm. Finally， the coarse-to-fine visual-lidar coupled loop framework further reduces the long-term cumulative drift of the system. The results of massive tests for validation on the open-source dataset KITTI show that compared with other commonly used algorithms， the proposed algorithm achieves higher accuracy and environmental adaptability. In addition， the real vehicle test on the self-built autonomous vehicle test platform also demonstrates its adaptability to the long-time and large scene environment.

Key words: autonomous vehicles, state estimation, SLAM, multi-sensor-fusion, closed-loop detection

蔡英凤,陆子恒,李祎承,陈龙,王海. 基于多传感器融合的紧耦合SLAM系统[J]. 汽车工程, 2022, 44(3): 350-361.

Yingfeng Cai,Ziheng Lu,Yicheng Li,Long Chen,Hai Wang. Tightly Coupled SLAM System Based on Multi-Sensor Fusion[J]. Automotive Engineering, 2022, 44(3): 350-361.

图/表 15

图1

图2

图3

图4

图5

图6

图7

表1

图8

图9

图10

表2

图11

表3

图12

参考文献 23

1	BAILEY T， DURRANT-WHYTE H. Simultaneous localization and mapping （SLAM）： part II ［J］. IEEE Robotics & Automation Magazine， 2006， 13（3）： 108-117.
2	王海，李洋，蔡英凤，等. 基于激光雷达的3D实时车辆跟踪［J］. 汽车工程， 2021， 43（7）： 1013-1021.
	WANG H， LI Y， CAI Y， et al. 3D real⁃time vehicle tracking based on lidar［J］. Automotive Engineering， 2021， 43（7）： 1013-1021.
3	娄新雨，王海，蔡英凤，等. 采用64线激光雷达的实时道路障碍物检测与分类算法的研究［J］. 汽车工程， 2019， 41（7）： 779-784.
	LOU Xinyu， WANG Hai， CAI Yingfeng， et al. A research on an algorithm for real-time detection and classification of road obstacle by using 64-line lidar［J］. Automotive Engineering， 2019， 41（7）： 779-784.
4	MUR-ARTAL R， MONTIEL J M M， TARDOS J D. ORB-SLAM： a versatile and accurate monocular SLAM system ［J］. IEEE Transactions on Robotics， 2015， 31（5）： 1147-1163.
5	MUR-ARTAL R， TARDóS J D. Orb-slam2： an open-source slam system for monocular， stereo， and rgb-d cameras ［J］. IEEE Transactions on Robotics， 2017， 33（5）： 1255-1262.
6	LEVINSON J， ASKELAND J， BECKER J， et al. Towards fully autonomous driving： systems and algorithms［C］. Proceedings of the 2011 IEEE Intelligent Vehicles Symposium （IV）.
7	ZHANG J， SINGH S. Low-drift and real-time lidar odometry and mapping ［J］. Autonomous Robots， 2017， 41（2）： 401-416.
8	李兴佳，李建芬，朱敏，等. 基于无迹卡尔曼滤波的定位融合与校验算法研究［J］. 汽车工程， 2021， 43（6）： 825-832.
	LI Xingjia， LI Jianfen，ZHU Min， et al. Research on positioning fusion and verification algorithm based on UKF［J］. Automotive Engineering， 2021， 43（6）： 825-832.
9	YE H， CHEN Y， LIU M. Tightly coupled 3d lidar inertial odometry and mapping［C］. 2019 International Conference on Robotics and Automation （ICRA）. IEEE， 2019： 3144-3150.
10	MOURIKIS A I， ROUMELIOTIS S I. A multi-state constraint Kalman filter for vision-aided inertial navigation［C］. Proceedings 2007 IEEE International Conference on Robotics and Automation. IEEE， 2007： 3565-3572.
11	SUN K， MOHTA K， PFROMMER B， et al. Robust stereo visual inertial odometry for fast autonomous flight ［J］. IEEE Robotics and Automation Letters， 2018， 3（2）： 965-972.
12	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.
13	LEUTENEGGER S， LYNEN S， BOSSE M， et al. Keyframe-based visual–inertial odometry using nonlinear optimization ［J］. The International Journal of Robotics Research， 2015， 34（3）： 314-334.
14	LOELIGER HA， DAUWELS J， HU J， et al. The factor graph approach to model-based signal processing ［J］. Proceedings of the IEEE， 2007， 95（6）： 1295-1322.
15	QIN T， LI P， SHEN S. Vins-mono： a robust and versatile monocular visual-inertial state estimator ［J］. IEEE Transactions on Robotics， 2018， 34（4）： 1004-1020.
16	SHAN T， 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）. IEEE， 2020： 5135-5142.
17	FORSTER C， CARLONE L， DELLAERT F， et al. IMU preintegration on manifold for efficient visual-inertial maximum-a-posteriori estimation［C］. Georgia Institute of Technology， 2015.
18	FORSTER C， CARLONE L， DELLAERT F， et al. On-manifold preintegration for real-time visual-inertial odometry ［J］. IEEE Transactions on Robotics， 2016， 33（1）： 1-21.
19	GRAETER J， WILCZYNSKI A， LAUER M. Limo： lidar-monocular visual odometry［C］. 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）. IEEE， 2018： 7872-7879.
20	SHAO W， VIJAYARANGAN S， LI C， et al. Stereo visual inertial lidar simultaneous localization and mapping［C］. 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）. IEEE， 2019： 370-377.
21	SHAN T， ENGLOT B， RATTI C， et al. LVI-SAM： tightly-coupled lidar-visual-inertial odometry via smoothing and mapping ［J］. arXiv preprint arXiv：， 2021.
22	GáLVEZ-LóPEZ D， TARDOS J D. Bags of binary words for fast place recognition in image sequences ［J］. IEEE Transactions on Robotics， 2012， 28（5）： 1188-9711.
23	GEIGER A， LENZ P， STILLER C， et al. Vision meets robotics： the KITTI dataset ［J］. The International Journal of Robotics Research， 2013， 32（11）： 1231-1237.

序列	指标
序列	帧数	时长/s	长度/m	最大速度/（m·s^-1）
序列0	4 544	471	3 682	12.9
序列5	2 762	288	2 205	11.1
序列8	5 177	536	3 213	12.5

方法	轨迹距离/m	数据集序列（RPE 平移（%）/ RPE 旋转（（°）·100 m^-1））								均值
方法	轨迹距离/m	KITTI 00 城镇	KITTI 01 高速	KITTI 05 郊区	KITTI 06 城镇	KITTI 07 城镇	KITTI 08 城镇	KITTI 09 城镇	KITTI 10 郊区	均值
本文方法	100	1.06/0.60	1.28/0.65	0.67/0.46	0.68/0.75	0.69/0.48	1.24/0.55	0.91/0.49	0.83/0.68	0.92/0.49
	200	0.97/0.57	1.26/0.62	0.80/0.40	0.59/0.62	0.74/0.27	1.15/0.43	1.13/0.48	0.67/0.56
	300	0.76/0.43	1.00/0.79	0.99/0.35	0.74/0.50	0.79/0.22	1.19/0.40	1.19/0.39	1.12/0.44
	400	0.70/0.22	1.02/0.65	0.67/0.27	0.41/0.48		1.24/0.35	1.17/0.39	0.94/0.69
LIMO	100	1.11/0.58	2.31/0.76	1.07/1.18	3.01/0.68	1.40/1.04	1.68/1.02	1.26/1.29	1.02/0.77	1.57/0.71
	200	1.18/0.45	2.57/0.72	1.05/0.87	2.50/0.44	1.31/0.53	1.48/0.85	1.62/1.15	0.91/0.58
	300	1.09/0.34	2.91/0.63	1.17/0.59	1.87/0.28	1.45/0.56	1.47/0.97	1.66/0.90	1.07/0.91
	400	1.19/0.36	3.08/0.61	1.04/0.31	0.81/0.30		1.17/0.52	2.26/0.86	0.99/0.90
LIO-SAM	100	1.51/0.90	1.57/1.39	0.99/0.94	0.94/0.67	1.37/0.53	1.63/1.01	1.05/0.92	0.92/0.90	1.35/0.74
	200	1.22/0.59	2.08/1.33	0.97/0.63	0.94/0.65	1.25/0.58	1.51/0.80	1.27/0.78	0.81/0.74
	300	1.20/0.44	2.48/1.21	1.06/0.53	2.11/0.65	1.17/0.53	1.41/0.64	1.29/0.65	1.19/0.65
	400	1.00/0.45	2.90/1.07	0.75/0.25	1.50/0.55		1.34/0.48	1.46/0.65	1.11/0.82
VINS-Mono	100	3.19/0.74	25.85/1.25		5.25/0.53	5.87/1.47	11.55/0.98	7.46/0.58	10.73/0.79	9.96/0.64
	200	2.84/0.55	31.41/1.10		4.09/0.37	4.80/0.14	10.68/0.76	7.38/0.60	9.32/0.50
	300	2.37/0.41	30.38/1.01		2.58/0.28	5.68/1.10	8.99/0.55	6.17/0.43	8.06/0.50
	400	1.86/0.33	39.54/0.89		1.24/0.32		7.54/0.33	7.10/0.41	7.12/0.37
LOAM	100	1.69/1.04	2.25/1.24	1.16/1.19	0.99/0.86	1.39/0.50	11.55/0.98	1.56/1.62	0.91/1.96	2.88/1.03
	200	1.52/0.61	2.85/0.18	1.12/1.02	1.16/0.85	1.70/0.54	10.68/0.76	1.57/1.98	1.36/1.56
	300	1.54/0.46	3.43/1.01	1.10/1.13	1.68/0.68	2.07/0.59	8.99/0.55	3.98/2.05	1.65/1.68
	400	1.42/0.50	3.86/0.85	1.45/0.98	1.38/0.70		7.54/0.33	4.27/1.95	1.45/1.44

[1]	付新科,蔡英凤,陈龙,王海,刘擎超. 不确定性环境下的自动驾驶汽车行为决策方法[J]. 汽车工程, 2024, 46(2): 211-221.
[2]	赵晓聪,房世玉,李子睿,孙剑. 社会性驾驶交互关键效用析取与应用[J]. 汽车工程, 2024, 46(2): 230-240.
[3]	马艳丽, 秦钦, 董方琦, 娄艺苧. 基于风险场的不同认知次任务下接管风险评估模型[J]. 汽车工程, 2024, 46(1): 9-17.
[4]	刘卫国,项志宇,刘伟平,齐道新,王子旭. 基于分布式强化学习的车辆控制算法研究[J]. 汽车工程, 2023, 45(9): 1637-1645.
[5]	王明,唐小林,杨凯,李国法,胡晓松. 考虑预测风险的自动驾驶车辆运动规划方法[J]. 汽车工程, 2023, 45(8): 1362-1372.
[6]	朱向雷,吴志新,张宇飞,赵帅,李克秋,孙博华. 基于场景降维及采样方法的场景库优化方法研究[J]. 汽车工程, 2023, 45(8): 1408-1416.
[7]	吴新政,邢星宇,刘力豪,沈勇,陈君毅. 基于错误注入的决策规划系统抗扰性测试与分析[J]. 汽车工程, 2023, 45(8): 1428-1437.
[8]	高锋,冯德福,胡秋霞. 面向NMPC运动规划系统的数值优化加速技术[J]. 汽车工程, 2023, 45(8): 1438-1447.
[9]	芦涛,金馨,廖毅霏,黄圣杰,杨依琳,谢国涛,秦晓辉. 基于雅克比域零空间边缘化的视觉SLAM[J]. 汽车工程, 2023, 45(8): 1457-1467.
[10]	伍文广,田双岳,张志勇,张斌. 非铺装道路凹凸不平特征语义分割方法研究[J]. 汽车工程, 2023, 45(8): 1468-1478.
[11]	林程, 汪博文, 吕沛原, 宫新乐, 于潇. 面向变曲率道路的自动驾驶汽车换道博弈运动规划与协同控制研究[J]. 汽车工程, 2023, 45(7): 1099-1111.
[12]	赵东宇, 赵树恩. 基于级联YOLOv7的自动驾驶三维目标检测[J]. 汽车工程, 2023, 45(7): 1112-1122.
[13]	李军, 周伟, 唐爽. 基于自适应拟合的智能车换道避障轨迹规划[J]. 汽车工程, 2023, 45(7): 1174-1183.
[14]	孙晓强, 王玉麟, 胡伟伟, 蔡英凤, 陈龙, Wong Pak Kin. 基于轮胎分段仿射辨识模型的车辆行驶状态估计策略研究[J]. 汽车工程, 2023, 45(7): 1212-1221.
[15]	浦震峰, 唐亮, 上官文斌, 王伟玮, 蒋开洪. 基于多传感器信息的汽车低速车速估计方法[J]. 汽车工程, 2023, 45(7): 1235-1243.