Real-time tightly coupled GNSS and IMU integration via Factor Graph Optimization

本文提出了一种基于增量优化与固定滞后边缘化的实时紧耦合 GNSS-IMU 因子图优化方法，并在城市复杂环境下利用 UrbanNav 数据集验证了其在信号遮挡与多径干扰场景中的高精度定位性能。

要点

这篇论文介绍了一种让手机或汽车在高楼林立的城市里也能精准定位的新技术。

为了让你更容易理解，我们可以把这个问题想象成：在一个全是高楼的“迷宫”里，如何不迷路？

1. 核心难题：为什么城市里容易“迷路”？

想象你手里拿着一个GPS 指南针（卫星信号）和一个小陀螺仪（IMU，手机里的运动传感器）。

• GPS 指南针（GNSS）： 它很准，但有个大毛病。在摩天大楼之间，信号会被挡住（就像你被墙挡住了视线），或者被墙壁反射（就像回声一样，让你以为信号来自错误的方向）。这导致在城市里，GPS 经常“瞎”或者“晕”。
• 小陀螺仪（IMU）： 它很勤快，每秒能测几百次你往哪走、转了多少度。它不受高楼影响。但是，它有个致命弱点：它会“ drift"（漂移）。就像你闭着眼睛走路，刚开始很准，但走久了，你会不知不觉地走偏，而且误差会越积越大，最后可能走到完全相反的方向。

以前的做法（松耦合）：
就像让两个人分工：一个人看指南针，一个人看陀螺仪。指南针准的时候，听指南针的；指南针瞎了，就听陀螺仪的。但这就像两个各说各话的人，配合不够默契，一旦指南针彻底失效，陀螺仪的误差很快就会把你们带偏。

2. 这篇论文的解决方案：让两个人“深度绑在一起”

这篇论文提出了一种叫**“紧耦合因子图优化”（Tightly Coupled FGO）**的新方法。

我们可以把它想象成两个侦探（GPS 和 IMU）被一根橡皮筋紧紧绑在一起，共同破案。

• 不再是简单的“听谁的”： 以前的方法是“谁准听谁的”。新方法则是把 GPS 的原始信号（比如每颗卫星传来的微弱电波）和 IMU 的原始数据（加速度、旋转）全部扔进一个**超级大脑（因子图优化）**里一起分析。
• 橡皮筋的作用（紧耦合）： 这根“橡皮筋”就是数学上的约束。即使 GPS 信号被高楼挡住了一半，或者陀螺仪有点晕，系统也会利用它们之间的物理关系互相“拉一把”。比如，陀螺仪告诉你“我刚转了个弯”，GPS 虽然信号弱，但能勉强告诉你“我还在大概这个街区”，系统就会结合这两点，算出你最可能在哪里，而不是直接放弃。

3. 最大的突破：从“事后诸葛亮”变成“实时反应”

以前的这种高级算法（因子图优化），通常像是一个**“事后诸葛亮”**。

• 旧模式（离线）： 就像拍完电影再剪辑。系统会等把一整天的路都跑完，收集所有数据，然后回头慢慢分析：“哦，原来当时那个信号是假的，我修正一下。”这很准，但不能用来实时导航，因为等你算完，人早就走到别处了。

• 新模式（实时 RTFGO）： 这篇论文的厉害之处在于，它让“事后诸葛亮”变成了**“实时反应侠”**。

  • 滑动窗口（Fixed-lag）： 想象你只保留最近 1 分钟的记忆。系统会不断计算这 1 分钟内的最佳路线，然后为了保持反应速度，把1 分钟以前的记忆“打包扔掉”（数学上叫“边缘化”），只留下一个总结性的线索。
  • 结果： 既保留了高级算法的高智商（能修正错误），又拥有了低延迟（能实时告诉你现在在哪）。

4. 实验结果：在“迷宫”里表现如何？

作者把这套系统放在香港最拥挤的街道（高楼大厦像迷宫一样）里测试。

