CN114018254B - 一种激光雷达与旋转惯导一体化构架与信息融合的slam方法 - Google Patents

Abstract

本发明公开了一种基于激光雷达与旋转惯导一体化构架与信息融合的SLAM方法，所述方法包括：特征提取步骤，通过IMU数据计算激光帧之间的平移与旋转，消除原始点云中的运动畸变，从中提取稳定的边缘与平面特征。LIO步骤，由状态递推子模块和更新子模块组成，进行迭代卡尔曼滤波，输出初始里程以及未失真的特征。建图步骤，用全局地图改进初始里程，输出新的里程，然后使用新的特征更新地图。本发明所提出方法可应用于中低精度的MEMS惯导，有效提高了长时间运行时惯导系统的定位精度，从而提升了建图的准确性和可靠性。

Description

一种激光雷达与旋转惯导一体化构架与信息融合的SLAM方法

技术领域

本发明涉及同步定位与建图领域，具体涉及一种基于旋转惯导与激光雷达数据融合的SLAM方法。

背景技术

如今SLAM(simultaneous localization andmapping，即时定位与地图构建)技术已在机器人、无人机、AVG等领域拥有广泛的应用，基于多线激光雷达传感器的定位与建图算法是当前SLAM领域的一个热点问题。研究如何提升激光雷达的SLAM系统在长时间运动过程中定位与建图的精度和稳定性，对我国机器人与智能驾驶等行业的发展有重要意义。

随着激光SLAM研究的不断发展，在理论、技术和产品落地上越来越成熟，对机器人和自动驾驶企业来说，激光SLAM仍是目前最稳定、最主流的定位导航方法。由于激光雷达本身测距的精度较高，误差模型简单，在强光直射以外的环境中运行稳定，所以激光SLAM算法在特定的环境中通常能取得比较好的效果。然而，基于运动估计的激光雷达点云存在稀疏性与运动失真的问题，导致仅使用激光雷达的SLAM系统在一些宽阔和开放的环境中表现不佳。最近的研究表明，通过融合惯性传感器IMU来弥补该缺陷是一个很好的解决方案。

目前，融合IMU的激光SLAM算法精度一定程度上仍依赖基于MEMS技术的IMU精度。为了提升其精度，现有的技术手段将IMU安装于旋转平台上并进行对准，从而准确地估计出载体当前的姿态信息和IMU的加速度计零偏与陀螺仪零偏。现有的方法IMU对准耗时较长，也不能达到可靠的精度。

发明内容

本发明索要解决的技术问题是，针对惯性传感器精度低下影响激光雷达/惯性SLAM性能的问题，为了提高IMU对准的速度与精度，本发明将在研究旋转捷联惯导系统的结构特点对导航系统性能影响的基础上，提出基于模糊自适应卡尔曼滤波的旋转惯导系统对准方法，实现惯导系统的快速对准。

本发明为解决上述技术问题采用以下技术方案：

(1)惯导对准：利用RWNN模型处理IMU数据，进行惯导系统动基座对准，对IMU误差进行修正，并输出修正后的惯导系统姿态和惯性信息；

(2)特征提取:使用修正后的惯性信息，计算激光帧之间的平移与旋转，消除原始点云中的运动畸变，从中提取稳定的边缘与平面特征，通过特征匹配计算两帧激光数据间的位姿变换；

(3)采用激光惯性里程计LIO模型进行导航解算：使用修正后的惯性信息进行惯性导航解算，作为滤波器估计值，再利用激光雷达的位姿解算结果作为滤波器的观测值，进行迭代卡尔曼滤波，输出系统在全局坐标系下的位姿以及未失真的特征；

(4)建图：采用基于全局地图的匹配，输出修正后的位姿，然后使用新的特征更新三维点云地图。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

本发明提出的基于RWNN的惯导动对准方案能有效减小长时间运行下IMU期间的累积误差，提高惯导系统在长时间运行时的导航精度以及可靠性，特别是中低精度的惯性传感器的定位精度；本发明采用的简化的激光惯性里程计(LIO)模型因为输入的加速度计与陀螺仪贬值已经过惯导对准与误差校正，减低了ESKF的维度，提高了滤波运算器的效率；本发明所提出的激光雷达与旋转惯导一体化构架与信息融合的SLAM方法相比于未对IMU做对准的算法，拥有更好的精度与稳定性，在算法在长时间运行的情况下，大幅减少了轨迹的漂移，显著提升了激光惯性里程计的定位精度与稳定性，对激光惯性融合SLAM算法的工程应用具有重要参考价值。

