IEEE Transactions on Robotics

Continuous-Time State Estimation Methods in Robotics: A Survey

作者：William Talbot, Julian Nubert, Turcan Tuna, Cesar Cadena, Frederike Dümbgen, Jesús Tordesillas, Timothy D. Barfoot, Marco Hutter · 机构：Robotic Systems Lab (RSL), ETH Zürich, Zürich, Switzerland; Max Planck Institute (MPI) for Intelligent Systems, Stuttgart, Germany; Computer Science Department of ENS, Willow, Inria, PSL Research University, Paris, France; Institute for Research in Technology, ICAI School of Engineering, Comillas Pontifical University, Madrid, Spain; Autonomous Space Robotics Laboratory (ASRL), University of Toronto, Toronto, ON, Canada

会议/期刊：IEEE Transactions on Robotics · 发布日期：2025 · 页码：4975-4999

运动规划控制优化传感器飞行机器人

精读笔记

Problem Setting

《Continuous-Time State Estimation Methods in Robotics: A Survey》（IEEE Transactions on Robotics / 2025）关注的是机器人状态估计中一个长期被工程技巧掩盖的问题：真实状态连续演化，但主流 estimator 以离散时刻状态作为基本变量。这个错位在低频、同步、全局快门传感器下不明显；但在现代机器人系统中，高频 IMU、扫描 LiDAR、FMCW LiDAR、RADAR、rolling shutter camera、event camera、多传感器异步时间戳、clock offset 和 motion distortion 同时出现，DT 表达会被迫在“变量爆炸”和“测量聚合误差”之间二选一。

以前方法卡住的点不是缺少优化器，而是信息流组织方式不对：为了让 DT estimator 可算，系统往往先把原始高频测量压缩成 pseudo-measurements，例如 scan-to-scan registration、scan deskew、RS compensation、IMU preintegration。问题是这些步骤通常需要已知轨迹才能正确做，而轨迹又是估计目标，形成 chicken-and-egg。CT estimation 试图把这些前端补偿问题移回后端：每个测量在自己的时间戳上直接查询轨迹，运动补偿成为残差计算的一部分，而不是前处理假设。

Motivation

作者的核心动机是：连续时间不是为了“更符合物理直觉”这么简单，而是为了打破测量频率与状态变量数量之间的耦合。DT smoother 的可扩展性来自 sparsity，但当每个高频测量都需要一个状态时，sparsity 也救不了变量数；如果不引入状态，就只能聚合测量，从而引入难以建模的误差。CT 方法提供了第三条路：变量按轨迹复杂度或支持点选择增长，而不是按传感器采样率增长。

关键缺口在于领域知识过于分散。LiDAR 领域把 CT 当作 motion distortion correction；视觉领域把它当作 rolling shutter compensation；calibration 领域把它当作 time offset estimation；GP 领域强调 exactly sparse kernel；spline 领域强调 smooth basis 和 local support。这篇论文的价值在于把这些都放回同一个问题：如何用有限变量表达可查询的连续轨迹，并使 residual 在真实测量时间成立。

Core Idea

本文真正的核心思想是把状态估计的基本对象从离散状态序列换成连续时间轨迹函数。优化变量不必等于测量时刻的状态，而是轨迹的参数：可以是 spline control points，也可以是 GP support states，也可以是 LI 的端点状态。测量因子通过 interpolation/inference 访问任意时间的状态。这使得 estimator 能直接处理异步、高频、非瞬时成像或扫描式观测。

本质区别在于 inductive bias 的位置发生了变化。DT 方法通常把运动假设放在相邻离散状态的 process factor 或预积分因子里；CT 方法把运动假设放进轨迹生成机制本身。Spline 通过 piecewise polynomial 和连续性约束编码平滑性；TGP 通过 LTV-SDE prior 编码随机动力学；LI 则是最弱也最工程化的 constant-rate 假设。它们共同重组了信息流：测量不再驱动状态创建，而是对已有连续轨迹施加约束。

Method

关键机制可以压缩成几条。

1. Interpolated measurement factors：测量残差不再直接连接某个离散状态，而是连接到生成该测量时刻状态的局部轨迹变量。这解决的是异步和高频测量接入问题；它的核心变化是把 motion compensation 从前端预处理变成后端优化的一部分。

