计算力矩控制：让动力学模型主动参与轨迹跟踪

上一篇文章里，我们已经为机械臂生成了一条完整轨迹：

q_d(t),\qquad \dot q_d(t),\qquad \ddot q_d(t)

它告诉机器人每一时刻希望处在什么位置、具有多大速度和加速度。但真实执行器并不认识“希望”。对于一台力矩控制机器人，驱动器最终接收的是：

\tau(t)

于是，轨迹与运动之间还隔着一个关键问题：

已知希望的关节运动，应该施加多大的关节力矩，才能让真实机器人跟上它？

为了让后面的推导不悬在公式里，我们始终想象同一个任务：一台竖直平面内的 2R 机械臂，要把末端负载从工作台平稳抬到货架。上一篇文章已经生成关节轨迹；本篇从其中任意一个采样时刻出发。此时控制器同时拥有两组信息：

轨迹给出的 $q_d,\dot q_d,\ddot q_d$ ，描述“此刻希望怎样运动”；
编码器与状态估计给出的 $q,\dot q$ ，描述“机器人实际上走到了哪里”。

控制器要把两者合并为一份力矩 $\tau$ 。下一个控制周期到来时，机器人已经在这份力矩作用下产生了新的位置和速度，控制器再重新计算。因而我们要解决的不是一次性的方程求解，而是一个不断闭合的因果环：

\text{轨迹采样} \longrightarrow \text{力矩计算} \longrightarrow \text{真实运动} \longrightarrow \text{状态反馈}

如果机械臂永远水平放置、运动很慢、负载从不变化，那么每个关节各自使用一个 PD 控制器，往往已经能够工作。可一旦机器人进入竖直平面、提高速度或改变构型，同一组位置误差对应的正确力矩便不再相同：

重力会随姿态改变；
质量矩阵会随构型改变；
一个关节加速会牵动其他关节；
高速运动还会产生科里奥利与离心力矩。

这正是动力学模型进入控制回路的理由。模型不再只用于离线仿真，而是在每个控制周期回答：

当前构型下，要实现这份加速度命令，机器人此刻真正需要多少力矩？

从期望轨迹到真实关节力矩的闭环

本文会从最简单的 PD 控制开始，逐步加入重力补偿、逆动力学前馈和计算力矩控制。每一步都不是为了让公式更复杂，而是因为前一步留下了一个明确的问题。

1. 轨迹给出运动，执行器却只认力矩

对一台全驱动刚性机械臂，关节动力学可以写成：

M(q)\ddot q+c(q,\dot q)+g(q)=\tau

其中：

$M(q)$ 是随构型变化的质量矩阵；
$c(q,\dot q)$ 汇总科里奥利与离心项；
$g(q)$ 是重力项；
$\tau$ 是执行器施加的关节力矩。

$M(q)$ 不只是若干惯性参数的集合。它是一个对称正定矩阵，把关节加速度映射为惯性力矩，并通过非对角元素描述关节之间的耦合。 $c$ 与 $g$ 已经具有力矩量纲，因此三项相加后才能与执行器输出的 $\tau$ 平衡。

从仿真角度看，已知 $\tau$ 后可以计算加速度：

\ddot q =M(q)^{-1} \left[ \tau-c(q,\dot q)-g(q) \right]

这叫正动力学。控制面对的方向恰好相反：轨迹生成器已经给出希望的 $\ddot q_d$ ，我们要反过来计算实现它所需的 $\tau$ 。这就是逆动力学。

如果机器人此刻恰好位于期望状态，逆动力学可以告诉我们“维持这条理想轨迹需要多少力矩”。但只要实际状态已经偏离，原先针对期望状态计算的力矩就不再对应真实机器人当前承受的惯性、重力和速度耦合。这正是开环计算与闭环控制之间的分界。

但只做一次逆运算还不够。真实机器人永远存在初始误差、参数误差、摩擦、扰动和测量噪声。一个只会根据期望轨迹算力矩、却不观察实际运动的控制器，一旦偏离便没有办法回来。

因此，轨迹跟踪同时需要两种信息：

前馈：根据期望运动，提前给出理论上应该需要的力矩；
反馈：根据实际误差，持续修正模型没有预测到的部分。

