技术摘要：平滑边缘

问题陈述

本文探讨了连续时间状态估计的挑战，其目标是根据离散且可能异步的测量值 $y_k$（在时刻 $t_k$ 获取），估计作为时间连续函数的轨迹 $x(t)$。虽然离散时间估计需要为每个测量值或时间步显式定义状态变量，但这种方法在处理高频传感器（如激光雷达、IMU）、异步数据以及需要为控制或建图等下游任务在任意时刻查询系统状态时面临困难。

现有的连续时间方法通常分为两类范式：

1. 参数化方法：使用基函数（如样条）表示轨迹。这需要精心选择基函数以避免欠拟合或过拟合。
2. 非参数化方法：使用高斯过程（GP）回归。虽然 GP 因其固有的正则化能力和灵活性而在技术上具有优势，但基于 GP 的连续时间估计在机器人领域的应用较少，部分原因是缺乏与主流的因子图估计范式的直观整合。

方法论

作者提出了一个框架，将 GP 回归与因子图统一起来，专门针对涉及李群（如 SE(3)）的机器人应用进行了定制。

1. 因子图视角下的 GP 回归

核心理论贡献是通过因子图的视角重新构建 GP 连续时间估计。

• 分解：将估计问题分解为两个不同的步骤：
  • 主求解：使用标准离散时间因子图工具求解离散测量时刻（$t_k$）的状态。
  • 插值（查询）：在主求解完成后，计算任意查询时刻（$\tau$）的状态。
• 消去算法：作者证明，在任何时刻 $\tau$ 查询轨迹等价于对查询状态 $x_\tau$ 应用因子图消去算法。这揭示了查询仅依赖于“分隔”状态（即 $\tau$ 前后紧邻的测量状态）的边际分布。
• 计算效率：通过选择源自线性时不变（LTI）随机微分方程（SDE）的核函数，例如加速度白噪声（WNOA）模型，逆核矩阵变为精确的块三对角矩阵。这使得主求解可以在 $O(K)$ 时间内完成（与测量次数成线性关系），而不是 $O(K^3)$。此外，查询步骤变为$O(1)$ 操作，仅需相邻状态的边际统计量。

2. 扩展到李群

为了处理位于流形（如 SE(3) 等李群）上的机器人位姿（旋转和平移），作者对线性 GP 方法进行了调整：

• 局部 GP 变量：他们不是定义单个全局 GP，而是在每个测量时刻周围的李代数中定义局部变量。具体而言，$\xi_k(t) = \text{Log}(T_k^{-1}T(t))$，其中 $T_k$ 是测量 $k$ 时的位姿。
• 线性化：对这些局部变量应用线性运动模型（如 WNOA）。李群元素与局部变量之间的非线性关系通过迭代求解器（如高斯 - 牛顿法或列文伯格 - 马夸尔特法）内的标准线性化技术（扰动模型）来处理。
• 协方差插值：引入了一种在李群上插值协方差的新方法，该方法比之前的方法（如 Anderson, 2016）更简单，且比其他方法（如 Barfoot, 2024）更高效。

3. 处理插值状态上的测量

一个重大的实际挑战是如何处理在没有任何显式优化状态的时刻发生的高频测量。

• 状态消去：作者展示了如何将与测量相关的状态完全从优化图中消去。
• 残差因子：消去带有测量的插值状态会在边界状态之间产生两个因子：一个运动先验因子和一个新测量因子。
• 独立性近似：为了保持稀疏图结构，作者引入了一种近似方法，即丢弃多个插值状态之间的交叉协方差。这使得多个测量可以被视为边界状态上的独立因子，显著减少了优化问题的规模。他们还讨论了一种“简化噪声模型”，该模型省略了某些协方差项以进一步加速，同时指出了其在精度和置信度方面的权衡。

实现

该方法已在 GTSAM（佐治亚理工学院平滑与建图）中实现，这是一个用于因子图优化的流行 C++ 库。

• 新因子：作者引入了 WnoaMotionFactor（用于运动先验）和 WnoaInterpFactor（一个包装器，将定义在插值状态上的因子转换为定义在边界状态上的因子）。
• 工作流程：用户可以定义包含“边界状态”（被优化）和“插值状态”（被消去）的图。该库自动构建等效图，求解它，并提供后处理步骤（updateInterpValues）以恢复完整的连续轨迹和协方差。
• 优化：实现了一个专用的 WnoaFactorGraph 类，用于缓存插值状态的均值和雅可比矩阵，防止在迭代过程中进行冗余计算。

结果与评估

该框架在三个从 1D 到 3D 的数据集上进行了评估：

1. 1D（巨大的牛奶杯）：一个带有激光测距和里程计的线性移动机器人。结果表明，即使测量稀疏（间隔长达 7.5 秒），连续时间方法也能产生平滑、准确的轨迹，并且在 1D 线性情况下，“简化”求解（带插值）产生的结果与“完整”求解完全相同。
2. 2D（迷失在森林中）：一个带有轮式机器人和路标的 SLAM 问题。研究表明，仅优化 30 个状态中的 1 个（插值其余状态），即可恢复出与完整求解几乎相同的轨迹。定量分析显示了一种权衡：虽然“简化噪声模型”提供了显著的速度提升，但它可能导致过于自信的估计，并且与“完整噪声模型”相比，平均精度略有下降。
3. 3D（星光之夜）：一个使用立体相机和 IMU 数据在 SE(3) 中的定位问题。即使线性化点减少了 80%（每第 5 个状态优化一次），插值解在涉及非线性的情况下，仍能保持与完整求解相当的精度和一致性。

意义与主张

本文声称通过提供以下内容，纠正了基于 GP 的连续时间估计在机器人领域未被充分利用的局面：

• 新见解：建立了 GP 回归与因子图之间的清晰联系，表明轨迹查询仅仅是消去算法的一个应用。这提供了一种新颖、直观的方式来进行协方差插值。
• 实际可及性：在 GTSAM 中提供了可工作的实现，使从业者能够轻松采用连续时间估计，而无需从头推导复杂的核矩阵或插值方程。
• 效率：证明了连续时间估计可以通过减少优化变量的数量来高效处理高频、异步传感器，同时不牺牲轨迹的平滑度或精度。

作者将这项工作定位为教程和实用指南，旨在降低机器人领域连续时间估计的入门门槛。他们承认了局限性，例如需要启发式地选择估计时间，以及在李群上使用局部 GP（可能会引入伪影），但他们认为该方法为参数化方法提供了一种稳健且灵活的替代方案。