精度1Robust High Accuracy Visual-Inertial-Laser SLAM System

参考大佬讲解 配合自己弱点一种视觉惯性+激光传感器的SLAM系统

论文基本信息

• 题目：Robust High Accuracy Visual-Inertial-Laser SLAM System
• 期刊：2019年的 IROS 会议
• 方向：多传感器融合技术（激光-惯导-视觉）

摘要

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

一： 引言 文献的贡献

• 高精度视觉惯性激光测距仪，从视觉-IMU紧密耦合的测距法开始，然后进行激光扫描匹配以完善估算。
• 可以灵活，自动地调整系统，以应对传感器性能下降的情况。
• 将基于视觉的闭环和基于激光的接近检测相结合，以增加全局约束。
• 6自由度全局姿势优化，以保持全局一致性

二：相关工作

视觉-IMU的融合方式有两中

• 1：松耦合：将IMU视为独立模块，以辅助视觉测量获得的运动估计。
• 2：紧耦合：从原始数据共同优化视觉和IMU信息
  
  紧耦合是把图像的特征加到特征向量中去，这样做优点是可以免去中间状态的累计误差，提高精度，缺点是系统状态向量的维数会非常高，需要很高的计算量；
  松耦合是把V处理后获得的变换矩阵和IMU融合，这样做优点是计算量小但是会带来累计误差；

三：假设和标注

四：系统总览

A. Visual-Inertial Odometry

系统先通过一个紧耦合的 VI 方法估计位姿，再通过激光扫描的结果来优化该估计值，最后再完成建图。该系统的 VI 前端是基于 VINS-Mono （提出的框架）系统实现的。

B. Scan Matching

这部分主要涉及激光定位，主要是 LOAM系统中的知识）扫描匹配（scan matching）模块的工作流程框图如下所示：

五：鲁棒性增强

由摘要所提：视觉-IMU和激光SLAM存在的缺点。作者构建的系统由视觉-IMU和激光两个独立的模块耦合组成。当某个模块失去工作能力时，另外一个模块能够独立工作。以保证系统的正常运行。这就是该系统具有较高鲁棒性的原因。

• 1：正常工作模式：
  VI和激光模块都正常工作，系统按照下图工作：
  

• 2：VI不工作，激光工作
  判断VI不工作方法：如果当前帧中跟踪的特征数低于阈值，滑动窗口中的估计位置和方向与先前的估计相比发生突变，并且IMU偏差估计发生较大变化，则系统确定视觉惯性里程计模块无效。
  实施方案：该模块基于LOAM组件， 简而言之，首先从${\tilde{P}}_k$中提取几何特征，并找到$P_{k-1}$中的边缘线或平面斑块作为它们的对应关系，然后计算从一个特征点到其对应点的距离，最后恢复激光雷达在两次连续扫描之间的激光雷达运动。 最小化特征点与其对应关系的总距离。
  

• 3 仅VI的工作模式
  此模式主要是在结构信息缺失的环境中，由于无法提取足够的几何特征，因此扫描匹配无法完善运动估计。
  
  此时跳出Scan Matching 的工作方式，直接通过视觉传感器得到环境的数据信息，当环境中结构信息增多，自动恢复正常的工作模式。

六：闭环检测和临近检测

• 闭环检测和临近检测的目的时消除累计误差，文献中的系统搭载了视觉-IMU、激光传感器，所以它实现闭环检测的方式有两种：
   1、基于视觉和词袋向量的闭环检测，也就是最经典的视觉闭环检测方法；
   2、基于激光雷达的临近检测。

A. Loop closure and Proximity Detection

视觉闭环检测：使用的为PNP算法，它必须观测到相同物体，才能检测到闭环(对机器的观测视角有要求)。因为激光雷达是 360° 扫描的，所以文献也运用了激光临近检测的方法。但是为了避免过多的约束和计算，某个关键帧在实现视觉闭环检测后，将不再进行临近检测。

• 临近检测方法
  
  1：在同一世界坐标系下计算当前关键帧和数据库所有关键帧的相对距离，如果相对距离小于$R_1$，则将圆中的数据作为预测数据。并通过scan-to-scan matching进行更新，如果遍历到关键帧得到更小的半径$R_2$，则选取$R_2$中对应的关键字，若出现距离相同的关键帧，则选择更远（因为这样可以获得较大的闭环，有利于消除更多的累积误差）的作为临近闭环的关键帧。
  2：同时使用视觉闭环和临近检测与只使用其中之一相比，最终精度的差距不是很大。但是为了保证系统的两个模块的独立性，这两个检测方法需要要同时存在。

B. Global Pose Graph Optimization

当完成闭环产生后，系统将执行全局位姿图优化。关键帧对应于位姿图的顶点，并且有三种连接顶点边的方法。
1：相邻关键帧的连续边：这个边计算的是相邻 KF 间的相对平移和旋转变换。
t ~ j i = ( q ~ i w ) − 1 ( t ~ j w − t ~ i w ) q ~ j i