计算力矩控制的核心，就是让动力学模型与反馈不再各做各的事，而是在同一个加速度命令中结合起来。

2. 为什么纯 PD 必须先看见误差

先从一根在竖直平面内转动的连杆开始。令 $q$ 从水平轴起算，它的动力学可写成：

J\ddot q+b\dot q+mgr\cos q=\tau

$J$ 是关节等效转动惯量， $b$ 是黏性阻尼， $r$ 是质心到关节的距离。

定义跟踪误差：

e=q_d-q

最常见的 PD 控制律是：

\tau=K_p e+K_d\dot e

它的直觉非常清楚：

偏离目标越远，虚拟弹簧产生的恢复力矩越大；
运动越快，虚拟阻尼产生的制动力矩越大。

但这里的 $K_p$ 、 $K_d$ 直接把误差换成力矩。它们的数值不仅决定“纠错有多强”，还与连杆惯量、减速器和负载共同决定实际加速度。换一台电机、换一个末端负载，甚至只是改变机械臂构型，同一组增益带来的运动响应都可能不同。

若连杆在水平面运动，没有重力，而且目标是固定位置，合适的 $K_p>0$ 、 $K_d>0$ 可以让误差收敛。但在竖直平面内，目标处需要一份持续的力矩抵抗重力。

当系统最终静止时， $\dot e=\ddot q=0$ ，于是：

K_p(q_d-q_\infty) =mgr\cos q_\infty

也就是说，比例项必须保留一部分位置误差，才能“挤出”抵抗重力的力矩。增大 $K_p$ 可以减小误差，却会同时提高刚度、放大噪声，并更容易碰到力矩饱和和未建模柔性。

问题并不在于 PD 不够聪明，而在于它只在误差出现后才行动。既然重力是可计算的已知物理量，为什么要等机器人掉下来一点，才让位置误差替我们承担它？

3. 第一层改进：先把已知的重力托住

最直接的改进，是把控制器内部的重力模型主动加入控制：

\tau =\hat g(q) +K_p e +K_d\dot e

这里的帽号不是说重力项只能由观测器“估计”出来，而是用来区分控制器采用的模型量 $\hat g$ 与真实系统的动力学量 $g$ 。 $\hat g$ 可以由 CAD 参数、负载标定或系统辨识得到；模型准确时二者相等，质量、质心或负载存在偏差时二者才会不同。

重力补偿通常在实际位置 $q$ 上计算，而不是在期望位置 $q_d$ 上计算。原因很直接：当前真正作用在机械臂上的重力由它此刻的姿态决定。跟踪误差很小时两者差别不大；误差较大时，使用 $\hat g(q)$ 才是在补偿机器人眼下承受的重量。

若 $\hat g(q)=g(q)$ ，对固定目标而言，重力项会在闭环方程中抵消：

M(q)\ddot q+c(q,\dot q) =K_p e+K_d\dot e

这样，机器人不必依靠稳态位置误差来提供托举力矩。即使反馈增益降低，静止保持也可以很准确，机械臂对外力还会显得更柔和。

不过闭环中仍然保留了 $M(q)$ 与 $c(q,\dot q)$ 。重力造成的静态偏置被移除了，但“同一份反馈力矩究竟会产生多大加速度”仍取决于当前构型和速度。于是问题从“能否托住”自然转向了“能否按期望节奏运动”。

但重力补偿只解决了“静止时为什么会掉下来”。当机器人快速运动时，另外两类问题仍然存在：

同样的加速度，在不同构型下需要不同的惯性力矩 $M(q)\ddot q$ ；
关节速度会产生耦合力矩 $c(q,\dot q)$ 。

所以重力补偿是模型控制的第一步，却还不是完整的轨迹跟踪。

从纯反馈到计算力矩控制的逐层演进

4. 第二层改进：提前计算轨迹所需的力矩

轨迹生成器已经给出了 $q_d,\dot q_d,\ddot q_d$ 。把这些期望量代入控制器采用的名义动力学模型，可以得到一份前馈力矩：

\tau_{ff} =\hat M(q_d)\ddot q_d +\hat c(q_d,\dot q_d) +\hat g(q_d)

如果初始状态完全准确、模型完全准确、外界没有扰动，那么单独使用 $\tau_{ff}$ 就能沿期望轨迹运动。

