💻 computer science

Factor Graph-Based Shape Estimation for Continuum Robots via Magnus Expansion

该论文提出了一种基于因子图的连续体机器人形状估计方法，通过结合 Magnus 展开推导的运动学因子将低维几何可变应变参数化嵌入概率框架，从而在稀疏噪声数据下实现了兼具紧凑状态表示与不确定性量化的精确形状重建。

原作者： Lorenzo Ticozzi, Patricio A. Vela, Panagiotis Tsiotras

发布于 2026-04-20

📖 1 分钟阅读☕ 轻松阅读

原作者： Lorenzo Ticozzi, Patricio A. Vela, Panagiotis Tsiotras

原始论文采用 CC BY 4.0 许可（http://creativecommons.org/licenses/by/4.0/）。 ✨ 这是对下方论文的AI生成解释。它不是由作者撰写或认可的。如需技术准确性，请参阅原始论文。阅读完整免责声明

这篇论文介绍了一种让软体机器人（Continuum Robots）“看清”自己身体形状的新方法。

想象一下，软体机器人就像一条没有骨头的橡皮蛇或章鱼触手。它们非常灵活，能钻进狭窄的管道或进行精细的手术。但正因为它们太软、太灵活，想要知道它们此刻弯成了什么样子（比如头在哪里、身体扭了多厉害），是一件非常困难的事情。

这就好比你在黑暗中摸一条橡皮蛇，手里只有几个模糊的传感器（比如只摸到了蛇头，或者只摸到了蛇身中间的一点），却想还原出整条蛇在空中的完整姿态。

1. 以前的难题：两个极端的“笨办法”

在解决这个问题时，以前的科学家主要面临两个选择，就像走钢丝：

方法 A：简单的“数学模型派”
- 做法：假设蛇的形状很简单（比如只是简单的弯曲），用很少的数学参数来描述。
- 优点：算得快，像用几根火柴棍就能搭出个大概。
- 缺点：太死板。如果蛇扭得很复杂，这个模型就“瞎”了，而且它不知道自己的判断有多大的把握（缺乏概率思维）。
方法 B：复杂的“像素派” (Cosserat 杆)
- 做法：把蛇切成几百段，每一段都当成一个独立的变量去计算。
- 优点：非常精确，能处理复杂的扭曲，还能告诉你哪里算得准、哪里不准。
- 缺点：太慢了！因为变量太多，就像要同时解几百个方程，电脑容易“死机”。而且，如果传感器数据很少，这种海量计算反而容易算错。

核心矛盾：要么算得快但不够准，要么算得准但太慢且太复杂。

2. 这篇论文的“魔法”：给橡皮蛇穿上“智能紧身衣”

这篇论文提出了一种混合策略，结合了上述两种方法的优点。他们做了一件很巧妙的事：

第一步：用“低维紧身衣”代替“像素点”

他们没有把蛇切成几百段，而是给蛇穿上了一件特制的“紧身衣”（学术上叫 GVS 参数化）。

比喻：想象这件紧身衣是由几根弹性弹簧（基函数）编织而成的。无论蛇怎么扭，它都只由这几根弹簧的拉伸程度（系数）来决定。
好处：我们不需要计算几百个点的状态，只需要计算这几根弹簧的状态。这就把问题从“几百个变量”压缩成了“几个变量”，瞬间变快了。

第二步：引入“马格努斯因子”作为“物理导航员”

这是论文最核心的创新点。他们引入了一种叫马格努斯展开（Magnus Expansion）的数学工具，并将其封装成一个“马格努斯因子”。

比喻：想象你在玩一个拼图游戏。
- 以前的方法：给你几块拼图（传感器数据），让你猜整幅画。
- 这篇论文的方法：除了给你几块拼图，还给了你一张**“物理定律说明书”**。这张说明书告诉你：“如果这根弹簧拉长了，蛇头必须往左偏 30 度，否则就违反物理定律了。”
- 这个“说明书”就是马格努斯因子。它把**蛇的弯曲（应变）和蛇的姿态（位置）**之间复杂的物理关系，用一种精确的数学公式锁死了。

