激光雷达-惯性-视觉融合算法

1、激光雷达

为什么狭窄空间里雷达会严重退化？

1. 多路径反射现象

狭窄空间中的墙壁、天花板和其他障碍物会导致激光束发生多次反射。激光雷达依赖光束从物体表面反射回接收器来计算距离，但在狭窄空间内，光束可能在不同的表面之间反复反射，导致接收到的信号失真，甚至产生虚假距离。这种多路径效应让雷达难以精确识别物体的位置和形状，影响建图和定位的精度。

2. 视野受限

LiDAR的工作原理是通过发射激光束并收集其反射信息来构建周围环境的点云。狭窄空间限制了激光束的发射角度和范围，导致视野受限，LiDAR难以获取足够的信息来生成精确的环境模型。这种空间受限会导致点云数据稀疏，使得LiDAR在局部环境的特征提取和匹配变得困难。

3. 距离测量误差增大

激光雷达通常设计用于测量较远的距离，但在狭窄空间内，物体与雷达之间的距离可能非常近。当激光雷达的探测距离接近其最小有效测量距离时，测量精度可能会下降。此外，狭窄空间中的复杂几何结构（如弯曲的墙壁或狭窄的缝隙）会导致反射光束的强度变化，从而影响距离测量的准确性。

4. 遮挡和缺少特征

狭窄空间往往具有较多的遮挡物，如柱子、家具、墙壁等。这些遮挡物会阻挡激光束的传播，导致激光雷达无法探测到完整的环境信息。同时，狭窄空间通常缺少足够的独特环境特征（例如广阔空间中明显的物体形状或结构），让激光雷达的点云匹配变得困难，从而影响SLAM的性能。

5. 回波信号强度变化

激光雷达的回波信号强度受到表面材质、反射角度和距离的影响。在狭窄空间内，物体表面的反射特性和几何形状可能导致信号强度的不均匀变化。例如，光滑的墙壁可能导致强反射，而粗糙或不规则的表面则可能吸收或散射激光，使雷达无法获得可靠的测量数据。

6. 动态环境干扰

在狭窄空间中，如果环境中有移动的物体（如人或机械设备），LiDAR的点云可能会被这些动态物体干扰，从而引发错误的定位或建图。由于空间狭小，这种动态干扰会更加明显，影响LiDAR生成稳定的点云数据。

结合视觉、IMU 等传感器信息进行多传感器融合后，可以显著提升 SLAM算法在狭窄空间或复杂环境中的性能。具体的提升主要体现在以下几个方面：

1.提高定位精度和鲁棒性

单一传感器的数据可能存在噪声或误差，特别是在复杂环境中。例如，LiDAR在狭窄空间内容易受到多路径反射和遮挡影响，而视觉传感器则在光照不足或纹理缺乏的情况下性能下降。通过融合多种传感器的数据，可以互补这些局限：

  • 视觉传感器：提供丰富的环境特征信息，特别适合在有足够光照和特征丰富的环境中工作。视觉数据可以帮助LiDAR识别更多细节，尤其是在距离近的情况下。
  • IMU：提供高频率的惯性信息（加速度和角速度），可以弥补LiDAR和相机帧率较低或在某些情况下传感器失效时的短暂空白。当视觉和LiDAR数据短暂丢失时，IMU可以通过惯性信息继续提供设备的运动轨迹，保持系统的连续定位。
  • LiDAR：提供高精度的深度信息，尤其在光照条件差或特征匮乏的环境中（如无纹理的墙壁）表现优越，与视觉数据结合可以大幅提高场景理解能力。

通过融合这些传感器的数据，系统可以对环境的建模和自身的定位更加准确，显著提高SLAM的鲁棒性。

2. 解决遮挡问题

在狭窄空间或遮挡物较多的环境中，单一传感器可能无法捕获足够的环境信息。例如，==LiDAR的激光束可能被墙壁、家具或其他障碍物阻挡，导致建图不完整。视觉传感器则可以通过拍摄不同角度的图像来补充遮挡区域的信息，而IMU可以帮助在移动过程中补偿这些信息。==

  • LiDAR-视觉融合：视觉相机可以提供丰富的图像特征，当LiDAR遇到遮挡时，相机可以捕捉到被遮挡物体的图像特征，减少环境模型中的盲区。
  • IMU补偿遮挡带来的抖动：当传感器被部分遮挡或无法获得可靠数据时，IMU的惯性测量数据可以为位姿估计提供连续的运动信息，避免系统因遮挡而丢失轨迹。

3. 增强姿态估计

