【论文阅读】LVI-SAM: Tightly-coupled Lidar-Visual-Inertial Odometry via Smoothing and Mapping

作者：Tixiao Shan et al.

机构：MIT

来源：ICRA 2021

🏷️摘要

我们提出了一个通过平滑和建图实现激光-视觉-惯性里程计的紧耦合框架LVI-SAM，其实现了高精度和鲁棒性的实时状态估计和地图构建。LVI-SAM采用因子图，由两个子系统组成：一个视觉惯性系统VIS和一个激光雷达惯性系统LIS。这两个子系统以紧耦合的方式，其中VIS利用LIS的估计进行初始化。利用激光雷达测量提取视觉特征的深度信息，提高了VIS的精度。反过来，LIS利用VIS提供初值进行scan-matching，并作为点云配准的初始值。首先由VIS进行回环检测，再由LIS进一步细化。当两个子系统中的一个发生故障时，LVI-SAM也可以发挥作用，这增加了它在无纹理和无特征环境中的鲁棒性。LVI-SAM 对从多个平台在各种规模和环境中收集的数据集进行了广泛评估。

🏷️前人工作总结

基于Lidar的方法可以远距离捕捉环境的细节。然而，当在诸如长走廊或平坦开阔的场地等无结构环境中操作时，此类方法通常会失败。尽管基于视觉的方法特别适合地点识别并且在纹理丰富的环境中表现良好，但它们的性能对光照变化、快速运动和初始化很敏感。因此，基于激光雷达和基于视觉的方法通常都与惯性测量单元 (IMU) 相结合，以提高它们各自的鲁棒性和准确性。激光雷达-惯性系统可以帮助点云去畸变并在短时间内解决缺乏结构特征的问题。视觉-惯性系统通过 IMU 测量可以恢复公制尺度和姿态。为了进一步提高系统性能，激光雷达、摄像头和 IMU 测量的融合越来越受到关注。

VIO可以分为两大类：基于滤波的和基于优化的。基于滤波的方法通常使用扩展卡尔曼滤波器 (EKF) ，使用相机和 IMU 的测量值来递推系统状态，如MSCKF、ROVIO、Open-VINS。基于优化的方法维护一个滑动窗口估计器并最小化视觉重投影误差以及 IMU 测量误差，如OKVIS(双目)、VINS-Mono(单目)。VINS-Mono 在滑动窗口设置中执行非线性优化，并使用单目相机实现SOTA精度。

LIO也可以分为两大类：松耦合和紧耦合。LOAM和LeGO-LOAM是松耦合系统，因为IMU测量不参与优化。紧耦合系统通常提供更高的准确性和鲁棒性，是目前的研究热点。在公开可用的紧耦合系统中。LIO-mapping采用了VINS-Mono的优化流程，并最小化了 IMU 和激光雷达测量的残差。由于LIO-mapping优化所有测量，因此没法保证实时性。LIO-SAM 通过引入激光雷达关键帧的滑动窗口来限制计算复杂度，利用因子图联合 IMU 和激光雷达约束进行优化。LINS专为地面车辆设计，使用误差状态卡尔曼滤波器递归地校正机器人的状态。

🏷️主要工作及贡献

我们利用因子图进行全局优化，可以通过回环检测定期消除机器人产生的漂移。在本文中，我们提出了一个通过平滑和映射实现紧密耦合激光雷达视觉惯性里程计的框架 LVI-SAM，用于实时状态估计和建图。LVI-SAM 建立在因子图之上，由两个子系统组成，一个视觉惯性系统 (VIS) 和一个激光雷达惯性系统 (LIS)。当检测到其中一个子系统出现故障时，这两个子系统可以独立运行，或者在检测到足够多的特征时联合运行。VIS 执行视觉特征跟踪，并可选择使用激光雷达帧提取特征深度。通过优化视觉重投影和 IMU 测量误差获得的视觉里程计可作为激光雷达scan-matching的初始猜测，并将约束引入因子图中。在使用 IMU对点云进行去畸变后，LIS提取激光雷达边缘和平面特征，并将它们与在滑动窗口中维护的特征图相匹配。LIS 中估计的系统状态可以发送到 VIS 以促进其初始化。对于回环检测，候选匹配首先由 VIS 识别，并由 LIS 进一步优化。视觉里程计、激光雷达里程计、IMU 预积分和回环的约束在因子图中联合优化。最后，利用优化的IMU偏差项来传播IMU测量值，以IMU帧率进行姿态估计。我们工作的主要贡献可以总结如下：

  • 一个基于因子图构建的紧耦合LVIO框架，在位置识别的辅助下实现多传感器融合和全局优化。

  • 我们的框架通过故障检测绕开故障子系统，使其对传感器退化具有鲁棒性。

  • 我们的框架已在不同规模、平台和环境中收集的数据进行了广泛验证。

