任务空间惯量整形无源性：比较PD+、计算力矩法与完全自然解耦法（FND）

1. 引言

在冗余机器人的多任务控制中，如何处理好“任务空间惯性矩阵”与“科氏力矩阵”的关系，是决定系统稳定性、解耦能力和硬件鲁棒性的核心问题。本文从统一的动力学基础出发，系统性地对比三种典型控制律：

• PD+ 控制（自然惯性，任务全耦合）
• 计算力矩法 / 逆动力学（强制常数惯性，完全解耦但依赖精确模型）
• 完全自然解耦法（FND）（保留自然对角惯性，完全解耦且无源性守恒）

---

2. 统一的动力学基础

2.1 关节空间动力学

刚性机器人关节空间动力学方程为：

$$M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = \tau + \tau^{\mathrm{ext}} \tag{1}$$

其中：

• $M(q) \in \mathbb{R}^{n\times n}$：正定对称惯性矩阵
• $C(q,\dot{q}) \in \mathbb{R}^{n\times n}$：科里奥利/离心力矩阵
• $g(q) \in \mathbb{R}^{n}$：重力项
• $\tau, \tau^{\mathrm{ext}} \in \mathbb{R}^{n}$：控制力矩与外力矩

核心物理性质（斜对称性）：

$$\dot{M}(q,\dot{q}) = C(q,\dot{q}) + C(q,\dot{q})^T \tag{2}$$

或等价地，$\dot{M} - 2C$ 为斜对称矩阵。这一性质保证自然系统满足能量守恒，是实现无源性的根基。

2.2 任务空间变换

定义 $r$ 个任务，总任务向量 $x \in \mathbb{R}^n$ 满足 $\sum m_i = n$。速度映射关系为：

$$\dot{x} = J(q)\dot{q}, \quad J(q) = \frac{\partial x}{\partial q} \in \mathbb{R}^{n\times n} \tag{3}$$

力的映射（虚功原理）为：

$$\tau = J^T F, \quad \tau^{\mathrm{ext}} = J^T F^{\mathrm{ext}} \tag{4}$$

将式 (1) 变换到任务空间，得到标准任务空间动力学：

$$\Omega \ddot{x} + \mu \dot{x} + \Omega J M^{-1} g = F + F^{\mathrm{ext}} \tag{5}$$

其中任务空间惯性矩阵定义为：

$$\Omega = (J M^{-1} J^T)^{-1} \tag{6}$$

任务空间科里奥利矩阵为：

$$\mu = \Omega (J M^{-1} C - \dot{J}) J^{-1} \tag{7}$$

自然铁律延续：坐标变换不改变物理本质，仍有：

$$\dot{\Omega} = \mu + \mu^T \tag{8}$$

---

3. 三种控制器的控制律与闭环动力学

3.1 PD+ 控制（自然惯性，完全耦合）

控制律

PD+ 控制在任务空间中的表达为（忽略重力，仅展示核心结构）：

$$F = \Omega \ddot{x}_d + \mu \dot{x} - D \dot{\tilde{x}} - K \tilde{x} \tag{9}$$

或等价地在关节空间表示为：

$$\tau = J^T (\Omega \ddot{x}_d + \mu \dot{x} - D \dot{\tilde{x}} - K \tilde{x}) + g \tag{10}$$

其中 $\tilde{x} = x - x_d$，$\dot{\tilde{x}} = \dot{x} - \dot{x}_d$，$K, D$ 为对角正定增益矩阵。

闭环动力学

将式 (9) 代入式 (5)，消去 $F$ 和 $\ddot{x}_d$：

$$\Omega \ddot{\tilde{x}} + (\mu + D)\dot{\tilde{x}} + K \tilde{x} = F^{\mathrm{ext}} \tag{11}$$

无源性分析

取存储函数：

$$V = \frac{1}{2} \dot{\tilde{x}}^T \Omega \dot{\tilde{x}} + \frac{1}{2} \tilde{x}^T K \tilde{x} \tag{12}$$

对时间求导：

$$\dot{V} = \dot{\tilde{x}}^T \Omega \ddot{\tilde{x}} + \frac{1}{2} \dot{\tilde{x}}^T \dot{\Omega} \dot{\tilde{x}} + \dot{\tilde{x}}^T K \tilde{x} \tag{13}$$