IMU 提供的惯性信息能快速响应设备的旋转和加速度变化，尤其在动态环境中（如机器人或无人机快速运动时）有助于及时更新姿态估计。视觉传感器和LiDAR虽然能提供位姿信息，==但它们的采样频率通常较低，而IMU能够提供高频率的姿态数据，从而减少由于运动模糊或快速变化场景导致的位姿估计误差==。

  • 视觉与IMU结合：视觉数据提供的帧间运动变化信息可以校正IMU的漂移，反过来IMU提供的高频运动信息可以弥补视觉传感器的低频率和运动模糊问题。
  • LiDAR与IMU结合：LiDAR的深度信息可以帮助修正IMU在长时间运行中积累的误差，特别是当IMU漂移严重时，LiDAR提供的外部环境信息可以重新校准位姿估计。

4. 提高系统鲁棒性和容错性

多传感器融合提高了系统的冗余性，能够容错并降低单一传感器失效带来的影响。比如：

  • 视觉传感器故障时：LiDAR和IMU可以提供持续的定位信息，即便视觉传感器无法工作，系统仍能继续运行。
  • LiDAR受限时：**==在狭窄或多反射场景中，LiDAR表现退化时，视觉数据和IMU可以接管环境建模与定位任务==**。
  • IMU漂移时：长时间使用IMU会积累误差，通过LiDAR和视觉的定期校准，可以纠正这种漂移，确保长期运行的定位精度。

5. 改善场景理解和环境建图

视觉传感器提供的图像包含了丰富的场景语义信息，例如**==物体的颜色、形状和边缘==，这些信息对于理解场景的结构至关重要。而==LiDAR则提供精准的距离测量信息，特别适合进行三维点云构建==**。将两者结合，系统可以构建更加完整且语义丰富的环境地图。

  • LiDAR-视觉融合建图：LiDAR生成的三维点云与视觉提供的2D图像结合后，可以生成具有深度和纹理信息的高精度地图，提高环境感知的准确性。
  • 语义分割与障碍检测：视觉数据可以用于语义分割，识别出具体的物体类别（如人、车辆、建筑物等），而LiDAR则可以提供精确的障碍物距离。二者结合可用于复杂环境下的高级任务，如自动驾驶中的障碍物检测和避障。

高效且精确的激光雷达-惯性-视觉融合定位及建图系统 FAST-LIVO2

该系统在实时三维重建和退化场景中的机器人机载定位等方面展现了巨大的潜力

中文版解析：港大震撼开源FAST-LIVO2！引领LiDAR-惯性-视觉里程计新时代，速度与精度的巅峰之作！ (qq.com)

  1. 实时高精度重建: 系统能够实时生成照片级**==高精度的稠密彩色点云==。更重要的是，它可以在==基于ARM架构的低算力平台 (如rk3588，Jetson Orin NX，RB5等) 上实时运行==。**

  2. 极端环境下的稳定性: 可以在**==极度退化且无GPS信号的矿洞隧道里 (超过25min采集) 稳定建图且返回原点==**。另外我们还在大量激光雷达/视觉退化数据 (超过2TB) 上进行过测试，验证了其高效和鲁棒性。

  3. 无人机自主导航突破: **==FAST-LIVO2是世界上首个将激光雷达-惯性-视觉里程计 (LIVO) 系统应用于无人机自动导航==**的方案。它使得无人机能够在激光雷达和视觉均退化的场景中保持稳定工作。

  4. ==航空测绘精度提升==: 有效解决了航空测绘中出现的激光雷达退化或点云测量不准导致的累计误差问题 (空对地距离太远，光斑效应明显)，进而得到像素级建图结果。

  5. 3D场景表达的下游应用支持：为下游应用 (如**==Mesh生成以及纹理贴图，深度监督的3D Gaussian Splatting==等) 快速生成稠密且准确的==大规模彩色点云和相机位姿==**。

  6. 实景三维扫描：凭借非接触、高精度、高细节、高效率、大尺度等特性获取古代建筑，地貌景观等三维数据，然后导入UE5建模软件中，使得游戏 (比如黑神话悟空DLC) 中建筑在细节上能与现实世界相媲美。

    ​

