R2LIVE论文翻译: A Robust, Rea-time, LiDAR-Inertial-Visual tightly-coupled state Estimator and mapping.

---

写在前面

文章参考:3D视觉工坊
论文原文:论文原文链接

摘要

本文提出一个鲁棒实时的多传感器紧耦合框架，可以融合激光雷达、惯导器件和相机的数据完成精确的状态估计。本文提出的框架由基于滤波方法的里程计模块和基于因子图的优化模块组成。为了保证实时性，我们只估计Kalman滤波器中的当前帧状态，然后在因子图优化环节进一步提高滑动窗口中的所有传感器位姿。得益于多模态传感器数据，我们的算法可以应对视觉退化、激光退化的场景，并且可以在端上计算平台实时运行。在我们采集的大量室内外数据上验证后，证明我们的框架得到的结果相对于其他先进的的激光-惯导里程计、视觉-惯导里程计方法提高了精度。

1. 引言

基于六自由度(DOF)估计自我运动的能力，同时构建周围环境稠密、高精度的地图,基于雷达的SLAM已广泛应用于自动驾驶车辆[1]、无人机[2,3]等领域。随着激光雷达技术的发展，低成本激光雷达（如 Livox 激光雷达[4]）的出现使激光雷达更容易获得。遵循这一趋势，一些相关的工作[5]-[8]引起了社区对这一研究领域的关注。然而，基于激光雷达的SLAM方法在那些很少有可用几何特征的情况下很容易失败（即退化），这对于那些具有小FoV[9]的激光雷达来说更为重要。在这项工作中，为了解决基于激光雷达里程计的退化问题，我们提出了一个激光雷达惯性-视觉融合框架，以获得更高的鲁棒性和准确性的状态估计。我们的工作的主要贡献是：

• 我们利用了来自激光雷达、惯性传感器和照相机传感器的测量数据，并以紧密耦合的方式融合它们。实验表明，我们的方法在各种具有挑战性的情况下，剧烈运动,传感器失效甚至是在狭窄隧道状环境中，有大量的移动物体和小的激光雷达视场也具有足够的鲁棒性。
• 我们提出了一个基于高速率滤波器的里程计和低速率因子图优化的框架。基于滤波器的里程计融合了雷达,惯性和相机传感器的测量数据,在误差状态迭代卡尔曼滤波器中实现实时性能.因子图优化细化了包含关键帧位姿和视觉路标点位置的局部地图.
• 通过紧耦合融合不同类型的传感器,我们实现了高精度的状态估计.试验结果表明我们的系统足够精确，可以用于重建大规模的、室内外密集的建筑结构三维图(见图。1).
  

我们的系统是精心设计和开源的1，以造福整个机器人社区。

2. 相关工作

在这一部分,我们回顾了与我们工作相关的现有工作,包括仅激光雷达的里程计和地图构建,激光雷达惯性融合和激光雷达惯性视觉方法.

A. 激光里程计与建图

zhang等人[10]首先提出了一个激光雷达里程计和建图框架LOAM，该方法将ICP方法[11]与点到平面和点到边缘的距离相结合。通过以不同的速度运行这两个模块，实现了良好的里程计和建图性能。为了使算法在计算有限的平台上实时运行，shan等[12]提出了一种轻量级的地面优化LOAM（Lego-loam），消除了地面分割步骤中不可靠的特征。这些工作主要是基于多线激光雷达。我们之前的工作[9,13]通过考虑具有小FOV(视场角)的固态激光雷达的低级物理特征，开发了一个精确和鲁棒的算法。然而，这些方法仅基于激光雷达测量，非常容易受到无特征的环境或其他退化情况的影响。

B. 激光惯性里程计

现有的激光雷达-惯性融合的工作可以分为两类：松耦合和紧耦合。松耦合方法分别处理两个传感器，以推断它们的运动约束，而紧耦合方法则通过联合优化直接融合激光雷达和惯性测量。与松散耦合方法相比，紧密耦合方法表现出较高的鲁棒性和准确性，因此近年来引起了越来越多的研究兴趣。例如，[14]的作者提出了一种基于激光雷达惯性里程计LIOM的先验和旋转约束优化方法的图优化。与前一种算法相比，LIO-SAM[15]在因子图中优化了关键帧姿态的滑动窗口，以获得更高的精度。同样，li等人也一样,提出了基于滑动窗口优化的传统和固态激光器的LiLi-OM算法[16]。LINS[17]是第一个通过迭代卡尔曼滤波解决6个自由度运动的紧密耦合的LIO。为了降低计算卡尔曼增益时的高计算负载，我们之前的工作快速Fast-LIO[5]提出了一个新的卡尔曼增益计算公式，结果的计算复杂度取决于状态维数而不是测量维数。这项工作在UAV上的嵌入式计算机上运行时，实现了高达50hz的里程计和建图速率。

