精读笔记

Problem Setting

论文标题：GNSS/Multisensor Fusion Using Continuous-Time Factor Graph Optimization for Robust Localization（IEEE Transactions on Robotics / 2024）。

这篇论文实际解决的不是“GNSS + IMU + LiDAR 怎么融合”这个老问题，而是城市复杂环境里，在线 FGO 的图结构如何避免被某个主传感器绑架。传统做法通常由 LiDAR、camera、IMU 或 GNSS 之一触发 keyframe / state creation，再把其他观测同步过去。这在正常场景没问题，但在城市峡谷、隧道、桥下、高速运动等情况下，主传感器一旦退化，图的时间骨架本身就会变坏：该创建状态时没创建，该接收的异步观测被丢弃或延迟吸收，其他传感器即使还有信息也无法及时约束状态。

真正困难点是双重的：一方面 GNSS 误差在城市中高度非高斯、非一致，PVT solution 的 covariance 往往不可信；另一方面 LiDAR odometry / SLAM 在隧道、特征贫乏、高速运动、错误 scan registration 时也会失效。因此核心矛盾不是“哪个传感器更可靠”，而是如何构造一个不依赖任何单一传感器的估计骨架，让局部有效的信息可以随时进入优化问题。

Motivation

作者的出发点很明确：已有 GNSS-FGO 已经证明 batch relinearization 和 robust loss 比 EKF 更稳，但很多工作仍默认测量和状态节点可以自然对齐，或者由某个主频传感器驱动图；已有 LiDAR/GNSS SLAM 可以在开放环境表现很好，但本质上仍是 LiDAR-centric，一旦 scan registration 错，GNSS 又恰好被污染，系统会被错误前端牵着走。

关键缺口在于异步、多频、多退化传感器下的图构建机制。现有方法常把“同步”当工程问题处理，但这里作者把它上升为建模问题：如果状态节点的存在依赖某个测量到来，那么估计系统天然继承了该测量模态的脆弱性。缺的是一个 sensor-independent 的时间结构，以及一个能在非节点时刻表达状态的连续轨迹模型。

Core Idea

核心思想可以概括为：把 factor graph 的 backbone 从传感器事件流改成时间轴。状态节点按预设时间间隔确定性创建，不等待 GNSS、LiDAR 或 camera；所有传感器观测按自己的 timestamp 进入图，如果观测时刻不落在状态节点上，就通过 GP continuous-time trajectory 在对应时刻查询一个插值状态来构造 factor。这样，图的存在性和优化节奏不再被任何单一模态控制。

这相比 prior 的本质区别不是又加了一个传感器，也不是换了一个 robust kernel，而是改变了信息流组织方式：传感器不再决定图，传感器只向已有时间骨架提供约束。GP-WNOJ 先验提供的是一种物理平滑 inductive bias：车辆轨迹在短时间内应满足近似连续的高阶运动结构。它使得状态之间不仅是离散变量连接，而是形成一条可查询的连续曲线。因此异步测量可以被“投影”到同一个连续轨迹上，而不是被粗暴同步到最近 keyframe。

Method

方法中值得保留的机制只有几项。

第一，time-centric graph construction。它解决的是主传感器退化导致图无法可靠扩展的问题。状态节点按时间创建，并由 GP motion prior、IMU preintegration 等连接。这带来的核心变化是估计问题的时间支架稳定存在，传感器只是约束来源，不再是图结构的调度者。

第二，GP continuous-time interpolation。它解决异步测量和状态节点不对齐的问题。测量 factor 可以作用在由相邻状态通过 GP 插值得到的 queried state 上。这个 queried state 不是新的优化变量，而是相邻状态的函数，因此不会显著破坏稀疏结构。它把同步误差从工程层面转成模型层面的连续轨迹约束。

第三，WNOJ motion prior。它解决的是车辆在城市驾驶中频繁加减速时，WNOA/constant velocity 先验过弱的问题。WNOJ 相当于允许更高阶动态，用 jerk noise 描述轨迹变化。它的作用不是提供绝对定位，而是在 GNSS/LiDAR 局部退化时防止轨迹在短时尺度上变得不物理。