​ 摘要：本文提出了 ==FAST-LIVO2==：一种快速、直接的 LiDAR 惯性视觉里程计框架，可在==同步定位和建图== (SLAM) 任务中实现准确且稳健的状态估计，并在实时机载机器人应用中提供巨大潜力。 FAST-LIVO2 通过**==错误状态迭代卡尔曼滤波器 (ESIKF) 有效地融合 IMU、LiDAR 和图像测量==。 为了解决异构激光雷达和图像测量之间的==尺寸不匹配==问题，我们在卡尔曼滤波器中使用==顺序更新策略==。 为了提高效率，我们==对视觉和 LiDAR 融合使用直接方法==**，其中 **==LiDAR 模块在不提取边缘或平面特征的情况下注册原始点==，而==视觉模块在不提取 ORB 或 FAST 角特征的情况下最大限度地减少直接光度误差==。 视觉和 LiDAR 测量的==融合基于单个统一体素图==，其中 LiDAR 模块构建==用于注册新 LiDAR 扫描的几何结构==，视觉模块将图像块附加到 LiDAR 点（即视觉地图点），从而实现==新的图像对齐==。 为了提高图像对齐的准确性，我们使用==体素图中 LiDAR 点的平面先验==（甚至在对齐过程中细化平面先验），并在新的图像对齐后动态更新参考补丁。 此外，为了==增强图像对齐的鲁棒性==，FAST-LIVO2采用==按需光线投射操作，并实时估计图像曝光时间==**。 我们对基准数据集和私有数据集进行了广泛的实验，证明我们提出的系统在准确性、鲁棒性和计算效率方面显着优于其他最先进的里程计系统。 此外，系统中关键模块的有效性也得到了验证。 最后，我们详细介绍了FAST-LIVO2的三个应用：

​ 无人机机载导航展示了系统**==实时机载导航的计算效率==，机载测绘展示了==系统的测绘精度==，==3D模型渲染==**（基于网格和基于NeRF）强调了适用性我们为后续渲染任务重建的密集地图。 我们在 GitHub上开源了这项工作的代码、数据集和应用程序，以使机器人社区受益。

​ 索引术语 — 同步定位与建图 (SLAM)、传感器融合、3D 重建、空中导航。

介绍

​ 近年来，同步定位和绘图（SLAM）技术取得了重大进展，特别是在未知环境中的实时3D重建和定位方面。由于能够实时估计姿态和重建地图，SLAM已经成为各种机器人导航任务中不可或缺的一部分。定位过程为机器人的车载控制器提供关键的状态反馈，而**==密集的3D地图提供关键的环境信息==，如自由空间和障碍物，这对有效的轨迹规划至关重要。==彩色地图还携带大量的语义信息，能够生动地呈现现实世界，从而开辟了巨大的潜在应用，如虚拟和增强现实、3D建模和人机交互==**。

​ 目前，几种SLAM框架已经成功实现了单测量传感器，主要是相机[1]-[4]或激光雷达[5]-[7]。尽管视觉和激光雷达SLAM在各自的领域都显示出前景，但它们各自都有内在的局限性，限制了它们在各种场景中的表现。

视觉SLAM

​ **==Visual SLAM利用具有成本效益的CMOS传感器和镜头，能够建立准确的数据关联，从而实现一定程度的定位精度。==丰富的色彩信息进一步丰富了语义感知。进一步利用这种增强的场景理解，采用深度学习方法进行鲁棒特征提取和动态对象过滤。然而，在==视觉SLAM中缺乏直接的深度测量，因此需要通过三角测量或深度过滤等操作同时优化地图点，这带来了巨大的计算开销，通常会限制地图的精度和密度==。视觉SLAM还遇到许多其他限制，例如不同尺度的测量噪声变化，==对照明变化的敏感性以及无纹理环境对数据关联的影响==**。

激光雷达SLAM

​ 激光雷达SLAM，利用激光雷达传感器，直接获得**==精确的深度测量==，在==定位和测绘任务==中提供相比于视觉SLAM==更高的精度和效率==。尽管有这些优势，激光雷达SLAM也有几个明显的缺点。一方面，它重建的点云图虽然详细，但==缺乏颜色信息，从而降低了信息尺度==。另一方面，在==几何约束不足的环境==中，如==狭窄的隧道、单墙和延伸墙等，LiDAR SLAM的性能往往会下降==**。

​ 随着在现实世界中操作智能机器人的需求的增长，特别是在通常**==缺乏结构或纹理的环境==中，越来越明显的是，依赖单个传感器的现有系统无法提供所需的准确和鲁棒的姿态估计。为了解决这一问题，==激光雷达、摄像头和IMU==等常用传感器的融合正受到越来越多的关注。这种策略不仅结合了这些传感器的优势来提供增强的姿态估计，而且有助于==构建准确、密集和彩色的点云图，即使在单个传感器性能退化的环境中也是如此==**。

​ 高效、准确的LiDAR-inertial-visual odometry （LIVO）和建图仍然是一个具有挑战性的问题：1)整个LIVO系统的任务是**==处理激光雷达测量==，包括每秒数百到数千个点，以及==高速率、高分辨率的图像==。充分利用如此大量的数据，特别是在==有限的机载资源下，这一挑战需要卓越的计算效率==**；

