⚡ electrical engineering

FusionCore: A 23-State Unscented Kalman Filter for IMU, Wheel Encoder, GPS, and Visual SLAM Fusion in ROS 2

FusionCore 是一个开源 ROS 2 包，它采用 23 状态无迹卡尔曼滤波器，将 IMU、轮式编码器、GPS 和视觉 SLAM 数据鲁棒地融合为高频里程计流，在多种 NCLT 数据集序列上实现了比标准 robot_localization 包显著更低的轨迹误差和更高的数值稳定性。

原作者： Manan Kharwar (Independent Researcher, Hamilton, ON, Canada)

发布于 2026-05-26

📖 1 分钟阅读☕ 轻松阅读

CC BY 4.0

原作者： Manan Kharwar (Independent Researcher, Hamilton, ON, Canada)

原始论文采用 CC BY 4.0 许可（http://creativecommons.org/licenses/by/4.0/）。 ✨ 这是对下方论文的AI生成解释。它不是由作者撰写或认可的。如需技术准确性，请参阅原始论文。阅读完整免责声明

想象一下，你正试图蒙着双眼、堵住耳朵，仅凭感受车轮转动来猜测位置，从而引导一辆机器人穿越城市。这本质上就是机器人在试图确定自身位置时所面临的困境。它们必须整合来自不同“感官”的数据，才能构建出清晰的自身位置图景。

本文介绍了FusionCore，这是一款专为使用 ROS 2 系统的机器人设计的新型软件工具，旨在充当整合这些感官的“大脑”。以下是其工作原理的通俗类比解释。

问题所在：“嘈杂”的团队

机器人通常有三种主要方式来猜测自身位置：

轮式编码器（计步器）： 统计车轮旋转的次数。它在短距离内表现优异，但如果车轮在冰面或泥地上打滑，就会陷入混乱。
GPS（卫星地图）： 告诉你确切的地球位置。但它速度较慢，有时会消失（例如当你驾车穿过桥下时），偶尔还会给出一个“故障”信号，声称你身处另一个城市。
IMU（内耳）： 一种感知加速度和旋转的传感器。它速度极快，但随着时间的推移，它会开始“漂移”（就像慢慢倾斜的旋转陀螺），导致机器人误以为自己正在移动，即使它实际上静止不动。

现有的软件（称为 robot_localization）试图混合这些信号，但它存在几个缺陷：

它将传感器中的“漂移”视为固定误差，而非随时间变化的量。
当 GPS 信号在地图边界处跳跃时，它会感到困惑。
它难以区分真实的 GPS 更新与疯狂的故障信号。

解决方案：FusionCore 的“智能大脑”

FusionCore 是一个新的“大脑”（数学滤波器），它将所有这些信号融合为单一、平滑、高速的位置数据流（每秒 100 次）。它使用一个 23 维的“状态”来追踪一切，包括其他系统忽略的一些要素。

以下是 FusionCore 使用的关键技巧，辅以隐喻进行解释：

1. “漂移侦探”（在线偏置估计）

想象你正与一位有点跛脚的朋友同行。如果你没注意到他的跛脚，你绘制的自身位置地图就会慢慢出错。

旧方法： 软件假设跛脚是固定的，并忽略它。
FusionCore 方法： 它内部有一位“漂移侦探”。它持续观察传感器并问道：“车轮在打滑吗？陀螺仪累了吗？”它会实时计算确切的“跛脚”（偏置）量并将其减去。这使得机器人即使在 GPS 暂时消失的情况下，也不会迷路。

2. “全球地址”（ECEF 原生 GPS）

GPS 坐标通常被转换为局部地图（如 UTM 区域）。如果你驾车跨越区域边界，地图可能会出现故障甚至崩溃。

FusionCore 方法： 它不使用局部街道地址，而是使用整个地球的语言（ECEF）。它将地球视为一个巨大的球体，因此永远不会因地图边界而感到困惑。这就像在整个世界使用经纬度，而不是每 10 英里就切换到一张新的城市地图。

3. “测谎仪”（自适应异常值门控）

有时 GPS 会发送一条消息说：“我现在在海洋里！”而机器人显然在道路上。

旧方法： 软件使用僵硬的规则：“如果误差大于 5 米，就忽略它。”但如果 GPS 只是稍微有点噪声，它可能会忽略好的数据。
FusionCore 方法： 它使用一个会学习的“测谎仪”。它观察误差的历史。如果 GPS 通常很准确但突然状态不佳，探测器会放松警惕；如果 GPS 行为疯狂，它就会彻底关闭大门。它自动调整自身的灵敏度，因此不会被故障信号欺骗，也不会拒绝好的数据。

