这篇博客

这篇论文“Robust High Accuracy Visual-Inertial-Laser SLAM System”发表于2019年的 IROS 会议上。它提出了一个融合了相机、惯性元件、激光这三种传感器的系统。通过不同传感器之间的互补作用，该系统，较之于视觉惯性系统和激光系统，具有更好的鲁棒性。

论文摘要

纯视觉SLAM在光照变化明显、纹理较少的环境中容易跟踪丢失，因此研究者将惯性测量元件(IMU)加入到视觉SLAM中，组成 VI-SLAM (视觉惯性SLAM)。但作者发现， IMU 只能在短期内解决纯视觉SLAM 所面对的问题，如果机器长期工作在光照变化明显、纹理缺失的环境中，系统仍会跟踪丢失(因为IMU的偏差是随机变化的，长时间不修正会直接影响位姿估计)。此外，作者还指出了激光SLAM的不足：在结构性特征缺失的环境中（(比如在走廊)）会跟踪丢失。所以作者将视觉惯性、激光两类SLAM相结合，搭建了基于三种传感器工作的更加鲁棒的系统。该系统可大致分为视觉惯性和激光两大位姿估计模块。它先通过视觉惯性模块估计出位姿的初值，再根据激光扫描的结果完成位姿的优化。两个模块可以来联合工作，也可以各自独立工作。这也是该系统鲁棒的原因：当一个模块跟丢了，系统也可以只通过剩余模块完成定位和建图。

一些假设和标注

1、假设相机内参KKK已知，同时三个传感器(相机、IMU、激光)之间的外参矩阵（相对位姿变换关系）也已知；
2、默认这三个传感器在时间上是同步的！！(这点很重要)；
3、三个传感器都有各自的坐标系，依次记为相机：CCC，IMU：III，激光：LLL。此外将世界坐标系记为 WWW 。在初始化后，将 LLL 坐标系作为主要观测坐标系（系统主要记录3D点在 LLL 上的坐标）；
4、使用 SiS_{i}Si 表示 iii 时刻的某个坐标系。并用 TbaT^{a}_{b}Tba 表示从 b 到 a 的位姿变换矩阵；
5、用 XjSX^{S}_{j}XjS 表示地图点 jjj 在 SSS 坐标系上的三维齐次坐标；
6、为了方便，下文用 VI 代表 “视觉惯性” 。

系统总览

系统先通过一个紧耦合的 VI 方法估计位姿，再通过激光扫描的结果来优化该估计值，最后再完成建图。整个过程的框图如下所示：

下面介绍每个模块。

VI 里程计

该系统的 VI 前端是基于 VINS-Mono 系统实现的，主要细节可查看 VINS-Mono 的论文。

扫描匹配（scan matching）优化

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

激光雷达会连续检测到地图点，并获得其在对应时刻 LtiL_{t_{i}}Lti 坐标系下的坐标。以第 kkk 次激光扫描为例，记 tkt_{k}tk 为这次扫描的开始时间， tk+1t_{k+1}tk+1 为结束时间。令 Pk~\widetilde{P_{k}}Pk 为第 kkk 次扫描过程中检测到的所有 3D 点的集合。因为所有传感器是时间同步的，同时 tkt_{k}tk、tk+1t_{k+1}tk+1 时刻机器的位姿已由 VI 里程计估计出，所以可通过 IMU 与激光传感器间的外参矩阵估计出 LkL_{k}Lk、 Lk+1L_{k+1}Lk+1 间的位姿关系，如下式：

式中 TILT^{L}_{I}TIL 是 IMU 与激光传感器间的外参矩阵， TIk+1Ik~\widetilde{T^{I_{k}}_{I_{k+1}}}TIk+1Ik 是 VI 里程计估计的结果。

此时要引入一个假设：在 tkt_{k}tk、tk+1t_{k+1}tk+1间，机器以恒定的速度运动。此假设使得我们能通过线性插值的方法获得 ti∈[tk,tk+1]t_{i}\in [t_{k},t_{k+1}]ti∈[tk,tk+1] 时刻，LiL_{i}Li 与LkL_{k}Lk间的位姿关系，如下所示：

