拉格朗日法：让能量组织机器人的动力学

上一篇谈旋量与 PoE 时，我们关心的是：关节运动怎样沿着螺旋轴组合，最终变成末端位姿？

现在把问题向前推进一步。即使运动学已经完全清楚，我们仍然不知道：

想让机械臂产生某个加速度，需要多大的关节力矩？
连杆之间为什么会相互牵动？
重力、惯性和关节速度，怎样共同出现在控制方程里？

这一次，我们不再追踪“每根杆受了哪些力”，而是从整个系统拥有多少动能与势能出发。

这就是拉格朗日法（Lagrangian method）：用少数几个广义坐标描述系统的构型，用一个标量函数汇总能量，再通过欧拉—拉格朗日方程得到运动规律。

先把全文最重要的三个符号说明白：

$T(q,\dot q)$ ：系统的总动能；
$V(q)$ ：系统的总势能；
$\mathcal L(q,\dot q)=T-V$ ：拉格朗日函数。

对第 $i$ 个广义坐标 $q_i$ ，动力学由下面的方程给出：

\frac{d}{dt}\left(\frac{\partial\mathcal L}{\partial\dot q_i}\right) -\frac{\partial\mathcal L}{\partial q_i}=Q_i

$Q_i$ 是与 $q_i$ 对应的广义力：若 $q_i$ 是转角，它通常是力矩；若 $q_i$ 是位移，它通常是力。它们之间最干净的联系是瞬时功率：

\mathcal P=Q^{\scriptscriptstyle\mathsf T}\dot q

牛顿—欧拉法沿着连杆传递力与力矩；拉格朗日法先把整台机器人的能量写成一个标量，再让广义坐标把运动方程“展开”出来。

两种方法描述的是同一套物理规律，不存在精度上的高下。它们真正不同的，是组织问题的视角。

从逐杆受力到整体能量的两种动力学视角

1. 为什么要换一种动力学语言

假设我们要分析一台两连杆机械臂。用牛顿—欧拉法，可以分别画出两根连杆的受力图：关节力、关节力矩、重力、惯性力，然后从末端向基座递推。

这种做法非常直接，也很适合高效计算。困难在于，随着连杆增加，我们会同时遇到许多并不是最终目标的量：关节之间的相互作用力、每根杆上的约束力，以及它们在不同坐标系中的表达。

拉格朗日法选择先换一个问题：

不追问每一处内部作用力究竟是多少，而是问系统的能量怎样随广义坐标变化。

对理想刚性关节而言，内部约束力在允许的虚位移上不做功，因此不会出现在最小坐标形式的方程里。这不是说那些力“不存在”，而是说在求关节运动时，我们可以暂时不计算它们。

如果后面确实需要闭环约束力、接触力或轴承载荷，仍然可以通过拉格朗日乘子等方法把它们找回来。拉格朗日法消除的是不必要的中间记账，而不是物理世界里的力。

1.1 从虚功到分析力学

18 世纪，达朗贝尔把动力学问题改写成一种虚功平衡；1788 年，拉格朗日在《分析力学》中系统整理了这条道路。其重要意义不只是得到一条新公式，而是把关注点从笛卡尔坐标中的逐点受力，转向能够描述系统自由度的广义坐标。

例如：

单摆只需要一个角度 $q$ ，不必同时把摆球的 $x$ 、 $y$ 当成独立变量；
平面 2R 机械臂只需要两个关节角 $q_1,q_2$ ；
沿直线运动的滑块可以直接用位移 $s$ ；
空间刚体也可以用一组适合其构型的坐标描述。

约束被吸收到坐标的选择里以后，动力学方程只围绕真正独立的自由度展开。这正是拉格朗日法看起来“少算了许多力”，却仍然得到完整运动规律的原因。

2. 广义坐标：先决定系统怎样被描述

设机器人有 $n$ 个自由度，我们选择：

q=\begin{bmatrix}q_1&q_2&\cdots&q_n\end{bmatrix}^{\scriptscriptstyle\mathsf T}

