IEEE Transactions on Robotics

Data-Driven Batch Localization and SLAM Using Koopman Linearization

作者：Zi Cong Guo, Frederike Dümbgen, James Richard Forbes, Timothy D. Barfoot · 机构：Robotics Institute, University of Toronto, Toronto, ON, Canada; Department of Mechanical Engineering, McGill University, Montreal, QC, Canada

会议/期刊：IEEE Transactions on Robotics · 发布日期：2024 · 页码：3964-3983

优化定位建图状态估计

精读笔记

Problem Setting

论文《Data-Driven Batch Localization and SLAM Using Koopman Linearization》（IEEE Transactions on Robotics / 2024）处理的是 data-driven batch localization / SLAM：机器人过程模型和观测模型不显式已知，或者已有模型存在偏差，但训练阶段可获得带 groundtruth 的数据，测试阶段要在新 landmark 或未知 landmark 条件下估计轨迹和地图。

真正困难点不是把 dynamics 学出来，而是把 learned model 放进 SLAM 推理里。SLAM 需要长期 batch consistency、pose-landmark coupling、稀疏求解、可估计 covariance；普通 black-box model 很难满足这些结构。另一方面，Koopman lifting 虽然把非线性系统变成高维线性/双线性形式，但高维 lifted space 比真实状态空间自由度大得多，优化会利用这些虚假自由度得到 off-manifold 解。这个问题在长时域和 SLAM 中比在短时 MPC/filtering 中严重得多。

以前方法卡在两个地方：model-based estimator 对 prior model 质量敏感；已有 Koopman state estimator 多用于固定 landmark localization，本质上把环境吸进一个整体观测模型，不能自然迁移到新 landmark，也不能把 landmark 当未知量估计。本文的关键矛盾是：既想利用 Koopman 的线性化和数据驱动建模，又不能失去 SLAM 的几何结构和 lifted manifold consistency。

Motivation

作者的出发点很清楚：在很多机器人系统里，采数据比建精确模型容易，尤其是 range/RFID/复杂传感器响应、外参偏差、尺度偏差、轮式运动误差等。传统 SLAM 的优化框架很成熟，但它假设过程和观测模型已知；一旦模型错，优化再精确也只是在错误因子图上求解。

Koopman 路线提供了一个诱人的中间层：不直接学习任意非线性函数，而是通过 lifting 把模型写成线性或双线性算子，从而仍然能使用线性代数和 batch estimation 结构。作者核心观察是，control-affine dynamics 和 pose-landmark separable measurement 在 lifted space 中天然对应双线性形式，这正好与 SLAM 中“已知输入 + pose-landmark coupling”的结构匹配。

关键缺口是：已有 Koopman estimation 没有显式 landmark 变量，也没有处理 lifted solution off-manifold 的机制。没有这两点，Koopman lifting 只能做局部/短时/固定环境估计，无法成为真正的 SLAM 方法。

Core Idea

论文真正的核心思想是：把 SLAM 的非线性建模问题改写成 lifted bilinear estimation，并把所有非线性复杂性从手工模型转移到数据学习的 lifted operators 和显式 manifold constraints 上。过程模型在给定输入后变成 time-varying linear factor；观测模型保持 pose 与 landmark 的双线性 coupling；因此 batch SLAM 的 factor graph 稀疏性仍然可用。

最重要的 inductive bias 是“显式 lifted manifold”。作者不是允许优化在高维空间里任意找低代价点，而是要求 lifted pose/landmark 必须对应某个真实 pose/landmark 的 lifting。这个约束等价于告诉优化器：高维 features 只是状态的函数，不是独立物理变量。它重新组织了信息流：measurement/process residual 在 lifted space 中计算，但最终自由度仍被限制在原始状态流形附近。

RCKL 相比 CKL 的关键变化是放弃完整 Koopman closure。它不再要求模型预测 lifted features 的下一步值，而只预测原始状态分量；lifted features 由当前原始状态通过约束决定。这是本文最实质的设计选择，因为有限维 Koopman basis 很难对所有 features 闭合，尤其 polynomial/random features 会产生无限级联需求。RCKL 本质上承认 closure 做不到，于是只保留 lifted features 作为回归基函数，而不是作为必须动态一致的状态变量。

Method

1. 数据驱动 lifted system identification：解决“没有模型或模型错”的问题。训练阶段用 groundtruth transition 和 landmark measurements，学习 A/B/H/C 以及 noise covariance。它的核心变化是把模型学习成能直接进入 batch estimator 的矩阵，而不是学一个独立 predictor。训练本身是正则化 least squares，不是论文最深的部分。