通过上式计算的位姿，可以将任意 tit_{i}ti 时刻检测到的点都转换到 Lk+1L_{k+1}Lk+1 坐标系上表示：

此时，将转换后所有在 Lk+1L_{k+1}Lk+1 坐标系中的点的集合记为 Pk{P_{k}}Pk。

根据 PkP_{k}Pk 中各点局部表面的平滑程度，决定某点属于边缘特征点或平面特征点。将提取出来的特征点的集合记为 FkF_{k}Fk 。此时，再根据已经通过优化得出的 LkL_{k}Lk 与世界坐标系的相对位姿结果 TLkWT^{W}_{L_{k}}TLkW 和公式 (1) ，推出 Lk+1L_{k+1}Lk+1 与世界坐标系之间的相对位姿估计值 TLk+1W~\widetilde{T^{W}_{L_{k+1}}}TLk+1W ：

通过公式(4) 求得的 TLk+1W~\widetilde{T^{W}_{L_{k+1}}}TLk+1W 将 FkF_{k}Fk 包含的所有特征点都投影到世界坐标系中，并为它们在已构建好的地图中寻找相匹配的线边缘、平面块等结构特征（由此就建立了 Lk+1L_{k+1}Lk+1 与地图的联系。根据这个联系就能构建出图优化的边）。然后计算 FkF_{k}Fk 中特征点到相匹配的线边缘或平面块的距离 ddd 。这个过程可以用下面的函数 fff 表示：

（PS:特征点寻找匹配的边缘线和平面块的方法，以及距离的计算方法可在 “LOAM: Lidar odometry and mapping in real time” 论文中查看）
将所有 did_{i}di 相加，即为需要优化的变量：

由此，激光的扫描匹配（scan matching）的结果建模出了一个非线性优化问题。通过牛顿梯度下降的方法使 ddd 的值趋近于 0 （因为理论上特征点到与其匹配的线或面的距离应该为 0）。如果结果能够收敛，则能获得一个关于激光传感器的优化后位姿：TLk+1WT^{W}_{L_{k+1}}TLk+1W。最后通过 TLk+1WT^{W}_{L_{k+1}}TLk+1W 将第 kkk 次激光扫描获得的点云图转换到世界坐标系中，更新地图。
（个人想法：从系统的流程可以看出，系统对于 VI 模块和激光扫描匹配模块采用的是松耦合的方式（先 VI 估计再扫描匹配优化）。因此激光模块优化的效果比较依赖于 VI 的估计初值。这或许能通过紧耦合的方式来改善，但会提高系统的复杂度且降低各模块的灵活性）

提高系统鲁棒性的措施

如最初在摘要中所说的，视觉惯性SLAM 和激光SLAM 在某些环境下都会存在跟踪丢失的风险。所以作者构建的这个系统主要是由两个独立性较强的模块（VI 位姿估计模块和激光扫描匹配模块）组成的。在某个模块失效后，另一个能够独立工作，以保证系统的正常运行。这就是该系统具有较高鲁棒性的原因。
根据系统工作时所使用的模块的不同，可分成以下三种工作模式：
（1）正常工作模式：
此时 VI 和激光模块都正常工作，系统按照前几节描述的流程完成定位和建图；
（2）scan to scan matching 工作模式：
此时 VI 模块跟踪丢失，启动 “两次激光扫描间的匹配（scan to scan matching）” 定位模块（自己取的名字，见笑见笑）。这个模块是基于 LOAM 系统构造的，它的大致工作过程和上一小节中的 “扫描匹配（scan matching）优化” 相似，只是这里估计的是 LkL_{k}Lk 和Lk−1L_{k-1}Lk−1 之间的位姿 TLk−1LkT^{L_{k}}_{L_{k-1}}TLk−1Lk ，而不是 TLkWT^{W}_{L_{k}}TLkW。此模式下系统的工作流程图如下所示：

