IEEE Transactions on Robotics

Continuous-Time Radar-Inertial and Lidar-Inertial Odometry Using a Gaussian Process Motion Prior

作者：Keenan Burnett, Angela P. Schoellig, Timothy D. Barfoot · 机构：University of Toronto, Institute for Aerospace Studies, Toronto, ON, Canada; Technical University of Munich, Munich, Germany

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

运动规划操作传感器定位建图状态估计

精读笔记

Problem Setting

论文标题：Continuous-Time Radar-Inertial and Lidar-Inertial Odometry Using a Gaussian Process Motion Prior（IEEE Transactions on Robotics / 2025）。

这篇论文处理的不是普通 LIO/RIO，而是机械扫描传感器在运动中逐点采样导致的连续时间估计问题。spinning lidar/radar 的 scan 是时间展开的观测集合，点与点之间 robot pose 不同；IMU 又是异步高频观测。如果仍按 scan-level pose 做离散估计，运动畸变只能靠前处理 deskew 或常速度假设吸收；如果给每个点/IMU 时刻建状态，因子图规模不可接受。

真正困难点在于三个约束同时存在：连续时间查询、紧耦合优化、实时稀疏求解。以前方法往往只能满足其中两项：经典 preintegration 适合低频外感知；B-spline 连续时间可查询但控制点和窗口设计工程性强；dense GP kernel 理论优雅但计算和局部化不友好；loosely coupled deskew 把轨迹估计先验化，点云配准不能反过来修正 deskew 轨迹。本文的关键矛盾就是：如何在不显式估计所有测量时刻状态的前提下，让每个异步测量仍然作用在其真实采样时刻的轨迹上。

Motivation

作者的核心观察是：在 lidar/radar 里，外感知并不是真的低频。scan rate 可能是 10 Hz，但点级时间戳是 kHz 级，机械 radar 也有 azimuth-level 时序。因此，把 lidar/radar 当作低频 sensor、IMU 当作高频 sensor 的经典 VIO/LIO 视角不够。

缺的不是又一个 ICP front-end，而是一个后端表示：它应该允许 point factors、gyro factors、accelerometer factors 全部以 continuous-time 方式进入同一个优化问题，同时保持 factor graph 局部稀疏。作者从自己之前 STEAM/GP continuous-time ICP 的路线自然延伸：既然 GP motion prior 已经能同时估计轨迹并 deskew lidar，那么下一步就是把 IMU 也纳入这个 posterior trajectory，而不是把 IMU 用作前端 deskew 工具。

另一个动机是 radar。机械扫描 radar 对恶劣天气有潜力，但数据稀疏、噪声大、2D、存在 Doppler distortion，纯 radar odometry 的几何约束弱。IMU 在这里不是锦上添花，而可能是把 radar odometry 从脆弱 scan matching 拉到可用状态的关键外部约束。

Core Idea

核心思想是用稀疏 GP motion prior 表示连续时间轨迹，而不是用 scan-level 离散 pose 或全局 spline basis 表示轨迹。具体地，状态包含 SE(3) pose 与 body-centric velocity，采用 white-noise-on-acceleration prior；这等价于在局部切空间中假设 velocity 随白噪声加速度演化，即常速度是默认 inductive bias，但允许由测量打破。

这个建模改变了信息组织方式：估计变量只放在少数 estimation times 上，任意 point/IMU timestamp 的状态由 posterior GP interpolation 从相邻两个估计状态得到。由于该 prior 对应 Markov SDE，inverse kernel 精确稀疏，motion prior 只连相邻状态；而插值是 O(1)。这使连续时间测量因子既能保留真实时间戳，又不会把图变成 dense。

和 prior 的本质区别在于：它不是先 IMU preintegration 得到 scan deskew，再做 ICP；而是把点云匹配和 IMU 观测同时作为对同一条连续时间 posterior trajectory 的约束。deskew 是优化后 posterior 的副产物，而不是独立前处理。这一点比“用了 GP”本身更关键。

Method

方法中机制层面最重要的是以下几件事。

1. 稀疏 GP motion prior：解决连续时间轨迹表示与实时求解之间的矛盾。white-noise-on-acceleration prior 在局部切空间中产生相邻状态间的常速度约束，inverse covariance block-tridiagonal，因此 batch/sliding-window 求解仍接近离散因子图复杂度。它带来的核心变化是：连续时间不再必然意味着 dense trajectory representation。

2. posterior GP interpolation：解决异步测量时刻不等于估计时刻的问题。每个 lidar/radar point、gyro measurement、accelerometer factor 都可以查询其真实时间的 pose/velocity，但线性化后只连接 bracketing states。这是本文可扩展性的关键，而不是某个具体 ICP variant。