作为广义坐标， $\dot q$ 与 $\ddot q$ 分别是广义速度和广义加速度。

“广义”并不意味着抽象到不可理解。它只是说坐标不必都用米表示：转角、滑块位移，甚至某些柔性模态的幅值，都可以成为坐标。唯一重要的要求是，它们能够在目标构型附近完整而不过度重复地描述系统。

选择好 $q$ 后，动力学的入口其实仍然是运动学。对第 $i$ 根连杆，我们需要知道：

质心位置 $p_{c_i}(q)$ ；
质心线速度 $v_{c_i}(q,\dot q)$ ；
连杆角速度 $\omega_i(q,\dot q)$ 。

它们通常可以写成雅可比与关节速度的乘积：

v_{c_i}=J_{v_i}(q)\dot q, \qquad \omega_i=J_{\omega_i}(q)\dot q

这也说明了本系列前两篇的关系：PoE 与雅可比负责说清“怎样运动”，拉格朗日法则利用这些运动学量计算“运动需要怎样的力”。

3. 把整台机器人的能量写出来

3.1 动能：平动与转动缺一不可

第 $i$ 根刚性连杆的动能由两部分组成：质心的平移动能，以及绕质心的转动动能。

若角速度与惯性张量都在世界坐标系表达，则：

T_i= \frac12m_i v_{c_i}^{\scriptscriptstyle\mathsf T}v_{c_i} +\frac12\omega_i^{\scriptscriptstyle\mathsf T}\,{}^0I_i\,\omega_i

其中：

$m_i$ 是连杆质量；
${}^0I_i=R_i I_i^cR_i^{\scriptscriptstyle\mathsf T}$ ；
$I_i^c$ 是质心固连坐标系中的惯性张量；
$R_i$ 把该固连坐标系旋转到世界坐标系。

这里有一个很常见的错误：角速度在世界坐标系表达，却直接乘上固连坐标系中的 $I_i^c$ 。惯性张量和角速度必须处在同一坐标系中。另一种同样正确的做法，是把角速度变换到连杆固连坐标系，再与 $I_i^c$ 相乘。

把所有连杆相加：

T=\sum_{i=1}^{N}T_i

由于 $v_{c_i}$ 和 $\omega_i$ 都对 $\dot q$ 线性，动能最终一定能整理成：

T(q,\dot q)=\frac12\dot q^{\scriptscriptstyle\mathsf T}M(q)\dot q

$M(q)$ 是机器人的质量矩阵，也常称惯性矩阵。它并不是简单地把每根连杆的质量排成对角线，而是在当前构型下，把关节加速度映射为广义惯性力。

3.2 势能：只与位置有关的储能

如果世界坐标系的竖直坐标为 $y$ ，向上为正，则重力势能可以写成：

V_g(q)=\sum_{i=1}^{N}m_i g\,y_{c_i}(q)

若系统还有关节弹簧，也应把弹性势能加入：

V_s(q)=\frac12(q-q_0)^{\scriptscriptstyle\mathsf T}K(q-q_0)

因此总势能是 $V=V_g+V_s+\cdots$ 。

有些资料把重力加速度写成向量，再使用 $-m_i\boldsymbol g^{\scriptscriptstyle\mathsf T}p_{c_i}$ 。两种写法都可以；关键是先说明坐标轴方向和 $\boldsymbol g$ 的符号，并在全文保持一致。

3.3 拉格朗日函数不是总能量

现在构造：

\mathcal L(q,\dot q)=T(q,\dot q)-V(q)

注意， $\mathcal L$ 是“动能减势能”，而机械能是：

E=T+V

这两个量很容易在第一次接触时混淆。拉格朗日函数是生成运动方程的对象；机械能则用于讨论储能与守恒。

4. 欧拉—拉格朗日方程到底做了什么

对每个 $q_i$ ，依次计算：

\frac{\partial\mathcal L}{\partial\dot q_i}, \qquad \frac{d}{dt}\left(\frac{\partial\mathcal L}{\partial\dot q_i}\right), \qquad \frac{\partial\mathcal L}{\partial q_i}

再代入：