这份力矩可以理解为轨迹的“物理预算”： $\hat M\ddot q_d$ 负责制造期望加速度， $\hat c$ 负责抵消运动中的速度耦合， $\hat g$ 负责托住重量。它不等机器人产生误差才行动，因此能显著减轻反馈控制器的负担。

现实中这些条件几乎不会同时成立。纯前馈控制没有观察 $q$ 和 $\dot q$ ，因此哪怕只出现很小的偏差，也没有自动修正机制。更常见的做法是加入力矩域的 PD 反馈：

\tau =\tau_{ff} +K_p e +K_d\dot e

这比纯 PD 更合理：大部分可预测的惯性、速度耦合和重力由模型提前承担，反馈主要处理剩余误差。

但把它代回真实动力学，可以看到反馈力矩仍要先经过质量矩阵的逆：

\ddot q =M(q)^{-1} \left[ \tau_{ff} -c(q,\dot q) -g(q) +K_p e +K_d\dot e \right]

这里同时存在两种不一致：前馈项在期望状态 $(q_d,\dot q_d)$ 上计算，真实动力学却发生在实际状态 $(q,\dot q)$ ；反馈项虽然形式固定，变成关节加速度时却被 $M(q)^{-1}$ 按当前构型重新缩放。只要误差不为零，闭环就不能化成一组构型无关的标准二阶方程。

计算力矩控制继续向前走一步：先在加速度层设计希望的误差修正，再让当前质量矩阵把它换算成力矩。

5. 第三层改进：在加速度层设计误差响应

仍然定义：

e=q_d-q,\qquad \dot e=\dot q_d-\dot q

先构造一份虚拟加速度命令：

v =\ddot q_d +K_d\dot e +K_p e

它由两部分组成：

$\ddot q_d$ ：轨迹本来要求的前馈加速度；
$K_d\dot e+K_p e$ ：为了把机器人拉回轨迹而增加的反馈加速度。

这里完成了一个关键的换层：反馈不再直接回答“还要补多少力矩”，而是先回答“为了追上轨迹，还希望增加多少加速度”。因此 $K_p$ 的量纲是 $\mathrm{s}^{-2}$ ， $K_d$ 的量纲是 $\mathrm{s}^{-1}$ ；它们首先描述误差希望以多快的节奏收敛，而不是某个具体构型下要输出多少牛米。

接着用当前状态下的动力学模型，把 $v$ 变成关节力矩：

\tau =\hat M(q)v +\hat c(q,\dot q) +\hat g(q)

这就是关节空间中的计算力矩控制（computed-torque control），也常被称为逆动力学控制或前馈加反馈线性化控制。

“逆动力学”容易让人误以为控制器必须显式计算 $M^{-1}$ 。其实这里做的是相反的映射：先设计加速度 $v$ ，再用 $\hat M(q)$ 将它转换成力矩。所谓“逆”，指的是从期望运动反求驱动力矩的因果方向，而不是对质量矩阵做数值求逆。

注意它与上一节的差别：

普通前馈在期望状态 $(q_d,\dot q_d)$ 上计算理论轨迹力矩，再额外叠加反馈力矩；
计算力矩在实际状态 $(q,\dot q)$ 上计算动力学，并用 $M(q)$ 把“前馈加速度 + 反馈加速度”整体转换成力矩。

当 $e=\dot e=0$ 时， $v=\ddot q_d$ ，计算力矩控制退化为理想逆动力学前馈；一旦出现偏差，反馈加速度与轨迹加速度会一起经过当前质量矩阵转换。这样，前馈和反馈不再是简单相加的两份力矩，而是在同一个动力学映射里汇合。

假设模型完全准确：

\hat M=M,\qquad \hat c=c,\qquad \hat g=g

将控制律代入真实动力学：

M(q)\ddot q+c+g =M(q)v+c+g

因为 $M(q)$ 可逆，所以：

\ddot q=v

再代入 $v$ 的定义：

\ddot e+K_d\dot e+K_p e=0

原本随构型变化、关节相互耦合的非线性动力学，在理想模型下变成了一组可以直接设计的线性误差方程。这就是“反馈线性化”的含义：不是机器人真的变成了线性系统，而是控制器利用模型抵消了已知非线性。

