拉格朗日动力学

前面所有关于机械臂的内容都是"位置层面"的：关节角 \(\mathbf q\) 给定，手爪在哪里（7.3），速度怎么传递（3.4），静止时力怎么传递（\(\boldsymbol\tau=J^\top F\)）。可电机给的是力矩，不是位置。只要你想做下面这些事，就必须知道"力矩 \(\boldsymbol\tau\) 会产生什么加速度 \(\ddot{\mathbf q}\)"：

• 仿真：MuJoCo、Isaac、Drake、Pinocchio 每一个时间步都在算 \(\ddot{\mathbf q} = M^{-1}(\boldsymbol\tau - C\dot{\mathbf q} - \mathbf g)\)，再积分（2.5 节）。强化学习的训练环境本质上就是这个方程。
• 力矩控制：计算力矩、阻抗控制、重力补偿、人机共融的力控机械臂，全都用模型预测"需要多大力矩才能产生这个加速度"（逆动力学）。
• 系统辨识与故障诊断：从测得的 \((\mathbf q,\dot{\mathbf q},\ddot{\mathbf q},\boldsymbol\tau)\) 反推质量、质心、惯量。
• 轨迹优化与 MPC（Part IV）：把方程当作约束，决定"最省能量的路径"。

怎样得到这个方程？牛顿的办法是对每一块刚体写 \(F=ma\)，再把关节处的约束力一个一个消去，连杆一多就是一场灾难。拉格朗日的办法换了个视角：选好描述构型的独立变量（广义坐标，7.1 节），写出整个系统的动能 \(T\) 和势能 \(V\)，然后做一次机械的求导。约束力不做功，所以它们根本不会出现。

把一个球向上抛，它在 \(t=0\) 离手、\(t=T\) 落回同一高度。球可以走无数条路径，比如先猛冲再慢慢爬，或者来回抖动，但它走的恰好是那条抛物线。能不能用一个全局的准则解释为什么？

定义拉格朗日量 \(\mathcal L = T - V\)（动能减势能），再定义整条路径的作用量 \(S[q]=\int_0^T \mathcal L\,dt\)。驻定作用量原理（哈密顿原理）说：真实运动是使 \(S\) 驻定的路径，也就是在固定两端的所有路径中，往任何方向做一点微小扰动，\(S\) 的一阶变化都为零。下图把真实路径加上一个扰动 \(\varepsilon\sin(k\pi t/T)\)（两端不动），画出 \(S\) 随 \(\varepsilon\) 的变化。

注意"驻定"不总是"最小"：路径时间足够长时，\(S\) 的谷底会变成鞍点（摆的 \(T\) 超过半个周期时就是这样）。这里选的 \(T\) 都足够短，所以看到的是真正的最小值。机器人学里我们只用到"一阶变化为零"这一点。

最简单的机器人关节：质量 \(m\) 的小球挂在长 \(l\) 的轻杆端点，杆绕枢轴转动，角度 \(\theta\) 从正下方量起（逆时针为正）。枢轴处有电机力矩 \(\tau\) 和黏性摩擦 \(b\dot\theta\)。只有一个自由度 \(\theta\)，所以只有一条方程。照着右下角实时更新的四步走：

1. 写动能和势能：\(T=\tfrac12 ml^2\dot\theta^2\)，\(V=mgl(1-\cos\theta)\)，\(\mathcal L=T-V\)。
2. 求 \(\partial\mathcal L/\partial\dot\theta = ml^2\dot\theta\)（"广义动量"）。
3. 对时间求导：\(\tfrac{d}{dt}\tfrac{\partial\mathcal L}{\partial\dot\theta}=ml^2\ddot\theta\)。
4. 求 \(\partial\mathcal L/\partial\theta=-mgl\sin\theta\)，代入 \(\tfrac{d}{dt}\tfrac{\partial\mathcal L}{\partial\dot\theta}-\tfrac{\partial\mathcal L}{\partial\theta}=\tau-b\dot\theta\)：

小角度时 \(\sin\theta\approx\theta\)（2.4 节），就是熟悉的弹簧振子。下图用 RK4 积分这条方程（2.5 节）。拖动小球设定初始角；右边是相图（\(\theta\) 对 \(\dot\theta\)），虚线是分界线（刚好能摆到最高点的轨道）；底部的能量条显示 \(T\)、\(V\) 和 \(T+V\)。切换到"牛顿视角"，看看同一个摆用牛顿的办法要多画什么。