​ 2)许多现有系统通常包含**==雷达惯性测距==LiDAR- inertial Odometry （LIO）子系统和==视觉惯性测距==Visual-Inertial Odometry （VIO）子系统，每个子系统都需要分别==从视觉和LiDAR数据中提取特征以减少计算负荷==**。在缺乏结构或纹理的环境中，这种提取过程通常会导致特征点有限。此外，为了优化特征提取，广泛的工程调整是必不可少的，以适应激光雷达扫描模式和点密度的变化；

​ 3)**==为了减少计算量，实现相机和LiDAR测量的更紧密结合==，需要==统一的地图来同时管理稀疏点和观测到的高分辨率图像测量==。然而，考虑到激光雷达和相机的测量结果不均匀，设计和维护这样的地图尤其具有挑战性；4)为==保证重建的彩色点云的精度，姿态估计需要达到像素级精度==。满足这一标准带来了相当大的挑战：适当的==硬件同步==，严格的==预先校准激光雷达和相机之间的外部参数==，==精确的曝光时间恢复==，以及能够实时达到==像素级精度的融合策略==**。

​ 基于这些问题，我们提出了FAST-LIVO2，这是一种高效的LIVO系统，通过**==顺序更新的误差状态迭代卡尔曼滤波器（ESIKF）将LiDAR，图像和IMU测量紧密集成在一起==。通过==IMU传播的先验信息==，系统状态依次更新，==首先通过LiDAR测量，然后通过图像测量，两者都使用基于单个统一体素图的直接方法==。具体来说，在==激光雷达更新中，系统将原始点注册到地图上==，以构建和更新其几何结构，而在==视觉更新中，系统直接重用LiDAR地图点作为视觉地图点==，而无需从图像中提取、三角测量或优化任何视觉特征。地图中选定的视觉地图点与先前观察到的参考图像块相连，然后投影到当前图像上，通过==最小化直接光度误差（即稀疏图像对齐）来对齐其姿态==。为了提高图像对准的精度，FAST-LIVO2==动态更新参考补丁，并使用从LiDAR点获得的平面先验==。为了提高计算效率，FAST-LIVO2==使用LiDAR点来识别当前图像中可见的视觉地图点，并在没有LiDAR点的情况下进行按要求的体素光线投射==。FAST-LIVO2还可以==实时估计曝光时间，以处理光照变化==**。

FAST-LIVO2是在我们之前的工作中首次提出的FAST-LIVO基础上开发的[8]。与FAST-LIVO相比，新的贡献如下：

​ 我们提出了一种**==有效的ESIKF框架==，采用==顺序更新来解决激光雷达和视觉测量之间的尺寸不匹配==**问题，提高了使用异步更新的FAST-LIVO的鲁棒性。

2)我们使用（甚至改进）激光雷达点的平面先验来提高精度。相比之下，FAST-LIVO假设贴片（补丁）中的所有像素共享相同的深度，这一大胆的假设大大降低了图像对齐中仿射变换的准确性。

3)为了提高图像对齐的精度，我们提出了一种参考补丁更新策略，通过选择高质量的、具有大视差和足够纹理细节的内层参考补丁。FAST-LIVO根据与当前视图的接近程度选择参考补丁，通常会导致低质量的参考补丁降低精度。

4)在线估计曝光时间，处理环境光照变化。FAST-LIVO没有解决这个问题，导致在明显的光照变化下图像对准的收敛性差。

5)我们提出了按需体素射线投射，以增强系统在激光雷达近距离盲区导致的没有激光雷达点测量的情况下的鲁棒性，这是FAST-LIVO中没有考虑的问题。

​ 在综合消融研究中对上述每一项贡献进行了评估，以验证其有效性。我们将提出的系统作为实用的开放软件来实现，精心优化了在英特尔和ARM处理器上的实时运行。该系统是多功能的，支持多线旋转激光雷达，新兴的具有非常规扫描模式的固态激光雷达，以及针孔相机和各种鱼眼相机。

​ 此外，我们在25个公共数据集序列（即Hilti和NTU-VIRAL数据集）以及各种具有代表性的私人数据集上进行了广泛的实验，以便与其他最先进的SLAM系统（例如**==R3LIVE， LVI-SAM， FAST-LIO2==**等）进行比较。定性和定量结果都表明，我们提出的系统在降低计算成本的情况下，在准确性和鲁棒性方面明显优于其他同行。

