Part II · 第 8 章动力学 · 8.3

质量矩阵与科氏力

机械臂的动力学方程 $M(\mathbf q)\ddot{\mathbf q}+C(\mathbf q,\dot{\mathbf q})\dot{\mathbf q}+\mathbf g(\mathbf q)=\boldsymbol\tau$ 里，三项各管一件事：手臂此刻"有多沉"（质量矩阵），一个关节的运动怎样拽动别的关节（科氏力与离心力），重力在拉什么。这一节把它们拆开来看：质量矩阵是衡量动能的"尺子"，科氏项是尺子随姿态变化的代价，再看工具端到底"感觉有多重"，最后用它们做计算力矩控制和参数辨识。

约 35 分钟前置：1.11 特征值 · 1.12 SVD 与椭圆 · 3.4 机器人雅可比 · 3.5 Hessian · 7.4 速度运动学 · 8.1 拉格朗日动力学 · 8.2 牛顿–欧拉 5 个交互

1为什么机器人需要它

8.1 节用拉格朗日方法、8.2 节用牛顿–欧拉递推，都得到了同一个方程：

$$ \underbrace{\hl{M}{M(\mathbf q)}\,\ddot{\mathbf q}}_{\text{inertia}} \;+\; \underbrace{\hl{C}{C(\mathbf q,\dot{\mathbf q})\,\dot{\mathbf q}}}_{\text{Coriolis, centrifugal}} \;+\; \underbrace{\hl{g}{\mathbf g(\mathbf q)}}_{\text{gravity}} \;=\; \boldsymbol\tau . $$

递推算法回答"给定运动，要多大力矩"，但它是一个黑盒。控制器要的往往是方程的结构：

重力补偿 / 手拖示教：只需要 $\mathbf g(\mathbf q)$。让关节输出 $\boldsymbol\tau=\mathbf g(\mathbf q)$，手臂就"悬浮"在空中，你可以用手推着它走。
计算力矩控制（逆动力学控制）：用 $M$、$C$、$\mathbf g$ 把非线性的手臂"抵消"成一堆互不干扰的积分器，再用简单的 PD 去控制。
阻抗 / 操作空间控制：想让手爪像一个"质量–弹簧–阻尼"，就得知道手爪端的等效质量 $\Lambda(\mathbf q)$，它由 $M$ 和雅可比算出。
仿真与全身控制：物理引擎每一步都在解 $\ddot{\mathbf q}=M^{-1}(\boldsymbol\tau-C\dot{\mathbf q}-\mathbf g)$；足式机器人的全身控制把 $M$ 放进二次规划的等式约束里（第四部分）。
参数辨识与 sim-to-real：方程对质量、质心、惯量是线性的，可以用最小二乘从数据里估计它们；仿真里惯量写错，策略迁移到真机就会"手感不对"。

这一节的目标是：看清每一项长什么样、为什么长这样、在控制里干什么。我们用同一个平面两连杆臂贯穿全篇：$l_1=1$ m，$l_2=0.8$ m，质量 $m_1=2$ kg，$m_2=1.5$ kg，两根连杆都是均匀杆（质心在中点，绕质心的转动惯量 $I=ml^2/12$），$\theta_i$ 从水平方向量起（$\mathbf q=(\theta_1,\theta_2)$），重力沿 $-y$。

2直觉：$M$ 是衡量动能的"尺子"

推一扇很重的门，推门轴附近的地方和推门的外沿，手感完全不同。手臂也是这样：关节 1 转起来"有多沉"，取决于质量离关节 1 有多远。手臂伸直时，质量都甩在外面，转动惯量大；折起来时，质量缩在身边，转动惯量小。所以手臂的惯性不是一个数，而是随姿态变化的一个矩阵 $M(\mathbf q)$。

怎么把"有多沉"量化？看动能。给定关节速度 $\dot{\mathbf q}$，手臂的总动能是一个关于 $\dot{\mathbf q}$ 的二次型：

$$ T(\mathbf q,\dot{\mathbf q}) \;=\; \tfrac12\,\dot{\mathbf q}^{\top}\,\hl{M}{M(\mathbf q)}\,\dot{\mathbf q} \;=\; \tfrac12\sum_{i,j} M_{ij}(\mathbf q)\,\dot q_i\dot q_j . $$

1.7 节的二次型，1.11–1.12 节的特征值与椭圆在这里直接派上用场：$M$ 对称正定，等能量面 $\{\dot{\mathbf q}:\tfrac12\dot{\mathbf q}^\top M\dot{\mathbf q}=E\}$ 是关节速度平面里的一个椭圆。它的半轴是 $\sqrt{2E/\lambda_i}$，沿 $M$ 的特征向量：特征值大的方向很"沉"，同样的能量只能转得很慢，椭圆在那里很窄；特征值小的方向很"轻"，椭圆很长。下图左边拖动手臂的姿态，右边拖动琥珀色的速度手柄 $\dot{\mathbf q}$，看动能怎么变。

FIG 1惯量椭圆：$M(\mathbf q)$ 是动能的尺子

姿态：拖动关节圆圈

关节速度平面 (q̇₁, q̇₂)

右图：琥珀色椭圆是等动能线 $T=E$（$E$ 可用滑块调），紫色虚线是穿过手柄的等动能线，青色虚圈是"速度大小相同"的圆。如果 $M$ 是单位阵，这两者重合；$M$ 越偏离单位阵，椭圆越扁。下图画出 $M_{11}$（红）、$M_{12}$（琥珀）、$M_{22}$（绿）和两个特征值（虚线）随肘部角 $\theta_2$ 的变化，竖线是当前姿态。末端负载加在连杆 2 的尖端，并入连杆 2 的质量、质心和惯量。

试一试