3. gyro direct factor：由于 angular velocity 是状态的一部分，陀螺仪可以直接观测状态。这绕开了传统 IMU preintegration 中对 rotation 的完整积分，使 gyro 信息以高频 unary/binary continuous-time factor 形式进入优化。实验也表明主要 inertial 增益来自 gyro。

4. accelerometer relative velocity factor：因为 acceleration 不在状态里，作者只对 accelerometer 做一次积分形成 velocity difference 约束，而不是形成 pose/rotation/velocity 的完整预积分。这样把加速度信息纳入系统，但避免把状态扩展到 acceleration 或引入更重的高阶 prior。

5. 连续时间 ICP/radar factors：点云因子在每个点的 timestamp 上评估 pose。lidar 使用 point-to-plane；radar 使用 point-to-point 加 robust loss，并额外补偿 Navtech 机械 radar 的 Doppler distortion。这里的核心不是 ICP 形式创新，而是这些约束直接作用在 continuous-time trajectory 上。

Key Insight / Why It Works

这篇论文真正有效的原因不是“GP 比 spline 高级”，而是选了一个同时满足物理合理性和计算稀疏性的 prior。white-noise-on-acceleration 在机器人局部运动中是足够强的 inductive bias：短窗口内常速度通常成立，偏离常速度的部分由点云和 IMU 观测修正。这个 prior 对 lidar deskew 很有效，因为机械扫描畸变主要来自 scan 内平滑运动；对 radar 更有效，因为 radar 几何约束弱，motion prior + gyro 能显著降低 scan matching 的自由漂移。

最核心贡献是把 continuous-time posterior trajectory 作为所有传感器共享的 latent structure。点云不再只给 scan-level relative pose，IMU 也不再只是 deskew 工具；两者共同约束同一条时间连续轨迹。这个 latent trajectory 同时提供 deskew、interpolation、uncertainty propagation 和 sliding-window marginalization 的统一接口。

增益归因上，我会比较明确地下判断：lidar 场景的主要收益来自 continuous-time ICP + 合理 motion prior，而不是 accelerometer；IMU 中主要是 gyro 在起作用。论文结论也支持这一点。accelerometer factor 更像辅助稳定项，尤其在短窗口 odometry 中，其可观测性和 bias/gravity 耦合限制了贡献。

radar 场景的 43% 提升是合理的，但不应全部归因于 GP-IMU 融合理论。radar pipeline 还强依赖 CFAR、robust loss、map culling、Doppler compensation、keyframing 等工程机制；附录中的 STEAM-RIO++ 通过窗口长度、loss、gyro 权重、keyframing 又显著提升，说明 radar 结果对工程选择很敏感。这里有一部分可能主要来自 engineering / tuning，而不是核心模型本身。

另一个重要 insight 是 sparse kernel 的价值。相比 squared exponential GP，本文不是在追求更平滑的函数拟合，而是在追求 Markovian structure。对在线 odometry 来说，kernel 的可稀疏化比表达能力更重要；否则长轨迹切成局部块时会隐式丢掉 dense connections，理论优雅性反而变成部署负担。

Relation To Prior Work

这篇属于 continuous-time state estimation + direct odometry + inertial fusion 的交叉谱系，最接近的是 STEAM/Barfoot 系列 GP trajectory estimation、CT-ICP 类连续时间 lidar odometry、B-spline LIO、以及 Le Gentil & Vidal-Calleja 的 GP-IMU 连续时间预积分路线。

相对经典 LIO，本文的本质差异是 measurement-time trajectory evaluation，而不是 scan-time state estimation。LOAM/LIO-SAM/DLIO 这类方法通常先用 IMU deskew，再把 ICP relative pose 与 IMU preintegration 在图里融合；本文则把 point factors 和 IMU factors 放到同一个 continuous-time optimization 中，属于更紧耦合的后端。

相对 B-spline 方法，本文用 SDE-induced GP prior 取代手工 basis/control point 表示。B-spline 的局部支撑也能稀疏，但控制点间隔、阶数、边界处理更工程化；GP prior 的优势是 covariance、interpolation、motion prior factor 都来自同一个概率模型。不过这不是全新思想，更多是把已有 sparse GP trajectory estimation 体系系统地落到实时 lidar/radar-inertial odometry 上。

相对 Le Gentil & Vidal-Calleja，真正差别在 kernel 和耦合方式。他们用 squared exponential kernel 建独立 GP，再积分形成相对运动因子；计算复杂度和图连接更重。本文用 Markov sparse prior，六自由度通过 SE(3)/body velocity motion prior 耦合，并且 measurement factor 直接进入点云配准优化。这里的实质创新是：不是用 GP 生成更好的 IMU upsampling，而是用 GP posterior 作为多传感器紧耦合轨迹本体。

