具有不确定性意识的激光雷达-惯性-视觉SLAM算法：结合自适应融合技术和多通道几何闭环机制 钟启学、 邢婧、 刘健、 罗录青

《Robotics》：Uncertainty-Aware LiDAR–Inertial–Visual SLAM with Adaptive Fusion and Multi-Channel Geometric Loop Closure Qixue Zhong, Jing Xing, Jian Liu and Luqing Luo

　　摘要：在复杂和动态环境中实现准确且鲁棒的定位与地图构建一直是自主系统面临的基本挑战。LiDAR–惯性–视觉里程计（LIVO）结合了LiDAR几何信息、视觉外观和惯性运动约束的互补优势。然而，现有的LIVO系统在应对传感器退化、环路闭合的鲁棒性以及跨模态一致性建模方面仍存在不足。本文提出了一种鲁棒的多传感器SLAM框架，该框架包含一个考虑不确定性的LIVO前端、一个基于几何的环路闭合模块以及一个跨模态一致性因子图后端。我们开发了一种考虑不确定性的迭代误差状态卡尔曼滤波器（iESKF），用于紧密融合LiDAR、视觉和惯性测量数据，并根据创新统计、特征匹配质量和可观测性动态调整测量协方差。为了提高全局一致性，我们提出了一个多通道二进制三角形约束（mBTC）描述符，用于基于LiDAR的环路检测，这增强了在不同视角变化和外观退化情况下的鲁棒性。此外，我们引入了一个跨模态一致性因子，以明确约束视觉和LiDAR里程计之间的相对运动一致性。在多个公开 benchmarks上的广泛实验表明，与最先进的LIVO系统相比，该框架在准确性、环路闭合可靠性和长期一致性方面均有显著提升。

1. 引言
在复杂现实环境中运行的自主系统（如移动机器人、自动驾驶车辆和无人机平台）迫切需要准确且鲁棒的定位与地图构建能力。这些系统必须在具有挑战性的条件下可靠地工作，包括剧烈的光照变化、动态物体、大规模场景和长期运行。在这些场景中实现高精度、无漂移的同时定位与地图构建（SLAM）一直是一个长期的研究难题。

单模态SLAM系统存在固有局限性。基于视觉的方法对光照变化、运动模糊和无纹理区域非常敏感，常常导致跟踪失败。基于LiDAR的方法虽然对光照条件具有鲁棒性，但通常只能提供稀疏的几何信息，并且在具有重复几何图案或结构退化的环境中表现不佳。惯性测量单元（IMUs）提供了高频运动约束，但由于传感器偏差和噪声，随时间推移会不可避免地积累漂移。为了克服这些个别缺陷，多传感器融合成为了一个有前景的方向，旨在利用LiDAR几何信息、视觉外观和惯性运动先验的互补优势。

在多传感器框架中，LiDAR–惯性–视觉里程计（LIVO）作为一种有效的解决方案脱颖而出，它紧密结合了LiDAR的度量准确性和光照不变性、相机的丰富外观信息以及IMUs提供的高频率连续性。早期的松耦合系统[1,2,3]在计算上高效，但未能充分利用跨模态约束。最近的紧密耦合方法（如LVI-SAM [4]、R2LIVE [5]、R3LIVE [6]、FAST-LIVO [7]和FAST-LIVO2 [8]）通过在一个统一的优化或滤波框架内统一视觉和LiDAR残差，显著提高了实时性能和准确性。然而，大多数现有的LIVO系统仍然依赖于固定的测量噪声模型，无法适应随时间变化的传感器退化（例如光照变化、运动模糊、部分遮挡或几何退化）。此外，它们在大视角变化下往往缺乏鲁棒的几何环路闭合能力，也没有明确强制视觉和LiDAR里程计之间的跨模态一致性。这些限制常常导致在具有挑战性的环境中鲁棒性降低、漂移累积和全局一致性欠佳。

为了解决上述问题，本文提出了一种鲁棒的多传感器SLAM框架，该框架包含一个考虑不确定性的LIVO前端、一个基于几何的环路闭合模块以及一个跨模态一致性因子图后端。本研究的主要贡献如下：
- 考虑不确定性的iESKF融合：我们提出了一种在迭代误差状态卡尔曼滤波器（iESKF）内的动态协方差适应策略，能够在不同的传感器质量和环境条件下动态平衡LiDAR、视觉和惯性测量数据。
- 多通道几何环路检测：我们引入了一种新的多通道二进制三角形约束（mBTC）描述符，编码了更丰富的几何上下文，显著提高了在重复性和视角变化场景中的环路检测鲁棒性和区分能力。
- 跨模态一致性因子图优化：我们设计了一个跨模态一致性因子，明确强制视觉和LiDAR里程计之间的短基线运动一致性，减少了长期漂移并提高了全局一致性。
- 全面评估：在多个公开的多传感器数据集上的广泛实验表明，所提出的系统在准确性、鲁棒性和长期一致性方面均优于最先进的LIVO方法。

2. 相关工作
2.1. 直接视觉和LiDAR里程计
直接视觉里程计和SLAM通过直接最小化图像帧之间的光度不一致性来估计相机运动，无需显式特征提取。代表性的稀疏直接方法（如DSO [9]）通过光度束调整共同优化相机姿态和稀疏深度，在稳定光照下实现了高精度。早期的半密集方法（如LSD-SLAM [10]）展示了在更大规模上进行直接映射的可行性，而半直接流程（如SVO [11]）结合了基于特征的跟踪和直接对齐，以平衡效率和准确性。尽管有效，但直接视觉方法对光照变化、运动模糊和弱纹理区域仍然非常敏感，常常导致跟踪失败。

