文章 # 具备自适应融合与多通道几何回环闭合功能的不确定性感知激光雷达-惯性-视觉SLAM算法 钟启雪 $^{1,2}$，邢静 $^{1}$，刘健 $^{1,*}$，罗路青 $^{1}$ $^{1}$ 中国科学院微电子研究所，北京 100029，中国； （钟启雪）；（邢静）；（罗路青） 2 中国科学院大学，北京 100049，中国 通讯作者： ## 摘要 在复杂动态环境中实现精确且鲁棒的定位与建图，始终是自主系统的核心挑战。激光雷达-惯性-视觉里程计（LIVO）融合了激光雷达几何信息、视觉外观特征以及惯性运动约束这三者的互补优势。然而，现有的LIVO系统在面对传感器性能退化时适应性有限，回环闭合的鲁棒性不足，且跨模态一致性建模能力也有待提升。本文提出了一种鲁棒的多传感器SLAM框架，集成了不确定性感知的LIVO前端、基于几何特征的回环闭合模块，以及跨模态一致性因子图后端。我们开发了不确定性感知的迭代误差状态卡尔曼滤波器（iESKF），用于紧密融合激光雷达、视觉和惯性测量数据，并根据创新统计量、特征匹配质量以及可观测性来动态调整测量协方差。为了提升全局一致性，我们提出了一种用于激光雷达回环检测的多通道二进制三角约束（mBTC）描述符，该描述符能够在视角变化和外观退化的情况下仍保持较高的鲁棒性。此外，我们还引入了跨模态一致性因子，用以显式约束视觉里程计与激光雷达里程计之间的相对运动一致性。在多个公开基准测试上的大量实验表明，与现有先进的LIVO系统相比，本文所提框架在精度、回环闭合可靠性以及长期一致性方面均有显著提升。 关键词：激光雷达-惯性-视觉SLAM；多传感器融合；不确定性感知的iESKF；回环闭合；跨模态一致性 ### 1. 引言 版权所有：© 2026 作者。 授权方：MDPI，瑞士巴塞尔。 本文为开放获取文章， 发布遵循相关条款与 Creative Commons 署名（CC BY）许可协议。 对于在复杂现实环境中运行的自主系统而言，如移动机器人、自动驾驶车辆以及无人飞行平台，精确且鲁棒的定位与建图是不可或缺的核心能力。这些系统需要在光照剧烈变化、存在动态物体、场景规模庞大、结构重复以及长时间运行等极具挑战性的条件下可靠工作。要在此类场景中实现高精度、无漂移的同时定位与建图（SLAM），仍是长期以来的研究难题。 单模态SLAM系统存在固有的局限性。基于视觉的方法对光照变化、运动模糊和无纹理区域极为敏感，常常导致跟踪失败。而基于激光雷达的方法虽然对光照条件具有鲁棒性，但通常只能提供稀疏的几何信息，在具有重复几何图案或结构退化现象的环境中表现不佳。惯性测量单元 --- （IMU）能够提供高频的运动约束，但由于传感器偏差和噪声的影响，其数据会随时间不可避免地产生漂移。为了克服这些单一传感器的弱点，多传感器融合成为了一种极具前景的方向，旨在充分利用激光雷达的几何信息、视觉的外观信息以及惯性的运动先验这三者的互补优势。 在多传感器框架中，激光雷达-惯性-视觉里程计（LIVO）已成为一种有效的解决方案，它能够紧密整合激光雷达的量测精度和光照不变性、摄像头提供的丰富外观信息，以及IMU所赋予的高频连续性。早期的松耦合系统[^1] [^2] [^3]虽然计算效率较高，但未能充分利用跨模态约束。近年来出现的紧耦合方法，如LVI-SAM [^4]、R2LIVE [^5]、R3LIVE [^6]、FAST-LIVO [^7]以及FAST-LIVO2 [^8]，通过在统一的优化或滤波框架内整合视觉与激光雷达残差，显著提升了实时性能与精度。然而，大多数现有的LIVO系统仍依赖于固定的测量噪声模型，无法适应传感器性能的时变退化情况（例如光照变化、运动模糊、部分遮挡或几何退化）。此外，它们在视角变化较大的场景下往往缺乏鲁棒的几何闭环检测能力，也未显式地强制视觉与激光雷达里程计之间的跨模态一致性。这些局限常常导致系统在复杂环境中的鲁棒性下降、累积漂移加剧，以及全局一致性不佳。 为解决上述问题，本文提出了一种鲁棒的多传感器SLAM框架，该框架集成了不确定性感知的LIVO前端、基于几何的闭环检测模块，以及跨模态一致性因子图后端。本研究的主要贡献如下： 本文的主要贡献总结如下： 1. 不确定性感知的iESKF融合。我们提出了一种在迭代误差状态卡尔曼滤波器（iESKF）内动态调整协方差的策略，该策略能够根据传感器质量与环境条件的差异，自适应地平衡激光雷达、视觉和惯性测量数据。 2. 多通道几何闭环检测。我们提出了一种新颖的多通道二进制三角形约束（mBTC）描述符，它能够编码更丰富的几何上下文信息，从而显著提升在重复图案场景及视角多变场景中的闭环检测鲁棒性与区分能力。 3. 跨模态一致性因子图优化。我们设计了跨模态一致性因子，该因子能够显式地强制视觉与激光雷达里程计之间在短基线运动上保持一致，从而减少长期漂移并增强全局一致性。 4. 全面的评估。我们在多个公开的多传感器数据集上进行了大量实验，结果表明所提出的系统相较于现有的先进LIVO方法，在精度、鲁棒性和长期一致性方面均表现出显著优势。 ### 2. 相关工作 #### 2.1. 直接视觉与激光雷达里程计 直接视觉里程计和SLAM通过直接最小化图像帧间的光度不一致性来估计相机运动，无需显式的特征提取。典型的稀疏直接法如DSO [^9]，通过光度束调整共同优化相机位姿与稀疏深度，在光照稳定的条件下可实现高精度。早期的半稠密方法，包括LSD-SLAM [^10]，证明了在更大范围内进行直接建图的可行性；而像SVO [^11]这样的半直接流程则结合了基于特征的跟踪与直接对齐技术，以平衡效率与精度。尽管效果显著，但直接视觉方法对光照变化、运动模糊以及弱纹理区域仍极为敏感，常常导致跟踪失败。 --- 与此同时，基于特征的视觉SLAM系统，如ORB-SLAM2 $^{[12]}$及其扩展版本，因其强大的关键帧管理、闭环检测及全局束调整能力而被广泛应用。与之密切相关的视觉-惯性系统，包括VINS-Mono $^{[13]}$及其后续变体 $^{[14,15]}$，通过引入惯性约束提升了短期鲁棒性，但在视觉条件较差的环境中性能依然有限。 在激光雷达领域，直接几何配准技术已被广泛研究。开创性的LOAM框架 [^16]及其变体 [^17] [^18]建立了用于激光雷达里程计与建图的点对边缘、点对平面约束。近期出现的直接激光雷达方法无需显式特征提取，可直接作用于原始点云或体素化点云 [^19] [^20]，从而实现高效的扫描到地图对齐。不过，仅依赖激光雷达的系统在结构稀疏的环境中可能会遇到几何退化问题，且缺乏视觉传感器所具备的丰富外观线索。 #### 2.2. 激光雷达-视觉-惯性SLAM 为了充分利用多种传感模式的互补优势，人们开发了松散耦合与紧密耦合两种架构的激光雷达-视觉-惯性SLAM系统。早期的松散耦合流程在状态层面将视觉里程计与激光雷达建图相整合，其中视觉估计主要作为激光雷达配准的运动先验 [^1] [^2] [^3]。尽管这种模块化设计较为灵活，但往往无法充分利用跨模态约束，导致信息利用率不足。 紧密耦合的方案通过联合优化或滤波多模态测量数据，展现出了更优的性能。基于优化的系统如LVI-SAM [^4]将激光雷达与视觉因子纳入统一的因子图中，虽然计算成本较高，但能实现高精度的建图与全局一致性。相比之下，基于滤波的流程因其实时性而备受青睐。R2LIVE [^5]在迭代误差状态卡尔曼滤波器（iESKF）中统一了激光雷达几何残差与视觉光度误差，能够实现高速的数据传播与快速更新；其后续版本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中的回环检测与一致性问题 回环检测对于纠正累积漂移并确保长期全局一致性至关重要。在基于激光雷达的位置识别中，Scan Context [^27] 等全局几何描述符以及M2DP [^28] 这类基于投影的方法因其计算效率高且不受光照影响而被广泛采用。后续的扩展研究[29–31]进一步提升了其在视角变化下的鲁棒性。不过，在重复或稀疏环境中，全局描述符可能会受到几何混淆的影响。 --- 近期的研究开始探索基于局部结构约束的几何不变表示，例如二进制或三角形配置 [^32] [^33]，这些表示在宽基线及外观退化条件下表现出更强的鲁棒性。然而，仅依赖单一几何属性的描述符在复杂场景中仍可能缺乏区分能力。 除了回环检测外，保持激光雷达与视觉估计之间的跨模态一致性仍是一个未被充分探讨的问题。许多多传感器SLAM系统隐含地假设视觉与激光雷达的运动估计是一致的，但二者之间的差异可能会不断累积并损害全局优化效果。受基于创新的统计检验 [^34] 和自适应协方差建模 [^35] 启发的一致性感知策略，为动态调整测量置信度并提高估计器的可靠性提供了理论依据。 ### 3. 材料与方法 #### 3.1. 系统概述 在预处理阶段，来自惯性测量单元、激光雷达和摄像头的原始数据流通过基于STM32的定时板进行硬件同步（见图1），其中STM32微控制器由位于瑞士日内瓦的意法半导体公司提供，同步方案遵循FAST-LIVO2的描述[^8]。同步之后，惯性测量单元的数据用于状态传播，而激光雷达和视觉观测则分别提供几何与光度约束。这些测量数据随后会在考虑不确定性的iESKF前端中被紧密融合。为了提升在不同传感条件下的鲁棒性，我们引入了自适应动态协方差调整策略，根据三种互补的线索——即新息一致性（NIS）、特征匹配质量以及几何可观测性——来调节每种模态的测量不确定性。如此一来，估计器便能够根据各传感器的即时可靠性，动态平衡它们的贡献。 ![Image](images/3575caa6973fc8272969914c4b20deacaa214eb9d20b09578a4be13be88a6183.jpg)
Figure 1. Overall framework of the proposed uncertainty-aware LiDAR–inertial–visual SLAM system. The solid arrows indicate data flow and information propagation, whereas the hollow arrows denote the interaction between the front-end local estimation and the back-end optimization. The front-end performs synchronized multi-sensor preprocessing, motion compensation, bidirectional feature matching, photometric residual construction, and uncertainty-aware state update with adaptive dynamic covariance adjustment. The back-end jointly incorporates IMU, LiDAR, visual, cross-modal consistency, and mBTC-based loop closure factors to improve global consistency.
--- 基于所估计的状态，前端会进一步维护一个由地图点更新、参考块更新以及基于体素地图的姿态表示构成的局部地图。这种局部建图机制为后续的状态估计提供了稳定的几何与光度参考，从而增强了前端里程计的连续性与一致性。 为了提升长期一致性，后端会在因子图框架内执行全局优化。具体而言，该因子图综合了惯性测量单元因子、激光雷达因子、视觉因子、跨模态一致性因子以及mBTC回环因子。同时，系统采用基于mBTC的回环检测模块来识别重复出现的地点，并将由此产生的回环闭合约束用于回环校正与全局轨迹优化。与仅编码几何形状信息的传统单通道BTC描述符相比，所提出的mBTC通过引入额外的多维上下文线索来扩展描述符，从而在重复或部分退化环境中提升了回环检测的鲁棒性与区分度。 通过基于不确定性感知的iESKF前端估计、局部地图维护、基于mBTC的回环检测以及跨模态因子图优化的分层集成，所提出的框架将局部里程计估计与全局一致性优化统一在单一的多传感器SLAM系统中，从而在复杂环境中实现了更高的鲁棒性与精度。 #### 3.2. 不确定性感知的iESKF融合 所提出的前端采用了迭代误差状态卡尔曼滤波器（iESKF）。该滤波器在统一的概率框架下紧密融合激光雷达、视觉与惯性测量数据。它利用高频的惯性测量数据来传播状态，随后再利用激光雷达与视觉观测结果对状态进行校正。这种方法能够在保持SE(3)流形上的局部一致性的同时实现实时估计。 传统的紧耦合滤波器依赖于固定且各向同性的测量协方差。相比之下，我们的方法引入了不确定性感知的自适应协方差机制。该机制会根据实时的统计一致性、匹配质量以及几何可观测性，动态调整每种传感模态的置信度。 这种设计提升了系统在变化的传感条件下的鲁棒性。即便在光照变化、运动模糊、纹理较弱或激光雷达几何特性退化的情况下，系统仍能维持可靠的状态估计。 ##### 3.2.1. 状态表示与误差定义 系统状态定义如下： $$ \mathbf{x}=[\mathbf{R}_{w b},\mathbf{p}_{w},\mathbf{v}_{w},\mathbf{b}_{g},\mathbf{b}_{a}], $$ 其中，$\mathbf{R}_{wb} \in \text{SO}(3)$ 是将向量从机体坐标系映射到世界坐标系的旋转矩阵，$\text{SO}(3)$ 表示三维旋转群。向量$\mathbf{p}_w \in \mathbb{R}^3$ 和 $\mathbf{v}_w \in \mathbb{R}^3$ 分别表示机体在世界坐标系中的位置与速度。陀螺仪偏差$\mathbf{b}_g \in \mathbb{R}^3$ 和加速度计偏差$\mathbf{b}_a \in \mathbb{R}^3$ 均为三维向量。对应的15维误差状态可表示为： $$ \delta\mathbf{x}=[\delta\boldsymbol{\theta}^{\top},\delta\mathbf{p}^{\top},\delta\mathbf{v}^{\top},\delta\mathbf{b}_{g}^{\top},\delta\mathbf{b}_{a}^{\top}]^{\top}, $$ 其中，$\delta\theta \in \mathbb{R}^3$ 是在SO(3)的切空间内表示的旋转误差向量（也称为姿态误差）。此处采用右不变误差表述方式，以便通过流形上的指数映射来实现姿态更新： --- $$ \mathbf{R}_{w b}\leftarrow\hat{\mathbf{R}}_{w b}\exp(\delta\boldsymbol{\theta}), $$ 从而保持流形上的局部一致性。 ##### 3.2.2. IMU传播、测量模型与迭代ESKF更新 前端采用迭代误差状态卡尔曼滤波器（iESKF）实现紧密的多传感器融合。IMU测量数据用于高频状态传播，而激光雷达的点面残差与视觉光度残差则提供互补校正。连续时间惯性传播模型及线性化动力学过程遵循基于iESKF的SLAM系统中常用的标准误差状态表述方式 [^5] [^21] [^22]。 视觉与激光雷达测量模型分别被定义为光度残差和点面残差。这些异构测量值被堆叠成联合残差向量，并通过标准创新形式在iESKF框架内完成融合。更新过程会迭代进行直至收敛，以提高精度。 关于IMU传播、测量模型线性化、雅可比矩阵计算、协方差传播以及完整的迭代ESKF更新方程的详细数学推导，详见附录A。 每次校正之后，标称状态会在SE(3)流形上更新为： $$ \mathbf{R}_{w b}\leftarrow\hat{\mathbf{R}}_{w b}\exp(\delta\boldsymbol{\theta}),\quad\hat{\mathbf{p}}_{w}\leftarrow\mathbf{p}_{w}+\delta\mathbf{p},\quad\hat{\mathbf{v}}_{w}\leftarrow\mathbf{v}_{w}+\delta\mathbf{v}. $$ ##### 3.2.3. 自适应不确定性建模与权重调节机制 上一小节将标准iESKF更新中的多模态融合表述为最大后验估计问题，其中测量协方差矩阵$\mathbf{R}_k^m$决定了各模态对联合创新信息的相对贡献度。然而在实际环境中，由于光照变化、运动模糊、动态干扰、稀疏对应点以及几何退化等因素，视觉与激光雷达观测的质量会随时间剧烈变化。在此类情况下，固定的协方差假设可能导致统计失配以及各传感模态间置信度分配不当。 为解决这一问题，本小节提出了一种自适应动态协方差建模机制。各模态的不确定性被分解为三个互补来源：统计一致性、感知/匹配质量以及几何可观测性。这三个因素以乘积形式结合，从而形成一种具备可解释性、可控且易于扩展的统一调节策略。如此一来，iESKF更新中所用的协方差便不再固定，而是会根据各传感模态的瞬时可靠性进行自适应调节。 ## 统一自适应协方差公式：三因子乘积分解 直观而言，不同模态的测量数据并非在任何时候都能被同等程度地信任。例如，在图像模糊或光照不佳的情况下，视觉观测的可靠性会降低；而在几何退化场景中，激光雷达数据的有效性也会减弱。为了反映这种特性，我们利用三个互补的指标——创新一致性、测量质量和可观测性——来在线调整各模态的名义协方差。对于每种传感模态 $m \in \{v, \ell\}$，设 $\mathbf{R}_0^m$ 表示其名义协方差。自适应协方差是通过缩放调制模型来定义的： $$ \mathbf{R}_{k}^{m*}=\eta_{k}^{m}\mathbf{R}_{0}^{m},\qquad\eta_{k}^{m}=\eta_{\mathrm{N I S},k}^{m}\cdot\eta_{\mathrm{q u a l},k}^{m}\cdot\eta_{\mathrm{o b s},k}^{m}. $$ --- 联合自适应协方差可表示为 $$ \mathbf{R}_{k}^{*}=\mathbf{b l k d i a g}(\mathbf{R}_{k}^{v*},\mathbf{R}_{k}^{\ell*}), $$ 该自适应协方差会替换创新协方差 S 和卡尔曼增益计算中的原始协方差，从而能够在统一的滤波框架内实现多模态权重的在线重新分配 [^36]。 这种三因素分解具有明确的物理和统计意义。具体而言，$\eta_{NIS}$ 用于保证统计一致性并防止过度自信，$\eta_{qual}$ 反映了传感与匹配过程的可靠性，而 $\eta_{obs}$ 则体现了几何可观测性并抑制沿退化方向的虚假信息增益。这三者的乘积确保了任一维度的性能下降都能触发协方差膨胀，从而避免由于单个指标失效而导致的错误置信度赋值。 ## 由NIS和卡方检验驱动的统计一致性 在更新时刻 k，设 $\mathbf{r}_k^m$ 表示模态 m 的创新（残差）向量。其线性化后的创新协方差为 $$ \mathbf{S}_{k}^{m}=\mathbf{H}_{k}^{m}\mathbf{P}_{k|k-1}\mathbf{H}_{k}^{m^{\top}}+\mathbf{R}_{0}^{m}, $$ 其中 $\mathbf{H}_k^m$ 是测量雅可比矩阵，$\mathbf{P}_{k|k-1}$ 是预测状态协方差。归一化创新平方（NIS）定义为 [^34] [^37] $$ \mathrm{N I S}_{k}^{m}=\mathbf{r}_{k}^{m\top}\left(\mathbf{H}_{k}^{m}\mathbf{P}_{k|k-1}\mathbf{H}_{k}^{m\top}+\mathbf{R}_{0}^{m}\right)^{-1}\mathbf{r}_{k}^{m}. $$ 如果噪声模型和线性化假设在统计上成立，那么 $$ \begin{array}{r}{\mathrm{N I S}_{k}^{m}\sim\chi^{2}(d_{m}),\qquad\mathbb{E}[\mathrm{N I S}_{k}^{m}]=d_{m},}\end{array} $$ 这里 $d_m$ 是模态 m 的残差维度（自由度）。当 $\mathrm{NIS}_k^m$ 持续超出其期望值 $d_m$ 时，说明测量噪声被低估，或者模型失配及异常值干扰加剧。在这种情况下，若直接使用固定的协方差 $\mathbf{R}_0^m$，滤波器就会变得过度自信。 因此，统计一致性缩放因子被定义为 $$ \eta_{\mathrm{N I S},k}^{m}=\frac{\mathrm{N I S}_{k}^{m}}{d_{m}}. $$ NIS用于衡量当前残差幅度是否与假设的噪声模型一致。如果滤波器校准良好，NIS应接近其预期的卡方分布范围。若NIS持续偏大，则说明该模态的可靠性低于预期，可能是由于模型失配或异常值所致。此时，我们会增大其协方差并降低其更新权重。 信息矩阵与可观测性视角：对退化方向的抑制 仅依赖创新统计量在几何退化场景下可能仍不够。当 $\mathbf{H}_k^m$ 的列空间出现秩亏或条件数较差时——例如在平面结构、纯平移运动或纹理匮乏的视觉场景中——短期内的创新值可能看似很小，但虚假的信息增益仍可能沿可观测性较差的方向被引入。结果就是协方差在退化方向上出现不切实际地收缩。 --- 从信息矩阵的角度来看，模态m对状态的瞬时信息贡献为 $$ \mathcal{I}_{k}^{m}=\mathbf{H}_{k}^{m^{\top}}(\mathbf{R}_{k}^{m*})^{-1}\mathbf{H}_{k}^{m}, $$ 其频谱结构决定了对不同自由度的约束强度。当$\mathbf{H}_k^{m\top} \mathbf{H}_k^m$的特征谱发生坍塌时，系统在相应方向上会变得不可观测或弱可观测。此时应主动降低该模态的影响，以避免在简并子空间中产生过度自信。 受此观察启发，引入了考虑可观测性的调制因子[^38]： $$ \eta_{\mathrm{o b s},k}^{m}=\frac{1}{\log\det\left(\mathbf{H}_{k}^{m}{^\top}\mathbf{H}_{k}^{m}+\epsilon\mathbf{I}\right)}, $$ 其中$\epsilon > 0$是为了数值稳定性而设定的小正数。随着$\det(\mathbf{H}^\top \mathbf{H})$的减小，$\eta_{obs,k}^m$会增大，从而放大协方差并削弱几何简并情况下的测量权重。实践中，为避免因对数行列式值过小或病态而导致的数值不稳定，可观测性因子会被限制在有限范围内$[\eta_{min}, \eta_{max}]$，也可选用指数移动平均法进行平滑处理。从物理意义上讲，该项相当于对注入滤波器的可观测信息量进行调控，从而防止在约束较弱的方向上产生不切实际的高置信度。 ## 最大似然解释与数据质量感知 除了统计一致性和几何可观测性外，多模态观测在数据质量上也存在显著差异。典型的例子包括视觉跟踪失败、光照突变、运动模糊、激光雷达对应点稀疏以及ICP拟合效果不佳等。这些现象通常表现为匹配可靠性降低、残差分布尾部更重以及异常值比例更高。 为了将此类信息显式纳入协方差建模中，引入了模态质量评分$score_k^m$，并定义了考虑质量因素的调制因子： $$ \eta_{\mathrm{q u a l},k}^{m}=\frac{1}{\mathbf{s c o r e}_{k}^{m}+\epsilon}. $$ 此处，$score_k^m$是特定于模态的质量度量指标。对于视觉模态，它可由特征跟踪成功率、光度残差均方根以及图像梯度方差等统计数据构建；对于激光雷达模态，则可依据ICP适配度、有效对应点数量以及局部表面各向异性来表征。该项类似于基于IRLS的鲁棒优化中的加权机制[^39] [^40]；但与纯残差驱动的鲁棒权重不同，它是源于可解释的传感与匹配质量指标，因而能在残差污染完全传入更新步骤之前就实现可靠性调节。 为使自适应协方差调整机制具备可复现性与实用性，必须以数学形式明确给出模态质量评分$score_{k}^{m}$的定义。据此，本文采用了加权归一化融合模型： $$ \mathbf{score}_{k}^{m}=\sum_{i=1}^{N_{m}}w_{i}^{m}\phi_{i}^{m}(k),\qquad\sum_{i}w_{i}^{m}=1, $$ 其中$\phi_i^m(k) \in [0,1]$为第i个归一化的质量指标，$w_i^m$为其权重系数。 视觉模态质量评分。 对于视觉模态$m = v$，定义了以下三个质量指标： --- 1. 特征跟踪成功率： $$ \phi_{1}^{v}(k)=\frac{N_{\mathrm{t r a c k e d}}}{N_{\mathrm{d e t e c t e d}}}. $$ 2. 光度残差稳定性： $$ \phi_{2}^{v}(k)=\exp\left(-\frac{\mathrm{R M S}_{\mathrm{p h o t o}}^{2}}{\sigma_{\mathrm{p h o t o}}^{2}}\right), $$ 其中$RMS_{photo}$为当前帧的光度残差均方根。 3. 图像梯度信息量： $$ \phi_{3}^{v}(k)=\frac{\mathrm{V a r}(\nabla I)}{\mathrm{V a r}_{\mathrm{r e f}}}. $$ 因此，视觉质量评分被定义为 $$ \mathbf{score}_{k}^{v}=w_{1}^{v}\phi_{1}^{v}+w_{2}^{v}\phi_{2}^{v}+w_{3}^{v}\phi_{3}^{v}. $$ 激光雷达模态的质量评分。 对于激光雷达模态$m = \ell$，可构建以下三个质量指标： 1. ICP拟合优度： $$ \phi_{1}^{\ell}(k)=\exp\left(-\frac{\mathrm{R M S E}_{\mathrm{I C P}}^{2}}{\sigma_{\mathrm{I C P}}^{2}}\right). $$ 2. 有效对应点比例： $$ \phi_{2}^{\ell}(k)=\frac{N_{\mathrm{c o r r e s p o n d e n c e}}}{N_{\mathrm{p o i n t s}}}. $$ 3. 表面结构各向异性指标： $$ \phi_{3}^{\ell}(k)=\frac{\lambda_{3}}{\lambda_{1}+\lambda_{2}+\lambda_{3}}, $$ 其中$\lambda_{i}$是局部协方差矩阵的特征值，用于量化结构非简并的程度。 随后定义激光雷达质量评分如下： $$ \mathbf{score}_{k}^{\ell}=w_{1}^{\ell}\phi_{1}^{\ell}+w_{2}^{\ell}\phi_{2}^{\ell}+w_{3}^{\ell}\phi_{3}^{\ell}. $$ 最终的质量感知调制因子为： $$ \eta_{\mathrm{q u a l},k}^{m}=\frac{1}{\mathbf{s c o r e}_{k}^{m}+\epsilon}. $$ 当观测质量下降时，该评分会降低，而$\eta_{qual,k}^{m}$会增大，进而导致协方差膨胀，该模态对后验估计的贡献也会随之减小。 从最大似然的角度来看，协方差可被解释为未知的噪声尺度参数，记为$\mathbf{R}_k^{m*} = \eta_k^m \mathbf{R}_0^m$。当观测质量降低时，合理的最大似然策略往往会增大噪声尺度，以使质量较差的观测仍能得到统计上的合理解释。从这个意义上说，式(13) --- 可被视为引入了一种由数据质量驱动的噪声尺度先验调整机制。它与基于NIS的在线尺度校正机制互为补充：$\eta_{NIS}$用于捕捉结果层面的统计偏差，而$\eta_{qual}$则用于捕捉过程层面的性能退化。 融合权重调节的统一解释与实现考量 综上所述，三因素模型起到了传感器可靠性调度器的作用：$\eta_{NIS}$反映统计一致性，$\eta_{qual}$体现传感可靠性，$\eta_{obs}$则抑制在约束较弱方向上的过度置信。三者的综合作用能够实现更均衡、更稳健的多模态融合。 一旦某个模态在上述任一维度上出现性能退化，$\eta_{k}^{m}$就会增大，进而使$\mathbf{R}_{k}^{m*}$膨胀，相应地削弱信息贡献$\mathcal{I}_{k}^{m}$。最终，所提出的机制能够以在线且符合原理的方式实现多模态融合权重的自适应重新分配。本质上，它将传感可靠性的自我评估循环嵌入到前端滤波器中，使系统能够在不断变化的环境和运动条件下持续重新平衡异构观测数据。 需要强调的是，这种自适应协方差机制主要通过在滤波器更新过程中调节模态置信度来提升前端融合的局部统计可靠性。不过，仅靠协方差重加权并不能显式约束不同传感模态在轨迹层面的长期一致性，也无法直接消除长时间运行产生的累积漂移。因此，为了进一步提升全局一致性，后续的后端模块会引入额外的几何闭环约束与跨模态一致性建模，从而在一个统一的SLAM框架内同时实现局部可靠的估计与全局一致的优化。 ### 3.3. mBTC：一种稳定的几何骨架驱动型多通道结构表示框架 在重复性或特征模糊的环境中，仅由单一类型几何统计信息构成的描述符往往缺乏足够的区分能力，这会降低全局重定位中环路检测的可靠性。为解决这一问题，本节提出了一种多通道二进制三角约束（mBTC）框架，该框架通过将稳定的几何骨架与互补的上下文通道相结合来增强位置表示能力。 给定原始观测空间$\mathcal{X}$后，位置识别可抽象为如下映射关系： $$ f:\mathcal{X}\to\mathcal{D}, $$ 其中$\mathcal{D}$表示用于检索和匹配的描述符空间。理想的描述符应在保持几何不变性的同时，尽可能保留更多具有区分度的位置信息。然而，在传统的单通道建模中，描述符通常仅由一类几何统计信息构成， $$ \mathbf{d}=\Bigl[d^{(1)},d^{(2)},\ldots,d^{(C)}\Bigr], $$ 此类通道之间往往高度相关或部分冗余。在这种情况下，有效的区分维度会受到限制，且位置区分信息S也无法显著优于占主导地位的单个通道所能提供的效果。 为克服这一局限，所提出的mBTC框架通过多个具备物理可解释性且统计上互补的通道来扩展描述符空间。mBTC并非仅编码局部结构的几何形状，而是将每个候选环路单元表示为稳定的基于三角形的几何骨架与一组上下文属性的组合。这种设计在保持刚体不变性的同时—— --- 还能提升描述符空间的有效秩，从而在复杂场景中提高区分能力。 #### 3.3.1. 多通道上下文建模 尽管三角形骨架能提供刚体不变的几何结构，但仅靠形状信息在重复性或不具明显特征的环境中往往不足以实现有效区分。为提高区分能力，mBTC会为每个稳定的三角形补充一组从其局部支撑区域中提取的上下文通道。 设重力对齐后的激光雷达点云表示为： $$ \mathcal{P}=\Big\{\mathbf{p}=[x,y,z]^{\top},\:I(\mathbf{p})\Big\}, $$ 其中z为重力方向，$I(\mathbf{p})$为激光雷达强度。对于由顶点$\Delta_{ijk}$构成的三角形$\mathbf{p}_i, \mathbf{p}_j, \mathbf{p}_k$，每个顶点周围会定义一个半径为r的局部邻域： $$ \mathcal{N}(u)=\{\mathbf{p}\in\mathcal{P}\mid\|\mathbf{p}-\mathbf{p}_{u}\|B. The Proposed Multi-Channel mBTC Descriptor
Figure 2. Comparison of the conventional single-channel Binary Triangle Constraint (BTC) descriptor and the proposed multi-channel Binary Triangle Constraint (mBTC) descriptor. The arrows indicate the extension from a single geometric channel to a multi-channel structural representation. (A) Conventional BTC encodes only the geometric scale information of the triangle, which is invariant under rigid-body transformations. (B) The proposed mBTC extends the same geometric skeleton with four additional contextual channels: vertical span $\Delta h$, spatial compactness $\rho$, surface irregularity $\nu$, and reflectance intensity $\bar{I}$. These multi-channel attributes enhance descriptor discriminability and robustness in repetitive or viewpoint-variant environments.
因此，一个三角形的完整连续表示可以写为 $$ \mathbf{f}_{i j k}=\left[\begin{matrix}{\mathbf{g}_{i j k}}\\ {\mathbf{q}_{i j k}}\\ \end{matrix}\right], $$ --- 其中 $\mathbf{g}_{ijk}$ 表示几何骨架属性，$\mathbf{q}_{ijk}$ 表示上下文通道。由于这些通道描述了局部环境的不同物理特性，它们通常相关性较弱，却能共同提升描述符空间的有效秩。这种统计上的互补性使得mBTC能够更好地区分几何布局相似但局部结构或辐射特性不同的位置。 最终，三角形 $(i, j, k)$ 的多通道特征向量定义为： $$ \mathbf{f}_{i j k}=\left[\tilde{l}_{1},\tilde{l}_{2},\tilde{l}_{3},\tilde{h}_{i j k},\tilde{\rho}_{i j k},\tilde{v}_{i j k},\tilde{I}_{i j k}\right]^{\top}\in[0,1]^{7}, $$ 其中前三个元素是来自BTC的几何不变量，后四个则是上下文属性。这种混合形式使mBTC不仅能编码三角形的内在形状几何信息，还能反映局部环境结构和材料响应。 通过后续的量化与编码步骤，这些多通道连续特征被转化为紧凑的二进制码，既实现了高效的大规模检索，又保留了关键的区分性信息。如此一来，mBTC在几何不变性、描述丰富度与匹配效率之间取得了平衡，非常适合在长期且大规模SLAM场景中进行鲁棒的回环检测。 ##### 3.3.2. 二进制编码与描述符生成 在每个三角形顶点处构建好多通道局部几何上下文后，需要一种高效的编码方案来将这些连续描述符转化为适合大规模回环检测的紧凑且具区分性的表示。本节将介绍二进制编码与描述符生成过程，该过程能将归一化的多通道属性转换为统一的二进制码，同时保留其相对几何关系。采用二进制编码是为了降低内存消耗，并通过位运算实现快速相似性计算，这对于长期SLAM中的实时回环检测至关重要。为了保留BTC的紧凑性并实现高效的回环检索，连续特征向量 $f_{ijk}$ 会被量化为二进制码，并聚合为帧级描述符。 mBTC的二进制表示与检索过程通过以下三个步骤实现： ### 1. 自适应量化 对于每个特征维度 $n \in \{1, \ldots, 7\}$，自适应阈值 $\{\tau_{n,q}\}$ 是根据当前帧内所有三角形的分位数确定的。 特征值 $f_{ijk}^{(n)}$ 会被量化为 $$ q_{n}(i,j,k)=\sum_{q=1}^{Q-1}\mathbb{I}\Big\{f_{i j k}^{(n)}>\tau_{n,q}\Big\},\qquad\mathbf{g}_{n}=\operatorname{G r a y}(q_{n})\in\{0,1\}^{\lceil\log_{2}Q\rceil}, $$ 格雷编码确保了汉明空间中的局部连续性。所有编码后的通道被拼接起来，形成mBTC二进制向量： $$ {\bf{b}}_{i j k}=\left[{\bf{g}}_{1}\parallel{\bf{g}}_{2}\parallel\cdots\parallel{\bf{g}}_{7}\right]\in\left\{0,1\right\}^{B},\qquad B=7\left\lceil\operatorname{l o g}_{2}Q\right\rceil. $$ ### 2. 直方图池化 直方图池化将局部二进制特征聚合为固定大小的帧级表示，通过词频-逆文档频率（TF-IDF）[^41]重加权来抑制重复结构的影响。这使得场景区分和相似性检索更加稳健，对于实时闭环检测而言至关重要。 --- 每个二进制码$\mathbf{b}_{ijk}$都通过局部敏感哈希函数$\phi(\cdot)$映射到一个离散索引，随后累加得到帧级直方图描述符： $$ \mathbf{h}_{t}=\sum_{(i,j,k)\in\mathcal{T}_{t}}\mathbf{e}_{\phi(\mathbf{b}_{i j k})},\qquad\mathbf{\hat{h}}_{t}=\frac{\mathbf{h}_{t}}{\|\mathbf{h}_{t}\|_{1}}. $$ 该加权方案遵循信息检索中的TF-IDF原则，即降低那些出现频繁但信息量较少的模式权重： $$ \tilde{\mathbf{h}}_{t}=\frac{\mathbf{w}\odot\hat{\mathbf{h}}_{t}}{\|\mathbf{w}\odot\hat{\mathbf{h}}_{t}\|_{1}},\qquad w_{k}=\log\frac{N}{1+\mathbf{d f}_{k}}. $$ ### 3. 相似性计算与快速匹配 经过TF-IDF重加权后，帧描述符便可用于全局相似性计算。在本研究中，采用余弦相似性作为相似性度量指标： $$ \mathrm{S i m}(c,r)=\frac{\mathbf{h}_{c}^{\top}\mathbf{h}_{r}}{\|\mathbf{h}_{c}\|_{2}\|\mathbf{h}_{r}\|_{2}}, $$ 其中c和r分别表示当前帧和候选帧。 #### 3.3.3. 闭环检索与验证 运行时，当前帧描述符$\tilde{\mathbf{h}}_{c}$会与闭环数据库中存储的历史描述符$\{\tilde{\mathbf{h}}_{r}\}$进行比较。排名靠前的闭环候选结果会通过点对面ICP几何配准进行验证，从而生成闭环约束。 $$ E_{\mathrm{l o o p}}=\mathbf{e}_{\mathrm{l o o p}}^{\top}\mathbf{\Omega}_{\mathrm{l o o p}}\mathbf{e}_{\mathrm{l o o p}},\qquad\mathbf{e}_{\mathrm{l o o p}}=\mathrm{L o g}\bigg(\left(\mathbf{T}_{i j}^{\mathrm{l o o p}}\right)^{-1}\mathbf{T}_{i w}^{-1}\mathbf{T}_{j w}\bigg), $$ 其中$\mathbf{T}_{ij}^{\text{loop}}$表示ICP估算的帧i与帧j之间的相对位姿，$\Omega_{\text{loop}}$则源自配准协方差。确认后的闭环会被插入全局因子图，以修正累积的漂移并保持长期的全局一致性。 这种考虑可靠性的闭环机制在性能上优于仅依赖外观的检索方法（如Scan Context），尤其是在几何退化或视觉挑战较大的环境中。 #### 3.4. 跨模态一致性因子图优化 为了实现全局一致的轨迹估计，所提出的框架引入了联合因子图优化模块，整合惯性、视觉、激光雷达以及闭环约束。不过，单独集成各模态的里程计因子可能会导致局部不一致，尤其在传感器暂时性能下降时更为明显。为缓解这种模态漂移问题，我们引入了跨模态一致性因子，以明确强制视觉与激光雷达的相对运动保持一致。 ##### 3.4.1. 因子图构建 设$\mathbf{T}_{wb,k} \in SE(3)$表示关键帧k处的机体位姿，$\mathbf{T}_{bc}, \mathbf{T}_{b\ell} \in SE(3)$分别代表相机与激光雷达已知的外部标定参数。后端优化通过最小化因子图上定义的所有残差之和，来寻求最大后验概率（MAP）估计值： --- $$ \begin{aligned}\mathcal{L}^{*}=\arg\min_{\left\{\mathbf{T}_{\mathrm{wb},k}\right\}}\Big(\sum_{(i,j)\in\mathcal{E}_{\mathrm{imu}}}\Big\|\mathbf{e}_{\mathrm{imu}}^{ij}\Big\|_{\Omega_{\mathrm{imu}}}^{2}+\sum_{(i,j)\in\mathcal{E}_{v}}\Big\|\mathbf{e}_{v}^{ij}\Big\|_{\Omega_{v}}^{2}+\sum_{(i,j)\in\mathcal{E}_{\ell}}\Big\|\mathbf{e}_{\ell}^{ij}\Big\|_{\Omega_{\ell}}^{2}\\+\sum_{(i,j)\in\mathcal{E}_{\mathrm{loop}}}\Big\|\mathbf{e}_{\mathrm{loop}}^{ij}\Big\|_{\Omega_{\mathrm{loop}}}^{2}+\sum_{(i,j)\in\mathcal{E}_{\mathrm{cm}}}\Big\|\mathbf{e}_{\mathrm{cm}}^{ij}\Big\|_{\Omega_{\mathrm{cm}}}^{2}\Big)\end{aligned} $$ 此处，$\mathcal{E}_{\mathrm{imu}}$、$\mathcal{E}_{v}$、$\mathcal{E}_{\ell}$、$\mathcal{E}_{\mathrm{loop}}$ 和 $\mathcal{E}_{\mathrm{cm}}$ 分别表示IMU预积分、视觉重投影、激光雷达点面匹配、回环闭合以及跨模态约束的集合；$\Omega$ 代表各因子对应的信息矩阵。如图3所示，后端优化在统一的因子图框架内，将IMU、激光雷达、视觉、回环闭合以及跨模态一致性因子整合在一起。 ![Image](images/f3b9d9cdda124c3a2e68f0819efa2ce6dfa7fe0217b4efe41ae8f7e47cd76df0.jpg)
Figure 3. Overview of the factor-graph optimization back-end with the proposed cross-modal consistency factor. The graph jointly incorporates: IMU factors (red), LiDAR-Inertial Odometry (LIO) factors (blue), Visual-Inertial Odometry (VIO) factors (green), the proposed cross-modal consistency factors (yellow), and loop closure factors (purple). Green hexagons indicate loop detection results, and purple loop factors are added when a revisited place is detected. The cross-modal consistency factors (yellow) explicitly enforce short-baseline motion agreement between visual and LiDAR odometries, bridging front-end estimation and global optimization.
##### 3.4.2. 相对运动约束建模 对于关键帧对 $(i,j)$，前端会提供来自相机和激光雷达的相对位姿测量结果： $$ \hat{\mathbf{T}}_{i j}^{c},~\hat{\mathbf{T}}_{i j}^{\ell}\in\mathrm{S E}(3), $$ 其对应的协方差分别为 $\Sigma_{ij}^{c}$ 和 $\Sigma_{ij}^{\ell}$。 根据当前轨迹估计得出的预测相对运动为： $$ \mathbf{T}_{i j}^{c}=\mathbf{T}_{b c}^{-1}\mathbf{T}_{w b,i}^{-1}\mathbf{T}_{w b,j}\mathbf{T}_{b c},\qquad\mathbf{T}_{i j}^{\ell}=\mathbf{T}_{b\ell}^{-1}\mathbf{T}_{w b,i}^{-1}\mathbf{T}_{w b,j}\mathbf{T}_{b\ell}. $$ 随后，视觉与激光雷达里程计残差被表述为： $$ \mathbf{e}_{v}^{i j}=\operatorname{l o g}\big((\hat{\mathbf{T}}_{i j}^{c})^{-1}\mathbf{T}_{i j}^{c}\big),\qquad\mathbf{e}_{\ell}^{i j}=\operatorname{l o g}\big((\hat{\mathbf{T}}_{i j}^{\ell})^{-1}\mathbf{T}_{i j}^{\ell}\big). $$ ##### 3.4.3. 跨模态一致性因子 尽管视觉与激光雷达里程计提供了独立的相对运动估计，但它们描述的是机器人在同一关键帧区间内相同的物理运动。我们并未引入额外的绝对运动测量值，而是定义了 --- 一个软性一致因子，用于在 $\mathfrak{s}\epsilon(3)$ 局部误差空间中惩罚两种模态特定相对运动残差之间的差异。 $$ \mathbf{e}_{cm}^{ij}=\mathbf{e}_{v}^{ij}-\mathbf{e}_{\ell}^{ij}, $$ 其中，$\mathbf{e}_{v}^{ij}$ 和 $\mathbf{e}_{\ell}^{ij}$ 表示根据3.4.2节中预测的相对运动得出的视觉与激光雷达里程计残差。 相应的马氏代价定义为 $$ E_{i j}^{c m}=({e}_{i j}^{c m})^{\top}\Omega_{i j}^{c m}{e}_{i j}^{c m}, $$ 在此处，基于视觉与激光雷达相对运动的不确定性在线性化误差空间中相互独立这一常见近似，它们残差差的协方差可近似为两个协方差项之和。据此，信息矩阵被定义为 $$ \Omega_{i j}^{c m}=(\Sigma_{i j}^{c}+\Sigma_{i j}^{\ell})^{-1}. $$ 上述公式自然反映了两种模态的置信水平：当视觉跟踪性能下降（例如出现运动模糊或纹理较少的情况）时，其不确定性会增加，从而在一致性因子中的影响减弱。相反，在几何特征退化的激光雷达环境中（如平面走廊），视觉模态能提供更强的约束。因此，当某一模态的不确定性较高时，一致性因子的影响力会降低；而当两种模态均能提供有效信息时，它仍能促使两者保持一致。 ##### 3.4.4. 线性化与雅可比矩阵结构 我们采用右乘扰动模型来更新位姿： $$ \mathbf{T}_{{w b},k}\leftarrow\mathbf{T}_{{w b},k}\operatorname{e x p}(\delta\xi_{k}),\qquad\delta\xi_{k}\in\mathfrak{s e}(3). $$ 围绕当前估计值对视觉与激光雷达残差进行线性化处理，可得： $$ \begin{array}{r}{\mathbf{e}_{v}^{i j}\approx\mathbf{e}_{v}^{i j,0}+\mathbf{J}_{v,i}^{i j}\delta\boldsymbol{\xi}_{i}+\mathbf{J}_{v,j}^{i j}\delta\boldsymbol{\xi}_{j},}\end{array} $$ $$ \begin{array}{r}{\mathbf{e}_{\ell}^{i j}\approx\mathbf{e}_{\ell}^{i j,0}+\mathbf{J}_{\ell,i}^{i j}\delta\boldsymbol{\xi}_{i}+\mathbf{J}_{\ell,j}^{i j}\delta\boldsymbol{\xi}_{j}.}\end{array} $$ 由于跨模态误差被定义为两个里程计残差之差，其一阶线性化结果为： $$ \begin{array}{r}{\mathbf{e}_{\mathrm{c m}}^{i j}\approx\mathbf{e}_{\mathrm{c m}}^{i j,0}+\mathbf{J}_{\mathrm{c m},i}^{i j}\delta\boldsymbol{\xi}_{i}+\mathbf{J}_{\mathrm{c m},j}^{i j}\delta\boldsymbol{\xi}_{j},\qquad\mathbf{e}_{\mathrm{c m}}^{i j,0}=\mathbf{e}_{v}^{i j,0}-\mathbf{e}_{\ell}^{i j,0},}\end{array} $$ $$ \mathbf{J}_{\mathrm{c m},i}^{i j}=\mathbf{J}_{v,i}^{i j}-\mathbf{J}_{\ell,i}^{i j},\qquad\mathbf{J}_{\mathrm{c m},j}^{i j}=\mathbf{J}_{v,j}^{i j}-\mathbf{J}_{\ell,j}^{i j}. $$ 跨模态因子的雅可比矩阵可直接通过视觉与激光雷达里程计因子中已有的雅可比矩阵相减得到，无需进行额外的符号推导或重新计算。 所提出的跨模态一致性因子能够有效将视觉与激光雷达里程计约束在统一的误差空间中，在抑制模态依赖的漂移的同时，兼顾两者各自的不确定性。这种联合约束能够生成更平滑的轨迹，提升在单模态性能退化情况下的鲁棒性，并增强全局一致性，尤其在长期导航任务中表现更为突出。 --- ### 4. 实验结果 所有方法均采用C++实现，运行环境为搭载Intel Core i7-10750H CPU和24 GB内存、安装Ubuntu 20.04与ROS Noetic的平台。对于各类基线方法，我们采用了其公开可用的实现代码，并尽可能保留推荐的默认参数设置。 对于所提出的方法，除非另有说明，主要实现相关参数在所有数据集上均保持一致。在不确定性感知融合模块中，协方差缩放因子的取值范围被限定在$[\eta_{\min},\eta_{\max}]=[0.5,5.0]$之内，而与可观测性相关的缩放项则通过系数为$\alpha=0.8$的指数移动平均法进行平滑处理。在质量感知调制环节，视觉质量权重被设为$(w_1^v,w_2^v,w_3^v)=(0.4,0.3,0.3)$，激光雷达质量权重被设为$(w_1^\ell,w_2^\ell,w_3^\ell)=(0.4,0.3,0.3)$。数值稳定性常数则固定为$\epsilon=10^{-6}$。 在回环检测环节，选取前K=10个检索候选结果；只有当两帧之间的时间间隔超过50帧时，才会将其判定为有效的回环对，以避免出现相邻的 trivial 匹配情况。在精确率-召回率评估中，若两帧之间的真实位姿欧几里得距离小于5米，则被视为真实的回环对。除非有明确说明，否则所提出的方法不会针对特定数据集进行参数调优。 为了评估所提出的框架，我们在四个公开的多传感器数据集上开展了实验：KITTI [^42]、M2DGR [^43]、M3DGR [^44]以及NCLT [^45]。这些数据集涵盖了室内与室外环境、动态场景、重复结构以及长期轨迹，提供了同步采集的激光雷达、视觉与惯性测量数据，可用于对激光雷达-惯性-视觉SLAM系统进行评估。 #### 4.1. 回环检测性能 为了评估所提出的多通道几何结构描述符（mBTC）在回环检测中的检索性能，我们首先在多个公开数据集上开展了对比实验。经典的激光雷达回环检测方法STD以及基于三角形几何的BTC描述符被选作基线方法。我们通过精确率-召回率（P–R）曲线、Recall@N以及平均精确率（AP）对这些方法的检索性能进行了全面评估。 实验在KITTI Odometry、NCLT和M2DGR三个数据集上进行。对于每一帧激光雷达扫描数据，系统会从历史数据库中检索出最相似的候选帧，随后根据真实轨迹判断这些检索结果是否属于真实的回环闭合。具体而言，若两帧之间的空间距离小于预设阈值，则被视为真实的回环对。 图4展示了不同方法在所有测试序列上的精确率-召回率曲线。可以看出，所提出的mBTC在所有数据集上均表现出更优的性能。与STD相比，mBTC即便在高召回率区域仍能保持更高的精确率，这表明所提出的描述符具有更强的判别能力，能够有效减少因结构相似场景导致的错误匹配。与单通道几何描述符BTC相比，mBTC的曲线总体上更偏向右上区域，说明引入多通道结构信息提升了闭环检测的稳定性与可靠性。 为了进一步量化不同方法的检索性能，表1报告了不同N值下的召回率@N数值和平均精确率（AP）。召回率@N表示在Top-N检索结果中至少包含一个正确闭环的概率，直接反映了描述符的检索能力。如表所示，mBTC在所有评估数据集上均取得了最高的Recall@1和Recall@10。例如，在KITTI数据集上，mBTC的Recall@1较STD提升了超过10%，同时相较于BTC也展现出明显优势。这一结果表明，引入多通道几何上下文信息能够增强描述符刻画环境中结构差异的能力。AP结果进一步证实了mBTC在整体检索性能上的优越性。由于AP综合考虑了不同阈值下的精确率和召回率，因此能更全面地评估描述符的性能。在三个数据集上，mBTC均取得了最高的AP值，证明了其在不同场景结构和环境条件下稳定可靠的检索性能。 --- 与STD相比，mBTC的Recall@1提升了超过10%，同时相较于BTC也展现出明显优势。这一结果表明，引入多通道几何上下文信息能够增强描述符刻画环境中结构差异的能力。AP结果进一步证实了mBTC在整体检索性能上的优越性。由于AP综合考虑了不同阈值下的精确率和召回率，因此能更全面地评估描述符的性能。在三个数据集上，mBTC均取得了最高的AP值，证明了其在不同场景结构和环境条件下稳定可靠的检索性能。 ![Image](images/53f31950d76213989a0129bf90a02fddca2597f399eb5f490e5755b0da18aa71.jpg)
Figure 4. PR curves on KITTI, NCLT and M2DGR datasets.
Table 1. Recall@N and AP summaries on KITTI00 and NCLT1 datasets.
DatasetMethodsRecall@1Recall@5Recall@10Recall@20AP
KITTI00STD0.750.850.890.920.80
BTC0.820.900.930.950.86
mBTC0.880.940.960.970.91
NCLT1STD0.780.860.900.930.82
BTC0.830.910.940.960.88
mBTC0.870.930.950.970.90
mBTC的性能提升主要源于两个方面。首先，由稳定的三角形结构构建的几何骨架具备对视角变化和刚体变换的不变性，从而提高了匹配稳定性。其次，引入的多通道结构属性，包括垂直结构分布、点云密度、表面不规则性以及反射强度，为每种几何结构提供了更丰富的上下文信息，使描述符能够更好地区分结构重复的场景。 #### 4.2. 关于mBTC通道设计的消融研究 为了进一步验证所提出的多通道几何结构表征在闭环检测中的有效性，本小节针对不同的编码策略开展消融研究，系统分析不同几何信息通道组合对描述符性能的影响。 --- 通过逐步引入不同的几何属性，并比较不同编码策略下的闭环检测性能，可以定量评估每个几何通道对结构表征的贡献。为了分析不同通道对整体性能的贡献，实验采用了以下编码策略的渐进累积设计： - A0（仅三角形）：仅使用基本的三角形结构信息进行编码； • A1（+高度）：在三角形结构基础上加入高度信息； - A2（+密度）：进一步纳入点云密度信息； A3（+法线）：加入表面法线信息； A4（+强度）：进一步引入了激光雷达反射强度信息； - A5（所有通道）：将所有几何信息通道进行联合融合。 实验在KITTI里程计数据集和NCLT1序列上进行。为了全面评估回环检测性能，采用了两项指标：Recall@N（N=1、5、10、20）和平均精度（AP）。Recall@N衡量的是在前N个候选结果中成功检索到至少一个真实回环闭合的查询比例，而AP则反映了精度与召回率之间的整体权衡。 从表2可以看出，随着纳入的几何通道数量增加，检索性能也稳步提升。在KITTI数据集上，Recall@10从A0的0.91提升至A5的0.96，AP则从0.85提高到0.91；在NCLT数据集上，Recall@10从0.89提升至0.94，AP从0.80提高到0.88。这些结果表明，每个新增通道都能为结构表示提供有用的补充信息。
Table 2. Comparison of Recall and AP performance for different channel combinations on the KITTI and NCLT datasets.
Channel CombinationRecall@1Recall@5Recall@10Recall@20AP
KITTI 00
Only Triangle (A0)0.820.890.910.940.85
+Height (A1)0.830.900.920.950.87
+Density (A2)0.840.910.930.960.88
+Normal (A3)0.850.920.940.970.89
+Intensity (A4)0.860.930.950.970.90
All Channels (A5)0.880.940.960.970.91
NCLT
Only Triangle (A0)0.790.870.890.920.80
+Height (A1)0.810.880.910.930.83
+Density (A2)0.820.890.920.940.84
+Normal (A3)0.830.900.930.950.86
+Intensity (A4)0.840.910.940.960.87
All Channels (A5)0.850.920.940.960.88
具体而言，高度、密度和法线通道通过提供垂直、空间和局部几何线索来丰富描述符，从而带来稳定的性能提升；而强度通道则通过捕捉表面间的反射差异进一步提高了区分能力。最佳结果由A5取得，即所有通道联合融合后的效果，这表明所提出的多通道表示方法相比单通道编码，能显著提升描述符的区分能力和鲁棒性。 消融实验结果证实，所提出的mBTC框架确实能从多通道几何信息中获益，并在不同数据集上展现出更好的稳定性和泛化能力。 --- 虽然增加通道数量能提升描述符的区分能力和检索精度，但也会在通道提取、二进制编码和直方图构建环节引入额外的计算开销。在我们的实现中，运行时间从A0到A5逐步增加，但由于回环检测的执行频率低于前端状态估计，因此开销仍处于可控范围。具体而言，与仅基于三角形的基线相比，全多通道配置（A5）的运行时间仅增加约15%~20%，却在KITTI和NCLT数据集上均带来了明显的Recall@N和AP提升。额外的成本主要源于上下文属性（高度、密度、法线和强度）的提取以及描述符维度的增加。对于实际的回环闭合应用而言，这种权衡是可接受的——毕竟鲁棒性和检索可靠性比每帧描述符的最低计算成本更为关键。 #### 4.3. 关于融合与一致性机制的消融研究 ##### 4.3.1. 自适应不确定性建模分析 为了评估所提出的自适应协方差调节机制的有效性，我们考虑了两种典型的退化场景，并分析了三个缩放因子$\eta_{NIS}$、$\eta_{qual}$和$\eta_{obs}$的时间变化规律。其中，$\eta_{NIS}$用于衡量创新量的统计一致性，$\eta_{qual}$反映观测质量，而$\eta_{obs}$则描述几何可观测性。 第一种场景是室外走廊-室外序列，系统从开阔区域进入走廊后再返回室外。由于走廊环境通常具有强烈的结构重复性和有限的横向约束，该场景适合用于评估所提方法在几何退化条件下的响应情况。第二种场景是夜间有车灯干扰的情况，车辆大灯引起的局部光照变化会显著影响视觉匹配质量和光度残差统计特性。该场景用于分析方法在视觉退化条件下的动态响应。 如图5所示，在开阔的室外区域中，这三个因子均保持较低且稳定的水平，表明激光雷达和视觉约束可靠。当系统进入走廊后，$\eta_{obs}$显著增大，说明几何可观测性大幅降低。同时，$\eta_{NIS}$也上升，表明创新统计量偏离理论预期。相比之下，$\eta_{qual}$的变化幅度很小，这说明该场景下主要的退化源是几何可观测性下降而非匹配质量差。一旦系统返回室外环境，这三个因子便逐渐降至稳定水平，证明所提机制能在环境再次变得信息丰富时自动减小协方差。 图6展示了夜间车灯场景下的因子变化情况。在正常的弱光条件下，这三个因子处于较高水平，因为微弱的光照限制了图像对比度和特征稳定性。当车灯照亮场景时，局部纹理和结构细节变得更清晰，导致$\eta_{qual}$明显减小，这表明匹配质量有所改善。同时，$\eta_{NIS}$减小，显示出更好的统计一致性，$\eta_{obs}$也略有下降，说明可观测性提高。当光照恢复到原来的夜间状态时，所有因子又会上升。这些结果证明，所提出的自适应不确定性建模机制既能在退化条件下抑制不可靠的信息，又能在感知条件改善时自动提高观测置信度。 --- ##### 4.3.2. 通过跨模态一致性因子抑制漂移 为了验证所提跨模态一致性因子的有效性，开展了对比实验，以分析在复杂条件下视觉里程计与激光雷达里程计之间的漂移分离情况。
Time Curves of Covariance Scaling Factors Under Degradation
![Image](images/e8e68be4c985d9660dbd1c60069f8cb5f81270e5da4f22a3b9a71f198391ee64.jpg)
Figure 5. Time curves of covariance scaling factors under degraded observation conditions. The blue, orange, and green curves represent the NIS-based factor, quality-based factor, and observability-based factor, respectively. The light-blue shaded region indicates the degraded observation interval considered in this example.
Adaptive Covariance Scaling Factors in Night Scene with Temporary Headlight Illumination
![Image](images/1c77c251079250a7274ba72b7fc9471a61bf2367ebacb04b09bc4b195da3185e.jpg)
Figure 6. Time curves of covariance scaling factors in the nighttime scene with headlight interference. The blue, orange, and green curves represent the NIS-based factor, quality-based factor, and observability-based factor, respectively. The light-blue shaded region indicates the headlight illumination segment considered in this example.
--- 在多模态SLAM中，传感模型和噪声特性的差异可能会逐渐导致视觉与激光雷达模态之间的估计不一致，尤其是在动态场景、弱纹理环境和转弯运动中。为评估这种效应，对比了三种配置： - 基线-0：仅包含激光雷达、视觉和惯性测量单元因子； - Baseline-2：在Baseline-0的基础上加入回环闭合约束； Ours-Full：在Baseline-2的基础上进一步加入所提出的跨模态一致性因子。 跨模态相对运动误差定义为 $$ e_{c m}(k)=\left\|\log\Big((\mathbf{T}_{k}^{L})^{-1}\mathbf{T}_{k}^{V}\Big)\right\|, $$ 其中$\mathbf{T}_k^L$和$\mathbf{T}_k^V$分别表示时间k时的激光雷达和视觉相对位姿估计。$e_{cm}(k)$的值越大，说明跨模态漂移越严重。 图7显示了$e_{cm}(k)$随时间的变化情况，表3则给出了相应的统计数据。
Cross-modal Relative Motion Inconsistency
![Image](images/b835ef0ed6777e5a8528588d7ac8d7f49ac634417abf384b8825598540837aae.jpg)
Figure 7. Time curve of cross-modal motion inconsistency $e_{cm}(k)$. The light-blue shaded region highlights the challenging interval used for detailed analysis, including the ground-facing and loop-closure regions.
Table 3. Cross-modal consistency error statistics.
MethodMean $e_{cm}$95% $e_{cm}$
Without cross-modal0.0013590.002082
With loop only0.0012940.002028
Ours (with cross-modal)0.0011400.001432
如图7所示，序列初期跨模态误差仍较小，这表明视觉与激光雷达里程计之间具有良好的一致性。当机器人进入视觉退化区域时，视觉估计会逐渐累积漂移，导致$e_{cm}(k)$增大。若没有所提出的跨模态因子，这种误差会在较长时间内保持较高水平。相比之下，借助跨模态一致性约束，视觉状态会持续受到激光雷达观测的校正，从而有效抑制误差的增长。经过回环闭合和全局优化后，累积的不一致性进一步降低。 --- 表3显示，所提方法的平均误差和95百分位误差均最低，分别降低了约16.1%和31.2%。这些结果证实，所提出的跨模态一致性因子确实能有效提升多模态状态估计的一致性与稳定性。 #### 4.4. 整体系统性能与建图结果 ##### 4.4.1. 基于M2DGR数据集的定量评估 表4报告了在未经回环闭合处理的M2DGR数据集上的定量定位结果。评估指标为绝对轨迹误差（ATE），单位为米。对比的方法包括VINS-Mono、A-LOAM、LIO-SAM、LVI-SAM以及所提出的方法。
Table 4. Localization accuracy (ATE, meters) on the M2DGR dataset without loop closure.
MethodWalk_01Hall_02Door_02Room_02Gate_03Street_05
VINS-Mono10.76fail1.530.425.384.32
A-LOAM4.330.400.180.211.260.57
LIO-SAM1.810.290.150.180.910.43
LVI-SAM1.450.270.160.150.930.44
Ours1.020.210.100.100.340.37
注：加粗数值表示最佳结果。 在没有回环闭合的情况下，所有方法仅依赖前端里程计和局部优化，因此更容易出现漂移累积现象。如表4上半部分所示，VINS-Mono在多个序列中表现不佳（如Hall_02和Street_05），说明其在视觉挑战性强或大规模环境中鲁棒性有限。这种现象在意料之中：VINS-Mono高度依赖稳定的视觉特征跟踪和几何初始化；一旦光照变化、纹理较弱、运动模糊或长期漂移破坏了视觉约束，估计器便无法维持可靠的位姿估计。 基于激光雷达及激光雷达-视觉的方法虽然稳定性有所提升，但在Walk_01和Street_05等较长或更复杂的序列中仍会出现明显漂移。相比之下，所提方法在大多数序列中均能实现最低的ATE，包括Hall_02、Room_02、Door_02、Gate_03、Walk_01和Street_05。这些结果表明，考虑不确定性的iESKF融合算法能够有效平衡激光雷达与视觉测量数据，即便在没有全局回环约束的情况下也能显著减小估计误差。 ##### 4.4.2. 基于M3DGR数据集的定量评估 表5报告了M3DGR数据集上在多种挑战性场景下的定量结果，包括视觉退化、激光雷达性能下降以及车轮打滑情况。其中，“车轮打滑”列用于评估不同方法在不平坦地形上应对车轮打滑的鲁棒性。总体而言，所提方法在所有测试条件下均表现出较强的鲁棒性，且在大多数场景中取得了最佳或接近最佳的性能。 ---
Table 5. Quantitative results on the M3DGR dataset under six challenging scenarios. The columns correspond to Dynamic, Dark, Occlusion, Outdoor, LiDAR Degeneration, and Wheel Slip conditions, respectively. "Wheel Slip" refers to scenarios with significant wheel slippage on uneven or slippery terrain, which introduces additional motion disturbances commonly encountered by wheeled robots. Lower values indicate better performance.
MethodVisual ChallengeStandardLiDAR Deg.Wheel Slip.
Dynamic01Dark01Occlusion01Outdoor01Corridor01Wheel-Float01
LVI-SAM1.434.5325.4541.129.060.74
R2LIVE0.611.737.6017.365.720.31
R3LIVE2.761.129.001.756.071.27
Fast-LIVO0.640.629.490.947.960.88
SR-LIVO1.230.101.712.45-0.94
Fast-LIVO20.510.180.290.872.150.61
Ours0.190.110.150.761.070.10
注：加粗数值表示最佳结果；“-”表示运行失败。 在Dynamic01、Dark01和Occlusion01等视觉挑战场景下，我们的方法在所有对比方法中始终能取得最低误差。具体而言，它在Dynamic01上的误差仅为0.19，在Occlusion01上的误差为0.15，显著优于现有的激光雷达-视觉-惯性系统。这些结果表明，所提出的不确定性感知融合与跨模态一致性优化机制能够有效减轻动态物体、光照退化以及部分遮挡所带来的影响。在标准的Outdoor01序列中，我们的方法取得了0.76的最低误差，略优于Fast-LIVO2（0.87）和Fast-LIVO（0.94），显示出更强的长期全局一致性。对于存在严重激光雷达退化现象的Corridor01序列，我们的方法表现最佳（误差为1.07），而SR-LIVO甚至无法生成有效结果；这凸显了在结构重复的环境中结合闭环检测与跨模态约束的优势。在模拟车轮打滑情况的Wheel-float01序列中，我们的方法以0.10的误差大幅优于所有基准方法，展现出对运动模型退化的更强鲁棒性。总体而言，这些结果验证了所提出的系统在各类恶劣传感条件下仍能保持精确且稳定的定位能力。 图8展示了M3DGR数据集中Outdoor01序列上的轨迹对比情况，不同区域突出了不同的评估维度。区域A对应轨迹终点，该区域的对比侧重于不同方法的最终位置偏差。可以看出，几种基准方法相对于真实轨迹存在明显的终点漂移现象，而我们提出的方法终点更接近真实位置，表明其长期误差累积更少。在区域B、C和D中，评估重点在于不同路径段上估计轨迹与真实轨迹之间的空间偏差。在这些区域中，我们提出的方法生成的轨迹始终比其他方法更贴近真实轨迹。具体而言，在弯曲路段（区域B），我们的方法在快速转向时仍能保持较小的偏差；在长直路段（区域C），基准方法的累积漂移更为明显，而我们提出的方法在长距离行驶过程中偏差更小；同样在区域D，我们提出的轨迹与真实轨迹的一致性也更高，反映出全局优化后的更好一致性。区域B、C和D的对比结果表明，所提出的方法在各种运动模式下都能实现更小的轨迹与真实轨迹间距，从而在整条序列中展现出更精确且一致的定位性能。 --- ![Image](images/fe68a47a9fb30df6a4aff8703848231274e2d9e0926de1d23941abbcd31338e0.jpg)
Figure 8. Qualitative trajectory comparison on the Outdoor1 sequence. The trajectories are projected onto the horizontal x-y plane, with the axes measured in meters. The dashed rectangles show zoomed-in regions, and the dashed arrows indicate the corresponding locations of these enlarged views in the full trajectory. The letters A–D mark representative local regions used for visual comparison. GT denotes the ground-truth trajectory.
图9展示了用于实际实验的低光照户外环境。图10对比了我们提出的方法与FAST-LIVO2的建图结果。如区域A和B所示，我们的方法重建出的点云密度更高、完整性更好，结构边界更清晰，缺失区域也更少。这些定性改进证明，不确定性感知的iESKF融合与自适应测量加权机制能够在视觉严重退化的情况下更可靠地利用激光雷达数据，从而带来更高的地图完整性与结构保真度。 ![Image](images/a021cd9d579b045652ae7b70320dcb62dacd3e80592cccc70fab3daad822b106.jpg)
(a) Real World on Dark01 (View 1)
![Image](images/b8bd7415899318897699df84099ba7c880beb34991b52ef887b314ddc2f9f2aa.jpg)
(b) Real World on Dark01 (View 2)
Figure 9. Camera-view observations of the Dark01 sequence.
--- ![Image](images/11a2c7023adbffb989f9f34174db0971f80e29d6ddcc337cee2d530c34b84ba6.jpg)
a. Proposed method
![Image](images/e9b42adf279dd37cb2cd2afe3db97706747e36a6bbafc79e370dd21c29068084.jpg)
b. FAST-LIVO2
Figure 10. Qualitative comparison of point cloud mapping results in a challenging low-light outdoor environment; see Figure 9 for the corresponding scene. (a) The proposed method generates denser and more complete maps. (b) FAST-LIVO2 produces relatively sparse and incomplete mapping results. The differences are particularly visible in regions A, corresponding to the building facade, and B, corresponding to the vertical pillar. This comparison illustrates the effectiveness of uncertainty-aware fusion and adaptive covariance weighting under severe illumination degradation.
### 5. 结论 本文提出了一种鲁棒的多传感器SLAM框架，该框架将激光雷达、视觉和惯性测量数据紧密融合，从而在复杂且动态的环境中实现精确且一致的定位与建图。通过分析现有激光雷达-惯性-视觉里程计（LIVO）系统的局限性，我们发现了与不确定性建模、闭环检测鲁棒性以及跨模态一致性相关的三个关键挑战。为解决这些问题，我们开发了一种考虑不确定性的迭代误差状态卡尔曼滤波器（iESKF），它借助创新统计信息、特征质量和可观测性来指导自适应协方差调整，从而动态平衡各类异构测量数据。此外，我们还引入了基于所提出的多通道二进制三角约束（mBTC）描述符的几何驱动型闭环检测模块，以提升在视角变化、光照差异和外观退化情况下的闭环检测鲁棒性。更进一步，我们将跨模态一致性因子纳入全局因子图优化中，以明确约束视觉里程计与激光雷达里程计之间的相对运动一致性，进而减少长期漂移现象并提高全局一致性。 在多个公开基准测试上的大量实验表明，所提出的系统在定位精度、闭环检测可靠性以及长期一致性方面均持续优于现有的先进LIVO方法，尤其是在动态且视觉条件恶劣的场景中。这些结果验证了将考虑不确定性的前端估计、基于几何的闭环检测以及一致性感知的后端优化整合到一个统一的概率框架内的有效性。 未来的工作将致力于将所提出的框架拓展至大规模长期自主场景，包括针对极端传感器性能退化情况的在线适应、基于学习的几何描述符增强，以及与语义感知的更紧密融合。此外，引入连续时间建模并探索更具表现力的跨模态约束，或许能进一步提升系统在高度动态且复杂环境中的鲁棒性。 --- 作者贡献：概念化：Q.Z.；方法论设计：Q.Z.和J.L.；软件开发：Q.Z.；验证：Q.Z.；正式分析：Q.Z.、J.X.和L.L.；调研：Q.Z.和J.X.；资源提供：Q.Z.；数据整理：Q.Z.；初稿撰写：Q.Z.；审阅与编辑：J.X.、J.L.和L.L.；可视化呈现：Q.Z.；监督指导：J.L.；项目管理：J.X.和J.L.；经费申请：J.L.和L.L.。所有作者均阅读了手稿，参与了撰写并批准了最终版本。所有作者均已阅读并同意发表该手稿。 经费支持：本研究由中国国家自然科学基金（NSFC）资助，项目编号为52278448。 数据可用性声明：本研究中所分析的公开KITTI数据集可在获取（访问日期：2026年4月26日）。研究中使用的公开M2DGR数据集可在获取（访问日期：2026年4月26日）。研究中分析的公开M3DGR数据集可在获取（访问日期：2026年4月26日）。研究中使用的公开NCLT数据集可在获取（访问日期：2026年4月26日）。由于隐私方面的考虑，实地实验中生成并分析的定制数据集并未公开，但如有合理请求，可向通讯作者索取。 利益冲突：作者声明不存在利益冲突。资助方在研究设计、数据收集、分析或解读、论文撰写以及发表决定方面均未发挥任何作用。 ## 术语表
Abbreviation/SymbolDescription
LIVOLiDAR-inertial-visual odometry
iESKFIterated error-state Kalman filter
mBTCMulti-channel Binary Triangle Constraint
BTCBinary Triangle Constraint
NISNormalized innovation squared
IMUInertial measurement unit
LiDARLight detection and ranging
VIOVisual-inertial odometry
LIOLIDAR-inertial odometry
ROSRobot Operating System
SE(3)Special Euclidean group in three dimensions
SO(3)Special orthogonal group in three dimensions
$R_{wb}$Body-to-world rotation matrix $(3 \times 3)$
$p_{w}$Position of the body in the world frame $(R^{3})$
$v_{w}$Velocity of the body in the world frame $(R^{3})$
$b_{g}, b_{a}$Gyroscope and accelerometer biases $(R^{3})$
$\delta\theta$Rotation error vector, also referred to as the attitude error
$\eta_{m}^{NIS}$Statistical consistency scaling factor
$\eta_{m}^{qual}$Perception/matching quality scaling factor
$\eta_{m}^{obs}$Geometric observability scaling factor
$R_{m}^{*}$Adaptive covariance matrix of modality m
### 附录A. 详细的数学推导 本附录给出了第3.2节中为简洁起见而概述的完整数学推导过程。 附录A.1. 惯性测量单元（IMU）传播与线性化动力学 给定测量的角速度$\omega_{m}$和加速度$a_{m}$，连续时间的惯性传播模型可表示为 --- $$ \begin{array}{r}{\dot{\mathbf{R}}_{w b}=\mathbf{R}_{w b}[\mathbf{\omega}_{m}-\mathbf{b}_{g}-\mathbf{n}_{g}]_{\times},}\end{array} $$ $$ \mathbf{\dot{v}}_{w}=\mathbf{R}_{wb}(a_{m}-\mathbf{b}_{a}-\mathbf{n}_{a})+\mathbf{g}, $$ $$ \dot{\mathbf{p}}_{w}=\mathbf{v}_{w}, $$ 其中$[.]\times$表示斜对称算子，$\mathbf{g}$为重力矢量。围绕标称轨迹线性化后可得到 $$ \dot{\delta\mathbf{x}}=\mathbf{F}\delta\mathbf{x}+\mathbf{G}\mathbf{n}, $$ 此处F为状态转移雅可比矩阵，G用于映射过程噪声 $$ \mathbf{n}=[\mathbf{n}_{g}^{\top},\mathbf{n}_{a}^{\top},\mathbf{n}_{w g}^{\top},\mathbf{n}_{w a}^{\top}]^{\top}. $$ 离散化后，协方差传播公式变为 $$ \mathbf{P}_{k|k-1}=\mathbf{F}_{d}\mathbf{P}_{k-1|k-1}\mathbf{F}_{d}^{\top}+\mathbf{Q}_{d}, $$ 其中$Q_{d}$由时间积分的过程噪声得出。 #### 附录A.2. 测量模型与线性化 对于视觉模态，三维地标$X_{i}$投影到图像平面上的表达式为 $$ \mathbf{u}_{i}=\pi(\mathbf{K}\mathbf{R}_{c w}\mathbf{X}_{i}+\mathbf{t}_{c w}), $$ 对应的光度残差为 $$ r_{i}^{v}=I(\mathbf{u}_{i})-I_{\mathrm{r e f}}(\mathbf{u}_{i,\mathrm{r e f}}). $$ 围绕当前估计值线性化后可得到 $$ \begin{array}{r}{r_{i}^{v}\approx\nabla I^{\top}\mathbf{J}_{\pi}\mathbf{J}_{T}\delta\mathbf{\xi}+n_{i}^{v}.}\end{array} $$ 对于激光雷达模态，每个点$\mathbf{p}_{\ell}^{j}$被约束在局部平面$(\mathbf{n}_{j}, d_{j})$上，通过点面残差来表示 $$ \begin{array}{r}{\boldsymbol{r}_{j}^{\ell}=\mathbf{n}_{j}^{\top}(\mathbf{R}_{w b}\mathbf{p}_{\ell}^{j}+\mathbf{t}_{w b})+d_{j}.}\end{array} $$ 其雅可比矩阵为 $$ \mathbf{H}_{j}^{\ell}=[-\mathbf{n}_{j}^{\top}\mathbf{R}_{w b}[\mathbf{p}_{\ell}^{j}]_{\times}\quad\mathbf{n}_{j}^{\top}]. $$ #### 附录A.3. 迭代扩展卡尔曼滤波（iESKF）更新 iESKF更新遵循标准的创新形式： $$ \mathbf{S}=\mathbf{H}\mathbf{P}_{k|k-1}\mathbf{H}^{\top}+\mathbf{R} $$ $$ \mathbf{K}=\mathbf{P}_{k|k-1}\mathbf{H}^{\top}\mathbf{S}^{-1}, $$ $$ \delta\hat{\mathbf{x}}=\mathbf{K}\mathbf{r}, $$ $$ \mathbf{P}_{k|k}=(\mathbf{I}-\mathbf{K}\mathbf{H})\mathbf{P}_{k|k-1}(\mathbf{I}-\mathbf{K}\mathbf{H})^{\top}+\mathbf{K}\mathbf{R}\mathbf{K}^{\top}. $$ --- 每次校正后，标称状态会在流形上进行更新： $$ \mathbf{R}_{w b}\leftarrow\mathbf{\hat{R}}_{w b}\exp(\delta\theta),\quad\mathbf{p}_{w}\leftarrow\mathbf{p}_{w}+\delta\mathbf{p},\quad\mathbf{v}_{w}\leftarrow\mathbf{v}_{w}+\delta\mathbf{v}. $$ 该更新过程会迭代进行直至收敛。 附录A.4. 自适应不确定性建模（三因子乘法分解） 每种模态的自适应协方差为$m \in \{v, \ell\}$ $$ \mathbf{R}_{m}^{*}=\eta_{m}\mathbf{R}_{m}^{0},\quad\eta_{m}=\eta_{m}^{\mathrm{N I S}}\cdot\eta_{m}^{\mathrm{q u a l}}\cdot\eta_{m}^{\mathrm{o b s}}. $$ $\eta_{m}^{\mathrm{NIS}}$、$\eta_{m}^{\mathrm{quai}}$和$\eta_{m}^{\mathrm{obs}}$的详细定义（包括归一化残差统计、可观测性因子和质量评分）遵循第3.2.3节中给出的表达式。 ## 参考文献 [^1]: Graeter, J.; Wilczynski, A.; Lauer, M. LIMO: Lidar-monocular visual odometry. In Proceedings of the 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS); IEEE: New York, NY, USA, 2018; pp. 7872–7879. [^2]: Zhu, Y.; Zheng, C.; Yuan, C.; Huang, X.; Hong, Y. CamVox: A low-cost and accurate LiDAR-assisted visual SLAM system. In Proceedings of the 2021 IEEE International Conference on Robotics and Automation (ICRA); IEEE: New York, NY, USA, 2021; pp. 5049–5055. [^3]: Shao, W.; Vijayarangan, S.; Li, C.; Kantor, G. Stereo visual inertial LiDAR simultaneous localization and mapping. In Proceedings of the 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS); IEEE: New York, NY, USA, 2019; pp. 370–377. [^4]: Shan, T.; Englot, B.; Ratti, C.; Rus, D. LVI-SAM: Tightly-coupled lidar-visual-inertial odometry via smoothing and mapping. In Proceedings of the 2021 IEEE International Conference on Robotics and Automation (ICRA); IEEE: New York, NY, USA, 2021; pp. 5692–5698. [^5]: Lin, J.; Zheng, C.; Xu, W.; Zhang, F. R2LIVE: A robust, real-time, LiDAR-inertial-visual tightly-coupled state estimator and mapping. IEEE Robot. Autom. Lett. 2021, 6, 7469–7476. [CrossRef] [^6]: Lin, J.; Zhang, F. R3LIVE: A robust, real-time, RGB-colored, LiDAR-inertial-visual tightly-coupled state estimation and mapping package. In Proceedings of the 2022 IEEE International Conference on Robotics and Automation (ICRA); IEEE: New York, NY, USA, 2022; pp. 10672–10678. [^7]: Zheng, C.; Zhu, Q.; Xu, W.; Liu, X.; Guo, Q. FAST-LIVO: Fast and tightly-coupled sparse-direct LiDAR-inertial-visual odometry. In Proceedings of the 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS); IEEE: New York, NY, USA, 2022; pp. 4003–4009. [^8]: Zheng, C.; Xu, W.; Zou, Z.; Hua, T.; Yuan, C.; He, D.; Zhou, B.; Liu, Z.; Lin, J.; Zhu, F.; et al. FAST-LIVO2: Fast, direct LiDAR-inertial-visual odometry. arXiv 2024, arXiv:2408.14035. [CrossRef] [^9]: Engel, J.; Koltun, V.C.D. Direct sparse odometry. IEEE Trans. Pattern Anal. Mach. Intell. 2017, 40, 611–625. [CrossRef] [^10]: Engel, J.; Schöps, T.; Cremers, D. LSD-SLAM: Large-scale direct monocular SLAM. In Proceedings of the European Conference on Computer Vision; Springer: Berlin/Heidelberg, Germany, 2014; pp. 834–849. [^11]: Forster, C.; Pizzoli, M.; Scaramuzza, D. SVO: Fast semi-direct monocular visual odometry. In Proceedings of the 2014 IEEE International Conference on Robotics and Automation (ICRA); IEEE: New York, NY, USA, 2014; pp. 15–22. [^12]: Mur-Artal, R.; Tardós, J.D. ORB-SLAM2: An open-source SLAM system for monocular, stereo, and RGB-D cameras. IEEE Trans. Robot. 2017, 33, 1255–1262. [CrossRef] [^13]: Qin, T.; Li, P.S.S. VINS-Mono: A robust and versatile monocular visual-inertial state estimator. IEEE Trans. Robot. 2018, 34, 1004–1020. [CrossRef] [^14]: Qin, T.; Pan, J.; Cao, S.; Shen, S. A general optimization-based framework for local odometry estimation with multiple sensors. arXiv 2019, arXiv:1901.03638. [CrossRef] [^15]: Cao, S.; Lu, X.; Shen, S. GVINS: Tightly coupled GNSS-visual-inertial fusion for smooth and consistent state estimation. IEEE Trans. Robot. 2022, 38, 2004–2021. [CrossRef] ## [^16]: Zhang, J.; Singh, S. LOAM: Lidar odometry and mapping in real-time. In Proceedings of the Robotics: Science and Systems, Berkeley, CA, USA, 12–16 July 2014; Volume 2, pp. 1–9 --- [^17]: Shan, T.; Englot, B. LEGO-LOAM: Lightweight and ground-optimized lidar odometry and mapping on variable terrain. In Proceedings of the 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS); IEEE: New York, NY, USA, 2018; pp. 4758–4765. [^18]: Lin, J.; Zhang, F. LOAM LIVOX: A fast, robust, high-precision LiDAR odometry and mapping package for LiDARs of small FoV. In Proceedings of the 2020 IEEE International Conference on Robotics and Automation (ICRA); IEEE: New York, NY, USA, 2020; pp. 3126–3131. [^19]: Yuan, C.; Xu, W.; Liu, X.; Hong, X.; Zhang, F. Efficient and probabilistic adaptive voxel mapping for accurate online lidar odometry. IEEE Robot. Autom. Lett. 2022, 7, 8518–8525. [CrossRef] [^20]: Liu, Z.; Li, H.; Yaun, C.; Liu, X.; Lin, J.; Li, R.; Zheng, C.; Zhou, B.; Liu, W.; Zhang, F. Voxel-SLAM: A complete, accurate, and versatile lidar-inertial SLAM system. arXiv 2024, arXiv:2410.08935. [^21]: Xu, W.; Zhang, F. FAST-LIO: A fast, robust LiDAR-inertial odometry package by tightly-coupled iterated Kalman filter. IEEE Robot. Autom. Lett. 2021, 6, 3317–3324. [CrossRef] [^22]: Xu, W.; Cai, Y.; He, D.; Lin, J.; Zhang, F. FAST-LIO2: Fast direct LiDAR-inertial odometry. IEEE Trans. Robot. 2022, 38, 2053–2073. [CrossRef] [^23]: He, D.; Xu, W.; Chen, N.; Kong, F.; Yuan, C.; Zhang, F. Point-LIO: Robust high-bandwidth light detection and ranging inertial odometry. Adv. Intell. Syst. 2023, 5, 2200459. [CrossRef] [^24]: Dellenbach, P.; Deschaud, J.E.; Jacquet, B.; Goulette, F. CT-ICP: Real-time elastic LiDAR odometry with loop closure. In Proceedings of the 2022 IEEE International Conference on Robotics and Automation (ICRA); IEEE: New York, NY, USA, 2022; pp. 5580–5586. [^25]: Devarajan, H.; Zheng, H.; Kougkas, A.; Sun, X.H.; Vishwanath, V. DLIO: A data-centric benchmark for scientific deep learning applications. In Proceedings of the 2021 IEEE/ACM 21st International Symposium on Cluster, Cloud and Internet Computing (CCGrid); IEEE: New York, NY, USA, 2021; pp. 81–91. [^26]: Shan, T.; Englot, B.; Meyers, D.; Wang, W.; Ratti, C.; Rus, D. LIO-SAM: Tightly-coupled LiDAR inertial odometry via smoothing and mapping. In Proceedings of the 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS); IEEE: New York, NY, USA, 2020; pp. 5135–5142. [^27]: Kim, G.; Kim, A. Scan context: Egocentric spatial descriptor for place recognition within 3D point cloud map. In Proceedings of the 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS); IEEE: New York, NY, USA, 2018; pp. 4802–4809. [^28]: He, L.; Wang, X.; Zhang, H. M2DP: A novel 3D point cloud descriptor and its application in loop closure detection. In Proceedings of the 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS); IEEE: New York, NY, USA, 2016; pp. 231–237. [^29]: Kim, G.; Choi, S.; Kim. A. Scan context++: Structural place recognition robust to rotation and lateral variations in urban environments. IEEE Trans. Robot. 2021, 38, 1856–1874. [CrossRef] [^30]: Wang, H.; Wang, C.; Xie, L. Intensity scan context: Coding intensity and geometry relations for loop closure detection. In Proceedings of the 2020 IEEE International Conference on Robotics and Automation (ICRA); IEEE: New York, NY, USA, 2020; pp. 2095–2101. [^31]: Wang, Y.; Sun, Z.; Xu, C.; Sarma, S.; Yang, J.; Kong, H. LiDAR iris for loop-closure detection. In Proceedings of the 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS); IEEE: New York, NY, USA, 2020; pp. 5769–5775. [^32]: Yuan, C.; Lin, J.; Zou, Z.; Hong, X.; Zhang, F. STD: Stable triangle descriptor for 3D place recognition. arXiv 2022, arXiv:2209.12435. [^33]: Yuan, C.; Lin, J.; Liu, Z.; Wei, H.; Hong, X.; Zhang, F. BTC: A binary and triangle combined descriptor for 3-D place recognition. IEEE Trans. Robot. 2024, 40, 1580–1599. [CrossRef] [^34]: Bar-Shalom, Y.; Li, X.R.K.T. Estimation with Applications to Tracking and Navigation: Theory, Algorithms and Software; John Wiley & Sons: Hoboken, NJ, USA, 2001. [^35]: Maybeck, P.S. Stochastic Models, Estimation, and Control; Academic Press: Cambridge, MA, USA, 1982; Volume 3. [^36]: Goodman, J.; Hilal, W.; Gadsden, S.; Eggleton, C.D. Adaptive SVSF-KF estimation strategies based on the normalized innovation square metric and IMM strategy. Results Eng. 2022, 16, 100785. [CrossRef] [^37]: Groves, P.D. Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems, 2nd ed.; Artech House: Boston, MA, USA, 20 [^38]: Lee, E. M.; Marsim, K.; Myung, H. LODESTAR: Degeneracy-aware LiDAR-inertial odometry with adaptive Schmidt-Kalman filter and data exploitation. IEEE Robot. Autom. Lett. 2025, 11, 922–929. [CrossRef] [^39]: Huber, P.J. Robust estimation of a location parameter. Ann. Math. Stat. 1964, 35, 73–101. [CrossRef] [^40]: Holland, P.W.; Welsch, R.E. Robust regression using iteratively reweighted least-squares. Commun. Stat. Theory Methods 1977, 6, 813–827. [CrossRef] [^41]: Salton, G.; Buckley, C. Term-weighting approaches in automatic text retrieval. Inf. Process. Manag. 1988, 24, 513–523. [CrossRef] [^42]: Geiger, A.; Lenz, P.; Stiller, C.; Urtasun, R. Vision meets robotics: The KITTI dataset. Int. J. Robot. Res. 2013, 32, 1231–1237. [CrossRef] ## [^43]: Yin, J.; Li, A.; Li, T.; Yu, W.; Zou, D. M2DGR: A multi-sensor and multi-scenario SLAM dataset for ground robots. IEEE Robot. Autom. Lett. 2021, 7, 2266–2273. [CrossRef] --- [^44]: Zhang, D.; Zhang, J.; Sun, Y.; Li, T.; Yin, H.; Xie, H.; Yin, J. Towards robust sensor-fusion ground SLAM: A comprehensive benchmark and a resilient framework. arXiv 2025, arXiv:2507.08364. [CrossRef] [^45]: Carlevaris-Bianco, N.; Ushani, A.K.E.R.M. University of Michigan North Campus long-term vision and LiDAR dataset. Int. J. Robot. Res. 2016, 35, 1023–1035. [CrossRef] 免责声明/出版商说明：所有出版物中所包含的陈述、观点和数据仅代表作者及贡献者个人的立场，不代表MDPI和/或编辑部的观点。对于因文中提及的任何想法、方法、说明或产品而导致的人身伤害或财产损失，MDPI和/或编辑部概不负责。