\frac{d}{dt}\left(\frac{\partial\mathcal L}{\partial\dot q_i}\right) -\frac{\partial\mathcal L}{\partial q_i}=Q_i

第一项可以理解为广义动量的变化率，第二项描述能量随构型变化形成的广义力。二者相减后，得到外界为了实现该运动必须提供的广义力。

对于保守系统，内部几何和能量会自动组织出机器人的标准动力学形式：

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

其中：

$M(q)\ddot q$ ：加速各连杆所需的惯性力；
$c(q,\dot q)$ ：由关节速度乘积产生的科里奥利与离心项；
$g(q)=\partial V/\partial q$ ：重力或其他保守势场形成的广义力；
$\tau$ ：驱动器施加的关节力或力矩。

工程中也常把速度项写成矩阵乘积：

c(q,\dot q)=C(q,\dot q)\dot q

于是得到熟悉的形式：

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

但要注意：** $c(q,\dot q)$ 是确定的，而满足 $C\dot q=c$ 的矩阵 $C$ 并不唯一。**因此比较两个推导结果时，不能只看 $C$ 的每个元素是否相同，还要检查它们乘上 $\dot q$ 后是否得到同一个速度项。

5. 从单摆开始：让公式重新变得直观

先看一根绕固定轴摆动的刚性杆。设：

$q$ 从竖直向下方向逆时针测量；
质心到转轴的距离为 $c$ ；
质量为 $m$ ；
绕质心、垂直运动平面的转动惯量为 $I_c$ ；
关节驱动力矩为 $\tau$ 。

单摆中的动能与势能

质心速度大小为 $c\dot q$ 。根据平行轴定理，系统绕关节的等效转动惯量为 $I_c+mc^2$ ，所以：

T=\frac12(I_c+mc^2)\dot q^2

把最低点选为势能零点，质心抬升的高度是 $c(1-\cos q)$ ：

V=mgc(1-\cos q)

拉格朗日函数为：

\mathcal L= \frac12(I_c+mc^2)\dot q^2-mgc(1-\cos q)

代入欧拉—拉格朗日方程：

(I_c+mc^2)\ddot q+mgc\sin q=\tau

这条方程几乎可以直接读出来：

第一项是“等效转动惯量 × 角加速度”；
第二项是重力试图把摆拉回最低点的力矩；
右侧是电机施加的力矩。

如果没有驱动力矩和耗散，令 $\tau=0$ ，则机械能保持不变：

E=\frac12(I_c+mc^2)\dot q^2+mgc(1-\cos q)

拉格朗日法的魅力在这个小例子里已经出现：我们没有画转轴反力，也没有求它在水平和竖直方向的分量，却得到了完整的角运动方程。

6. 串联实例：平面 2R 机械臂

现在让单摆多一个关节。考虑一台在竖直平面内运动的 2R 机械臂：

$q_1$ 是连杆 1 相对水平 $x$ 轴的角度；
$q_2$ 是连杆 2 相对连杆 1 的角度；
$l_1$ 是第一根连杆长度；
$c_1,c_2$ 是两根连杆各自质心到近端关节的距离；
$m_1,m_2$ 是质量， $I_1,I_2$ 是各自绕质心的平面转动惯量；
$y$ 轴竖直向上，重力加速度大小为 $g$ 。

平面 2R 机械臂的质心、速度与能量耦合

6.1 先写质心运动学

两个质心的位置为：

p_{c_1}= \begin{bmatrix} c_1\cos q_1\\ c_1\sin q_1 \end{bmatrix}

p_{c_2}= \begin{bmatrix} l_1\cos q_1+c_2\cos(q_1+q_2)\\ l_1\sin q_1+c_2\sin(q_1+q_2) \end{bmatrix}

两根连杆的角速度分别是：

\omega_1=\dot q_1, \qquad \omega_2=\dot q_1+\dot q_2

到这里还没有任何动力学，只是在回答“质心怎样运动”。接下来把这些运动学量放进动能和势能，拉格朗日方程就会替我们整理剩余部分。

6.2 动能让质量矩阵自然出现