同时，基于特征的视觉SLAM系统（如ORB-SLAM2 [12]及其扩展）由于其鲁棒的关键帧管理和环路闭合检测以及全局束调整而得到广泛应用。与之密切相关的视觉–惯性系统（包括VINS-Mono [13]及其后续变体[14,15]）通过结合惯性约束提高了短期鲁棒性，但在视觉退化环境中性能仍有限。

在LiDAR方面，直接几何配准已被广泛研究。开创性的LOAM框架[16]及其变体[17,18]建立了用于LiDAR里程计和映射的点对边和点对平面约束。最近的直接LiDAR方法通过处理原始或体素化点云[19,20]避免显式特征提取，实现了高效的扫描到地图对齐。然而，仅依赖LiDAR的系统在结构稀疏的环境中可能会遇到几何退化问题，并且缺乏视觉中可用的丰富外观线索。

2.2. LiDAR–视觉–惯性SLAM
为了利用互补的感知模式，已经开发了LiDAR–视觉–惯性SLAM系统，这些系统可以采用松耦合或紧密耦合的架构。早期的松耦合流程在状态层将视觉里程计与LiDAR映射集成，其中视觉估计主要作为LiDAR配准的运动先验[1,2,3]。尽管这些方法是模块化的，但往往无法充分利用跨模态约束，导致信息利用不足。紧密耦合的公式通过联合优化或滤波多模态测量数据，展示了更优的性能。基于优化的系统（如LVI-SAM [4]）将LiDAR和视觉因素纳入一个统一的因子图中，实现了高精度映射和全局一致性，尽管计算成本增加。相比之下，基于滤波的流程因其实时能力而受到欢迎。R2LIVE [5]在迭代误差状态卡尔曼滤波器（iESKF）内统一了LiDAR几何残差和视觉光度误差，实现了高频率传播和快速更新。其后继者R3LIVE [6]通过辐射度建模和多线程视觉渲染进一步提高了鲁棒性。最近的努力集中在提高紧密耦合LIVO的效率和鲁棒性上。FAST-LIVO [7]在iESKF框架内引入了一种稀疏直接融合策略，显著加速了前端估计。其后继者FAST-LIVO2 [8]改进了视觉到地图的对齐和残差建模，建立了强大的实时基线。相关的紧密耦合系统（包括Fast-LIO [21]、Fast-LIO2 [22]、Point-LIO [23]、CT-ICP [24]、DLIO [25]和LIO-SAM [26]进一步探索了高效的残差公式、连续时间建模和改进的地图表示。

尽管取得了这些进展，大多数现有的LIVO系统仍然依赖于假设不同模态之间固定可靠性的静态或启发式测量噪声模型。实际上，由于光照变化、部分遮挡和动态物体的存在，传感器质量会有显著变化，因此需要能够适应变化的自不确定性融合策略。

2.3. 多模态SLAM中的环路闭合和一致性
环路闭合检测对于纠正累积的漂移和确保长期全局一致性至关重要。在基于LiDAR的位置识别中，由于计算效率和光照不变性，广泛采用了全局几何描述符（如Scan Context [27]）和基于投影的方法（如M2DP [28]）。后续的扩展[29,30,31]在视角变化下进一步提高了鲁棒性。然而，全局描述符在重复或稀疏环境中可能受到几何混叠的影响。

最近的工作探索了基于局部结构约束的几何不变表示，例如二进制或三角形配置[32,33]，在宽基线和外观退化条件下表现出更好的鲁棒性。然而，依赖于单一几何属性的描述符在复杂场景中可能仍然缺乏区分能力。除了环路闭合之外，保持LiDAR和视觉估计之间的跨模态一致性仍然是一个尚未充分研究的问题。许多多传感器SLAM系统隐含地假设视觉和LiDAR运动估计之间的一致性，但差异可能会累积并降低全局优化效果。受基于创新统计测试[34]和自适应协方差建模[35]启发的策略为动态调整测量置信度和提高估计器可靠性提供了原则性基础。

3. 材料与方法
3.1. 系统概述
在预处理阶段，来自IMU、LiDAR和相机的原始数据流使用基于STM32的定时板进行硬件同步（图1），其中STM32微控制器由瑞士日内瓦的STMicroelectronics提供，遵循FAST-LIVO2 [8]的同步方案。同步后，IMU测量数据用于状态传播，而LiDAR和视觉观测分别提供几何和光度约束。这些测量数据随后在考虑不确定性的iESKF前端中紧密融合。为了在不同感知条件下提高鲁棒性，引入了一种适应性动态协方差调整策略，根据三种互补线索（即创新一致性（NIS）、特征匹配质量和几何可观测性）调节每种模式的测量不确定性。这样，估计器可以根据不同传感器的即时可靠性动态平衡它们的贡献。

图1. 所提出的考虑不确定性的LiDAR–惯性–视觉SLAM系统的整体框架。实线箭头表示数据流和信息传播，而空心箭头表示前端局部估计与后端优化之间的交互。前端执行同步的多传感器预处理、运动补偿、双向特征匹配、光度残差构建以及具有适应性动态协方差调整的不确定性感知状态更新。后端结合了IMU、LiDAR、视觉、跨模态一致性和基于mBTC的环路闭合因子，以提高全局一致性。基于估计的状态，前端进一步维护由地图点更新、参考块更新和基于体素的姿态表示组成的局部地图。这种局部映射机制为后续的状态估计提供了稳定的几何和光度参考，从而增强了前端里程计的连续性和一致性。

为了提高长期一致性，后端在因子图框架内进行全局优化。具体来说，因子图结合了IMU因子、LiDAR因子、视觉因子、跨模态一致性因子和mBTC环路因子。同时，使用基于mBTC的环路检测模块来识别重复访问的地点，并引入结果环路闭合约束以进行环路闭合校正和全局轨迹优化。与仅编码几何形状信息的原始单通道BTC描述符相比，所提出的mBTC通过引入额外的多维上下文线索，增强了重复或部分退化环境中的环路鲁棒性和区分能力。

通过分层集成考虑不确定性的iESKF前端估计、局部地图维护、基于mBTC的环路检测和跨模态因子图优化，所提出的框架在一个单一的多传感器SLAM系统中统一了局部里程计估计和全局一致性优化，实现了在具有挑战性环境中的增强鲁棒性和准确性。

3.2. 考虑不确定性的iESKF融合
所提出的前端使用迭代误差状态卡尔曼滤波器（iESKF）。该滤波器在统一的概率框架内紧密融合LiDAR、视觉和惯性测量数据。它以高频率传播状态，并使用LiDAR和视觉观测数据进行状态校正。这种方法在保持SE(3)流形上的局部一致性的同时实现了实时估计。

传统的紧密耦合滤波器依赖于固定和各向同性的测量协方差。相比之下，我们的方法引入了一种考虑不确定性的自适应协方差机制。该机制根据实时统计一致性、匹配质量和几何可观测性动态调整每种感知模式的置信度。这种设计在变化感知条件下提高了鲁棒性。即使在光照变化、运动模糊、纹理弱或LiDAR几何退化的情况下，系统也能保持可靠的状态估计。

3.2.1. 状态表示和误差定义
系统状态定义为
(1)
其中 是将体框架中的向量映射到世界框架的旋转矩阵， 表示3D旋转群。向量 和 分别表示世界框架中的体位置和速度。陀螺仪偏差 和加速度计偏差 都是三维的。相应的15维误差状态表示为
(2)
其中 是旋转误差向量（也称为姿态误差），在切空间中表示。采用了一个右不变误差公式，以便通过流形上的指数收缩来进行姿态更新：
(3)
从而保持流形上的局部一致性。

3.2.2. IMU传播、测量模型和迭代ESKF更新
前端使用迭代误差状态卡尔曼滤波器（iESKF）进行紧密的多传感器融合。惯性测量单元（IMU）的测量数据用于高频状态传播，而激光雷达（LiDAR）的点对平面残差和视觉光度残差提供了互补的修正。连续时间惯性传播模型和线性化动力学遵循在基于iESKF的SLAM系统中常用的标准误差状态公式[5,21,22]。视觉和LiDAR的测量模型分别定义为光度残差和点对平面残差。这些异构测量数据被堆叠成一个联合残差向量，并通过标准的创新形式在iESKF框架内融合。更新过程迭代进行，直到收敛以提高精度。IMU传播、测量模型线性化、雅可比矩阵计算、协方差传播以及完整的迭代ESKF更新方程的详细数学推导在附录A中提供。

每次修正后，名义状态会在流形上更新，如（4）所示。

3.2.3 自适应不确定性建模和权重调节机制
前一小节将标准iESKF更新中的多模态融合表述为最大后验估计问题，其中测量协方差矩阵决定了每种模态对联合创新信息的相对贡献。然而，在现实世界环境中，由于照明变化、运动模糊、动态干扰、稀疏对应关系和几何退化，视觉和LiDAR观测的质量变化很大。在这种情况下，固定的协方差假设可能导致统计不匹配和感知模态之间的置信度分配不当。为了解决这个问题，本小节开发了一种自适应动态协方差建模机制。每种模态的不确定性被分解为三个互补的来源：统计一致性、感知/匹配质量和几何可观测性。这三个因素以乘法形式结合在一起，产生了一种可解释、可控且易于扩展的统一调整策略。这样，iESKF更新中使用的协方差不再固定，而是根据每种感知模态的即时可靠性进行自适应调整。

统一自适应协方差形式：三因素乘法分解
直观来说，不同模态的测量数据不应一直被同等信任。例如，在模糊或照明不良的情况下，视觉观测的可靠性会降低；而在几何退化的场景中，LiDAR约束可能会减弱。为了反映这种行为，我们使用三个互补的指标在线调整每种模态的名义协方差：创新一致性、测量质量和可观测性。对于每种感知模态，用表示其名义协方差。自适应协方差通过以下公式定义：

然后将联合自适应协方差表示为（6），这取代了创新协方差和卡尔曼增益计算中的原始协方差，从而在统一的滤波框架内实现多模态权重的在线重新分配[36]。

这种三因素分解具有明确的物理和统计学意义。具体来说，确保统计一致性并防止过度自信；反映感知和匹配过程的可靠性；编码几何可观测性并抑制沿退化方向的虚假信息增益。它们的乘法组合确保任何一个维度的退化都会触发协方差膨胀，从而避免由于任何一个指标失败而导致的错误置信度分配。

3.2.3 自适应不确定性建模和权重调节机制
在更新时刻k，用表示模态m的创新（残差）向量。其线性化创新协方差为（7），其中是测量雅可比矩阵，是预测状态协方差。归一化创新平方（NIS）定义为[34,37]（8）。如果噪声模型和线性化在统计上是有效的，则（9），其中是模态m的残差维度（自由度）。当持续超过其预期时，这表明测量噪声被低估或模型不匹配以及异常值污染增加。在这种情况下，直接使用固定协方差将导致滤波器过度自信。因此，统计一致性缩放因子定义为（10），用于衡量当前残差幅度是否与假设的噪声模型一致。如果滤波器校准良好，NIS应保持在其预期的卡方范围内。持续较大的NIS表明该模态的可靠性低于预期，可能是由于模型不匹配或异常值造成的。在这种情况下，我们增加其协方差并减少其更新权重。

3.3.3 mBTC：一种稳定的几何骨架驱动的多通道结构表示框架
在重复或退化的环境中，仅由一种几何统计量构建的描述符往往具有不足的分离能力，这会降低全局重新定位的循环检测可靠性。为了解决这个问题，本节引入了一种多通道二进制三角形约束（mBTC）框架，通过结合稳定的几何骨架和补充的上下文通道来增强位置表示。给定一个原始观测空间，位置识别可以抽象为映射（24），其中表示用于检索和匹配的描述符空间。理想的描述符应保持几何不变性，同时尽可能保留位置区分信息。然而，在传统的单通道公式中，描述符通常只包含一类几何统计量（25），其中通道往往高度相关或部分冗余。在这种情况下，有效的区分维度是有限的，位置区分信息S无法显著改善。

为了克服这一限制，提出的mBTC框架通过多个物理上可解释且统计上互补的通道扩展描述符空间。mBTC不仅编码局部结构的几何形状，还将每个候选循环单元表示为基于稳定三角形骨架和一组上下文属性的组合。这种设计在保持刚体不变性的同时，提高了描述符空间的有效秩，从而在复杂场景中提高了区分能力。

3.3.1 多通道上下文建模
尽管三角形骨架提供了刚体不变的几何结构，但在重复或特征不明显的环境中，仅靠形状信息往往是不够的。为了提高区分能力，mBTC通过从其局部支持区域提取一组上下文通道来增强每个稳定的三角形。设重力和对齐的LiDAR点云表示为（26），其中z是重力方向，是LiDAR强度。对于由顶点形成的三角形，定义每个顶点周围半径为r的局部邻域（27）。然后定义联合支持区域为（28），这种基于联合的支持区域对于顶点排列是对称的，并比单点邻域提供更稳定的统计信息。在（28）中，提取了四个互补的上下文通道：首先，垂直跨度通道描述了局部垂直结构的变化（29），并将其归一化为（30）；其次，密度通道衡量了局部结构的空间紧凑性（31），并通过（32）进行归一化；第三，法线变化通道捕获了局部表面不规则性（33）；第四，平均强度通道捕获了局部反射特性（34），并在每个帧内进行标准化并限制在（35）范围内。如图2所示，传统的BTC描述符仅编码三角形的几何尺度信息（左），而提出的mBTC（右）通过四个额外的上下文通道——垂直跨度、空间紧凑性、表面不规则性和反射强度——来增强相同的刚体不变骨架。这些多通道属性提供了超出纯几何信息的补充数据，显著提高了在复杂和重复环境中的地点识别鲁棒性和区分能力。图2展示了传统的单通道二进制三角形约束（BTC）描述符与提出的多通道二进制三角形约束（mBTC）描述符之间的比较。箭头指示了从单一几何通道到多通道结构表示的扩展。(A) 传统的BTC仅编码三角形的几何尺度信息，该信息在刚体变换下是不变的。(B) 提出的mBTC在同一几何骨架的基础上增加了四个额外的上下文通道：垂直跨度、空间紧凑性、表面不规则性和反射强度。这些多通道属性增强了描述符在重复或视点变化环境中的区分能力和鲁棒性。因此，三角形的完整连续表示可以写为(38)，其中表示几何骨架属性，表示上下文通道。由于这些通道描述了局部环境的不同物理属性，它们通常相关性较弱，共同提高了描述符空间的有效秩。这种统计互补性使mBTC能够更好地区分具有相似几何布局但局部结构或辐射特性不同的地点。最后，三角形的多通道特征向量定义为(39)，其中前三个元素是BTC中的几何不变量，后四个是上下文属性。这种混合公式使mBTC不仅能编码三角形的固有形状几何结构，还能编码局部环境结构和材料响应。

这些多通道连续特征通过后续的量化和编码步骤转换为紧凑的二进制代码，从而在保留关键区分信息的同时实现高效的大规模检索。通过这种方式，mBTC在几何不变性、描述丰富性和匹配效率之间取得了平衡的设计，使其适用于长期和大规模SLAM场景中的鲁棒循环检测。

3.3.2. 二进制编码和描述符形成
在每个三角形顶点构建了多通道局部几何上下文后，需要一个高效的编码方案将这些连续描述符转换为适合大规模循环检测的紧凑且具有区分性的表示。本节描述了二进制编码和描述符形成过程，该过程将规范化的多通道属性转换为统一的二进制代码，同时保留它们的相对几何关系。采用二进制编码来减少内存消耗，并通过位运算实现快速相似性计算，这对于长期SLAM中的实时循环检测至关重要。为了保持BTC的紧凑性并实现高效的循环检索，连续特征向量被量化为二进制代码，并聚合成帧级描述符。
mBTC的二进制表示和检索过程通过以下三个步骤实现：

自适应量化
对于每个特征维度，自适应阈值是通过当前帧内所有三角形的分位数来确定的。
特征值被量化为(40)，其中灰度编码确保了汉明空间中的局部连续性。所有编码的通道被连接起来形成mBTC二进制向量：(41)

直方图池化
直方图池化将局部二进制特征聚合成固定大小的帧级表示，通过术语频率-逆文档频率（TF-IDF）[41]重加权来抑制重复结构的影响。这使得场景区分和相似性检索更加鲁棒，这对于实时循环闭合至关重要。

相似性计算和快速匹配
在TF-IDF重加权之后，帧描述符可以用于全局相似性计算。在这项工作中，采用余弦相似性作为相似性度量：(44)，其中c和r分别表示当前帧和候选帧。

