第三章 动力学入门：牛顿-欧拉法、拉格朗日法、关节力矩计算
动力学，说白了就是研究「力与运动的关系」。
在轨迹跟踪里，我们不光要知道机器人该走什么路径，还得算出来——每个关节需要出多大的力，才能让机器人真的按这个路径动起来。你想想看，如果力矩算错了，要么电机堵转，要么机器人直接趴窝。
我个人习惯把动力学分成两派：一派是牛顿-欧拉法，另一派是拉格朗日法。今天咱们就把这两套东西掰开揉碎，再聊聊关节力矩到底怎么算。
3.1 牛顿-欧拉法：力与加速度的硬核关系
牛顿-欧拉法，本质上就是「力的平衡」加上「力矩的平衡」。
我记得刚入行时，带我的老工程师说了一句话：「你只要记住，每个连杆都受三个东西——重力、惯性力、相邻关节的约束力。」
嗯，这句话我到现在都觉得是精髓。
3.1.1 基本方程
对于单个刚体，牛顿方程和欧拉方程长这样：
牛顿方程：F = m * a_c
欧拉方程：τ = I * α + ω × (I * ω)其中：
- F 是作用在质心上的合力
- m 是连杆质量
- a_c 是质心加速度
- τ 是合力矩
- I 是惯性张量
- α 是角加速度
- ω 是角速度
这里有个坑，我刚开始做项目时经常忘掉那个叉乘项 ω × (I × ω)。
为什么？因为低速时它很小，但人形机器人跑起来之后，这个项能占到总力矩的 30% 以上。我曾经因为漏掉它，导致仿真里机器人直接「甩飞」了。
3.1.2 递推计算流程
实际工程中，我们不会对每个连杆单独列方程。而是用「递推」的方式：
- 正向递推：从基座到末端，计算每个连杆的速度、加速度
- 反向递推：从末端到基座，计算每个关节的力/力矩
我习惯把正向递推叫做「算运动」，反向递推叫做「算力」。你想想看，这其实很符合直觉——先知道怎么动，再算需要多大劲。
核心公式（反向递推关键一步）：
f_i = m_i * a_ci + f_{i+1}
τ_i = I_i * α_i + ω_i × (I_i * ω_i) + r_i × f_i + τ_{i+1}其中 f_i 和 τ_i 是第 i 个关节对第 i-1 个关节的作用力和力矩。
3.2 拉格朗日法：从能量角度看问题
如果说牛顿-欧拉法是「硬算」，那拉格朗日法就是「巧算」。
拉格朗日法不关心每个关节的约束力，它只盯着系统的能量——动能和势能。
3.2.1 拉格朗日方程
拉格朗日函数 L 定义为：
L = T - V其中 T 是总动能，V 是总势能。
然后动力学方程就是：
d/dt (∂L/∂q̇) - ∂L/∂q = τ这里 q 是广义坐标（关节角度），q̇ 是广义速度（关节角速度），τ 是广义力（关节力矩）。
说实话，我第一次看到这个方程时觉得挺抽象的。但后来做项目多了，发现它有个巨大的好处——你不需要处理复杂的约束力，只需要算能量。
我的个人经验：
当机器人结构比较复杂（比如有并联机构或浮动基座）时，我倾向于用拉格朗日法。因为牛顿-欧拉法里那些约束力会让人算到崩溃。而拉格朗日法，你只要把动能和势能写对，剩下的交给数学。
3.2.2 一个简单的二连杆例子
假设一个平面二连杆机器人，两个连杆质量分别为 m1、m2，长度分别为 l1、l2。
它的动能 T 和势能 V 可以写成：
T = 0.5 * m1 * v1^2 + 0.5 * I1 * ω1^2
+ 0.5 * m2 * v2^2 + 0.5 * I2 * ω2^2
V = m1 * g * h1 + m2 * g * h2然后代入拉格朗日方程，就能得到两个关节的力矩表达式。
嗯，这里要注意，v1、v2、h1、h2 都是 q 和 q̇ 的函数，所以展开后其实挺长的。但好处是——你永远不会漏掉科里奥利力和离心力项，因为它们是自动从 ∂L/∂q 里出来的。
3.3 关节力矩计算：从理论到代码
理论讲完了，咱们来点实际的。
关节力矩计算，说白了就是把上面两种方法落地成代码。我个人在实际项目中更常用牛顿-欧拉法，因为它计算效率高，适合实时控制。
3.3.1 牛顿-欧拉法的代码实现
下面是一个简化版的牛顿-欧拉递推代码（伪代码风格，但可以直接用）：
def compute_torques(q, qd, qdd, robot_params):
"""
q: 关节角度 (n,)
qd: 关节角速度 (n,)
qdd: 关节角加速度 (n,)
robot_params: 包含质量、惯性张量、连杆长度等
"""
n = len(q)
# 正向递推：计算每个连杆的加速度
omega = [np.zeros(3)] * (n+1)
alpha = [np.zeros(3)] * (n+1)
a = [np.zeros(3)] * (n+1) # 连杆坐标系原点加速度
ac = [np.zeros(3)] * (n+1) # 质心加速度
for i in range(1, n+1):
# 从关节 i 的旋转轴和角速度递推
z = robot_params['axis'][i-1] # 旋转轴方向
omega[i] = omega[i-1] + qd[i-1] * z
alpha[i] = alpha[i-1] + qdd[i-1] * z + qd[i-1] * np.cross(omega[i-1], z)
# ... 加速度递推略，实际需要根据连杆构型计算
# 反向递推：计算关节力矩
f = [np.zeros(3)] * (n+2)
tau = [np.zeros(3)] * (n+2)
for i in range(n, 0, -1):
# 惯性力 + 重力 + 相邻连杆的力
f[i] = robot_params['mass'][i-1] * ac[i] + f[i+1]
# 力矩平衡
tau[i] = (robot_params['I'][i-1] @ alpha[i]
+ np.cross(omega[i], robot_params['I'][i-1] @ omega[i])
+ np.cross(robot_params['r'][i-1], f[i])
+ tau[i+1])
# 投影到关节轴方向
joint_torque = tau[i].dot(robot_params['axis'][i-1])
return joint_torques避坑指南：
我曾经在反向递推时，把 f[i+1] 的方向搞反了。结果算出来的力矩符号全错，机器人仿真里直接往反方向跑。排查了整整两天才发现——嗯，坐标系方向一定要统一，建议全程用基座坐标系。
3.3.2 拉格朗日法的矩阵形式
拉格朗日法最终可以写成标准形式：
M(q) * q̈ + C(q, q̇) * q̇ + G(q) = τ其中：
- M(q)：惯性矩阵，跟关节角度有关
- C(q, q̇)：科里奥利力和离心力矩阵
- G(q)：重力项
这个形式在轨迹跟踪里特别有用。因为你可以直接用它做前馈补偿——把期望的 q̈、q̇、q 代入，算出前馈力矩，然后加上反馈控制。
我个人习惯在 Simulink 或 ROS2 里，把 M、C、G 分别封装成独立的函数。这样调试时，可以单独验证重力补偿对不对，惯性项有没有算错。
3.4 两种方法的对比与选择
| 对比项 | 牛顿-欧拉法 | 拉格朗日法 |
|---|---|---|
| 计算效率 | 高（O(n) 递推） | 低（O(n^4) 矩阵运算） |
| 物理直观性 | 强（力与力矩平衡） | 弱（能量抽象） |
| 约束力处理 | 需要显式计算 | 自动消去 |
| 适合场景 | 实时控制、嵌入式 | 离线仿真、结构分析 |
你想想看，如果是在嵌入式控制器里跑，每毫秒都要算一次力矩，那肯定选牛顿-欧拉法。但如果是做离线轨迹优化，拉格朗日法的矩阵形式更方便做数学推导。
我的建议：
初学者先掌握牛顿-欧拉法，因为它更直观，调试起来也更容易定位问题。等你对动力学有了感觉，再回头学拉格朗日法，你会发现很多公式其实是一一对应的。
3.5 本章小结
动力学入门，说白了就是两件事：
- 用牛顿-欧拉法算「力怎么传」
- 用拉格朗日法算「能量怎么变」
关节力矩计算是轨迹跟踪的基石。力矩算不准，轨迹跟踪就是空中楼阁。
嗯，我记得第一次在真机上跑动力学前馈时，看到机器人真的按照期望轨迹平滑运动，那种感觉——比仿真里跑通一百次都爽。