C. 激光-惯性-视觉里程计

在激光雷达惯性方法的基础上，结合视觉传感器测量结果的激光雷达惯性视觉里程计具有较高的鲁棒性和精度。在[18]的工作中，激光雷达测量被用于为相机图像提供深度信息，形成一个类似于rgb-d相机的系统，可以利用现有的视觉SLAM工作，如ORB-SLAM[19]。这是一种松散耦合的方法，因为它忽略了激光雷达测量所施加的对状态的直接约束。zuo等人[20]提出了一种基于MSCKF框架的结合IMU测量、稀疏视觉特征、激光雷达平面和边缘特征与在线时空校准的LIC-Fusion框架，并称该框架比现有最先进的方法更准确、稳健。他们的进一步工作称为LIC-Fusion 2.0[21]改进了一种滑窗内多个激光雷达扫描的平面特征跟踪算法，使激光雷达扫描匹配更加鲁棒性。

据我们所知，我们的工作是第一个开源的紧密耦合的激光雷达-惯性视觉融合系统。通过融合不同类型的传感器测量值，我们实现了更高的精度和鲁棒性的状态估计。结果表明，我们的系统比最先进的激光惯性和视觉惯性融合估计器（如FAST-LIO[5]和VINS-mono[22]）更准确和鲁棒。

3. 系统概述

我们的系统概述如图2所示，其中基于滤波器的里程计利用LiDAR、相机和惯性传感器的测量值，在误差状态迭代卡尔曼滤波器框架内估计状态，如第IV节所述。为了进一步改善视觉测量，我们利用因子图优化来优化局部滑动窗口内的视觉路标点，如第五部分中所述。

4. 基于滤波器的里程计

A. 箱加和箱减运算

本文,我们利用箱加和箱减运算封装在流形M上，以简化符号和推导。设M是系统状态所在的流形。由于一个流形与Rn是局部同态的，其中n是M的维数，我们可以使用两个箱加和箱减运算，在M的局部邻域和它的切空间Rn[23]之间建立一个双射映射：

对于复合流形M=SO(3)XRn,我们有:

罗德里格斯变换表明旋转向量到旋转矩阵之间的变换.

B. 连续时间运动学模型

在我们目前的工作中,我们假设所有的三个传感器,激光雷达,相机和IMU之间的时间偏移都是预先校准的.此外,我们假设激光雷达和IMU之间的外参在出厂前已经集成和校准好了,但是相机和IMU的外参需要在线估计.此外,激光雷达通常按顺序扫描点,而激光雷达帧中的点可以在不同的机体位姿下进行测量.如[5]所展示的,该运动可以被IMU的反向传播所补偿,因此假设激光雷达帧中的点被同时测量.这样,我们系统的输入数据序列可以简化为图3.

我们假设IMU,激光雷达和相机传感器与激光雷达和IMU(雷达帧和IMU帧)之间的外参和相机与IMU之间的外参刚性连接在一起.为了方便起见,我们以IMU作为机体框架,从而得到以下运动学模型:

其中G表示在全局坐标系下的一个向量,GRI和GpI是IMU相对与世界坐标系的姿态和位置,Gg是重力加速度,ωm和am是原始陀螺仪和加速度计读数,na和ngIMU测量的白噪声,ba和bg是陀螺仪和加速度计的偏差，它们被建模为由高斯噪声nbg和nba驱动的随机行走。

C. 离散IMU模型

我们以IMU速率离散化连续模型(2).设xi是第i次IMU测量时的状态向量：

通过零阶保持器(即一个采样周期内的IMU测量∆t是常数)，我们得到

D. 传播

在我们的工作中,我们利用流形上迭代误差状态卡尔曼滤波器[24]来估计状态向量xi，其中状态估计ˆxi在的切空间中描述了状态估计误差δˆxi：


一旦接收到新的IMU测量值，将通过将（3）中的过程噪声设置为零来传播状态估计：

相关的估计误差在线性化的误差空间中传播如下(详见[24]):

其精确值在附录A中计算。

