CN116878501A - 一种基于多传感器融合的高精度定位与建图***及方法 - Google Patents
一种基于多传感器融合的高精度定位与建图***及方法 Download PDFInfo
- Publication number
- CN116878501A CN116878501A CN202310852579.3A CN202310852579A CN116878501A CN 116878501 A CN116878501 A CN 116878501A CN 202310852579 A CN202310852579 A CN 202310852579A CN 116878501 A CN116878501 A CN 116878501A
- Authority
- CN
- China
- Prior art keywords
- visual
- imu
- laser
- radar
- odometer
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 37
- 238000013507 mapping Methods 0.000 title claims abstract description 26
- 230000004927 fusion Effects 0.000 title claims abstract description 16
- 230000000007 visual effect Effects 0.000 claims abstract description 82
- 238000005457 optimization Methods 0.000 claims abstract description 38
- 238000001514 detection method Methods 0.000 claims abstract description 28
- 238000005259 measurement Methods 0.000 claims abstract description 12
- 238000007781 pre-processing Methods 0.000 claims abstract description 6
- 238000004422 calculation algorithm Methods 0.000 claims description 15
- 238000001914 filtration Methods 0.000 claims description 10
- 238000005286 illumination Methods 0.000 abstract description 5
- 238000012217 deletion Methods 0.000 abstract 1
- 230000037430 deletion Effects 0.000 abstract 1
- 239000013598 vector Substances 0.000 description 16
- 230000010354 integration Effects 0.000 description 13
- 239000011159 matrix material Substances 0.000 description 13
- 239000004973 liquid crystal related substance Substances 0.000 description 12
- 230000009466 transformation Effects 0.000 description 12
- 230000001133 acceleration Effects 0.000 description 9
- 230000008569 process Effects 0.000 description 9
- 238000000605 extraction Methods 0.000 description 8
- 230000008878 coupling Effects 0.000 description 6
- 238000010168 coupling process Methods 0.000 description 6
- 238000005859 coupling reaction Methods 0.000 description 6
- 230000000694 effects Effects 0.000 description 6
- 238000005516 engineering process Methods 0.000 description 5
- 230000008859 change Effects 0.000 description 4
- 238000011161 development Methods 0.000 description 4
- 238000009825 accumulation Methods 0.000 description 3
- 230000007547 defect Effects 0.000 description 3
- 238000013459 approach Methods 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 230000015556 catabolic process Effects 0.000 description 2
- 238000006731 degradation reaction Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 230000007246 mechanism Effects 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000001351 cycling effect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 238000007499 fusion processing Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000010606 normalization Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000005295 random walk Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000010187 selection method Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
本发明公开了一种基于多传感器融合的高精度定位与建图***及方法，包括前端和后端，所述前端与后端连接，前端包括IMU、激光雷达和单目相机三个传感器的预处理和雷达惯性里程计与视觉惯性里程计；所述后端包括回环、因子图优化和地图；激光雷达和IMU连接雷达惯性里程计，单目相机、IMU和雷达连接视觉惯性里程计，IMU预积分、激光雷达惯性里程计、视觉惯性里程计和回环检测四大因子连接因子图优化，因子图优化连接地图；解决了目前单一传感器SLAM出现的纹理特征复杂、几何特征缺失、光照不稳定、随时间存在累积误差的问题。
Description
技术领域
本发明涉及高精度定位领域，特别是一种基于多传感器融合的高精度定位与建图***及方法。
背景技术
而对于自动驾驶来说，环境感知，路径规划以及导航都需要同时定位和建图(Simultaneous Localization and Mapping，简称SLAM)技术发展。定位和建图是自动驾驶的基础。通过定位技术获得无人车的位姿，通过建图技术获得周围环境的信息。传统的定位建图的方法具有局限性，比如，GPS信号不稳定，INS惯导***零点漂移严重等。而定位建图技术的发展，很大程度上依赖于传感器。当今用于定位建图的传感器主要有视觉相机、激光雷达和惯性测量单元(IMU)等，它们各有优缺点。基于视觉相机的定位建图方法，成本低，重量轻，但对初始化、光照敏感，并且消耗计算资源；基于雷达的定位建图方法，可以直接获得稠密点云，精度高，不过弊端是分辨率低、刷新率低，帧内点云有运动畸变，且在环境几何特征不足的情况下会退化。基于IMU的方法测量频率高，抗干扰能力强，但是测量值存在随机游走的Bias，并且存在累积误差，长时间的积分结果不可靠。
面对上述传感器的特点，可以看出单一传感器的定位建图技术因为传感器本身的作用机理而产生的缺陷，在算法方面是难以弥补的，所以将不同种类的传感器进行融合互补，实现定位与建图最优的准确性、鲁棒性与实时性。
SLAM常见传感器有相机和激光雷达，分别对应视觉SLAM和激光SLAM。2015年，Facebook Reality Labsde研究科学家RaúlMurArtal发表了ORB-SLAM，这篇文章首先选用ORB特征，基于ORB特征的特征匹配和重定位都比PTAM具有更好的视觉不变性；其次，在PTAM基础上加入回环检测，以消除误差累积，严格意义上来讲这是第一篇完整SLAM过程的视觉文章；再者，PTAM在扩展场景时条件较为苛刻，不容易达到，ORB-SLAM先宽松后严格的手段使得选择机制更加鲁棒。该作者又在2017年发表了ORB-SLAM2，这是首个基于单目、双目和深度相机的开源完整SLAM方案，并且文章中提出的BA优化较之前的ICP配准拥有更精确的效果，在此基础上，还能够在拥有较为轻量级的重用地图定位模式。
在ORB-SLAM2得到广泛应用时，2021年该作者又在ORB-SLAM2的基础上发表了ORB-SLAM3，与ORB-SLAM2的最大区别就是加入了IMU，ORB-SLAM3是第一个能应用在单目、双目和深度相机多种视觉传感器的算法，还能加入惯性传感器实现视觉惯性算法。
在ORB-SLAM3中，作者之所以要加入惯性，是因为纯视觉SLAM有一些缺陷，需要多传感器进行弥补。视觉对于场景中的纹理特征捕捉能力较好，但对于环境中的结构特征捕捉能力不足，而激光雷达恰恰就是捕捉结构特征非常典型的传感器。
激光雷达SLAM中，对于三维空间感知的需求使得三维激光雷达在近几年发展起来。三维激光雷达SLAM的奠基之作LOAM，是JiZhang等人在2014年发表的文章，后来发展的一系列文章都是在LOAM的基础上进行改进。这篇文章的核心就在于同时定位和建图的分布进行，高频(10HZ)低精度处理雷达速度估计，低频(1HZ)高精度进行点云匹配注册。这样做的目的是粗精度地进行里程计的位姿估计，建图以较低的频率进行，从而使建图算法能够合并大量的特征点，并使用足够多的迭代次数进行收敛，从而实现低频高精度的建图效果。文章中说明，如果有IMU可以提供运动先验来帮助预测高频运动，效果会更好。
2018年TixiaoShan在LOAM的基础上发表LeGo-LOAM，这篇文章提出了一个轻量级的实时6自由度的位姿估计，可以在嵌入式***中操作。该作者2020年又发表了LeGo-LOAM的扩展版本LIO-SAM[19]，这篇文章是紧耦合的激光惯性里程计，在全局优化的部分采取因子图优化，优化过程添加了IMU预积分因子和GPS因子，用帧图匹配代替了帧帧匹配，相对于LOAM有更好的效果和更短的时间。但是LIO-SAM算法是激光雷达和IMU的紧耦合，并没有加入视觉，所以此算法的实验场景结构特征非常突出，如果针对纹理特征突出的场景，LIO-SAM精度会大大降低。
从以上的激光和视觉SLAM发展情况可以看出，两个方向都是从纯视觉纯激光发展成视觉激光、激光惯性和视觉惯性等融合，这就是SLAM发展的一个重要方向—多传感器融合SLAM。
具体描述LIO-SAM方案如下：
如图6所示，LIO-SAM的因子图。图6中主要包含三种因子。第一种是IMU预积分因子，由两个相邻关键帧之间的IMU测量积分得到。
第二种是激光里程计因子，由每个关键帧和之前n个关键帧之间的帧图匹配结果得到。
第三种是回环因子，由每个关键帧和候选回环关键帧的时序相邻的2m+1个关键帧之间的帧图匹配得到。此外，因子图优化通过iSAM2完成。
1.IMU预积分因子
IMU的测量频率较高，测量值是角速度和线加速度。通过对IMU在两个雷达关键帧和两个图像帧之间进行积分，得到在这段时间内小车的姿态、位置和速度。
2.激光里程计因子
特征提取通过评估局部区域上的点的曲率来提取边缘和平面特征。曲率较大的点被划分为边缘特征，曲率小的被分类为平面特征。将第i时刻激光雷达扫描帧提取的边缘和平面特征分别表示为和/>将在同一时刻提取的所有特征组成一个激光雷达帧Fi，其中
关键帧选择的方法：与之前的状态xi相比，当机器人姿态变化超过用户定义的阈值时，选择激光雷达帧Fi+1作为关键帧。
在因子图中，新保存的关键帧Fi+1与一个新的机器人状态节点xi+1关联。
两个关键帧之间的激光雷达帧被丢弃。
通过这种方式添加关键帧，既能平衡建图的密度和内存消耗，又能保持相对稀疏的因子图，适用于实时非线性优化。
增加一个新的关键帧的位置和旋转变化阈值被选择为1米和10°。
激光雷达里程计因子的生成步骤如下：
1)体素地图的子关键帧：实现了一个滑动窗口的方法来创建一个包含固定数量的最近激光雷达扫描的点云地图。没有优化两个连续的激光雷达扫描之间的转换，而是提取n个最近的关键帧，称之为子关键帧，用于估计。
2)edge和plane进行匹配。
3)利用点-线与点-面之间的距离关系，建立位姿求解方程。
边缘点集合εk+1中的一个点i与对应边线满足如下几何关系：
其中，dε是特征点i到对应直线的距离，是点i在雷达坐标系中的坐标，是[tk+1,t]之间的雷达位姿变换矩阵，fε(·)是边缘点集合εk+1中的一个点与对应边线的几何关系；
平面点集合中的一个点i与对应平面满足如下几何关系：
其中，是特征点i到对应平面的距离，/>是点i在雷达坐标系中的坐标，是[tk+1,t]之间的雷达位姿变换矩阵，/>是平面点集合/>中的一个点与对应平面的几何关系。
用Levenberg-Marquardt方法来求解激光雷达的运动。把上面两个式子εk+1和的每个特征点叠加，得到一个非线性函数f(·)：
其中，f的每一行对应一个特征点，d包含对应的距离，是[tk+1,t]之间的雷达位姿变换矩阵。计算f关于/>的雅可比矩阵，记作J，其中/>将上式通过非线性迭代求解，使d趋于零，
其中，是[tk+1,t]之间的雷达位姿变换矩阵，J是f关于/>的雅可比矩阵，d包含特征点到对应线或面的距离，λ是由Levenberg-Marquardt方法确定的因子。
3.回环因子
和LeGO-LOAM一样使用的基于欧式距离的回环检测。回环检测对于校正机器人高度的漂移特别有用，因为GPS的高程测量不准确。
LIO-SAM算法是激光雷达和IMU的紧耦合，并没有加入视觉，所以其实验场景皆是结构特征非常突出的场景，但是相对于纹理特征突出的场景，LIO-SAM精度会大大降低。或者在隧道以及细窄的走廊里，由于激光只在隧道前进的垂直方向上有约束，而在前进方向缺少约束，而视觉恰恰能弥补这方向上的约束，所以在这样的场景下，视觉信息是必不可少的。
发明内容
为解决现有技术中存在的问题，针对于目前单一传感器SLAM出现的各种问题，如纹理特征复杂、几何特征缺失、光照不稳定、随时间存在累积误差等，以IMU、多线激光雷达和单目相机为传感器的融合同时定位和建图，能够在特殊的、复杂的环境中得到精度较高的定位和建图效果，并且尽可能的减少计算消耗和时间消耗。
一种基于多传感器融合的高精度定位与建图***，包括前端和后端，所述前端与后端连接，所述前端包括IMU、激光雷达和单目相机三个传感器的预处理和雷达惯性里程计与视觉惯性里程计；所述后端包括回环、因子图优化和地图；
激光雷达和IMU连接雷达惯性里程计，单目相机、IMU和雷达连接视觉惯性里程计，IMU预积分、激光雷达惯性里程计、视觉惯性里程计和回环检测四大因子连接因子图优化，因子图优化连接地图。
优选地，一种基于多传感器融合的高精度定位与建图方法，包括以下步骤：
步骤S1：预处理多传感器信息；
步骤S2：IMU预积分得到预测的位姿估计，将此作为激光雷达惯性里程计和视觉里程计优化的初始值，并为因子图提供因子；
步骤S3：对激光雷达获取的点云进行基于线和面特征提取，并进行帧-图匹配，得到激光雷达的位姿估计，并为因子图提供因子；
步骤S4：单目相机将二维视觉特征与同一时间戳处的激光点云的深度配准，获得视觉三维特征，通过ICP三维点云配准算法，获得视觉位姿估计，并为因子图提供因子；
步骤S5：耦合的激光视觉惯性里程计输出初步位姿估计和局部地图；
步骤S6：基于视觉惯性里程计进行回环检测，并为因子图提供因子；
步骤S7：回环检测后，将四大因子基于滑窗因子图优化；
步骤S8：得到***最优位姿估计和全局点云地图。
优选地，步骤S1包括以下子步骤：
子步骤S11：预积分IMU；
子步骤S12：根据激光点云去畸变与滤波；
子步骤S13：提取与跟踪视觉特征。
优选地，步骤S5包括以下子步骤：
子步骤S51：计算得到激光惯性里程计；
子步骤S52：计算得到视觉惯性里程计。
本发明基于多传感器融合的高精度定位与建图***及方法的有益效果如下：
1.本发明***输入为IMU、激光雷达和单目相机三个传感器采集的数据信息，通过对数据进行处理，得到IMU预积分、激光雷达惯性里程计、视觉惯性里程计和回环检测四大因子，作为因子图的输入，输出为***最优位姿估计和全局地图。
2.本发明对IMU的测量值进行预积分，得到预测的位姿估计，将此作为激光雷达惯性里程计和视觉里程计优化的初始值，并为因子图提供因子，提高优化效率与准确率。
3.本发明的激光雷达获取的点云首先经过点云去畸变和滤波，滤波是对点云进行聚类，少于10个点的聚类被标记为离群值并被剔除，以减少点云数量，节省计算量。
4.本发明将二维视觉特征与同一时间戳处的激光点云的深度配准，获得视觉三维特征。通过ICP三维点云配准算法，获得视觉位姿估计，并为因子图提供因子。
5.本发明的激光雷达、视觉和IMU紧耦合方案中的回环检测由基于词袋模型的视觉回环检测方法完成。
6.本发明优化后的状态反过来更新IMU的偏置参数和雷达惯性里程计中用于帧-图匹配的变换矩阵参数。
7.本发明因子图优化基于时间滑动窗口，根据新增位姿估计，边缘化老数据，更新局部地图。
附图说明
图1为本发明的流程图。
图2为本发明的视觉惯性里程计图。
图3为本发明的耦合的激光视觉惯性里程计输出位姿估计和局部地图。
图4为本发明的基于滑窗的因子图优化图。
图5为本发明的从里程计输出的位姿估计和局部地图得到融合优化后的位姿估计和点云地图。
图6为本发明的述LIO-SAM方案图。
具体实施方式
下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。
本方法***框架如图1所示，此方法通过前端和后端分两个线程实现。前端在负责特征点提取和数据关联的基础上,分别进行雷达帧和图像特征点的粗匹配与位姿的粗估计﹐而后端则负责某一时间滑窗内的整***姿优化和地图优化。
前端虽然会产生一定的位姿漂移，但在后端优化线程中则会对漂移进行校正﹐这样既保证了位姿估计精度﹐也提升了算法的实时性。
***输入为IMU、激光雷达和单目相机三个传感器采集的数据信息，通过对数据进行处理，得到IMU预积分、激光雷达惯性里程计、视觉惯性里程计和回环检测四大因子，作为因子图的输入，输出为***最优位姿估计和全局地图。
IMU的测量值为线加速度和角速度，通过对测量值进行预积分，得到预测的位姿估计，将此作为激光雷达惯性里程计和视觉里程计优化的初始值，并为因子图提供因子，提高优化效率与准确率。
激光雷达获取的点云首先经过点云去畸变和滤波，再对点云进行基于线和面特征提取，并进行帧-图匹配，得到激光雷达的位姿估计，并为因子图提供因子。
单目相机获取为二维图像信息，对图像特征进行检测与跟踪，获得二维特征。
由于单目相机不能对深度进行绝对测量，将二维视觉特征与同一时间戳处的激光点云的深度配准，获得视觉三维特征。
通过ICP三维点云配准算法，获得视觉位姿估计，并为因子图提供因子。
ICP三维点云配准算法具体过程如下：
有一组配对好的3D点P和P′：
P＝{p1,…,pn},P′＝{p′1,…,p′n}
其中，p1,…,pn，p′1,…,p′n，分别表示两组点的坐标，想要找一个欧氏变换R,t，使得：
其中，pi和p′i是匹配好的一对点，R是从p′i到pi的旋转矩阵，t是从p′i到pi的平移向量。
求解ICP的一种方式是使用非线性优化。以李代数ξ表达位姿时，目标函数可以写成：
其中，pi和p′i是匹配好的一对点，李代数ξ表示pi和p′i之间的相对位姿。
以迭代的方式去找最优值。
随着时间误差累计，估计的运动产生漂移，所以需要对其进行回环检测。回环检测由基于词袋模型的视觉回环检测方法完成。
最后，因子图由IMU预积分因子、激光惯性里程计因子、视觉惯性里程计因子和回环因子四大因子构成，优化后的状态反过来更新IMU的偏置参数和雷达惯性里程计中用于帧-图匹配的变换矩阵参数。同时基于时间滑动窗口，根据新增位姿估计，边缘化老数据，更新局部地图。优化四大因子得到最优位姿估计和全局点云地图。
1.前端
1.1多传感器信息预处理
对多种传感器接收的原始测量数据进行融合处理。由于IMU直接获取的加速度和角速度，单目相机获取的二维图像以及多线激光雷达获取的原始点云数据量很大，在现有计算资源能力基础上达不到实时的效果，所以需要对其进行预处理。
1.1.1IMU预积分
IMU的测量频率较高，测量值是角速度和线加速度。通过对IMU在两个雷达关键帧和两个图像帧之间进行积分，得到在这段时间内小车的姿态、位置和速度。假设第k帧和第k+1帧对应的时刻分别是tk和tk+1，在连续时间[tk,tk+1]内，IMU测得的tk+1时刻小车的姿态位置/>和速度/>分别是：
其中，b系是IMU坐标系，W系为世界坐标系；是tk时刻从IMU坐标系到世界坐标系的旋转四元数，Ω是四元数右乘，/>是IMU的角速度测量值，/>是IMU角速度的偏置，nω是IMU角速度测量值的白噪声；/>是tk时刻小车的位置，/>是tk时刻从IMU坐标系到世界坐标系的旋转矩阵，/>是IMU的加速度测量值，/>是IMU加速度测量值的偏置，gW是重力加速度；/>是tk时刻小车的速度。
1.1.2激光点云去畸变与滤波
激光点云坐标是根据激光点采集时刻的激光雷达坐标系计算的，而一帧激光点云中激光点采集时间不同，所以每一帧激光点云中的激光点基于的坐标系不同。采取下列办法对点云进行运动补偿，使每一帧点云转换到同一个坐标系下，每一帧起始时刻的雷达坐标系，以去除激光点云的畸变：
通过前面的IMU预积分得到两帧之间小车方向、位置和速度的相对增量，再计算激光点采集时间与这一帧中起始时刻的时间差，根据匀速运动模型进行线性插值，得到激光点到帧起始时刻的变换矩阵：
其中，是第k帧中的第i个点云到第k帧起始时刻的变换矩阵，/>是由IMU预积分得到的[tk,tk+1]时间段中雷达位姿的相对变换，ti是第k帧中的第i个点云的采集时间，之后/>乘以激光点坐标即可以补偿该畸变。
点云去畸变完成后，进行滤波操作。对点云进行聚类，少于10个点的聚类被标记为离群值并被剔除，因为它们很可能是噪声。经过滤波操作，减少点云数量以节省计算量。
1.1.3视觉特征提取与跟踪
视觉特征点的提取方式为FAST特征点检测，并且用KLT光流跟踪特征。跟踪的同时检测新的特征点，保证同一张图像中的特征点数量为300～400，通过设置相邻特征之间的最小像素间隔来强制实现均匀的特征分布，并通过RANSAC剔除外点。
1.2基于融合数据的里程计
该里程计将视觉惯性和激光惯性进行耦合，利用三个传感器的互补性——视觉的纹理捕捉、激光的绝对深度测量和IMU的初始预测使得里程计输出高精度的位姿估计。
1.2.1激光惯性里程计
激光惯性里程计以IMU辅助激光的方式进行耦合，主要完成激光点云特征的提取和匹配，以及基于IMU初始位姿预测得到激光点云的位姿估计结果。
1.2.1.1激光点云特征提取
对预处理后的点云，计算点i的曲率c：
其中，表示第k帧激光点云中的第i个点在雷达坐标系中的坐标，/>表示第k帧激光点云中的第j个点在雷达坐标系中的坐标，/>表示与点i邻近的点的集合。对各点的曲率值进行排序，曲率大的优先选为边缘特征点，曲率小的优先选为平面特征点。同时，为使特征点均匀分布，将一帧点云划分为4个子区域，设置每个子区域最多提取2个边缘特征点和4个平面特征点。避免选取和激光束扫描平面几乎平行的局部平面上的点和在被遮挡区域的边界上的点。这样，一帧中用边缘特征点和平面特征点来代表整个数据，而不是考虑每一个点云。
1.2.1.2特征匹配
特征匹配是关键帧与局部地图的匹配，提高地图的利用率。
定义小车的状态x为：
x＝[R,p,v,b]
其中，R是旋转矩阵，p是位置向量，v是速度，b＝[ba,bw]是IMU的偏置，ba和bw分别是加速度和角速度的偏置向量。
关键帧的选取方式是，与之前的状态xi相比，当小车位姿变化超过定义的阈值(e.g.15°和1m)时，选取激光雷达帧Fi+1作为关键帧，关键帧之间的激光雷达帧被丢弃。并且在因子图中，关键帧Fi+1与状态xi+1相关联。
将与当前时刻时间最近的n+1个关键帧{Fi-n,…,Fi}作为子关键帧，与之对应的位姿为{Ti-n,…,Ti}，{Ti-n,…,Ti}将{Fi-n,…,Fi}转换到世界坐标系下构成局部地图Mi。把Fi+1的特征点用IMU预积分估计的变换矩阵投影到世界坐标系中，表示为Qi+1。通过优化激光雷达位姿/>将Qi+1与Mi进行匹配。/> 是边缘特征地图，/>是平面特征地图。/>是边缘特征点，/>是平面特征点。问题转化为：通过优化激光雷达位姿/>将/>与/>匹配，/>与/>匹配。
Qi+1周围区域的点集为计算/>的协方差矩阵，记为M，M的特征值和特征向量分别记为V和E。对于/>如果V中有一个特征值明显大于另外两个特征值，那么E中与最大特征值相关联的特征向量表示/>中与/>对应的边线的方向向量；对于/>如果V包含两个大特征值，第三个特征值明显较小，那么E中与最小特征值相关联的特征向量表示/>中与/>对应的平面的法向量。边线或平面的位置是通过/>的几何中心确定的。
完成匹配后，计算从特征点到对应线或平面的距离。在线上选择两个点(j,l)，用下列公式计算特征点到对应直线的距离dε：
其中，是第k+1帧激光点云中第i个点在雷达坐标系中的坐标，/>是第k帧激光点云中第j个点在雷达坐标系中的坐标，/>是第k帧激光点云中第l个点在雷达坐标系中的坐标。
在平面上选择三个点(j,l,m)，用下列公式计算特征点到对应平面的距离
其中，是第k+1帧激光点云中第i个点在雷达坐标系中的坐标，/>是第k帧激光点云中第j个点在雷达坐标系中的坐标，/>是第k帧激光点云中第l个点在雷达坐标系中的坐标，/>是第k帧激光点云中第m个点在雷达坐标系中的坐标。
通过IMU预积分提供初始位姿估计，通过后端优化来最小化残差距离，得到小车较为准确的位姿。
1.2.2视觉惯性里程计
1.2.2.1视觉特征与深度配准
由于单目相机不能测量到特征的绝对深度信息，所以需要利用同一时间戳的激光雷达点云与视觉特征点进行深度配准，使视觉特征点具有深度信息。如图2所示，将经过预处理的激光点云和视觉特征点转换到相机归一化平面(即以相机为中心的单位球面)。然后，对归一化平面上的激光点云进行下采样并按照极坐标进行存储为二维K-D树。根据视觉特征的极坐标，搜索二维K-D树，在球面上找到距离视觉特征最近的三个激光深度点。由视觉特征与相机中心Oc形成的直线，与笛卡尔空间中三个深度点形成的平面相交。Oc到交点的距离就是视觉特征对应的深度。如果特征对应的深度点之间的距离大于一定阈值(e.g.2m),则特征没有关联的深度。
1.2.2.2视觉惯性运动估计
预处理阶段已经将视觉特征进行了匹配与跟踪。在对视觉特征深度配准之后，视觉特征点获得了三维坐标。之后通过IMU预积分提供的初始位姿估计，再用ICP(IterativeClosest Point,迭代最近点)对3D视觉特征进行匹配和位姿估计，完成视觉惯性里程计的运动估计。
2.后端
耦合的激光视觉惯性里程计输出位姿估计和局部地图，如图3所示，但是随着时间误差累计，估计的运动产生漂移，所以需要对其进行回环检测和全局的优化。后端的优化过程也是传感器信息耦合的过程，将激光、视觉和惯性信息进行紧耦合，从里程计输出的位姿估计和局部地图得到融合优化后的位姿估计和全局一致的点云地图。
2.1存取关键帧
回环检测和基于滑动窗口的因子图优化都需要***的历史关键帧。当前帧相对于上一个关键帧的状态变化达到一定阈值，并且特征点达到一定的数量阈值被选取为激光或视觉的关键帧。***里程计所保存的关键帧包括激光点云的关键帧和图像视觉关键帧，并且视觉关键帧和激光关键帧通过视觉特征深度配准相关联。
2.2回环检测
回环检测基于视觉惯性里程计来进行的，方法基于词袋模型(Bag-of-Words)，应用DBoW2库正逆序索引图像数据库。词袋模型是通过计算统计的词袋向量与当前帧的相似度判断是否产生回环。
首先，将所有视觉特征进行聚类，一类特征，即局部相邻特征点的集合，是一个“单词”，这样所有特征就是一个“字典”。
如果想把很多图像中的一共N个特征点归为k类，将“字典”做成k叉树，进行存储和查询。具体做法是，在根节点，用K-means算法把所有样本聚成k类，这样得到了第一层。对第一层的每个节点，把属于该节点的样本再聚成k类，得到下一层。依此类推，最后得到叶子层。叶子层即为所谓的“单词”。
K-means算法具体过程如下：
(1)从n个数据对象任意选择k个对象作为初始聚类中心；
(2)根据每个聚类对象的均值(中心对象)，计算每个对象与这些中心对象的距离；并根据最小距离重新对相应对象进行划分；
(3)重新计算每个(有变化)聚类的均值(中心对象)；
(4)循环(2)到(3)直到每个聚类不再发生变化为止。
通过判断一张图像中有“字典”中的哪些“单词”，就可以用一个向量描述一张图像。具体过程如下：
某个单词wi中的特征数量为ni，所有特征数量为n，那么该单词的IDF为：
单词wi在图像A中出现了ni次，而一共出现了n次，那么TF为：
wi的权重ηi等于TF乘IDF之积：
ηi＝TFi×IDFi
对于某个图像A，它的特征点可对应到许多个单词，组成它的词袋。通过词袋，用单个向量vA描述一个图像A：
其中，w1,…,wN是图像A在字典中所具有的单词，η1,…,ηN是w1,…,wN所对应的权重，vA是描述图像A的向量。这样，就可以计算两张图像A和B之间的相似度s(vA-vB)：
其中，vA是描述图像A的向量，vB是描述图像B的向量，s(vA-vB)是两张图像A和B之间的相似度，vAi表示描述图像A的向量vA的第i个分量，vBi表示描述图像B的向量vB的第i个分量。如果相似度超过某一阈值(e.g.3)，认为可能出现回环。用ICP构建回环约束因子。
2.3基于滑窗的因子图优化
如图4所示，因子图优化过程也是传感器信息耦合的过程，将激光、视觉和惯性信息进行紧耦合，从里程计输出的位姿估计和局部地图得到融合优化后的位姿估计和点云地图，如图5所示。因子图有两类节点，一类是变量节点，为待优化的变量，本方法中变量节点为小车的状态变量x＝[R,p,v,b](公式参数的具体含义)，其中，R是旋转矩阵，p是位置向量，v是速度，b＝[ba,bw]是IMU的偏置，ba和bw分别是加速度和角速度的偏置向量；另一类为因子节点，一个因子节点提供一个变量间的约束。本方法的因子图由四个因子构成，包括IMU预积分因子、激光惯性里程计因子、视觉惯性里程计因子和回环因子。
连续两个关键帧的IMU测量值积分得到IMU预积分因子，每个当前的关键帧与之前的n+1个激光雷达子关键帧匹配得到激光里程计因子，每帧视觉特征点进行匹配得到视觉里程计因子，回环关键帧拉回漂移误差得到回环因子。而因子图的实质就是调整各个变量的值，使得满足各个约束的概率最大。
本方法采用基于时间滑动窗口的因子图优化的方式。基于时间滑动窗口，根据新增位姿估计，边缘化老数据，更新局部地图。
基于时间的滑动窗口更新并维护局部地图，将局部地图整合，形成全局点云地图。
本发明结合了多传感器各自的优势，可在各种复杂、恶劣环境中实现较高的精度。当光照条件不稳定或过暗过亮时，雷达进行激光扫描获取点云，不受光照的影响，同时单目相机无法获得绝对深度，雷达获取的三维点云使得视觉特征具有深度信息。在几何特征缺失的情况下，如狭长的走廊，雷达会发生退化现象，这时可使用相机采集视觉特征进行运动的估计。当环境中细节复杂、纹理繁多，雷达的激光难以捕获所有的细节，这时相机采集的图像可以细致地获得微小的细节。当在空旷的平地，环境特征严重缺失，这时IMU可提供位姿信息。随着时间的进行，IMU由于自身偏置的存在无法避免地会产生误差的累计，这时基于激光点云和视觉图像的里程计会进行有效的修正。
本方案将激光雷达、相机、IMU紧耦合，紧耦合***提供更高的精度。IMU的测量值预积分得到预测的位姿估计，作为激光雷达惯性里程计和视觉里程计优化的初始值，同时，优化后的状态反过来更新IMU的偏置参数和雷达惯性里程计中用于帧-图匹配的变换矩阵参数。更重要的是，与各传感器都相关的四大因子一起放到因子图中同步优化，最大限度地利用了各传感器间的互补性。
本方案兼顾了精度与实时性。因子图优化本身就兼具滤波和图优化的优势，既有滤波的速度也有图优化的精度。同时本方法采用基于时间滑动窗口的因子图优化方式，根据新增位姿估计，边缘化老数据，更新局部地图，大大减少了计算量。
本发明实现了更好的回环效果。由于激光的特性，激光雷达判断回环只能根据其几何关系，所以激光的回环检测准确度相对较低，而视觉的回环检测已经较为成熟。本方法采取视觉回环检测，基于视觉词袋模型，通过计算统计的词袋向量与当前帧的相似度判断是否产生回环，应用DBoW2库正逆序索引图像数据库。
在视觉特征提取与跟踪部分：视觉特征点的提取方式为FAST特征点检测，也可替代为Harris角点检测等其他关键点检测方法。
KLT光流跟踪，可替代为计算描述子进行匹配，描述子可选为SIFT特征描述子、ORB特征描述子等其他描述子。
Claims (4)
1.一种基于多传感器融合的高精度定位与建图***，其特征在于，包括前端和后端，所述前端与后端连接，所述前端包括IMU、激光雷达和单目相机三个传感器的预处理和雷达惯性里程计与视觉惯性里程计；所述后端包括回环、因子图优化和地图；
激光雷达和IMU连接雷达惯性里程计，单目相机、IMU和雷达连接视觉惯性里程计，IMU预积分、激光雷达惯性里程计、视觉惯性里程计和回环检测四大因子连接因子图优化，因子图优化连接地图。
2.一种基于多传感器融合的高精度定位与建图方法，其特征在于，包括以下步骤：
步骤S1：预处理多传感器信息；
步骤S2：IMU预积分得到预测的位姿估计，将此作为激光雷达惯性里程计和视觉里程计优化的初始值，并为因子图提供因子；
步骤S3：对激光雷达获取的点云进行基于线和面特征提取，并进行帧-图匹配，得到激光雷达的位姿估计，并为因子图提供因子；
步骤S4：单目相机将二维视觉特征与同一时间戳处的激光点云的深度配准，获得视觉三维特征，通过ICP三维点云配准算法，获得视觉位姿估计，并为因子图提供因子；
步骤S5：耦合的激光视觉惯性里程计输出初步位姿估计和局部地图；
步骤S6：基于视觉惯性里程计进行回环检测，并为因子图提供因子；
步骤S7：回环检测后，将四大因子基于滑窗因子图优化；
步骤S8：得到***最优位姿估计和全局点云地图。
3.根据权利要求2所述的基于多传感器融合的高精度定位与建图方法，其特征在于，所述步骤S1包括以下子步骤：
子步骤S11：预积分IMU；
子步骤S12：根据激光点云去畸变与滤波；
子步骤S13：提取与跟踪视觉特征。
4.根据权利要求2所述的基于多传感器融合的高精度定位与建图方法，其特征在于，所述步骤S5包括以下子步骤：
子步骤S51：计算得到激光惯性里程计；
子步骤S52：计算得到视觉惯性里程计。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310852579.3A CN116878501A (zh) | 2023-07-12 | 2023-07-12 | 一种基于多传感器融合的高精度定位与建图***及方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310852579.3A CN116878501A (zh) | 2023-07-12 | 2023-07-12 | 一种基于多传感器融合的高精度定位与建图***及方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116878501A true CN116878501A (zh) | 2023-10-13 |
Family
ID=88254399
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310852579.3A Pending CN116878501A (zh) | 2023-07-12 | 2023-07-12 | 一种基于多传感器融合的高精度定位与建图***及方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116878501A (zh) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117367412A (zh) * | 2023-12-07 | 2024-01-09 | 南开大学 | 一种融合捆集调整的紧耦合激光惯导里程计与建图方法 |
| CN117808882A (zh) * | 2024-02-29 | 2024-04-02 | 上海几何伙伴智能驾驶有限公司 | 退化场景下基于多传感器融合的slam漂移检测与补偿方法 |
| CN117823741A (zh) * | 2024-03-06 | 2024-04-05 | 福建巨联环境科技股份有限公司 | 一种结合智能机器人的管网非开挖修复方法及*** |
| CN117906598A (zh) * | 2024-03-19 | 2024-04-19 | 深圳市其域创新科技有限公司 | 无人机设备的定位方法、装置、计算机设备和存储介质 |
-
2023
- 2023-07-12 CN CN202310852579.3A patent/CN116878501A/zh active Pending
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117367412A (zh) * | 2023-12-07 | 2024-01-09 | 南开大学 | 一种融合捆集调整的紧耦合激光惯导里程计与建图方法 |
| CN117367412B (zh) * | 2023-12-07 | 2024-03-29 | 南开大学 | 一种融合捆集调整的紧耦合激光惯导里程计与建图方法 |
| CN117808882A (zh) * | 2024-02-29 | 2024-04-02 | 上海几何伙伴智能驾驶有限公司 | 退化场景下基于多传感器融合的slam漂移检测与补偿方法 |
| CN117808882B (zh) * | 2024-02-29 | 2024-05-17 | 上海几何伙伴智能驾驶有限公司 | 退化场景下基于多传感器融合的slam漂移检测与补偿方法 |
| CN117823741A (zh) * | 2024-03-06 | 2024-04-05 | 福建巨联环境科技股份有限公司 | 一种结合智能机器人的管网非开挖修复方法及*** |
| CN117823741B (zh) * | 2024-03-06 | 2024-05-31 | 福建巨联环境科技股份有限公司 | 一种结合智能机器人的管网非开挖修复方法及*** |
| CN117906598A (zh) * | 2024-03-19 | 2024-04-19 | 深圳市其域创新科技有限公司 | 无人机设备的定位方法、装置、计算机设备和存储介质 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110070615B (zh) | 一种基于多相机协同的全景视觉slam方法 | |
| CN111739063B (zh) | 一种基于多传感器融合的电力巡检机器人定位方法 | |
| CN112634451B (zh) | 一种融合多传感器的室外大场景三维建图方法 | |
| CN113345018B (zh) | 一种动态场景下的激光单目视觉融合定位建图方法 | |
| CN112258600A (zh) | 一种基于视觉与激光雷达的同时定位与地图构建方法 | |
| CA2982044C (en) | Method and device for real-time mapping and localization | |
| Huang | Review on LiDAR-based SLAM techniques | |
| CN116878501A (zh) | 一种基于多传感器融合的高精度定位与建图***及方法 | |
| CN114526745B (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及*** | |
| CN108615246B (zh) | 提高视觉里程计***鲁棒性和降低算法计算消耗的方法 | |
| CN110361027A (zh) | 基于单线激光雷达与双目相机数据融合的机器人路径规划方法 | |
| CN111024066A (zh) | 一种无人机视觉-惯性融合室内定位方法 | |
| CN111583369A (zh) | 一种基于面线角点特征提取的激光slam方法 | |
| CN113432600A (zh) | 基于多信息源的机器人即时定位与地图构建方法及*** | |
| CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
| CN113506318B (zh) | 一种车载边缘场景下的三维目标感知方法 | |
| CN111366153B (zh) | 一种激光雷达与imu紧耦合的定位方法 | |
| Ceriani et al. | Pose interpolation slam for large maps using moving 3d sensors | |
| CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
| WO2024114119A1 (zh) | 一种基于双目相机引导的传感器融合方法 | |
| CN114648584B (zh) | 一种用于多源融合定位的鲁棒性控制方法和*** | |
| CN117367427A (zh) | 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法 | |
| CN112432653B (zh) | 基于点线特征的单目视觉惯性里程计方法 | |
| CN113487631A (zh) | 基于lego-loam的可调式大角度探测感知及控制方法 | |
| WO2023130842A1 (zh) | 一种相机位姿确定方法和装置 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |