PAPER DEEP DIVE
绳驱机器人的移动时域估计与非线性模型预测控制
提出绳驱机器人的移动时域估计与非线性模型预测控制方法,处理绳驱系统的柔性与时变特性,提升轨迹跟踪精度。
1. 论文概览：缆驱动软体操作器的移动视野估计与非线性模型预测控制
本文由 Lingxiao Xun、Haihong Li、Gang Zheng 于 2026 年 7 月发表，提出了基于简化 Cosserat 动力学的缆驱动软体操作器移动视野估计（MHE）与非线性模型预测控制（NMPC）框架。软体操作器的精确控制因难以开发既准确又计算可处理的模型而极具挑战。简化 Cosserat 杆模型提供了基于物理、面向控制的软体机器人动力学描述，是纯数据驱动输入-输出表示的显式替代。本文开发了平滑的缆长度驱动建模表述——通过近似缆张力与缆松弛间的互补关系，实现无需直接张力传感的缆长度控制。基于此，MHE 方法估计简化状态并从末端位姿测量与缆长度信息重建操作器构型；NMPC 控制器在缆长度与缆速率约束下实现任务空间控制。框架通过仿真与四缆物理原型实验验证，展示了位姿调节与实时末端位置跟踪。
2. 核心问题：软体操作器的建模与控制困境
软体操作器的精确控制面临根本困难：连续体动力学高度非线性、无限维，且缆驱动的单边张力-松弛行为使传统力控制需集成力传感器并实现复杂算法管理缆张力波动。力控制在工程实践中挑战显著，而控制缆长度被证明是有效替代，极大简化实际应用。但传统力控制算法难以过渡到软体操作器的缆长度控制。本文要解决的核心问题是：如何为缆驱动软体操作器建立既物理准确又计算可处理的模型，并在此基础上实现状态估计与约束控制。
3. 方法：MHE-NMPC 框架
3.1 简化 Cosserat 动力学模型
在 Cosserat 框架中，软体操作器被视为沿中心线的一系列刚性横截面。任意横截面的齐次变换矩阵为：
$$\forall s \in [0, L], \quad \boldsymbol{g}(s, t) = \begin{bmatrix} \boldsymbol{R}(s, t) & \boldsymbol{p}(s, t) \\ \boldsymbol{0} & 1 \end{bmatrix} \in SE(3)$$其中 $\boldsymbol{p}(s, t) \in \mathbb{R}^3$ 为位置向量，$\boldsymbol{R}(s, t) \in SO(3)$ 为旋转矩阵，$L$ 为总弧长。应变与速度定义为齐次变换矩阵的左平凡化切空间：
$$\hat{\boldsymbol{\xi}}(s, t) = \boldsymbol{g}^{-1} \boldsymbol{g}' \in \mathfrak{se}(3) \simeq \mathbb{R}^6, \quad \hat{\boldsymbol{\eta}}(s, t) = \boldsymbol{g}^{-1} \dot{\boldsymbol{g}} \in \mathfrak{se}(3) \simeq \mathbb{R}^6$$为用有限变量集表示状态，用简化自由度集参数化应变场：
$$\boldsymbol{\xi}(s, t) = \boldsymbol{\xi}_0 + \boldsymbol{\Phi}(s)\, \boldsymbol{q}(t)$$其中 $\boldsymbol{q}(t)$ 为简化广义坐标，$\boldsymbol{\Phi}(s)$ 为形函数矩阵。
3.2 含驱动约束的缆驱动动力学
缆通过操作器内部路径布线并锚定于某一横截面（通常自由端）。缆的单边行为表现为张力-松弛互补条件：缆只能拉不能推。传统互补条件不可微，无法直接用于基于梯度的优化。本文引入松弛变量，将互补条件近似为平滑等式约束：
$$\text{互补条件: } \lambda \geq 0, \quad \delta \geq 0, \quad \lambda \cdot \delta = 0 \quad \Longrightarrow \quad \text{平滑近似: } \lambda \cdot \delta \approx \epsilon$$其中 $\lambda$ 为缆张力，$\delta$ 为松弛量，$\epsilon$ 为小参数。这使缆长度输入可纳入可微优化框架。
3.3 NMPC-SQP 框架
将时间连续优化重新表述为时间离散的非线性规划（NLP）问题。定义离散时间点 $t_{-M}, \dots, t_0, \dots, t_P$，其中 $t_{-M}=t-t_{\text{est}}$（估计窗），$t_0=t$（当前），$t_P=t+t_{\text{ctrl}}$（预测窗），步长 $h$。状态 $\boldsymbol{x}_i = [\boldsymbol{q}_i^\top, \boldsymbol{q}_{i-1}^\top]^\top$。估计与预测作为独立优化问题求解。NMPC 控制器在缆长度与缆速率约束下计算任务空间控制：
$$\arg\min_{\boldsymbol{u}_{0:P-1}} \sum_{i=0}^{P-1} \mathcal{J}_{\text{ctrl}}(\boldsymbol{x}_i, \boldsymbol{u}_i) + \mathcal{J}_{\text{term}}(\boldsymbol{x}_P)$$受动力学约束、缆长度约束 $\boldsymbol{u}_{\min} \leq \boldsymbol{u}_i \leq \boldsymbol{u}_{\max}$ 与缆速率约束 $|\Delta \boldsymbol{u}_i| \leq \Delta \boldsymbol{u}_{\max}$。MHE 估计问题类似地在过去视野上最小化估计残差。
4. 数值仿真
4.1 仿真设置
仿真用 16 缆软体操作器验证。测试包括 MHE 构型估计、恒定位姿控制、轨迹跟踪控制、应变-位姿耦合控制。位姿控制目标设为 $\boldsymbol{\phi}_r = [1, -2, -3]^\top$、$\boldsymbol{p}_r = [5, 10, 8]^\top$，控制输入为缆长度。仿真时间步 0.005 s，预测视野内相邻预测构型间隔 0.01 s。
4.2 仿真结果
位姿控制测试中，软体操作器从重力影响下的稳态快速移动到固定目标位姿，预测构型与实际构型一致。轨迹跟踪与应变-位姿耦合控制均展示框架能力。预测视野时长的影响也被分析。
5. 实验验证
5.1 实验设置
物理原型为四缆软体操作器：硅胶锥体主体，沿轴向等距分布变径刚性套筒，四缆穿过套筒锚定于终端套筒。材料参数：杨氏模量 $E = 2.563 \times 10^5$ Pa，剪切模量 $G = 8.543 \times 10^4$ Pa，密度 $\rho = 1.41 \times 10^3$ kg/m³。控制平台含 4 步进电机、磁传感器（LIBERTY 240/16）与计算机。
| 组件 | 规格 | 用途 |
|---|---|---|
| 软体操作器主体 | 硅胶锥体 + 变径刚性套筒 | 承载缆接触力并传递至硅胶体 |
| 驱动 | 4 台步进电机 | 控制四根缆 |
| 传感 | LIBERTY 240/16 磁传感器 | 检测末端位置 |
| 计算 | 计算机 | 运行观测器与控制器算法 |
5.2 实验结果
MHE 观测器：操作者随机拉拽四缆产生运动，MHE（时域 0.3 s，离散 3 步每步 0.1 s）基于末端测量轨迹估计操作器形状。估计误差快速收敛并保持有界，视觉对比定性确认估计形状与物理原型变形一致。轨迹跟踪：闭环实验集成 MHE 观测器与 MPC，在预定轨迹与随机变化轨迹两种场景下评估末端跟踪性能，展示准确的位置跟踪。实时标记跟踪与计算性能验证了实时可行性。
| 实验项 | 设置 | 结果 |
|---|---|---|
| MHE 观测器 | 时域 0.3s, 3 步×0.1s | 误差快速收敛且保持有界 |
| 轨迹跟踪 | 预定 + 随机轨迹 | 准确末端位置跟踪 |
| 实时标记跟踪 | 四缆闭环 | 实时可行 |
| 计算性能 | 四缆原型 | 满足实时约束 |
6. 局限性
- 平滑近似的精度：缆互补条件的平滑近似引入小参数 $\epsilon$ 的近似误差，在缆接近完全松弛时精度下降。
- 材料参数标定：依赖标定的杨氏模量、剪切模量与密度，材料参数不确定性影响模型精度。
- 传感器依赖：MHE 需末端位姿测量与缆长度信息，磁传感器在特定环境下可能受干扰。
- 四缆限制：实验原型仅四缆，更多缆或更复杂构型的可扩展性需进一步验证。
- 形状控制未实验：实验聚焦末端位置跟踪，形状控制等更复杂目标仅在仿真中验证。
7. 总结
本文为缆驱动软体操作器提出统一建模、估计与控制框架。缆驱动机构的单边张力-松弛行为先用互补条件表述，再通过松弛变量近似为平滑等式约束，使缆长度输入纳入可微优化。推导了操作器动力学与缆驱动约束的解析 Jacobian。基于简化 Cosserat 杆动力学开发 MHE-NMPC 框架：移动视野估计器从末端位姿与缆长度重建简化状态与构型，NMPC 控制器在缆长度与缆速率约束下计算缆长度命令。数值仿真展示位姿调节、轨迹跟踪与应变相关控制能力；四缆原型实验验证实时实现与准确末端位置跟踪。核心洞见是：将缆的互补条件平滑化，使物理准确的 Cosserat 动力学与基于梯度的 MPC 首次在软体操作器上实时协同——精确建模与约束控制不再对立。
flowchart TD
A["缆长度输入 u"] --> B["平滑缆驱动约束
互补条件 → 平滑等式"]
C["末端位姿测量"] --> D["MHE 移动视野估计器"]
E["缆长度信息"] --> D
D --> F["估计简化状态 q
重建操作器构形"]
F --> G["NMPC 控制器
缆长度+缆速率约束"]
B --> G
G --> H["最优缆长度命令"]
H --> I["步进电机执行"]
I --> A
J["简化 Cosserat 动力学
ξ = ξ₀ + Φ·q"] --> G
J --> D