（5)和(7）中的两种传播从融合最近的激光雷达/相机测量(如第k次测量，见第4-I节)后的最佳状态和协方差估计开始，并重复到收到下一次激光雷达/相机测量(如第k+1次测量)。时间指数 i 和 k 之间的关系如图3所示。

E. 先验分布

让(5)和(7)中的两个传播停止在第k+1次激光雷达/相机测量(见图4),传播的状态估计和协方差分别为ˆxk+1和Σδˆxk+1.在融合第k+1次测量值之前，它们基本上对状态xk+1施加了一个先验分布，如下所述：

F. 迭代更新的初始化

(8)中的先验分布将与激光雷达或相机的测量值融合,以产生xk+1的最大后验(MAP)估计值(表示为ˇxk+1)。这个最大后验估计被初始化为先验估计,并由问题的非线性化性质进行迭代细化.在每一次迭代中,真实状态和当前估计状态之间的误差被定义为:

这将通过最小化考虑（8）式中先验的后验分布和激光雷达/视觉测量来解决。因此，用（8）式中xk+1表示的先验分布应转换为用δˇxk+1表示的等效先验分布：

详细的计算方法见附录B，（10）本质上对δˇxk+1施加了一个先验分布，如下：

G. LiDAR测量

如果第k+1次测量是激光雷达帧,我们从[5]中的原始三维点中提取平面特征点，并在IV-B节中补偿帧内运动。表示Lk+1为运动补偿后的特征点集，计算每个特征点的残差，其中j为特征点的索引，上标L表示该点在lidar参考系中表示。

以ˇxk+1为xk+1的当前估计值，我们可以将第j帧特征点从激光雷达坐标系转换到全局坐标系。正如之前的LOAM传输途径在[5,9]中所做的那样，我们搜索地图中最近的平面特征点，并使用它们来拟合具有正常uj和平面内点qj的平面，测量残差为：

设nj为点Lpj的测量噪声，通过补偿来自Lpj的噪声，我们可以得到真实的点位置：

这个真点位置和真实状态xk+1一起导致（12）中的零残差，即，

它构成了δˇxk+1的后验分布。在（14）中，xk+1被在（9）和αj中定义的误差δˇxk+1参数化：

Hlj的详细计算方法见附录C.

H. 视觉测量

如果第k+1帧是相机图像，我们从未失帧图像中提取FAST角特征点Ck+1，并使用KLT光流跟踪当前滑动窗口关键帧看到的Ck+1中的特征点(V节)。如果Ck+1中的一个特征点丢失或之前没有被跟踪，我们将三维空间中的新特征点（视觉地标）进行三角定位。

在第k+1帧中，视觉标志与其跟踪特征点之间的重投影误差用于更新当前状态估计ˇxk+1。对于提取的角点Cps，其中s为角点索引，其三维空间对应路标为GPs，Cps的测量残差为：

其中，π（.）为针孔投影模型。

现在，考虑到测量噪声，我们有：

其中GPgts和Cpgts分别是GPs和Cps的真实值。利用这些方法，我们得到了真零残差的一阶泰勒展开式：

Hcs和FPs的详细计算方法见附录D。

I. 误差状态迭代卡尔曼滤波器的更新

结合先验分布（11)、激光雷达测量(14)的后验分布和视觉测量(19）的后验分布，得到了δˇxk+1的最大后验(MAP)估计：

其中}x}2Σ=xΣxT。请注意，激光雷达和照相机的测量值可能不会同时出现（见图4），因此，ml（或mc）可以为零。表明

根据[5]，我们将卡尔曼增益计算为：

然后，我们可以更新状态估计为：

