机器人操作数学重学 03：从虚功到动力学与控制

用一个具体问题串起这一章：两根长度为 1 米的杆折成直角，末端受到向右 1 牛的外力，两个关节分别感受到多大的转矩？先用数值和图验证雅可比转置，再加入惯性、重力与反馈控制，逐步看清每个矩阵对应的物理量。

系列学习地图 · 上一篇：02 运动学与雅可比 · 下一篇：04 抓取与非完整运动

1. 先算外力怎样传到两个关节

仍用前一篇的平面 2R 机械臂：两段杆长都为 1 m，关节角为 q = (0°, 90°)。第一根杆向右，第二根向上，末端位于 (1, 1) m。

末端位置雅可比把关节角速度变成末端点的平移速度；此姿态下：

$$\dot p=J_p(q)\dot q,\qquad J_p=\begin{bmatrix}-1&-1\\1&0\end{bmatrix}$$

现在让环境对末端施加向右的力 f_ext = (1, 0) N。这股力相对两个关节都有顺时针转动趋势；按照逆时针为正的约定，两个外力转矩都应为负。

$$\tau_{\mathrm{ext}}=J_p^T f_{\mathrm{ext}} =\begin{bmatrix}-1&1\\-1&0\end{bmatrix} \begin{bmatrix}1\\0\end{bmatrix} =\begin{bmatrix}-1\\-1\end{bmatrix}\ \mathrm{N\,m}$$

这里得到的是外力作用到关节上的广义转矩。若忽略重力、摩擦，并要求静止不动，驱动器的保持转矩要反向抵消它：τ_act = (1, 1) N·m。把这两个概念混在一起，就会在控制代码中写反符号。

交互图用于观察关节角改变后可产生的末端速度方向与速度椭圆，也可改变单位外力的方向，查看它对应的广义转矩；上面的转矩数值专门对应 q = (0°, 90°)，换姿态后要重新计算 J_p(q)。

2. 为什么是转置：从功率对上账

假设此刻关节速度是 q̇ = (0.2, −0.1) rad/s，末端速度为：

$$\dot p=\begin{bmatrix}-1&-1\\1&0\end{bmatrix} \begin{bmatrix}0.2\\-0.1\end{bmatrix} =\begin{bmatrix}-0.1\\0.2\end{bmatrix}\ \mathrm{m/s}$$

外力在末端输入的功率为 1 × (−0.1) + 0 × 0.2 = −0.1 W；传到关节端后，功率为 −1 × 0.2 + (−1) × (−0.1) = −0.1 W。两个端口的结果相同，负值表示该外力此刻从机器人吸收机械能。

把这个数值例子改写成符号：

$$f_{\mathrm{ext}}^T\dot p =f_{\mathrm{ext}}^TJ_p\dot q =(J_p^Tf_{\mathrm{ext}})^T\dot q =\tau_{\mathrm{ext}}^T\dot q$$

换成虚位移 δp = J_p δq，就是虚功关系。雅可比转置不是需要单独死记的力公式，而是与速度映射配对、让功率保持一致的映射。

六维情况沿用本系列顺序：速度旋量 V = (v, ω)，力旋量 F = (f, τ)，功率为 FᵀV。两者必须使用匹配的参考坐标与参考点。前面的 J_p 只包含末端点的二维平移速度，不是完整的六维空间雅可比；不要把它和空间旋量的线速度部分直接混用。

3. 动力学方程：转矩都花到哪里去了

当机器人开始加速，驱动转矩不再只是平衡末端外力。用关节坐标写出：

$$M(q)\ddot q+C(q,\dot q)\dot q+\mathbf g(q) =\tau_{\mathrm{act}}+J_p(q)^Tf_{\mathrm{ext}}$$

这里的 g(q) 是重力项，和表示坐标变换的 g_ab 不是同一个对象。实际模型还可能包含摩擦、柔性、负载变化与驱动器动态。

在惯性参数物理有效、广义坐标独立的常规刚体模型中，质量矩阵 M 对称正定，因为动能为：

$$T=\frac{1}{2}\dot q^TM(q)\dot q$$

