【LVI-SAM论文全文翻译】： LVI-SAM: Tightly-coupled Lidar-Visual-Inertial Odometry via Smoothing and Mapping

---

写在前面

本文参考：惊鸿一博

作者简介：LVI-SAM是Tixiao Shan的最新力作，Tixiao Shan是Lego-loam(基于激光雷达里程计的SLAM框架)和Lio-sam(基于惯性-雷达紧耦合的SLAM框架)的作者，LVI-SAM是Tixiao Shan最新开源的基于视觉-激光-惯导里程计SLAM框架，结合了io-sam和Vins-Mono的视觉-激光-惯导融合的SLAM框架。

论文原文：论文原文链接

开源代码:开源代码链接

演示视频:

lidar-vision-imu slam: lvi-sam vs LIO-SAM vs LeGO-LOAM

摘要

本文提出了一个紧耦合的雷达视觉惯导SLAM系统，可以实时高精度鲁棒的进行状态估计和建图。LVI-SAM构建在因子图之上，并且由两个子系统组成：一个视觉惯导系统和一个激光惯导系统。这两个子系统利用紧耦合的方法，视觉惯导的系统利用激光惯导的估计来做初始化。视觉惯导系统利用激光雷达的测量给视觉特征提供深度来提升精度。同时激光惯导系统利用视觉惯导的估计值作为初值来做帧的匹配。利用视觉做闭环检测，把检测的结果给激光惯导系统来做优化。当激光惯导和视觉惯导子系统的一个失败的时候LVI-SAM仍然可以工作，这极大的提升了系统在缺少纹理或者特征场景中的鲁棒性。LVI-SAM系统在数据集上进行了测试，取得了很好的效果。

一、引言

同时定位和地图构建(SLAM)是许多移动机器人导航任务所需的基础功能。在过去的20年里，使用SLAM在单个感知传感器等具有挑战性的设置下，使用激光雷达或相机进行实时状态估计和地图构建，取得了巨大的成功。基于Lidar的方法可以远程捕获环境的细节。然而，这种方法通常在无结构环境中运行时失败，如长走廊或平坦的空地。虽然基于视觉的方法特别适合于位置识别，并且在纹理丰富的环境中表现良好，但它们的性能对光照变化、快速运动和初始化都很敏感。因此，基于激光雷达和基于视觉的方法通常都与惯性测量单元(IMU)耦合，以提高其各自的鲁棒性和准确性。激光雷达惯性系统可以帮助纠正点云失真，并解释在短时间内缺乏特征。度量尺度和态度可以通过IMU测量来恢复，以帮助一个可视化的惯性系统。为了进一步提高系统性能，激光雷达、照相机和IMU测量的融合吸引了越来越多的关注。

这一段介绍VIO系统,最好的是VINS-MONO

我们的工作与视觉惯性测速(VIO)、激光雷达惯性测速(LIO)和激光雷达可视化惯性测速(LVIO)最密切相关。我们注意到，我们在本文中没有考虑非惯性系统，尽管我们意识到有成功的非惯性激光雷达视觉系统，如[1]，[2]。视觉惯性测速法(VIO)可分为两大类：基于滤波器的方法和基于优化的方法。基于滤波器的方法通常使用扩展的卡尔曼滤波器(EKF)，使用来自相机和IMU的测量值来传播系统状态。基于优化的方法保持了一个滑动窗口估计器，并最小化了视觉重投影误差和IMU测量误差。在我们的工作中，我们只考虑单目相机。在最流行的公开VIO管道中，MSCKF[3]、ROVIO[4]和Open-VINS[5]是基于过滤器的，而OKVIS[6]、Kimera[7]和VINS-Mono[8]是基于优化的。虽然OKVIS使用立体声相机显示出优越的性能，但它并没有针对单目相机进行优化。VINS-Mono在滑动窗口设置中执行非线性优化，并使用单目相机[9]实现了最先进的精度。

这一段介绍LIO系统,最好的是LIO-SAM

根据激光雷达惯性测速法的设计方案，激光雷达惯性测速法还可以分为两大类：松散耦合方法和紧密耦合方法。LOAM[10]和LeGO-LOAM[11]是松散耦合的系统，因为在优化步骤中不使用IMU的测量值。紧耦合系统，通常提供提高系统的准确性和鲁棒性，是目前正在进行的[12]研究的主要重点。在公开可用的紧密耦合系统中，LIO-mapping[13]适应了[8]的优化方式，并最小化了IMU和激光雷达测量的残差。因为LIO-mapping的设计旨在优化所有度量，因此无法实现实时性能。LIO-SAM[14]通过引入激光雷达关键帧的滑动窗口来限制计算复杂度，利用一个因子图进行联合IMU和激光雷达约束优化。专门为地面车辆设计的LINS[15]使用错误状态卡尔曼滤波器来递归地纠正机器人的状态。