不加负载，把姿态调到让椭圆最圆（条件数 $\kappa(M)=\lambda_{\max}/\lambda_{\min}<5.6$）。这时肘部大概弯了多少？上图曲线里哪个位置最圆？
把速度手柄拖到椭圆长轴的顶端（沿 $M$ 最小特征值的方向，动能与 $E$ 相差不到 10%）。同样的动能，关节速度的大小和在最"沉"的方向上相比差了多少倍？

核心洞见

$M(\mathbf q)$ 不是"质量"，而是关节空间里衡量动能的度量（尺子）：同一个关节速度，在不同姿态下的动能不同；同一个姿态下，沿不同方向的"沉"也不同。它只依赖手臂的形状：对这个平面臂，$M$ 只与 $\theta_2$ 有关，与 $\theta_1$（整条手臂怎么摆）无关。上图读数里"$\theta_1$ 改变后 $M$ 的变化"始终是 0。

3动手玩：离心力与科氏力是从哪来的

"科氏力"听上去神秘，其实只是牛顿第二定律。让关节 1 以恒定角速度转动，连杆 2 的质心在画圆，圆周运动必须有指向圆心的加速度 $\mathbf a=-\omega^2\mathbf r$，也就必须有一个指向圆心的合力 $m\mathbf a$。这个力要由关节 2 的销钉和电机提供，它对关节 2 的力矩就是电机必须出的力矩。如果电机不出，连杆就会"甩出去"：这就是离心的感觉。

下图保持两个关节速度 $\dot\theta_1,\dot\theta_2$ 恒定（$\ddot{\mathbf q}=0$，先忽略重力），琥珀色箭头是维持这种运动，每根连杆质心所需的合力 $m_i\mathbf a_i$。右侧的条形图是关节力矩里与速度有关的部分 $C(\mathbf q,\dot{\mathbf q})\dot{\mathbf q}$，按来源拆开：

离心项（含 $\dot q_i^2$）：某个关节自己的速度平方，例如 $\tau_2\ni h\dot\theta_1^2$。
科氏项（含 $\dot q_i\dot q_j,\ i\ne j$）：两个关节速度的乘积，例如 $\tau_1\ni-2h\dot\theta_1\dot\theta_2$。

其中 $h=m_2l_1l_{c2}\sin\theta_2$。按下"播放"让手臂真的转起来（$\theta_2$ 会跟着变），或者拖动滑块与姿态手柄。

FIG 2离心与科氏力矩实验室

τ₁ 科氏 −2hθ̇₁θ̇₂

τ₁ 离心 −hθ̇₂²

τ₂ 离心 +hθ̇₁²

τ₂ 科氏 (恒为 0)

条形图量程 ±24 N·m。琥珀色箭头 $m_i\mathbf a_i$（0.05 m ↔ 1 N）画在连杆质心上；虚线是关节 2 到质心 2 的杠杆臂，$\tau_2$ 就是这个力对关节 2 的力矩。右上角切换 $C$ 的两种写法（见下）：它们给出同一个 $C\dot{\mathbf q}$，但只有克里斯托费尔写法使 $\dot M-2C$ 斜对称。"牛顿 − 公式"是把两根连杆当刚体、直接用 $\mathbf r\times m\mathbf a$ 算出的力矩与 $C\dot{\mathbf q}$ 的最大差。

试一试

只转关节 1（$\dot\theta_2=0$，$|\dot\theta_1|>3$ rad/s）。关节 2 明明没有加速，为什么要出 $|\tau_2|>3$ N·m？把 $\theta_2$ 拉到 0 或 π（手臂伸直 / 折回），这个力矩怎么变？
让两个关节都在转（$|\dot\theta_1|>1$，$|\dot\theta_2|>1$），而关节 1 的速度相关力矩几乎为零（$|\tau_1|<0.3$ N·m，$|\sin\theta_2|>0.35$）。提示：$2\dot\theta_1\dot\theta_2+\dot\theta_2^2=0$。
在右上角切换到"另一种 $C$"：$C\dot{\mathbf q}$ 的数值不变，但"斜对称误差"不再是 0。这说明了什么？

核心洞见

离心项和科氏项不是新的物理力，而是用关节角作坐标描述牛顿定律时多出来的"速度乘速度"项：关节角是曲线坐标，$M$ 随坐标变化，$\dot{\mathbf q}$ 变化的同时尺子自己也在变，$\tfrac{d}{dt}(M\dot{\mathbf q})$ 里便冒出了 $\dot M\dot{\mathbf q}$。如果手臂的某个姿态变化不改变 $M$（$h=0$，伸直或折回时），这些项就消失。

4第二、第三视角：工具端"感觉"有多重

在关节空间里 $M$ 说得清楚，可是人和任务关心的是手爪端：用手推一下末端，它"像多重的东西"？在方向 $\mathbf u$ 上推一个力 $f$，手爪的加速度由 $\ddot{\mathbf x}=J\ddot{\mathbf q}$（忽略速度项与重力）和 $\ddot{\mathbf q}=M^{-1}J^\top\mathbf F$（3.4 节的 $\boldsymbol\tau=J^\top\mathbf F$）给出：

$$ \ddot{\mathbf x} \;=\; \underbrace{J M^{-1}J^{\top}}_{\hl{Lam}{\Lambda^{-1}}}\,\mathbf F, \qquad \Lambda(\mathbf q)=\big(JM^{-1}J^{\top}\big)^{-1},\qquad m(\mathbf u)=\frac{1}{\mathbf u^{\top}\Lambda^{-1}\mathbf u}. $$

$\Lambda$ 叫操作空间惯量（Khatib，1987），$m(\mathbf u)$ 是沿 $\mathbf u$ 推时的等效质量：推 1 N 沿 $\mathbf u$ 方向得到的加速度分量是 $1/m$。它和 3.4 节的可操作度椭圆是亲戚：若 $M=I$，$\Lambda^{-1}=JJ^\top$，等效质量就是 $1/\sigma_i^2$，手爪"容易动"的方向（椭圆长轴）等效质量小，"难动"的方向（短轴）等效质量大。一般的 $M$ 只是把关节速度的"尺子"换成了动能：速度椭圆变成"单位动能 $\tfrac12\dot{\mathbf x}^\top\Lambda\dot{\mathbf x}=\tfrac12$ 的手爪速度集合"。

