3、动力学基础：牛顿-欧拉方程、拉格朗日法、关节空间与任务空间动力学模型

动力学，说白了就是研究「力与运动的关系」。你给机器人关节施加力矩，它怎么动？反过来，你想让机器人末端走一条轨迹，关节需要出多大的力？这就是动力学要回答的问题。

我刚开始做人形机器人控制时，总觉得动力学是理论家的事。直到有一次，我写了一个纯运动学的轨迹跟踪程序，仿真跑得挺好，一上真机就抖得像筛糠。嗯，从那以后，我再也不敢轻视动力学了。

3.1 牛顿-欧拉法：递推的暴力美学

牛顿-欧拉法，我个人觉得是最符合直觉的动力学建模方法。它分两步走：先正向运动学递推，算出每个连杆的速度和加速度；再反向动力学递推，从末端往基座算力和力矩。

你想想看，人形机器人的腿在摆动时，髋关节的力不仅要克服自身惯性，还要承受来自小腿的「反作用力」。牛顿-欧拉法就是一层层把这些力算清楚。

伪代码长这样：

// 正向递推：从基座到末端
for i = 1 to n:
    ω_i     = ω_{i-1} + q̇_i * z_{i-1}
    ω̇_i     = ω̇_{i-1} + q̈_i * z_{i-1} + ω_{i-1} × (q̇_i * z_{i-1})
    v̇_i     = v̇_{i-1} + ω̇_i × r_{i-1,i} + ω_i × (ω_i × r_{i-1,i})
    v̇_c_i   = v̇_i + ω̇_i × r_{i,ci} + ω_i × (ω_i × r_{i,ci})
    F_i      = m_i * v̇_c_i
    N_i      = I_i * ω̇_i + ω_i × (I_i * ω_i)

// 反向递推：从末端到基座
for i = n down to 1:
    f_i      = F_i + f_{i+1}
    n_i      = N_i + n_{i+1} + r_{i,ci} × F_i + r_{i,i+1} × f_{i+1}
    τ_i      = n_i · z_{i-1}  // 关节力矩投影

我曾经在一个双足平衡项目里，用牛顿-欧拉法做实时力矩前馈。当时仿真里跑得挺好，但一上真机，右腿在摆动相末端总有个小抖动。查了半天，发现是反向递推时，我忘了加相邻连杆之间的「交叉耦合项」。嗯，这种细节，仿真里不明显，真机上一抓一个准。

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

拉格朗日法，说白了就是「能量守恒」的升级版。你不需要关心每个关节具体的约束力，只需要写出系统的动能和势能，然后对广义坐标求偏导就行。

公式很简单：

L = T - V
τ = d/dt(∂L/∂q̇) - ∂L/∂q

其中 T 是总动能，V 是总势能，L 是拉格朗日量。

我建议你在做人形机器人全身动力学分析时，先用拉格朗日法推导出闭式方程。为什么？因为你能直观地看到惯性矩阵 M(q)、科里奥利矩阵 C(q,q̇) 和重力项 G(q) 的物理构成。这对于后续的控制器设计——比如计算重力补偿、解耦惯性项——非常有帮助。

两种方法的对比，我整理了一个表格：

3.3 关节空间动力学模型

关节空间动力学，就是直接用关节角度 q 来描述系统。标准形式是：

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

其中：

• M(q)：惯性矩阵，描述关节间的惯性耦合
• C(q,q̇)：科里奥利和离心力矩阵
• G(q)：重力项
• τ：关节驱动力矩

我在做轨迹跟踪时，最常用的就是关节空间的「计算力矩法」。说白了，就是根据期望轨迹 q_des、q̇_des、q̈_des，反算出需要的力矩 τ，然后加上 PD 反馈修正。

3.4 任务空间动力学模型

任务空间动力学，也叫操作空间动力学。它描述的是机器人末端（比如手或脚）在笛卡尔空间中的力与加速度关系。

公式长这样：

Λ(x) * ẍ + μ(x, ẋ) + p(x) = F

其中：

• Λ(x)：任务空间惯性矩阵
• μ(x,ẋ)：任务空间科里奥利项
• p(x)：任务空间重力项
• F：末端施加的广义力（力和力矩）

你可能会问：为什么不用关节空间，非要用任务空间？

原因很简单——人形机器人的很多任务，比如「脚底保持水平」、「手端画圆」，都是在笛卡尔空间定义的。你在任务空间设计控制器，可以直接控制末端的阻抗特性，比在关节空间绕来绕去方便得多。

Λ(x) = (J * M⁻¹ * Jᵀ)⁻¹
F = J⁻ᵀ * τ

3.5 两种空间的选择策略

在实际项目中，我一般这样选：

• 关节空间：适合单关节轨迹跟踪、位置控制模式、低速运动
• 任务空间：适合末端力控、阻抗控制、全身协调运动、接触环境交互

但说实话，人形机器人最常用的其实是「混合控制」——关节空间做底层位置环，任务空间做上层力/阻抗环。两层之间通过动力学模型做前馈补偿。