总动能是：

T=\frac12m_1\dot p_{c_1}^{\scriptscriptstyle\mathsf T}\dot p_{c_1} +\frac12I_1\dot q_1^2 +\frac12m_2\dot p_{c_2}^{\scriptscriptstyle\mathsf T}\dot p_{c_2} +\frac12I_2(\dot q_1+\dot q_2)^2

将它整理为 $T=\frac12\dot q^{\scriptscriptstyle\mathsf T}M(q)\dot q$ ，得到：

M(q)= \begin{bmatrix} M_{11}&M_{12}\\ M_{12}&M_{22} \end{bmatrix}

其中：

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

为什么 $M_{12}$ 不为零？因为关节 1 转动时会带着整根连杆 2 一起运动；关节 2 转动时，连杆 2 的速度又叠加在关节 1 已经产生的速度上。两个关节对动能的贡献不能彼此分开，于是质量矩阵出现了非对角耦合项。

更有意思的是， $M$ 只依赖 $q_2$ ，而不依赖 $q_1$ 。原因不是公式偶然消掉了 $q_1$ ，而是整个机械臂绕基座刚体旋转时，质量之间的相对几何关系并没有改变；真正改变惯性耦合的，是两根连杆之间的夹角。

6.3 势能给出重力项

由于 $y$ 轴向上：

V= m_1gc_1\sin q_1 +m_2g\left[l_1\sin q_1+c_2\sin(q_1+q_2)\right]

对 $q$ 求梯度：

g(q)= \begin{bmatrix} (m_1c_1+m_2l_1)g\cos q_1+m_2gc_2\cos(q_1+q_2)\\ m_2gc_2\cos(q_1+q_2) \end{bmatrix}

这正是静止保持当前姿态时，电机需要抵消的重力力矩。

6.4 速度项揭示关节之间的动态牵连

令：

b=m_2l_1c_2\sin q_2

科里奥利与离心项可以写成：

c(q,\dot q)= \begin{bmatrix} -b\left(2\dot q_1\dot q_2+\dot q_2^2\right)\\ b\dot q_1^2 \end{bmatrix}

因此完整动力学是：

M(q)\ddot q+c(q,\dot q)+g(q)= \begin{bmatrix}\tau_1\\\tau_2\end{bmatrix}

如果一定要写成 $C\dot q$ ，下面是一种常用选择：

C(q,\dot q)= \begin{bmatrix} -b\dot q_2&-b(\dot q_1+\dot q_2)\\ b\dot q_1&0 \end{bmatrix}

直接相乘即可验证 $C\dot q=c$ 。换一本教材时， $C$ 的元素可能长得不同，只要乘积相同，并且所选形式满足需要的结构性质，就不构成矛盾。

6.5 拉格朗日法真正省掉了什么

回头看这段推导，我们只做了三件事：

写出两个质心的位置和两根连杆的角速度；
把所有平动、转动动能与重力势能相加；
让欧拉—拉格朗日方程整理结果。

我们没有求关节 2 对连杆 1 的作用力，也没有再利用作用力与反作用力把它消掉。内部力被广义坐标与能量表达自动略过了。

但“推导结构清楚”并不等于“手算永远轻松”。即使只有两个关节，展开式也已经相当长。自由度继续增加时，通常应让符号工具、自动微分或成熟的刚体动力学库完成机械求导，而不是把手工展开当成拉格朗日法的必要组成部分。

7. 并联实例：闭环约束并没有消失

上一篇的五杆机构有两个主动关节和两个被动关节，两条支链共同连接到末端点 $P$ 。在运动学里，我们用闭环条件保证两条路径到达同一点；到了动力学，这条约束仍然存在。

五杆机构的闭环动力学与约束力

一种直接而通用的做法，是暂时把机构沿某个关节“切开”，将它视为一棵没有闭环的运动树。设包含主动和被动关节的坐标为 $q$ ，然后写出未闭合系统的动能与势能。

闭环几何用约束方程表示：

\phi(q)=0

其约束雅可比为：