在下图拖动手臂，再拖动手爪上的琥珀色小圆改变推的方向（同时画出加速度响应）。右图是极坐标图，半径 = 该方向上的等效质量。

FIG 3工具端的等效质量 $\Lambda(\mathbf q)$

工作空间：拖动关节与推力方向

等效质量 m(u)（kg），极坐标

左：琥珀色实线椭圆是 $\tfrac12\dot{\mathbf x}^\top\Lambda\dot{\mathbf x}=\tfrac12$ 的手爪速度集合（长轴方向 = 轻方向），青色虚线是 3.4 节的可操作度椭圆（$\|\dot{\mathbf q}\|\le1$）；琥珀色箭头是推力 $\mathbf F$（1 N ↔ 0.5 m），绿色箭头是手爪的加速度 $\Lambda^{-1}\mathbf F$（1 m/s² ↔ 0.3 m）：除非沿特征方向推，手爪不会朝你推的方向加速。右：琥珀色曲线是 $m(\mathbf u)$，紫色虚圈是整条手臂的总质量 3.5 kg，半径超过 5 kg 的部分被截断。

试一试

找出让等效质量最各向同性的姿态（$\kappa(\Lambda)<4.6$）。它是"伸直"还是"弯成约 90°"？能不能做到 $\kappa=1$？
把手臂拉到接近伸直，让最大的等效质量超过 20 kg。手臂总共才 3.5 kg，这 20 kg 是哪里来的？

一个概念，三个视角

矩阵视角：$M=\partial^2T/\partial\dot{\mathbf q}^2$ 就是动能对速度的 Hessian（3.5 节），因为 $T$ 是二次的，Hessian 就是 $M$ 本身。几何视角：$M(\mathbf q)$ 给关节空间定义了一个随位置变化的"长度尺子" $\|\dot{\mathbf q}\|_M^2=\dot{\mathbf q}^\top M\dot{\mathbf q}$（黎曼度量）。不受力、无重力时手臂走的是这个度量下的"直线"（测地线），克里斯托费尔符号就是描述"直线"如何弯曲的联络，所以 $C$ 恰好由 $\partial M/\partial\mathbf q$ 构造（第 5 节）。操作空间视角：把这把尺子经 $J$ 投影到手爪端，就是 $\Lambda$。

5再看数学

提示：把鼠标悬停在公式中带底色的符号上，对应的图元素会被高亮（FIG 1 的椭圆、FIG 2 的条形图、FIG 3 的等效质量椭圆）。

从动能得到 $M$，并证明它对称正定

第 $i$ 根连杆的质心速度与角速度都是关节速度的线性函数：$\mathbf v_{ci}=J_{vi}(\mathbf q)\dot{\mathbf q}$，$\boldsymbol\omega_i=J_{\omega i}(\mathbf q)\dot{\mathbf q}$（7.4 节的雅可比，对象换成各连杆的质心）。刚体动能是平动加转动：

$$ T=\sum_{i=1}^{n}\Big(\tfrac12 m_i\|\mathbf v_{ci}\|^2+\tfrac12\boldsymbol\omega_i^{\top}\,{}^{0}\!I_i\,\boldsymbol\omega_i\Big)=\tfrac12\dot{\mathbf q}^{\top}\Big[\sum_{i=1}^{n}\big(m_iJ_{vi}^{\top}J_{vi}+J_{\omega i}^{\top}\,{}^{0}\!I_i\,J_{\omega i}\big)\Big]\dot{\mathbf q}, $$

其中 ${}^{0}I_i=R_iI_iR_i^\top$ 是连杆 $i$ 的惯量张量在基座系中的表示。方括号就是 $M(\mathbf q)$。用 5.10 节的 twist 写，它更紧凑：$M=\sum_i J_{bi}^\top\mathcal G_iJ_{bi}$，$J_{bi}$ 是连杆 $i$ 质心系的 $6\times n$ 体雅可比，$\mathcal G_i=\mathrm{diag}(I_i,\,m_i\mathbf 1_3)$ 是空间惯量（角部分在前，和本课程的 twist 顺序一致）。

对称正定的证明。每一项 $m_iJ_{vi}^\top J_{vi}$ 与 $J_{\omega i}^\top{}^{0}I_iJ_{\omega i}$ 都是对称半正定的（$m_i>0$，$I_i$ 正定）。对任何 $\dot{\mathbf q}\ne0$，$T=0$ 要求每根连杆的质心速度和角速度都为零，也就是整台机器人静止，而关节速度非零必然使至少一根连杆在动（链式结构中第一个有非零关节速度的连杆本身就在转或移动）。所以 $\dot{\mathbf q}^\top M\dot{\mathbf q}=2T>0$，$M$ 正定。又因为对转动关节的臂，$M$ 的元素是 $\mathbf q$ 的有界连续（三角）函数，存在与姿态无关的常数 $0<\underline m\le\lambda_{\min}(M)\le\lambda_{\max}(M)\le\overline m$：这是控制器稳定性证明里反复用到的"有界性"。

对本课的平面两连杆臂，展开 $T$（质心 2 的速度平方为 $l_1^2\dot\theta_1^2+l_{c2}^2(\dot\theta_1+\dot\theta_2)^2+2l_1l_{c2}\cos\theta_2\,\dot\theta_1(\dot\theta_1+\dot\theta_2)$）并读出系数：

