第二章：刚体动力学基础——让关节听话的数学魔法
做机器人控制这些年，我最大的体会就是：不懂动力学，你永远在跟关节打架。你让它往东，它偏要往西，还跟你抖得像筛糠一样。说白了，动力学就是描述「力怎么让机器人动起来」的那套数学工具。
这一章，我们来啃三块硬骨头：牛顿-欧拉方程、拉格朗日方程，还有关节空间与任务空间的动力学关系。别怕，我会用我踩过的坑帮你铺路。
2.1 牛顿-欧拉方程：最朴素的力与运动
先问个问题：一个刚体在空间里运动，它受的力和力矩怎么算？
答案其实就两句话：
- 牛顿第二定律：\( F = m \cdot a \) —— 力等于质量乘加速度，管你平动还是转动
- 欧拉方程：\( \tau = I \cdot \alpha + \omega \times (I \cdot \omega) \) —— 力矩等于惯量乘角加速度，再加一个耦合项
嗯，这里要注意：欧拉方程里那个叉乘项 \( \omega \times (I \cdot \omega) \)，很多人第一次写代码时都会漏掉。我当年做六轴机器人仿真时，就因为这个项没加，仿真出来的关节像喝醉了酒一样乱晃。排查了整整两天才找到原因。
核心思想：牛顿-欧拉法是「从前往后推」——先算每个连杆的速度和加速度（正向递推），再算每个关节的力和力矩（反向递推）。
实际工程中，我建议用递归牛顿-欧拉算法（RNEA）。它的计算复杂度是 \( O(n) \)，n是关节数。对于六轴工业机器人，一次完整计算大概只要几十微秒，完全够实时控制用。
2.2 拉格朗日方程：从能量角度看问题
牛顿-欧拉法虽然直观，但有个毛病：你得把每个连杆的约束力都算出来，很多是冗余的。拉格朗日法就不一样了——它只看系统的动能和势能，约束力自动消失。
拉格朗日方程长这样：
d/dt (∂L/∂q̇) - ∂L/∂q = τ其中 \( L = T - V \)，T是动能，V是势能，q是广义坐标（对机器人来说就是关节角度）。
你想想看，这个方程多优雅：你只需要写出系统的能量，然后求导，就能得到动力学方程。不用管那些乱七八糟的约束反力。
我的经验：对于少于4个自由度的系统，我习惯用手推导拉格朗日方程。但超过4个自由度，手算容易出错，建议用符号计算工具（比如SymPy）自动推导。我曾经手算过一个7自由度机械臂的拉格朗日方程，结果少写了一项，导致仿真结果差了30%。从那以后，超过4个自由度我就用工具了。
拉格朗日法写出来的动力学方程，最终形式是：
M(q)q̈ + C(q, q̇)q̇ + G(q) = τ其中：
- \( M(q) \) —— 惯性矩阵，跟关节位置有关
- \( C(q, q̇) \) —— 科里奥利力和离心力矩阵
- \( G(q) \) —— 重力项
- \( \tau \) —— 关节驱动力矩
这个形式，说白了就是控制理论里最常用的「标准形式」。后面做力矩控制、阻抗控制，都离不开它。
2.3 关节空间 vs 任务空间动力学
搞清楚了单个刚体的动力学，接下来要面对一个现实问题：你的机器人到底在哪个空间里控制？
我刚开始做机器人时，一直以为关节空间就够了——毕竟电机装在关节上嘛。直到有一次，我要让机械臂末端画一个圆，结果在关节空间里规划了半天，末端轨迹歪歪扭扭的。后来才明白：任务空间才是用户关心的空间。
关节空间动力学
就是上面那个方程：
M(q)q̈ + C(q, q̇)q̇ + G(q) = τ所有变量都是关节角度、角速度、角加速度。控制器的输入是关节力矩，输出是关节运动。这是最底层的动力学模型，也是实时性最好的。
任务空间动力学
把关节空间的动力学映射到末端执行器的笛卡尔空间：
Λ(x)ẍ + μ(x, ẋ)ẋ + p(x) = F其中：
- \( \Lambda(x) \) —— 任务空间惯性矩阵
- \( \mu(x, ẋ) \) —— 任务空间科里奥利矩阵
- \( p(x) \) —— 任务空间重力项
- \( F \) —— 末端执行器受到的力/力矩
任务空间和关节空间之间通过雅可比矩阵 \( J(q) \) 联系：
Λ(x) = (J M^{-1} J^T)^{-1}注意：当机器人接近奇异位形时，雅可比矩阵会变得病态，任务空间惯性矩阵的计算会不稳定。我曾经在调试一个协作机器人时，就因为没处理奇异点，导致末端在奇异位形附近剧烈抖动。解决方案是加一个阻尼项，或者干脆在奇异点附近切换到关节空间控制。
2.4 知识体系总览
为了让你对本章内容有个整体把握，我画了一张图：
2.5 实际工程中的选择
说了这么多理论，你可能会问：到底用哪个？
我的建议是：
| 场景 | 推荐方法 | 原因 |
|---|---|---|
| 实时控制（1kHz以上） | 牛顿-欧拉（RNEA） | 计算量小，O(n)复杂度 |
| 离线仿真/轨迹优化 | 拉格朗日法 | 物理意义清晰，便于分析 |
| 力控制/阻抗控制 | 任务空间动力学 | 直接控制末端力/位置 |
| 参数辨识 | 拉格朗日法（线性化形式） | 可以写成Y(q,q̇,q̈)·θ = τ |
一个小技巧：如果你在做参数辨识，记得把动力学方程写成关于惯性参数的线性形式：τ = Y(q, q̇, q̈) · θ。这样就能用最小二乘法直接辨识出质量、惯量、质心位置等参数。我做过一个项目，用这个方法把六轴机器人的动力学参数误差从15%降到了3%以内。
好了，这一章的内容就到这里。动力学是机器人控制的「内功」，练好了后面学什么都快。记住：牛顿-欧拉给你速度，拉格朗日给你优雅，空间变换给你灵活。三者缺一不可。
可可精品资料库