3. 他们是怎么工作的？（因子图侦探）

他们使用了一个叫**“因子图” (Factor Graph)** 的工具，这就像是一个超级侦探网络：

变量节点（圆圈）：代表蛇的几根“弹簧”状态（我们要猜的）和蛇身上几个关键点的姿态。
因子节点（方块）：代表各种约束条件。
- 测量因子：传感器说“我摸到了这里”，这就是一个约束。
- 马格努斯因子（核心）：物理定律说“如果你这样弯，你就必须那样转”，这也是一个约束。
- 先验因子：告诉侦探“蛇尾巴通常是固定的”。

侦探的工作：把所有这些约束（传感器数据 + 物理定律）放在一起，寻找一个最可能的答案，让所有的矛盾最小化。

4. 结果如何？

他们在电脑里模拟了一个 40 厘米长的软体机器人，做了三次测试：

只有姿态测量（知道头和尾在哪）。
混合测量（知道中间某点的弯曲程度，也知道头尾）。
只有位置测量（只知道几个点在哪，完全不知道方向）。

惊人的结果：

精度极高：即使在只有位置信息（不知道方向）的情况下，他们的误差也比以前的方法（高斯过程回归）小了6 倍！
为什么？ 因为那个“马格努斯因子”（物理说明书）强行约束了方向。即使传感器没告诉你方向，物理定律也告诉你：“如果你身体弯成这样，头不可能朝那个方向”。
速度快：虽然考虑了复杂的物理关系，但因为只计算几根“弹簧”，速度依然很快，适合实时控制。

总结

这篇论文就像给软体机器人装上了一个**“物理直觉”**。

以前的方法要么靠猜（模型太简单），要么靠死算（模型太复杂）。
现在的方法，是用少量的参数（几根弹簧）来描述形状，同时用严格的物理定律（马格努斯因子）来确保这些参数符合现实。

一句话概括：
这就好比你在黑暗中摸一条橡皮蛇，以前你可能只能摸到几个点就瞎猜；现在，你手里多了一本**“橡皮蛇物理指南”**，哪怕只摸到一个点，你也能根据指南里的定律，精准地推断出整条蛇在空中的完美姿态。这让软体机器人能更聪明、更安全地执行任务（如微创手术或太空维修）。

基于因子图与 Magnus 展开的连续体机器人形状估计技术总结

1. 研究背景与问题定义

连续体机器人（Continuum Robots）因其柔顺性和安全性，在微创手术、药物递送及太空服务等领域应用广泛。然而，由于其具有无限维的分布参数特性和强几何非线性，从稀疏且含噪的传感器数据中重构其形状（Shape Estimation）极具挑战性。

现有的形状估计方法主要存在两种范式及其局限性：

参数化方法（如分段常数曲率 PCC、几何可变应变 GVS）：能提供紧凑的状态表示，便于模型控制，但缺乏概率结构，难以处理不确定性。
基于因子图的 Cosserat 杆推理：能提供原则性的不确定性量化和模块化，但状态维度随空间离散化节点数量线性增长，计算效率低，且生成的状态向量（节点位姿）不直接适用于基于模型的控制。

核心问题：如何结合参数化方法的紧凑性与因子图方法的概率优势，在稀疏测量条件下实现高精度、低维度的连续体机器人形状估计？

2. 方法论

本文提出了一种基于因子图（Factor Graph）框架，结合几何可变应变（GVS）参数化与Magnus 展开的新型形状估计方法。

2.1 核心数学框架

GVS 参数化：将应变场 $\xi(s)$ 投影到有限基函数（如 B 样条）上，用低维广义应变坐标向量 $q$ 表示。这使得机器人构型由低维向量 $q$ 和离散节点位姿 $g_i$ 共同描述，而非无限维函数。
Magnus 展开：利用 Magnus 展开将应变场积分转化为闭合形式的位姿更新公式。即相邻节点位姿 $g_{i+1}$ 与 $g_i$ 之间的关系由应变场在区间上的积分决定：
$g_{i+1} = g_i \exp(\hat{\Omega}_{i+1}^i(q))$
其中 $\hat{\Omega}$ 是应变场的 Magnus 展开项。

