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

文章目录

• 前言部分
• 摘要
• 一、引言部分
• 二、全面的激光视觉惯导SLAM系统

  • A. 系统概述部分

  • B. 视觉惯导模块

  • C. 雷达辅助模块

  • 三、实验

  • • A.消融研究
    • B. Jackal Dataset
    • C. Handheld Dataset

  • 四、总结

---

写在前面

本文参考：惊鸿一博

个人简介：LVI-SAM代表了Tixiao Shan在最新领域的突破。作为两个关键领域的开创者之一，在此领域内有着卓越的研究成果。该框架由Tixiao Shan推出，并整合了视觉、激光雷达与惯性导航技术。通过融合IO-SAM与VINS-MONO等先进算法实现这一目标。

论文原文：论文原文链接

开源代码:开源代码链接

演示视频:

该 iframe 元素配置为展示哔哩哔哩的视频播放器，默认设置为支持自动启动，并且允许用户点击播放

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

摘要

本文提出了一种集成式的雷达视觉惯导SLAM系统 ，能够实时地实现高精度的状态估计与建图 。该方法基于因子图构建，并由两个核心子系统构成：一个是专门负责视觉感知的任务单元（VSS），另一个是专注于激光雷达数据处理的核心模块（LSS）。通过紧密耦合的方式，在VSS中引入LSS提供的位置信息以辅助初始化；与此同时，在LSS中采用基于VSS预估结果作为初始配准依据以提高定位精度。此外还设计了闭式检测机制（CCK），通过获取环境反馈数据来优化LSS性能。尽管在实际应用中可能会出现单个模块失效的情况 LVI-SAM仍能维持稳定运行状态 这一显著特点使得其在复杂场景下的鲁棒性得到极大增强 在测试数据集上该算法表现出了良好的应用效果

一、引言

同时实现定位与地图构建（SLAM）是许多移动机器人导航任务所需的基础功能。过去20年间，在单个感知传感器等具有挑战性的环境中利用激光雷达或相机进行实时状态估计与地图构建取得了显著成效。基于激光雷达的方法能够远程捕捉环境细节。然而，在无组织空间中该方法往往失效（如长走廊或平坦场地）。尽管基于视觉的方法特别适合位置识别，并且在纹理丰富的环境中表现良好[1] ，但其性能易受光照变化、快速运动以及初始化问题影响[2] 。因此将激光雷达方法与惯性测量单元（IMU）结合通常可提升系统的鲁棒性与准确性[3] 。通过融合激光雷达、摄像头及IMU测量数据可进一步增强系统性能[4]

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

我们的研究与视觉惯性测速(VIO)、激光雷达惯性测速(LIO)以及激光雷达可视化惯性测速(LVIO)方法具有密切关联。值得注意的是，在本文中我们未考虑非惯性系统，在此之前我们虽意识到成功的非惯性激光雷达视觉系统[1]、[2]的存在。视觉惯性测速法(VIO)主要可分为两类：基于滤波器的方法与基于优化的方法。其中基于滤波器的方法通常采用扩展卡尔曼滤波器(EKF)，通过融合来自摄像头和加速度计的测量数据来推导系统的状态信息；而基于优化的方法则构建了一个滑动窗口估计模型，并通过最小化视觉重投影误差以及加速度计测量误差来更新状态估计值。在本研究中我们仅关注单目摄像头的应用场景。在当前最流行的公开VIO处理框架中包括MSCKF[3]、ROVIO[4]及Open-VINS[5]这三类基于滤波器的技术；而OKVIS[6]、Kimera[7]及VINS-Mono[8]则代表了基于优化方法的发展方向。尽管OKVIS借助立体声摄像头实现了优异的效果但它并未对单目摄像头应用进行专门优化处理。相比之下VINS-Mono则通过滑动窗口内非线性优化的方式实现了高精度定位功能并成功应用于单目摄像头场景中[9]。

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

基于惯性测速法的设计思路