上述过程（第IV-G节至第IV-I节）被迭代至收敛性（即，更新小于给定的阈值）。然后将收敛状态估计用于（1)新激光雷达帧中的项目点附加到世界坐标系，并将它们附加到现有点云图中；（2）如果是关键帧，将当前帧的新视觉路标点三角化；(3）作为下一个周期IV-D节中传播的起点：

5. 因子图优化

如第IV-I节所述，新添加的关键帧中未被跟踪的视觉路标点被三角化，以创建新的视觉路标。由于关键帧位姿估计误差，这种三角测量通常精度较低。为了进一步提高视觉标志、关键帧位姿的质量，并同时校准相机和LiDAR-IMU子系统之间的时间偏移量，我们利用因子图优化来优化图像关键帧滑动窗口内的相机位姿和视觉路标。

我们的因子图优化类似于VINS-Mono[22]，但进一步纳入了由于激光雷达测量而引起的位姿约束，如图5所示。由于IMU预集成造成的约束也包括将激光雷达因子与相机因子连接起来。为了保持后端优化的轻量级，位姿图中的激光雷达位姿是固定的，而激光雷达的原始点测量并不参与位姿图的优化。

6. 实验结果

A. 数据采样设备

我们的手持数据采样设备如图6(a)所示,其中包含电源单元,机载DJI manifold-2c3 计算机平台(配备一个英特尔i7-8550uCPU和8GB内存)，一个全局快门摄像头和一个LiVOX AVIA4激光雷达。相机的视场角是 82.9°X66.5°雷达的视场角是70.4°X77.2°.为了定量评估我们的算法的精度(我们的实验在第VI-E部分，我们在我们的设备上安装了一个差分gps(D-GPS)实时运动学(RTK)系统，如图6(b)所示.

B. 实验1：剧烈运动和传感器故障的鲁棒性评估

在这个实验中,我们评估了我们的算法在剧烈运动场景下的鲁棒性(见图7(a-b)),其中最大角速度高达300°/s(见图8陀螺仪读数).此外,我们还模拟传感器故障通过故意阻塞摄像头(见图7(e))和雷达(见图7(f)).

我们估计的姿态和位置的结果如图8所示。其中我们使用不同颜色的阴影和不同颜色的测试阶段。如图8所示,我们可以看到,我们估计的轨迹即使在严重的旋转和平移中,也可以严格的跟踪实际运动.即使相机和激光雷达没有提供测量值,估计的轨迹也非常平滑没有明显的退化.这结果表明,我们的算法具有足够的鲁棒性,可以忍受侵略运动甚至传感器故障的情况.我们请读者参考XXX上的视频，展示实践中实验的细节。此外,我们还在图9©中一起绘制了Vins-mono和Fast-LIO的估计轨迹,我们可以看到在这个实验中我们的方法整体表现最好.

C. 实验2：具有移动物体的狭窄隧道环境下的鲁棒性评估

在这个实验中,我们挑战了基于相机和基于激光雷达的SLAM范围内最困难的场景之一,一个港铁站和一个香港大学站.这是一个典型的狭窄隧道环境(见图9(a)),最大长度可达190米.此外,有许多移动的行人经常显示在传感器视图中.所有的这些因素使得SLAM极具挑战性:1)长隧道结构显著降低了基于激光雷达的SLAM的几何特征特别是当沿隧道行走或在隧道一端转动激光雷达时;2)高度重复的视觉特征(地板上图案)导致匹配和跟踪视觉特征的错误;3)许多移动的行人可能会导致已经有的一些点和视觉特征的异常值.

尽管存在这些挑战,但是我们的系统是足够鲁棒的应对这些场景.在图9(b)中,我们将点云数据与香港大学地铁站的街道地图对齐,可以发现他们是紧密匹配的.这表明我们的定位具有高鲁棒性和高精度.

D. 实验3：大型室内外城市环境中的高精度地图建设

在这个实验中,我们证明了我们提出的方法足够精确,可以构建三维稠密,高精度,大范围的室内外城市环境地图.我们收集了香港大学主楼的外部和内部的数据.实时重建的三维图如图12所示,其中清晰高质量的点云表明,我们提出的方法具有较高的准确性.值得一提的是,没有任何额外的处理(回环检测),我们的算法仍然可以关闭回环本身(见图12©)穿越876米后,这说明我们的方法漂移极低.最后,我们将我们的地图与卫星地图合并在一起,发现他们紧密匹配(见图12(e)).

E. 实验4：使用D-GPS RTK对精度进行定量评价

在这个实验中,我们通过比较它们估计的轨迹与地面真值轨迹(见图10)从实时差分GPS（D-GPS）运动学(RTK)中获得,定量评估了Vins-Mono (IMU+Camera),Fast-LIO(IMU+LiDAR)以及我们算法的精度.数据的最大角速度达到130°/s(见图10中陀螺仪读数).我们计算了所有可能的长度子序列的平移和旋转误差,这些方法之间的相对位姿误差(RPE)如图11所示.

F. 运行时间分析

实验1- 4的平均时间消耗量如表1所示,从中证明了R2LIVE可以在桌面电脑和嵌入式计算平台上实时实现。注意到因子图优化在单独的线程上运行，因此允许以较低的速率运行。

7. 结论

本文，我们提出了一个鲁棒的，基于高速滤波的里程计和因子图优化的实时激光惯性视觉融合框架。我们在一个误差状态迭代的卡尔曼滤波器融合雷达,惯性和相机传感器的测量值,并使用因子图优化在一个包含图像关键帧和视觉路标点的滑动窗口内细化局部地图.我们的系统在大规模,室内外和狭窄的隧道环境,挑战传感器故障和剧烈运动的情况下测试.在所有的测试中,我们的方法在定位和地图构建方面都达到了高水平的精度和鲁棒性.