​ 为了进一步强调我们系统的实际适用性和多功能性，我们部署了三个不同的应用程序。首先，全机载自主无人机导航，展示了系统的实时能力，标志着在**==现实世界的自主无人机飞行中使用激光雷达惯性视觉系统的开创性实例==**。其次，在实际应用中，航空测绘显示了系统在无结构环境下的像素级精度。最后，网格、纹理和NeRF模型的高质量生成强调了系统对渲染任务的适用性。我们将代码和数据集放在GitHub上。

直接的方法

​ 直接方法在视觉和激光雷达SLAM中都是快速姿态估计的重要方法。与基于特征的方法[5,6,9,10]不同，这些方法需要提取图像中的显著特征点（例如角点和边缘像素）；激光雷达扫描中的平面和边缘点)以及生成用于匹配的鲁棒描述符，直接方法通过最小化基于光度误差或点对平面残差的误差函数来直接利用原始测量来优化传感器姿态[11]，例如[3,12]-[14]。**==通过消除耗时的特征提取和匹配，直接方法提供了快速的姿态估计==。==然而，缺乏特征匹配需要相当精确的状态先验估计，以避免局部最小值。==**

​ 直观SLAM中的直接方法大致可分为==密集直接法、半密集直接法和稀疏直接法==。密集直接方法主要用于全深度测量的RGB-D相机，如[15]-[17]所示，采用图像-模型对齐进行姿态估计。相比之下，半密集直接方法[3,18]通过利用具有显著灰度梯度的像素进行估计来实现直接图像对齐。==稀疏直接方法==[2,12]专注于仅通过几个精心选择的原始补丁提供准确的状态估计，因此与密集和半密集直接方法相比，进一步减少了计算负担。

​ 与直接视觉SLAM方法不同，==直接LiDAR SLAM系统[13,14,19,20]不区分密集和稀疏方法==，并且通常在==每次扫描中使用空间下采样或时间下采样的原始点来构建姿态优化的约束==。在我们的工作中，我们利用激光雷达和视觉模块的直接方法的原理。==我们系统的LiDAR模块改编自VoxelMap[14]，视觉模型基于稀疏直接法的一种变体[12]==。

​ 在从[12]中的稀疏直接图像对齐中获得灵感的同时，我们的视觉模块的==不同之处在于重新利用LiDAR点作为视觉地图点，从而减轻了密集的后端计算==（即特征对齐、滑动窗口优化和/或深度滤波）。

雷达视觉SLAM

​ 在激光雷达-视觉惯性SLAM中集成了多个传感器，使系统能够处理各种具有挑战性的环境，特别是当一个传感器出现故障或部分退化时。在此激励下，研究界已经看到了各种激光雷达-视觉惯性SLAM系统的出现。

​ 现有的方法一般可以分为==松耦合和紧耦合两大类==。分类可以从==状态估计水平和原始测量水平==两个角度来确定。在状态估计层面，==关键是一个传感器的估计是否可以作为另一个传感器模型的优化目标==。在原始测量层面，它涉及到来自不同传感器的原始数据是否合并。

​ Zhang等人提出了一种激光雷达-视觉-惯性SLAM系统[21]，该系统在状态估计层面是松耦合的。在该系统中，==VIO子系统仅为LIO子系统的扫描配准提供初始位姿，而没有与扫描配准共同优化==。VIL-SLAM[22]采用了类似的松耦合方法，没有利用LiDAR、相机和IMU测量的联合优化。

​ 一些系统（例如DEMO[23]、LIMO[24]、CamVox[25]、[26]）==使用3D激光雷达点为视觉模块提供深度测量==[1,4,27]。虽然这些系统表现出测量级的紧密耦合，但它们在状态估计中仍然是松耦合的，这主要是由于在状态估计中==缺乏直接来自LiDAR测量的约束==。另一个问题是，由于==分辨率不匹配==，==3D激光雷达点与2D图像特征点和/或线没有一对一的对应关系==。这种不匹配需要在深度关联中进行==插值==，从而引入潜在的错误。为了解决这个问题，DVL-SLAM[28]采用了一种直接的视觉跟踪方法，将LiDAR点直接投影到图像中，以确定相应像素位置的深度。

​ 上述工作==在状态估计层面没有实现紧耦合==。为了追求更高的精度和鲁棒性，近年来出现了许多以紧密耦合的方式共同优化传感器数据的研究。举几个例子，==基于MSCKF[30]框架的LIC-Fusion[29]紧密融合了IMU测量值、稀疏视觉特征以及LiDAR平面和边缘特征。==

​ 随后的LIC-Fusion2.0[31]通过在==滑动窗口内实现平面特征跟踪来增强LiDAR姿态估计==。VILENS[32]==依靠固定滞后平滑，通过统一的因子图对视觉、激光雷达和惯性数据进行联合优化==。==R2LIVE[33]将激光雷达、相机和IMU测量结果紧密融合在一个非流形迭代卡尔曼滤波器中==[34]。对于R2LIVE中的VIO子系统，使用滑动窗口优化对地图中视觉特征的位置进行三角测量。

