4. 动力学基础（上）：牛顿-欧拉法、拉格朗日法、惯性张量与质量矩阵

动力学，说白了就是研究「力怎么让机器人动起来」。

你给关节电机发一个力矩指令，机器人到底会怎么加速？各个关节之间会怎么互相影响？这些问题的答案，全在动力学模型里。

我个人习惯把动力学分成两派：牛顿-欧拉法和拉格朗日法。前者像搭积木，一个关节一个关节地算；后者像写诗，从能量角度一把搞定。两种方法各有各的战场，咱们都得会。

4.1 牛顿-欧拉法：从关节到关节的递推

这个方法特别直观。你想想看，机器人是一串连杆连起来的，每个关节的运动都会影响后面的关节。牛顿-欧拉法就是沿着这个链条，一步一步算过去。

具体分两步走：

1. 前向递推：从基座开始，算每个连杆的角速度、角加速度、线加速度。
2. 反向递推：从末端开始，算每个关节受到的力和力矩，最后得到驱动力矩。

我曾经在一个六轴协作臂项目里，用牛顿-欧拉法做实时力矩前馈。当时控制器周期只有1ms，拉格朗日法算一次要0.8ms，根本跑不动。换成牛顿-欧拉法，优化后只要0.15ms，省出来的时间刚好够做阻抗控制。

4.2 拉格朗日法：从能量角度看问题

拉格朗日法不跟你纠结每个关节的受力细节。它只关心两件事：动能和势能。

公式长这样：

L = T - V

其中：
L = 拉格朗日量
T = 系统总动能
V = 系统总势能

然后对每个广义坐标 qᵢ 求导：
τᵢ = d/dt(∂L/∂q̇ᵢ) - ∂L/∂qᵢ

嗯，这里要注意。拉格朗日法虽然写起来漂亮，但手动推导一个六轴机器人的方程，能把你写到怀疑人生。我一般用它来做离线分析，或者验证牛顿-欧拉法的结果对不对。

两种方法算出来的结果应该完全一致。如果不一致，那肯定是你哪一步算错了。我在调试时经常两边对照着看，能快速定位问题。

4.3 惯性张量：一个物体的「旋转惯性」

说到惯性张量，很多初学者会懵。其实它就是转动惯量的三维版本。

一个刚体绕某个轴旋转时，它的惯性用转动惯量 I 描述。但机器人连杆在三维空间里转，方向是任意的，所以需要一个 3×3 的矩阵来描述：

I = | Ixx  Ixy  Ixz |
    | Iyx  Iyy  Iyz |
    | Izx  Izy  Izz |

对角线元素 Ixx、Iyy、Izz 是绕三个坐标轴的转动惯量。非对角线元素 Ixy 这些，叫惯性积，描述的是质量分布不对称带来的耦合效应。

惯性张量怎么算？最靠谱的办法是用三维CAD软件（SolidWorks、Fusion 360等）直接算。你建好连杆模型，软件自动给出相对于质心的惯性张量。然后你需要用平行轴定理，把它转换到关节坐标系下。

平行轴定理公式：

I_new = I_cm + m * (r² * E - r * rᵀ)

其中：
I_cm = 质心处的惯性张量
m = 连杆质量
r = 从质心到新原点的位移向量
E = 3×3 单位矩阵

4.4 质量矩阵 M(q)：多关节的「等效惯性」

质量矩阵 M(q) 是动力学方程里最重要的矩阵。它描述了：当你给某个关节一个加速度时，所有关节需要多大的力矩来配合。

动力学方程的标准形式：

τ = M(q) * q̈ + C(q, q̇) * q̇ + G(q)

其中：
τ = 关节力矩向量
M(q) = 质量矩阵（n×n，n为关节数）
C(q, q̇) = 科里奥利力和离心力矩阵
G(q) = 重力向量

M(q) 有几个重要性质：

性质	含义	实际影响
对称正定	M(q) = M(q)ᵀ，且所有特征值 > 0	保证控制算法稳定，可以求逆
与位形相关	M 是关节角度 q 的函数	不同姿态下，同样的力矩产生不同加速度
对角线元素	每个关节的「自惯性」	决定该关节的加速能力
非对角线元素	关节间的「耦合惯性」	一个关节加速，另一个关节也会感受到力矩

举个例子。人形机器人的髋关节和膝关节，质量矩阵的非对角线元素很大。你想想看，抬大腿的时候，小腿也会跟着晃。这就是耦合惯性在起作用。力矩控制如果不补偿这个耦合，你单独控髋关节，膝关节就会乱动。

4.5 两种方法的对比与选择

说了这么多，到底什么时候用哪个？我整理了一个对比表：

对比维度	牛顿-欧拉法	拉格朗日法
计算复杂度	O(n)，适合实时控制	O(n⁴)，适合离线分析
物理意义	清晰，每个力都能追踪	抽象，从能量角度出发
推导难度	中等，递推关系明确	高，多关节时极易出错
代码实现	容易，循环递推即可	较难，需要符号计算
适用场景	实时力矩控制、力控	动力学分析、仿真验证

我个人习惯是：实时控制用牛顿-欧拉法，离线分析用拉格朗日法。两个方法我都会实现一遍，互相验证。如果算出来的 M(q) 矩阵不一致，那肯定有地方搞错了。