附图说明

图1为本发明设计的激光雷达与旋转惯导融合的SLAM方法流程图。

图2为本发明采用的基于RWNN近似算法的网络结构图。

图3为本发明提出的简化激光雷达里程计(LIO)模型结构图。

图4是实验场景卫星地图。

图5是室外实验场景各算法定位轨迹对比。

图6是建图结果；其中a)是LINS算法建图结果，b)是本发明算法建图结果。

图7是激光雷达与旋转惯导信息融合的SLAM算法流程。

图8是本发明设计的激光雷达与旋转惯导融合的SLAM算法系统框架图。

具体实施方式

下面结合附图对本发明的技术方案做进一步的详细说明：

本技术领域技术人员可以理解的是，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)具有与本发明所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样定义，不会用理想化或过于正式的含义来解释。

本发明提出一种激光雷达与旋转惯导一体化构架与信息融合的SLAM方法，所述方法包括激光雷达与旋转惯导信息融合的SLAM算法总体框架，基于递归小波神经网络(RWNN)的惯性导航系统动对准方案，一种简化的激光惯性里程计(LIO)模型。

如图1所示，本发明提出了一种激光雷达与旋转惯导一体化构架与信息融合的SLAM方法，参照图8所示，总体框架主要包括惯导对准、特征提取、激光惯性里程计LIO和建图三个主要模块，具体流程为：首先在惯导对准模块对IMU进行误差修正，并输出修正后的惯导系统姿态和惯性信息，特征提取模块利用修正后的IMU数据进行状态更新，通过IMU数据计算激光帧之间的平移与旋转，消除原始点云中的运动畸变，从中提取稳定的边缘与平面特征。LIO模块完成系统在全局坐标系下的位姿以及未失真的特征输出，由状态递推子模块和更新子模块组成，进行迭代卡尔曼滤波，输出初始里程以及未失真的特征。建图模块步骤用全局地图改进初始里程，输出新的里程，然后使用新的特征更新地图。

在惯导系统动基座对准部分，本发明设计了一种基于递归小波神经网络RWNN的惯性导航系统动对准方案，利用基于RWNN的观测器使系统的输出与参考值之间的偏差最小化。其中，基于RWNN的近似算法部分，所采用的RWNN网络主要包含输入层，递归小波层，产生层以及输出层四层结构，具体结构图如图2所示。首先将输入的传感器数据与最新状态向量归一化，使其大小在[0,1]范围内；之后在递归小波层中与母小波推导得到的激活函数进行向量积计算；再进行乘积操作最后求和可以得到RWNN的最终输出。该方案能有效提高惯导系统在长时间运行时的导航精度以及可靠性。

在激光惯性里程计LIO，本发明提出了一种简化的LIO模块，主要包括状态递推子模块和更新子模块，其流程结构图如图3所示。首先，通过惯导系统基座动对准得到的姿态惯性信息进行状态递推；将误差状态代入先验状态可以求解最终观测状态，并在随后的状态更新过程中，根据IMU的测量值，递推得到误差状态、误差状态协方差矩阵和先验状态，再据此进一步进行状态更新，最后完成全局位姿的更新。本方案中使用的LIO模型在定位精度与运算速度上拥有更好的性能。

作为本发明的一个实施例，所述激光雷达与旋转惯导一体化构架与信息融合的SLAM方法的硬件组成是将激光发射器与IMU安装于一个旋转平台上，构成激光雷达与旋转惯导的一体化装置。该装置工作时，旋转平台以10Hz频率旋转，数据与算法处理计算机接收激光发射器与IMU的测量数据，进行导航解算。算法总体框架如图7所示。

进一步地，所述基于RWNN的惯性导航系统动对准方案，其中基于RWNN的观测器系统动力方程为：

其中，和/>分别表示估计状态向量与估计输出向量。使用估计状态向量/>以及传感器输出向量u作为RWNN的输入，可以得到不确定向量δu的近似值。在这种近似方法中，τ是输出的权重向量，Ψ_w表示小波向量，/>以及/>分别表示输入反馈权重、小波的缩放和平移量。观测器的增益由L表示，并且满足矩阵/>为Hurwitz矩阵。采用/>表示RWNN的近似值。

基于RWNN的近似方法部分，所采用的RWNN网络主要包含输入层，递归小波层，产生层以及输出层四层结构。算法中母小波为Mexican-hat小波形式，如下所示：

式中，μ(s)ε[0,1]是标准化的输入向量，而||μ(s)||²＝μ^T(s)μ(s)。