​ 有几个系统在测量和状态估计级别上都实现了完全的紧密耦合。==LVI-SAM[35]将激光雷达、视觉和惯性传感器融合在一个紧密耦合的平滑和映射框架中，该框架建立在一个因子图之上。VIO子系统执行视觉特征跟踪，并使用激光雷达扫描提取特征深度。==

​ ==R3LIVE[36]通过LIO构建全局地图的几何结构，通过VIO渲染地图纹理==。这两个子系统通过将各自的LiDAR或视觉数据与imu融合来共同估计系统状态。高级版本r3live++[37]可以实时估算曝光时间并提前进行光度校正[38]，使系统能够恢复地图点的辐亮度。与之前提到的大多数lidar -惯性视觉系统（LIO和VIO子系统都依赖基于特征的方法）不同，==R3LIVE系列[36,37]对LIO和VIO子系统都采用了不需要特征提取的直接方法，这使得它们即使在无纹理或无结构的场景中也能捕捉到细微的环境特征==。

​ 我们的系统还使用LiDAR，图像和IMU数据联合估计状态，并在测量级别保持紧密耦合的体素地图。此外，我们的系统使用直接方法，利用原始激光雷达点进行激光雷达扫描配准，并使用原始图像补丁进行视觉跟踪。我们的系统和R3LIVE（或r3live++）之间的关键区别在于，==R3LIVE（和r3live++）在VIO中的单个像素级别上运行==，而我们的系统在==图像补丁（块）级别上运行==。这种差异使我们的制度具有明显的优势。

​ 首先，在鲁棒性方面，我们的方法==使用简化的一步帧到映射稀疏图像对齐来进行姿态估计，减轻了对R3LIVEs中帧到帧光流必须获得的精确初始状态的严重依赖==。因此，我们的系统简化并改进了==R3LIVE中的两阶段帧到帧和帧到映射操作==。其次，从计算的角度来看，==R3LIVE中的VIO主要采用密集的直接方法==，这在计算上是昂贵的，需要大量的残差构建和渲染点。相比之下，==我们的稀疏直接方法提供了更高的计算效率==。最后，我们的系统以原始图像补丁的分辨率利用信息，而R3LIVE则以其点图的分辨率为上限。

​ 我们系统的视觉模块与DVLOAM[39]、SDV-LOAM[40]和LVIO-Fusion[41]最为相似，它们==将带有补丁的LiDAR点投影到新图像中，并通过最小化直接光度误差来跟踪图像==。然而，它们有几个关键的区别，例如视觉和激光雷达使用单独的地图，依赖于视觉模块中斑块扭曲的恒定深度假设，状态估计级别的松耦合，以及图像对齐的帧到帧和帧到关键帧两个阶段。相比之下，我们的系统==在迭代卡尔曼滤波器中紧密集成了帧图图像对准，激光雷达扫描配准和IMU测量==。此外，由于激光雷达和视觉模块都是单一的统一地图，我们的系统可以==直接利用激光雷达点提供的平面先验来加速图像对准==。

系统概述

​ 我们的系统概述如图2所示，其中包含四个部分：==ESIKF==（第IV节），==局部测绘==（第V节），==激光雷达测量模型（第VI节）和视觉测量模型（第VII节）==。

fast-livo2系统框架图

​ Fig. 2: System overview of FAST-LIVO2

左侧部分：传感器输入

  • IMU（惯性测量单元）：频率范围为10~100Hz，用于感知加速度和角速度。IMU数据输入后，经过Forward Propagation（前向传播），然后通过Backward Propagation（后向传播）结合其他数据进行处理。
  • LiDAR（激光雷达）：频率为10~100Hz，用于生成环境的点云。LiDAR数据通过Scan Recombination（扫描重组）处理后，继续更新LiDAR Update模块。
  • Camera（相机）：频率为10~50Hz，摄像头数据进入Sparse Direct Photometric Error Construction（稀疏直接光度误差构建）模块，同时有一个Affine Warp Reference Patch（仿射变形参考块）来辅助生成光度误差。