• 普通 GPS： 经常找不到路，或者指错方向（就像在迷宫里乱撞）。
• 旧版算法（离线）： 走得很准，但只能事后看录像。
• 新版算法（实时紧耦合）：
  • 可用性更高： 即使信号很差，它也能给出一个位置，而不是直接报错。
  • 水平方向更准： 在东西南北的平面上，它比普通的融合方法准得多。
  • 小缺点： 在**高度（上下）**方向上，因为高楼太密，卫星很难判断你是在几楼，所以垂直方向还是会有一点误差。但这对于开车或走路的人来说，水平位置准才是最重要的。

总结

这篇论文就像发明了一种**“超级导航员”：
它不再依赖单一的“指南针”或“陀螺仪”，而是把两者深度绑定**，利用一种聪明的**“滑动记忆”算法，既能在高楼大厦的信号干扰中保持清醒**，又能实时告诉你正确的方向。

这对于未来的自动驾驶汽车、无人机在复杂城市环境中安全行驶，是一个非常重要的进步。它让机器在“信号盲区”里，也能像老司机一样，凭经验和直觉（传感器融合）稳稳地开过去。

技术摘要

论文技术总结：基于因子图优化的实时紧耦合 GNSS/IMU 集成

1. 研究背景与问题 (Problem)

在密集的城市环境中，基于全球导航卫星系统（GNSS）的定位面临严峻挑战。由于建筑物遮挡导致的信号频繁中断、多径效应（Multipath）以及非视距（NLOS）接收，GNSS 性能显著下降甚至完全不可用。

• 现有方案局限：
  • 惯性测量单元（IMU）：虽然不受环境干扰且短期精度高，但存在随时间累积的偏差（Drift）。
  • 卡尔曼滤波（KF）：当前主流的 GNSS/IMU 融合方案多基于 KF 及其变种。由于测量和运动模型的非线性，单步估计器（Single-pass estimators）在复杂环境下的精度受限。
  • 因子图优化（FGO）：虽然 FGO 通过多轮优化和时序平滑（Smoothing）在离线场景下表现出极高的鲁棒性和精度，但大多数现有 FGO formulations 是非因果的（Non-causal），即需要所有数据才能求解，导致高延迟，难以满足实时导航（Real-time）的需求。
• 核心问题：如何在保证实时性（低延迟、仅使用因果数据）的前提下，利用 FGO 的优化优势，实现密集城市环境下的高精度紧耦合 GNSS/IMU 融合？

2. 方法论 (Methodology)

论文提出了一种名为 RTFGO-TC（Real-Time Tightly Coupled Factor Graph Optimization）的方法，实现了基于因子图的实时紧耦合 GNSS/IMU 集成。

2.1 核心架构

• 紧耦合（Tightly Coupled）：直接融合原始 GNSS 观测值（伪距和伪距率/多普勒）与 IMU 预积分数据，而非融合解算后的位置/速度。这使得系统在卫星可见性差（如少于 4 颗卫星）时仍能工作。
• 状态向量：包含位置 ($p$)、速度 ($v$)、姿态矩阵 ($C_b^n$)、IMU 偏差（加速度计 $b_a$ 和陀螺仪 $b_\omega$）以及接收机钟差和钟漂 ($\delta t, \delta \dot{t}$)。
• 因子图构建：
  • 先验因子 (Prior)：锚定初始状态。
  • IMU 预积分因子：约束相邻时刻状态间的相对运动，考虑 IMU 偏差。
  • 随机游走因子 (Random Walk)：用于建模缓慢变化的 IMU 偏差和接收机钟差/钟漂。
  • GNSS 伪距因子：约束接收机位置和钟差。
  • GNSS 多普勒因子：约束接收机速度和钟漂。

2.2 实时化关键技术

为了实现实时因果估计，作者对标准 FGO 进行了三项关键改进：

1. 增量固定滞后平滑 (Incremental Fixed-Lag Smoothing)：
   • 利用 iSAM2 求解器进行增量优化。
   • 引入固定滞后（Fixed-lag）机制：仅保留最近 $L$ 秒的状态窗口。当新数据进入时，超出窗口的旧状态被边缘化（Marginalization），其信息被压缩为新的先验项保留在图中。
   • 这确保了计算复杂度和内存占用有界，且仅依赖因果数据。