近年来，激光雷达视觉惯性系统在传感器退化任务[16]中的鲁棒性引起了越来越多的关注。[17]提出了一种以顺序处理管道为特征的紧密耦合LVIO系统，解决了从粗到细的状态估计问题。粗估计从IMU预测开始，然后通过VIO和LIO进一步细化。[17]目前在KITTI基准测试[18]上实现了最先进的精度。基于MSCKF的框架，[19]具有在线时空多传感器校准功能。[17]和[19]的实现不可公开。我们的工作不同于上述的工作，因为我们利用一个因子图进行全局优化，它可以通过循环闭合检测定期消除机器人产生的漂移。

本段介绍了LVI-SAM使用的方法

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

• 实现了一个激光-视觉-惯性的紧耦合系统,通过因子图实现多传感器的融合,全局优化和回环检测；
• 通过故障检测机制,避免了单一子系统故障,导致系统不能使用的情况,提高了系统的鲁棒性；
• 利用不同的数据集进行了完善的验证。

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

二、完整的激光视觉惯导SLAM系统

A.系统概述

系统接受3D激光点云、单目图像和IMU信息作为输入,如图1所示。我们的框架由两个关键的子系统组成：视觉惯性系统(VIS)和激光雷达惯性系统(LIS)。VIS系统接受图像和IMU信息，雷达点云是可选的输入。通过最小化IMU和视觉的测量残差来得到视觉里程计。激光里程计通过最小化检测到的线面特征到特征图的距离得到。特征图保存在一个滑窗中来实时的执行。最后状态估计问题，可以公式化为一个MAP问题。利用iSAM2来优化因子图中IMU预积分，视觉里程计，激光里程计和闭环的约束的残差。需要注意的是，LIS中采用的多传感器图优化旨在减少数据交换并提高系统效率。

B. 视觉惯导系统

我们为我们的VIS调整了来自[8]的处理管道，如图2所示。视觉特征使用角点检测器[22]检测，并由Kanade–Lucas–Tomasi算法[23]跟踪。在VIS初始化后，我们利用视觉里程计对齐激光帧的点云得到一个稀疏的深度图来得到特征的深度完成视觉惯导初始化。系统在滑窗中执行BA，其中系统状态x∈X可以写为：

R是SO3表示旋转矩阵，p表示位置，v表示速度，b=[b_a, b_w]是IMU的偏置。变换矩阵T是SE3表示从机体坐标系到世界坐标系的变换 。下边将详细的介绍VIS的初始化和特征深度估计。读者们可以读VINS-Mono的原文来了解细节。

1）初始化：基于优化的VIO系统由于很强的非线性导致初始化发散。初始化的质量取决于两个主要因素：初始化的传感器运动和IMU参数的精度。在实际的实验中，我们发现VINS很容易在系统速度很小或者匀速的时候初始化失败。这是由于加速度激励不够大导致尺度不可观。IMU的参数包含缓慢变化的偏差和高斯白噪声。这影响原始加速度和角速度的测量，在初始化的时候好的初值可以帮助系统快速的优化。

为了改善VIS初始化的鲁棒性，我们利用LIS系统来估计系统的状态X和IMU的偏置b。因为直接可以通过雷达的观测得到深度信息。我们首先初始化LIS系统获得x和b。然后我们通过插值来把激光帧和视觉帧通过时间戳对齐。假设IMU的偏置在两个图象关键帧之间是常数。最后把LIS系统初始化得到的x和b作为VIS初始值，这很有效的改善初始化的速度和鲁棒性。

2）深度特征关联：根据VIS初始化的结果，我们利用视觉里程计对齐视觉帧和雷达帧。由于当前的3D lidar扫描的是稀疏的点，我们把多帧激光点云组合起来得到一个稠密的深度图。为了把特征和深度值关联起来，我们把视觉特征和雷达点投影到以相机为圆心的单位圆上。然后对深度点进行降采样并利用极坐标进行保存,保证点的密度是常数的。我们利用二维的KD-tree（极坐标）来找视觉特征点周围最近的三个点，最后特征点的深度计算的是相机中心和特征点的连线的长度，这条线和三个深度点得到的平面在笛卡尔坐标系中有交点。可视化如下图3(a)所示，特征深度是虚线的长度。

