SLAM多传感器融合调研

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

---

前言

多传感器融合SLAM 经过一段时间发展，逐步形成了视觉惯性系统，激光惯性系统和激光视觉惯性系统，通过对综述文章的学习，并聚焦于激光视觉惯性融合这一方向，分为松耦合与紧耦合，对相关融合算法文献进行调研。
—

提示：以下是本篇文章正文内容，下面案例可供参考

一、松耦合

1.DEMO

1.1解决的问题
在使用RGB-D相机和范围有限的激光器时会遇到深度信息不足的情况，如果没有足够的深度，求解视觉测程法相对困难的。

1.2算法原理
通过单目相机获取RGB图像，通过LiDAR或者是RGB-D相机获取深度信息。然后对RGB图像进行特征点的提取，进而对深度图进行处理，将相邻两帧图像能够匹配上的特征点所对应的深度值进行分类，分为传感器获取的，利用三角化、kd-tree恢复的，以及无法获得深度信息的这三类。然后根据分类利用不同的公式，进行位姿的求解。接着最后是BA优化，这里的BA优化是每5帧选择一帧，也就是40帧挑选8帧出来进行优化。
特点是既能够适应退化场景又兼具激光雷达惯性系统的高精度平滑轨迹。使用激光雷达的点云深度值为视觉特征点提供深度信息，可以提供更高精度的位姿估计和更高质量的地图。整体的系统框图在文中如下图所示。

2.V-LOAM

zhang 和 Singh(2015)基于 LOAM 算法,集成单目特征跟踪与 IMU 测量来为激光雷达扫描匹配提供距离先验信息提出V-LOAM(visual-lidar odometry and mapping)。

2.1解决的问题
仅使用每个传感器都存在缺点。（VIO和DIO的缺陷）视觉里程计需要适度的照明条件，并且如果不能获得不同的视觉特征则会失败。另一方面，通过移动激光雷达的运动估计涉及点云中的运动失真，因为在连续激光雷达运动期间的不同时间接收范围测量。因此，运动通常必须用大量变量来解决。在诸如由平面区域支配的退化场景中，扫描匹配也失败。

2.2系统原理
提出了一种结合单目相机（monocular camera）和三维激光雷达（3D lidar）进行自我运动（ego-motion）估计的基本方法。视觉里程计部分利用激光里程计辅助获得的视觉图像，以图像帧速率估计传感器的帧与帧之间的运动。在本节中，特征跟踪块The feature tracking block提取并匹配连续图像之间的视觉特征。深度映射注册块The depth map registration block在本地深度映射上注册激光雷达点云，并将深度与视觉特性关联起来。帧对帧运动估计块The frame to frame motion estimation block采用视觉特征来计算运动估计。扫描到扫描细化块the sweep to sweep refinement block匹配连续扫描之间的点云，以细化运动估计并消除点云中的失真。然后，扫描到地图注册块the sweep to map registration匹配并注册当前构建的地图上的点云，并发布与地图相关的传感器姿态。

2.3实验方法：
A.精度测试
包括了室内和室外两种环境，传感器（两种镜头，广角和鱼眼）以0.7m/s速度行走持握。得出，虽然鱼眼相机的视觉测程不如广角相机精确，但是激光雷达测程可以将精度提高到相同的水平。
B.鲁棒性测试（快速运动）
分为廊道环境与楼梯环境，从快速运动试验中。在这两种测试中，当使用广角相机进行快速运动时，都遇到了一些问题，即在快速转弯过程中视觉跟踪不准确，导致运动估计失败。与广角相机相比，鱼眼相机的精度略有下降，但在快速运动中具有较强的鲁棒性。
C.鲁棒性测试（光照剧烈变化）
传感器沿着走廊移动。当灯熄灭时，视觉里程表停止工作，取而代之的是恒速预测。每扫一次激光雷达测程可以校正漂移。

