论文笔记_S2D.66_ICRA_2021_LVI-SAM: 紧耦合的激光视觉惯导SLAM系统
目录
基本情况
摘要
介绍
完整的激光视觉惯导SLAM系统
A. 系统概述
B. 视觉惯导系统
C.雷达惯导系统
实验
基本情况
题目: LVI-SAM: A High-Performance LiDAR-Visual-Inertial Odometry System through Smoothing and Mapping
出处: ICRA 2021
* 作者: Shan T, Englot B, Ratti C, et al. "LVI-SAM: A High-Performance LiDAR-Visual-Inertial Odometry System through Smoothing and Mapping." arXiv preprint arXIV:2104.10831, 2021.
- GitHub链接: https://github.com/TixiaoShan/LVI-SAM
论文：https://arxiv.org/abs/2104.10831
Demo：
参考：公众号「3D视觉工坊」https://cloud.tencent.com/developer/article/1818019
我认为激光视觉惯导融合系统的搭建过程对于许多人来说都是相同的。然而最大的挑战则是将个人的想法高质量地实现出来。我们无法做到但有经验的人却能做到。
摘要
本文提出了一种紧密集成的雷达视觉惯导SLAM系统 ，能够实时地实现高精度可靠的状态估计与环境建模 。基于因子图构建而成，并且分为两个功能模块：
- 一个视觉惯导系统
- 一个激光惯导系统 。
这两个子系统利用紧耦合的方法，
- 视觉惯导航系统通过激光惯导估计进行初始化。
- 视觉惯导航系统借助激光雷达获取数据以提升深度测量精度。
- 同时该系统的初始值来源为视觉 inertial navigation system 的量测结果。
- 循环检测结果被用于优化该系统的参数。
当激光惯导或视觉惯导子系统出现故障时，LVI-SAM仍能正常运行。这一改进使系统在缺少纹理及特征场景中展现出显著的鲁棒性提升。经过对相关数据集的测试实验，在实际应用中取得了良好的性能表现
介绍
该SLAM系统可利用雷达技术获取环境中的详细信息，在缺乏结构信息的情况下容易失效。例如，在长廊或开阔的广场等场景中，基于雷达的SLAM系统表现良好。而基于视觉的方法在纹理丰富的场景中表现良好，并且容易实现场景重识别过程。但对光照变化、快速运动以及初始化问题较为敏感。因此，激光雷达与惯性导航系统常被结合使用以提高定位精度和可靠性。惯性导航系统能够维持一段时间内的稳定定位，并有助于视觉系统恢复尺度信息。
本文提出了一种紧凑型雷达视觉惯导里程计系统的集成方案，在LIS（激光惯导辅助）系统失效的情况下仍可独立运行（此时能够实现良好的匹配），而在多特征点检测场景下则可实现模块化融合（此时能够实现良好的匹配）。该系统通过利用雷达帧恢复深度信息来追踪视觉特征，并通过优化视觉重投影误差与imu测量误差以获得精确的视觉里程计结果作为激光雷达scan-matching初值估计，并将相关约束项融入到因子图中进行优化求解。当采用imu滤波去除点云畸变后，在LIS系统中对检测到的点云边缘和平面特征进行提取并与其存储在滑动窗口中的features map进行准确定位匹配。LIS状态量估计结果可作为VIS（视觉惯导辅助）系统的初始化输入（这一过程实现了闭环控制）。在此过程中候选匹配帧将基于视觉数据构建出完整的词袋模型, 并在LIS系统中完成路径优化规划。来自多源感知器（如视觉里程计、激光里程计、imu预积分）以及闭环优化的信息都被整合到因子图中进行最优解求取操作。最终得到的结果可用于递推计算出IMU传感器频率下的位姿信息。本文的主要贡献体现在以下几个方面：
- 基于因子图中因子间的紧密耦合关系设计了LVIO系统，在确保了多传感器融合能力的同时，并通过场景重识别实现全局优化。
- 采用故障检测技术来规避故障发生时运行中的子系统失效情况，在确保其对传感器性能下降时仍能保持稳定工作状态。
- 采用多样化的数据集进行充分验证和评估。
就系统而言，在研究方法上该团队提出了具有创新性的解决方案。他们成功整合了VIO与LIO领域的最新技术成果，并开发出了高精度的LVI系统。基于此研究基础，我们期待进一步推动激光惯导视觉融合系统的进步和发展。
完整的激光视觉惯导SLAM系统
A. 系统概述
该系统接收多种传感器数据作为输入信号；其中Vision模块主要处理图像与惯性测量单元的数据；而雷达点云则作为可选输入供进一步处理；为了实现Visual odometry；该系统通过最小化视觉与惯性测量误差实现Visual定位；同时LIDAR定位模块则基于线面特征与预设地图之间的距离进行最优匹配；为此；我们采用滑动窗口机制以实时更新特征图；最终状态估计问题通常被建模为最大后验概率优化问题；为了提高计算效率与数据传输效率；在多传感器融合框架下构建全局优化模型并利用iSAM2算法对因子图中的各子模块误差进行迭代优化
B. 视觉惯导系统
视觉惯导的pipeline如图2所示。
主要视觉特征通过角点检测器捕捉，并结合KLT算法实现目标跟踪。随后通过使用视觉里程计将激光帧的点云对齐至一定范围后方，在此区域内生成稀疏深度图以完成特征深度的估计。系统在滑窗窗口内执行BA优化过程，在此过程中状态X可以表示为：
R代表SO(3)，而p、v分别代表位置和速度。b=[b_a, b_w]构成了IMU的偏置参数。变换矩阵T属于SE(3)，负责将机体坐标系转换到世界坐标系。下面将详细介绍VIS的初始化流程以及特征深度估计的方法。建议参考VINS-Mono的原始论文以获取详细信息。
基于优化设计的视觉惯性 Odometry（VIO）系统因存在显著的非线性特性而导致初始化过程出现发散现象。系统的初始化质量主要受两个关键因素的影响：初始传感器运动状态及Inertial Measurement Unit（IMU）参数精度。通过实际实验研究发现，在系统运行速度较低或处于匀速状态时，采用常规算法如VINS可能会导致初始化失败。这归因于加速度测量激励不足所致的状态量不可观测问题。此外，IMU参数包括渐变偏置项和高斯白噪声等元素，并对加速度与角速度测量产生直接影响，在 初始化阶段若能提供较为理想的初始值，则有助于提升系统的收敛效率
为了缓解VIS初始化过程中的鲁棒性问题，我们可以借助LIS系统来估算系统的状态X以及IMU的偏差b。鉴于雷达观测可以直接提供深度信息，在初始化过程中我们首先通过LIS系统获得x和b。随后我们使用插值方法将激光帧与视觉帧按照时间戳对齐。基于这一前提假设，在两个图像关键帧之间IMU的偏差保持不变。最终我们将LIS系统所得出的状态x和b作为VIS初始设置参数输入数据流处理过程。这一改进措施显著地提高了初始化的速度与稳定性
2）深度特征关联：基于VIS初始化的结果，在此基础上我们通过视觉里程计实现了视觉帧与雷达帧的空间对齐过程。值得注意的是，在现有的3D LIDAR传感器下所采集的数据均为稀疏点云数据，在本研究中我们通过将多帧激光雷达数据融合处理从而构建了一个较为密集的深度图。在此过程中为了实现特征与深度值的有效关联我们将提取出的视觉特征与雷达反射点投影至以相机中心为基准建立的单位圆上随后对获取到的所有深度点执行降采样处理并采用极坐标系表示法来保持采样点密度的一致性。在此基础上最后计算得到各特征点相对于相机中心的距离即为其深度值这一指标并通过二维KD-tree（极坐标）方法快速定位其空间位置关系如图a所示展示了该过程的具体实施效果其中所标示的关键参数如虚线长度等均体现了该方法的核心输出结果
我们通过评估特征点最近三个连续深度点的距离来验证关联特征深度。叠加来自不同时间戳的激光雷达帧可能会导致不同对象出现深度模糊问题。具体示意图见图3（b）。在时间t_i时观察到的各处深度数据均以绿色标注。相机在t_j时刻移动后观测到了更多的灰度区域。而在t_i时观测到的部分被灰色框选中的区域可能仍然可见于t_j时刻的空间位置中。因此使用来自不同对象的多个深度值来进行关联会导致估算结果出现偏差。与该领域的研究论文——Laser-Visual-Inertial Odometry and Mapping with High Robustness and Low Drift——的做法一致，在计算各特征周围各测量区域的最大间距时发现其超出2米的情形则不会进行数据关联处理
如图4所示，在该示意图中展示了对齐后的深度图像及其对应的视觉特征分布情况。具体而言，在图4中，a和c部分通过使用视觉里程计将点云数据进行对齐并生成深度图像后投影至对应的图像区域。对于图像b和d中的区域，在恢复视觉特征的过程中被标记为绿色部分。值得注意的是，在图4a中所展示的深度数据覆盖了大部分图像区域；但因其中许多视觉特征集中在窗户角落位置而导致深度关联检测未能成功完成。
3）失败检测：当运动变化剧烈时，在光照变化或环境缺乏纹理的情况下会导致VIS系统的失效。这是因为这种情况下跟踪到的目标特征点数量会显著减少，在特征不足的情况下会导致优化过程无法完成。当VIS系统出现失效时会导致IMU估算出的偏差较大；因此，在目标特征点数量少于预设阈值或IMU估算出的偏差超过一定范围时我们将视作VIS系统的失效发生。为了保证本文系统的可靠性我们采用了主动失效监测机制以便让系统的故障不会影响到LIS的工作状态；一旦发生故障 VIS系统将会重新启动并在必要时向LIS发送警报信息
4）闭环检测：我们采用DBoW2模型来实施闭环检测流程。每当有一个新的图像关键帧进入处理流程时，系统会提取该帧上的BRIEF特征并将其与之前提取的特征进行配对。当DBoW2算法识别出潜在的闭环候选帧时（这些候选帧的时间戳记录在系统中），其时间戳会被传递给LIS系统以完成后续验证步骤。
C.雷达惯导系统
如图5所示，所开发的雷达惯导系统以LIO-SAM为基础，并主要采用因素图来实现全局位姿优化。
包含以下四种类型的约束：IMU的预积分约束、视觉里程计的约束、雷达里程计的约束以及闭环检测相关的约束等。这些约束均被纳入因子图进行优化过程。其中，激光雷达里程计相关的约束主要基于扫描匹配技术实现：即通过扫描匹配技术实现当前激光雷达关键帧与全局特征图之间的匹配关系建立。在此过程中，首先由视觉里程计生成候选帧，并通过扫描匹配技术进一步优化其精度。为了保证计算效率，在特征图中维护了一个基于激光雷达的关键点滑动窗口模型：该模型能够有效平衡有限计算资源与地图更新频率之间的关系。当机器人位置超出预设阈值范围时，则会触发新的关键帧更新机制：具体而言是选择一个全新的激光雷达关键帧节点，并删除处于旧关键帧和新关键帧之间的一般激光雷达采集到的数据帧段。随后将新的机器人状态量x作为节点节点加入因子图中完成更新操作：这种动态更新机制不仅能够有效平衡内存占用与地图密度之间的关系：而且还能确保因子图结构保持相对稀疏化以支持实时优化运算需求：这部分内容可参考LIO-SAM论文中的相关论述来获取进一步的技术细节信息：在后续章节中我们将重点探讨如何提升系统在复杂环境下的鲁棒性表现以及具体的优化策略设计方法
在初始值估计方面：实验表明，在剧烈运动的情况下，在LIS系统的初始化阶段之前和之后初值来源存在差异。具体而言，在LIS系统初始化前阶段中，“首先假设机器人处于静止状态”，并且将IMU设备测得的数据中的偏差与噪声置零后，“通过对IMU原始数据进行积分计算得到两组雷达关键帧的位置参数及姿态数据作为扫描匹配算法的初始输入”。经过进一步研究发现，“这种方法能够在具有挑战性的场景下成功应用于Initialization（其中Initial velocity低于10m/s且Angular velocity低于180°/s）”。当完成了LIS系统的Initialization后，“随后立即用于估算因子图中的IMU偏差、机器人的位姿参数以及运动速度参数，并将这些参数传递给VIS模块以启动其Initialization流程”。
当LIS系统的初始化完成后
2）失败检测：虽然激光雷达能够获取场景中较远范围内的详细信息，但会在某些情况下导致扫描匹配失败（如图6所示）。我们采用了On Degeneracy of Optimization-based State Estimation Problems论文中所述的方法来判断LIS系统是否出现了故障。
scan-match的非线性优化可以写成一个线性最小二乘的形式：
其中A和b是从T处获得的线性化产物。当矩阵A^TA在其最小特征值上低于第一次优化迭代设定的标准时，在此情况下LIS报告未能成功完成，则不会将激光雷达里程计的数据约束纳入因子图中。有关这些假设在Optimization-based State Estimation Problems领域的详细分析，请参阅On Degeneracy of Optimization-based State Estimation Problems一文。
实验
该设备组由多个关键传感器组成：包括具有十六通道采集能力的Velodyne雷达系统、基于FLIR BFS-U3-04S2M-CS平台的单眼摄像头、采用3DM-GX5-25芯片的高精度IMU模块以及集成GPS定位功能的Reach RS+装置。其中核心组件包括Intel i7-10710U处理器运行于Ubuntu Linux操作系统环境中
作者在三个自己录制的数据集上进行了测试都取得了很不错的效果。