两根均匀杆：\(m_1=2\) kg，\(l_1=1\) m；\(m_2=1.5\) kg，\(l_2=0.8\) m；质心在杆中点，\(l_{c}=l/2\)，绕质心的转动惯量 \(I=ml^2/12\)。角度 \(\theta_1\) 从水平的 \(+x\) 轴量起，\(\theta_2\) 是相对第一根杆的角度，重力沿 \(-y\)。两个关节力矩 \(\tau_1,\tau_2\)。把它们收成向量：\(\mathbf q=(\theta_1,\theta_2)\)，\(\boldsymbol\tau=(\tau_1,\tau_2)\)。同样的做法：写 \(T\) 与 \(V\)，对 \(\theta_1,\theta_2\) 各做一次欧拉–拉格朗日，结果整理成

颜色表示每一项的来源：青色来自动能（质量矩阵 \(M\) 和科氏/离心项 \(C\dot{\mathbf q}\)），琥珀色来自势能（重力 \(\mathbf g\)），紫色是外部输入（力矩）。把鼠标悬停在公式的各项上，图里会高亮对应的物理量。下图是完整的仿真器：给两个力矩，开关重力，选积分器和步长，右图是能量守恒的检验。

小车（质量 \(M_c\)）沿水平导轨滑动，摆杆（长 \(l\)，端点质量 \(m\)，角度 \(\theta\) 从竖直向上量起）铰接在车上。两个自由度 \((x,\theta)\)，但只有一个驱动：只能推小车，没有电机直接转动摆杆。这叫欠驱动（7.1 节的自由度 vs. 驱动数）。拉格朗日框架处理它毫无困难，只是右端的广义力 \(\boldsymbol\tau=(F,0)\)，第二项为零。下图把"能不能同时指定两个加速度"画了出来：右边平面的横轴是小车加速度 \(\ddot x\)，纵轴是摆杆角加速度 \(\ddot\theta\)，当前状态下，无论推多大的力 \(F\)，能到达的 \((\ddot x,\ddot\theta)\) 只落在一条直线上。

注意：不是所有加速度都能指定，但不代表系统"不可控"。平衡一根倒立摆需要"让车跑到摆杆下面去"，而想把小车拉回中间，必须先把摆杆往中间倾，也就是先把车推离开中间（试试自动平衡：LQR 对小车位置 \(x\) 的反馈增益符号看上去是"反的"）。欠驱动系统的控制要利用 \(M\) 的结构，不能直接"选加速度再求力矩"。步行机器人的浮动基座正是这样的欠驱动系统（9.2 节）。

取 \(n\) 个独立变量 \(\mathbf q=(q_1,\dots,q_n)\) 描述整个机器人的构型（7.1 节的自由度个数，7.2 节的构型空间），这就是广义坐标。关节角、滑动关节的位移都可以做广义坐标。用了它们，刚性连接和铰链带来的约束就已经"吃掉"了：不再有约束力要处理。

先看一块刚体。设质心速度为 \(v_c\)，角速度为 \(\omega\)，绕质心的转动惯量为 \(I\)（平面情形，三维时是惯量张量，\(\tfrac12\omega^\top I_c\,\omega\)）。把刚体分成质点 \(dm\)，每个质点的速度是 \(v=v_c+\omega\times r\)（\(r\) 是相对质心的位置）：

中间项为零，因为 \(r\) 是从质心量起的（\(\int r\,dm=0\)）。这就是"平动动能 + 转动动能"，也是常见的第一个坑：忘记转动惯量项。每个连杆的 \(v_c\) 和 \(\omega\) 都是 \(\dot{\mathbf q}\) 的线性函数，系数正是 3.4 节的雅可比（质心的雅可比，也是"杠杆臂转 90°"）：\(v_{ci}=J_{vi}(\mathbf q)\dot{\mathbf q}\)，\(\omega_i=J_{\omega i}(\mathbf q)\dot{\mathbf q}\)。把所有连杆加起来：

\(T\) 是 \(\dot{\mathbf q}\) 的二次型（3.5 节），\(M=\partial^2T/\partial\dot{\mathbf q}^2\) 正是它的 Hessian，称为质量矩阵（惯量矩阵）。因为对任何 \(\dot{\mathbf q}\ne0\) 都有 \(T>0\)，\(M\) 是对称正定的，永远可逆，这是"给力矩就能解出加速度"的根本原因。\(V\) 只依赖 \(q\)，\(h_i\) 是质心的高度。