$$ \begin{aligned} M_{11}&=m_1l_{c1}^2+I_1+m_2\big(l_1^2+l_{c2}^2+2l_1l_{c2}\cos\theta_2\big)+I_2,\\ M_{12}&=m_2\big(l_{c2}^2+l_1l_{c2}\cos\theta_2\big)+I_2,\qquad M_{22}=m_2l_{c2}^2+I_2 . \end{aligned} $$

只有 $\cos\theta_2$ 出现：$\theta_2=0$（伸直）时 $M_{11}$ 最大；$\theta_2=\pi$（折回）时最小，FIG 1 下方的曲线就是这三个式子。$M_{22}$ 是常数：关节 2 单独转动时，连杆 2 绕关节 2 的转动惯量不随姿态变，但是它和关节 1 的耦合 $M_{12}$ 变了。

克里斯托费尔符号：从 $\partial M/\partial\mathbf q$ 造出 $C$

8.1 节的欧拉–拉格朗日方程 $\tfrac{d}{dt}\tfrac{\partial T}{\partial\dot q_k}-\tfrac{\partial T}{\partial q_k}+\tfrac{\partial V}{\partial q_k}=\tau_k$ 对二次型 $T=\tfrac12\sum M_{ij}\dot q_i\dot q_j$ 求导，得到

$$ \sum_{j}M_{kj}\ddot q_j+\sum_{i,j}\Big(\frac{\partial M_{kj}}{\partial q_i}-\tfrac12\frac{\partial M_{ij}}{\partial q_k}\Big)\dot q_i\dot q_j+\frac{\partial V}{\partial q_k}=\tau_k . $$

括号里的系数对 $i,j$ 不对称，但 $\dot q_i\dot q_j$ 对称，所以可以只保留其对称部分，这样得到的系数叫第一类克里斯托费尔符号：

$$ \hl{C}{c_{ijk}}=\tfrac12\Big(\frac{\partial M_{kj}}{\partial q_i}+\frac{\partial M_{ki}}{\partial q_j}-\frac{\partial M_{ij}}{\partial q_k}\Big),\qquad C_{kj}(\mathbf q,\dot{\mathbf q})=\sum_{i}c_{ijk}\,\dot q_i,\qquad \big(C\dot{\mathbf q}\big)_k=\sum_{i,j}c_{ijk}\dot q_i\dot q_j . $$

两连杆算例。$M$ 只依赖 $q_2=\theta_2$，所以只有 $\partial M/\partial q_2$ 非零：$\partial M_{11}/\partial q_2=-2h$，$\partial M_{12}/\partial q_2=-h$，$\partial M_{22}/\partial q_2=0$，其中 $h=m_2l_1l_{c2}\sin\theta_2$。代入得 $c_{211}=-h$，$c_{121}=c_{221}=-h$，$c_{112}=h$，其余为零，即

$$ C=\begin{bmatrix}-h\dot\theta_2 & -h(\dot\theta_1+\dot\theta_2)\\ h\dot\theta_1 & 0\end{bmatrix},\qquad C\dot{\mathbf q}=\begin{bmatrix}\ci{-h\,\dot\theta_2^{2}}\ \ \ci{-\,2h\dot\theta_1\dot\theta_2}\\ \cj{h\,\dot\theta_1^{2}}\end{bmatrix}. $$

这正是 FIG 2 四根条形图的来源：含 $\dot\theta_i^2$ 的是离心项，含 $\dot\theta_1\dot\theta_2$ 的是科氏项。代码里的"克里斯托费尔 − 闭式"读数，就是对 $M$ 数值求偏导后用上面的公式算出 $C$，与这个闭式的差（约 $10^{-9}$）。

$C$ 不唯一，$C\dot{\mathbf q}$ 唯一；斜对称性与能量守恒

运动方程里真正出现的只有向量 $C\dot{\mathbf q}$，它由 $c_{ijk}$ 的对称部分唯一确定。但"哪个矩阵乘 $\dot{\mathbf q}$ 得到它"有很多种分法：加上任何满足 $S\dot{\mathbf q}=0$ 的 $S$ 都行。例如 $C_b=\begin{bmatrix}0&-h(2\dot\theta_1+\dot\theta_2)\\ h\dot\theta_1&0\end{bmatrix}$ 给出同样的 $C\dot{\mathbf q}$。克里斯托费尔的分法的特别之处是下面的性质：

$$ \big(\dot M-2C\big)_{kj}=\sum_i\Big(\frac{\partial M_{ij}}{\partial q_k}-\frac{\partial M_{ki}}{\partial q_j}\Big)\dot q_i \;=\; -\big(\dot M-2C\big)_{jk}\quad\Longrightarrow\quad \dot M-2C\ \text{ is skew-symmetric.} $$

（把 $\dot M_{kj}=\sum_i\partial_iM_{kj}\dot q_i$ 与 $2C_{kj}=\sum_i(\partial_iM_{kj}+\partial_jM_{ki}-\partial_kM_{ij})\dot q_i$ 相减即得；交换 $k\leftrightarrow j$ 并利用 $M$ 的对称性，符号反号。）斜对称矩阵的二次型为零，所以对任何满足 $C\dot{\mathbf q}$ 相同的分法都有 $\dot{\mathbf q}^\top(\dot M-2C)\dot{\mathbf q}=0$，进而得到功率平衡：

$$ \dot T=\dot{\mathbf q}^{\top}M\ddot{\mathbf q}+\tfrac12\dot{\mathbf q}^{\top}\dot M\dot{\mathbf q}=\dot{\mathbf q}^{\top}\big(\boldsymbol\tau-C\dot{\mathbf q}-\mathbf g\big)+\tfrac12\dot{\mathbf q}^{\top}\dot M\dot{\mathbf q}=\dot{\mathbf q}^{\top}(\boldsymbol\tau-\mathbf g)+\tfrac12\dot{\mathbf q}^{\top}(\dot M-2C)\dot{\mathbf q}=\dot{\mathbf q}^{\top}(\boldsymbol\tau-\mathbf g). $$

