广告
LINS：用于鲁棒高效导航的激光雷达-惯导状态估计器
2020-08-18 20:19:03· 来源：同济智能汽车研究所
编者按：对于自动驾驶任务来说，高精度的定位信息作为后续决策规划等模块的输入，尤为重要。而定位领域中，高频率的自身运动估计必不可少，尤其是在高速行驶的任务场景下。近年来基于激光雷达或相机等传感器数据的同步定位与建图技术发展迅速，但受限于传感器工作频率，无法获得高频的定位输出。惯性测量单元(IMU)则可以提供高频的测量数据，因此，基于多传感器融合的算法极具应用价值。本文基于误差状态卡尔曼滤波，将激光雷达与惯导进行紧耦合，可以实现精确而稳定的运动估计。
文章译自：
LINS：A Lidar-Inertial State Estimator for Robust and Efficient Navigation
文章来源：
2020 International Conference on Robotics and Automation (ICRA)
作者：
Qin, Chao and Ye, Haoyang and Pranata, Christian E and Han, Jun and Zhang, Shuyang and Liu, Ming
原文链接：
https://arxiv.org/abs/1907.02233
摘要：LINS是一种轻量级的激光雷达惯性状态估计器，用于实时的自身运动估计。本文所提出的方法通过紧密耦合6轴IMU和3D激光雷达，能够在挑战性环境（如特征稀疏的场景）中为地面车辆进行鲁棒而高效的导航。本文设计了迭代的误差状态卡尔曼滤波器（ESKF），以通过在每次迭代中生成新的特征对应关系来递归地校正估计状态，并使系统在计算上易于处理。此外，为了防止长时间运行时滤波器发散，我们使用以机器人为中心的公式表示局部运动帧的状态。为了验证鲁棒性和通用性，在各种情况下都进行了广泛的实验。实验结果表明，LINS在稳定性和准确性方面的性能与最新的激光雷达惯性里程表相当，并在速度上实现了数量级的提高。
关键词：激光雷达，惯性导航，多传感器融合，卡尔曼滤波
1 前言
自身运动估计是大多数移动机器人应用的基本先决条件，实时性差和算法失败会很快破坏硬件及其周围环境。为此，提出了诸如激光雷达的有源传感器来完成该任务，即所谓的同时定位和建图（SLAM）。典型3D激光雷达的一些关键优势包括（i）宽阔的水平视场（FOV）[1]和（ii）对环境照明条件的不变性[2]。但是，基于激光雷达的导航系统对周围环境敏感。此外，运动畸变[3]和点云的稀疏性[4]在某些挑战性场景（例如宽阔开放区域）中会使算法的性能下降。
最近的研究表明，可以通过融合IMU来弥补激光雷达的不足。与激光雷达不同，IMU对环境不敏感。它提供了准确的短期运动约束，并且通常在高频下（例如100Hz-500Hz）工作。这些功能可以帮助激光雷达导航系统从高度动态的运动畸变中恢复点云，从而提高准确性。但是，目前最佳的基于图优化的激光雷达惯性里程计（LIO）[5]由于计算量大，无法直接应用于实时导航; 对于单次扫描，要花费超过100毫秒的时间来进行激光雷达惯性里程计的计算，甚至需要更多时间以维护地图。
图1 LINS在广东某港口用Velodyne VLP-16和Xsens MTi-G-710 IMU构建的3D地图。即使在某些特征稀疏的环境中，我们也可以观察到生成的地图与Google Map的良好对齐，如右图所示。
在本文中，我们提出了LINS，这是一种用于无人地面车辆（UGV）实时导航的轻量级激光雷达惯性状态估计器。迭代错误状态卡尔曼滤波器（ESKF）旨在确保准确性和效率。为了实现长期稳定性，我们引入了一种以机器人为中心的状态公式，在该状态下，局部参考帧依照激光雷达的时间步长进行移动，两个连续局部帧之间的相对姿态估计用于更新全局姿态估计。我们工作的主要贡献如下：
1.提出了一种紧密耦合的激光雷达惯性里程计算法，该算法比我们以前的工作[5]快一个数量级。
2.我们提出了一个以机器人为中心的迭代式ESKF，该ESKF在各种挑战性场景中均得到了验证，并显示出优于现有技术的性能。
3.源代码可在网上获得（https://github.com/ChaoqinRobotics）。据我们所知，LINS是第一个通过迭代卡尔曼滤波解决6自由度自身运动的紧密耦合LIO算法。
其余部分的组织方式如下。在第二章节，讨论相关的文献。在第三章节我们将概述整体系统的完整框架。在第四章节说明了实验结果。在第五章节得出结论。
图2 LINS系统的框架。激光雷达惯性里程计模块由状态传播和更新子模块组成，它使用IMU测量值和从特征提取模块中提取的点云特征，进行迭代卡尔曼滤波。建图模块输出精确的姿态估计以及全局3D地图。精确的姿态估计值与IMU测量值相结合，以产生高频率的输出结果。请注意，这项工作的重点是激光雷达惯性里程计模块。
2 相关工作
文献中有数百篇激光雷达相关的里程计算法。我们将注意力集中在有关6个自由度自身运动估计量和相关融合算法的相关工作上，它们分为松耦合和紧耦合。
A.仅使用激光雷达的方法
许多仅使用激光雷达的方法是众所周知的基于扫描到扫描配准的迭代最近点（ICP）扫描匹配方法的变体。[6]，[7]调研了ICP的有效变体算法。对于实时应用，[8]设计了LOAM算法，将提取的边缘和平面特征注册到增量构建的全局地图中。[9]提出了将原始LOAM应用于UGV应用的LeGO算法。通过使用地面提取和点云分割，LeGO滤除了不可靠的特征，并在覆盖有嘈杂物体（例如草木）的区域表现出极大的稳定性。[10]提供了一种有效的闭环机制，可以实时进行大规模建图。
B.松耦合Lidar-IMU
松耦合方法分别处理两个传感器以推断它们的运动约束，这些约束稍后融合（例如，[11]，[12]）。IMU辅助的LOAM[8]将IMU计算的方向和平移作为优化的先决条件。[13]将IMU测量值与姿态估计值结合起来，后者从基于激光雷达的高斯粒子滤波器和预先构建的地图获得。通常，松耦合计算很高效[14]，但是激光雷达和惯性约束的解耦会导致信息丢失[15]。
C.紧耦合Lidar-IMU
紧耦合方法通过联合优化直接将激光雷达和惯性测量融合在一起，可将其分为基于优化的[16]，[17]和基于扩展卡尔曼滤波器（EKF）的[18]，[15]。[19]通过最小化来自IMU和激光雷达的约束来进行局部轨迹优化。[20]提出了LIPS，在IMU预积分约束[21]和激光雷达的平面约束的基础上利用了图优化。[5]提出了LIO建图算法（为简便起见，在以下简称为LIOM），它也是基于图优化的，但具有新的旋转约束建图方法来优化最终的姿势和地图。但是，局部地图窗口中的约束构造和批次优化对于实时应用而言太过耗时。[22]介绍了一种基于2D激光雷达的激光雷达辅助的惯性EKF算法。但是它的应用场景仅限于室内环境，因为它要求所有周围的平面都呈正交结构。
众所周知，EKF容易受到线性误差的影响，线性误差可能导致性能不佳甚至导致发散[23]，[24]。当涉及激光雷达观察到的扫描到扫描约束时，此缺点变得很明显，如果初始姿势不正确并导致错误的特征匹配结果，则该误差被认为是高度非线性的。为了消除由错误匹配引起的误差，我们提出了一个迭代卡尔曼滤波[26]，它可以在每次迭代中反复找到更好的匹配。此外，我们采用误差状态表示法来保证线性化的有效性[27]。该特性将我们的方法与迭代扩展卡尔曼滤波器[25]区别开。
3 激光雷达-惯性里程计与建图
A.系统概况
考虑到配备IMU和3D激光雷达的UGV，我们的目标是估计其6自由度的自身运动并同时建立全局地图，如图1所示。系统框架如图2所示。整个系统包括三个主要模块：-特征提取，LIO和建图。（i）特征提取模块旨在从原始点云中提取稳定的特征。（ii）由传播和更新子模块组成的LIO模块执行迭代卡尔曼滤波，并输出初始里程计以及无畸变的特征。（iii）建图模块通过全局地图优化初始里程计，然后通过使用特征更新地图，以输出新的里程计结果。由于空间问题，我们只专注于里程计模块。我们请读者参考[8]，[9]了解特征提取和建图的详细过程。
B.特征提取
该模块输入原始点云并输出一组边缘特征Fe和一组平面特征Fp。读者可以参见[9]，[8]了解详细的实现。
C.使用迭代ESKF的激光雷达-惯性里程计
LIO模块使用IMU测量和两次连续扫描中提取的特征来估计车辆的相对位姿变换。我们使用以机器人为中心的公式来构建迭代的ESKF，因为它可以防止由于不确定性不断增加而导致的线性误差[28]，[29]。令Fw代表固定的世界坐标系，Fbk代表在第k帧的IMU附加帧，而Flk代表在第k帧的激光雷达点云帧。请注意，在我们的工作中，局部帧始终在先前的激光雷达时间步设置为IMU附加帧。
1)状态定义
定义为Fw相对于Fbk的位姿，定义为当前状态，即描述了从Fbk+1到Fbk的相对位姿：
是Fw相对于Fbk的平移，是为Fw相对于Fbk的旋转单位四元数。和表示了从Fbk+1到Fbk的平移和旋转。是相对于Fbk的速度，ba是加速度偏差，bg是陀螺仪偏差，注意局部重力，gbk也是局部状态的一部分。为了在状态估计[30]中具有良好的特性，一个误差状态的表示被用来求解它的误差向量被定义为：
其中是3自由度的角度误差。
根据ESKF的要求，一旦误差向量被求解，就可以通过将其插入到的状态先验估计中，以获得最终的即：
其中表示四元数乘法，exp将角度向量映射到四元数旋转中。
2）传播
在这一步，如果有新的IMU数据，则预测误差状态，误差状态的协方差矩阵Pk以及状态先验估计。IMU误差状态的线性化的连续时间模型[32]如下：
其中是高斯噪声向量（定义与[16]相同）。Ft是误差状态的转换矩阵，Gt是在t时刻的噪声雅克比矩阵：
其中将一个三维向量变换为它的反对称矩阵。是从t时刻IMU附加帧获得的相对于Fbk的旋转矩阵。和是t时刻的加速度和角速度，并通过原始数据，去除偏差和重力影响进行计算得到：
离散方程(5)推导出以下的传播方程：
其中，分别为连续的IMU时间，Q为w的协方差矩阵，通过离线的传感器标定获得。
为了预测，需要建立以机器人为中心状态的离散时间传播模型。读者可以参照[15]，[27]以查看IMU测量值积分的细节。
3)更新
我们提出了一种迭代更新方案，这是这项工作的主要贡献。
在迭代的卡尔曼滤波中，考虑到先验的偏差和从测量模型中得到的残差函数f(·)，状态更新可以作为一个优化问题[7]，[29]：
其中||·||表示马氏距离，Jk是f(·)对测量噪声的雅克比矩阵，Mk为测量噪声的协方差矩阵。f(·)的输出实际上是根据点-边或点-平面对计算出的堆叠残留向量。给定，f(·)中的误差形式与相关，即Fk+1中的第i个特征点，描述为：
又有
其中是从Fk+1变换到Fk的点。和共同表示了激光雷达和IMU之间的外参（通过离线标定计算得出）。
公式13的物理解释如下。对于一个边缘点，它表示了其到对应边缘的距离。对于一个平面点，它表示了其到对应平面（由三个点定义）的距离。如何选择这三个点的细节可以参考[8]。
公式12通过以下的迭代更新公式进行求解：
其中表示第j次迭代的修正向量。Hk,j是对的雅克比矩阵。注意到，对于每次迭代，寻找新的匹配边和匹配面以最小化误差，然后计算新的H,J和K矩阵。当第n次迭代小于一个确定阈值时，通过如下公式更新Pk：
使用公式4可以获得最终的原始的畸变特征可以通过估计的相对位姿进行畸变校正。
最后，我们进行下一帧状态的初始化：
q0表示单位四元数，和可以通过和
计算得到。注意，关于速度，偏差和局部重力的协方差保留在协方差矩阵中，而与相对位姿相对应的协方差设置为零，即，对于机器人中心坐标系的本身参考不存在不确定性。
4)状态组合
在机器人中心的公式中，每次更新完成后，需要更新全局位姿，通过如下的组合步骤：
5）初始化
如第三章节-C所述，以机器人为中心的公式可以促进滤波器状态的初始化。关于初始参数设置，在我们的实现中，（i）初始加速度偏差和激光雷达-IMU外参是通过离线标定获得的，而初始陀螺仪偏差是相应静态测量值的平均值，（ii）初始侧倾和俯仰是从移动前的无偏加速度测量中获得的；（iii）通过使用（ii）中的初始侧倾和俯仰，将导航帧中表示的重力向量转换到当前局部帧，来获取初始的局部重力。
4 结果
我们在具有2.4GHz四核和8Gib内存的便携式计算机上评估LINS在不同情况下的性能，并将其与LeGO[9]，LOAM[8]和LIOM[5]进行比较。所有方法都用C++实现，并在Ubuntu Linux中使用机器人操作系统（ROS）[33]执行。在以下实验中，LINS的建图模块由LeGO[9]中提出的建图算法实现。之前的大多数文献仅分析了最终轨迹的性能，即地图模块已进行优化了的里程计。但是，我们发现初始里程计，即里程计模块的输出，对整体性能有很大影响。因此，我们将两者都考虑在内。为了区分这两种测距法，我们将地图优化的里程计称为地图精制里程计（MRO），初始里程计称为纯里程计（PO）。
A. 室内实验
在室内测试中，选择一个停车场作为实验区域，如图4（a）所示。我们将传感器套件安装在大巴上，如图4（b）所示，其中RS-LiDAR-16安装在顶部，IMU放置在大巴内部。图5（a），5（b）和5（c）分别提供了LINS，LeGO和LOAM的结果。尽管我们没有地面真值，但我们仍然可以目视检查LINS-PO的轨迹是否可以与MRO轨迹精确对齐（通常，MRO在室内几乎没有漂移，并且比PO更准确），而LeGO-PO和LIOM-PO都在偏航角上有明显的漂移。
B.大尺度的室外实验
为了验证通用性和稳定性，在四个室外应用场景中进行了实验：城市，港口，工业园区和森林。图3展示了LINS生成的一些环境照片和相应的地图。我们测量了GPS接收器产生的地面真相与提供的估计位置之间的差距通过每种方法来指示漂移量，然后将其与行进距离进行比较以产生相对漂移。实验结果列于表I。
图3 （a）宽阔而开放的港口区域；（b）存在许多建筑物，树木和汽车的工业园区；（c）穿过森林区域的整洁道路照片和相应的地图（由LINS制作）。LINS在所有测试环境中均表现良好。
图4.室内测试的传感器配置。（a）LINS建造的停车场地图。（b）激光雷达安装示意，IMU固定在大巴上。
总而言之，LINS在所有测试的场景中均表现良好。特定环境的详细分析如下。
1）港口实验：我们评估了广东某港口的LINS效果。该传感器套件包括一个Velodyne VLP-16激光雷达和一个固定在汽车顶部的Xsens MTi-G-710 IMU。地面真实轨迹由GPS模块提供。我们从一个集装箱包围的路径开始记录数据。汽车驶向码头，然后行驶了1264米的距离，回到了原来的地点。值得一提的是，这些集装箱会不断地进出，从而改变了全局地图，这可能会破坏MRO的性能。
表1 运动估计的相对漂移误差
根据表一，我们发现LINS和LIOM的漂移最少。LINS-MRO的相对漂移为1.56％，略高于LIOM的1.40％，而LINS-PO的相对漂移仅为2.75％。结果表明，IMU和激光雷达的组合可以有效地提高精度。尽管LOAM和LeGO的相对漂移似乎很小，但它们可能会产生巨大的方向误差。图6（a）和6（b）提供了LeGO和LINS的详细轨迹和地图。与地面真值（绿线）相比，我们发现LeGO（包括MRO和PO）的轨迹在第一次转弯时转向了错误的方向。我们还可以目视检查图6（c）中由LeGO构建的地图发生了变形。相比之下，LINS表现出与地面真实轨迹的良好对齐，并且生成的地图对真实环境具有很高的保真度。即使在特征不足的第一次转弯中（每次扫描仅可使用约30个边缘特征），LINS仍然表现出色，这表明我们的算法对特征稀疏的场景更为鲁棒。
图5 （a）LINS，（b）LeGO和（c）LOAM在室内实验中得到的轨迹。我们观察到，所有方法的MRO轨迹看起来都很相似，但是它们的PO轨迹完全不同。与其他方法相比，LINS的PO轨迹与其MRO轨迹对齐的更好。
图6 LeGO和LINS估算的轨迹和地图。请注意，PO轨迹以蓝线绘制，MRO轨迹以红线绘制，GPS地面真值以绿线绘制。我们看到，LINS的轨迹接近于地面真实情况，因此生成的地图的保真度高于LeGO。
2）城市实验：我们使用图4（b）中相同的传感器套件进行了城市实验。GPS接收器产生的位置被用作地面真值。值得一提的是，在此场景中，每次扫描的平均边缘特征数仅为56，这是所有测试场景中最低的。
图7 在城市实验中通过不同方法生成的MRO轨迹，以红色线绘制。
图8. LINS和LIOM的MRO和PO之间的绝对轨迹误差比较。
我们首先看一下LeGO和LOAM的输出，分别在图7（b）和7（c）中可以看到。我们观察到几乎每次转弯都会发生巨大的方向误差。图7（a）展示了在同一数据集上运行时LINS的结果。生成的轨迹与现实世界的道路显示出良好的对齐，这证实了LINS即使在特征稀疏的场景中也可以稳定运行。LINS的最终MRO和PO漂移分别为1.79％和4.42％，非常接近LIOM的MRO和PO漂移，分别为1.76％和4.44％。此外，与图7（d）所示的LIOM轨迹相比，以及图8（a）和8（b）所示的绝对轨迹误差（ATE），我们可以看到LINS的性能就准确性而言，接近LIOM的性能。我们分析了LIOM在建图步骤中受益于旋转约束的细化，从而导致MRO结果的准确性更高。
C.运行时间比较
表2比较了LINS和LIOM中激光雷达惯性里程计模块的平均运行时间。我们看到LINS的速度比LIOM快得多，LINS处理一次扫描所需的时间少于30毫秒，而LIOM始终需要100毫秒以上。在某些极端情况下，例如停车场，那里的特征非常丰富，LIOM最多需要223毫秒，而LINS仅需要25毫秒。结果表明，LINS的实时性优于LIOM。
表2 每帧LIO模块的运行时间
LIOM的LIO模块中最耗时的部分是局部地图约束构造和批处理优化，其中它在多个激光雷达扫描中维护局部地图，并通过MAP估计求解所有的相对状态。LINS出色的计算速度背后的主要原因是，它使用卡尔曼滤波器而不是批处理MAP，因为卡尔曼滤波器通过按时间顺序分解批解决问题，并以递归形式求解来隐式减小了优化问题的维度[25]。另一个原因是我们仅使用来自先前激光雷达扫描的点云进行匹配。这样，尽管使用的点云比LIOM中构建的本地地图稀疏，但我们仍可以借助IMU来获得准确的结果。
5 结论
在本文中，我们开发了一种用于机器人导航的轻量级激光雷达惯性状态估计器。通过使用以机器人为中心的公式化的迭代ESKF，我们的算法能够在具有挑战性的环境下提供实时，长期，可靠和高精度的自身运动估计。该算法在城市，港口，工业园区，森林和室内停车场等各种场景中得到了验证。实验结果表明，LINS优于仅使用激光雷达的方法，并与最先进的激光雷达惯性里程计算法的性能相当，而计算成本却更低。
参考文献：
END
联系人：唐老师
电话：021-69589116
邮箱：20666028@tongji.edu.cn
广告
广告
编辑推荐
最新资讯
-
标准立项 ||《乘用车智能底盘矢量及舒适加
2024-11-06 07:38
-
博世新一代SiC功率模块PM6：重新定义车规级
2024-11-06 07:37
-
直播｜革新汽车仿真：探索 VI-grade 2025.1
2024-11-05 16:03
-
用于高压电池测试的加速度计
2024-11-05 14:44
-
卡特彼勒新一代240吨电动矿卡投入使用
2024-11-05 14:43
广告
广告