设 \(\mathcal L=T-V\)，真实运动使 \(S=\int\mathcal L\,dt\) 驻定。取一个满足 \(\delta\mathbf q(0)=\delta\mathbf q(T)=0\) 的变分 \(\delta\mathbf q(t)\)（FIG 1 中的扰动 \(\eta\)），一阶变化为

对任意 \(\delta\mathbf q\) 都为零，被积函数必须恒为零。若还有不来自势能的力（电机力矩、摩擦、手爪受的外力），用虚功原理：它们在虚位移 \(\delta\mathbf q\) 上做的功是 \(Q^\top\delta\mathbf q\)，其中 \(Q_i=\sum_k F_k\cdot\partial r_k/\partial q_i\) 称为广义力。关节力矩直接就是 \(Q_i=\tau_i\)；手爪上的力 \(F\) 的广义力就是 \(J^\top F\)，正是 3.4 节的力–力矩对偶。于是

把 \(T=\tfrac12\dot{\mathbf q}^\top M(\mathbf q)\dot{\mathbf q}\) 代入。每一项都是一步机械的求导：

注意第二项的 \(\dot M\dot{\mathbf q}\)：因为 \(M\) 依赖 \(\mathbf q\)，对时间求导时必须多出这一项（常见错误之一）。它和 \(\tfrac12\dot{\mathbf q}^\top\partial_{q_i}M\,\dot{\mathbf q}\) 合并，得到 \(C(\mathbf q,\dot{\mathbf q})\dot{\mathbf q}\)；分量形式是 Christoffel 符号：

第二根杆的质心速度为 \(v_{c2}=l_1\dot\theta_1(-s_1,c_1)+l_{c2}(\dot\theta_1+\dot\theta_2)(-s_{12},c_{12})\)，两项的方向夹角正是 \(\theta_2\)，所以 \(\|v_{c2}\|^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)\)。加上两根杆的转动动能 \(\tfrac12I_1\dot\theta_1^2+\tfrac12I_2(\dot\theta_1+\dot\theta_2)^2\)，对 \(\dot\theta\) 求 Hessian 得到

这些公式已经用暴力数值微分的欧拉–拉格朗日做过核对（随机姿态、速度、力矩，加速度吻合到 \(10^{-5}\) 量级，即有限差分的精度）。FIG 3 的读数里用的就是它们。几点观察：\(M_{12}\ne0\) 且依赖 \(\theta_2\)，所以两关节的惯性耦合；\(g_1\) 里的 \(\cos\) 来自"\(\theta\) 从水平量起"，水平伸直时重力力臂最大；\(C\) 的所有元素都含 \(h\propto\sin\theta_2\)，臂完全伸直或折回（\(\theta_2=0,\pi\)）时科氏力消失。

1. \(M\) 对称正定，所以正动力学 \(\ddot{\mathbf q}=M^{-1}(\boldsymbol\tau-C\dot{\mathbf q}-\mathbf g-b\dot{\mathbf q})\) 总有唯一解。
2. \(\dot M-2C\) 是反对称的（\(C\) 取 Christoffel 形式时）。于是 \(\dot{\mathbf q}^\top(\dot M-2C)\dot{\mathbf q}=0\)，对 \(T\) 求导得 \(\dot T=\dot{\mathbf q}^\top(\boldsymbol\tau-\mathbf g-b\dot{\mathbf q})\)；再加上 \(\dot V=\dot{\mathbf q}^\top\mathbf g\)，总能量 \(E=T+V\) 满足 \(\dot E=\dot{\mathbf q}^\top\boldsymbol\tau-b\|\dot{\mathbf q}\|^2\)：能量的变化等于外力做的功。这是 FIG 3 右图的账本，也是基于无源性的控制证明稳定性的出发点。我们数值验证了 \(\dot M-2C\) 的反对称（残差 \(\sim10^{-10}\)）。注意只有 \(C\dot{\mathbf q}\) 这个乘积是唯一的，\(C\) 本身不唯一，反对称性依赖于 Christoffel 的选择。
3. 对动力学参数线性：存在回归矩阵 \(Y(\mathbf q,\dot{\mathbf q},\ddot{\mathbf q})\)，使 \(\boldsymbol\tau=Y\,\pi\)，\(\pi\) 是质量、一阶矩、惯量等组成的参数向量。这是系统辨识的基础：用足够多的测量点做最小二乘（1.13 节）。