科氏力与离心力不做功：它们只在关节之间重新分配动能，总动能的变化只来自电机输出功率 $\dot{\mathbf q}^\top\boldsymbol\tau$ 和重力做功。（对 $n=2$，克里斯托费尔分法是同时满足"给出 $C\dot{\mathbf q}$"和"$\dot M-2C$ 斜对称"的唯一矩阵；$n\ge3$ 时还有别的。自适应控制的证明用到的是斜对称性，所以工程上默认用克里斯托费尔分法，Pinocchio 的 computeCoriolisMatrix 给出的 $C$ 也是如此。）

重力项与"对参数线性"

重力来自势能：$V=\sum_i m_ig\,h_i(\mathbf q)$（$h_i$ 是质心高度），$\mathbf g(\mathbf q)=\partial V/\partial\mathbf q$。平面臂里 $g_1=(m_1l_{c1}+m_2l_1)g\cos\theta_1+m_2l_{c2}g\cos(\theta_1+\theta_2)$，$g_2=m_2l_{c2}g\cos(\theta_1+\theta_2)$：每一项都是 $\partial(\text{高度})/\partial\theta$ 乘以重量，也就是重力对该关节的力臂。把前面所有的式子合起来，每一项都是"一个只和 $(\mathbf q,\dot{\mathbf q},\ddot{\mathbf q})$ 有关的已知函数 × 一个只含物理参数的数"，所以整个方程可以写成

$$ \boldsymbol\tau=Y(\mathbf q,\dot{\mathbf q},\ddot{\mathbf q})\,\boldsymbol\pi,\qquad \boldsymbol\pi=\begin{bmatrix}a\\b\\c\\d\\e\end{bmatrix}=\begin{bmatrix}m_1l_{c1}^2+I_1+m_2(l_1^2+l_{c2}^2)+I_2\\ m_2l_1l_{c2}\\ m_2l_{c2}^2+I_2\\ m_1l_{c1}+m_2l_1\\ m_2l_{c2}\end{bmatrix}. $$

$Y$ 叫回归矩阵。两根连杆有 6 个惯性参数（$m_i,l_{ci},I_i$），但只有 5 个组合 $a,\dots,e$ 能被力矩数据区分，称为基参数（Gautier–Khalil，1990）。$\boldsymbol\pi$ 的辨识就是一个线性最小二乘问题（1.13 节）：把 $N$ 个时刻的 $Y_k$ 叠成大矩阵 $A$，解 $\hat{\boldsymbol\pi}=\arg\min\|A\boldsymbol\pi-\boldsymbol\tau\|^2$；$A$ 的条件数决定了测量噪声被放大多少，因此要选"激励充分"的轨迹（FIG 5）。

正动力学、逆动力学，以及怎样算 $M$

$$ \underbrace{\boldsymbol\tau=M(\mathbf q)\ddot{\mathbf q}+\mathbf b(\mathbf q,\dot{\mathbf q})}_{\text{inverse dynamics (RNEA)}},\qquad \underbrace{\ddot{\mathbf q}=M(\mathbf q)^{-1}\big(\boldsymbol\tau-\mathbf b(\mathbf q,\dot{\mathbf q})\big)}_{\text{forward dynamics (simulation)}},\qquad \mathbf b=C\dot{\mathbf q}+\mathbf g . $$

8.2 节的牛顿–欧拉递推（RNEA）一次 $O(n)$ 地算出逆动力学，不需要显式的 $M$ 和 $C$。要得到 $M$ 本身有一个小技巧：令 $\dot{\mathbf q}=0$、重力为零，那么 $\tau=M\ddot{\mathbf q}$，取 $\ddot{\mathbf q}=\mathbf e_j$，RNEA 的输出就是 $M$ 的第 $j$ 列。更高效的是 Featherstone 的组合刚体算法（CRBA），而正动力学则有 $O(n)$ 的关节体算法（ABA）。偏置项 $\mathbf b$ 是令 $\ddot{\mathbf q}=0$ 的 RNEA。MuJoCo 里的 qfrc_bias 就是这个 $\mathbf b$。

带浮动基座的机器人（足式、人形，9.2 节）把广义坐标扩成 $\mathbf q=(\text{基座位姿},\mathbf q_{\text{joints}})$，$M$ 变成 $\begin{bmatrix}M_{bb}&M_{bj}\\M_{jb}&M_{jj}\end{bmatrix}$，基座那 6 行没有电机，$\boldsymbol\tau$ 的对应行为零，由接触力 $J_c^\top\mathbf f_c$ 提供：形式不变，只是多了接触。

计算力矩控制与能量（无源性）论证

令 $\mathbf e=\mathbf q_d-\mathbf q$。计算力矩控制律用模型 $\hat M,\hat C,\hat{\mathbf g}$ 去抵消手臂：

$$ \boldsymbol\tau=\hat M(\mathbf q)\,\mathbf a+\hat C(\mathbf q,\dot{\mathbf q})\dot{\mathbf q}+\hat{\mathbf g}(\mathbf q),\qquad \mathbf a=\ddot{\mathbf q}_d+K_d\dot{\mathbf e}+K_p\mathbf e . $$

模型精确时代入动力学方程，$M(\ddot{\mathbf q}-\mathbf a)=0$，而 $M$ 可逆，所以 $\ddot{\mathbf e}+K_d\dot{\mathbf e}+K_p\mathbf e=0$：误差满足线性、各关节互相解耦的二阶系统，$K_p=\omega_n^2\mathbf 1$、$K_d=2\omega_n\mathbf 1$ 时每个关节都是临界阻尼，指数收敛。模型不精确时，同样的推导给出

$$ \ddot{\mathbf e}+K_d\dot{\mathbf e}+K_p\mathbf e=-\boldsymbol\eta,\qquad \boldsymbol\eta=M^{-1}\Big[(\hat M-M)\,\mathbf a+(\hat C-C)\dot{\mathbf q}+\hat{\mathbf g}-\mathbf g\Big] . $$