在 radar odometry 方面，论文的新意更偏系统首次验证：机械 spinning radar + full IMU（gyro 和 accelerometer）+ continuous-time factor graph。radar 前端本身并不是最先进，和 CFEAR/CFEAR++ 相比，本文后端建模更统一，但 radar-specific perception engineering 未必占优。

Dataset / Evaluation

评估覆盖三类场景：KITTI-raw 用来验证 motion-distorted lidar odometry 在标准车载数据上不落后；NCD 用来验证 handheld aggressive motion 下 continuous-time/inertial 的价值；Boreas 用来验证真实季节/天气变化下 lidar 与 mechanical radar 的对比，以及 radar-inertial 的收益。整体覆盖面不错，尤其 Boreas 的长期、多天气、真实车载数据使 radar claim 更可信。

实验基本支持论文核心 claim：稀疏 GP continuous-time odometry 可以实时运行；在 NCD 这类高动态场景优于简单常速度 deskew；IMU 对 radar 的收益显著大于对车载 lidar 的收益。作者也没有过度声称 IMU 总是提升 lidar，这点比较诚实。

但 evaluation 有几个限制。第一，KITTI-raw 没有原始 IMU，所以不能验证 LIO，只能证明 continuous-time LO 竞争力。第二，NCD 的 ATE 受 implicit loop closure/map reuse 影响，odometry 与 SLAM 边界有点模糊；低 ATE 不完全等同于短期里程计更强。第三，Boreas 中 radar 是 SE(2)、lidar 是 SE(3)，把 3D ground truth 投影到 2D 再按 KITTI metric 比较会引入评价偏差；论文关于 radar-lidar gap 的判断是有价值的，但绝对差距不应过度解读。第四，radar pipeline 的 ablation 不够细，IMU、Doppler compensation、map culling、loss、window length 的贡献没有完全剥离。

Limitation

最根本的前提是局部线性化和短窗口平滑运动。GP prior 在 SE(3) 上通过局部 Lie algebra 近似实现，作者也明确要求 process noise 和相邻估计时刻旋转不能太大。极端快速旋转、低估计频率、时间同步误差或强非平滑运动都会破坏这个近似。

第二个限制是 prior 的常速度 bias。white-noise-on-acceleration 的确提供稀疏性，但它也会在外感知退化时把系统拉向常速度解释。对地面车这是合理的；对足式、无人机、强 jerk 运动未必够。作者试过 white-noise-on-jerk / Singer prior 和 acceleration state，但可靠性下降，这说明更高阶状态并不是免费午餐，也说明当前方法的动态表达上限存在。

第三，posterior interpolation factor 是近似。文中明确说这不等价于 marginalizing out measurement times，只是实践中快且好用。这个近似在 dense measurement 下通常没问题，但理论一致性、协方差校准和高动态边界未充分说明。

第四，系统对前端 correspondence 仍然高度依赖。GP prior 能缓解 motion distortion，但不能解决错误匹配、动态物体、radar ghost、snow clutter 的根本问题。Boreas 中需要 voxel culling 防止 snow detections 累积导致失败，这说明 robustness 很大程度来自地图维护工程。

第五，scalability 的上限不在 GP prior 本身，而在 continuous-time measurement factor 的数量和 Jacobian 计算。timestamp binning 是必要工程折中；降低 timestamp frequency 后仍能保持大部分性能，但这也说明所谓逐点连续时间在部署时被离散粗化了。这里的实时性部分来自模型稀疏性，部分来自 aggressive downsampling。

第六，泛化性仍主要是系统级而非学习式泛化。Q、IMU noise、bias prior、radar threshold、map culling 时间等都需要数据集/平台调参。论文展示跨 KITTI/NCD/Boreas 可用，但不能说明无需重新 tuning。

Takeaway

  • 1. 对机械扫描传感器，连续时间轨迹不是装饰，而是正确的信息接口；尤其当外感知和 IMU 都是高频异步时，scan-level 离散状态会系统性丢信息。
  • 2. 稀疏 GP prior 的真正价值在 Markovian inverse kernel，而不是 GP 这个名字。
  • 未来在线连续时间估计更应关注“可查询 + 可稀疏 + 可边缘化”的 prior 设计，而不是更复杂的 dense smooth kernel。
  • 3. IMU 在 lidar 和 radar 中的作用不同：车载 lidar 几何充分时，GP continuous-time ICP 已经很强，IMU 增益有限；radar 几何弱且噪声大，gyro/motion prior 对稳定 scan matching 更关键。

一句话总结

这篇论文把稀疏 GP 连续时间轨迹估计从 lidar deskew 推进到 lidar/radar-inertial 紧耦合里程计，真正贡献是用 Markovian motion prior 统一异步高频传感器约束，而不是提出新的 ICP 或 IMU 预积分技巧。