首先该算法将输入的传感器输出数据与最新状态向量归一化在[0,1]范围内，归一过程如下：

之后在递归小波层中，每个输入信号(μ_i)都与j索引指定的三个激活函数进行向量积计算。此激活函数是根据公式(2)的母小波推导得到的，具体过程如下：

σ_d,t(μ(s))＝2^0.5dσ(2^dμ(s)-t) (4)

其中，d和t分别表示小波函数的缩放和平移量。然后，通过使用项σ_d,t(Z^-1(s))来存储网络的过去信息，在该项中，根据上一刻的存储项计算下一刻的存储项/>

式中表示递归小波层反馈项。公式(5)可以改写成：

在产生层中，每一个节点在小波层由Π函数表示，乘以来自第二层的输入信号，如下所示：

最后，根据公式(8)计算RWNN的最终输出。

其中，out_l是近似的不确定度向量，表示输出层的连接权重。

进一步地，所述简化的激光惯性里程计(LIO)模型中，其状态方程如式(9)、(10)所示：

其中，表示/>相对于F_w的位姿。/>表示描述从/>到/>的相对转换的局部状态。/>是/>相对于F_w的位置，/>是描述从/>到F_w旋转的单位四元数。/>和/>表示从/>到/>的平移和旋转。/>是相对于/>的速度。b_a是加速度偏置，b_g是陀螺仪偏置。/>(用/>表示)是局部重力。此处的b_a、b_g经过惯导对准与误差校正，其数量级已足够小，在建模过程中可以略去，从而减低ESKF的维度，提高滤波器运算效率，如式(11)所示：

为了在状态估计中具有良好的特性，使用误差状态表示来求解用δ表示一个误差项，并将/>的误差向量定义为：

δx＝[δp,δv,δθ,δg] (12)

其中δθ是3DOF姿态误差角。

根据ESKF的框架，求解δx后，可以通过将误差δx代入到先验状态中来获得最终的观测状态/>这是通过自定义运算符#进行的，该运算符定义为：

其中表示四元数乘积，exp运算符通过李群将δθ映射到四元数旋转。

随后在状态更新过程中，根据IMU的测量值，递推得到误差状态δx、误差状态协方差矩阵P_k和先验状态据此进行状态更新，状态更新方程如式(14)、(15)、(16)：

δx_j+1＝δx_j+Δx_j (16)

每次更新完成后，根据式(17)进行状态合成更新全局位姿，完成LIO模块的任务。

以下具体实例可以看出本发明在实际应用中的效果。

将旋转惯导与激光雷达安装于载体上并做好外参标定，运行算法的计算机性能如下：2.8GHz8核CPU，16G内存。所有方法均采用C++实现，并使用Linux中的机器人操作系统(ROS)执行。本发明提出的算法对比对象为未做惯导对准的激光惯性SLAM算法、使用卡尔曼滤波和使用强跟踪滤波的激光惯性融合SLAM算法，以此来说明加入惯导系统动对准算法后对激光惯性SLAM算法的性能提升，以及本发明的对准算法相比于另外两种对准算法的优越性。

为验证本算法定位与建图的性能，选择在图4所示场景进行实验。本实验以RTK的定位结果(在图4中以红线标出)作为标准轨迹，对比各类算法的定位与建图效果，各算法解算出的运动轨迹如图5所示。由图5可以看出，未对IMU进行对准的LINS算法的定位信息在第一个转弯处已经出现一定的漂移，到最后定位误差达到了10m以上；而使用基于卡尔曼滤波的对准算法由于对IMU做了对准处理，惯性信息精度有所提升，但长距离运动后，仍然不免出现了位姿的漂移；使用基于强跟踪滤波的对准算法在定位结果上要更加优于常规卡尔曼滤波，但在第三次转弯后，对姿态的估计出现了较大误差，最终影响到定位结果的准确性；而本发明采用的对准方法，相比于强跟踪滤波与常规卡尔曼滤波，拥有更快的收敛速度和更精确的零偏估计，有效的提高了惯导系统姿态精度以及惯性数据的测量精度，从而提升了LIO模块的定位精度，与此同时，建图效果也随之提升。图6展示了LINS与本文算法的建图结果。在图6的a)中，标出部分为LINS算法由于IMU误差的累积，最终的定位结果并未回到起点，从而导致建图出现重影的部分。而本发明的方法由于会对惯导不断进行对准，并修正误差，所以位姿漂移较小，能够达到较高的建图精度，建图结果并未出现重影，如图6的b)所示。

本实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (3)