近年来，激光雷达视觉惯性系统在传感器退化任务[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数据进行融合运算；LIS模块则结合雷达点云数据进一步提升精度。其中：

• 视觉里程计通过最小化IMU与视觉测量间的残差来获得运动估计结果；
• 激光里程计则通过最小化检测到的线面特征与特征图之间的距离实现定位；
• 特征图被一个滑动窗口持续更新以实现实时处理；
  最后的状态估计问题可通过构建最大后验概率(MAP)模型进行求解；
  为了提高系统的优化效率，
  采用iSAM2算法对因子图中的IMU预积分、视觉里程计、激光里程计以及闭环约束等元素进行残差优化处理。

B. 视觉惯导系统

我们按照文献[8]所提出的方案对VIS模块进行了处理管道优化，并将其流程示于图2中。该模块主要包含两个核心部分：首先采用Harris角点检测器[22]进行视觉特征定位；接着通过Kanade-Lucas-Tomasi算法框架实现特征跟踪功能。当VIS完成初始化后，在此基础上我们构建了一个基于激光扫描数据集的稀疏深度图模型；随后通过该深度图模型完成了特征深度估计任务；最后将这些估算结果综合起来实现了系统的视觉惯性导航能力

R代表SO(3)空间中的旋转矩阵；p即位置信息；v代表速度向量；b由[GYRO_b_a, ACCELEROMETER_b_w]组成。变换矩阵T属于SE(3)，用于描述机体坐标系与世界坐标系之间的转换关系 。下面会详细讲解VIS初始化方法及其特征深度估计过程。建议读者参考VINS-Mono原始论文以获取更多技术细节。

1）初始化：基于优化的VIO系统因存在强非线性特性而导致初始状态出现发散现象。初始状态的质量主要取决于两个关键因素：初始传感器运动轨迹的准确性以及IMU参数的精确度。在实际测试中发现，在系统运行速度较低或处于匀速状态时完成初始化变得异常困难。这一现象主要是由于加速度计的激励强度不足以实现对尺度参数的有效估计所导致的。值得注意的是，在本研究中所采用的IMU参数包含缓慢变化的趋势偏差项和高斯白噪声项等特征参数。这些特征对原始加速度计和陀螺仪测量值的质量产生显著影响，在初始化阶段提供较为合理的初值能够显著提高系统的优化效率和收敛效果。

为增强VIS初始化过程中的鲁棒性, 我们采用了LIS系统来进行状态X及IMU偏差b的估计, 其中雷达观测能够直接提供深度信息. 随后使用LIS系统进行初始化工作以获得x与b, 接着采用插值方法将激光帧与视觉帧按照时间戳进行精确配准. 假设在两次图像关键帧之间IMU偏差保持不变的状态下, 最终将由LIS系统所得x及b作为VIS初始参数输入, 这一过程有效地提升了初始化的速度与可靠性

在VIS初始化的基础上，在此基础上

我们通过评估特征点及其邻近三个深度点之间的距离来进一步确认相关特征深度的存在性。

如图4所示，展示了对齐后的深度图及其对应的视觉特征。在图4(a)和(c)中,基于视觉里程计对齐后的点云生成了深度图并投射到图像中。对于图4(b)和(d),通过深度信息恢复出相应的视觉特征,这些特征被标记为绿色以突出显示.值得注意的是,尽管在图4(a)中这些点覆盖了图像的大部区域,但由于在图4(b)中许多关键特征集中在窗户角落位置,导致深度关联检测未能成功完成.

3）失效检测机制：当运动变化剧烈、光照发生变化或环境缺乏纹理特征时，在这种情况下会导致Vision-based tracking（简称VIS）系统的失效出现。这是因为，在这种特定条件下被跟踪的目标特征数量会显著下降，在特征不足的情况下将无法实现优化计算的有效性保证。一旦Vision-based tracking系统的性能无法满足所需的要求标准，则其估计出的姿态偏差（即IMU偏差）将会变得非常大程度地不可靠甚至不可用的状态下才会被视为失效状态发生的一种现象。为此本研究采用了主动式失效检测机制来确保此类问题不会对后续引入的位置与姿态信息融合（简称LIS）系统造成任何影响。每当检测到任何故障迹象时Vision-based tracking系统将立即重新启动并且向位置与姿态信息融合系统的相关模块发出相应的警示信息。

4）闭环检测

C.雷达惯导系统

如图5所示

1）初始值估算：实验结果表明，在连续的扫描匹配过程中，初始值的作用至关重要，在剧烈运动的情况下表现得尤为突出。LIS系统的初始化过程前后所使用的初始值来源存在差异

在LIS系统的初始化阶段前

在LIS系统的初始化阶段前

当LIS系统的初始化流程完成后

2. 失败检测机制 ：然而激光雷达能够捕捉到远处的详细信息；但在某些特定环境下可能导致扫描匹配失效（如图6所示）。为了判断LIS系统是否出现故障状态，则采用第[26]篇论文所介绍的方法进行分析判断。在扫描匹配过程中所涉及的非线性优化问题可被建模为一种迭代求解线性方程的过程：

\min_{x}\|Ax - b\|^2

其中变量A和b是从T处进行线性化处理而得来。当矩阵A^TA计算出其最小特征值低于首次优化迭代设定的阈值时，则LIS报告出现故障现象。一旦发生故障情况，则激光雷达里程计相关的约束信息将被排除于因子图之外。The detailed analysis of these assumptions can be found in Reference [26], where comprehensive insights into the foundational principles are provided.

三、实验

本研究中我们进行了系列实验，并在三个自收集的数据集中验证了所提出的框架。这些数据集分别命名为Urban、Jackal和Handheld。为了在三个自收集的数据集中验证所提出的框架，在实验过程中采用了多种传感器套装进行测试。具体而言，该传感器套装包含了以下设备：高精度Velodyne VLP-16激光雷达、FLIR BFS-U3-04S2M-CS热成像相机、MicroStrain 3DM-GX5-25惯性测量单元（IMU）以及ReachRS+GPS系统（其中后者配置为地面真相参考）。此外，在对所提出的框架进行性能评估时，我们将其与现有的开源方案进行了对比分析。所有方法均采用C++语言实现，并在基于Inteli7-10710U处理器的Ubuntu Linux系统上进行了测试与评估。具体实现细节可参考以下链接获取信息。

A.消融研究

我们探讨了城市数据集提议框架性能受系统各模块设计的影响。该数据集以其独特之处著称——包括建筑物、停放车辆、移动车辆以及行人等特征，并通过操作员步行及携带传感器套件进行采集。特意将其放置于关键位置（图6(a)），以便测试其在退化情况下的抗干扰能力。因头顶植被茂密而被GPS信号丢失或干扰。选择相同位置进行起始与结束的数据采集，并参考表1结果进行分析。

A1类型的视觉惯性里程计通过融合激光雷达的特征深度信息来提升定位精度。通过禁用LIS中的扫描匹配并仅使用VIS进行姿态估计，在图7中被展示两种情况的结果轨迹（即A1）。基于时钟的时间轴来计算运动轨迹方向。当将深度信息与视觉特征关联起来时（如表一所示），端到端的姿态误差明显降低。

视觉惯性里程计的作用：我们不再依赖VIS，在姿态估计中转而仅依靠LIS。在图7中以标记A2标注的情况中发现，在退化场景中可能会出现几次发散现象。

我们目前共同采用Vision-LiDAR系统，并通过更换Vision中的深度对齐模块来对比得出LiViO轨迹结果。利用视觉特征的深度信息，在纯测速模式下验证系统的性能时发现, 翻译误差减少了约29%, 从45.42米降至32.18米。特别指出, 在本次测试中排除了环路闭合检测, 仅以单一速度模式验证系统的性能表现

我们通过在VIS中激活该机制以消除系统的漂移。当在框架中启用每个模块时，在图7中标注了系统轨迹的结果为A4

B. Jackal Dataset

Jackal Dataset是基于安装于Clearpath Jackal无人驾驶地面车辆（UGV）上的传感器套件而生成的数据集。我们在一个功能完善的环境中进行机器人测试，在保证起点与终点一致的情况下完成操作。如图8(a)所示，在此环境下包括植被、不同类型的路面以及复杂的地形结构组成测试区域。GPS接收机的有效覆盖区域用白色标记表示。

我们对比了多种方法，并于图9(a)展示了它们的变化趋势。我们通过手动禁用及启用该功能来进行验证。基准测试结果可在表二中找到。与GPS相比，LVI-SAM表现出最小的均方根误差（RMSE）。可视为地面真实值的标准是其测量精度最高者。最小化的是端到端平移误差的技术是LINS系统所采用。该系统基于LeGOLOAM[11]进行了改进行业应用设计。在旋转精度方面再次取得突破性进展的是LVI-SAM方案。

C. Handheld Dataset

Handheld dataset is acquired by users equipped with a sensor package in several open areas, as illustrated in Figure 8(b). This dataset also begins and ends at the same location. To enhance the challenges of this dataset, we introduce an open field positioned at the top center of the image. When traversing this area, the camera and LiDAR primarily capture grass and ground surfaces (as shown in Fig. 6(b) and (d)). Due to these simplification issues, laser-based methods cannot produce meaningful results. Our proposed framework, LVI-SAM, successfully demonstrated its performance under both loop-closure enabled and disabled conditions. The experimental results presented in Table 2 clearly show that our approach achieves state-of-the-art performance across all three benchmarks.

四、总结

我们开发了LVI-SAM框架，并将其应用于复杂环境中的实时状态估计与地图构建任务中。该框架由多个关键组件构成：包括基于视觉的特征提取与运动估计模块以及基于激光雷达的数据融合与路径规划模块。这些关键组件被精心设计为以高度集成的方式协同工作，并通过多传感器数据的实时融合实现了更高的定位精度与环境感知能力。通过对不同规模、平台和环境下的实验数据进行分析对比后发现：我们的方法较现有的公开可用方案在定位精度上具有显著优势并展现出更强的鲁棒性