这一步依赖三个前提：机器人是全驱动的，每个广义坐标都有可用执行器； $M(q)$ 在工作空间内保持正定；控制器能够足够快地读取状态并输出力矩。若机构欠驱动、存在强柔性，或上层只能发送位置命令，上面的等式就不能原样落到真实硬件上。后文会回到这些边界。

计算力矩如何把非线性机器人变成可设计的误差动力学

6. 为什么一定要乘上质量矩阵

质量矩阵不是一个为了凑齐动力学方程而存在的系数。它描述的是：当前构型下，某个关节加速度方向有多“重”。

继续使用前文的竖直平面 2R 机械臂。为了只观察惯性影响，先设关节速度为零，并假设重力已被准确补偿：

\dot q=0,\qquad \tau-g(q)=M(q)\ddot q

若只让肩关节产生加速度：

\ddot q= \begin{bmatrix} \alpha\\ 0 \end{bmatrix}

肩关节所需的惯性力矩为：

\tau_1-g_1(q)=M_{11}(q_2)\alpha

而前文已经得到：

\begin{aligned} M_{11}(q_2) &=I_1+I_2+m_1c_1^2\\ &\quad +m_2\left( l_1^2+c_2^2+2l_1c_2\cos q_2 \right) \end{aligned}

机械臂伸直时 $q_2=0$ ，远端质量离肩关节更远；完全折回时 $q_2=\pi$ ，远端质量更靠近肩部。两种构型的差为：

M_{11}(0)-M_{11}(\pi) =4m_2l_1c_2

所以，同一个肩关节加速度 $\alpha$ ，伸直构型需要更大的肩部惯性力矩。

取一个便于感受数量级的例子：若 $m_2=2\,\mathrm{kg}$ 、 $l_1=0.6\,\mathrm{m}$ 、 $c_2=0.25\,\mathrm{m}$ ，仅由构型变化带来的肩部等效惯量差就是：

4m_2l_1c_2 =1.2\,\mathrm{kg\,m^2}

当肩关节加速度为 $2\,\mathrm{rad/s^2}$ 时，伸直构型仅这一项就要比完全折回多提供 $2.4\,\mathrm{N\,m}$ 的惯性力矩。这不是控制器增益变化造成的，而是同一台机器人在不同姿态下确实呈现了不同的动力学“重量”。

同一关节加速度在不同构型下需要不同力矩

耦合还体现在非对角项。若只给肘关节加速度：

\ddot q= \begin{bmatrix} 0\\ \alpha \end{bmatrix}

即使肩关节期望加速度为零，它仍然需要输出：

\tau_1-g_1(q)=M_{12}(q_2)\alpha

来抵消肘部加速对整条机械臂的牵动。逐关节独立 PD 并不知道这些关系；计算力矩则通过整张 $M(q)$ 同时分配各关节力矩。

现在可以回看第 4 节留下的问题：若反馈直接输出相同力矩，伸直与折回时产生的加速度必然不同；若反馈先指定相同加速度，再乘以各自的 $M(q)$ ，控制器才会主动为这种“轻重变化”补足不同力矩。

7. 增益终于有了清楚的动态意义

在理想计算力矩控制下：

\ddot e+K_d\dot e+K_p e=0

若 $K_p$ 与 $K_d$ 取对角形式，每个关节都对应一个标准二阶系统：

\ddot e_i +2\zeta_i\omega_{n,i}\dot e_i +\omega_{n,i}^2e_i =0

因此可以选择：

\begin{aligned} K_{p,i}&=\omega_{n,i}^2\\ K_{d,i}&=2\zeta_i\omega_{n,i} \end{aligned}

$\omega_n$ 主要决定误差收敛速度， $\zeta$ 决定阻尼与超调。与直接输出力矩的 PD 不同，这里的增益先生成虚拟加速度，再由 $M(q)$ 换算成力矩，所以理想闭环响应不再随构型中的惯性变化而改变。

例如希望各关节接近临界阻尼，可以先取 $\zeta\approx1$ ；若希望误差动态的自然频率为 $8\,\mathrm{rad/s}$ ，理想连续系统中的起点就是：

K_p=64,\qquad K_d=16