正动力学"给力矩求加速度"，用于仿真（再积分，2.5 节）；逆动力学"给想要的加速度求力矩"，用于控制。FIG 3 的 \(\boldsymbol\tau=\mathbf g(\mathbf q)\) 保持按钮，就是 \(\ddot{\mathbf q}_d=0,\dot{\mathbf q}=0\) 的逆动力学。对 \(n\ge3\) 的机械臂，手推符号式的拉格朗日表达式爆炸式地变长（每多一个自由度，项数成倍增加），实用的算法是 8.2 节的牛顿–欧拉递推（逆动力学，\(O(n)\)）和 8.3 节的质量矩阵算法；数值上 \(M\) 常用复合刚体算法（CRBA）直接构造。

广义坐标 \(\mathbf q=(x,\theta)\)，\(\theta\) 从竖直向上量起。摆杆端点的位置是 \((x+l\sin\theta,\ l\cos\theta)\)。

写成矩阵形式 \(M(\mathbf q)\ddot{\mathbf q}+C\dot{\mathbf q}+\mathbf g=B\,u\)，输入矩阵 \(B=(1,0)^\top\)。若 \(B\) 是 \(n\times n\) 可逆矩阵，系统是全驱动，任何 \(\ddot{\mathbf q}\) 都能通过逆动力学实现（FIG 3 的 2R 机械臂）；这里 \(B\) 的秩是 1，小于 \(n=2\)，所以欠驱动。第二个方程里没有 \(F\)，它就是 FIG 4 右图直线的来源。

哈密顿形式：令广义动量 \(p=\partial\mathcal L/\partial\dot{\mathbf q}=M\dot{\mathbf q}\)，哈密顿量 \(H=\tfrac12p^\top M^{-1}p+V(\mathbf q)\)（能量），运动方程变成一阶的 \(\dot{\mathbf q}=\partial H/\partial p\)，\(\dot p=-\partial H/\partial \mathbf q+Q\)。它是辛积分器、端口哈密顿控制的出发点，但对机器人入门，二阶形式更直接。

约束：闭链机构（并联机器人、四连杆）和接触（脚踩地面）带有约束 \(\phi(\mathbf q)=0\)。若保留冗余坐标，约束力又回来了：\(M\ddot{\mathbf q}+C\dot{\mathbf q}+\mathbf g=\boldsymbol\tau+A(\mathbf q)^\top\lambda\)，\(A=\partial\phi/\partial\mathbf q\)，拉格朗日乘子 \(\lambda\) 就是约束力，由 \(A\ddot{\mathbf q}+\dot A\dot{\mathbf q}=0\) 一并解出。8.4 节的接触就是这个套路，并加上"不等式"（只能推，不能拉）。所以"约束力从不出现"精确地说是：选了满足约束的独立坐标后。

让 2R 机械臂跟踪一条关节轨迹 \(\mathbf q_d(t)\)。最朴素的是对每个关节做 PD：\(\boldsymbol\tau=K_p e+K_d\dot e\)（\(e=\mathbf q_d-\mathbf q\)），可手臂自重要吃掉一部分力矩，快速运动时惯性耦合和科氏力又会引起误差。用动力学模型可以逐级改进：

• PD + 重力补偿：\(\boldsymbol\tau=K_pe+K_d\dot e+\hat{\mathbf g}(\mathbf q)\)，抵消静态的重力。
• 计算力矩（又叫反馈线性化）：\(\boldsymbol\tau=\hat M(\mathbf q)\big(\ddot{\mathbf q}_d+K_d\dot e+K_pe\big)+\hat C(\mathbf q,\dot{\mathbf q})\dot{\mathbf q}+\hat{\mathbf g}(\mathbf q)\)。模型完美时，把它代入 \(M\ddot{\mathbf q}+C\dot{\mathbf q}+\mathbf g=\boldsymbol\tau\)，得到 \(\ddot e+K_d\dot e+K_pe=0\)：误差满足一条与姿态无关的线性方程，非线性被逆动力学抵消了。

下面三条控制器同时跟踪同一条轨迹（紫色虚线是目标）。控制律以 500 Hz 运行（零阶保持）。"模型误差"让控制器使用的质量与真实值差 \(\pm\) 若干个百分点，"未建模负载"是手爪抓了一个控制器不知道的物体（质量加在末端）。