一种适用于非结构化场景中结合惯导的激光SLAM技术

摘要

本发明涉及一种紧耦合的LIDAR‑IMU SLAM(雷达‑惯性测量单元即时定位与地图构建)，用于对非结构化道路环境中针对位置、姿态、速度和加速度计、陀螺仪漂移进行精确可靠的估计。该方法基于对激光雷达点云和IMU(惯性测量单元)积分产生的残差的优化。第一部分残差来自于同时建立的相关图中当前扫描点云与体素质心之间的距离之和。剩余量的第二部分来自于考虑激光雷达和IMU校准误差的预积分过程。与仅有激光雷达参与的SLAM(即时定位与地图构建)相比，LIDAR‑IMU SLAM在鲁棒性和精确姿态估计方面表现出更好的性能。此外，由于该系统具有提取重力方向的能力，估计的俯仰和滚动角度不会偏离。LIDAR‑IMU SLAM可以保持10Hz的频率，同时进行扫描匹配和建图。

说明书

技术领域

本发明涉及无人车技术领域，尤其涉及非结构化场景中结合惯导的激光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数据被发送到实时操作系统中，以获得它们的同步时间戳。然后将初始化估计状态X

如果没有额外的输入，除了点云和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帧的位移引起的误差，a

如果标定的表示从激光雷达坐标系到IMU坐标系变换的外方量不够准确，则径向矢量r 的估计是必不可少的。由于无法获得精确的角加速度，故将a

n

b

n

角加速度的近似值是由一系列最近的角速度的差值来估计的，c是一个常系数。目前，已经建立了激光雷达坐标系下惯性测量单元误差模型。

IMU预积分过程的误差估计公式从计算加速度指示位置变换的二重积分，加速度指示速度变化的单积分，以及前一帧身体坐标系

并采用中点积分法得到离散增量，如公式所示。注意，

结合IMU误差模型和X

C.姿势估计

在IMU建模之后，可以进行紧密耦合的LIDAR-IMU姿态估计。此过程中的优化变量如问题1中的X

cost函数中的第一项表示当前扫描和相对建图中激光雷达点之间的重投影误差。具体来说，利用全局坐标系中估计的变换和姿态得到地图中的变换点，然后利用类似于Mahalanobis 距离的方法计算该点所在网格的质心距离，如公式所示。

τ

残差来自于测量值X

建立成本函数后，应用Ceres求解器求解该优化问题。当前位置的初始估计是通过IMU 积分后的偏置校正得到的，类似于上式，但在世界坐标系中。为了确保结果是全局最优的，然后进行了多次迭代。当优化完成后，利用当前位置和姿态的解将当前扫描的点云添加到相关子映射中，并由另一个线程执行。