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

动力学，说白了就是研究「力怎么让机器人动起来」的学问。做轨迹规划的时候，如果你只懂运动学，那就像只会看地图却不会开车——你知道该往哪走，但不知道踩多大油门、打多大方向盘。动力学就是帮你算清楚这些「力」和「力矩」的。

我个人习惯把动力学分成两派：一派是牛顿-欧拉，另一派是拉格朗日。前者像搭积木，一层层递推；后者像写诗，能量守恒一把抓。咱们今天都过一遍，再聊聊关节空间和任务空间里的动力学表达。

3.1 牛顿-欧拉方程：从单刚体到多连杆

先看最简单的。一个刚体在空间里运动，牛顿告诉我们：

F = m * a      （平动）
τ = I * α + ω × (I * ω)   （转动）

嗯，这里要注意——转动那项里的 ω × (I * ω) 是科里奥利力项，很多人刚学时容易漏掉。我在项目中遇到过一位同事，调试六轴机械臂时力矩总是不对，查了两天才发现是忽略了这一项。你想想看，高速运动时这个耦合项能占到总力矩的 30% 以上，漏掉它，轨迹跟踪肯定炸。

对于多连杆机器人，牛顿-欧拉法采用「向外递推 + 向内递推」的两步策略：

1. 向外递推：从基座到末端，计算每个连杆的速度、加速度
2. 向内递推：从末端到基座，计算每个关节的力和力矩

我曾经用这个算法给一个七轴协作臂做实时力矩补偿，在 1kHz 的控制周期里跑通了。说实话，牛顿-欧拉法的计算效率非常高，适合嵌入式实时系统。

向外递推：
ω_{i+1} = R_i^{i+1} * ω_i + θ̇_{i+1} * z_{i+1}
α_{i+1} = R_i^{i+1} * α_i + θ̈_{i+1} * z_{i+1} + ω_{i+1} × (θ̇_{i+1} * z_{i+1})
a_{i+1} = R_i^{i+1} * (a_i + α_i × r_i + ω_i × (ω_i × r_i))

向内递推：
f_i = R_{i+1}^i * f_{i+1} + m_i * a_{c,i}
τ_i = R_{i+1}^i * τ_{i+1} + I_i * α_i + ω_i × (I_i * ω_i) + r_i × f_i

3.2 拉格朗日动力学：能量视角

拉格朗日法走的是另一条路。它不关心具体的力是怎么传递的，只盯着系统的能量：

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

其中 T 是动能，V 是势能。你想想看，只要你能写出整个系统的能量表达式，剩下的就是纯数学推导了。我个人觉得，对于自由度不太多（比如少于 6 个）的系统，拉格朗日法写起来更直观，不容易出错。

但要注意——拉格朗日法推导出来的方程通常长这样：

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

M 是惯性矩阵，C 是科里奥利和离心力矩阵，G 是重力项。这个形式在控制理论里非常有用，因为你可以直接基于它设计控制器。

3.3 关节空间 vs 任务空间动力学

好，现在咱们聊聊这两个空间的区别。关节空间的动力学方程就是上面那个：

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

所有变量都在关节角度、角速度、角加速度里表达。这很自然，因为机器人本来就是由关节驱动的。

但问题来了——很多时候我们关心的是末端执行器在笛卡尔空间里的运动。比如你让机械臂去抓一个杯子，你更关心的是末端的位置和姿态，而不是每个关节转了多少度。

任务空间动力学就是把方程映射到笛卡尔空间：

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

其中 Λ 是任务空间惯性矩阵，μ 是科里奥利项，p 是重力项，F 是末端力。这个映射关系用到了雅可比矩阵 J：

Λ = (J * M^{-1} * J^T)^{-1}
μ = J^{-T} * C * q̇ - Λ * J̇ * q̇
p = J^{-T} * G

3.4 动力学知识体系总览

下面这张图把本章的核心逻辑串起来了，你可以对照着看：

3.5 两种方法的对比与选型

我把牛顿-欧拉和拉格朗日法的特点整理成了表格，方便你对比：

3.6 关节空间与任务空间的转换实战

最后，咱们看一个实际例子。假设你有一个六轴机械臂，需要在笛卡尔空间做阻抗控制。任务空间的动力学方程是：

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

其中 F_ctrl 是你要设计的控制力。阻抗控制的目标是让末端表现出「质量-弹簧-阻尼」特性：

Λ_d * (ẍ - ẍ_d) + B_d * (ẋ - ẋ_d) + K_d * (x - x_d) = F_ext

把这两个方程联立，就能解出需要的关节力矩：

τ = J^T * [Λ * ẍ_d + μ + p - Λ * Λ_d^{-1} * (B_d * (ẋ - ẋ_d) + K_d * (x - x_d) - F_ext)]

嗯，这个式子看起来有点复杂，但逻辑很清楚——先把任务空间的控制力算出来，再用雅可比转置映射回关节空间。我在做手术机器人力控时就用过这个框架，效果不错。