（PS：当发生以下几种情况时，系统会认定 VI 模块失效：当前跟踪到的特征点很少、IMU 偏差变化较大、VI 滑动窗口中的位姿估计值与先前估计的结果偏差较大）
（3）仅使用 VI 工作模式：
此时系统仅使用 VI 模块完成定位和建图，流程图如下所示：

（PS：此模式主要是在结构信息缺失的环境中使用）
这三种工作模式使得系统能在多种不同的、难度较高的环境下继续工作。同时，当环境条件变好后，系统仍会重新启用之前失效的模块，变回正常的工作模式。
（疑惑：以第三个模式为例，当激光模块重新工作时，由 VI 模块构建的地图作为激光模块的初始地图，还是激光模块会重新构建新的地图？即 VI 和激光模块两者所构建的地图是否能够互相使用。）

闭环检测和临近检测

老规矩，为了消除累积误差，系统需要有闭环检测的能力。因为当前系统搭载了视觉、激光传感器，所以它实现闭环检测的方式有两种：
1、基于视觉和词袋向量的闭环检测，也就是最经典的视觉闭环检测方法；
2、基于激光雷达的临近检测。
作者指出，第一种方法存在一定缺点：必须观测到相同物体，才能检测到闭环(对机器的观测视角有要求)。而第二种方法则没有这个限制，因为激光雷达是 360° 扫描的。为了避免过多的约束和计算，某个关键帧( KF )在实现视觉闭环检测后，将不再进行临近检测。
闭环检测想必大家都较为了解，所以这里主要介绍一下临近检测（这部分内容也是第一次看，理解不对之处望谅解和指出），它的过程图如下：

首先在 KF 数据库中计算各 KF 与当前关键帧( KFcKF_{c}KFc)在世界坐标系下的相对距离（即二者三维坐标的距离）。记所有与 KFcKF_{c}KFc 距离小于 R1R_{1}R1 关键帧的集合为 KFsloopKFs_{loop}KFsloop 。以 KFsloopKFs_{loop}KFsloop 中的一个关键帧（KFaKF_{a}KFa）为例：
在 KFcKF_{c}KFc 和 KFaKF_{a}KFa 已有的估计位姿基础上，求出两者间的相对位姿 TKFcKFcT^{KF_{c}}_{KF_{c}}TKFcKFc ，再根据先前提到的 “两次激光扫描间的匹配方法” ，优化 TKFcKFcT^{KF_{c}}_{KF_{c}}TKFcKFc 。此时如果 KFcKF_{c}KFc 和 KFaKF_{a}KFa 之间的相对距离小于 R2R_{2}R2，则两帧满足临近检测要求。
（PS：每个关键帧包含机器的位姿、对应图片的特征点和从激光扫描中获得的几何特征点信息）
对 KFcKF_{c}KFc 和 KFsloopKFs_{loop}KFsloop 中每个 KFs 都进行如上操作。如果有多个 KFs 满足临近检测要求，则只选择产生时间最早的那个 KF 与 KFcKF_{c}KFc 构成闭环（因为这样可以获得较大的闭环，有利于消除更多的累积误差）。此外，假设 KFcKF_{c}KFc 的编号为 kkk ，那么编号为 k−e,...,k−1k-e,...,k-1k−e,...,k−1的 KF 不会参与临近检测的判定（ e 的值根据经验设定）。这样可以降低计算量。
（个人理解：从论文的实验结果来看，同时使用视觉闭环和临近检测与只使用其中之一相比，最终精度的差距不是很大。但是为了保证系统的两个模块的独立性，这两个检测方法需要要同时存在。）

全局位姿图优化

当完成闭环产生后，系统将执行全局位姿图优化。图中 KF 作为顶点，它包含三种边：
1、相邻 KF 之间的连续边，这个边计算的是相邻 KF 间的相对平移和旋转变换：