这里的数值首先规定的是误差方程，而不是直接规定力矩。控制器随后会根据每一时刻的 $M(q)$ 自动计算实现这份误差加速度所需的不同力矩。

这句话必须带上“理想”二字。真实系统中，增益仍然受到采样周期、延迟、噪声、柔性模态、摩擦和力矩饱和的限制。反馈线性化给了我们清楚的设计起点，却没有取消硬件边界。

8. 稳定性为什么可以用一份“能量”看清

假设 $K_p$ 、 $K_d$ 都是对称正定矩阵。对理想误差动力学，选择：

V(e,\dot e) =\frac12\dot e^{\scriptscriptstyle\mathsf T}\dot e +\frac12e^{\scriptscriptstyle\mathsf T}K_p e

它看起来像“误差速度的动能 + 位置误差的势能”，并且除 $e=\dot e=0$ 外始终为正。

沿闭环轨迹求导：

\begin{aligned} \dot V &=\dot e^{\scriptscriptstyle\mathsf T} \left( \ddot e+K_p e \right)\\ &=-\dot e^{\scriptscriptstyle\mathsf T}K_d\dot e \le0 \end{aligned}

虚拟阻尼 $K_d$ 持续耗散误差能量，因此误差不会自行增大，并在标准条件下收敛到零。

这里 $\dot V$ 只是半负定：当 $\dot e=0$ 时，它可能暂时等于零。继续检查这个集合中的运动，如果 $\dot e=0$ 却有 $e\neq0$ ，误差方程会给出 $\ddot e=-K_p e\neq0$ ，系统不可能永远停留其中。利用 LaSalle 不变性原理，最大的不变集合只剩 $e=\dot e=0$ ，因此理想闭环才得到渐近收敛，而不只是“能量不增加”。

不过，这个 $V$ 是为已经线性化的误差系统构造的 Lyapunov 函数，并不等同于机器人的真实机械能。计算力矩控制的思路是先尽量抵消原有动力学，再塑造一套简单误差动力学。

至此，计算力矩控制的主线已经完整：用模型把非线性机器人转成简单误差系统，再在这个误差系统上设计收敛速度。此时再回看第 3 节的“重力补偿 + PD”，会发现它并不只是计算力矩控制的残缺版本。若目标是固定位置而不是任意时变轨迹，它还可以从真实机械能的角度得到另一种完整解释。

9. 从误差能量回到真实机械能：能量整形

先考虑固定目标 $q_\star$ ，定义：

\tilde q=q-q_\star

使用重力补偿加 PD：

\tau =g(q) -K_p\tilde q -K_d\dot q

这正是第 3 节控制律在固定目标下的另一种写法。之前我们说它“尚未抵消 $M$ 与 $c$ ”；现在换一个视角，这些没有被抵消的机械结构反而成为稳定性证明的一部分。

为了显式使用机器人动力学的结构性质，把速度耦合向量写成 $c(q,\dot q)=C(q,\dot q)\dot q$ ，闭环动力学为：

M(q)\ddot q +C(q,\dot q)\dot q +K_p\tilde q +K_d\dot q =0

此时可以选择：

V(q,\dot q) =\frac12\dot q^{\scriptscriptstyle\mathsf T}M(q)\dot q +\frac12\tilde q^{\scriptscriptstyle\mathsf T}K_p\tilde q

第一项是真实动能，第二项是控制器创造的人工势能。重力势能被补偿，目标 $q_\star$ 被改造成新势能的最低点。

利用机器人动力学中 $\dot M-2C$ 的斜对称结构，可以得到：

\dot V =-\dot q^{\scriptscriptstyle\mathsf T}K_d\dot q \le0

也就是说，虚拟弹簧把目标变成能量最低点，虚拟阻尼不断带走动能。机器人不是因为非线性被完全消掉而稳定，而是因为它被塑造成了一个会自然滚向目标、并逐渐停下的机械系统。

重力补偿、人工势能与阻尼共同塑造新的能量地形

这就是最简单的全驱动能量整形。它与计算力矩控制的哲学不同：

方法	主要动作	理想闭环
计算力矩	用模型抵消已知非线性	线性的轨迹误差动力学
能量整形	保留惯性与科里奥利结构，改造势能和阻尼	具有目标能量最低点的机械系统