利用式 (11) 将 $\Omega \ddot{\tilde{x}}$ 替换为 $-(\mu + D)\dot{\tilde{x}} - K \tilde{x} + F^{\mathrm{ext}}$：

$$\dot{V} = \dot{\tilde{x}}^T F^{\mathrm{ext}} - \dot{\tilde{x}}^T D \dot{\tilde{x}} + \frac{1}{2} \dot{\tilde{x}}^T (\dot{\Omega} - 2\mu) \dot{\tilde{x}} \tag{14}$$

利用自然铁律 $\dot{\Omega} = \mu + \mu^T$，得：

$$\frac{1}{2} \dot{\tilde{x}}^T (\dot{\Omega} - 2\mu) \dot{\tilde{x}} = \frac{1}{2} \dot{\tilde{x}}^T (\mu + \mu^T - 2\mu) \dot{\tilde{x}} = 0 \tag{15}$$

因此：

$$\dot{V} = \dot{\tilde{x}}^T F^{\mathrm{ext}} - \dot{\tilde{x}}^T D \dot{\tilde{x}} \le \dot{\tilde{x}}^T F^{\mathrm{ext}} \tag{16}$$

结论：PD+ 是严格输出无源的，功率端口为 $(\dot{x}, F^{\mathrm{ext}})$。

---

3.2 计算力矩法 / 逆动力学（强制常数惯性）

控制律

计算力矩法（逆动力学 #1）强制闭环惯量为单位阵 $I$：

$$F = \Omega \ddot{x}_d + \mu \dot{x} - D \dot{\tilde{x}} - K \tilde{x} \tag{17}$$

关节空间实现为：

$$\tau = J^T \Omega (\ddot{x}_d + \mu \dot{x} - D \dot{\tilde{x}} - K \tilde{x}) + g \tag{18}$$

注意前置因子 $J^T \Omega$ 的物理含义：通过 $J^T\Omega$ 来消除真实惯量 $\Omega$，再强行注入单位惯量 $I$。

闭环动力学

将式 (17) 代入式 (5)，得：

$$I \cdot \ddot{\tilde{x}} + D \dot{\tilde{x}} + K \tilde{x} = F^{\mathrm{ext}} \tag{19}$$

无源性分析

取存储函数：

$$V = \frac{1}{2} \dot{\tilde{x}}^T \dot{\tilde{x}} + \frac{1}{2} \tilde{x}^T K \tilde{x} \tag{20}$$

求导得：

$$\dot{V} = \dot{\tilde{x}}^T F^{\mathrm{ext}} - \dot{\tilde{x}}^T D \dot{\tilde{x}} \le \dot{\tilde{x}}^T F^{\mathrm{ext}} \tag{21}$$

数学结论：在完美模型假设下，计算力矩法是严格输出无源的。

现实警示：式 (18) 中前置因子 $J^T\Omega$ 会对任何模型误差（如 $\Omega$ 的辨识偏差、摩擦、延迟）产生放大作用。误差项 $\Delta$ 将进入闭环方程，破坏无源性：

$$\dot{V} = \dot{\tilde{x}}^T F^{\mathrm{ext}} - \dot{\tilde{x}}^T D \dot{\tilde{x}} + \dot{\tilde{x}}^T \underbrace{(J^T\Omega)^{-1} \Delta}_{\text{扰动项}} \tag{22}$$

扰动项符号不定，可导致能量注入 → 失去鲁棒无源性。

---

3.3 完全自然解耦法（FND）

控制律

FND 的核心思想：只取自然惯量的对角块作为目标惯量。

定义目标惯量：

$$\Omega_d = \mathrm{diag}(\Omega_{1,1}, \Omega_{2,2}, \dots, \Omega_{r,r}) \tag{23}$$

定义目标科里奥利：

$$\mu_d = \mathrm{diag}(\mu_{1,1}, \mu_{2,2}, \dots, \mu_{r,r}) \tag{24}$$

FND 控制律（关节空间形式）：

$$\tau = J^T \Omega \Omega_d^{-1} \left( \Omega_d \ddot{x}_d + \mu_d \dot{x}_d - D \dot{\tilde{x}} - K \tilde{x} + \mu \dot{x} - \mu_d \dot{x} \right) + g \tag{25}$$