2.4文章结论
提出了一种利用摄像机与三维激光雷达相结合的实时测程测图方法。这是通过一种视觉里程方法在高频率估计自我运动，并通过激光雷达里程计在低频率细化运动估计和纠正漂移。两部分的协同工作实现了精确鲁棒的运动估计，即视觉里程计处理快速运动，激光雷达里程计保证低漂移。

3.Visual-Inertial-Laser

Wang等人(2019)通过维护关键帧数据库来进行全局位姿图优化，从而提升全局一致性。克服了退化问题。
3.1解决问题
视觉-IMU缺点：IMU通过减少照明变化或无纹理区域对视觉跟踪的影响显着提高了运动估计性能，如果机器长期工作在光照变化明显、纹理缺失的环境中，系统仍会跟踪丢失(因为IMU的偏差是随机变化的，长时间不修正会直接影响位姿估计)。
激光SLAM的缺点：在结构性特征缺失的环境中（比如在走廊）会跟踪丢失，此外，由于连续地接收距离测量，所以通过移动激光雷达进行运动估计存在失真的问题。
3.2解决方案
将视觉-IMU-激光三种传感器信息进行融合。系统可大致分为视觉-IMU和激光两大位姿估计模块。它先通过视觉-IMU模块估计出位姿的初值，再根据激光扫描的结果完成位姿的优化。两个模块可以来联合工作，也可以各自独立工作。这也是该系统鲁棒的原因：当一个模块跟丢了，系统也可以只通过剩余模块完成定位和建图。
3.3技术运用
为了进一步提高精度，实施了闭环和接近检测以消除漂移累积。 当检测到回路或接近时，我执行六个自由度（6-DOF）姿态图优化，以实现全局一致性。 在公共数据集上验证了系统的性能，实验结果表明，与其他最新算法相比，该方法具有更高的精度。
3.4系统结构
系统先通过一个紧耦合的 VI 方法估计位姿，再通过激光扫描的结果来优化该估计值，最后再完成建图。该系统的 VI 前端是基于 VINS-Mono （提出的框架）系统实现的。

会分为三种不同的工作模式：
正常工作模式：VI和激光模块都正常工作，如上图；
VI不工作，激光工作：如果当前帧中跟踪的特征数低于阈值，滑动窗口中的估计位置和方向与先前的估计相比发生突变，并且IMU偏差估计发生较大变化，则系统确定视觉惯性里程计模块无效。采取LOAM进行。
仅VI的工作模式：此模式主要是在结构信息缺失的环境中，由于无法提取足够的几何特征，因此扫描匹配无法完善运动估计。

4.Pronto

Camurri等人(2020)提出用于腿足机器人的Pronto，用视觉惯性里程计为激光雷达里程计提供运动先验信息，并能校正视觉与激光之间的位姿。
解决问题
腿足机器人需要在或平稳或崎岖的地形中快速可靠导行，并能够感知和掌控环境，避免与障碍物和行人相撞，因此需要快速可靠的对于机器人的位姿估计器。

二、紧耦合

1.LIMO

该文提出一种从激光雷达测量数据中提取摄像机特征轨迹的深度提取算法。并基于BA算法，通过鲁邦关键帧去估计运动，采用了语义标注用于地标离群点的剔除与加权。

A、B. 特征提取和预处理
特征提取包括特征跟踪和特征关联。特征跟踪使用viso2库中使用的方法（它包括非最大抑制，通过流动的异常值排除和子像素优化），在30-40ms内提取2000个特征对应。然后需要剔除动态的特征点。为此，先建立语义图片，然后在每个特征点周围寻找语义属于动态的点的个数，如果大于一定阈值，则认为该特征点也属于动态点。
数据累积和时间推断是提高准确性和鲁棒性的重要步骤。 特别是对于此处介绍的深度测量稀疏的方法而言，通过许多帧收集的数据的联合优化使其不易出错。 因此，在后端对关键帧执行BA。
S、为了估计尺度，需要从LiDAR中提取特征点的深度。使用了单帧激光点云来获得特征点的深度。虽然这导致更少可用的点云数据由于没有使用累积点云，但是这也避免了对运动估计的依赖。
核心思想：常见的VSLAM解决方法基于双目相机，但其需要准确的相机标定，而LiDAR不需要额外的外参标定，能得到准确的点云深度，想要组合LiDAR准确的深度估计和相机的特征跟踪能力。提出了从LiDAR中提取图片中特征点深度方法，

