当前位置：首页 > 编程日记 > 正文

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

编程日记 2024-06-08 15:00:00

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

这篇博客
论文摘要
一些假设和标注
系统总览
- VI 里程计
- 扫描匹配（scan matching）优化
提高系统鲁棒性的措施
闭环检测和临近检测
- 全局位姿图优化
总结

这篇博客

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

论文摘要

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

一些假设和标注

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

系统总览

系统先通过一个紧耦合的 VI 方法估计位姿，再通过激光扫描的结果来优化该估计值，最后再完成建图。整个过程的框图如下所示：
在这里插入图片描述
下面介绍每个模块。

VI 里程计

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

扫描匹配（scan matching）优化

（这部分主要涉及激光定位，主要是 LOAM系统中的知识）扫描匹配（scan matching）模块的工作流程框图如下所示：
在这里插入图片描述
激光雷达会连续检测到地图点，并获得其在对应时刻 $L_{t_{i}}$ 坐标系下的坐标。以第 $k$ 次激光扫描为例，记 $t_{k}$ 为这次扫描的开始时间， $t_{k+1}$ 为结束时间。令 $Pk~\widetilde{P_{k}}$ 为第 $k$ 次扫描过程中检测到的所有 3D 点的集合。因为所有传感器是时间同步的，同时 $t_{k}$ 、 $t_{k+1}$ 时刻机器的位姿已由 VI 里程计估计出，所以可通过 IMU 与激光传感器间的外参矩阵估计出 $L_{k}$ 、 $L_{k+1}$ 间的位姿关系，如下式：
在这里插入图片描述
式中 $TILT^{L}_{I}$ 是 IMU 与激光传感器间的外参矩阵， $TIk+1Ik~\widetilde{T^{I_{k}}_{I_{k+1}}}$ 是 VI 里程计估计的结果。

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

在这里插入图片描述
通过上式计算的位姿，可以将任意 $t_{i}$ 时刻检测到的点都转换到 $L_{k+1}$ 坐标系上表示：

在这里插入图片描述
此时，将转换后所有在 $L_{k+1}$ 坐标系中的点的集合记为 ${P_{k}}$ 。

根据 $P_{k}$ 中各点局部表面的平滑程度，决定某点属于边缘特征点或平面特征点。将提取出来的特征点的集合记为 $F_{k}$ 。此时，再根据已经通过优化得出的 $L_{k}$ 与世界坐标系的相对位姿结果 $TLkWT^{W}_{L_{k}}$ 和公式 (1) ，推出 $L_{k+1}$ 与世界坐标系之间的相对位姿估计值 $TLk+1W~\widetilde{T^{W}_{L_{k+1}}}$ ：
在这里插入图片描述
通过公式(4) 求得的 $TLk+1W~\widetilde{T^{W}_{L_{k+1}}}$ 将 $F_{k}$ 包含的所有特征点都投影到世界坐标系中，并为它们在已构建好的地图中寻找相匹配的线边缘、平面块等结构特征（由此就建立了 $L_{k+1}$ 与地图的联系。根据这个联系就能构建出图优化的边）。然后计算 $F_{k}$ 中特征点到相匹配的线边缘或平面块的距离 $d$ 。这个过程可以用下面的函数 $f$ 表示：
在这里插入图片描述
（PS:特征点寻找匹配的边缘线和平面块的方法，以及距离的计算方法可在 “LOAM: Lidar odometry and mapping in real time” 论文中查看）
将所有 $d_{i}$ 相加，即为需要优化的变量：

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

提高系统鲁棒性的措施

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

（PS：当发生以下几种情况时，系统会认定 VI 模块失效：当前跟踪到的特征点很少、IMU 偏差变化较大、VI 滑动窗口中的位姿估计值与先前估计的结果偏差较大）
（3）仅使用 VI 工作模式：
此时系统仅使用 VI 模块完成定位和建图，流程图如下所示：
在这里插入图片描述
（PS：此模式主要是在结构信息缺失的环境中使用）
这三种工作模式使得系统能在多种不同的、难度较高的环境下继续工作。同时，当环境条件变好后，系统仍会重新启用之前失效的模块，变回正常的工作模式。
（疑惑：以第三个模式为例，当激光模块重新工作时，由 VI 模块构建的地图作为激光模块的初始地图，还是激光模块会重新构建新的地图？即 VI 和激光模块两者所构建的地图是否能够互相使用。）