1.一种激光雷达与旋转惯导一体化构架与信息融合的SLAM方法，其特征在于，包括：

(1)惯导对准：利用递归小波神经网络RWNN模型处理融合惯性传感器IMU数据，进行惯导系统动基座对准，对IMU误差进行修正，并输出修正后的惯导系统姿态和惯性信息；

(2)特征提取：使用修正后的惯性信息，计算激光帧之间的平移与旋转，消除原始点云中的运动畸变，从中提取稳定的边缘与平面特征，通过特征匹配计算两帧激光数据间的位姿变换；

(3)采用激光惯性里程计LIO模型进行导航解算：使用修正后的惯性信息进行惯性导航解算，作为滤波器估计值，再利用激光雷达的位姿解算结果作为滤波器的观测值，进行迭代卡尔曼滤波，输出系统在全局坐标系下的位姿以及未失真的特征；激光惯性里程计LIO模型包括状态递推子模块和更新子模块，该模块的状态方程如式(11)、(12)所示：

其中，表示/>相对于F_w的位姿，/>表示描述从/>到/>的相对转换的局部状态，是/>相对于F_w的位置，/>是描述从/>到F_w旋转的单位四元数，/>和/>表示从/>到的平移和旋转，/>是相对于/>的速度，b_a是加速度偏置，b_g是陀螺仪偏置，/>是局部重力用/>表示；在建模过程中略去b_a、b_g，如式(13)所示：

使用误差状态表示来求解用δ表示一个误差项，并将/>的误差向量定义为：

δx＝[δp,δv,δθ,δg] (14)

其中δθ是3DOF姿态误差角；

根据ESKF的框架，求解δx后，通过将误差δx代入到先验状态中来获得最终的观测状态/>是通过自定义运算符#进行的，该运算符定义为：

其中表示四元数乘积，exp运算符通过李群将δθ映射到四元数旋转；

随后在状态更新过程中，根据IMU的测量值，递推得到误差状态δx、误差状态协方差矩阵P_k和先验状态据此进行状态更新，状态更新方程如式(16)、(17)、(18)：

δx_j+1＝δx_j+Δx_j (18)

每次更新完成后，根据式(14)进行状态合成更新全局位姿，完成LIO模块的任务

(4)建图：采用基于全局地图的匹配，输出修正后的位姿，然后使用新的特征更新三维点云地图。

2.根据权利要求1所述的方法，其特征在于，RWNN模型使系统的输出与参考值之间的偏差最小，系统动力学公式如下：

其中，和/>分别表示估计状态向量与估计输出向量，使用估计状态向量/>以及传感器输出向量u作为RWNN的输入，得到不确定向量δu的近似值，τ是输出的权重向量，Ψ_w表示小波向量，/>以及/>分别表示输入反馈权重、小波的缩放和平移量，观测器的增益由L表示，并且满足矩阵/>为Hurwitz矩阵；采用/>表示RWNN的近似值，动态模拟状态估计误差量和输出误差量，如公式(2)所示：

其中，和/>分别表示状态误差量和输出误差量；将/>部分改写成如下形式：

其中，τ^*TΨ(δx,u,ζ^*,t^*,d^*)+ε是λ(δx,u)的理想近似集，ε表示有界近似误差值，||ε||≤ε_m且ε_m为正实数。

3.根据权利要求1所述的方法，其特征在于，RWNN模型所采用的RWNN网络包含输入层，递归小波层，产生层以及输入层四层结构；算法中母小波为Mexican-hat小波形式，如下所示：

式中，μ(s)ε[0,1]是标准化的输入向量，而||μ(s)||²＝μ^T(s)μ(s)；

首先该算法将输入的传感器输出数据与最新状态向量归一化在[0,1]范围内，归一过程如下：

接着在递归小波层中，每个输入信号(μ_i)都与j索引指定的三个激活函数进行向量积计算；此激活函数是根据公式(4)的母小波推导得到的，具体过程如下：

σ_d,t(μ(s))＝2^0.5dσ(2^dμ(s)-t) (6)

其中，d和t分别表示小波函数的缩放和平移量；

然后，通过使用项σ_d,t(Z^-1(s))来存储网络的过去信息，在该项中，根据上一刻的存储项计算下一刻的存储项/>

式中表示递归小波层反馈项，将公式(7)改写成：

在产生层中，每一个节点在小波层由Π函数表示，乘以来自第二层的输入信号，如下所示：

最后，根据公式(10)计算RWNN的最终输出

其中，out_l是近似的不确定度向量，表示输出层的连接权重。