能量整形特别适合解释定点调节、无源性与柔顺行为；计算力矩更直接地服务于任意时变轨迹跟踪。工程控制器也可以结合两者，而不是把它们视为互斥阵营。

前两节给出了两种理想视角：一种重塑误差动力学，另一种重塑系统能量。它们都依赖一个尚未认真面对的条件——控制器中的模型与真实机器人足够一致。下面把这个理想条件放开。

10. 回到现实：模型不完美时，抵消会剩下什么

控制器中的 $\hat M,\hat c,\hat g$ 通常来自 CAD 参数、系统辨识和负载标定，它们未必与真实系统的 $M,c,g$ 完全一致。把速度相关项、重力和摩擦统一记为：

h(q,\dot q) =c(q,\dot q)+g(q)+\tau_f(q,\dot q)

若把关节摩擦包含在 $h$ 中，并把碰撞、接触或未建模外力折算为关节扰动力矩 $\tau_{ext}$ ，真实机器人满足：

M(q)\ddot q+h(q,\dot q) =\tau+\tau_{ext}

而控制器输出：

\tau=\hat M(q)v+\hat h(q,\dot q)

代入后：

\ddot q =v+d_m+d_{ext}

其中模型残差为：

d_m =M(q)^{-1} \left[ \left(\hat M-M\right)v +\left(\hat h-h\right) \right]

而外部扰动对应：

d_{ext}=M(q)^{-1}\tau_{ext}

因此真实误差动力学变成：

\ddot e+K_d\dot e+K_p e =-d_m-d_{ext}

这条式子把“模型不准”和“外界扰动”从一句提醒变成了明确结果：参数误差、未知负载和摩擦进入 $d_m$ ，接触与碰撞进入 $d_{ext}$ 。反馈能够压低它们对跟踪误差的影响，却不能让它们凭空消失；若反馈带宽过高，反而会把传感器噪声和未建模柔性一同放大。

这也解释了为什么模型并不是越复杂越好：

错误的惯性参数可能把反馈加速度换算成错误力矩；
速度噪声会污染 $c(q,\dot q)$ 和微分反馈；
摩擦模型在低速换向处往往最不可靠；
执行器饱和后，理论上的抵消关系立即失效；
柔性、回差和内层电流环不在刚体模型里，却可能决定稳定边界。

一个不够精确但结构正确、计算稳定的模型，配合适度反馈，往往比一份复杂却未经辨识和验证的模型更可靠。

这也给出了清楚的工程分工：模型负责降低控制器本来就能预见的负担，反馈负责在有限带宽内修正残差，限幅与安全逻辑负责处理二者都无法保证的边界情况。

11. 计算力矩并不适用于所有机器人接口

计算力矩控制隐含一个重要前提：上层控制器能够以足够高的带宽命令并实现关节力矩。

但许多工业机械臂只开放位置或速度接口。此时底层驱动器已经包含厂商自己的电流环、速度环、位置环和补偿逻辑；上层即使计算出 $\tau$ ，也没有直接施加它的位置。

因此，“有动力学模型”与“能够实现计算力矩控制”是两回事。模型在不同接口下能发挥的作用并不相同：

对外命令接口	上层实际发送	动力学模型通常怎样使用
位置	$q_d$	多用于轨迹可行性、碰撞预测或厂商内部补偿，外部难以直接闭合力矩环
速度	$\dot q_d$	可以改善速度前馈与限幅，但仍受底层速度环动态支配
力矩 / effort	$\tau$	才能直接实现本文形式的关节空间计算力矩控制

即使开放的是电机电流命令，也不能立即把它等同于关节力矩。电机转矩常数、减速比、传动效率、摩擦以及弹性都会参与从电流到关节输出的映射。

即使硬件提供 effort 接口，也要继续确认：

命令接口的物理含义是关节侧力矩、电机侧力矩、归一化 effort，还是电机电流；
减速器摩擦、效率和回差是否已由底层补偿；
力矩命令是否还会经过额外滤波、限幅或安全控制；
控制周期和状态时间戳是否足以支撑模型计算；
关节速度来自直接测量、观测器还是编码器差分。

例如 ROS 2 的 Joint Trajectory Controller 可以在 effort 接口下把轨迹中的 effort 作为前馈量加入 PID 输出，但这并不自动等于计算力矩控制。 $\hat M,\hat c,\hat g$ 在哪里计算、误差反馈处于力矩层还是加速度层，仍然需要由具体控制器实现决定。

