Reliability-Aware Sensor Fusion via Bidirectional Diffusion for Robust Robot Odometry

技术摘要：通过双向扩散实现可靠性感知传感器融合，以提升鲁棒机器人里程计性能

问题陈述
在动态或退化环境（如森林、灾区、城市环境）中运行的自主机器人依赖于多模态传感器融合（LiDAR、RGB-D 相机、IMU）进行状态估计。然而，现有的融合框架存在三个主要局限性：

1. 时间异步性： 传感器频率不匹配（例如，IMU 为 400 Hz，而 LiDAR 为 10 Hz）会导致随时间增长的积分误差，现有方法通常仅在残差出现后才进行反应式处理。
2. 静态噪声假设： 标准滤波器（EKF、IEKF）假设固定高斯噪声，无法应对由运动模糊、遮挡或动态光照引起的非高斯、时变噪声。它们无法显式地预测短时界限内的传感器可靠性，从而在状态更新前调整协方差。
3. 空间信息丢失： 许多系统将 3D 数据投影到 2D 网格中，丢弃了复杂地形穿越所需的关键高度和表面法线信息。

核心挑战在于如何将预测的短时界限传感器可靠性转化为自适应滤波器协方差，从而在受损测量值导致过度自信的状态修正之前进行干预。

方法论：PUD–DIKF
本文提出了一种名为 PUD–DIKF（预测不确定性驱动的扩散信息卡尔曼滤波器，Prediction-Uncertainty-Driven Diffusion-Informed Kalman Filter）的统一估计框架，该框架由一个学习到的可靠性模型驱动中心卡尔曼滤波器。其架构是层级式的，由支持前端模块向核心估计器提供数据。

1. 统一多模态张量 (PUT)： 核心接口是预测不确定性张量 ($\Psi_t$)，这是一个集成了异构传感器流（RGB、深度、法线、LiDAR 点/强度、IMU 数据）的 3D 体素网格。它编码了三种不确定性分量：

   • 偶然不确定性 (Aleatoric)： 不可消除的传感器噪声。
   • 认知不确定性 (Epistemic)： 由于训练数据有限导致的模型无知。
   • 预测不确定性 ($U_{pred}$)： 短时界限内的可靠性预测。
     这些分量被限定并组合成有效不确定性 ($U_{eff}$)，用于调节滤波器的协方差。

2. 双向扩散同步 (BDS)：

   • 回顾性扩散估计器 (RDE)： 对当前传感器观测值进行去噪，以在卡尔曼更新前减少测量噪声。
   • 前瞻性扩散预测器 (PDP)： 通过学习历史不确定性轨迹中的模式（例如，在遮挡发生前的方差增加），预测未来的传感器可靠性 ($U_{pred}$)。
   • PDP 的输出能够自适应地调节扩散噪声调度，从而允许系统将残余的时间异步性建模为结构化噪声。

3. 扩散增强标定 (DAMC)： 使用 Cayley 参数化进行旋转建模，对 LiDAR 和相机进行在线外参标定。它联合优化几何对齐与扩散正则化，并利用预测不确定性来调整优化步长。

4. SE(3)-等变特征提取： 使用扩展了扩散不确定性通道的 SE(3)-等变卷积提取鲁棒特征。这保证了在刚体变换下的几何一致性，并将每个区域的可靠性传递给滤波器。

5. 核心 PUD–DIKF 估计器：

   • 基于 FAST-LIO2 的误差状态结构构建。
   • 自适应协方差： 过程噪声 ($Q_k$) 和测量噪声 ($R_k$) 通过来自 PUT 的正定半定、状态相关的增量进行增强。
   • 单调性保证： 本文通过（命题 1）证明，增加预测的不确定性不会增加测量信息量，也不会使传播的协方差低于固定噪声基准。这确保了退化的可靠性会导致保守（信心较低）的更新，防止过度修正。

6. 下游建图 (POF)： 预测占用场 (Predictive Occupancy Field) 利用估计的状态和不确定性进行密集建图，但不会将姿态修正反馈回估计器，从而保持严格的单向流。

主要贡献

1. 核心估计器： 一种扩散信息驱动的卡尔曼滤波器，能够将回顾性和前瞻性不确定性映射为 SO(3) 误差状态滤波器上的状态相关、正定半定协方差增量。
2. 理论联系： 一个单调性条件，证明了更高的预测不确定性会严格限制测量信息并增加传播的协方差，从而防止滤波器在退化期间变得过度自信。
3. 受控验证： 在自定义的多模态校园数据集和公开基准测试（HILTI–Oxford, KITTI）上进行了广泛实验，包括隔离各组件贡献的消融研究。

实验结果

• 校园数据集 (438.58 m 评估窗口)： PUD–DIKF 实现了 0.034 m 的轨迹 RMSE 和 0.008 m 的 RPE，中值点云对齐误差为 0.008 m。系统在每帧 LiDAR（10 Hz）下运行速度为 42 ms。
  • 与 FAST-LIVO2 相比，RMSE 下降了 19% (0.042 m $\to$ 0.034 m)。
  • 与 R3LIVE 相比，RMSE 下降了 35% (0.052 m $\to$ 0.034 m)。
• 公开基准测试：
  • HILTI–Oxford： 在三个建筑序列中实现了最低的 ATE RMSE（均值 0.0155 m），在几何复杂且部分遮挡的环境中表现优于基准模型。
  • KITTI： 在四个序列中报告了最低的平移漂移 ($t_{rel}$，均值 0.0183%)，尽管作者指出这些是初步的内部评估，尚需官方服务器验证。
• 消融研究：
  • 移除回顾性去噪 (RDE) 会使 RMSE 增加 32%，表明其为该数据集中最关键的组件。
  • 移除前瞻性预测 (PDP) 会使 RMSE 增加 21%，验证了前瞻性协方差自适应的假设。
  • 使用标准 MLP 代替 SE(3)-等变特征会使 RMSE 增加 15%。
  • 移除相机模态后，RMSE 仅增加了 3%，表明该系统主要是几何惯性驱动的，相机仅提供适度的、依赖于数据集的修正。
• 密集建图： 将 PUT 不确定性集成到建图模块中，使占用率 RMSE 比 TSDF 基准降低了 53%，并将 F1 分数从 0.73 提高到 0.89。

意义与声明
本文声称 PUD–DIKF 通过在状态更新前建模可靠性变化，成功解决了反应式融合的局限性。其核心假设得到了验证：将预测的不确定性转化为自适应协方差，可以防止受损的测量值主导状态更新。作者强调，系统的优势在结构复杂和部分遮挡的环境中最为显著，因为这些环境下的传感器退化频繁发生。

本文对其声明保持了审慎态度：

• 明确指出 KITKI 的结果属于“初步基准背景”而非经过统计验证的优越性，因为这些结果并非在相同的硬件上通过配对运行或经由官方服务器验证得出的。
• 承认视觉输入（相机）是辅助性的而非主导性的。
• 指出了一些局限性，包括 DAMC 在高噪声下可能出现的非单调参数变化、假设至少有一个空间传感器保持有效，以及需要在其他机器人平台（如足式或飞行器）上进行评估。

这项工作并不将其视为独立模块的集合，而是一个统一的假设，其中支持性元素（BDS、DAMC、SE(3) 特征）都服务于由预测不确定性张量驱动的协方差自适应这一核心机制。