LVI-SAM：紧耦合的激光视觉惯导SLAM系统（Tixiao Shan新作，已开源）

论文地址： 在公众号「3D****视觉工坊 」，后台回复「LVI-SAM 」，即可直接下载。

我认为激光视觉惯导融合系统的搭建对许多人来说都是相同的流程和方法。然而真正困难的是将自己的创新思路高质量地实现出来。我们团队对此深感无力但那些在这一领域造诣深厚的同行依然能够轻而易举地完成！

摘要

本文开发了一个融合雷达与视觉信息的紧耦合惯导SLAM系统,该系统能够实现实时状态估计与地图构建,并具备良好的鲁棒性。LVI-SAM架构基于因子图框架,整合了视觉与激光双重感知模块。其中,两个子系统分别负责不同的感知任务:一个是依赖于其他系统的初始估计来进行初始化,另一个则采用基于视觉信息的迭代优化方法进行帧对齐。通过多 camera闭环检测机制生成环路闭合数据 fed back to 激光模块用于优化调整。即使其中一个感知模块失效,LVI-SAM仍能维持稳定运行,从而极大提升了其在复杂环境中的适用能力。实验结果表明,LVI-SAM在标准数据集上表现优异,验证了其优越性

介绍

利用雷达技术构建的SLAM系统能够全面感知广泛区域的环境细节，在缺乏环境结构信息的情况下，其性能可能会显著下降。例如，在长走廊或开阔广场等复杂环境中应用视觉方法时会遇到挑战。视觉方法在纹理丰富的环境中展现出良好的效果，并且对光照变化敏感；但对快速运动和初始化问题较为敏感。因此，在实际应用中为了提高定位精度和系统可靠性惯导技术常被用于融合激光数据与视觉数据。

本文设计了一种紧凑集成的雷达视觉惯导里程计系统，在LIS（激光惯性导航系统）失效的情况下仍能正常运行（当检测到较少特征点时），而在高特征点检测场景下则实现了系统的耦合运行（通过因子图整合约束信息）。视觉惯导模块基于雷达帧恢复深度信息进行特征跟踪，并通过优化视觉重投影误差与IMU预积分误差实现精确初始化（将约束加入因子图中）。利用IMU消除点云畸变后（在滑动窗口中保存特征图），该模块能够识别点云边缘和平面特征并与其匹配（在LIS中进行优化处理）。LIS状态估计结果可传递至VIS模块作为初始化输入（通过词袋模型匹配候选帧并在LIS中进行全局优化）。闭环检测中的多约束信息包括来自视觉里程计、激光里程计、IMU预积分以及闭环匹配的结果（整合到因子图中），最终解算得到IMU偏差并用于递推计算高频率位姿估计。本文的主要贡献在于：

该系统依托因子图构建紧密耦合的LVIO架构，并通过多传感器数据融合与场景重识别技术实现全局优化。

利用故障检测技术实现对失效子系统的规避，并使传感器性能的降低具备一定的抗性

利用不同的数据集进行了完善的验证。

从系统性研究的角度来看，该研究工作具有创新性并整合了VIO与LIO领域的最新技术成果。该研究团队成功开发了一种高精度的激光视觉惯性（LVI）系统，并期待基于此成果进一步探索激光惯导视觉融合系统的技术发展路径。

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

A. 系统概述

该系统采用多源传感器数据进行信息融合：激光点云、单目图像及惯性测量单元（IMU）数据作为主要输入；其中视觉系统仅依赖图像信息与IMU数据进行处理；雷达点云可作为可选输入以提升定位精度；通过残差最小化方法分别构建激光里程计与视觉里程计模型；特征图采用滑动窗口机制以实现实时更新；最终的状态估计问题可通过最大化后验概率（MAP）框架进行建模；基于iSAM2算法对因子图中的预积分项、视觉、激光里程计及闭环约束进行优化求解；值得注意的是，在多传感器协同优化框架中实现了数据共享与资源分配效率的提升

B. 视觉惯导系统

视觉惯导的pipeline如图2所示。

通过角点检测器捕捉的视觉特征随后被KLT算法用于目标跟踪过程。基于激光雷达的数据生成稀疏深度图的过程中实现了特征深度信息的有效提取从而完成了视觉惯性导航系统的初始化工作。该系统采用滑动窗口策略，在此框架下执行优化计算以实现更为精确的状态估计

R被定义为SO3空间中的旋转矩阵；p代表位置信息；v代表速度信息；b向量由[ba, bw]组成，并作为IMU校准参数存在；变换矩阵T属于SE3群，并将机体坐标系转换至全局参考框架。接下来详细阐述VIS初始化方法及特征深度估计流程。如需更深入的技术细节，请参阅VINS-Mono原始论文。

1） 初始化：基于优化设计的视觉惯性（VIO）系统因存在较强的非线性特性而导致初始化过程出现发散现象。
2） 初始化的成功与否主要取决于两个关键因素：初始传感器运动状态以及IMU参数配置的质量。
3） 通过实验研究发现，在系统运行速度较低或处于匀速状态时，VINS算法往往难以实现有效的初始化。
4） 其原因在于加速度计的有效激励不足，导致尺度估计失去可观察性。
5） IMU的相关参数包括渐变型偏差（bias）和高斯白噪声（Gaussian white noise），这些因素直接影响着加速度和角速度测量的有效性。
6） 在Initialization阶段选取合适的初始值能够显著提升系统的收敛效率与准确性。