中央部分：处理流程

  • IMU和LiDAR数据相结合，经过前向传播、扫描重组后，进行Point-to-plane Residual Computation（点到平面残差计算），结合LiDAR更新以改进测量模型。
  • 在Local Mapping（局部建图）中，LiDAR和相机数据共同作用于Visual Update（视觉更新）模块。该模块负责将视觉和LiDAR点生成的地图更新，并通过Visual Map Point Generation and Update（视觉地图点生成和更新）进行局部映射，随后有Reference Patch Update（参考块更新）和Normal Refine（法向量优化）（独立线程中执行）来优化视觉点。
  • 同时，系统通过Raycasting on Demand（按需光线投射）和Visible Voxel Query（可视体素查询）来动态调整和校验视觉地图点。

​ 这张图片展示了系统如何整合IMU、LiDAR和相机的数据来进行精确的环境建图和定位。系统通过不同的模块进行数据处理、残差计算、更新和优化，最终生成局部地图，并通过视觉和LiDAR的协同工作进行动态建图和校验。

右侧部分：Voxel Map（体素地图）

  • 显示的是一个==局部体素地图，用于局部建图==。==体素中包含不同颜色的点，表示参考块、LiDAR点、视觉地图点==等。该体素地图用于进行==地图分块，以便更高效地进行数据管理和可视化查询==。
  • 右下角展示了一个带有点的平面，代表系统如何处理从LiDAR点生成的平面和法向量。

​ ==异步采样的激光雷达点首先通过扫描重组在相机采样时间重新组合成扫描==。然后，我们==通过顺序更新的ESIKF将激光雷达、图像和惯性测量紧密耦合==，其中系统状态依次更新，首先通过激光雷达测量，然后通过图像测量，两者都使用==基于单个统一体素地图==的直接方法（第IV节）。为了在ESIKF更新（第VI节）中构建激光雷达测量模型，我们==计算帧到地图的点到平面残差==。为了建立视觉测量模型（第七节），我们==从地图中提取当前视场内的视觉地图点，利用可见体素查询和按需光线投射；提取后，我们识别并丢弃离群视觉地图点==（例如，被遮挡或表现深度不连续的点）；然后，我们==计算帧到地图图像的光度误差进行视觉更新==。

​ 用于视觉和LiDAR更新的局部地图都是体素图结构（第V节）：LiDAR点构建和更新地图的几何结构，而视觉图像将图像补丁附加到选定的地图点（即视觉地图点）并动态更新参考补丁。更新后的参考图像块会在独立线程中进一步精优化其法向量。

基于惯序更新的误差状态迭代卡尔曼滤波器

符号和状态转移模型

​ 系统假设三个传感器（LiDAR、IMU和相机）之间的时间偏移是已知的，并且可以通过校准或提前同步来获得。将IMU坐标系（记作I）作为机体坐标系，并将第一个机体坐标系作为全局坐标系（记作G）。此外，假设这三个传感器是刚性连接的，且其外参已经预先校准。然后，第i个IMU测量时刻的离散状态转移模型为：

$\mathbf{x}_{i+1}=\mathbf{x}_i \boxplus\left(\Delta t \mathbf{f}\left(\mathbf{x}_i, \mathbf{u}_i, \mathbf{w}_i\right)\right)$

其中∆t为IMU采样周期，定义状态x、输入u、过程噪声w、函数f如下：
$$
\begin{aligned}
& \mathcal{M} \triangleq S O(3) \times \mathbb{R}^{16}, \operatorname{dim}(\mathcal{M})=19 \
& \mathbf{x} \triangleq\left[\begin{array}{lllllll}
{ }^G \mathbf{R}_I^T & { }^G \mathbf{p}_I^T & { }^G \mathbf{v}_I^T & \mathbf{b}_g^T & \mathbf{b}_a^T & { }^G \mathbf{g}^T & \tau
\end{array}\right]^T \in \mathcal{M} \
& \mathbf{u} \triangleq\left[\begin{array}{ll}
\boldsymbol{\omega}_m^T & \mathbf{a}_m^T
\end{array}\right]^T, \quad \mathbf{w} \triangleq\left[\begin{array}{lllll}
\mathbf{n}_g^T & \mathbf{n}a^T & \mathbf{n}{\mathbf{b}g}^T & \mathbf{n}{\mathbf{b}_a}^T & n_\tau
\end{array}\right]^T \
& \mathbf{f}(\mathbf{x}, \mathbf{u}, \mathbf{w})=\left[\begin{array}{c}
\boldsymbol{\omega}_m-\mathbf{b}_g-\mathbf{n}_g \
{ }^G \mathbf{v}_I+\frac{1}{2}\left({ }^G \mathbf{R}_I\left(\mathbf{a}_m-\mathbf{b}_a-\mathbf{n}_a\right)+{ }^G \mathbf{g}\right) \Delta t \
{ }^G \mathbf{R}_I\left(\mathbf{a}_m-\mathbf{b}a-\mathbf{n}a\right)+{ }^G \mathbf{g} \
\mathbf{n}{\mathbf{b}g} \
\mathbf{n}{\mathbf{b}a} \
\mathbf{0}{3 \times 1} \
n_\tau
\end{array}\right]
\end{aligned}
$$
​ 其中，$G R{I 、} G p_I$ 和 $G v_I$分别表示IMU在全局坐标系中的姿态、位置和速度，$G {g}$是全局坐标系中的重力向量，$\tau$是相当于第一个图像帧的逆曝光时间，$n_\tau$是建模为随机游走的高斯噪声，$\omega_m$ 和 $a_m$是IMU的原始测量值，$n_g$ 和 $n_a$ 分别是 $\omega_m$ 和 $a_m$ 中的测量噪声，$b_a$ 和 $b_g$是IMU的偏置，建模为由高斯噪声驱动的随机游走。