从系统的角度来看，我们的工作是新颖的，代表了VIO和LIO中最先进技术的独特集成，以实现LVIO系统，提供更高的鲁棒性和准确性。我们希望我们的系统可以作为一个坚实的基线，其他人可以轻松地在此基础上推进激光雷达视觉惯性里程计的最新技术。

项目地址：https://github.com/TixiaoShan/LVI-SAM

🏷️算法流程

我们的框架由两个子系统组成：视觉惯性系统 (VIS) 和激光雷达惯性系统 (LIS)。 VIS 处理图像和 IMU 测量，激光雷达测量是可选的。视觉里程计是通过最小化视觉和 IMU 测量的联合残差来获得的。LIS 提取激光雷达特征并通过将提取的特征与特征图匹配来执行激光雷达里程计。为了实时性能，特征图以滑动窗口方式维护。最后，通过联合优化 IMU 预积分约束、视觉里程计约束、激光雷达里程计约束和回环约束在使用 iSAM2的因子图中的贡献来解决状态估计问题，该问题可以表述为最大后验 (MAP) 问题。LIS 中采用的多传感器图优化旨在减少数据交换并提高系统效率。

🏷️VIS视觉惯性系统

1.Initialization 改进VIO初始化

VIO初始化的质量在很大程度上取决于两个因素：传感器的初始运动和IMU参数的准确性。当传感器以较小或恒定速度移动时，VINS常常无法初始化。这是因为当加速度激励不够大时，度量尺度是不可观测的。IMU参数包括缓慢变化的bias和white noise，这会影响原始加速度和角速度测量。在初始化时准确估计这些参数有助于优化更快地收敛。因为从激光雷达可以直接观察到深度，所以首先初始化LIS并获得系统状态x和IMU偏差b。然后，根据图像时间戳插值并将它们与每个图像关键帧关联。注意，假设两个图像关键帧之间的IMU偏移是恒定的。最后，使用LIS中估计的x和b作为VIS初始化的初始估计，提高了初始化速度和鲁棒性。

2. Feature depth association 特征深度关联

堆叠了多个激光雷达帧，以获得一个密集的深度图，将视觉特征和激光雷达深度点投影到以摄影机为中心的单位球体上，然后，使用球体上恒定密度的极坐标对深度点进行下采样和存储。通过在极坐标上使用二维K-D树搜索，找到视觉特征在球体上最近的三个深度点，特征深度是由视觉特征和照相机中心Oc形成的线的长度，它与笛卡尔空间中由三个深度点形成的平面相交。该过程如图3(a)中，其中特征深度是虚线直线的长度。

因为叠加来自不同时间戳的激光雷达帧可能会导致来自不同对象的深度模糊，图 3(b) 显示了这种情况，ti时刻观察到的深度点以绿色表示。将相机移到tj时刻的新位置，观察新的深度点以灰色表示。灰色虚线环绕的绿点，由于激光雷达帧的叠加仍可观察到，使用不同对象的深度点关联视觉特征会导致不准确的估计。通过检查三个特征深度点之间的最大距离来拒绝此类估计。如果最大距离大于2m，则该视觉特征没有关联的深度。图(a)和图(c)表示深度图和RGB图像配准后的结果，在（a）和（c）中，深度图的颜色变化表示深度变化。在 (b) 和 (d) 中，绿点是与深度成功关联的视觉特征，红色是关联失败的视觉特征。可以看到尽管深度图覆盖了图 4(a) 中的大部分图像，但由于验证检查失败，4(b) 中位于窗口角落的许多视觉特征与深度关联失败。

3. Failure detection 故障检测

当发生剧烈运动，光照变化，无纹理环境，VIS系统会出现故障。当机器人剧烈运动或进入无纹理环境中时，跟踪的特征点数量会大大减少。特征不足可能导致优化发散。因此，当跟踪特征点的数量低于一定阈值时，或估计出的IMU的bias超过一个阈值时，我们会认为VIS 故障。为了不破坏LIS的功能，主动故障检测在我们的系统中是必要的，一旦检测到故障，VIS会重新初始化并通知LIS。

4. Loop closure detection 回环检测

我们利用 DBoW2 进行回环检测。对于每个新的图像关键帧，我们提取简要描述符 并将它们与先前提取的描述符进行匹配。DBoW2 返回的回环候选图像时间戳被发送到 LIS 进行进一步验证。