为了增强VIS初始化过程的鲁棒性，我们采用了LIS系统来估计系统的状态X以及IMU（惯性测量单元）的偏差b。基于雷达观测能够直接获取深度信息这一前提，在具体实施中我们首先通过LIS系统获得了x和b的具体值。随后通过插值技术将激光帧与视觉帧按照时间戳进行精确对齐。在此过程中假设在两次图像关键帧之间，IMU的偏差保持恒定不变。最终将通过LIS系统所得出的x与b作为VIS初始化的基础参数输入，在这一方法下显著提升了初始化速度与鲁棒性

2）深度特征关联：基于VIS初始化的结果，通过视觉里程计对齐视觉帧与雷达帧。由于当前的3D LiDAR扫描的是稀疏点，我们将多帧激光雷达数据整合生成一个稠密深度图。为了将特征与其对应的深度值关联起来，我们将视觉特征及其对应的雷达点投影至以相机为中心的单位圆上。随后对深度点进行降采样，并采用极坐标表示法来保持点密度恒定。我们使用二维KD树（基于极坐标）来查找与每个视觉特征点最近的三个邻居。最后计算每个特征点与其相机中心连线长度作为其深度值；该连线与三个参考深度值所确定平面在笛卡尔坐标系中存在交集。

为了进一步验证关联特征深度的有效性, 我们采用了评估关键点之间最近三个深度数据的方法. 这种方法的原因在于, 通过连续时间戳下的激光雷达帧叠加可能会引发不同对象间的深度模糊. 如图3（b）所示, 该图展示了在时间t_i处观测到的深度点以绿色表示. 随后, 在时间t_j时相机移位并捕捉到灰度区域. 然而, 在时间t_i捕捉到的部分灰度区域可能仍可见于后续时刻. 因此, 基于不同对象间深度值较大的差异进行特征关联将导致估算结果出现偏差. 与近期研究中采用的方法一致, 我们通过计算特征周围最大邻域距离来进行关联判断. 当最大距离超过2米时, 将避免对该特征进行数据关联.

如图4所示，在a和c子图中通过视觉里程计将点云配准到图像空间中后生成了深度图，并将其在图像上进行投影。而在b与d子图中则通过从深度图中提取出相应的视觉特征并将其标记为绿色区域。值得注意的是，在图4a中尽管深度点覆盖了图像的大部区域但因许多关键特征集中在窗户角落导致深度关联检验未能成功完成。

3）失败检测：当运动变化剧烈或者光照变化时，在缺乏纹理特征的情况下会导致VIS系统失效（即出现故障）。这是因为在这种情况下跟踪到的目标特征数量会显著减少（甚至完全消失），从而导致跟踪算法无法正常运行。当出现这种情况时会导致IMU出现较大的偏差（即偏置过大）。因此，在以下两种情况下需要判断为VIS系统的失效状态：一是目标跟踪到的有效特征点数量少于预设阈值；二是由IMU估算出的偏置超出预设范围。为了确保系统的可靠性，在本文的设计中必须实现主动式的失效检测功能（即实时监测系统运行状态），这样即使在部分故障发生的情况下也不会影响到LIS系统的正常运行。一旦检测到系统出现故障状态，则会立即重新初始化VIS模块，并向LIS模块发送警报信息以供处理

在本研究中,我们采用DBoW2方法进行闭环检测,每当接收一个新图像的关键帧时,系统会提取该帧上的BRIEF特征描述符,并将其与之前获取的特征描述符进行匹配。随后,系统将这些候选帧的时间戳信息传递给LIS模块进行后续验证

C.雷达惯导系统

如图5所示，在本研究中所设计的雷达惯导系统以LIO-SAM为理论基础，并主要采用了因子图来进行全局位姿优化。

这些主要的约束包括IMU预积分约束、视觉里程计约束、雷达里程计约束以及闭环检测相关的约束被整合到因子图中以参与优化过程。其中用于激光雷达里程计计算的核心技术来源于扫描匹配算法，在这一过程中将当前时刻采集的关键帧数据与全局特征图进行精确匹配处理以提高定位精度。对于闭环检测机制而言，在初始候选框架下主要依赖于视觉信息来源进行初步筛选随后通过进一步的扫描匹配优化来提升检测准确性。为了维持计算效率有限度我们在特征图中维护了一个包含激光雷达关键帧数据的滑动窗口结构这种设计既能保证实时性又能有效平衡内存占用与地图更新频率之间的关系。当机器人位置发生显著位移超过预先设定阈值时系统会自动触发新的关键帧采集动作并丢弃处于现有关键帧之间区域内的多余激光雷达测量数据以减少冗余信息处理负担同时保证状态更新的有效性和计算资源的合理利用。这种方法不仅能够实现对复杂动态环境下的稳定跟踪还能显著降低系统运行能耗并为后续优化工作奠定基础建议读者参考LIO-SAM的相关文献以获取更为深入的技术细节与实现方案在后续章节我们将重点探讨如何通过改进算法策略提升系统的鲁棒性表现与适应性能力。

