【论文阅读：激光SLAM】R3LIVE

摘要

论文提出了一种新颖的激光雷达-惯性-视觉传感器融合框架，称为 R3LIVE，它利用激光雷达、惯性和视觉传感器的测量优势来实现稳健而准确的状态估计。

R3LIVE 包含两个子系统，即激光雷达-惯性里程计（LIO）和视觉-惯性里程计（VIO）。

• LIO 子系统（FASTLIO）利用激光雷达和惯性传感器的测量数据，构建全局地图的几何结构（即三维点的位置）。
• VIO 子系统利用视觉惯性传感器的数据，渲染地图的纹理（即三维点的颜色）。

更具体地说，VIO 子系统通过最小化帧到地图的光度误差，直接有效地融合视觉数据。所开发的 R3LIVE 系统是在我们之前的成果 R2LIVE 的基础上开发的，经过了精心的架构设计和实施。

实验结果表明，与目前的同类产品相比，R3LIVE 系统具有更强的鲁棒性和更高的状态估计精度。R3LIVE是一个面向各种可能应用的多功能、精心设计的系统，它不仅可以作为实时机器人应用的SLAM系统，还可以为测绘等应用重建密集、精确的RGB彩色三维地图。

此外，为了提高R3LIVE的可扩展性，开发了一系列用于重建和纹理网格的离线实用程序，从而进一步缩小了R3LIVE与模拟器、视频游戏等各种三维应用之间的差距。

简介

最近，激光雷达传感器越来越多地应用于各种机器人领域，如自动驾驶汽车、无人机等。特别是随着低成本固态激光雷达的出现，基于这些激光雷达的更多应用推动了机器人界的发展。

然而，对于基于激光雷达的 SLAM 系统来说，在没有足够几何特征的情况下很容易失效，尤其是固态激光雷达的 FoV 通常有限。为解决这一问题，将激光雷达与其他传感器（如摄像头和超宽带 (UWB)）相结合，可以提高系统的鲁棒性和准确性。

最近，机器人界提出了各种激光雷达与视觉融合框架。V-LOAM是早期的激光雷达-惯性-视觉系统作品之一，它利用松耦合的视觉-惯性里程测量（VIO）作为初始化激光雷达测绘子系统的运动模型。同样，在有人提出了一种立体视觉-惯性激光雷达 SLAM，它将紧耦合的立体视觉-惯性里程测量与激光雷达映射和激光雷达增强视觉闭环结合在一起。最近，Wang 等人提出了 DV-LOAM，这是一种直接的视觉-激光雷达融合框架。该系统首先利用两阶段的直接视觉里程测量模块进行高效的粗略状态估计，然后利用激光雷达测绘模块对粗略姿态进行细化，最后利用闭环模块修正累积漂移。上述系统在松耦合的层面上融合了激光雷达-惯性-视觉传感器，其中激光雷达测量并没有与视觉或惯性测量一起进行联合优化。

最近有人提出了更紧耦合的激光雷达-惯性-视觉融合框架。例如，的 LIC-fusion 是一种紧耦合的激光雷达-惯性-视觉融合框架，它在多态约束卡尔曼滤波器（MSCKF）框架内将 IMU 测量、稀疏视觉特征、激光雷达特征与在线空间和时间校准结合在一起。为了进一步提高激光雷达扫描匹配的鲁棒性，他们的后续工作（称为 LIC-Fusion 2.0 ）提出了一种在滑动窗口内跨越多个激光雷达扫描的平面特征跟踪算法，并在窗口内完善姿态轨迹。Shan 等人中提出的 LVI-SAM 通过紧密耦合的平滑和映射框架融合了激光雷达-视觉-惯性传感器，该框架建立在因子图之上。当检测到其中一个子系统出现故障时，LVI-SAM 的激光雷达-惯性子系统和视觉-惯性子系统可独立运行；当检测到足够多的特征时，则可联合运行。

之前的研究成果 R2LIVE 将激光雷达-惯性-视觉传感器的数据紧密融合，提取激光雷达和稀疏视觉特征，在误差状态迭代卡尔曼滤波框架内通过最小化特征重投影误差来估计状态，从而实现实时性能，同时通过滑动窗口优化提高整体视觉映射精度。R2LIVE 能够在各种具有挑战性的场景中运行，如剧烈运动、传感器故障，甚至在具有大量移动物体和较小激光雷达 FoV 的狭窄隧道环境中。

在本文中，基于激光雷达、惯性和视觉测量的紧耦合融合，解决了实时同步定位、三维制图和地图渲染的问题。贡献如下：

• 提出了一种实时同步定位、绘图和着色框架。该框架由用于重建几何结构的激光雷达-惯性里程计（LIO）和用于纹理渲染的视觉-惯性里程计（VIO）组成。整个系统能够实时重建密集的三维 RGB 彩色环境点云
• 提出了一种基于 RGB 彩色点云图的新型 VIO 系统。VIO 通过最小化观察到的地图点的 RGB 颜色与其在当前图像中测量到的颜色之间的光度误差来估计当前状态。这一过程不需要环境中突出的视觉特征，并节省了相应的处理时间（如特征检测和提取），这使得系统更加稳健，尤其是在无纹理环境中。
• 所提出的方法应用到一个完整的系统 R3LIVE 中，该系统能够实时、低漂移地构建密集、精确的三维 RGB 彩色环境点云图。我们在各种室内和室外环境中对整个系统进行了验证。结果表明，系统在行驶 1.5 公里后，平移漂移仅为 0.16 米，旋转漂移仅为 3.9 度。
• 在 Github 上开源了系统。开发了几款离线工具，用于从着色点云重建网格并对网格进行纹理处理。