2. 显式 landmark lifting 与双线性观测：解决 KoopSE 无法处理测试时新 landmark/SLAM 的问题。观测写成 y = C(ell ⊗ x)，使 landmark 不再被环境级模型隐式记忆，而成为优化变量。这一点是从 localization 走向 SLAM 的结构性改动。

3. Manifold-constrained optimization：解决 lifted variables 自由度过大和 off-manifold 解的问题。约束形式要求 x=[xi, p_tilde(xi)]、ell=[psi, p_tilde(psi)]。这不是装饰性约束，而是使 learned lifted model 在测试时仍处在训练分布支撑上的必要机制。

4. SQP + nullspace sparse solve：解决约束优化的可扩展性问题。SQP 将非线性约束局部线性化，nullspace 投影把求解限制在约束切空间，保留 SLAM arrowhead sparsity。机制上，它让 constrained Koopman SLAM 的每次迭代复杂度与经典 batch SLAM 同阶。

5. Reduced CKL：解决 lifted feature dynamics 拟合差的问题。RCKL 只对原始状态分量构造过程 residual，measurement residual 仍使用完整 lifted features。核心变化是把 features 从“需要预测的状态”降级为“受约束的非线性基函数”。这一步比 UKL→CKL 更微妙，也更像真正让方法可用的关键。

Key Insight / Why It Works

这篇论文有效的最主要原因不是 Koopman 理论本身保证了线性化，而是三个 bias 的组合刚好适合 SLAM：第一，lifted bilinear form 给过程/观测模型足够表达力，能吸收传感器尺度、外参、小的动力学偏差；第二，SLAM 的显式 pose-landmark 结构被保留，没有退化成环境记忆；第三，manifold constraints 防止高维优化利用虚假自由度。

最可能的核心贡献是 RCKL 的 reduced process model。完整 Koopman process 要求 lifted feature set 近似不变子空间，这在有限维随机 Fourier features 或多项式 features 下通常不成立。CKL 虽然把状态拉回 manifold，但如果模型同时约束 feature evolution，错误的 feature dynamics 会通过约束反作用到原始状态。RCKL 切掉这一部分，相当于把 Koopman lifting 从“线性动态系统状态空间”重新解释成“非线性回归 basis + constrained latent representation”。这个解释比论文表面上的 Koopman linearization 更可靠。

UKL 失败也很有信息量：没有 manifold constraints 时，localization 在频繁观测下可能勉强工作，因为 measurement residual 会把解拉近训练 manifold；但 SLAM 中 landmark 也未知，pose 和 landmark 可以一起漂到 lifted space 的非物理区域，代价仍低。这个现象说明高维 lifting 在 estimation 中不是免费 lunch，必须有 representation alignment。

实验中 RCKL 优于 imperfect MB 很可能主要来自学习了 measurement/process 的系统偏差，而不是发现了某种更深的 SLAM 推理能力。尤其仿真里的尺度因子、真实 RFID 的量测 scale/response，都是 data-driven calibration 很容易带来收益的地方。RCKL 较少陷入 local minima 是有趣现象，但增益来源不清：可能是 lifted residual 改变了优化地形，也可能是 learned model/regularization/covariance 改变了权重，使 basin 更宽。文中未充分说明。

SQP、Cholesky、RTS 等主要是把方法做成可运行的 engineering/scaling 贡献；它们重要，但不是 conceptual novelty。SERFF 也是合理的 feature engineering，不应被解读为核心理论突破。

Relation To Prior Work

最接近的是 KoopSE、Koopman system identification、kernel/GP-based state estimation，以及传统 batch nonlinear SLAM。相对 KoopSE，本文的本质差异是把 landmark 从环境整体模型中解耦出来，形成可泛化到新 landmark 的双线性观测因子；这是真正新增的信息结构。相对传统 Koopman MPC/filtering，本文关注长时域 batch estimation，并正面处理 lifted manifold drift，这是更接近 SLAM 需求的问题。

与 GP/kernel SLAM 相比，本文不是把训练样本作为在线核回归记忆，而是离线压缩成固定维 lifted operators，因此推理复杂度不随训练数据增长。这是 scalability 上的实质差异。不过其表达力仍依赖 chosen features；只是从 nonparametric memory 转成 finite feature approximation。

