计算力矩法与 PD+ 控制器的区别

1. 控制方程对比

我们使用统一的机器人动力学符号：

$M(q)\ddot{q} + C(q,\dot{q})\dot{q} + G(q) = \tau$

其中 $q$ 是关节位置，$\tilde{q} = q - q_d$ 是跟踪误差，$q_d$ 为期望轨迹。

$\tau = M(q)\big( \ddot{q}_d - K_v \dot{\tilde{q}} - K_p \tilde{q} \big) + C(q,\dot{q})\dot{q} + G(q)$

$\tau = M(q)\ddot{q}_d + C(q,\dot{q})\dot{q}_d + G(q) - K_v \dot{\tilde{q}} - K_p \tilde{q}$

控制组成部分	计算力矩法	PD+ 控制器
前馈项（基于期望轨迹）	$M(q)\ddot{q}_d$	$M(q)\ddot{q}_d + C(q,\dot{q})\dot{q}_d$
反馈项（基于误差）	$- M(q)K_v \dot{\tilde{q}} - M(q)K_p \tilde{q}$	$- K_v \dot{\tilde{q}} - K_p \tilde{q}$
非线性补偿项	完全补偿实际科里奥利力： $+ C(q,\dot{q})\dot{q}$	仅前馈期望科里奥利力： $+ C(q,\dot{q})\dot{q}_d$（无动态补偿）

将两种控制律分别代入动力学方程，我们可以从频域角度直观地看到闭环系统带宽的根本区别：

对比项	计算力矩法	PD+ 控制器
闭环误差动态方程	$\ddot{\tilde{q}} + K_v \dot{\tilde{q}} + K_p \tilde{q} = 0$	$M(q)\ddot{\tilde{q}} + \big[ C(q,\dot{q}) + K_v \big] \dot{\tilde{q}} + K_p \tilde{q} = 0$
是否为线性定常系统？	是。可整理为标准二阶系统，特征方程独立于 $q$。	否。系统矩阵左侧明确包含时变/状态依赖项 $M(q)$。
闭环带宽特性	恒定。闭环系统的固有频率和阻尼比完全由 $K_p, K_v$ 决定，与机器人当前姿态 $q$（惯量大小）无关。	随构型变化。当手臂伸直（惯量大）时，系统等效带宽自动降低；蜷缩（惯量小）时，带宽自动升高。

客观解读：计算力矩法通过精确抵消非线性，在数学上创造了一个全频域特性恒定的线性误差空间；而 PD+ 将物理世界的惯量变化“反射”到了误差动态中，使得系统的频宽具备天然的载荷自适应特性。

核心思想：通过精确抵消系统的非线性，将机器人“伪装”成一个理想的线性双积分器（单位质量块）。
做法：利用完整动力学模型（包含实际速度 $\dot{q}$），完全消除离心力、科里奥利力和重力。剩下的只是一个纯线性定常系统，直接套用经典的线性 PD 控制。
目标：使误差动力学彻底线性定常化，无论机器人处于何种负重或姿态，频域响应保持一致。

核心思想：尊重系统的非线性物理本性，只提供“辅助性”的前馈力，而不强行改变系统的固有动态。
做法：仅利用期望轨迹（$\dot{q}_d, \ddot{q}_d$）计算前馈力，对真实存在于开环系统中的离心力 $C(q,\dot{q})\dot{q}$ 不进行抵消。该力作为系统固有的自然特性留在方程中。
目标：保持系统的物理属性（如大惯量下带宽自然降低），让反馈环节纯粹以恒定增益处理剩余扰动，简化反馈环的频带设计。

由于质量矩阵 $M(q)$ 在反馈环中的位置不同，两者在工程实现上表现出截然不同的结构性特征：

计算力矩法：有效反馈增益为 $M(q)K_p$。这意味着即使工程师设定常值矩阵 $K_p$，实际的控制力矩也会随着惯量的增大而自动线性放大（因为 $M(q)$ 变大了）。这本质上是将惯量信息引入反馈增益。
PD+ 控制器：有效反馈增益仅为 $K_p$。它是一个纯粹的常值增益，物理意义更接近一个恒定刚度的“虚拟弹簧”，不受物理惯量变化的影响。

抛开性能好坏的主观评价，两者在数学结构上的核心区别可以用一句话概括：

计算力矩法将惯性矩阵 $M(q)$ 左乘到反馈误差上（闭环内），旨在抹去非线性以实现恒定的带宽特性；而 PD+ 将 $M(q)$ 仅保留在前馈通道中（闭环外），旨在顺应物理规律以获得简洁、鲁棒的反馈结构。