IEEE Transactions on Robotics

Stochastic Adaptive Estimation in Polynomial Curvature Shape State Space for Continuum Robots

作者：Guoqing Zhang, Long Wang · 机构：Department of Mechanical Engineering, Stevens Institute of Technology, New Jersey, NJ, USA

会议/期刊：IEEE Transactions on Robotics · 发布日期：2026 · 页码：261-280

传感器状态估计

精读笔记

Problem Setting

Stochastic Adaptive Estimation in Polynomial Curvature Shape State Space for Continuum Robots（IEEE Transactions on Robotics / 2026）要处理的是连续体机器人在 limited / sparse sensing 下的实时 shape state estimation。这里的“shape”不是末端 pose，也不是 CC 的一个 bending angle，而是沿弧长分布的曲率函数。

真正困难点是信息不匹配：机器人形状本质上是连续分布，传感器却通常只有一个或少数离散 pose/position/orientation；同时外载、摩擦、传动误差会让真实曲率偏离常曲率假设。以前 CC/PCC + EKF 卡在模型 bias，CRT/GPR 等路线又卡在计算、输入需求或在线部署复杂度。关键矛盾是：表达力越强，需要估计的模态越多，可观性和抗噪性越差；表达力越弱，滤波更稳但系统性错。

Motivation

作者不是单纯想把 EKF 换成 IMM，而是认为 shape estimation 的状态空间选错了。很多现有方法在 Cartesian / configuration space 做滤波，实际是在末端观测误差上打补丁；但连续体机器人的非理想行为首先体现在 curvature distribution 的变化上。把曲率模态作为状态，能让测量 residual 更直接地约束形状生成机制。

关键缺口有三个：第一，缺少 sparse sensing 下直接在 curvature modal space 做 stochastic estimation 的框架；第二，缺少把 polynomial order 作为在线不确定性处理的多模型滤波；第三，sensor arrangement 的分析多数停留在 rank 或几何直觉，没有把 measurement noise 对曲率模态可辨识性的影响纳入。论文的动机可以概括为：不要再假设一个固定曲率复杂度，而应让估计器在测试时根据观测选择形状复杂度。

Core Idea

核心思想是用低阶 polynomial curvature kinematics 构造一个可滤波的 shape latent space：状态不是末端角度，而是 \([m_0,m_1,m_2,...]\)，这些系数定义 \(\kappa(s)=\sum m_i s^i\)，再通过积分得到整条 backbone 的 pose。这个表示的 inductive bias 很明确：形状可以复杂于常曲率，但仍是平滑、低维、全局耦合的曲率场。

第二个核心是把 CC、PCK-1、PCK-2 看成同一 curvature family 下的不同模型阶数，用 IMM-EKF 同时维护多个假设并按 measurement likelihood 加权。它改变的信息流是：测量不再只更新一个固定维度的状态，而是同时更新“哪个曲率复杂度更可信”。这比单一高阶 EKF 更稳，因为简单场景下低阶模型不会被高阶噪声自由度拖累；也比单一 CC 更 general，因为复杂加载时高阶模型可以接管。

Method

1. Polynomial curvature state：解决 CC/PCC 的结构性偏差。它把 shape estimation 变成低维函数系数估计，而不是末端姿态拟合。核心变化是将曲率分布作为 latent state，使传感器 residual 能通过 analytic Jacobian 反传到各阶曲率模态。

2. PCK measurement model + EKF：解决 sparse pose/position/orientation 如何约束曲率系数的问题。测量函数由曲率积分得到位置和角度，Jacobian 显式计算。这部分并不新奇，但它是整个 estimator 能在线运行的关键，因为 PCK 给了可微且便宜的 forward model。

3. IMM over polynomial orders：解决模型阶数未知和工况切换。每个阶数一个 EKF，innovation likelihood 更新模型概率，再融合估计。机制上相当于在线 bias-variance control。

4. Dimension-aware IMM mixing：解决不同 polynomial order 状态维度不一致。简单补零会把高阶项的未知性错误编码成确定为零，造成混合 bias。作者的 augmentation / reset 策略是合理的，但具体形式偏工程化。

5. Adaptive TPM：解决固定 Markov transition 在突然切换时响应慢的问题。它根据模型概率变化和 likelihood ratio 调整转移概率。这个模块可能有用，但依赖阈值，理论上不如前面几部分干净。

6. Noise-weighted observability：解决 sensor placement 不能只看 full rank 的问题。用 \(O^T R^{-1}O\) 衡量 noisy sensing 下曲率模态的可辨识性，本质是 Fisher-information-like 的局部分析。

Key Insight / Why It Works

这篇最有效的部分大概率不是“EKF”或“IMM”本身，而是 representation alignment：真实形变误差发生在曲率分布，状态也放在曲率分布的低维模态里。相比在末端 pose 空间修正，PCK state 让滤波器的每一次更新都对应一条全局一致的 backbone shape；这会显著减少 sparse sensing 下的自由度浪费。

IMM 的价值在于 test-time model complexity adaptation。低阶模型提供强 regularization，高阶模型提供 expressivity，模型概率就是一个在线复杂度选择器。这个机制特别适合连续体机器人，因为外载或接触并不一定改变 actuator command，却会改变 curvature profile 的复杂度。单一 PCK-2 虽然表达力强，但在真实简单场景下可能把噪声解释成高阶曲率；IMM 可以让低阶模型在这些阶段占优。

