基于摄像头、激光雷达和 IMU 的多传感器融合 SLAM

近年来，同步定位与地图构建 (SLAM) 技术已在自动驾驶、智能机器人、增强现实 (AR) 和虚拟现实 (VR) 等广泛应用中盛行。使用最流行的三种传感器（例如视觉传感器、LiDAR 传感器和 IMU）的多传感器融合在 SLAM 中变得无处不在，部分原因是互补的感知能力以及独立传感器在具有挑战性的环境中不可避免的不足（例如，低精度和长期漂移）。

同步定位与地图构建 (SLAM) 是一种估计机器人状态（例如位置、方向、速度、传感器偏差和校准参数）的技术，同时使用机器人上的传感器感知的数据构建机器人移动环境的模型。

虽然全球导航卫星系统 (GNSS) 可以提供绝 对位置，但在隧道、洞穴、城市峡谷等环境中并不总是可用或准确的。低成本轻量级的 IMU 已被广泛使用，但其测量结果受到噪声和偏差的影响，因此无法为长期导航提供可靠的位姿估计。单目相机存在尺度漂移问题，而 LiDAR 在无结构环境中会失效。因此，通过多传感器融合，可以弥补独立传感器的不足，并提供更可靠的估计。

3.1 视觉-惯性融合算法

在导航系统中，我们希望估计传感平台的六个自由度 (DOF) 姿态（方向和位置）。IMU 因其体积小、重量轻、成本低，以及最重要的是能够以高频测量其刚性连接的传感平台的三轴角速度和线性加速度而广泛应用于导航系统。然而，仅使用 IMU 的导航系统会因 IMU 测量与偏差和噪声的结合而遭受无限误差，无法为长期导航提供可靠的姿态估计。需要额外的传感器来克服这个问题。小型轻便的单目摄像头可提供良好的跟踪和丰富的地图信息，这些信息有关传感平台周围的环境，可以作为 IMU 的理想补充传感器之一。IMU 和摄像头的融合产生了视觉惯性导航系统 (VINS)，在过去二十年中引起了广泛关注。通常，VINS 算法可以根据数据融合的类型分为基于优化的方法和基于过滤的方法。

3.2 激光雷达-惯性融合算法

近年来，人们越来越关注 LiDAR-惯性融合算法，因为 IMU 以高频率测量瞬时运动，可用于从高度动态的运动失真中恢复点云并预测两个 LiDAR 帧之间的相对姿态。根据传感器融合类型，LiDAR-惯性融合算法可分为松耦合方法或紧耦合方法。松耦合方法对运行时间有吸引力，分别考虑 LiDAR 的估计和 IMU 的估计，导致信息丢失和估计不准确。而紧耦合方法旨在实现准确估计，在基于优化或基于过滤的框架中融合点云和 IMU 测量值，计算成本更高。

3.2.1 松耦合方法

LOAM是一种经典的 3D LiDAR SLAM 方法，其结构由三个主要模块组成，即特征提取、里程计和地图构建。现有工作通常沿用该结构。在 LOAM 中，从当前扫描的增长点云中提取的边缘点和平面点用于在上一次扫描中查找对应关系，以更新上一次递归中的位姿变换。假设在一次扫描期间角速度和线速度恒定，可以通过对上一次递归中的位姿变换进行线性插值来计算一次扫描中不同时间的位姿变换。然而，当速度变化较快时，LOAM 的精度较低，而 IMU 可以缓解这一问题。集成 IMU 测量值可提供一次扫描期间不同时间的位姿，可以有效补偿运动失真，从而大大提高精度和鲁棒性。

LION是一种松耦合的 LiDAR 惯性里程计算法，它与 LOAM 共享类似的里程计，无需特征提取和映射，从而降低计算成本。条件数在 LION 中用作可观测性度量，以确定是否需要使用其他更可靠的里程计源。为了更好地利用历史帧信息，紧耦合的 LiDAR-惯性方法通常采用扫描到局部地图配准，其中局部地图由少量最近的 LiDAR 帧组成。

3.2.2 紧耦合方法