2.VIL-SLAM

将里程计与地图构建结合整体是将VIL-VIO与激光雷达建图松耦合在一起。将VIO在相对较高频率上提供实时准确的状态估计，作为LiDAR建图算法的运动模型。

1视觉前端从立体相机获取立体图像对。它执行帧到帧跟踪和立体匹配，并输出立体匹配作为视觉测量。
2立体VIO进行立体匹配和IMU测量，在姿势图上执行IMU预积分和紧耦合的固定滞后平滑。该模块以IMU速率和摄像机速率输出VIO姿态。
3LiDAR mapping 模块使用来自VIO的运动估计并执行LiDAR点变形（dewarping）和扫描帧到地图的注册(scan to map registration)。
4闭环模块进行视觉环路检测和初始环路约束估计，并通过稀疏点云ICP对齐进一步精细配准。约束所有LiDAR姿势的全局姿势图被增量式优化，以获取（输出:）全局校正的轨迹和实时的LiDAR姿势校正。它们被发送回LiDAR mapping 模块以进行地图更新和重新定位。

3.LIC Fusion与2.0

本文在计算效率高的多状态约束卡尔曼滤波(MSCKF)框架下，提出了一种快速、紧耦合、单线程的激光-惯性-视觉(LIC)里程计算法，该算法具有在线时空多传感器标定功能。
开发了一个有效的滑动窗口平面特征跟踪算法来处理三维激光雷达点云测量.将这种跟踪算法集成到我们以前的LIC融合估计器中,从而使LIC2.0具有更好的性能.特别是,在平面特征跟踪过程中,我们提出了一种新的离群点剔除标准,通过考虑激光雷达帧变换的不确定性来提高特征匹配质量.此外,深入研究了线性化LIC系统模型的可观测性特性.并识别了具有平面特征的激光雷达-惯性测量单元时空校准的退化情况.该方法已经在模拟数据集和真实数据集上得到了验证,并显示出比最先进的算法更高的精度.未来将研究激光雷达扫描中的滑动窗口边缘特征跟踪.。

4.LVI-SAM

已开源：https://github.com/TixiaoShan/LVI-SAM
有两个子系统：视觉惯性系统VIS与雷达惯性系统LIS，经常与IMU耦合增强各自的鲁棒性和精确性，雷达惯性系统可以帮助矫正点云畸变，当然视觉与雷达有各自的缺点，这里不表
相关工作：
该文工作与VIO，LIO以及LVIO相关类似，
VIO：
Filter-based： MSCKF 、ROVIO、Open—VINS
Optimization—based：OKVIS、kimera、VINS-Mono
LIO ：
Lossely-coupled: LOAM LeGO-LOAM 主要特点是IMU measurements 不使用在优化阶段。
Tightly-coupled : LIO-mapping;设计去优化所有测量，因此没有实现实时表现， LIO-SAM引入雷达关键帧的滑窗，结合IMU与lidar结合优化。 特征地图通过滑窗来实现，实现实时性 提高accuracy、robustness.
LVI 尤其在解决场景退化问题上。 最大后验问题MAP
该文工作与以上工作区分点在于，使用了因子图用于全局优化，定期用闭环检测去做漂移。

该文的主要贡献为：
1紧耦合的LVIO框架，建造因子图，通过地点认知，实现了多传感器融合和全局优化。
2.该框架绕过了失败的子系统，通过失败检测，使之更具鲁棒性，当出现传感器退化问题。
3.该框架更为扩展了收集信息的有效性，在多个场景，范围，平台，环境。