稳定的线性系统被有界的输入 $\boldsymbol\eta$ 驱动，误差有界，且 $\|\boldsymbol\eta\|$ 正比于模型误差：这就是"优雅退化"。增大 $\omega_n$ 能迅速压低误差（本课仿真里大致按 $1/\omega_n^2$ 下降），但会放大噪声、触及电机饱和与采样延迟，所以实际要靠更好的模型（FIG 5）或自适应、鲁棒控制（Slotine–Li，第四部分）。

只用重力补偿的 PD 为什么也稳定？用 $\tilde{\mathbf q}=\mathbf q-\mathbf q^*$，控制律 $\boldsymbol\tau=\mathbf g(\mathbf q)-K_p\tilde{\mathbf q}-K_d\dot{\mathbf q}$（$K_p,K_d$ 正定），取能量函数 $W=T+\tfrac12\tilde{\mathbf q}^\top K_p\tilde{\mathbf q}$。由功率平衡，

$$ \dot W=\dot{\mathbf q}^{\top}(\boldsymbol\tau-\mathbf g)+\tilde{\mathbf q}^{\top}K_p\dot{\mathbf q}=-\dot{\mathbf q}^{\top}K_d\dot{\mathbf q}\le0 . $$

能量只减不增；$\dot W=0$ 时 $\dot{\mathbf q}=0$，进而 $\ddot{\mathbf q}=0$，方程给出 $K_p\tilde{\mathbf q}=0$，所以 LaSalle 不变原理保证 $\mathbf q\to\mathbf q^*$。关键一步用到了斜对称性：科氏项在 $\dot W$ 里被消掉了。这就是"无源性"论证：手臂是一个以 $\dot{\mathbf q}^\top\boldsymbol\tau$ 为功率输入的耗能系统。

操作空间惯量的推导

令手爪受力 $\mathbf F$，关节力矩 $\boldsymbol\tau=J^\top\mathbf F$。由 $\ddot{\mathbf x}=J\ddot{\mathbf q}+\dot J\dot{\mathbf q}$ 和 $\ddot{\mathbf q}=M^{-1}(J^\top\mathbf F-C\dot{\mathbf q}-\mathbf g)$ 消去 $\ddot{\mathbf q}$：

$$ \hl{Lam}{\Lambda}\,\ddot{\mathbf x}+\boldsymbol\mu+\mathbf p=\mathbf F,\qquad \Lambda=(JM^{-1}J^{\top})^{-1},\quad \boldsymbol\mu=\Lambda\big(JM^{-1}C-\dot J\big)\dot{\mathbf q},\quad \mathbf p=\Lambda JM^{-1}\mathbf g . $$

手爪端也满足"质量×加速度 + 速度项 + 重力 = 力"，只是质量变成了 $\Lambda$。它还有一个最小动能的解释：满足 $J\dot{\mathbf q}=\dot{\mathbf x}$ 的所有关节速度里，动能最小的一个是 $\dot{\mathbf q}=M^{-1}J^\top\Lambda\dot{\mathbf x}$，最小动能就是 $\tfrac12\dot{\mathbf x}^\top\Lambda\dot{\mathbf x}$（FIG 3 左边的椭圆正是它的等值线）。当 $J$ 不满行秩时（奇异位形，或任务维数大于关节数），$JM^{-1}J^\top$ 奇异，$\Lambda$ 在丢失的方向上趋于无穷：该方向上手爪根本无法被关节加速。

6机器人中的它：计算力矩控制与参数辨识

下面是一个真实的仿真：手爪要在 1 秒量级的周期里画一个圆（或 8 字），关节用 RK4 以 1 ms 步长积分，控制器以 1 kHz 更新（零阶保持）。三种控制器共用同一个初始误差：(a) 纯 PD；(b) PD + 重力补偿；(c) 计算力矩。PD 的增益按沿参考轨迹的平均惯量缩放，使三者名义带宽 $\omega_n$ 相同。"模型质量误差 $\delta$"让控制器以为所有质量都是真实值的 $1+\delta$ 倍，"关节摩擦 $b$"是控制器不知道的粘滞摩擦 $-b\dot{\mathbf q}$。

把 $\delta$ 和 $b$ 都拖到 0，看计算力矩的误差塌缩到仿真精度；再拖开，看它如何"退化"。把"误差"切换成对数坐标，指数收敛是一条直线。

FIG 4为什么基于模型的控制有用

手爪轨迹（虚线 = 参考）

手爪位置误差 (m) vs 时间 (s)

紫色 = PD，青色 = PD + 重力补偿，琥珀色 = 计算力矩；参考与初始误差 $(0.12,-0.10)$ rad。读数里的 RMS 是 $t\ge1$ s 之后的手爪位置误差均方根（m）；"τ₁ 峰值"是关节 1 的最大力矩，这里没有加饱和限制，真实电机的力矩上限会让大误差时的收益打折。圆周参考半径 0.38 m，周期默认 1.5 s（最大手爪速度约 1.6 m/s）。

试一试

把模型质量误差 $\delta$ 和摩擦 $b$ 都调到 0：计算力矩的 RMS 降到 1 mm 以下，而纯 PD 仍超过 5 cm。
把模型质量误差调到 $-30\%$（模型偏轻）。计算力矩的误差上升了，但仍然比"PD + 重力补偿"小吗？再试 $+30\%$，结果一样吗？
让模型几乎准确（$|\delta|\le5\%$，$b\le0.2$），把 $\omega_n$ 降到 7 rad/s 以下：纯 PD 的 RMS 超过 30 cm，计算力矩仍在 3 cm 以内。是谁在做主要的工作？