2. 多普勒观测融合：
   • 显式引入 GNSS 多普勒（伪距率）因子，为速度和钟漂提供额外约束，提升姿态和位置估计精度。
3. 无外部姿态辅助：
   • 不依赖外部传感器（如磁力计或轮速计）提供姿态信息。
   • 姿态和惯性偏差通过系统动力学（速度与位置的耦合）间接可观测。这要求轨迹具有一定的动态特性（如转弯或加速），但在低动态场景下可能导致收敛时间变长。

3. 主要贡献 (Key Contributions)

1. 提出了首个实时紧耦合 GNSS/IMU 因子图优化框架：成功将 FGO 从离线平滑问题转化为实时因果估计问题，解决了 FGO 在实时系统中的延迟瓶颈。
2. 实现了纯数据驱动的实时紧耦合：无需外部姿态源，仅通过 IMU 预积分和原始 GNSS 观测（伪距 + 多普勒）即可在优化框架内联合估计姿态、位置和偏差。
3. 系统性的性能评估：在具有挑战性的 UrbanNav 数据集（香港密集城区）上进行了验证，对比了 GNSS 单点定位、松耦合 FGO、离线紧耦合 FGO 和提出的实时紧耦合 FGO。

4. 实验结果 (Results)

实验使用了 UrbanNav-HK-MediumUrban-1 数据集，该数据集包含严重的多径效应和信号遮挡（GNSS 可用性仅约 40%）。

• 服务可用性 (Service Availability)：
  • 在 2D 均方根误差（RMSE）阈值为 10 米时，RTFGO-TC 和离线 SFGO-TC 的可用性均达到 ~80%。
  • 相比之下，GNSS 单点定位和松耦合方案（LC）的可用性仅为 ~40%。
  • RTFGO-TC 在卫星可见性极差时仍能保持运行，显著减少了定位中断时间。
• 定位精度 (Accuracy)：
  • 水平精度 (2D)：RTFGO-TC 显著优于 GNSS-only 和松耦合方案。Loop 1 的 2D RMSE 从 GNSS-only 的 8.87m 降低至 6.44m；Loop 2 从 8.25m 降低至 7.16m。
  • 垂直精度 (3D)：在部分场景下（如 Loop 2），3D RMSE 略有恶化（从 10.75m 增至 22.88m）。这是由于城市环境中垂直方向卫星几何构型差，且缺乏显式的垂直约束（如高度先验），导致垂直误差在惯性积分中累积。但这主要影响高度，对地面导航最关键的平面精度影响有限。
• 计算性能：
  • 随着滞后窗口（Marginalization Lag）增加，精度提升有限，但计算时间显著增加。
  • 在 60 秒窗口下，RTFGO-TC 的平均优化时间约为 59ms，而松耦合版本仅需 6ms。这表明紧耦合带来了更高的计算成本，但在可接受范围内。

5. 意义与展望 (Significance & Future Work)

• 意义：
  • 证明了 FGO 不仅可以用于离线高精度后处理，通过固定滞后边缘化和增量求解，完全可以满足实时导航的严苛要求。
  • 在 GNSS 信号严重退化的城市峡谷环境中，紧耦合 FGO 提供了比传统卡尔曼滤波和松耦合方案更高的鲁棒性和可用性。
  • 为自动驾驶和行人导航在复杂城市环境下的连续定位提供了新的技术路径。
• 未来工作：
  • 扩展至载波相位定位（PPP/RTK），在因子图中显式估计整周模糊度。
  • 引入针对车辆和行人的运动约束（如非完整约束、零速更新 ZUPT、杠杆臂模型），以进一步抑制垂直漂移并提高低动态场景下的收敛性。

总结：该论文成功构建了一个兼顾高精度、高鲁棒性和实时性的紧耦合 GNSS/IMU 导航系统，填补了因子图优化在实时自动驾驶定位领域的空白，特别是在应对城市多径和遮挡挑战方面展现了巨大潜力。