J_c(q)=\frac{\partial\phi}{\partial q}

约束在速度和加速度层分别要求：

J_c\dot q=0

J_c\ddot q+\dot J_c\dot q=0

现在把维持闭环所需的力用拉格朗日乘子 $\lambda$ 表示：

M(q)\ddot q+c(q,\dot q)+g(q) =B\tau+J_c^{\scriptscriptstyle\mathsf T}(q)\lambda

其中 $B$ 把电机输入映射到完整的广义坐标， $J_c^{\scriptscriptstyle\mathsf T}\lambda$ 是保证两条支链始终闭合的广义约束力。

这里恰好能看清一句常被说得过头的话：

拉格朗日法不是“永远不计算约束力”，而是在最小坐标足够时不必计算内部约束力；当闭环反力本身重要时，乘子 $\lambda$ 会把它显式带回来。

对于五杆机构，也可以只选两个独立坐标，把其余关节通过闭环几何消去。这样方程维度更低，但需要处理装配分支、奇异构型和局部坐标失效。冗余坐标加约束乘子的形式维度更高，却往往更容易保持模型对称，也更便于直接求取支链约束力。

这就是拉格朗日法在并联机构中的魅力：每根连杆仍然只需要贡献自己的动能与势能，闭环的特殊性则被集中放进 $\phi(q)$ 和 $\lambda$ ，而不是散落在每一根杆的受力平衡里。

8. 标准动力学方程里藏着哪些结构

得到 $M$ 、 $c$ 和 $g$ 之后，真正有价值的不只是一个可用于仿真的公式，还包括几条可以直接服务控制与校验的结构性质。

8.1 质量矩阵是对称正定的

在独立且非退化的广义坐标下：

M(q)=M^{\scriptscriptstyle\mathsf T}(q), \qquad x^{\scriptscriptstyle\mathsf T}M(q)x>0\quad(x\ne0)

对称性来自动能二次型，正定性则意味着任何非零广义速度都会产生正的动能。若计算出的质量矩阵明显不对称，或在正常构型下出现负特征值，通常说明惯性参数、坐标变换或推导过程出了问题。

8.2 适当选择的 $C$ 具有斜对称结构

虽然 $C$ 不唯一，但使用 Christoffel 符号构造的一类标准 $C$ 满足：

\dot M-2C

是斜对称矩阵，因此对任意 $x$ ：

x^{\scriptscriptstyle\mathsf T}(\dot M-2C)x=0

这条性质会让能量推导中的速度二次项恰好抵消，是许多无源性证明、计算力矩控制和自适应控制分析的基础。

8.3 动力学方程自带能量收支

对没有摩擦、没有时变约束的保守机器人：

E=T+V

沿系统轨迹求导可得：

\dot E=\dot q^{\scriptscriptstyle\mathsf T}\tau

也就是说，执行器输入的机械功率等于系统机械能的变化率。若加入黏性阻尼 $D\dot q$ ：

M\ddot q+c+g+D\dot q=\tau

则：

\dot E=\dot q^{\scriptscriptstyle\mathsf T}\tau-\dot q^{\scriptscriptstyle\mathsf T}D\dot q

第二项永远不会增加能量，它正是耗散。拉格朗日法之所以特别适合能量整形、无源控制和稳定性分析，并不只是因为公式优美，而是因为能量收支从建模开始就被保留在结构中。

9. 拉格朗日法与牛顿—欧拉法：不是二选一

维度	拉格朗日法	牛顿—欧拉法
出发点	系统的动能、势能与广义坐标	每个刚体的力、力矩与动量变化
内部约束力	最小坐标下通常不必显式求解	在逐杆受力递推中自然出现
闭式表达	便于整理 $M,c,g$ 和分析结构	也能得到相同动力学，但通常以递推形式计算
计算实现	符号推导、自动微分与建模工具很自然	递归牛顿—欧拉逆动力学可做到线性复杂度
适合观察	能量、耦合、无源性、控制结构	载荷传递、关节反力、高效逆动力学
物理结果	与牛顿—欧拉法等价	与拉格朗日法等价