模型从哪里来？厂家给的 CAD 质量参数往往不准，负载（夹爪、工件）又会变。方法是辨识：让手臂跑一条"激励充分"的轨迹，记录 $(\mathbf q,\dot{\mathbf q},\ddot{\mathbf q},\boldsymbol\tau)$，解线性最小二乘 $A\boldsymbol\pi\approx\boldsymbol\tau$。下图中，数据是上面的真实动力学加上高斯力矩噪声。选三种激励：丰富（两个关节都大幅运动）、只动关节 1（关节 2 冻结）、缓慢小幅运动，看 $A$ 的条件数和各参数的相对误差。

FIG 5从数据辨识惯性参数

激励轨迹 (θ₁, θ₂)，60 个样本

左：激励轨迹在关节角平面里的样子。右：五个基参数的真值、估计值与相对误差条（对数刻度，0.1% 到 100%；绿 < 5%，琥珀 5–30%，红 > 30%）。$\kappa(A)$ 是回归矩阵 $A$ 的条件数（未做列缩放）。"重新采样噪声"换一组随机数，看估计值的抖动。

试一试

选"丰富"激励，噪声 $\sigma\ge0.5$ N·m，让五个参数的相对误差都低于 10%。
选"缓慢小幅"激励，噪声 $\sigma\ge0.5$ N·m：最差的参数误差超过 100%，$\kappa(A)$ 是多少？为什么手臂缓慢运动时惯量几乎看不出来？

真实系统里

重力补偿与手拖示教：协作臂（UR 的 freedrive、KUKA LBR iiwa、Franka 的手拖模式）读出关节力矩传感器或电流，只补 $\mathbf g(\mathbf q)$（加一点摩擦补偿），手臂就能悬浮、被人推着走。力矩控制臂：Franka（libfranka 的 franka::Model 提供 mass、coriolis、gravity）和 iiwa 以 1 kHz 在内环里用 $M,C,\mathbf g$ 做计算力矩与阻抗控制。动力学库：Pinocchio（crba、rnea、aba、computeCoriolisMatrix）、Drake（CalcMassMatrix、CalcBiasTerm）、MuJoCo（mj_fullM、qfrc_bias）都在做本节的计算。操作空间 / 全身控制：Khatib 的操作空间框架用 $\Lambda$ 做任务空间控制；四足与人形的全身控制器（第四部分）把 $M\ddot{\mathbf q}+\mathbf b=S^\top\boldsymbol\tau+J_c^\top\mathbf f_c$ 当作二次规划的等式约束。辨识：Atkeson 等（1986）开创了用最小二乘估计连杆与负载的惯性参数，如今商用臂出厂时和换夹爪后都会做负载辨识。sim-to-real：仿真里的惯量、电机转子惯量、摩擦与真机不符，是策略迁移失败的常见原因，所以强化学习常做动力学随机化（Peng 等，2018）；也有人用神经网络直接学 $M$、$C$，同时保持"$M$ 正定"之类结构（Lutter 等，Deep Lagrangian Networks，2019）。

7常见误区

误区 1：把 $M$ 当成常数

FIG 1 里 $M$ 的条件数从伸直时的 45 左右降到弯曲时的 5 左右，$M_{11}$ 差了近三倍。用固定增益的关节 PD，有效带宽 $\sqrt{K_p/M}$ 随姿态变化：同一组增益在一个姿态下临界阻尼，在另一个姿态下就欠阻尼振荡。直觉在哪里失灵？对工业臂的每个关节单独调参常常"够用"，是因为大减速比让电机转子惯量压过了 $M$ 的变化（见误区 4）；换成直驱或低减速比的臂（力矩控制的协作臂、四足的腿）就不行。

误区 2：把科氏力当成"真实的力"

这里的科氏项与离心项是坐标的产物：一个不受力的质点，在笛卡尔坐标里 $\ddot x=0$，一点速度项都没有；换成极坐标，$\ddot r-r\dot\phi^2=0$ 里立刻冒出"离心项"。机械臂的 $C\dot{\mathbf q}$ 同理，是用关节角描述运动的代价。判断标准是它们不做功（斜对称性）：只改变动能在关节间的分配，不改变总动能。另外它不同于地球自转的科氏力，不要把两者混为一谈。

误区 3：以为 $C$ 是唯一的

唯一的是向量 $C\dot{\mathbf q}$，不是矩阵 $C$。FIG 2 里两种 $C$ 给出同样的力矩，却一个满足 $\dot M-2C$ 斜对称、一个不满足。需要做无源性或自适应控制的证明，或者要用 $\dot{\mathbf q}^\top(\dot M-2C)\dot{\mathbf q}=0$ 之外的性质（如 $\dot M=C+C^\top$）时，必须说明用的是克里斯托费尔写法；只算力矩时，任何一种都行，不必纠结。

误区 4：忽略电机转子惯量与关节摩擦

本节的 $M$ 只含连杆。真实关节还有转子惯量 $I_m$，经减速比 $N$ 折算到关节侧是 $N^2I_m$（"反射惯量"）：$N=100$ 时，它可以比连杆的惯量大一到两个数量级，使 $M$ 近似为常数对角阵，粘滞、库仑和 Stribeck 摩擦也常常与 $C\dot{\mathbf q}$ 一样大。FIG 4 里没建模的摩擦会让计算力矩的误差升到毫米以上。真实的辨识要把转子惯量和摩擦也放进回归矩阵（9.4 节）。

误区 5：在秩亏的 $J$ 上用等效质量公式

FIG 3 中把手臂拉直，$J$ 奇异，$\Lambda^{-1}=JM^{-1}J^\top$ 出现零特征值，沿丢失方向的 $\Lambda$ 趋于无穷。这不是数值问题，而是物理事实：那个方向上手爪根本动不了。写代码时别直接对 $JM^{-1}J^\top$ 求逆，而要先检查 $\sigma_{\min}(J)$，必要时加阻尼或降维。冗余臂（$J$ 行满秩而列多）没有这个问题，但关节空间的逆应该用动力学一致的广义逆 $\bar J=M^{-1}J^\top\Lambda$，而不是 $J^+$。还有，位置和姿态混在一个 6 维 $\Lambda$ 里时单位是 kg 与 kg·m²，对它求特征值没有意义（同 3.4 节的误区 4）。