LiDAR帧重组

​ 通过重新分帧，将高频、顺序采样的LiDAR原始点云重新组合成与相机采样时刻同步的扫描数据，如图3所示。这确保了相机和LiDAR数据以相同的频率（例如10 Hz）进行同步，从而允许在同一时间点更新状态。

image-20241017101459891

​ Fig. 3: Illustration of scan recombination, forward propagationand backward propagation applied to input data

​ 输入数据的扫描重组、正向传播和反向传播示意图

传播

​ 在ESIKF框架中，状态和协方差从时间 $t_{k-1}$ （接收到上一个LiDAR扫描和图像帧的时刻）传播到时间 $t_k$ （接收到当前LiDAR扫描和图像帧的时刻）。这种前向传播通过设置过程噪声 $w_i$ 为零，来预测每个IMU输入 $u_i$在 $t_{k-1}$ 和 $t_k$ 之间的状态。将传播状态记作 $\hat{x}$ 和协方差记作 $P$ ，它们将作为随后的更新的先验分布。此外，为了补偿运动畸变进行反向传播，确保点在LiDAR扫描结束时刻 $t_k$ 被”测量”。

惯序更新

​ IMU 传播得到的状态 $\hat{\mathbf{x}}$ 和协方差 $\hat{\mathbf{p}}$ 提供了时间点 $t_k$ 系统状态 $x$ 的先验分布:

​ $\mathbf{x}\boxplus\widehat{\mathbf{x}}\sim\mathcal{N}(\mathbf{0},\widehat{\mathbf{P}})$

将上述先验分布记作$\ p(x)$，并将LiDAR和相机的测量模型表示为$\begin{bmatrix}\mathbf{y}_l\\mathbf{y}_c\end{bmatrix}=\begin{bmatrix}\mathbf{h}_l(\mathbf{x},\mathbf{v}_l)\\mathbf{h}_c(\mathbf{x},\mathbf{v}_c)\end{bmatrix}$

$\text{其中,}v_l\sim N(0,\Sigma_{v_l})\text{ 和 }v_c\sim N(0,\Sigma_{v_c})\text{ 分别表示LiDAR和相机的测量噪声。}$

​ 标准的ESIKF将使用所有当前测量数据 (包括LiDAR测量$y_l$和图像测量$y_c$)来更新状态$x$。然而，LiDAR和图像测量是两种不同的感知方式，它们的数据维度不匹配。此外，图像测量的融合可以在图像金字塔的各个层次上进行。为了处理维度不匹配并为每个模块提供更多灵活性，提出了一种惯序更新策略。该策略理论上等同于使用所有测量值的标准更新，前提是给定状态向量$x$的LiDAR测量$y_{l}$和图像测量$y_{c}$​​是统计独立的(即测量值被统计独立的噪声扰动)。

$\text{为了引入惯序更新,将当前状态 }x\text{ 的总条件分布重写为}:$

image-20241017104903980

公式 (5) 表明，总条件分布$p(x|y_l,y_c)$可以通过两次顺序贝叶斯更新得到。第一步仅融合LiDAR测量$y_{l}$与IMU传播的先验分布$p(x)$以获得分布$p(x|y_l):$

​ $p(\mathbf{x}|\mathbf{y}_l)\propto p(\mathbf{y}_l|\mathbf{x})p(\mathbf{x})\quad(6)$

$\text{第二步然后融合相机测量 }y_c\text{ 与 }p(x|y_l)\text{ 以获得最终的状态后验分布}:$

​ $p(\mathbf{x}|\mathbf{y}_l,\mathbf{y}_c)\propto p(\mathbf{y}_c|\mathbf{x})p(\mathbf{x}|\mathbf{y}_l)\quad(7)$

有趣的是，两个融合步骤（6）和（7）遵循相同的形式：

image-20241017112958317