与传统 model-based SLAM 相比，它没有改变 SLAM 的优化范式，仍是 residual + sparse linear algebra；改变的是 factor 的来源：从手工解析模型变成数据学习的 lifted bilinear model。看似“model-free”，实质上是“structured learned factors”。

与 SINDy 的关系也值得注意。RCKL 的 reduced model 很像用 nonlinear basis 回归原始状态演化，而不是完整预测 lifted state。这说明本文最有效部分并不是严格 Koopman invariant subspace，而是把 SINDy-like basis regression 与 constrained SLAM 优化结合起来。实质创新在于把这些已有思想组织成能处理 landmark unknown 的 batch estimator，并给出可扩展约束求解。

Dataset / Evaluation

评估覆盖两个仿真场景和两个真实数据场景：室内轮式机器人 range-to-cylinder landmarks，以及户外 golf cart RFID range。任务上同时评估 localization 和 SLAM，且测试 landmark 与训练 landmark 分离，能验证“不是记忆固定地图”的基本 claim。真实世界实验是本文可信度较强的部分，尤其 RFID sporadic measurement 对 UKL failure 和 RCKL necessity 有较好说明。

但 evaluation 主要验证的是同一系统/同一传感器类型内的泛化，而不是跨机器人、跨传感器、跨动力学类别泛化。训练需要 groundtruth trajectory 和 landmark measurement pairs，且真实实验使用了数据增强注入平移/旋转不变性；这意味着泛化能力部分来自已知 symmetry，而不是纯数据自动发现。

与 MB/MBI 的比较支持“当手工模型有偏差时，数据驱动模型更好”这一 claim；但 MBI 的偏差设置相对可学习，尤其是尺度/外参类误差。更难的模型错设，如非平稳噪声、滑移、遮挡相关 bias、错误 association，并未充分覆盖。论文没有大段证明 RCKL 的优势来自 Koopman lifting 而不是 data-driven calibration + feature regression。

Limitation

第一，方法强依赖训练数据覆盖。lifted matrices 只在训练 manifold 附近可信；测试状态、输入、landmark relative geometry 超出训练分布时，manifold constraints 只能保证变量形式合法，不能保证 learned operator 正确。核心能力可能主要来自数据覆盖。

第二，所谓 model-free 有明显前提：系统需能写成 control-affine 或近似 control-affine，观测需能分解成 state feature 与 landmark feature 的双线性形式，landmark association 默认已知，训练有 groundtruth。它没有解决数据关联、闭环鲁棒性、动态物体、感知前端不确定性等真实 SLAM 难点。

第三，lifting function 选择是隐含上限。SERFF 给了通用近似能力，但维度、频率尺度、正则化都会影响结果。文中手调超参数和数据增强仍占较大比重；如果换成更复杂状态空间或高维观测，feature dimension 和样本复杂度可能迅速变成瓶颈。

第四，RCKL 的理论解释仍偏经验。它避免拟合 feature dynamics 的理由很合理，但何时 reduced model 优于 full Koopman model、何时某些 features 的 dynamics 反而应被保留，文中未充分说明。作者也承认可能需要 hybrid method。

第五，covariance consistency 的解释有限。learned Q/R 来自 residual 和 priors，但当模型错设、lifting approximation error 与真实噪声混合时，这些 covariance 是否具有统计意义并不清楚。实验上 Mahalanobis 更好不等于理论一致。

第六，local minima 改善是有趣但未归因的现象。不能排除它主要来自 lifted cost 的重参数化、正则化、不同噪声权重或初始化路径，而不是 Koopman 表达的普遍优化优势。

Takeaway

  • 1. Koopman lifting 用于 SLAM 的关键不是“线性化”本身，而是如何防止 lifted optimizer 离开真实状态 manifold；representation alignment 是数据驱动状态估计的核心问题。
  • 2. 对有限维 Koopman 方法，完整预测 lifted features 往往是负担。
  • 把 lifted features 当作 constrained basis，而不是必须动态闭合的状态，可能是更实用的路线。
  • 3. 数据驱动 SLAM 最有希望的近期形态不是端到端替代因子图，而是学习 structured factors：保留 pose-landmark sparsity、显式几何变量和可解释优化，只替换难建模的过程/观测函数。

一句话总结

这篇论文把 Koopman/SINDy-style lifted regression 与 constrained batch SLAM 结合起来，真正贡献不是泛泛的 model-free SLAM，而是证明了在显式 manifold 约束和 reduced feature dynamics 下，数据学习的 lifted factors 可以成为传统 SLAM 因子的可扩展替代。