2. Local support：无论是 B-spline 的 k 个 control points，还是 exactly sparse TGP 的相邻两个 support states，插值只依赖局部变量。这解决的是连续表示可能导致 dense coupling 的问题；没有 local support，CT 方法会不可扩展。

3. Trajectory prior / smoothness prior：Spline 用阶数、knot、continuity 约束轨迹；TGP 用 WNOA/WNOJ/WNOV/Singer 等 SDE prior 约束轨迹。这解决的是中间时刻不可观测的问题；其代价是引入了强运动假设，prior mismatch 会直接造成 bias。

4. Lie group formulation：机器人姿态/位姿不能简单在欧氏空间插值。Lie group spline 的 cumulative formulation 和 TGP 的 local LTV-SDE 都是在解决 manifold 上插值、导数和优化一致性问题。这里的核心变化不是数学装饰，而是避免 vector-space pose interpolation 导致的奇异性、错误连续性和非物理轨迹。

5. Time offset as interpolation shift：多传感器时间偏移可以通过移动查询时间进入优化。这一点非常重要，因为它把 clock synchronization 从硬件/预处理问题变成可估计参数，但前提是轨迹变化和观测几何提供足够 excitation。

Key Insight / Why It Works

最关键的 insight 是：CT 方法的性能增益大多不是因为“连续时间更真实”，而是因为它减少了错误的信息压缩。DT 系统为了实时性常把一段时间内的原始观测聚合成一个 pseudo-measurement，这等价于提前假设了这段时间内的运动；一旦运动补偿错了，误差已经写进测量。CT 系统则允许每个点、每条 scan line、每个 event、每个 radar return 在自己的时间戳上解释，因此减少了前端不可逆误差。

第二个核心原因是变量数与测量数解耦。CT 并不会减少 residual 数量，但能减少 state 数量，并通过 local support 保持稀疏结构。对高频传感器，这比“为每个测量建状态”更 scalable；对低频但异步的多传感器系统，则减少了复杂同步逻辑。

第三个原因是 inductive bias 更明确。B-spline 的平滑性、TGP 的 SDE prior、WNOJ/WNOA 的导数阶假设，本质上都是 regularization。Johnson et al. 的比较很说明问题：当可微阶数、motion prior 和测量相同，spline 与 TGP 的结果接近。这意味着很多文献中“某类 CT 方法优于另一类”的说法可能归因不清，真正起作用的是轨迹先验、支持点密度、残差设计和工程实现，而不是方法标签。

我认为本文最实质的贡献不是新公式，而是把 CT 方法的有效性归因从“更高阶曲线/更复杂 GP”拉回到三个机制：local support、direct raw-time measurement usage、explicit motion prior。LI 的大量成功也说明：很多实际增益来自避免聚合误差和使用真实时间戳，而不是来自高阶连续性。高阶 spline/GP 在动态复杂或需要导数连续的场景才更有必要。

需要警惕的是，一些 CT 系统的 SOTA 结果可能主要来自 engineering / scaling：per-point residual、更强 map management、更高效 marginalization、更细粒度时间戳处理，而不是 CT theory 本身。文中没有也不可能完全剥离这些因素。所谓“更 general”的 claim 也应谨慎：CT 表达本身 general，但每个具体 prior 都很不 general。

Relation To Prior Work

这篇论文位于 DT smoothing/filtering、trajectory representation、motion compensation、GP regression、spline theory 的交叉处。与传统 DT filter/smoother 相比，它不是改变 MAP estimation 或 factor graph 的基本范式，而是改变状态变量与时间的绑定关系。DT 方法估计 snapshots；CT 方法估计可查询轨迹。

与早期 interpolation/MDC 工作相比，现代 CT 方法的区别在于不把插值只当作前端补偿，而是把它纳入优化残差。LI scan deskew 是工程技巧；per-point residual + interpolated state factor 则是 estimator 层面的建模变化。

与 spline 文献相比，TGP 的本质新增信息是把轨迹先验写成 SDE，使 inverse covariance exactly sparse，并自然提供 posterior covariance interpolation。与普通 GP regression 相比，exactly sparse TGP 的关键不是“用了 GP”，而是用了 Markovian LTV-SDE prior，使得计算复杂度可控。