闭环检测和临近检测

老规矩，为了消除累积误差，系统需要有闭环检测的能力。因为当前系统搭载了视觉、激光传感器，所以它实现闭环检测的方式有两种：
1、基于视觉和词袋向量的闭环检测，也就是最经典的视觉闭环检测方法；
2、基于激光雷达的临近检测。
作者指出，第一种方法存在一定缺点：必须观测到相同物体，才能检测到闭环(对机器的观测视角有要求)。而第二种方法则没有这个限制，因为激光雷达是 360° 扫描的。为了避免过多的约束和计算，某个关键帧( KF )在实现视觉闭环检测后，将不再进行临近检测。
闭环检测想必大家都较为了解，所以这里主要介绍一下临近检测（这部分内容也是第一次看，理解不对之处望谅解和指出），它的过程图如下：
在这里插入图片描述
首先在 KF 数据库中计算各 KF 与当前关键帧( $KF_{c}$ )在世界坐标系下的相对距离（即二者三维坐标的距离）。记所有与 $KF_{c}$ 距离小于 $R_{1}$ 关键帧的集合为 $KFs_{loop}$ 。以 $KFs_{loop}$ 中的一个关键帧（ $KF_{a}$ ）为例：
在 $KF_{c}$ 和 $KF_{a}$ 已有的估计位姿基础上，求出两者间的相对位姿 $T^{KF_{c}}_{KF_{c}}$ ，再根据先前提到的 “两次激光扫描间的匹配方法” ，优化 $T^{KF_{c}}_{KF_{c}}$ 。此时如果 $KF_{c}$ 和 $KF_{a}$ 之间的相对距离小于 $R_{2}$ ，则两帧满足临近检测要求。
（PS：每个关键帧包含机器的位姿、对应图片的特征点和从激光扫描中获得的几何特征点信息）
对 $KF_{c}$ 和 $KFs_{loop}$ 中每个 KFs 都进行如上操作。如果有多个 KFs 满足临近检测要求，则只选择产生时间最早的那个 KF 与 $KF_{c}$ 构成闭环（因为这样可以获得较大的闭环，有利于消除更多的累积误差）。此外，假设 $KF_{c}$ 的编号为 $k$ ，那么编号为 $k - e, . . ., k - 1$ 的 KF 不会参与临近检测的判定（ e 的值根据经验设定）。这样可以降低计算量。
（个人理解：从论文的实验结果来看，同时使用视觉闭环和临近检测与只使用其中之一相比，最终精度的差距不是很大。但是为了保证系统的两个模块的独立性，这两个检测方法需要要同时存在。）

全局位姿图优化

当完成闭环产生后，系统将执行全局位姿图优化。图中 KF 作为顶点，它包含三种边：
1、相邻 KF 之间的连续边，这个边计算的是相邻 KF 间的相对平移和旋转变换：
在这里插入图片描述
2、闭环检测边。这个边计算的是闭环 KF 之间的相对平移和旋转变换，使用的是 PnP方法。
3、临近检测边。和闭环边类似，只不过采用的是 “两次激光扫描间的匹配方法”。
所以，图中顶点 $i$ 和 $j$ 之间的误差可表示为：
在这里插入图片描述
（个人理解：式中减号左边的项可能由 IMU 预积分获得，因为 IMU 测量值的预积分结果可能更接近于真值）
整个优化图中的总误差为：

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

总结

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

https://www.dkcj.cn/info/9825.html

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

这篇博客

论文摘要

一些假设和标注

系统总览

VI 里程计

扫描匹配（scan matching）优化

提高系统鲁棒性的措施

闭环检测和临近检测

全局位姿图优化

总结

相关文章：