1. 摘要
我们提出了一个通过平滑和建图实现激光-视觉-惯性里程计的紧耦合框架LVI-SAM，其实现了高精度和鲁棒性的实时状态估计和地图构建。LVI-SAM采用因子图，由两个子系统组成：一个视觉惯性系统VIS和一个激光雷达惯性系统LIS。这两个子系统以紧耦合的方式，其中VIS利用LIS的估计进行初始化。利用激光雷达测量方法提取视觉特征的深度信息，提高了VIS的精度。反过来，LIS利用VIS对位姿进行估计，并作为点云配准的初始值。首先由VIS进行闭环检测，再由LIS进一步细化。当两个子系统中的一个发生故障时，LVI-SAM也可以发挥作用，这增加了它在无纹理和无特征环境中的鲁棒性。LVI-SAM在从多个平台收集的数据集上进行了评估。
2. 引言
同时定位和建图（SLAM）是许多移动机器人导航任务所需的一种基础能力。在过去的二十年里，使用单个感知传感器进行实时状态估计和建图已经取得了巨大的成功。基于激光雷达的方法可以长距离捕捉环境的细节。然而，这种方法通常在无结构环境中运行时失败，如长走廊或平坦的空地。虽然基于视觉的方法特别适合于位置识别，并且在纹理丰富的环境中表现良好，但其性能对光照变化、快速运动和初始化都很敏感。因此，基于激光雷达和基于视觉的方法通常都与IMU融合，以提高它们各自的鲁棒性和精度。激光雷达惯性系统可以帮助纠正点云失真，并补充在短时间内缺乏特征。视觉惯性系统可以通过imu测量来恢复尺度和姿态。为了进一步提高系统性能，激光雷达、相机和imu测量结果的融合引起了越来越多的关注。我们的工作与LIO、VIO和LVIO最为密切。
我们注意到，尽管我们知道存在成功的非惯性激光雷达视觉系统，例如[1]，[2]，但本文中并未考虑非惯性系统。视觉惯性里程计（VIO）可以分为两大类：基于滤波器的方法和基于优化的方法。基于滤波器的方法通常使用扩展的卡尔曼滤波器（EKF）来使用相机和IMU的测量值来传播系统状态。基于优化的方法可维护滑动窗口估计器，并将视觉重投影误差以及IMU测量误差降至最低。在我们的工作中，我们只考虑单目相机。在最流行的可公开使用的VIO系统中，MSCKF [3]，ROVIO [4]和OpenVINS [5]是基于滤波器的，而OKVIS [6]和VINS-Mono [7]是基于优化的。尽管OKVIS使用立体相机显示出卓越的性能，但它并未针对单目相机进行优化。 VINS-Mono在滑动窗口设置中执行非线性优化，并通过单目相机实现了最先进的精度[8]。
激光雷达-惯性里程计也可以主要分为两类：松耦合方法和紧耦合方法。LOAM[9]和Lego-LOAM[10]是松耦合系统，因为在优化步骤中不使用IMU测量量。紧耦合系统通常可提高的系统的准确性和鲁棒性，是目前正在进行研究的主要重点[11]。在公开的紧耦合系统中，LIO-mapping[12]采用[7]的优化原理，最小化imu和激光雷达测量的残差。由于LIO-mapping被设计用来优化所有的测量值，因此不能实现实时性能。LIO-SAM[13]通过引入激光雷达关键帧的滑动窗口来限制计算复杂度，利用因子图进行联合imu和激光雷达约束优化。针对地面车辆，LINS[14]使用误差状态卡尔曼滤波器来递归地纠正机器人的状态。
近年来，激光雷达-视觉-惯性系统在传感器退化任务[15]中的鲁棒性引起了越来越多的关注。[16]提出了一种以紧耦合为特征的LVIO系统，解决了从粗到细的状态估计问题。粗估计从IMU预测开始，然后由vio和lio进一步细化。[16]目前在KITTI测试[17]上达到了最先进的精度。基于MSCKF的框架，[18]具有多传感器在线时空校准功能。但[16]和[18]的实现不可公开。我们的工作不同于上述的工作，因为我们利用因子图进行全局优化，它可以通过闭合检测定期消除机器人产生的漂移。
在本文中，我们提出了一个通过平滑和建图实现激光-视觉-惯性里程计的紧耦合框架LVI-SAM，用于实时状态估计和建图。其采用因子图的形式，由两个子系统，一个视觉惯性系统和一个激光雷达惯性系统组成。当两个子系统检测到故障时或者当检测到足够的特性时，可以独立工作。VIS进行视觉特征跟踪，并可选择使用激光雷达帧提取特征深度信息。通过优化视觉重投影的误差和imu测量，视觉里程计可作为激光雷达扫描匹配的初始值，并在因子图中引入约束条件。在使用imu测量值进行点云纠正后，LIS提取激光雷达的边缘和平面特征，并将其与在滑动窗口中维护的特征图相匹配。在lis中估计的系统状态可以发送到vis，以方便其初始化。对于闭环检测，候选匹配首先由vis识别，并由lis进一步优化。在因子图中，联合优化了视觉里程计、激光雷达里程计、IMU预积分和闭环的约束条件。最后，整个系统以IMU的频率输出位姿估计。
主要贡献：
- 实现了一个激光-视觉-惯性的紧耦合系统，通过因子图同时实现多传感器融合、全局优化和回环检测。
- 通过故障检测机制，避免单一子系统故障导致系统不能使用的情况，提高了整个系统的鲁棒性。
- 框架经过了不同尺度、平台和环境下采集的数据的充分验证。
从系统的角度来看，我们的工作是新颖的，代表了vio和lio中最先进技术的独特集成，以实现lvio系统，提供改进的鲁棒性和准确性。我们希望我们的系统能够作为一个坚实的基线，其他人可以很容易地建立，以推进最先进的激光雷达视觉惯性里程计。
3. 带有平滑和建图的激光视觉惯性里程计
3.1. 综述
所提出的激光雷达-视觉-惯性系统的概述，它接收来自三维激光雷达，单目相机和IMU的数据。我们的框架由两个关键的子系统组成：一个视觉惯性系统和一个激光雷达惯性系统。VIS处理图像和imu测量，激光雷达测量是可选的。视觉里程计是通过最小化视觉测量和imu测量残差来得到的。LIS提取激光雷达的特征，并通过将提取的特征与特征图相匹配来进行激光雷达里程计。特征图以滑动窗口的方式进行维护，以实现实时性能。最后，通过利用isam2[19]联合优化因子图中预积分约束、视觉里程计约束、激光雷达里程计约束和闭环约束，并采用最大后验问题（MAP）进行状态估计。请注意，LIS中采用的多传感器图优化是为了降低数据交换，提高系统效率。
3.2. 视觉-惯性系统
我们调整了来自[7]的处理原理，如图2所示。
使用corner detect[20]检测视觉特征，并通过KLT算法[21]进行跟踪。在VIS初始化时，我们利用视觉里程计配准激光雷达帧，得到了一个用于特征深度估计的稀疏深度图像。系统在滑动窗口设置中执行BA，其中系统状态x属于X可以写为：
X=[R, p, v, b]
R属于SO(3)是旋转矩阵，p是位置矩阵，v是速度，b = [ba,bw]是IMU的bias。变换矩阵T=[R|p]定义为载体坐标系B到世界坐标系W的变换。在下面的几个部分中，我们给出了改进于初始化和特征深度估计的详细程序。这部分主要参考的是VINS-MONO。
3.2.1. 初始化
基于优化的vio通常由于解决初始化时的高度非线性问题而存在分歧。初始化的质量在很大程度上取决于两个因素：初始传感器的运动和imu参数的精度。在实践中，我们发现当传感器以小或恒定的速度移动时，[8]通常无法初始化。这是由于当加速度激励不够大时，尺度无法获得。imu参数包括缓慢变化的bias和白噪声，这影响了原始加速度和角速度测量。在初始化时对这些参数进行很好的预测有助于优化更快地收敛。
为了提高我们对初始化的鲁棒性，我们利用了从LIS估计的系统状态x和imu的bias b。由于深度可以直接从激光雷达上观察到，所以我们首先初始化LIS并得到x和b，然后根据图像时间戳插值并将它们与每个图像关键帧相关联。注意，假设关键帧在两个图像之间的imu bias是恒定的。最后，利用从LIS估计的x和b作为对初始化的初始猜测，显著提高了初始化速度和鲁棒性。在没有LIS的帮助下，与初始化的比较可以在补充视频（https://youtu.be/8CTl07D6Ibc）中找到。
3.2.2. 特征深度关联
在初始化相对值时，我们使用估计的视觉里程计将激光雷达帧配准到相机帧中。由于三维激光雷达通常会产生稀疏的扫描，我们堆叠了多个激光雷达帧，以获得一个密集的深度图。为了将一个特征与一个深度值联系起来，我们首先在一个以相机中心为球心的单位球体上投影视觉特征和激光雷达深度点。深度点被以恒定密度向下采样，并用极坐标存储。我们利用视觉特征的极坐标搜索二维K-D树，找到了球体上最近的三个深度点（激光点）。最后，特征深度是由视觉特征和照相机中心
形成的线的长度，它与笛卡尔空间中由三个深度点形成的平面相交。该过程如图3(a)中，其中特征深度是虚线直线的长度。
我们通过检查三个最近的深度点之间的距离，从而进一步验证了相关的特征深度。这是因为从不同的时间戳中叠加激光雷达帧可能会导致不同对象的深度模糊。这个例子的说明如图3(b)所示。在时间ti时观察到的深度点用绿色表示。相机将移动到tj的新位置，并观察到新的灰色深点。然而，由于激光雷达帧的叠加，上的深度点被一条虚灰线环绕，仍然可以在上观测到。使用来自不同对象的深度点来关联特征深度会导致不准确的估计。与[16]类似，我们通过检查一个特征的深度点之间的最大距离来拒绝这种估计。如果最大距离大于2m，则该特征没有相关的深度。
3.2.3. 故障检测
相对设备由于剧烈运动、光照变化和无纹理环境而出现故障。当机器人经历攻击性运动或进入无纹理的环境时，被跟踪特征的数量会大大减少。特征不足可能会导致优化散度。我们还注意到，当VIS失败时，大的imu bias将会被估计。因此，当跟踪的特征数量低于阈值或估计的IMU偏差超过阈值时，我们将报告VIS失败。主动故障检测是我们系统的必要条件，以便其故障不会损坏LIS的功能。一旦检测到故障，VIS将重新初始化并通知LIS。
3.2.4. 闭环检测
我们利用DBoW2[22]进行环闭合检测。对于每个新的图像关键帧，我们提取BRIEF的描述符[23]，并将它们与之前提取的描述符进行匹配。DBoW2返回的候选回环检测的图像时间戳被发送到LIS以进行进一步验证。
3.3. 激光-惯性系统
如图5所示，所提出的基于[13]的激光雷达惯性系统，保持了一个全局姿态优化的因子图。图中加入了四种约束，预积分约束、视觉里程计约束、激光雷达里程计约束和闭环约束，并联合优化。
激光雷达里程计约束是从扫描匹配推导出来的，其中我们将当前的激光雷达关键帧匹配到一个全局特征图。首先由vis提供环闭环约束的候选条件，然后通过扫描匹配进行进一步优化。我们为特征图维护了一个激光雷达关键帧的滑动窗口，它保证了有界的计算复杂度。当机器人姿势的改变超过了一个阈值时，就会选择一个新的激光雷达关键帧。位于成对的关键帧之间的间歇性激光雷达帧被丢弃。在选择一个新的激光雷达关键帧后，将一个新的机器人状态x作为节点添加到因子图中。以这种方式添加关键帧不仅可以实现内存消耗和地图密度之间的平衡，而且还有助于维护一个相对稀疏的因子图以进行实时优化。由于空间限制，我们将向读者参考[13]以了解实施细节。在下面几部分中，我们着重介绍提高系统鲁棒性的新程序。
3.3.1. initial guess
我们发现initial guess对扫描匹配的成功起着关键的作用，特别是当传感器经历攻击性运动时。在LIS初始化前后initial guess的来源是不同的。在LIS初始化之前，我们假设机器人从一个以零速度开始的静态位置开始。然后假设bias和噪声为零值，积分原始的测量值。两个激光雷达关键帧之间的整合平移和旋转变化产生了扫描匹配的initial guess。我们发现，当初始线速度小于10m/s，角速度小于180°时，该方法可以在具有挑战性的条件下成功地初始化系统。一旦初始化，我们估计因子图中的imu偏差、机器人姿态和速度。然后我们把它们发送到vis来帮助它的初始化。
在LIS初始化之后，我们可以从两个来源获得initial guess：具有修正偏差的积分imu测量和VIS。我们使用视觉惯性里程计作为它可用时的initial guess。如果VIS报告失败，那么我们将切换到imu测量值来进行初始猜测。这些程序提高了在丰富纹理和无纹理的环境中的初始猜测精度和鲁棒性。
3.3.2. 故障检测
虽然激光雷达可以远程捕获环境的细节，但它仍然会遇到扫描匹配限制不足的降级场景。我们采用[24]的方法进行激光故障检测。扫描匹配中的非线性优化问题可以表示迭代求解线性问题：
其中a和b是由T时的线性化得到的。当 的最小特征值小于第一次优化迭代时的阈值时，LIS报告失败。当发生故障时，激光雷达测程约束不会添加到因子图中。我们向读者参考[24]进行这些假设的详细分析。
4. 实验结果
参考文献
SLAM经典文献之：LVI-SAM（激光-IMU-相机紧耦合图优化） – 知乎
【泡泡一分钟】LVI-SAM: 通过平滑和建图实现的紧耦合激光-视觉-惯性里程计
LVI-SAM: Tightly-coupled Lidar-Visual-Inertial Odometry via Smoothing and Mapping