与 TGP 相比，spline 的优势是参数化直接、局部支撑清晰、推理快、在 planning 中已有大量几何性质可复用；劣势是 knot selection、uncertainty interpolation、under/over-fitting 更棘手。二者很多时候并非竞争关系，而是同一条谱系上的不同轨迹先验实现。论文中最有价值的判断之一是：当 motion model 和 differentiability 匹配时，B-spline 与 TGP 的差异可能没有社区想象中大。

Dataset / Evaluation

作为 survey，本文没有统一 benchmark 实验，而是整合了截至 2024 年 10 月的大量应用证据。覆盖范围很广：LiDAR/RADAR odometry、LIO、VIO、rolling shutter SLAM、event camera、calibration、多传感器融合、GNSS/inertial、teach-and-repeat、motion planning 等。它确实支持一个弱 claim：CT 表达在异步高频传感器和运动畸变场景中是反复有效的通用建模工具。

但它没有强力支持更强的 claim：例如“CT 全面优于 DT”或“某一类 CT 方法更优”。跨论文比较存在实现、传感器配置、地图管理、残差选择、compute budget、数据集动态程度等混杂因素。文中引用的若干比较表明 CT solve time 可以接近 DT，TGP 与 spline 在匹配条件下表现接近，但这仍不是统一、严格控制变量的评估。

benchmark 层面的核心缺口很明显：现有 ATE/RPE 不足以验证 CT 的核心优势，因为 CT 的价值往往体现在测量时间对齐、运动畸变处理、连续查询不确定性、下游控制接口，而不是只体现在轨迹采样点误差。公开数据集也很少提供真正适合 CT 评估的高频异步原始测量和连续 ground truth。

Limitation

CT 方法成立依赖的前提比论文宣传语更强。首先，轨迹必须能被所选表示近似：uniform B-spline 假设运动 excitation 在时间上相对均匀；WNOA/WNOJ 假设某阶导数受白噪声驱动；local LTV-SDE 假设相邻支持状态之间线性化误差可控。对冲击、振动、接触切换、打滑、非平滑控制输入，这些假设会明显失配。

其次，CT 把 DT 的一部分问题转移了，而不是消灭了。DT 要选关键帧；spline 要选 knots；TGP 要选 support state times 和 prior hyperparameters。过密会导致计算上升和局部变量欠约束，过稀会 underfit 或线性化误差变大。文中承认 knot/state time selection 仍是开放问题，这其实是 CT 方法部署中的核心工程痛点。

第三，很多 CT 系统依赖高质量时间戳。虽然可以估计 time offset，但这通常需要足够运动激励和传感器几何约束；若 clock drift、rolling exposure model、LiDAR firing pattern、event latency 等更复杂，简单 offset shift 不够。文中未充分说明复杂时间误差模型下 CT 优势能保留多少。

第四，uncertainty 仍不成熟。TGP 在 covariance interpolation 上理论更完整；Lie group spline 的 covariance interpolation 文中指出几乎缺位。对于需要 risk-aware planning/control 的系统，这不是边缘问题。

第五，增益归因不清。许多 reported gains 可能来自 per-point raw measurement usage、better mapping、more compute、更精细 residual weighting，而不是 continuous-time representation 本身。严格 ablation 在现有文献中不足。

Takeaway

  • 1. CT state estimation 最值得记住的不是“用 spline/GP 拟合轨迹”，而是把测量接入时间从 estimator 外部搬到 estimator 内部；这会系统性减少 motion compensation 和 sensor synchronization 的前处理误差。
  • 2. Spline 与 exactly sparse TGP 的分歧没有表面上大。
  • 真正决定行为的是 local support、轨迹先验、可微阶数、support density 和 residual 设计。
  • 未来比较方法时应该按这些维度对齐，而不是按方法名分类。

一句话总结

这篇论文把连续时间状态估计从一组分散的 motion compensation / spline / GP 技巧重新定位为一种以“可查询轨迹 + 局部支撑 + 显式运动先验”为核心的机器人状态估计范式，并指出其真正瓶颈已从优化器转移到轨迹先验、支持点选择和评估标准。