noise-weighted observability 的 insight 也值得迁移：在稀疏传感形状估计中，“可观”不是二值 rank 问题，而是哪些 latent modes 在当前噪声水平下可区分。这个比单纯比较 sensor 数量更接近工程现实。

我会把贡献排序为：第一，curvature modal state + sparse stochastic estimation；第二，IMM 作为 polynomial order adaptation；第三，noise-weighted observability；第四，adaptive TPM 和具体 mixing trick。后两者更像强化稳定性和性能的 engineering refinement。DAIMM/SAIMM 的差异说明，过度激进的模式切换未必是根本，稳定的 representation 和合理的模型集合更重要。

Relation To Prior Work

最接近的谱系是 CC/PCC-EKF、modal curvature representation、PCK kinematics、以及 tracking 领域的 IMM。论文的新意不是发明 PCK，也不是发明 IMM，而是把 IMM 放到 curvature modal shape space，并在 sparse sensing 的连续体机器人形状估计里系统验证。

与 CC/PCC-EKF 的本质差异：不是把 backbone 分成更多常曲率段，而是用全局 polynomial curvature basis 表示非均匀曲率。这带来更少的参数和更强的平滑先验，但也牺牲局部形变表达。

与 CRT/GPR 的差异：它不显式建力学，也不追求物理完整性，而是选择一个足够轻的几何生成模型用于实时 observer。换句话说，这是 model-based filtering 的实用主义路线，不是 mechanics-first 路线。

与已有 modal fitting / optimization 的差异：本文强调动态 stochastic observer 和在线模型阶数混合，而不是静态曲线拟合。真正新增的信息在于：多项式阶数的不确定性可以被作为 Markov mode 处理，并且 sparse sensor 的可观性应在 curvature state 下评估。

Dataset / Evaluation

评估设计比较有针对性：仿真里先看传感器布置和 observability，再看 IMM 与单模型 EKF；真机里用 cable-driven prototype 做单段、双段、加载/卸载，并且故意只用单 tip pose 作为 estimator 输入。这能较好支持“limited sensing 下 IMM-PCK 比 CC 和固定 PCK 更稳”的 claim。

但 evaluation 的覆盖范围仍有限。主要是 planar bending，速度较慢，加载方式偏准静态，真实 ground truth 来自视觉 marker 重建，本身有标定误差。双段实验能说明方法可扩展到更复杂形状，但还不能证明 full 3D、扭转、强接触和快速动态下成立。

benchmark 没有明显泄漏问题，因为不是 data-driven learning；但增益归因仍有一点混合：PCK 表达力、IMM 混合、TPM adaptation、状态维度处理都同时存在。作者用 single-PCK EKF 做 baseline 已经部分隔离了 IMM 的贡献，这是优点；不过 adaptive TPM 和 dimension mixing 各自贡献还可以更干净地 ablate。

Limitation

最大限制是 operating envelope 很窄：平面弯曲、不可伸长、低阶平滑曲率、无显式扭转、外力作为过程噪声。这些假设在很多 tendon-driven bench-top 场景合理，但不是通用连续体机器人形状估计。

稀疏 sensing 的信息上限没有被突破。单 tip pose 对高阶曲率模态天然不充分，IMM 只能在候选模型中选择最能解释观测的一个/几个，不能恢复观测不可区分的内部形变。如果存在局部接触导致曲率尖峰，低阶 PCK 会系统性平滑掉。

process model 较弱。用 tip bending angle increment 通过 pseudoinverse 分配到 modal coefficients，本质上是一个 kinematic prior，不是真实 shape dynamics。外载、摩擦、线缆迟滞都被塞进噪声；这在估计上可行，但也意味着泛化主要依赖传感器更新和模型集合覆盖，而不是动力学预测能力。

adaptive TPM 的理论地位不强，阈值依赖经验。DAIMM 在仿真切换中表现好，但真实部署中 likelihood 不可分、传感器噪声非高斯或标定漂移时，激进切换可能引入抖动。文中未充分说明这些阈值如何跨平台选择。

扩展到 SE(3) 不是简单加一个 \(\delta\)。一旦 twist、out-of-plane curvature 和 bending plane dynamics 进入状态，维度、可观性和模型切换复杂度都会显著上升；当前 polynomial order IMM 的简洁性可能被破坏。

Takeaway

  • 1. 对 sparse shape sensing，最重要的不是滤波器多复杂，而是 state representation 是否和真实形变机制对齐；curvature modal space 是比末端 pose / CC parameter 更合理的中间层。
  • 2. 模型阶数应该被视为在线 latent variable。
  • 连续体机器人在不同载荷和接触下的曲率复杂度会变，固定低阶或固定高阶都不是最优。
  • 3. Sensor placement 应围绕 latent-state observability 设计，而不是只看传感器数量或几何直觉；noise-weighted observability 这个思想可以迁移到软体机器人、形变物体、柔性器械等问题。

一句话总结

这篇论文把连续体机器人稀疏形状估计从固定常曲率滤波推进到“曲率模态表示 + 在线模型阶数自适应”的实用 observer 路线，真正贡献在 representation alignment 和 test-time complexity selection，而不是 EKF/IMM 算法本身。