LIOM为受视觉惯性启发的紧耦合 LiDAR 惯性融合方法提供了第 一个开源实现。与 LOAM 一样，LiDAR 运动假设成立，通过对 IMU 传播预测的 LiDAR 运动进行线性插值，可以校正扫描过程中每个点的位置。LIOM 维护一个由当前 LiDAR 扫描和最近扫描组成的滑动窗口，其中枢轴 LiDAR 扫描的帧用作局部帧，窗口中的所有扫描都转换到局部帧以获得局部地图。LIOM 仅在滑动窗口中通过包含边缘化先验项的成本函数以及相对 LiDAR 约束和 IMU 约束的残差来优化枢轴 LiDAR 位姿和后续位姿，而不是当前位姿。在局部窗口内进行非线性优化之前，通过 VINS-mono 中的方法初始化 IMU 状态在初始化步骤中使用 LOAM 提供的 IMU 测量值和 LiDAR 姿态。为了提高运行时效率，LINS引入了迭代 ESKF 和机器人中心公式。在 LIO-SAM 中，LiDAR-惯性里程表是在因子图上制定的，特别适合多传感器融合。

固态 LiDAR 通常具有非重复和不规则的扫描模式，且 FoV 较小，常见的特征提取方法不太适合。为了解决这个问题，LILI-OM提出了第 一个紧耦合固态 LiDAR-惯性融合算法，通过在时间域中对小点块分割进行特征分解，开发了一种新的特征提取方法。此外，在滑动窗口优化中使用关键帧方案来确保实时性能，因为利用所有传感器读数非常耗时。LILI-OM 采用与 LOAM 类似的方法来补偿运动失真，而在 FAST-LIO2，进行反向传播过程。FAST-LIO2 不提取任何特征，而是直接将原始点注册到增量 kd 树数据结构维护的地图中。kd 树建议支持树上的下采样，确保地图的稀疏性和快速 k-最近搜索。Faster-LIO基本上是在FAST-LIO2的基础上发展而来的，提出了一种基于稀疏和增量体素的LiDAR-惯性里程计，用于快速跟踪。

视觉-激光雷达融合算法

视觉传感器（例如单目摄像机）通常价格低廉，并且通过提取视觉特征可以实现回环检测。然而，基于视觉的导航系统对光照变化和纹理缺陷很敏感。激光雷达作为一种主动传感器，在变化的环境中表现出更好的精度和鲁棒性，但在长廊等无结构场景中，即使存在丰富的纹理信息，也会受到影响。由于这两类传感器的优势互补，已提出了一些工作，可分为两类：松耦合方法和紧耦合方法。一些工作侧重于前端集成，而另一些工作则注重后端优化，下面将对它们进行详细讨论。

3.3.1 松耦合方法

