CN112923934A - 一种适用于非结构化场景中结合惯导的激光slam技术 - Google Patents
一种适用于非结构化场景中结合惯导的激光slam技术 Download PDFInfo
- Publication number
- CN112923934A CN112923934A CN201911246441.9A CN201911246441A CN112923934A CN 112923934 A CN112923934 A CN 112923934A CN 201911246441 A CN201911246441 A CN 201911246441A CN 112923934 A CN112923934 A CN 112923934A
- Authority
- CN
- China
- Prior art keywords
- imu
- lidar
- slam
- mapping
- integration
- 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
- 238000005516 engineering process Methods 0.000 title claims description 5
- 230000010354 integration Effects 0.000 claims abstract description 12
- 238000000034 method Methods 0.000 claims abstract description 10
- 230000033001 locomotion Effects 0.000 claims description 6
- 238000004422 calculation algorithm Methods 0.000 claims description 4
- 238000010276 construction Methods 0.000 claims description 3
- 238000005259 measurement Methods 0.000 abstract description 7
- 238000005457 optimization Methods 0.000 abstract description 6
- 238000013507 mapping Methods 0.000 abstract description 5
- 230000005484 gravity Effects 0.000 abstract description 3
- 230000008569 process Effects 0.000 abstract description 3
- 102000008115 Signaling Lymphocytic Activation Molecule Family Member 1 Human genes 0.000 description 14
- 108010074687 Signaling Lymphocytic Activation Molecule Family Member 1 Proteins 0.000 description 14
- 230000001133 acceleration Effects 0.000 description 9
- 230000006870 function Effects 0.000 description 5
- 239000011159 matrix material Substances 0.000 description 4
- 230000009466 transformation Effects 0.000 description 4
- 238000005295 random walk Methods 0.000 description 3
- 230000000007 visual effect Effects 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 238000012937 correction Methods 0.000 description 2
- 238000006073 displacement reaction Methods 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 239000011435 rock Substances 0.000 description 2
- 206010034719 Personality change Diseases 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000036461 convulsion Effects 0.000 description 1
- 239000000428 dust Substances 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000007667 floating Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000007477 logistic regression Methods 0.000 description 1
- 238000007726 management method Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000012847 principal component analysis method Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000000717 retained effect Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
Images
Classifications
-
- 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
- G01C21/30—Map- or contour-matching
- G01C21/32—Structuring or formatting of map data
-
- 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
-
- 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/18—Stabilised platforms, e.g. by gyroscope
Abstract
本发明涉及一种紧耦合的LIDAR‑IMU SLAM(雷达‑惯性测量单元即时定位与地图构建)，用于对非结构化道路环境中针对位置、姿态、速度和加速度计、陀螺仪漂移进行精确可靠的估计。该方法基于对激光雷达点云和IMU(惯性测量单元)积分产生的残差的优化。第一部分残差来自于同时建立的相关图中当前扫描点云与体素质心之间的距离之和。剩余量的第二部分来自于考虑激光雷达和IMU校准误差的预积分过程。与仅有激光雷达参与的SLAM(即时定位与地图构建)相比，LIDAR‑IMU SLAM在鲁棒性和精确姿态估计方面表现出更好的性能。此外，由于该系统具有提取重力方向的能力，估计的俯仰和滚动角度不会偏离。LIDAR‑IMU SLAM可以保持10Hz的频率，同时进行扫描匹配和建图。
Description
技术领域
本发明涉及无人车技术领域，尤其涉及非结构化场景中结合惯导的激光SLAM技术。
背景技术
精确定位在自主驾驶中起着至关重要的作用，是环境感知的基础，也是决策、路径规划和运动控制的重要输入。通常情况下，GNSS(全球导航卫星系统)可以满足无人驾驶车辆在开放场景下的基本需求。特别是应用实时运动学RTK(实时运动学)校正大气、卫星时钟和轨道误差时，相对于CEP(圆概率误差)，定位精度可提高到厘米。然而，由于多径效应和信号干扰，在隧道、森林、山地等越野场景中，GNSS并不可靠甚至缺失。IMU是另一种定位设备。它是完全独立的，没有任何外部输入。此外，加速度计和陀螺仪的测量提供了无人驾驶车辆的底层运动状态，这是车辆遭受剧烈颠簸时姿态估计的关键。然而，IMU测量随时间漂移，没有外部校正。惯性导航系统也是在没有GPS(全球定位系统)信号的环境下解决定位问题的一个完整的解决方案。但在没有GPS辅助的情况下，它仍然存在漂移的问题，而且成本太高，无法应用于智能汽车。SLAM是一种独特的技术，用于估计车辆和周围障碍物的位置和姿势，这也是自成一体的。根据它所依赖的传感器类型，SLAM可以分为基于相机和基于激光雷达的激光雷达SLAM的视觉SLAM。视觉SLAM很容易受到光线变化和天气的影响。剧烈的颠簸也会导致系统崩溃。与此相反，LIDAR SLAM更加稳定，因为它不受光线变化的影响，并且对恶劣天气不敏感。此外，由于位置和姿势是根据多帧地图的匹配计算，暴力颠簸对系统稳定性的影响较小。然而，由于平面和线特征的缺乏以及浮尘造成的干扰，越野环境仍然是一个挑战。总之，单传感器在复杂的越野环境中很难满足本地化的需要。
发明内容
鉴于上述的分析，针对每种传感器类型的特点，提出了一种基于当前扫描和相对建图之间的重投影误差优化以及IMU数据预积分估计的IMU积分误差的紧耦合的LIDAR-IMUSLAM系统。主要工作是对CPFG-SLAM的改进，并进行了一系列车载实验，以评估LIDAR- IMUSLAM的性能。与仅激光雷达系统相比，LIDAR-IMU SLAM在鲁棒性和精确姿态估计方面表现出更好的性能。此外，由于提取重力方向的能力，估计的俯仰和滚动角度不会偏离。 LIDAR-IMU SLAM可以保持10Hz的频率，同时进行扫描匹配和建图。
本发明的目的主要是通过以下技术方案实现的：
根据LIDAR-IMU SLAM系统架构。点云和IMU数据被发送到实时操作系统中，以获得它们的同步时间戳。然后将初始化估计状态Xglobal，即位置、四元数和世界帧中的速度，以及加速度计和陀螺仪的随机行走偏差。rb表示激光雷达和IMU帧之间的杠杆臂。由于激光雷达坐标系的原点是在车辆后桥的中间进行标定，所以选择了车身坐标系b。
如果没有额外的输入，除了点云和IMU数据之外只能得到车辆的全局滚动和俯仰角，因为全局姿态和航向角是不可观测的，必须设置为零。但是，如果外部传感器输入可用，可以参照全局导航坐标系统将不可观测状态初始化为真值。由于整个系统只支持静态启动，因此速度设置为零，通过计算车辆静止时的平均误差来估计惯性单元偏差。启动后，同步数据通过扫描匹配模块进行处理，首先对IMU数据进行预积分处理。基于最小二乘法的优化后，将点云与全局最优的位置和姿态输入子图，更新子图的占据概率和点分布，进行下一步扫描匹配。
附图说明
附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。
图1为本发明SLAM系统架构；
具体实施方式
下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理。
非结构化场景中结合惯导的激光SLAM技术的实现，包括以下步骤：
A.地图构建
SLAM系统共维护三种地图，一种基于logistic回归的概率地图更新，一种高分辨率的点分布特征图，包括质心点和相关矩阵，一种低分辨率的点分布特征图。为了有效地管理和索引，所有的建图都用八叉树表示。
特征图用于描述障碍物的位置和形状。对于每个网格，当不同的激光雷达扫描点的数目足够大时，通过这些采样点的计算可以估计出质心和相关矩阵。然后应用主成分分析法将网格划分为平面、直线和聚类三类。然后根据网格的类型调整相关矩阵。对于平面，沿着最小特征向量的数据被忽略。对于直线，只保留沿着最大特征向量的数据。对于聚类，只将三个方向上特征值较小的网格作为特征网格，否则没有特征，不参与扫描匹配。特征类型分类后，根据新的特征值重新计算特征网格的协方差。高分辨率图像用于精确的扫描匹配，而低分辨率图像用于粗略地检查退化运动，这对惯性单元偏差估计有负面影响。
概率图用于计算每个三维网格的占用概率，并成为当前帧中点云与地物图之间的重投影误差的权重。具体来说，概率是由激光雷达命中和错过的次数决定的，并由sigmoidlike函数更新。
命中次数n表示网格命中和未命中的次数。一旦检测到一个点，这个点所在网格的命中次数就会增加，导致概率上升。点的径向线上网格的命中次数减少。命中次数越大，网格的概率越接近1。当命中概率为-0.5时，此网格更有可能是空的，位于此网格中的几个点将被忽略。如果该概率低于-0.5的漏失概率阈值，则此网格被分类为空，不再更新。
B.IMU误差建模与预积分。
VINS-mono(视觉惯性系统)提供了一个完整的算法来预测IMU误差。该算法虽然能很好地对车辆进行平缓运动，但在运动状态变化较快的非结构化道路环境中，其精度和鲁棒性难以保持。在实验中，当车轮遇到岩石时，加速度计测量可以达到20°/s，陀螺仪测量的peek 值60°/s当车轮遇到岩石。因此，必须考虑激光雷达和IMU帧的位移引起的误差，aL表示激光雷达坐标系的加速度，aI表示IMU坐标系的加速度，这是IMU的实际输出。r代表向量从IMU坐标系统的起源到激光雷达坐标系和α角加速度。位移误差有两部分，一部分是与角速度有关的径向arn部分，另一部分是与角加速度有关的径向art部分
如果标定的表示从激光雷达坐标系到IMU坐标系变换的外方量不够准确，则径向矢量r 的估计是必不可少的。由于无法获得精确的角加速度，故将art视为高斯噪声。因此，惯性测量模型
和
形成了
na和nw代表加速度计和陀螺的高斯噪声。它们的分布遵循
ba和bw为随机游走偏差，其导数为高斯白噪声。nα代表噪声由角加速度，所以
nα-N(0，c·α)
角加速度的近似值是由一系列最近的角速度的差值来估计的，c是一个常系数。目前，已经建立了激光雷达坐标系下惯性测量单元误差模型。
IMU预积分过程的误差估计公式从计算加速度指示位置变换的二重积分，加速度指示速度变化的单积分，以及前一帧身体坐标系
和
中角速度指示姿态变化的积分开始。预积分的本地状态是
并采用中点积分法得到离散增量，如公式所示。注意，
是四元数的乘积。
结合IMU误差模型和Xlocal各变量之间的关系，在下面公式中建立了局部状态误差的预测方程。该公式有助于得到关于加速度扰动和陀螺随机游走误差ba、bg的Xlocal估计雅可比矩阵。在估计的ba，bg略有变化的情况下，我们可以通过添加相对于偏差的线性项来调整Xlocal，而不是将IMU数据从上一帧重新整合到当前帧，这在IMU输出频率不小于100Hz的情况下耗费了大量的计算资源和时间。
C.姿势估计
在IMU建模之后，可以进行紧密耦合的LIDAR-IMU姿态估计。此过程中的优化变量如问题1中的Xlocal所示。在假设所有噪声均为高斯噪声的情况下，用最小二乘法建立包含IMU 预积分残差和scan-to-map重投影误差的代价函数f，使其最小，从而获得最大的后验估计值
cost函数中的第一项表示当前扫描和相对建图中激光雷达点之间的重投影误差。具体来说，利用全局坐标系中估计的变换和姿态得到地图中的变换点，然后利用类似于Mahalanobis 距离的方法计算该点所在网格的质心距离，如公式所示。
τoccupy举例说明了网格的占用概率，这在地图构建中被提到。μ是网格重心的位置，∑是网格的重新计算协方差。cost函数中的第二项表示通过预积分估计的Xlocal与通过测量全局状态 Xglobal提供的Xlocal之间的残差。局部状态和全局状态的度量之间的关系是
残差来自于测量值Xlocal和估计值1的相减。
建立成本函数后，应用Ceres求解器求解该优化问题。当前位置的初始估计是通过IMU 积分后的偏置校正得到的，类似于上式，但在世界坐标系中。为了确保结果是全局最优的，然后进行了多次迭代。当优化完成后，利用当前位置和姿态的解将当前扫描的点云添加到相关子映射中，并由另一个线程执行。
Claims (4)
1.非结构化场景中结合惯导的激光SLAM技术，包括以下步骤：
1)地图构建
2)IMU误差建模与预积分
3)位姿估计。
2.根据权利要求1所需要的三种地图，特征图用于描述障碍物的位置和形状；不同分辨率的图像分别用于精确的扫描匹配和检查退化运动；概率图用于计算每个网格的占用概率。
3.根据权利要求1利用算法来预测IMU误差。
4.根据权利要求1，IMU建模之后，进行紧密耦合的LIDAR-IMU姿态估计。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911246441.9A CN112923934A (zh) | 2019-12-06 | 2019-12-06 | 一种适用于非结构化场景中结合惯导的激光slam技术 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911246441.9A CN112923934A (zh) | 2019-12-06 | 2019-12-06 | 一种适用于非结构化场景中结合惯导的激光slam技术 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN112923934A true CN112923934A (zh) | 2021-06-08 |
Family
ID=76162134
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911246441.9A Pending CN112923934A (zh) | 2019-12-06 | 2019-12-06 | 一种适用于非结构化场景中结合惯导的激光slam技术 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112923934A (zh) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113436260A (zh) * | 2021-06-24 | 2021-09-24 | 华中科技大学 | 基于多传感器紧耦合的移动机器人位姿估计方法和系统 |
| WO2023159668A1 (en) * | 2022-02-25 | 2023-08-31 | Xiamen University | System and method of capturing large scale scenes using wearable inertial measurement devices and light detection and ranging sensors |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20180088234A1 (en) * | 2016-09-27 | 2018-03-29 | Carnegie Mellon University | Robust Localization and Localizability Prediction Using a Rotating Laser Scanner |
| CN109945856A (zh) * | 2019-02-18 | 2019-06-28 | 天津大学 | 基于惯性/雷达的无人机自主定位与建图方法 |
-
2019
- 2019-12-06 CN CN201911246441.9A patent/CN112923934A/zh active Pending
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20180088234A1 (en) * | 2016-09-27 | 2018-03-29 | Carnegie Mellon University | Robust Localization and Localizability Prediction Using a Rotating Laser Scanner |
| CN109945856A (zh) * | 2019-02-18 | 2019-06-28 | 天津大学 | 基于惯性/雷达的无人机自主定位与建图方法 |
Non-Patent Citations (1)
| Title |
|---|
| ZHEHUA ZHANG等: "A tightly coupled LIDAR-IMU SLAM in off-road environment", 2019 IEEE INTERNATIONAL CONFERENCE ON VEHICULAR ELECTRONICS AND SAFETY (ICVES), pages 1 - 6 * |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113436260A (zh) * | 2021-06-24 | 2021-09-24 | 华中科技大学 | 基于多传感器紧耦合的移动机器人位姿估计方法和系统 |
| CN113436260B (zh) * | 2021-06-24 | 2022-04-19 | 华中科技大学 | 基于多传感器紧耦合的移动机器人位姿估计方法和系统 |
| WO2023159668A1 (en) * | 2022-02-25 | 2023-08-31 | Xiamen University | System and method of capturing large scale scenes using wearable inertial measurement devices and light detection and ranging sensors |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110243358B (zh) | 多源融合的无人车室内外定位方法及系统 | |
| US10295365B2 (en) | State estimation for aerial vehicles using multi-sensor fusion | |
| CN109887057B (zh) | 生成高精度地图的方法和装置 | |
| CN109931926B (zh) | 一种基于站心坐标系的小型无人机无缝自主式导航方法 | |
| US9026263B2 (en) | Automotive navigation system and method to utilize internal geometry of sensor position with respect to rear wheel axis | |
| CN112639502A (zh) | 机器人位姿估计 | |
| Xiong et al. | G-VIDO: A vehicle dynamics and intermittent GNSS-aided visual-inertial state estimator for autonomous driving | |
| CN114199259B (zh) | 一种基于运动状态与环境感知的多源融合导航定位方法 | |
| CN111426320B (zh) | 一种基于图像匹配/惯导/里程计的车辆自主导航方法 | |
| JP2009019992A (ja) | 位置検出装置及び位置検出方法 | |
| JP4984659B2 (ja) | 自車両位置推定装置 | |
| JP2011122921A (ja) | 位置標定装置、位置標定方法、位置標定プログラム、速度ベクトル算出装置、速度ベクトル算出方法および速度ベクトル算出プログラム | |
| CN114526745A (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
| CN114019552A (zh) | 一种基于贝叶斯多传感器误差约束的定位置信度优化方法 | |
| CN110989619B (zh) | 用于定位对象的方法、装置、设备和存储介质 | |
| CN112923934A (zh) | 一种适用于非结构化场景中结合惯导的激光slam技术 | |
| CN115930977A (zh) | 特征退化场景的定位方法、系统、电子设备和可读存介质 | |
| JP2014240266A (ja) | センサドリフト量推定装置及びプログラム | |
| CN115183762A (zh) | 一种机场仓库内外建图方法、系统、电子设备及介质 | |
| Srinara et al. | Performance analysis of 3D NDT scan matching for autonomous vehicles using INS/GNSS/3D LiDAR-SLAM integration scheme | |
| CN113052855B (zh) | 一种基于视觉-imu-轮速计融合的语义slam方法 | |
| US20220268584A1 (en) | Range image aided ins with map based localization | |
| CN116576849A (zh) | 一种基于gmm辅助的车辆融合定位方法及系统 | |
| Emter et al. | Stochastic cloning and smoothing for fusion of multiple relative and absolute measurements for localization and mapping | |
| Volden et al. | Development and experimental evaluation of visual-acoustic navigation for safe maneuvering of unmanned surface vehicles in harbor and waterway areas |
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 |