第四，tight GNSS raw-observation fusion。loose PVT fusion 直接用接收机解算位置速度，收敛快但高度依赖 receiver 给出的 covariance 和城市环境下的解算质量；tight fusion 用 pseudorange / Doppler 直接约束状态与接收机钟差，提供更多卫星级冗余约束。它解决的是 PVT solution 在城市中被接收机黑箱处理后不可信的问题。

第五，LiDAR odometry 作为相对运动约束，而不是系统 backbone。这里 LiDAR 不再负责触发图或定义 keyframe 主时间轴，只提供相邻 scan 的相对位姿约束。这个变化很重要：LiDAR 可以退化，但其退化不会直接摧毁图结构。

Key Insight / Why It Works

这篇论文真正有效的原因不是某个单独 factor 很新，而是把鲁棒性问题从“测量 outlier 怎么处理”推进到“估计图如何不被退化传感器控制”。这是核心贡献。

最重要的机制是 representation alignment：所有异步、多频传感器最终都对齐到同一条连续时间轨迹，而不是对齐到某个传感器 keyframe。这个设计显著减少了信息丢失，也降低了单传感器 failure mode 向系统级 failure 扩散的概率。传统 sensor-centric FGO 中，主传感器坏了不仅损失该传感器信息，还会损坏图的调度和状态创建；time-centric FGO 至少保证其他约束仍有地方施加。

第二个有效点是 tight GNSS raw observation 的冗余性。相比 PVT factor，一个 epoch 内多个卫星 pseudorange/Doppler 给出多个约束，即使部分卫星受污染，robust loss 和批优化仍有机会利用剩余约束。这个增益属于 better measurement granularity + redundancy，不是 continuous-time 本身带来的。

第三个有效点是 GP motion prior 提供短时动态 regularization。它本质上是一个物理 inductive bias，而不是数据驱动泛化。WNOJ 比 WNOA 更适合城市车辆的加减速，但其增益依赖 Qc 调参。文中显示 WNOJ 在速度插值上更好，但在旋转上优势有限；因此不能把整体鲁棒性主要归功于 WNOJ。更准确地说，WNOJ 是使 time-centric 异步融合可用的关键支撑之一，但不是全部性能来源。

哪些可能只是 engineering / scaling？系统中大量性能来自工程完整性：GNSS preprocessing、RTK correction、robust kernels、LiDAR frontend、near-zero velocity detection、固定 lag smoother、参数调节、外参标定。论文把这些整合得很好，但其中不少不是算法新意。尤其 LiDAR odometry noise 手工设定、Qc 手工调参、Cauchy loss 使用等，都可能对结果有显著影响，增益来源并未完全拆清。

我的判断：这篇的实质创新是 time-centric continuous-time FGO 在 GNSS/multisensor online localization 中的系统化落地；实验性能的相当一部分来自 tight GNSS + LiDAR + robust loss + 工程调参，但它们能稳定协同工作的前提，是图结构不再由单个传感器驱动。

Relation To Prior Work

它处在三条技术谱系的交点：GNSS factor graph optimization、LiDAR/visual-inertial multisensor SLAM、continuous-time trajectory estimation via GP。

和 GNSS-FGO 相比，它不是只讨论 pseudorange、carrier phase、Doppler 或 robust GNSS outlier rejection，而是把 GNSS 放进一个更一般的异步多传感器连续时间图里。GNSS 仍然重要，但不再是唯一或主导约束。

和 LIO-SAM / LiDAR-centric SLAM 相比，本质差异是 LiDAR 从 graph scheduler 变成普通 factor provider。LIO-SAM 的失败模式在这里被直接针对：scan registration 失败不应导致状态创建、图优化和全局约束吸收一起失败。

和 GVINS、VIO/GNSS tight fusion 等视觉惯性路线相比，它们也能融合异步 global measurements，但常常仍围绕视觉/IMU keyframe 或 IMU propagation 组织状态。本文更强调状态时间戳由估计器主动设定，而非由某个观测流决定。