系统概述

系统概览如上图所示，提出的框架包含两个子系统：LIO 子系统（上部）和 VIO 子系统（下部）。

• LIO 子系统构建全局地图的几何结构，该结构注册输入的激光雷达扫描数据，并通过最小化点到面残差来估计系统状态。
• VIO 子系统构建地图纹理，用输入图像渲染每个点的 RGB 颜色，通过最小化帧到帧的 PnP 重投影误差和帧到地图的光度误差来更新系统状态。

符号

以下是论文中使用到的各类运算符和变量：

状态向量

在我们的工作中，我们定义完整状态向量 $\mathbf{x} \in \mathbb{R}^{29}$ 为:

$$\mathbf{x}=\left[{ }^G \mathbf{R}_I^T,{ }^G \mathbf{p}_I^T,{ }^G \mathbf{v}^T, \mathbf{b}_{\mathbf{g}}^T, \mathbf{b}_{\mathbf{a}}^T,{ }^G \mathbf{g}^T,{ }^I \mathbf{R}_C^T,{ }^I \mathbf{p}_C^T,{ }^I t_C, \phi^T\right]^T$$

式中， ${ }^G \mathbf{g} \in \mathbb{R}^3$ 是以全局帧（即第一个激光雷达帧）表示的重力矢量， ${ }^I t_C$ 是 IMU 和相机之间的时间差，而激光雷达被假定已经与 IMU 同步, $\phi=\left[f_x, f_y, c_x, c_y\right]^T$ 是相机内参, 包括 $\left(f_x, f_y\right)$ 像素焦距大小和 $\left(c_x, c_y\right)$ 中心点从图像平面左上角的偏移量。（其他在上表中解释）

地图表述

我们的地图由体素和点组成，其中点包含在体素中，是地图的最小元素。

1、体素：为了在 VIO 子系统中快速找到地图中的点进行渲染和跟踪，设计了一个固定大小（例如 0.1m ˆ 0.1m ˆ 0.1m）的容器，命名为体素（voxel）。如果一个体素最近（例如最近 1 秒内）被添加了点，我们就会将该体素标记为已激活。否则，该体素将被标记为停用。

2、点： 点 $\mathbf{P}$ 是一个大小为 6 的向量：

$$\mathbf{P}=\left[{ }^{G} \mathbf{p}_{x},{ }^{G} \mathbf{p}_{y},{ }^{G} \mathbf{p}_{z}, \mathbf{c}_{r}, \mathbf{c}_{g}, \mathbf{c}_{b}\right]^{T}=\left[{ }^{G} \mathbf{p}^{T}, \mathbf{c}^{T}\right]^{T}$$

式中，${ }^{G} \mathbf{p}=\left[{ } \mathbf{p}_{x},{ } \mathbf{p}_{y},{ }\mathbf{p}_{z}\right]$ 表示点在全局帧中的三维位置，$\mathbf{c}= \left[\mathbf{c}_{r}, \mathbf{c}_{g}, \mathbf{c}_{b}\right]$ 是点的 RGB 颜色。此外，我们还记录了该点的其他信息，如 3×3 协方差矩阵 $Σ_p$ 和 $Σ_c$，它们分别表示 ${ }^{G} \mathbf{p}$ 和 $c$ 的估计误差，以及该点创建和渲染的时间戳。

激光雷达-惯性里程计子系统

R3LIVE 的 LIO 子系统构建了全局地图的几何结构。对于传入的激光雷达扫描，如 Fast-lio 所示，帧内连续运动造成的运动失真由 IMU 反向传播补偿。然后，我们利用误差状态迭代卡尔曼滤波器（ESIKF）最小化点-面残差来估计系统状态。

最后，根据收敛后的状态，将此次扫描的点添加到全局地图中，并将相应的体素标记为激活或停用。全局地图中累积的三维点构成几何结构，也用于为我们的 VIO 子系统提供深度。关于 R3LIVE 中 LIO 子系统的详细实现方法，请读者参阅之前的相关工作 R2live 和 Fast-lio2。

视觉惯性里程计子系统

VIO 子系统渲染全局图的纹理，并通过最小化光度误差来估计系统状态。

更具体地说，将一定数量的点（即跟踪点）从全局地图投射到当前图像，然后在 ESIKF 框架内通过最小化这些点的光度误差来迭代估计系统状态。为了提高效率，跟踪的地图点是稀疏的，这通常需要建立一个输入图像的金字塔。然而，金字塔对平移或旋转并不不变，因此也需要对其进行估计。

在提出的框架中，利用单个地图点的颜色来计算光度误差。在 VIO 中同时渲染的颜色是地图点的固有属性，与摄像机的平移和旋转无关。为确保稳健而快速的收敛，我们设计了图 2 所示的两步流水线，首先利用帧到帧的光学流来跟踪地图点，并通过最小化所跟踪地图点的透视点（PnP）投影误差来优化系统状态。然后，我们通过最小化跟踪点之间的帧到地图光度误差，进一步完善系统的状态估计。有了收敛后的状态估计和原始输入图像，我们就可以进行纹理渲染，更新全局地图中点的颜色。