“正定”意味着非零速度具有正动能。C 的矩阵表示并不唯一，重要的是它给出的速度二次项；按常用 Christoffel 形式构造时，Ṁ − 2C 为斜对称矩阵。于是 q̇ᵀ(Ṁ − 2C)q̇ = 0，速度耦合不会无中生有地产生能量。这个性质可用于检查推导，但不能把它随意套到任意写法的 C 上。

4. 一根摆杆：检查重力符号和单位

考虑质量集中在距关节 r = 0.5 m 处的单摆，质量 m = 1 kg，关于关节的转动惯量 I = 0.25 kg·m²。角度 q 从竖直向下计，重力加速度取 9.81 m/s²。

选择势能 U(q) = mgr(1 − cos q)，因此重力项为 dU/dq = mgr sin q：

$$I\ddot q+mgr\sin q=\tau_{\mathrm{act}}$$

当 q = 30° 且希望静止保持，q̈ = 0：

$$\tau_{\mathrm{act}} =1\times9.81\times0.5\times\sin30^\circ =2.4525\ \mathrm{N\,m}$$

这个正转矩抵消重力的负向转动趋势。若电机不出力，此刻加速度为 −2.4525 / 0.25 = −9.81 rad/s²，杆会朝竖直向下回落。若再要求正向加速度 2 rad/s²，驱动转矩变为 0.25 × 2 + 2.4525 = 2.9525 N·m。

这比只检查公式长相更有效：单位正确吗？在最低点 q = 0 时重力项是否为零？松手后加速度是否朝向最低点？

5. 计算力矩控制：先写误差，再看抵消

设期望轨迹为 q_d(t)，定义误差 e = q_d − q。先假设模型精确、驱动器能完全实现转矩、没有外力，也暂不考虑摩擦。选择：

$$\tau_{\mathrm{act}} =M(q)\bigl(\ddot q_d+K_d\dot e+K_pe\bigr) +C(q,\dot q)\dot q+\mathbf g(q)$$

代回动力学方程，两侧的速度项和重力项抵消；再用 M 可逆，可得：

$$\ddot q=\ddot q_d+K_d\dot e+K_pe \quad\Longrightarrow\quad \ddot e+K_d\dot e+K_pe=0$$

这才是控制律的目的：让机器人复杂的运动误差满足我们能分析的二阶系统。若每个关节采用 K_d = 10、K_p = 25 的标量增益，对应特征多项式为 (s + 5)²；理想误差会衰减。这里的增益单位分别为 s⁻¹ 与 s⁻²，不是两个任意的无量纲数字。

把这条控制律代回刚才的单摆：当前位置为 30°，目标再高 0.1 rad，当前和目标速度均为零，目标加速度也为零。取 K_p = 25，控制器要求的加速度是 2.5 rad/s²，于是输出 0.25 × 2.5 + 2.4525 = 3.0775 N·m。其中 2.4525 N·m 用于抵消重力，剩余 0.625 N·m 用于减小位置误差。若执行器最多只能输出 2 N·m，连当前位置的静态重力都无法抵消，单纯继续增大反馈增益也解决不了这个能力限制。

在真实机器人上，负载估计错误、接触外力、摩擦、采样延迟和转矩饱和都会破坏这种精确抵消。尤其在饱和后，实际转矩不再等于公式要求的转矩，不能继续声称误差必然满足上面的方程。实际验证应同时看跟踪误差和转矩需求，并加入输出限制、状态估计及异常处理。

本节讨论的是明确假设下的计算力矩控制。阅读原书的其他控制证明时，应同时查看作者勘误；作者页面还单独列出了第 4 章第 5 节 PD 控制证明的修正版。

6. 三个小练习，把符号真正对齐

1. 2R 例子中把外力改成向左 f_ext = (−1, 0) N。外力广义转矩和忽略重力时的保持转矩分别是多少？
2. 单摆保持在 −30°，驱动转矩的符号应怎样变化？
3. 计算力矩控制中模型把惯量低估一半，能否直接沿用理想误差方程？

本篇为原创数值推导与学习示例，对应 Murray、Li、Sastry《A Mathematical Introduction to Robotic Manipulation》的动力学与控制主题。书籍版本和勘误以作者页面为准。

如果相同动作每次都出现相似误差，可以继续学习迭代学习控制专题：在已有反馈控制之外，保存上一轮的误差，逐次生成下一轮的前馈补偿。专题配有单关节、两连杆画圆与柔性输出的逐轮实验。