我们通过检查和特征点最近的三个深度点之间的距离来进一步验证关联的特征深度。这是因为堆叠来自不同时间戳的激光雷达帧可能会导致来自不同对象的深度模糊。这种情况的图示在图3(b)中。在时间ti处观察到的深度点以绿色表示。相机在tj移至新位置并观测到了灰色的深度点。但是在ti观测到的被灰色线框起来的点，由于点云堆叠在tj时刻可能还是能看到。所以使用来自不同对象的深度点来关联特征深度会导致估算不准确。与类似[17]中的做法一样，我们通过检查特征周围深度点之间的最大距离，如果最大距离大于2m，就不对该特征点进行数据关联。

图4显示了对齐的深度图和视觉特征。图4的(a)和©，利用视觉里程计对齐点云得到深度图投影到了图像上。在图4(b)和(d)上，通过深度图恢复视觉特征的被标记成了绿色。需要注意的是，虽然图4(a)中深度点覆盖了图像的大部分区域，但是由于图4(b)中的很多特征落在窗户的角上导致深度关联检验失败。

3）失败检测：如果运动变化剧烈，光照变化或者环境缺少纹理就会导致VIS系统失败。这是由于在这种场景中跟踪的特征点会显著减少，而特征太少会导致优化失败。当VIS系统失败的时候会导致IMU的bias很大，所以当VIS系统跟踪的特征点少于一个阈值或者IMU估计出来的bias大于一个阈值的时候我们认为VIS系统失败。主动失效检测对于本文的系统是必需的，以便其故障不会影响LIS系统。一旦检测到故障，VIS将重新初始化并通知LIS。

4）闭环检测：本文利用DBoW2来做闭环检测，对于每一个新来的图像关键帧，检测出BRIEF描述子并把他和原来检测的描述子匹配。通过DBoW2检测到的闭环候选帧的时间戳会给到LIS系统来做进一步的验证。

C.雷达惯导系统

如图5所示，提出的雷达惯导系统是基于LIO-SAM[14]的，主要是利用因子图来做全局的位姿优化。主要有四种约束，IMU的预积分约束，视觉里程计的约束，雷达里程计的约束和闭环检测的约束加入到因子图中参与优化。激光雷达里程计约束来自扫描匹配，在这里，我们将当前的激光雷达关键帧与全局特征图进行匹配。闭环检测的约束候选帧首先由VIS提供，然后通过扫描匹配进一步优化。我们为特征图维护了一个激光雷达关键帧的滑动窗口，这保证了有限的计算复杂性。当机器人位姿变化超过阈值时，将选择一个新的激光雷达关键帧。丢弃位于关键帧之间的普通激光雷达帧。选择新的激光雷达关键帧后，新的机器人状态x将作为节点添加到因子图中。以这种方式添加关键帧，不仅可以在内存消耗和地图密度之间取得平衡，而且还有助于维护相对稀疏的因子图以进行实时优化。可以通过LIO-SAM的论文了解细节。在以下各节中，我们重点介绍提高系统鲁棒性的新策略。

1）初始值估计：通过实验发现初始值对于连续的扫描匹配有着重要的作用，尤其是在剧烈运动的情况下。在LIS系统初始化之前和之后初值的来源是不同的。

在LIS系统初始化之前，我们假设机器人在初始位置静止，然后假设IMU的bias和噪声都为0，对IMU原始值积分得到两个雷达关键帧的位置和姿态信息作为scan-match的初值。经过实验发现这种方法能在有挑战的场景（初始速度小于10m/s，角速度小于180°/s）中初始化系统。一旦LIS系统初始化完成，我们估计因子图中的IMU的bias，机器人的位姿，速度。然后把这些量传到VIS系统中完成其初始化。

在LIS系统初始化完成之后，我们可以通过两个途径得到Scan-match的初值：IMU的积分和VIS系统。当VIS系统的里程计可以输出位姿时我们以他为初值，如果VIS系统报告失效的时候，我们利用IMU的积分作为初值。这个过程在纹理丰富和缺乏纹理的环境中都增加了初值的精度和鲁棒性。

2）失败检测：尽管激光雷达可以得到场景中很远范围内的细节，但是也会在一些场景中导致扫描匹配失败，如图6所示。我们利用[26]论文中介绍的方法来检测LIS系统是不是失败了。扫描匹配中的非线性优化问题可以表示为迭代求解一个线性问题：


其中A和b是由T处的线性化得到的。当$A^{T}A$的最小特征值小于第一次优化迭代时的阈值时，LIS报告失败。当发生故障时，激光雷达里程计约束不会添加到因子图中。我们请读者参考[26]进行这些假设所基于的详细分析。