4. “时间旅行者”（延迟补偿）

GPS 数据通常会有微小的延迟，因为卫星在发送信号前需要进行计算。

FusionCore 方法： 它保留了一个“记忆缓冲区”（环形缓冲区），存储过去一秒的 IMU 数据。当迟到的 GPS 消息最终到达时，软件不会将其直接应用于当前时刻。它会“倒带”到 GPS 信号被采集的确切时刻，应用校正，然后“快进”回现在。这确保了即使传感器反应缓慢，机器人的位置也能完美同步。

5. “视觉记忆”（VSLAM 融合）

如果机器人失去 GPS（例如驶入隧道），它可以利用摄像头（视觉 SLAM）来追踪房间内的特征。

FusionCore 方法： 它可以无缝切换到摄像头的视角。如果摄像头迷失方向并必须“重启”其地图，FusionCore 知道如何将机器人的位置重新校准回现实，而不会让机器人惊慌或崩溃。

结果：大考

作者使用来自公共数据集的 12 个长驾驶序列（每个长达 92 分钟）对 FusionCore 与标准软件（robot_localization）进行了测试。

获胜者： FusionCore 在12 场比赛中的 10 场获胜。
优势幅度： 在某些情况下，其准确度比标准软件高出 22 倍。
失败者： 标准软件的高级版本（UKF）在所有 12 个序列中都崩溃（发散）了，意味着它完全丢失了机器人的位置追踪。
FusionCore 输掉 2 场比赛的原因：
1. 长时间 blackout： 在一场比赛中，GPS 信号消失了近 8 分钟。即使是最优秀的侦探，在没有地图的情况下也无法在那样长的时间内完美猜测，机器人因此发生了轻微漂移。
2. “对抗性”故障： 在另一场比赛中，GPS 发送了一组极具迷惑性的谎言，成功欺骗了滤波器。标准软件恰好因为其规则过于严格而意外地拒绝了这些信号，而 FusionCore 因为试图过于“乐于助人”而上了当。作者指出，这些是具体且可修复的问题。

总结

FusionCore 是一种更智能、更具适应性的方法，可帮助机器人知晓自身位置。它将传感器误差视为随时间变化的动态事物，学会忽略谎言，并铭记过去以修正现在。虽然并非完美，但在大多数现实场景中，它的表现显著优于当前的行业标准。

技术摘要：FusionCore

问题陈述

精确的姿态估计是自主机器人导航的基础，然而传感器融合系统的实际部署仍面临挑战。现有解决方案存在若干关键局限：

偏差漂移：IMU 陀螺仪和加速度计的偏差持续漂移，且轮式编码器的偏航率偏差（差速驱动中的系统性偏移）很少被作为滤波器状态处理，导致在 GPS 中断期间出现不可观测的航向误差累积。
架构脆弱性：主流的 ROS 包（如 robot_localization）依赖独立节点进行 GPS 坐标转换（UTM），从而引发区域边界失效。它们还采用固定的噪声协方差和标量异常值拒绝阈值，未考虑测量维度。
操作缺口：现有系统在无 GPS 操作、地图重初始化恢复以及集成多样化传感器（如视觉 SLAM 和雷达）方面存在困难，且缺乏复杂的协调机制。

方法论

FusionCore 是一个开源的 ROS 2 包，实现了一个23 状态无迹卡尔曼滤波器（UKF），将 IMU、轮式编码器里程计、GPS 和视觉 SLAM 姿态融合为单一的 100 Hz 里程计数据流。

核心架构

状态向量：滤波器维护一个 23 维状态向量，包括位置（ $p$ ）、姿态（ $q$ ）、线/角速度（ $v, \omega$ ）、线加速度（ $a$ ）、陀螺仪偏差（ $b_g$ ）、加速度计偏差（ $b_a$ ），以及一个关键的轮式编码器偏航率偏差（ $b_{ewz}$ ）。
偏差估计：与以往方法不同，陀螺仪、加速度计和编码器偏航偏差作为一等滤波器状态，并拥有专用的过程噪声。这使得滤波器能够在无需单独校准阶段的情况下，在线收敛于特定传感器的偏差值。
GPS 融合：系统原生在**地心地固（ECEF）**坐标系中融合 GPS 数据，仅在滤波器状态中转换为 ENU 坐标系。这消除了对独立 navsat_transform 节点的需求，并避免了 UTM 区域边界失效。
测量模型：
- IMU：处理 6 自由度原始数据及 2/3 自由度姿态更新，考虑重力与偏差。
- 轮式编码器：在积分前从报告的角速度中减去估计的 $b_{ewz}$ 。通过自适应噪声强制执行非完整约束（零垂直速度和加速度）。
- VSLAM：接受来自任何视觉里程计/SLAM 系统的 6 自由度姿态测量，包括通过在连续异常值拒绝后重新锚定变换，实现从地图重初始化中的自动恢复。
- 速度：支持 GPS 多普勒速度和雷达多普勒自车速度以进行打滑检测。