闭环动力学

将式 (25) 代入任务空间方程，经过化简（关键：方程左右两侧的 $\mu\dot{x}$ 项精确对消），得到：

$$\Omega_d \ddot{\tilde{x}} + (\mu_d + D) \dot{\tilde{x}} + K \tilde{x} = F^{\mathrm{ext}} \tag{26}$$

无源性分析

取存储函数：

$$V = \frac{1}{2} \dot{\tilde{x}}^T \Omega_d \dot{\tilde{x}} + \frac{1}{2} \tilde{x}^T K \tilde{x} \tag{27}$$

求导：

$$\dot{V} = \dot{\tilde{x}}^T F^{\mathrm{ext}} - \dot{\tilde{x}}^T D \dot{\tilde{x}} + \frac{1}{2} \dot{\tilde{x}}^T (\dot{\Omega}_d - 2\mu_d) \dot{\tilde{x}} \tag{28}$$

核心等式：因为 $\Omega_d$ 和 $\mu_d$ 都是从自然 $\Omega$ 和 $\mu$ 中“切割”出的对角块，且切割操作与求导可交换，所以有：

$$\dot{\Omega}_d = \mu_d + \mu_d^T \tag{29}$$

因此：

$$\frac{1}{2} \dot{\tilde{x}}^T (\dot{\Omega}_d - 2\mu_d) \dot{\tilde{x}} = 0 \tag{30}$$

最终：

$$\dot{V} = \dot{\tilde{x}}^T F^{\mathrm{ext}} - \dot{\tilde{x}}^T D \dot{\tilde{x}} \le \dot{\tilde{x}}^T F^{\mathrm{ext}} \tag{31}$$

结论：FND 是严格输出无源的，且不依赖于模型精度。

---

4. 多维度对比

对比维度	PD+ 控制	计算力矩法（逆动力学）	FND（本文）
闭环惯量	自然全耦合 $\Omega$	强制常数 $I$（或固定 $\Omega_d(t_0)$）	自然对角 $\mathrm{diag}(\Omega_{ii})$
闭环科氏	自然全耦合 $\mu$	抵消为 0（$C_d = 0$）	自然对角 $\mathrm{diag}(\mu_{ii})$
任务解耦	任务间全耦合	完全解耦（线性化）	完全解耦（自然）
能量守恒	$\dot{\Omega} = \mu + \mu^T$	完美模型下成立，实际可能被破坏	$\dot{\Omega}_d = \mu_d + \mu_d^T$
无源性（理想）	严格无源	数学上无源	严格无源
无源性（鲁棒）	对模型误差不敏感	模型误差被 $\Omega$ 放大	对模型误差不敏感
硬件鲁棒性	+++（最安全）	—（极易失稳）	+++（安全）
理论收敛速度	0（受耦合影响）	+++（指数收敛，理想）	++（指数收敛，实际）
奇异性行为	+（PD 调节器不受影响）	—（必须避开奇异）	—（必须避开奇异）
实现复杂度	低	中等（需精确模型）	较高（需实时计算 $\Omega$ 对角块）

---

5. 总结

三种控制方法在“如何处理任务空间惯性矩阵”上的本质差异决定了它们的性能边界：

1. PD+ 控制：不修改自然惯量，保留全耦合，因此无条件无源、硬件最安全，但任务间相互干扰严重，适用于对精度要求不高、对安全性要求极高的场景。

2. 计算力矩法（逆动力学）：强制常数惯量（如单位阵），理论上实现完美解耦和指数收敛，但依赖精确模型，任何误差都会被巨大的反馈增益放大，在真实硬件上极易失稳。它仅具备“条件无源性”，不具备“鲁棒无源性”。

3. 完全自然解耦法（FND）：仅切除自然惯量的非对角耦合块，保留对角块作为目标惯量，同时从自然科里奥利矩阵中提取相应的对角块。因“切割”与“求导”可交换，能量守恒律 $\dot{\Omega}_d = \mu_d + \mu_d^T$ 得以完整保留，实现了兼具完全解耦与鲁棒无源性的最优平衡。

FND 的核心启示是：强制修改惯量为定值并非实现解耦的唯一路径，保留自然对角惯性同样能完全解耦，且因“同源”特性天然具备低增益和高鲁棒性。