2、闭环检测边。这个边计算的是闭环 KF 之间的相对平移和旋转变换，使用的是 PnP方法。
3、临近检测边。和闭环边类似，只不过采用的是 “两次激光扫描间的匹配方法”。
所以，图中顶点 iii 和 jjj 之间的误差可表示为：

（个人理解：式中减号左边的项可能由 IMU 预积分获得，因为 IMU 测量值的预积分结果可能更接近于真值）
整个优化图中的总误差为：

式中A、B、CA、B、CA、B、C 分别对应上述 1、2、3 类边。这个非线性优化问题通过 Ceres Solver解决。为了降低计算量，系统会限制 KF 数据库中关键帧的数量。构成闭环的 KFs 会被保留，而那些与自己邻近 KFs 距离较近的关键帧将被剔除。

总结

这个系统应该是多传感器融合 SLAM 中较简单的一个实现方法。它将视觉惯性和激光匹配两种位姿估计方法进行松耦合（先 VI 估计，再激光匹配优化），让两个不同、相对独立的模块共同完成机器的位姿估计。这使得系统的工作流程更为清晰，降低了复杂度，同时也保证系统能够灵活地对切换工作模式，以应对不同的环境条件。此外，系统在闭环检测时采用的两种不同方法，也能提高整体精度。

一种视觉惯性+激光传感器的SLAM系统相关推荐

【开源分享】VIDO-SLAM：一种视觉惯性动态物体SLAM系统
文章:Inertial-Only Optimization for Visual-Inertial Initialization 作者:Carlos Campos, Jose M.M. Montiel ...
视觉惯性组合导航技术最新综述：应用优势、主要类别及一种视觉惯性组合导航无人系统开发验证平台分享
导读: 随着无人机.无人车以及移动机器人的井喷式发展,导航技术成为了制约无人平台广泛应用的瓶颈技术之一.在应用需求的牵引下,视觉惯性组合导航技术,特别是视觉与微惯性传感器的组合,逐渐发展成为当前自主导 ...
（视觉和激光传感器）SLAM 做室内GPS与室外真实GPS在无人机上的对比
1.室外无人机GPS的作用 1)记录实时无人机起飞点与当前飞行无人机的绝对位置关系,显示当面无人机离起飞点的真实距离 2)做室外无人机悬停的功能,使用GPS当前点与悬停点GPS经纬度做对比 3)无人机 ...
视觉惯性单目SLAM （三）优化方法
1. 最小二乘法最小二乘法:Least Squares Method (LSM) 函数: y=f(x,θ) y = f ( x , θ ) y=f(x,\theta) θ θ \theta为待估参数 ...
视觉惯性单目SLAM （二）算法基础知识
1. 基本概念视觉惯性:Visual-Inertial (VI) VI ORB-SLAM:视觉惯性ORB-SLAM VI ORB-SLAM输入: IMU数据(用B表示,加速度:aB:角速度:ωB\m ...
视觉惯性单目SLAM （一）算法描述
1.视觉惯性ORB-SLAM基本知识视觉惯性:Visual-Inertial (VI) VI ORB-SLAM:视觉惯性ORB-SLAM VI ORB-SLAM输入: IMU数据(用B表示,加速度: ...
视觉惯性单目SLAM （五）矩阵微积分
1. 基本概念矩阵微积分:Matrix Calculus 微积分的核心思想:局部线性化\color {red}{微积分的核心思想:局部线性化} 矩阵函数(包括向量函数)对标量的导数:等于它的各个元素 ...
视觉惯性里程计 VIO
视觉惯性里程计 VIO - Visual Inertial Odometry 视觉−惯性导航融合SLAM方案视觉惯性SLAM专栏 VINS技术路线与代码详解 VINS理论与代码详解0--理论基础白话 ...
腿式机器人激光SLAM系统
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者:robot L | 来源:知乎 https://zhuanlan.zhihu.com/p/148 ...

一种视觉惯性+激光传感器的SLAM系统