所以在写控制律之前，第一张应该确认的“模型”不是 $M(q)$ ，而是整条控制链：谁读取编码器、谁估计速度、谁执行电流环、谁负责限幅，以及上层命令最终在哪一层被解释。

12. 一次控制周期里真正发生什么

一套最小的计算力矩控制循环可以写成：

读取 q, dq 与可靠时间戳
从轨迹采样 qd, dqd, ddqd

e  = continuous_difference(qd, q)
de = dqd - dq
v  = ddqd + Kd * de + Kp * e

tau = M_hat(q) * v
    + c_hat(q, dq)
    + g_hat(q)
    + friction_hat(q, dq)

tau = apply_limits(tau, torque_limit, torque_rate_limit)
发送 tau，并记录实际状态、命令和饱和标志

这段伪代码的顺序并非随意。轨迹、状态和模型必须对应同一个时间基准；误差先形成虚拟加速度，模型再把它映射成力矩；最后的力矩限幅必须作用在所有前馈与反馈项相加之后。任何一个环节使用旧数据，都等价于在闭环中额外加入延迟。

真正部署时，还需要逐项确认：

连续旋转关节的误差采用正确的角度展开；
$q,\dot q$ 与模型处在一致的坐标、单位和符号约定中；
轨迹的 $\ddot q_d$ 不是由噪声位置数据临时二次差分得到；
力矩和力矩变化率在模型计算后再次检查；
出现过期状态、NaN、模型异常或通信超时时安全退出；
从低速、轻载开始，对比预测力矩与实际电流或力矩估计；
分开记录前馈、重力、科里奥利和反馈分量，便于定位误差来源。

调参时也不应一开始就启用完整抵消并给出很高带宽。更稳妥的顺序是：

先验证符号与重力补偿，再验证低速逆动力学前馈，最后逐步加入加速度层反馈并提高带宽。

如果机械臂在静止重力补偿阶段就向错误方向加速，问题通常不是 $K_p$ 太小，而是模型、坐标或力矩符号还没有对齐。

当重力补偿正确、低速前馈方向正确、力矩限幅可靠之后，再提高 $\omega_n$ 才有意义。每次提高带宽，都应同时观察跟踪误差、各力矩分量、饱和占空比和高频振动，而不是只看末端是否“更快到达”。

13. 把这篇文章放回整个系列

现在可以把前面的文章连成一条完整链：

PoE 与李群描述机器人位姿怎样随关节运动；
雅可比把关节速度连接到末端速度与力；
逆运动学从任务目标得到关节目标；
轨迹生成给出连续的 $q_d,\dot q_d,\ddot q_d$ ；
拉格朗日法建立 $M,c,g$ ；
计算力矩控制把期望运动与当前误差变成真实关节力矩。

这条链的最后一步并不是简单地“把动力学方程倒过来”。纯模型无法从误差中恢复，纯反馈又只能在误差出现后行动。计算力矩控制真正做的是：

让模型承担可预测的物理，让反馈处理不可预测的现实。

它也为下一篇阻抗控制留下了自然入口。轨迹跟踪关心的是“怎样准确到达”；当机器人开始接触环境，我们还要进一步追问：

如果目标位置与真实世界发生冲突，机器人应该更用力，还是应该允许自己退让？

参考与延伸阅读

Kevin M. Lynch, Frank C. Park, Modern Robotics: Mechanics, Planning, and Control, Chapter 11.4: Motion Control with Torque or Force Inputs. 官方课程资源
Russ Tedrake, Robotic Manipulation, Chapter 8: Manipulator Control. 在线课程讲义
Mark W. Spong, Seth Hutchinson, M. Vidyasagar, Robot Modeling and Control, Wiley, 2006.
Bruno Siciliano, Lorenzo Sciavicco, Luigi Villani, Giuseppe Oriolo, Robotics: Modelling, Planning and Control, Springer, 2009.
ROS 2 Control, Joint Trajectory Controller documentation
Russ Tedrake, Underactuated Robotics, Chapter 3: Acrobots, Cart-Poles, and Quadrotors. 能量整形课程讲义