三、实验

我们现在描述了一系列的实验，以在三个自收集的数据集上验证所提出的框架，这些数据集被称为Urban, Jackal, and Handheld.。以下各部分提供了这些数据集的详细信息。我们用于数据收集的传感器套件包括一个弹性差Velodyne VLP-16 lidar, a FLIR BFS-U3-04S2M-CS相机、一个MicroStrain 3DM-GX5-25 IMU和一个ReachRS+GPS（为地面真相）。我们将所提出的框架与开源解决方案进行了比较，其中包括VINS-Mono、LOAM、LIO-mapping、LINS和LIO-SAM。所有的方法都是在C++中实现的，并在UbuntuLinux中使用Inteli7-10710U的笔记本电脑上执行。我们的LVI-SAM和数据集的实现可在下面的链接中获得.

A.消融研究

我们展示了系统中每个模块的设计如何影响使用城市数据集的提议框架的性能。该数据集以建筑物、停放和移动的汽车、行人、骑自行车者和植被为特色，由操作员步行和携带传感器套件收集。我们还故意将传感器套件放置在具有挑战性的位置(图6(a))，以验证系统在退化场景下的鲁棒性。由于头顶植被茂密，该地区被gps拒绝了。我们在相同的位置开始和结束数据收集过程，以验证端到端平移和旋转错误，如表1所示。

1)A1-包含激光雷达特征深度信息的视觉惯性测里程计的影响：我们禁用LIS中的扫描匹配，并依靠VIS只执行姿态估计.启用和不启用深度配准的结果轨迹在图7中被标记为A1。运动轨迹的方向是按时钟计算的。当将深度与视觉特征相关联时，如表一所示的端到端姿态误差大大减少。

2)视觉惯性里程计的影响：我们禁用VIS，只使用LIS进行姿态估计。图7中标记为A2，当遇到退化场景时，轨迹会发散几次。

3)A3-激光雷达视觉惯性里程计中的激光雷达的特征深度信息的影响：我们现在一起使用VIS和LIS，切换VIS中的深度配准模块来比较得到的LVIO轨迹。借助视觉特征的深度，翻译误差进一步降低了29%，从45.42米降低到32.18米。注意，在此测试中禁用环路闭合检测，以纯测速模式验证系统。

4)A4-视觉环闭合检测的影响：我们通过在VIS中启用环闭合检测功能来消除系统的漂移。当在框架中启用每个模块时，最终的轨迹在图7中被标记为A4。

B. Jackal Dataset

Jackal Dataset是通过将传感器套件安装在Clearpath Jackal无人驾驶地面车辆(UGV)上来收集的。我们在一个功能丰富的环境中手动驱动机器人，开始和结束在同一位置。如图8(a)所示的环境以结构、植被和各种路面为特征。GPS接收可用的区域用白点标记。

我们比较了各种方法，并在图9(a)中显示了它们的轨迹。我们通过手动禁用和启用它，进一步验证了具有循环闭包功能的方法的准确性。基准测试结果见表二。与GPS测量相比，LVI-SAM的平均均方根误差(RMSE)最低，这被视为地面真值。最低的端到端平移误差是由LINS实现的，它改编自LeGOLOAM[11]，是专门为UGV操作设计的。LVI-SAM再次实现了最低的端到端旋转误差。

C. Handheld Dataset

Handheld dataset是由带着传感器套件的操作员在几个开放区域中收集的，如图8(b)所示。该数据集也在相同的位置开始和结束。我们通过一个位于图像顶部中心的一个开放棒球场来增加这个数据集的挑战。当通过这个领域时，相机和激光雷达收集的主要观测结果分别是草和地面(图6(b)和(d)）。由于上述简并性问题，所有基于激光雷达的方法都不能产生有意义的结果。提出的框架LVI-SAM成功地在启用了或没有循环闭包的情况下完成了测试，并在表二中所有三个基准标准中实现了最低的误差。

四、总结

我们提出了LVI-SAM，一个通过SAM紧密耦合激光雷达-视觉-惯性里程计的框架，用于在复杂环境中进行实时状态估计和地图构建。该框架由视觉惯性系统和激光雷达惯性系统两个子系统组成。这两个子系统被设计成以紧密耦合的方式交互，以提高系统的鲁棒性和准确性。通过对不同规模、平台和环境上的数据集进行评估，我们的系统显示出与现有公开可用方法可比有更好的准确性。我们希望我们的系统将作为一个坚实的基线，其他人可以很容易地建立，以推进激光视觉惯性测速的技术。