2.2 因子图构建

作者构建了一个包含以下因子的因子图，以最大化后验概率（MAP）：

Magnus 因子（核心创新）：
- 作为先验约束，连接相邻的位姿变量 $g_i, g_{i+1}$ 和全局应变系数 $q$ 。
- 编码了 GVS 参数化下的运动学闭合解，强制位姿变化符合物理应变模型。
- 即使在没有直接姿态测量的情况下，也能通过应变与位姿的非线性耦合约束姿态估计。
测量因子：
- 支持多种传感器模态：应变测量、位姿测量（Pose）、位置测量（Position）。
- 将传感器读数与模型预测值之间的残差建模为高斯势函数。
先验因子：
- 对根节点位姿 $g_0$ 施加初始条件约束（恒等矩阵）。
- 对应变系数 $q$ 施加正则化（Tikhonov 正则化），防止过拟合。

2.3 优化求解

将最大后验估计转化为非线性最小二乘问题（NLLS），利用 Levenberg-Marquardt (LM) 算法求解。由于使用了局部支撑的 B 样条基函数，残差雅可比矩阵具有稀疏性，保证了计算效率。

3. 主要贡献

范式融合：成功将低维 GVS 参数化嵌入因子图框架，既保留了紧凑的状态表示（利于控制），又具备因子图的概率推理能力和模块化优势。
新型 Magnus 因子：提出了一种基于 Magnus 展开的因子，将软体机器人的运动学闭合解作为先验知识编码进图中。这解决了传统方法中应变与位姿解耦或依赖离散节点的问题。
稀疏测量下的鲁棒性：证明了在仅有位置测量（无直接姿态测量）的情况下，该方法仍能通过 Magnus 因子隐含的非线性耦合约束，准确推断出姿态和应变场。
计算效率与精度平衡：相比传统的 Cosserat 杆离散化方法，状态维度不随离散节点数增加而爆炸；相比纯参数化方法，具备处理噪声和不确定性的能力。

4. 实验结果

在仿真中，使用了一根 0.4 米长的三腱驱动连续体机器人（TDCR），在三种不同的测量配置下进行了评估：

场景 S1（位姿测量）：在中间和尖端有姿态测量。平均位置误差 1.45 mm，姿态误差 1.46°。
场景 S2（应变 + 位姿混合）：在部分节点有应变和位姿测量。位置估计在中间区域方差稍大，但整体精度依然很高。
场景 S3（仅位置测量）：仅在 5 个节点有位置测量。
- 平均位置误差 1.23 mm。
- 尽管没有直接姿态测量，平均姿态误差控制在 10.60°。
- 对比基线：与基于高斯过程（GP）回归的基线方法相比，在仅位置测量场景下，本方法的姿态误差降低了6 倍（基线方法在类似条件下姿态误差高达 63°）。

计算性能：虽然由于全局应变变量 $q$ 与所有节点连接导致因子图稠密性增加，但得益于低维参数化，求解时间（约 1-2ms）仍具有竞争力，且远优于基线方法在稀疏测量下的收敛困难问题。

5. 意义与结论

理论意义：提出了一种新的软体机器人状态估计范式，证明了利用 Magnus 展开将高阶几何耦合关系作为先验约束的有效性。
应用价值：
- 该方法生成的低维应变系数 $q$ 可直接用于基于模型的反馈控制，填补了感知与控制之间的鸿沟。
- 在传感器稀疏或仅能提供部分状态（如仅有位置）的极端条件下，表现出极强的鲁棒性，适合实际应用中传感器受限的场景。
未来展望：该方法为连续体机器人的实时状态估计和模型预测控制（MPC）提供了坚实的理论基础，后续工作将致力于硬件验证。

总结：本文通过引入 Magnus 因子，巧妙地将连续体机器人的物理运动学约束融入概率图模型，解决了传统方法在“紧凑性”与“不确定性量化”之间的权衡难题，显著提升了在稀疏测量下的形状估计精度和鲁棒性。