视觉系统：
1.初始化Initialization
取决于两方面，初试传感器的移动，与IMU 参数的准确性。往往初始化失败在于 当传感器移动以一个小的速度或者恒速。
首先初始化的是激光惯性子系统。获得机器人的状态量x与零偏b插值、关联到每个图像的关键帧。借助于LIS来初始化VIS。
2.特征关联
在VIS初始化后，使用雷达帧，到相机帧。Tips：激光雷达采集的数据较为稀疏，将几帧的雷达数据叠加后产生稠密深度图。通过使用视觉特征的极坐标搜索二维K-D树，我们在球面上找到距离视觉特征最近的三个深度点。通过检查三个最近的深度点之间的距离来进一步验证相关的特征深度。（lidar几帧叠加不同观测，可能造成不关联，需要筛选：检查三个点的最大距离。）
3.失败检测
4 回环检测.
激光系统：
1.初始化
在LIS的初始化开始之前与之后，初始估计是不同的。 在LIS初始化之前，假设这个机器人开始在零速度静止位姿。之后集成初始IMU测量估计零偏与噪声是归零的。
2.失败检测
该文采用了将扫描匹配中的非线性优化问题转化为线性问题 ，当的最小特征值小于初次迭代优化的一个界限时，LIS会声明失败，

实验:
该文采用的是自己收集的数据集 Urban、Jackal、Handheld

用于数据收集的传感器套件包括一个动态动态VLP-16激光雷达，一个FLIR BFS-U3-04S2M-CS相机（鱼眼相机？投影到球面上），一个微型应变3DM-GX5-25 IMU，和一个ReachRS+GPS（用于地面真值）。提出的框架与开源解决方案进行比较，其中包括VINS-Mono、LOAM、Lio映射、LINS和LIO-SAM。
所有的方法都是用C++实现的，并在Ubuntu Linux中使用Intel i7-10710U的笔记本电脑上执行。

Urban 消融实验，由操作员步行和携带传感器套件收集。特点是建筑物、停放和移动的汽车、行人、骑自行车的人和植被。
Jackal ：安装在Jackal无人地面车辆上收集。以结构、植被和各种路面为特征。
端到端移动误差最优是由LINS实现。与GPS点的RMSE以及旋转误差最小值由LVI-SAM实现。
Handheld Dataset
通过一个位于图像顶部中心的开放棒球场，增加了这个数据集的挑战。通过该场时，相机和激光雷达采集的主要观测数据分别以草和地平面为特征，由于简并度问题，所有基于激光雷达的方法都不能产生有意义的结果。所提出的框架LVI-SAM在启用或不启用循环关闭的情况下成功地完成了测试。

---

参考文献

[1]李晟硕. 基于松紧耦合的多传感器融合SLAM算法研究[D].西安电子科技大学,2021.DOI:10.27389/d.cnki.gxadu.2021.002065.
[2]Zhang J, Kaess M, Singh S. Real-time depth enhanced monocular odometry[C]//2014 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2014: 4973-4980.
[3]Zhang J, Singh S. Visual-lidar odometry and mapping: Low-drift, robust, and fast[C]//2015 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2015: 2174-2181.
[4]Graeter J, Wilczynski A, Lauer M. Limo: Lidar-monocular visual odometry[C]//2018 IEEE/RSJ international conference on intelligent robots and systems (IROS). IEEE, 2018: 7872-7879.
[5]Zuo X, Geneva P, Lee W, et al. Lic-fusion: Lidar-inertial-camera odometry[C]//2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2019: 5848-5854.
[6]Zuo X, Yang Y, Geneva P, et al. Lic-fusion 2.0: Lidar-inertial-camera odometry with sliding-window plane-feature tracking[C]//2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2020: 5112-5119.
[7]Shan T, Englot B, Ratti C, et al. Lvi-sam: Tightly-coupled lidar-visual-inertial odometry via smoothing and mapping[C]//2021 IEEE international conference on robotics and automation (ICRA). IEEE, 2021: 5692-5698.]