3.3.3. 循环检索和验证
在运行时，将当前帧描述符与存储在循环数据库中的历史描述符进行比较。然后通过使用点对平面ICP进行几何配准来验证排名最高的循环候选者，生成循环约束。(45)，其中表示帧i和j之间的ICP估计的相对姿态，是从配准协方差中得出的。确认的循环闭合被插入到全局因子图中，以纠正累积的漂移并保持长期的全局一致性。这种考虑可靠性的循环闭合方法比仅基于外观的检索方法（如Scan Context）在几何退化或感知上具有挑战性的环境中表现更好。

3.4. 跨模态一致性因子图优化
为了实现全局一致的轨迹估计，所提出的框架结合了一个联合因子图优化模块，整合了惯性、视觉、LiDAR和循环闭合约束。然而，独立集成特定于模态的里程计因子可能会导致局部不一致性，特别是在传感器瞬态退化存在的情况下。为了减轻模态漂移差异，我们引入了一个跨模态一致性因子，以显式强制视觉和LiDAR相对运动之间的一致性。

3.4.1. 因子图构造
让表示关键帧k时的机体姿态，表示相机和LiDAR的已知外部校准。后端优化通过最小化因子图上定义的所有残差之和来寻找最大后验（MAP）估计：(46)。这里，分别表示IMU预积分、视觉重投影、LiDAR点对平面、循环闭合和跨模态约束的集合；表示每个因子的关联信息矩阵。如图3所示，后端优化在统一的因子图框架内整合了IMU、LiDAR、视觉、循环闭合和跨模态一致性因素。图3展示了带有所提出的跨模态一致性因子的因子图优化后端。该图共同结合了：IMU因子（红色）、LiDAR-惯性里程计（LIO）因子（蓝色）、视觉-惯性里程计（VIO）因子（绿色）、所提出的跨模态一致性因子（黄色）和循环闭合因子（紫色）。绿色六边形表示循环检测结果，当检测到重新访问的地点时，会添加紫色的循环因子。跨模态一致性因子（黄色）显式强制视觉和LiDAR里程计之间的短基线运动一致性，连接了前端估计和全局优化。

3.4.2. 相对运动约束建模
对于一对关键帧，前端提供了来自相机和LiDAR的相对姿态测量：以及相关的协方差。从当前轨迹估计得出的预测相对运动为：(47)。然后，视觉和LiDAR里程计残差被表述为：(48)。

3.4.3. 跨模态一致性因子
尽管视觉和LiDAR里程计提供了独立的相对运动估计，但它们描述了机器人在同一关键帧间隔内的相同物理运动。我们没有引入额外的绝对运动测量，而是定义了一个软一致性因子，用于惩罚两种特定于模态的相对运动残差在局部误差空间中的差异。(49)。其中，和分别表示从第3.4.2节中推导出的视觉和LiDAR里程计残差。相关的马氏距离成本定义为(50)，在视觉和LiDAR相对运动不确定性在线性化误差空间中局部独立的共同假设下，它们残差差异的协方差可以近似为两个协方差项之和。因此，信息矩阵定义为(51)。上述公式自然反映了两种模态的信心水平：当视觉跟踪退化（例如运动模糊或纹理低）时，其不确定性增加，从而减少了其在一致性因子中的影响。相反，在几何退化的LiDAR环境中（例如平面走廊），视觉模态提供了更强的约束。因此，当一种模态高度不确定时，一致性因子的影响较小，而当两种模态都保持信息量时，一致性因子仍然具有鼓励一致性。

3.4.4. 线性化和雅可比结构
我们采用右乘法扰动模型进行姿态更新：(52)。围绕当前估计值线性化视觉和LiDAR残差得到：(53)、(54)。由于跨模态误差定义为两个里程计残差之间的差异，其一阶线性化为：(55)。通过直接减去视觉和LiDAR里程计因子的现有雅可比矩阵来获得跨模态因子的雅可比矩阵，无需任何额外的符号推导或重新计算。所提出的跨模态一致性因子有效地将视觉和LiDAR里程计在一个统一的误差空间中绑定在一起，抑制了模态依赖的漂移，同时尊重它们各自的不确定性。这种联合约束产生了更平滑的轨迹，在单模态退化下具有更好的弹性，并在长期导航中提供了更强的全局一致性。

4. 结果
所有方法都是在C++中实现的，并在配备有Intel Core i7-10750H CPU和24 GB RAM的平台上使用Ubuntu 20.04和ROS Noetic执行的。对于所有基线方法，我们使用了它们公开可用的实现，并尽可能保持推荐的默认设置。对于所提出的方法，除非另有说明，否则所有数据集的主要实现相关参数都是固定的。在考虑可靠性的融合模块中，协方差缩放因子被限制在范围内，与观测相关的缩放项使用指数移动平均进行平滑处理。在考虑质量的调制中，视觉质量权重被设置为，LiDAR质量权重被设置为。数值稳定性常数被固定为。对于循环检测，前K个检索候选者是用选出的，只有当时间间隔大于50帧时，循环对才被视为有效，以避免 trivial的邻近匹配。在精确度-召回率评估中，如果两个帧之间的欧几里得距离小于5米，则认为它们是真实的循环对。对于所提出的方法，除非明确说明，否则没有进行特定于数据集的参数调整。为了评估所提出的框架，在四个公共的多传感器数据集（KITTI [42]、M2DGR [43]、M3DGR [44] 和 NCLT [45]）上进行了实验。这些数据集涵盖了室内和室外环境、动态场景、重复结构以及长期轨迹，为基准测试LiDAR-惯性-视觉SLAM系统提供了同步的LiDAR、视觉和惯性测量数据。