8小练习

1. 关于刚体机械臂的质量矩阵 $M(\mathbf q)$，下列哪个说法是对的？

$M$ 来自动能二次型，所以对称、且对 $\dot{\mathbf q}\ne0$ 有 $T>0$，即正定；它依赖关节构型。奇异位形是雅可比 $J$ 的性质，不影响 $M$（只要质量和惯量为正），$M$ 在任何姿态下都可逆。不要把"$J$ 奇异"和"$M$ 奇异"混淆。

2. 本课的两连杆臂（$l_1=1$ m，$l_2=0.8$ m，$m_2=1.5$ kg，$l_{c2}=0.4$ m）在 $\theta_2=\pi/2$ 时，关节 1 以恒定 $\dot\theta_1=2$ rad/s 转动，$\dot\theta_2=0$、$\ddot{\mathbf q}=0$。忽略重力，关节 2 的电机需要提供多大力矩？

$h=m_2l_1l_{c2}\sin\theta_2=1.5\times1\times0.4\times1=0.6$ kg·m²，$\tau_2=h\dot\theta_1^2=0.6\times4=2.4$ N·m。直接用牛顿定律检验：质心 2 离基座 $\sqrt{1+0.16}\approx1.077$ m，向心加速度 $\omega^2r\approx4.31$ m/s²；取 $m_2\mathbf a$ 对关节 2 的力矩，同样得到 2.4 N·m。

3. $\dot M-2C$ 斜对称意味着什么？

由 $\dot T=\dot{\mathbf q}^\top M\ddot{\mathbf q}+\tfrac12\dot{\mathbf q}^\top\dot M\dot{\mathbf q}$ 和动力学方程，$\dot T=\dot{\mathbf q}^\top(\boldsymbol\tau-\mathbf g)+\tfrac12\dot{\mathbf q}^\top(\dot M-2C)\dot{\mathbf q}$，斜对称使最后一项为零。它与 $C$ 是否为零、$M$ 是否常数、有没有摩擦都无关。

4. 手臂接近伸直（奇异位形）时，FIG 3 里沿"丢失方向"的等效质量变得很大。为什么？

沿丢失方向 $J$ 的像趋于零，$\mathbf u^\top JM^{-1}J^\top\mathbf u\to0$，所以 $m(\mathbf u)=1/(\mathbf u^\top\Lambda^{-1}\mathbf u)\to\infty$。质量没变，$M$ 也没有奇异，也与重力无关；它是运动学（3.4 节奇异位形）在动力学里的影子。

5. 在计算力矩控制里，模型质量有 30% 的误差。FIG 4 的结果最接近下面哪种描述？

误差方程 $\ddot{\mathbf e}+K_d\dot{\mathbf e}+K_p\mathbf e=-\boldsymbol\eta$ 是被有界扰动 $\boldsymbol\eta$ 驱动的稳定线性系统，$\boldsymbol\eta$ 正比于模型误差。所以误差有界、随失配增长；在这个例子里偏重和偏轻的结果并不对称：模型偏轻（$-30\%$）的误差约是偏重（$+30\%$）的两倍，但两种情况下计算力矩都仍比"PD + 重力补偿"好约 1.5 倍。

9延伸阅读

M. W. Spong, S. Hutchinson & M. Vidyasagar, Robot Modeling and Control, 2nd ed., Wiley, 2020, Ch. 7–8 — 动力学（质量矩阵、克里斯托费尔符号、斜对称性、线性参数化）与多变量控制（逆动力学控制）
J.-J. E. Slotine & W. Li, Applied Nonlinear Control, Prentice Hall, 1991, Ch. 9 — 机器人动力学的性质、无源性证明、自适应控制
R. M. Murray, Z. Li & S. S. Sastry, A Mathematical Introduction to Robotic Manipulation, CRC Press, 1994, Ch. 4 — 机械臂动力学与控制，用旋量写法推导 $M$、$C$
K. Lynch & F. Park, Modern Robotics, Ch. 8 and Ch. 11 — 开链动力学（拉格朗日、牛顿–欧拉、空间惯量）与机器人控制（计算力矩、任务空间运动控制）
B. Siciliano, L. Sciavicco, L. Villani & G. Oriolo, Robotics: Modelling, Planning and Control, Springer, 2009, Ch. 7–8 — 动力学（含参数线性化）与运动控制
O. Khatib, A Unified Approach for Motion and Force Control of Robot Manipulators: The Operational Space Formulation, IEEE J. Robotics and Automation 3(1), 1987 — 操作空间惯量 $\Lambda$ 的出处
R. Featherstone, Rigid Body Dynamics Algorithms, Springer, 2008 — RNEA、CRBA、ABA 的权威参考
C. G. Atkeson, C. H. An & J. M. Hollerbach, Estimation of Inertial Parameters of Manipulator Loads and Links, Int. J. Robotics Research 5(3), 1986; M. Gautier & W. Khalil, Direct Calculation of Minimum Set of Inertial Parameters of Serial Robots, IEEE Trans. Robotics and Automation 6(3), 1990 — 惯性参数辨识与基参数
J.-J. E. Slotine & W. Li, On the Adaptive Control of Robot Manipulators, Int. J. Robotics Research 6(3), 1987 — 用斜对称性证明的自适应控制
M. Lutter, C. Ritter & J. Peters, Deep Lagrangian Networks: Using Physics as Model Prior for Deep Learning, ICLR 2019 — 用神经网络学习保持物理结构的 $M$、$C$