初始值估计方面：基于实验结果表明，在连续扫描匹配中，初始值扮演着十分关键的角色。特别是在机器人 undergo剧烈运动的情况下表现更为突出。值得注意的是，在LIS系统的初始化阶段，默认情况下认为机器人处于静止状态，并假设其惯性测量单元（IMU）的偏置和噪声均为零状态。通过对 IMU 原始数据进行积分运算，获得两个雷达关键帧的位置信息及其姿态描述作为 scan-match 过程的初始条件。经过大量实验验证发现该方法能在较为复杂的情形下（即当机器人起始速度低于 10 m/s 且角速度小于 180°/s 时）成功实现 LIS 系统的 初始化过程。随后当 LIS 系统完成初始化后阶段，则需估算因子图中的 IMU 偏置、机器人的位姿以及运动速度参数，并将这些估计结果传递至 Vision 初始化模块（VIS）以完成后续初始化工作。

完成LIS系统的初始化后, 通过两种途径可以获得Scan-match的初始值: 一种是IMU的速度积分方法, 另一种是基于VIS系统的位姿输出. 当VIS系统能够提供位姿信息时, 则采用该信息作为初始值; 如果VIS系统的状态指示失效, 则可依赖IMU的速度积分结果作为初始估计. 这一方法不仅适用于纹理丰富的环境, 在纹理稀疏的情况下同样表现出良好的稳定性和准确性.

2）失败检测：然而激光雷达能够捕捉到场景中较远处的详细信息。同时这些设备在某些特定环境中可能导致扫描匹配未能成功。如图6所示。通过借鉴该论文中的相关方法对LIS系统的运行状态进行评估。

scan-match的非线性优化可以写成一个线性最小二乘的形式：

其中变量A和b是从系统T处进行线性化处理得到。当矩阵A^TA计算所得的最小特征值低于第一次优化迭代设定的标准时，在此情况下LIS报告未能成功完成，则不会将激光雷达里程计约束加入到因子图中。这些假设的相关理论分析可在文献On Degeneracy of Optimization-based State Estimation Problems中找到详尽讨论。

实验

设备：具有十六通道采样的Velodyne雷达系统；该单目相机采用FLIR BFS-U3-04S2M-CS技术；集成有MicroStrain 3DM-GX5-25芯片组的惯性测量单元；配置于Ubuntu Linux系统中的Intel i7-10710U处理器；集成GPS接收机功能的Reach RS+模块

作者在三个自己录制的数据集上进行了测试都取得了很不错的效果。

备注： 作者也是我们**「3D视觉从入门到精通」** 特邀嘉宾：一个超干货的3D视觉学习社区

本文仅做学术分享，如有侵权，请联系删文。

下载1

在「3D视觉工坊」公众号后台回复"3D视觉"：即可获取涵盖相机标定、三维重建、立体视觉等多个领域的深度学习相关文档干货。

下载2

在「3D视觉工坊」公众号后台发送**《3D视觉github资源合集》消息后会自动弹出包含结构光相关代码库**、标定算法源码、深度估计与深度修复技术代码、点云处理工具包代码资源等在内的各类技术资料

下载3

在「3D视觉工坊」公众号后台回复：「相机标定技术」, 即可获取相机标定方案的学习资源包；后台回复：「立体视觉匹配技术」, 即可查找立体视觉匹配技术的学习资料包。

重磅！3DCVer-学术论文写作投稿交流群已成立

通过扫描二维码与小助手进行微信联系即可轻松加入3D视觉工坊-学术论文写作与投稿 微信交流群主要用于分享和讨论顶会、顶刊、SCI、EI等领域的学术写作与投稿经验。

此外，我们也欢迎加入我们的细分方向交流群。目前主要有哪些细分领域？包括但不限于以下内容：3D视觉技术发展现状及应用前景分析；计算机视觉（CV）及深度学习模型优化探讨；SLAM技术的最新研究进展；三维重建技术的应用案例分享；点云后处理方法的改进策略；自动驾驶系统设计与优化方案讨论；多传感器融合算法研究；计算机视觉入门知识讲解；三维测量工具的实际应用技巧；VR/AR开发经验交流；基于3D的人脸识别技术研究与应用分析；医疗影像分析方法探讨；工业缺陷检测算法优化策略分享；行人重识别系统的性能提升方法讨论；目标跟踪算法的创新思路分享；计算机视觉在产品落地中的实际案例解析以及未来发展趋势预测等。

请务必在注释栏中包含以下信息：
您的研究方向（如3D视觉）+ 您的所属机构（如上海交大） + 您的个人标识（如静静）
例如如下填写：
研究方向 + 学校/公司 + 昵称
请严格按照以下格式进行填写

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球 ：针对3D视觉领域的**视频课****程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、orb-slam3等视频课程）、**知识点汇总、入门进阶学习路线、最新paper分享、疑问解答 五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近2000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

圈里有高质量教程资料、可答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~******

**