🏷️LIS雷达惯性系统

LIS在LIO-SAM基础上修改，四种类型的约束被添加到图中并联合优化：IMU 预积分约束、视觉里程计约束、激光里程计约束和回环约束。激光里程计来自scan-matching，将当前激光雷达关键帧与全局特征地图相匹配获得。回环约束的候选首先由VIS提供，在通过scan-matching进一步优化。我们为特征地图维护一个激光雷达关键帧的滑动窗口，这保证了有限的计算复杂度，当机器人位姿的变化超过阈值时，将选择一个新的激光雷达关键帧，当机器人位姿的变化超过阈值时，将选择一个新的激光雷达关键帧，位于相邻两关键帧之间的激光雷达帧将被丢弃。在选择新的激光雷达关键帧后，新的机器人状态 x 作为节点添加到因子图中。以这种方式添加关键帧不仅可以在内存消耗和地图密度之间取得平衡，还有助于保持相对稀疏的因子图以进行实时优化。由于篇幅限制，我们请读者参考LIO-SAM了解实现细节。在以下部分中，我们将重点介绍提高系统鲁棒性的新方法。

1. Initial guess 初始估计

我们发现初始估计在scan-matching的成功中起着至关重要的作用，尤其是当传感器经历剧烈运动时。 LIS 初始化前后的初始猜测来源不同。

在LIS初始化之前，我们假设机器人从零速度的静止状态开始，然后假设IMU的偏置和噪声为0，积分IMU的原始测量。两个激光雷达关键帧之间的预积分平移和旋转变化给scan-matching提供了一个初始估计。我们发现，当初始线速度小于10m/s，角速度小于180°时，该方法可以在具有挑战性的条件下成功地初始化系统。一旦LIS初始化成功，我们估计因子图中的IMU bias、机器人姿态和速度。然后我们把它们发送到VIS来帮助它的初始化。

在LIS初始化之后，我们可以从两个来源获得初始估计：(1) 带有校正bias的积分IMU测量；(2) VIS

当VIS可用时，我们用它作为初始猜测，如果VIS发生故障，切换到IMU测量以进行初始估计。这些程序在纹理丰富和缺失的环境下都能提高初始估计的准确性和鲁棒性。

2. Failure detection 故障检测

激光雷达虽然能捕获环境远距离的细节，但仍然会遇到scan-matching受限的退化场景，如图6所示，（a）和（c）中，激光雷达面向地面放置，在（b）和（d）中，激光雷达处于平坦、开放和无结构的场景中，白点表示激光雷达扫描的内容，颜色变化表示高度变化。

scan-matching中的非线性优化问题可以表述为迭代求解线性问题：

A和b是在T处线性化得到，当$\mathbf{A^TA}$在优化的第一次迭代中小于一定阈值，我们认为LIS故障，激光雷达里程计约束不会加入因子图优化中。我们建议读者参考 [24] 以获得这些假设所依据的详细分析。

🏷️实验

我们在三个自制数据集上做了实验，分别是Urban, Jackal, Handheld，我们将提出的框架与开源方案进行比较，包括 VINS-Mono、LOAM、LIO-mapping、LINS 和 LIO-SAM，所有方法都在 C++ 中实现，并在带有 Intel i7-10710U 的 Ubuntu Linux 笔记本电脑上执行。

1.消融实验

A1-包含来自激光雷达的特征深度信息对视觉惯性里程计

A2-包含视觉惯性里程计

A3-包含来自激光雷达的特征深度信息对激光雷达视觉惯性里程计

A4-包含视觉回环检测

2. Jackal数据集

我们在功能丰富的环境中手动驱动机器人(UGV)，在同一位置开始和结束。如图 8(a) 所示，环境以结构、植被和各种路面为特征。可以接收 GPS 的地区标有白点。LINS 实现了最低的端到端位置误差，LVI-SAM 次实现了最低的端到端旋转误差。

3. Handheld 数据集

手持数据集由携带传感器套件的操作员在几个空旷的场地周围走动收集，数据集也在同一位置开始和结束。数据集中包括一个开放棒球场来增加挑战（位于图片8(b)上中部）。当通过该场景时，相机和激光雷达收集的主要观察结果分别为草地和地平面（图 6（b）和（d））。由于退化问题，所有基于激光雷达的方法都发生了失效。我们的框架 LVI-SAM 在启用或不启用回环的情况下都成功完成了测试，在表II中列出的三个测试标准中都实现了最低的误差。