滤波器设计创新

每传感器卡方门控：实施马氏距离异常值拒绝，其阈值针对每个传感器路径的自由度（DOF）进行校准（例如 3 自由度 GPS 对比 1 自由度航向），而非使用单一标量阈值。
自适应噪声协方差：利用创新序列的滑动窗口和指数移动平均，实时自动调整传感器噪声协方差（ $R$ ），消除了因环境变化而需手动重新调参的需求。
延迟补偿：利用 1 秒 IMU 环形缓冲区执行回溯预测，将延迟的 GPS 或 VSLAM 测量值应用于正确的历史状态，然后再向前重传播。
零速度更新（ZUPT）：检测静止时段以抑制偏差漂移。
滑行模式：在 GPS 中断期间，从编码器数据中减去估计的编码器偏航偏差，以减少航向漂移。

主要贡献

本文提出了十项具体贡献：

一种 23 状态 UKF 公式，连续在线估计陀螺仪、加速度计和轮式编码器偏航率偏差。
原生 ECEF GPS 融合，消除对 UTM 投影的依赖。
每传感器马氏卡方异常值门控，配备经维度校准的阈值。
基于创新序列的自动噪声协方差自适应。
针对静止时段的自动零速度更新（ZUPT）。
用于延迟补偿的 IMU 环形缓冲区回溯预测。
视觉 SLAM 姿态融合，具备从地图重初始化中自动恢复的能力。
GPS 速度融合，用于跨传感器打滑检测。
雷达多普勒自车速度融合，适用于 4D 成像雷达平台。
在十二个完整长度的 NCLT 序列上针对 robot_localization 的定量评估。

评估与结果

该系统在十二个完整长度序列（每个 55–92 分钟）上进行了评估，数据来自NCLT 公开数据集，涵盖多种季节条件。评估使用绝对轨迹误差（ATE）RMSE 将 FusionCore 与 robot_localization EKF 和 UKF 进行了对比。

性能：FusionCore 在12 个序列中的 10 个上实现了比 robot_localization EKF 更低的 ATE。在获胜序列上的改进幅度为1.2 倍至 22.2 倍。
稳定性：robot_localization UKF 在所有十二个序列的前 30 秒内均出现数值发散（NaN），证实了已知的高频输入不稳定性问题。
失效分析：
- 2012-06-15（FC 失败）：由于 7.7 分钟的 GPS 中断，FusionCore 输给了 robot_localization EKF（49.2 米对 18.2 米）。论文将此归因于在如此长时间内缺乏绝对航向参考（例如磁力计）时，基于编码器的航位推算的局限性。
- 2012-08-20（FC 失败）：由于在 blackout 边界处出现 105 个距离真值 720–840 米的“对抗性 GPS 簇”，FusionCore 表现显著较差（98.3 米对 10.6 米）。论文指出，robot_localization 校准不当的门控恰好拒绝了这些异常值，而 FusionCore 为恢复所需的宽松门控则接受了它们。作者确定速度一致性预检查是解决方案。
协方差不匹配：论文强调，robot_localization 依赖固定且往往不准确的 GPS 协方差报告，导致有效数据被拒绝并陷入航位推算。FusionCore 的自适应噪声模型通过当创新误差超过报告协方差时膨胀噪声模型来缓解这一问题。

意义与主张

论文声称，FusionCore 通过提供一个单一生命周期节点（无需外部协调器），解决了现有 ROS 2 传感器融合堆栈的实际局限性。其意义在于：

鲁棒性：在现有 UKF 实现数值失效的长周期真实场景中，实现了更优越的轨迹精度。
适应性：通过自适应噪声和感知维度的门控，处理动态传感器质量和 GPS 协方差不匹配。
操作连续性：支持无 GPS 操作，并实现从 VSLAM 地图重初始化中的无缝恢复。
可复现性：提供完全开源的实现，包含针对标准平台（TurtleBot3、Husky）的基准测试脚本和配置，以及与 rtabmap_ros 的集成。

作者总结道，尽管存在特定的失效模式（长时间中断、对抗性 GPS 簇），但它们是可诊断的，并且已为未来工作确定了架构修复方案（磁力计集成、速度预检查）。