张等人在 DEMO 中利用 LiDAR 深度信息增强视觉里程计。他们利用相机的估计姿势来注册深度图，其中添加了来自相机前方点云的新点。地图点被转换为球面坐标系，并基于两个角度坐标存储在 2D kd 树中。然后，对于每个特征，可以通过将其投影到由 kd 树中特征的三个最近点形成的平面面片上来获得深度。该方法没有充分利用 LiDAR 信息。此外，没有提到 LiDAR 点云的不失真性。该方法未考虑的回环检测在后面的参考文献 [ 中进行了讨论。通过应用 ORB 特征和词袋。Shin 等人。采用了和 DEMO 类似的策略，利用 LiDAR 的深度信息来增强视觉 SLAM。他们没有像 DEMO 那样从图像中提取特征，而是在 DSO 内部解决问题[75]框架通过将 LiDAR 点投影到图像上作为特征。然后执行与 DSO 相同的多帧光度优化来估计关键帧的姿势。Yan 等人。简单地结合了最 先进的视觉里程计和 LiDAR 里程计以松散耦合的方式，仅当视觉里程计失败时才使用 LiDAR 里程计。

为了应对具有挑战性的环境，人们已经利用了学习方法。LIMO利用深度学习的力量去除动态物体的特征。LIV-LAM提出了一种用于对象发现的无监督学习方法，并使用检测到的对象特征作为界标特征。

3.3.2 紧耦合方法

张和辛格基于他们之前的研究成果 DEMO 和 LOAM 提出了 V-LOAM，而无需借助 IMU 测量来补偿快速运动。在 V-LOAM 中，摄像机的频率远高于 LiDAR，因此可以使用具有可观测尺度的增强型视觉里程计来去畸变 LiDAR 点云。此外，用扫描内的线性运动对视觉里程计的漂移进行建模可以提高去畸变程序的性能。然后将未失真的点云匹配并注册到当前构建的地图以细化估计的位姿。然而，消除畸变主要依赖于视觉里程计的结果，这使其容易受到无纹理或动态环境的影响，而视觉里程计可能会在这些环境中失败。此外，由于其帧间运动估计。

为了提高姿态估计的准确性和鲁棒性，最近的研究利用了其他环境结构特征，如线特征和平面特征。黄等人介绍了一种新的视觉激光雷达里程计方法，利用线段检测器检测到的点和线特征并由线带描述符.黄等人提出了一种基于网格的方法，从点云中明确检测场景平面，以在光度项中包含尽可能多的像素信息。为了减少遮挡点的恶化，他们利用一种新方法来预测哪些 LiDAR 点会在视点变化期间被遮挡。Seo 和 Chou尝试以一种新颖的方式充分利用视觉和 LiDAR 测量数据，以避免将 LiDAR 的深度分配给不对应的视觉特征的潜在问题。他们分别维护视觉和 LiDAR 测量数据，并构建两个不同的地图，即 LiDAR 体素图和视觉图，在求解残差进行姿态估计时一起使用它们。Wang 等人。提出了一种直接视觉LiDAR融合SLAM框架，类似于DVL-SLAM 。为了在各种复杂环境中获得更好的鲁棒性，他们的框架采用了帧到帧的跟踪策略、基于LiDAR的扫描到地图匹配方法以及并行全局和局部搜索环路闭合检测（PGLS-LCD）模块。

相机-激光雷达外部校准通常在现有工作中被忽略，但在最近的工作中得到了考虑。TVL-SLAM [二十九]是一种紧耦合的视觉-激光雷达融合算法，其中视觉和激光雷达测量在前端独立使用，而不是通过彼此增强，而所有测量都以紧耦合的方式纳入后端优化中。假设激光雷达点云和立体图像对是在同一时间戳获取的，并且在全局光束调整之前已知并固定相机-激光雷达外部参数，从而可以通过求解一束优化来使用所有视觉和激光雷达残差来细化位姿。当检测到视觉或激光雷达环路时，在全局光束调整中估计相机-激光雷达外部参数，并将每个视觉地图点与最近的激光雷达体素匹配以创建约束，确保良好的收敛。还讨论了在走走停停场景中的运动物体移除，但只考虑了视觉特征。孟等人。[三十]还在类似 TVL-SLAM 的统一框架中联合优化了视觉和 LiDAR 测量，只不过视觉特征通过 LiDAR 深度信息得到了增强。

3.4 LiDAR-视觉-惯性融合算法

仅使用 LiDAR 的方法容易受到几何形状退化环境的影响，例如长隧道或宽阔的空间。IMU 测量可以很好地补充仅使用 LiDAR 的方法，但是它们只能在几秒钟内提供可靠的姿态估计。因此，LiDAR-惯性方法也容易受到退化情况的影响，尤其是对于视场较小的固态 LiDAR。为了解决这些问题，与其他传感器（尤其是提供丰富视觉信息的相机）融合是必要的，并且越来越受到关注。为了保持一致性，我们还将 LiDAR-视觉-惯性方法分为上述两类。

3.4.1 松耦合方法

邵等人提出了一种 VIL-SLAM，它使用立体摄像机作为视觉传感器，以在某些退化情况下实现更好的性能，例如穿越隧道，而纯 LiDAR 系统通常会在这种情况下失效。通过在紧密耦合的固定滞后平滑中融合立体匹配和 IMU 测量，立体 VIO 输出 IMU 速率和摄像机速率 VIO 姿势，用于消除运动失真并在 LiDAR 测绘中执行扫描到地图的配准。他们使用纯视觉信息来检测回环并构建初始回环约束估计，并通过 LiDAR 测量进一步完善。Wang 等人也提出了类似的工作。并额外考虑了模块故障。Camurri 等人提出了一种用于在现实场景中运行的腿式机器人的松耦合框架，Khattak 等人提出了一种用于地下环境中空中机器人姿态估计的互补多模态传感器融合方法。

3.4.2 紧耦合方法

张和辛格提出了一种顺序、多层处理流程，其中首先通过 IMU 测量预测运动，然后通过视觉惯性里程计估计运动，最后通过扫描到地图配准进行细化。为了补偿不同步传感器之间可能出现的校准差异，Zuo 等人。在 MSCKF 框架内提出了一种轻量级处理流程，称为 LIC-Fusion。为了高效、稳健地处理 LiDAR 测量数据，他们还在 LIC-Fusion 中引入了一种新颖的平面特征跟踪算法，并提出了 LIC-Fusion 2.0，其中在消除 IMU 测量的失真后，从 LiDAR 点中提取平面点，并通过考虑 LiDAR 扫描变换的不确定性，采用异常值拒绝标准在滑动窗口内进行跟踪，以实现更高质量的数据关联。

松耦合方法具有简单性、可扩展性和低计算需求的特点，而紧耦合方法在准确性和鲁棒性方面表现出更好的性能。为了结合松耦合方法和紧耦合方法的优点，提出了超级里程计采用以 IMU 为中心的数据处理流程，由三部分组成：IMU 里程计、视觉惯性里程计和 LiDAR 惯性里程计。IMU 偏差受视觉惯性里程计和 LiDAR 惯性里程计提供的位姿先验约束，后者接收来自 IMU 里程计的运动预测。此外，还应用了动态八叉树以确保实时高性能。其设计背后的关键见解是，如果偏差漂移受到其他传感器的良好约束，则 IMU 里程计的估计会非常准确，因为 IMU 产生的测量结果平滑，噪声小，但异常值少。

通过整合 VINS-mono 和 LIO-SAM，Shan 等人提出了一个公开可用的系统 LVI-SAM，该系统建立在因子图之上，由两个子系统组成，即视觉惯性系统 (VIS) 和激光雷达惯性系统 (LIS)。与超级里程计不同，在 LVI-SAM 中，可以使用深度关联方法从激光雷达扫描中选择性地提取特征深度，然后首先由 VIS 识别用于回环的候选匹配，然后由 LIS 进一步优化。因子图用于联合优化来自 VIS、LIS、IMU 预积分和回环的所有约束。

为了实现实时性能，Lin 等人提出了一种误差状态迭代卡尔曼滤波器框架，其中 LiDAR 点到平面残差、图像重新投影误差和 IMU 传播紧密融合。对于每个摄像机输入图像，使用视觉地标图检测和跟踪快速角点以计算重新投影误差。此外，利用因子图优化进一步提高局部滑动窗口内视觉测量的准确性。郑等人没有从 LiDAR 点云和图像中提取特征，而是提出了 FAST-LIVO ，由两个直接里程计子系统组成：一个直接改编自 FAST-LIO2 的 LIO 子系统以及与 Ref. [ 类似的 VIO 子系统。LIO 构建的地图点会附加图像块，然后用于通过最小化直接光度误差在 VIO 中对齐新图像，从而节省后端时间。R3LIVE 采用了类似的框架并附加了 Perspective-n-Point (PnP) 投影误差。以上三种方法均未启用环路闭合，并且 LiDAR 传感器为固态 LiDAR。

4 挑战与未来研究方向

尽管近年来已经提出了许多具有不同框架的多传感器融合算法，但仍然存在一些挑战，例如传感器到传感器的校准、有效的数据关联、良好的初始化和动态环境。

在相机到 IMU 校准方面，早期的方法依赖于人工标记或准确的初始化。为了解决这些问题，杨和沈提出了一种能够即时获得精 确的相机-IMU 外部校准的方法。然而，他们的方法假设可以跟踪足够的特征。对于相机-LiDAR 校准，Geiger 等人。提出了一种通过检测和匹配摄像头和 LiDAR 视场中的特殊标记板，使用单次拍摄的自动方法。在参考文献中介绍了一种通过最 大化传感器测量的表面强度之间的相互信息来进行无标记校准的方法。为了确保良好的收敛，Chou 和 Chou 通过在视觉地图点和 LiDAR 体素图之间进行配准，添加纯几何约束，使外部变量成为全局光束调整中的可调变量，但该方法需要良好的初始值，而纯视觉方法可能无法为视觉地图点提供准确的比例。

更多数据可以带来更高的准确率，但通常需要更多的计算资源。如图所示，超级里程计由三部分组成：IMU 里程计、视觉惯性里程计和 LiDAR 惯性里程计，这意味着在资源有限的平台上无法保证实时性。融合里程计的结果可能比直接关联传感器数据花费更多的时间。LVI-SAM利用精 确的 LiDAR 深度信息极大地促进了视觉惯性里程计初始化和 R3LIVE 直接利用 LiDAR 点云对图像进行特征跟踪，而无需特征提取和三角测量，这显著加速了视觉惯性里程计。然而，当使用机械 LiDAR 时，这种方法可能会失败。在 LiDAR-Visual-Inertial 融合算法中，更通用和高效的数据关联仍然具有挑战性。

由于视觉惯性方法的非线性，不良的初始化会对其性能产生巨大影响。通过利用短期 IMU 预积分中的相对旋转，参考文献提出了一种不考虑陀螺仪偏差的线性估计器初始化方法，当视觉特征远离传感器套件时，导致初始化不可靠。在参考文献中推导出了视觉惯性运动结构 (SfM) 问题的闭式解。 并在参考文献 [ 中进行了改进 通过对陀螺仪偏差进行建模。建立在 ORB-SLAM 之上，参考介绍了一种 IMU 初始化方法，该方法需要几秒钟才能完成尺度收敛。为了实现快速而稳健的初始化，Qin 和 Shen 对齐度量 IMU 预积分与仅视觉的 SfM 结果以获取初始值。Cheng 等人不使用 SfM，而是使用使用 ORB-SLAM 实现更快的收敛。此外，新方法最近出现，以实现更快、更准确的初始化。

大多数现有的融合算法都假设环境是静态的；然而，在现实世界中情况并非总是如此。例如，行走的人和移动的车辆是现实世界中常见的动态物体。来自动态物体的点云会降低扫描到地图配准或扫描到扫描配准的准确性，导致错误的相对姿态估计。与机械激光雷达相比，相机的视场要小得多，这使其更容易受到运动物体的影响。假设运动物体上的跟踪特征没有被正确拒绝。在这种情况下，运动估计器将计算出错误的运动，这会进一步恶化局部或全局优化并导致系统失败。

根据所审查的文献和上述挑战，我们提出了一些未来的研究方向：

•多功能、高效的融合框架：目前最 先进的算法通常是为特定平台设计的，很难部署到具有类似传感器的其他平台上。传感器之间的自动校准至关重要，应保证准确的初始化，尤其是对于配备视觉传感器的平台。此外，应利用高效的数据关联来确保实时性能。

•深度学习辅助方法：利用深度学习是多传感器融合框架中一个不断发展的领域，可用于特征提取、运动物体检测、环境呈现等。

•分布式协作方法：不同的机器人配备不同的传感器来执行相同的SLAM任务将大大减轻单个机器人的负担，但这是一个相当具有挑战性的问题而且关于它的文献很少。

5 结论

多传感器融合技术近年来在机器人领域受到越来越多的关注。本研究简要介绍了著名的状态估计形成，并总结了过去十年来的多传感器融合方法。我们首先根据传感器的组合将多传感器融合算法分为四类，然后根据数据融合对其进行分类。介绍了每种方法最 具代表性的技术。此外，还讨论了挑战和未来的研究方向，以使该技术通用、稳健和实用。