和 continuous-time GP trajectory work 相比，GP-WNOA/WNOJ 本身不是新发明，WNOJ 也直接来自 Tang/Barfoot。本文的新意在于将这些连续时间工具用于在线 GNSS/多传感器鲁棒定位，并系统比较 loose/tight GNSS fusion、smoother 类型、城市退化场景。也就是说，很多构件是已有思想重组，但“用 continuous-time GP 解除多传感器 FGO 对主传感器的依赖”是实质新增的信息组织方式。

Dataset / Evaluation

实验是这篇论文比较强的部分：真实车载平台，多城市、多长距离、多退化场景，包括 Aachen、Düsseldorf、Cologne，高速、隧道、桥下、城市峡谷都有覆盖。相比只在 KITTI 或短序列上展示，这更能验证 deployment-oriented robustness。

evaluation 支持了两个核心 claim：第一，sensor-centric LiDAR SLAM 在某些场景会因 scan registration / GNSS 污染崩溃，而 time-centric gnssFGO 更不容易崩；第二，tight raw GNSS fusion 在城市退化环境下通常比 loose PVT fusion 更稳。

但实验也有明显 limitation。ground truth 依赖 RTK-fixed GNSS-PVA，而最困难区域往往 RTK-fixed 不可用，所以误差统计对真正最坏场景的覆盖不完整。作者用可视化和相对讨论补足了一部分，但严格量化仍不足。另一个问题是 ablation 不够干净：time-centric graph、GP-WNOJ、tight GNSS、LiDAR odometry、robust loss、near-zero velocity detection、lag smoother、参数调节之间的贡献没有完全解耦。因此实验能证明系统有效，但不能完全证明每个机制的独立必要性。

Limitation

第一，方法把同步问题解决得很漂亮，但没有根治传感器观测本身的不可信。GNSS NLOS/multipath 仍然可能把 tight fusion 拉偏；LiDAR odometry 在高速、隧道、特征贫乏区域仍可能产生错误相对约束。系统只是更不容易因为某一个传感器坏掉而立即崩，不等于能识别并修复所有坏观测。

第二，成立依赖强时间假设。文中假设估计器和所有测量有同一时钟，delay 已知。真实系统中，跨设备时钟漂移、timestamp jitter、driver latency 往往很复杂；如果 delay 本身需要估计，continuous-time 查询的优势会被削弱。文中未充分说明在时钟不准时的鲁棒性。

第三，GP-WNOJ 的 Qc 是核心但手工调。这个超参数直接决定先验强弱和插值行为。调得过强会压制真实机动，调得过弱会失去 regularization。论文承认未来需在线学习，但当前结果中 Qc 调参贡献不清。这里存在明显 engineering sensitivity。

第四，LiDAR odometry covariance 手工设定是一个硬伤。scan matching 的可靠性高度场景相关，但图里通常给定固定或经验噪声。若错误 LiDAR constraint 被过度信任，time-centric graph 也会被拉偏。论文指出这一点，但没有解决。

第五，长时间 GNSS-denied 仍没有本质突破。没有 global reference 时，系统退化为 IMU/LiDAR/GP 的相对估计，漂移不可避免。所谓 robust localization 主要指短时/局部退化下保持一致，而不是长期无 GNSS 场景的全局无漂移。

第六，scalability 上限来自在线优化预算。虽然 iSAM2/fixed lag 可控，但状态频率、lag size、传感器数量、raw GNSS factor 数量都会增加计算负担。作者给出经验设置，但没有形成系统的资源-精度 trade-off 理论。

Takeaway

• 1. 多传感器 FGO 的鲁棒性不应只从 robust loss 和 outlier rejection 入手，图的时间组织方式本身就是 failure mode 的来源。
• time-centric graph 是一个值得迁移的设计原则。
• 2. Continuous-time trajectory 的最大价值不只是“插值更准”，而是让异步传感器都能对齐到统一 latent trajectory，从而减少同步带来的信息损失。
• 这一 insight 可迁移到 radar/camera/LiDAR/GNSS、event camera、多机器人等异步融合问题。

一句话总结

这篇论文把 GNSS/多传感器在线定位从“由主传感器驱动的离散 FGO”推进到“由连续时间轨迹驱动的 sensor-independent FGO”，真正贡献是重构了异步多源约束进入优化问题的方式，而不是发明了新的单一传感器模型。