4.1. 循环检测性能
为了评估所提出的多通道几何结构描述符（mBTC）在循环检测中的检索性能，首先在几个公共数据集上进行了比较实验。选择经典的LiDAR循环检测方法STD和基于三角形几何的BTC描述符作为基线方法。使用精确度-召回率（P–R）曲线、Recall@N和平均精确度（AP）全面评估了它们的检索性能。实验在三个数据集上进行，即KITTI Odometry、NCLT和M2DGR。对于每个LiDAR扫描，系统从历史数据库中检索最相似的候选帧，然后根据真实轨迹将检索结果判断为真实或错误的循环闭合。具体来说，如果两个帧的空间距离小于预定义的阈值，则认为它们是真实的循环对。图4显示了不同方法在所有测试序列上的精确度-召回率曲线。可以观察到，提出的mBTC在所有数据集上一致地实现了性能的提升。与STD相比，mBTC即使在高召回率区域也保持了更高的精确度，表明所提出的描述符具有更强的区分能力，并能有效减少由结构相似场景引起的错误匹配。与单通道几何描述符BTC相比，mBTC的曲线通常向右上区域偏移，表明引入多通道结构信息提高了循环检测的稳定性和可靠性。

为了进一步量化不同方法的检索性能，表1报告了不同N值的Recall@N值和平均精确度（AP）。Recall@N表示在前N个检索候选者中至少包含一个真实循环闭合的概率，直接反映了描述符的检索能力。如表中所示，mBTC在所有评估的数据集上都获得了最高的Recall@1和Recall@10。例如，在KITTI数据集上，mBTC的Recall@1比STD提高了10%以上，同时也明显优于BTC。这一结果表明，引入多通道几何上下文信息可以增强描述符表征环境结构差异的能力。AP结果进一步证实了mBTC在整体检索性能上的优越性。由于AP同时考虑了不同阈值下的精确度和召回率，因此它提供了对描述符的更全面评估。在所有三个数据集上，mBTC都取得了最高的AP值，证明了在不同场景结构和环境条件下的稳定和可靠的检索性能。表1总结了KITTI00和NCLT1数据集上的Recall@N和AP。mBTC的性能提升主要来自两个方面。首先，由稳定三角形结构构建的几何骨架确保了对视图变化和刚体变换的不变性，从而提高了匹配的稳定性。其次，引入的多通道结构属性，包括垂直结构分布、点云密度、表面不规则性和反射强度，为每个几何结构提供了更丰富的上下文信息，使描述符能够更好地区分结构上重复的场景。4.2. mBTC通道设计的消融研究为了进一步验证所提出的多通道几何结构表示在循环检测中的有效性，本小节提出了一个消融研究，系统地分析了不同几何信息通道组合对描述符性能的影响。通过逐步引入不同的几何属性并比较在不同编码策略下的循环检测性能，可以定量评估每个几何通道对结构表示的贡献。为了分析不同通道对整体性能的贡献，实验采用了以下编码策略的渐进累积设计：A0（仅三角形）：仅使用基本的三角形结构信息进行编码；A1（+高度）：在三角形结构的基础上添加高度信息；A2（+密度）：进一步结合点云密度信息；A3（+法线）：添加表面法线信息；A4（+强度）：进一步引入LiDAR反射强度；A5（所有通道）：联合融合所有几何信息通道。实验在KITTI测距数据集和NCLT1序列上进行。为了全面评估循环检测性能，采用了两个指标，即回收率@N（Recall@N）和平均精度（Average Precision，AP）。回收率@N衡量的是在前N个候选者中至少成功检索到一个真实循环闭合的查询比例，而AP反映了精度和回收率之间的整体权衡。从表2可以看出，随着更多几何通道的加入，检索性能不断提高。在KITTI上，回收率@10从A0的某个值增加到A5的某个值，而AP从某个值提高到另一个值。在NCLT上，回收率@10从某个值增加到另一个值，AP从某个值提高到另一个值。这些结果表明，每个额外的通道都为结构表示提供了有用的补充信息。表2. 在KITTI和NCLT数据集上不同通道组合的回收率和AP性能比较。特别是，高度、密度和法线通道通过丰富描述符的垂直、空间和局部几何线索，带来了稳定的性能提升，而强度则通过捕捉表面之间的反射差异进一步提高了区分能力。最佳结果是A5，即所有通道联合融合，表明所提出的多通道表示与单通道编码相比，可以显著提高描述符的区分能力和鲁棒性。消融结果证实，所提出的mBTC框架有效地利用了多通道几何信息，并在不同的数据集上实现了更好的稳定性和泛化能力。虽然增加更多通道可以提高描述符的区分能力和检索精度，但它也引入了额外的计算负担，包括通道提取、二进制编码和直方图构建。在我们的实现中，运行时间从A0逐渐增加到A5，但由于循环检测的执行频率低于前端状态估计，所以开销仍然适中。特别是，全多通道配置（A5）与仅使用三角形的基线相比，额外的运行时间大约增加了15-20%，同时在KITTI和NCLT上提供了明显的回收率@N和AP的提升。额外的成本主要来自上下文属性（高度、密度、法线和强度）的提取和描述符维度的增加。对于实际的循环闭合应用来说，这种权衡是可以接受的，因为在这些应用中，鲁棒性和检索可靠性比最小的每帧描述符成本更为重要。4.3. 融合和一致性机制的消融研究 4.3.1. 自适应不确定性建模的分析为了评估所提出的自适应协方差调节机制的有效性，考虑了两个代表性的退化场景，并分析了三个缩放因子、、和的时间变化。这里，测量创新的统计一致性，反映观察质量，描述了几何可观察性。第一个场景是户外-走廊-户外序列，系统从一个开放区域移动到一个走廊，然后再回到户外。由于走廊环境通常表现出强烈的结构重复性和有限的侧向约束，这个场景适合评估所提出方法在几何退化下的响应。第二个场景是夜间场景，其中车头灯干扰导致局部照明变化，显著影响了视觉匹配质量和光度残差统计。这个场景用于分析方法在视觉退化下的动态响应。如图5所示，在开放的户外区域，这三个因子保持较低且稳定，表明LiDAR和视觉约束是可靠的。系统进入走廊后，显著增加，表明几何可观察性大幅降低。同时，也上升，表明创新统计量偏离了理论预期。相比之下，仅略有变化，表明这种场景下的主要退化源是几何可观察性的降低而不是匹配质量的差错。一旦系统回到户外环境，这三个因子逐渐降至稳定水平，表明所提出的机制可以在环境再次变得有信息量时自动降低协方差。图6显示了夜间车头灯场景下的因子变化。在正常低光条件下，三个因子保持相对较高的水平，因为弱照明限制了图像对比度和特征稳定性。当车头灯照亮场景时，局部纹理和结构细节变得更加清晰，导致显著降低，表明匹配质量得到改善。同时，降低，显示出更好的统计一致性，也略有下降，表明可观察性得到改善。这些结果表明，所提出的自适应不确定性建模机制可以在退化情况下抑制不可靠的信息，并在感知条件改善时自动增加观察信心。4.3.2. 通过跨模态一致性因子抑制漂移为了验证所提出的跨模态一致性因子的有效性，进行了比较实验，以分析在具有挑战性条件下视觉和LiDAR测距之间的漂移分离。图5. 在退化观察条件下的协方差缩放因子的时间曲线。蓝色、橙色和绿色曲线分别代表基于NIS的因子、基于质量的因子和基于可观察性的因子。浅蓝色阴影区域表示本例中考虑的退化观察区间。图6. 夜间车头灯干扰场景下协方差缩放因子的时间曲线。蓝色、橙色和绿色曲线分别代表基于NIS的因子、基于质量的因子和基于可观察性的因子。浅蓝色阴影区域表示本例中考虑的车头灯照明段。在多模态SLAM中，感知模型和噪声特征的差异可能导致视觉和LiDAR模态之间的估计不一致，尤其是在动态场景、纹理较弱的环境和转弯运动中。为了评估这种效应，比较了三种配置：基线-0：仅LiDAR、视觉和IMU因子；基线-2：基线-0加上循环闭合约束；我们的方法-完整：基线-2进一步增加了所提出的跨模态一致性因子。跨模态相对运动误差定义为（57），其中和分别表示时间k时的LiDAR和视觉相对姿态估计。较大的值表示更强的跨模态漂移。图7显示了的时间演变，表3报告了相应的统计结果。图7. 跨模态运动不一致性的时间曲线。浅蓝色阴影区域突出了用于详细分析的具有挑战性的区间，包括面向地面的区域和循环闭合区域。表3. 跨模态一致性误差统计。如图7所示，在序列的早期阶段，跨模态误差保持较小，表明视觉和LiDAR测距之间具有良好的一致性。当机器人进入视觉退化区域时，视觉估计逐渐累积漂移，导致增加。如果没有所提出的跨模态因子，这种误差会持续很长时间。相比之下，通过跨模态一致性约束，视觉状态通过LiDAR观测不断得到规范，误差增加被有效抑制。循环闭合和全局优化后，累积的不一致性进一步减少。表3显示，所提出的方法实现了最低的平均误差和95%百分位数误差，分别减少了大约16.1%和31.2%。这些结果证实，所提出的跨模态一致性因子有效地提高了多模态状态估计的一致性和稳定性。4.4. 整体系统性能和映射结果 4.4.1. 在M2DGR数据集上的定量评估表4报告了在没有循环闭合的M2DGR数据集上的定量定位结果。评估指标是绝对轨迹误差（ATE），以米为单位。比较的方法包括VINS-Mono、A-LOAM、LIO-SAM、LVI-SAM和所提出的方法。表4. 在没有循环闭合的M2DGR数据集上的定位精度（ATE，米）。在没有循环闭合的情况下，所有方法都完全依赖于前端测距和局部优化，因此更容易受到漂移积累的影响。如表4的上半部分所示，VINS-Mono在几个序列（例如Hall_02和Street_05）中失败，表明在视觉挑战性或大规模环境中鲁棒性有限。这种行为是可以预期的，因为VINS-Mono强烈依赖于稳定的视觉特征跟踪和几何初始化；当照明变化、纹理弱化、运动模糊或长期漂移降低视觉约束时，估计器无法保持可靠的姿态估计。基于LiDAR的方法和LiDAR-视觉方法表现出更好的稳定性，但在较长或更复杂的序列（如Walk_01和Street_05）中仍然受到明显漂移的影响。相比之下，所提出的方法在大多数序列中始终实现了最低的ATE，包括Hall_02、Room_02、Door_02、Gate_03、Walk_01和Street_05。这些结果表明，不确定性感知的iESKF融合有效地平衡了LiDAR和视觉测量，即使在缺乏全局循环约束的情况下也能显著降低估计误差。4.4.2. 在M3DGR数据集上的定量评估表5报告了在包括视觉退化、LiDAR退化和车轮打滑等多种挑战性条件下的M3DGR数据集上的定量结果。特别是，“车轮打滑”列评估了不同方法在不平坦地形上对抗车轮打滑的鲁棒性。总体而言，所提出的方法在所有评估条件下都表现出鲁棒性，并在大多数场景中实现了最佳或接近最佳的性能。表5. 在 six种挑战性条件下的M3DGR数据集上的定量结果。各列分别对应动态（Dynamic）、黑暗（Dark）、遮挡（Occlusion）、户外（Outdoor）、LiDAR退化（LiDAR Degeneration）和车轮打滑（Wheel Slip）条件。“车轮打滑”指的是在不平坦或滑溜的地形上发生显著车轮打滑的场景，这通常会给轮式机器人带来额外的运动干扰。较低的值表示更好的性能。在视觉挑战条件下，包括Dynamic01、Dark01和Occlusion01，我们的方法在所有比较方法中始终产生最低的误差。特别是在Dynamic01上实现了0.19的误差，在Occlusion01上实现了0.15的误差，显著优于现有的LiDAR-视觉-惯性系统。这些结果表明，所提出的不确定性感知融合和跨模态一致性优化有效地减轻了动态物体、照明退化和部分遮挡的影响。在标准的Outdoor01序列中，我们的方法实现了最低的误差（0.76），略优于Fast-LIVO2（0.87）和Fast-LIVO（0.94），显示出增强的长期全局一致性。对于受到严重LiDAR退化的Corridor01序列，我们的方法实现了最佳性能（1.07），而SR-LIVO未能产生有效结果。这突显了在结构重复环境中集成循环闭合和跨模态约束的优势。在模拟车轮打滑的Wheel-float01序列中，我们的方法显著优于所有基线，误差为0.10，显示出对运动模型退化的出色鲁棒性。总体而言，这些结果验证了所提出的系统在各种不利感知条件下保持了准确和稳定的定位。图8显示了M3DGR数据集的Outdoor01序列上的轨迹比较，不同区域突出了不同的评估方面。在区域A中，对应于轨迹的终点，比较重点关注不同方法之间的最终位置偏差。可以观察到，几种基线方法相对于真实终点显示出明显的漂移，而所提出的方法终止得更接近真实终点，表明长期误差积累减少。在区域B、C和D中，评估强调了估计轨迹与路径不同段之间の空间偏差。在这些区域，所提出的方法产生的轨迹始终比其他方法更接近真实终点。特别是在曲线段（区域B）中，我们的方法在快速方向变化期间保持较小的偏差。在长直线段（区域C）中，基线方法的累积漂移更为明显，而所提出的方法在较长的行驶距离上表现出较小的偏差。同样，在区域D中，所提出的轨迹与真实终点的对齐更加紧密，反映了全局优化后的改进一致性。B、C和D中的区域比较表明，所提出的方法在多种运动模式下都能实现较小的轨迹与地面真实值之间的距离，从而在整个序列中实现更准确和一致的定位性能。图8展示了Outdoor1序列上的轨迹定性比较。轨迹被投影到水平x-y平面上，轴以米为单位进行测量。虚线矩形表示放大区域，虚线箭头指示这些放大视图在完整轨迹中的对应位置。字母A-D标记用于视觉比较的代表性局部区域。GT表示地面真实轨迹。图9展示了用于实际实验的低光室外环境。图10比较了所提出方法与FAST-LIVO2的相应映射结果。如图A和B所示，我们的方法重建的点云更加密集和完整，结构边界更加清晰，缺失区域更少。这些定性改进验证了不确定性感知的iESKF融合和自适应测量加权能够在严重视觉退化的情况下更可靠地利用激光雷达测量数据，从而提高地图的完整性和结构保真度。图9是Dark01序列的相机视图观测结果。图10是在具有挑战性的低光室外环境中的点云映射结果定性比较；请参见图9了解相应的场景。(a) 所提出的方法生成了更密集和更完整的地图。(b) FAST-LIVO2产生的映射结果相对稀疏和不完整。这些差异在区域A（对应于建筑物立面）和区域B（对应于垂直立柱）尤为明显。这一比较说明了在严重光照退化情况下，不确定性感知融合和自适应协方差加权的有效性。5. 结论本文提出了一个健壮的多传感器SLAM框架，该框架紧密集成激光雷达、视觉和惯性测量数据，以在复杂和动态环境中实现准确和一致的定位和映射。通过重新审视现有的激光雷达-惯性-视觉测距（LIVO）系统的局限性，我们发现了与不确定性建模、闭环鲁棒性和跨模态一致性相关的三个关键挑战。为了解决这些问题，开发了一个不确定性感知的迭代误差状态卡尔曼滤波器（iESKF），该滤波器通过自适应协方差调整动态平衡异构测量数据，这种调整由创新统计、特征质量和可观测性引导。此外，引入了一个基于所提出的多通道二进制三角形约束（mBTC）描述符的几何驱动闭环模块，以提高在视角变化、光照变化和外观退化下的闭环检测鲁棒性。进一步地将跨模态一致性因子纳入全局因子图优化中，以明确约束视觉和激光雷达测距之间的相对运动一致性，从而减少长期漂移并提高全局一致性。在多个公共基准测试上的广泛实验表明，所提出的系统在定位精度、闭环可靠性和长期一致性方面始终优于现有的LIVO方法，特别是在动态和视觉具有挑战性的场景中。这些结果验证了在统一的概率框架内结合不确定性感知前端估计、基于几何的闭环检测和一致性感知后端优化的有效性。未来的工作将集中于将所提出的框架扩展到大规模长期自主系统，包括对极端传感器退化的在线适应、学习辅助的几何描述符增强以及与语义感知的更紧密集成。此外，结合连续时间建模和探索更具表现力的跨模态约束可能会进一步提高在高度动态和复杂环境中的鲁棒性。