3. 机器人动力学基础
好，咱们接着聊运动学。上一节我们把刚体变换、旋转矩阵这些基础工具捋了一遍，现在该上真家伙了——怎么用一套标准方法，把机器人手臂的“骨头”和“关节”用数学描述清楚，然后算出末端到底在哪儿，再反过来算每个关节该转多少度。
这部分我当年啃了很久。说实话，刚接触DH参数时觉得它就是一堆莫名其妙的规则，后来在项目里调了半年机械臂，才真正体会到它的巧妙。你想想看，一个六轴臂，六个关节，你要让末端去抓一个螺丝孔，位置精度得控制在0.1mm以内——没有一套系统的建模方法，根本不可能。
3.1 DH参数法：给每个关节一个“身份证”
DH参数法，全称Denavit-Hartenberg参数法。说白了，就是用四个参数来描述相邻两个关节坐标系之间的变换关系。这四个参数分别是：
| 参数 | 符号 | 物理含义 |
|---|---|---|
| 连杆长度 | ai-1 | 沿xi-1轴，从zi-1到zi的距离 |
| 连杆扭转角 | αi-1 | 绕xi-1轴，从zi-1到zi的转角 |
| 连杆偏距 | di | 沿zi轴，从xi-1到xi的距离 |
| 关节角 | θi | 绕zi轴，从xi-1到xi的转角 |
这里有个关键点：DH参数有两种约定——标准DH和改进DH。我在项目中吃过这个亏。有一次从里抄了一组DH参数，直接套进自己的代码，结果末端位置差了十几厘米。查了两天才发现，用的是标准DH，我代码里写的是改进DH的变换顺序。
避坑指南：标准DH的坐标系建在连杆末端，变换顺序是 Rot(z,θ) → Trans(z,d) → Trans(x,a) → Rot(x,α)。改进DH的坐标系建在连杆首端，顺序是 Rot(x,α) → Trans(x,a) → Rot(z,θ) → Trans(z,d)。两者不通用，选一种就别换了。
我个人习惯用改进DH，因为它的坐标系更直观——每个关节的z轴就是关节旋转轴，x轴沿着连杆方向指出去。建坐标系时记住两条规则：
- z轴：沿关节旋转轴方向（旋转关节）或移动方向（移动关节）
- x轴：沿相邻两个z轴的公垂线方向，从zi-1指向zi
嗯，这里要注意：如果两个z轴平行，公垂线不唯一，那就选一条过前一个坐标系原点的。如果两个z轴相交，x轴方向取两个z轴的叉积。
3.2 正运动学建模：从关节空间到操作空间
正运动学，就是已知每个关节的角度（或位移），求末端执行器的位姿。说白了，就是“我告诉你每个关节转了多少，你告诉我手在哪儿”。
有了DH参数，正运动学就变成了一个机械的矩阵乘法过程。相邻两个连杆的变换矩阵是：
改进DH的变换矩阵 T(i-1→i) =
[ cosθi -sinθi 0 ai-1 ]
[ sinθi*cosαi-1 cosθi*cosαi-1 -sinαi-1 -di*sinαi-1 ]
[ sinθi*sinαi-1 cosθi*sinαi-1 cosαi-1 di*cosαi-1 ]
[ 0 0 0 1 ]把每个关节的变换矩阵连乘起来，就得到从基座到末端的齐次变换矩阵：
T0→n = T0→1 · T1→2 · ... · Tn-1→n
这个矩阵的左上角3×3是末端姿态（旋转矩阵），右上角3×1是末端位置（平移向量）。
小技巧：写代码时别手算矩阵乘法，用符号计算库（比如SymPy）先把符号表达式推出来，再转成数值计算。我早期手算六轴臂的正运动学，算错三次，后来老老实实用符号工具，十分钟搞定。
举个例子，一个六轴工业机器人，DH参数表可能是这样的：
| 关节i | ai-1 (mm) | αi-1 (°) | di (mm) | θi (°) |
|---|---|---|---|---|
| 1 | 0 | 0 | 400 | θ1 |
| 2 | 300 | -90 | 0 | θ2 |
| 3 | 0 | 0 | 0 | θ3 |
| 4 | 0 | -90 | 350 | θ4 |
| 5 | 0 | 90 | 0 | θ5 |
| 6 | 0 | -90 | 100 | θ6 |
把每个关节的变换矩阵乘起来，末端位姿就是θ1到θ6的函数。正运动学计算量不大，实时性要求高的场景（比如1kHz控制周期）也能跑得动。
3.3 逆运动学求解：从操作空间回到关节空间
逆运动学就麻烦多了。已知末端位姿，求每个关节的角度。说白了，就是“手要放到那个位置，每个关节该转多少”。
为什么麻烦？因为逆运动学通常有多解、无解、甚至无穷多解。你想想看，一个六轴臂，末端在空间里一个点，手臂可以“弯着”过去，也可以“伸着”过去，肘关节朝上或朝下——这些都是不同的关节角度组合。
求解方法分两大类：解析法和数值法。
3.4 解析法：快，但有条件
解析法，就是通过代数或几何推导，直接写出关节角度的闭式表达式。它的优点是快——一次计算就能出结果，适合实时控制。缺点是只适用于特定构型的机器人。
什么样的机器人能用解析法？Pieper准则说：如果机器人的三个相邻关节轴交于一点，或者三个相邻关节轴平行，就可以用解析法。最常见的例子就是六轴工业机器人，它的后三个关节（腕部）的轴线交于一点——这就是所谓的“球形腕”。
解析法的求解步骤一般是：
- 分离位置和姿态：利用球形腕的特性，把腕部中心位置和末端姿态解耦
- 求前三个关节角：根据腕部中心位置，用几何法求θ1、θ2、θ3
- 求后三个关节角：根据末端姿态和已求出的前三个关节角，用欧拉角或RPY角求θ4、θ5、θ6
我曾经在一个焊接机器人项目里用解析法做逆解。焊接路径有几百个点，每个点都要算逆解，数值法根本跑不动。换成解析法后，一个控制周期内把所有点都算完了，还有余量做插补。
关键点：解析法求出的解通常有多个（比如肘上/肘下、腕翻转/不翻转），需要根据关节限位和避障要求选一个最优解。我一般用“最小关节位移”原则——选离当前关节角度最近的那组解。
3.5 数值法：通用，但慢
数值法，说白了就是迭代逼近。给定一个初始猜测的关节角度，计算末端位姿误差，然后不断修正关节角度，直到误差小于阈值。
最常用的数值法是牛顿-拉夫森法和雅可比伪逆法。核心思想是：
Δx = J · Δq
其中Δx是末端位姿误差（6维向量：3个位置误差+3个姿态误差），J是雅可比矩阵（6×n矩阵），Δq是关节角度修正量。然后：
Δq = J† · Δx
J†是雅可比矩阵的伪逆。迭代直到||Δx||小于设定阈值。
数值法的优点是通用——不管机器人什么构型，都能算。缺点是：
- 慢：每次迭代都要算雅可比矩阵，还要做矩阵求逆或SVD分解
- 依赖初值：初值选不好，可能收敛到错误的解，甚至发散
- 可能有奇异点：雅可比矩阵在某些位姿下会降秩，伪逆不稳定
避坑指南：数值法在奇异点附近会出现关节速度突变。我曾经在调试一个七轴协作臂时，末端走一条直线路径，经过奇异点时关节4突然以每秒500度的速度猛转——要不是有急停，电机就烧了。后来加了阻尼最小二乘法（DLS），才把这个问题压住。
3.6 知识体系总览
下面这张图把本章的核心逻辑串起来了。从DH参数开始，到正运动学建模，再到逆运动学的两种求解路径——解析法和数值法。你可以把它当作一个快速索引。
3.7 两种方法的对比与选择
在实际项目中怎么选？我一般这样判断：
| 场景 | 推荐方法 | 原因 |
|---|---|---|
| 六轴工业机器人（球形腕） | 解析法 | 快，稳定，适合实时控制 |
| 七轴冗余机器人 | 数值法 | 解析法推导复杂，数值法可处理冗余 |
| 非球形腕构型 | 数值法 | 解析法不适用 |
| 高精度离线规划 | 解析法+数值法混合 | 先用解析法求初值，再用数值法精调 |
| 实时控制（1kHz以上） | 解析法 | 数值法迭代次数不可控，可能超时 |
最后说一句：不要迷信任何一种方法。我在一个协作机器人项目里，一开始死磕解析法，花了两周推导公式，结果发现机器人构型不满足Pieper准则。后来换成数值法，一天就调通了。反过来，在另一个高速分拣项目里，数值法死活跑不到1kHz，最后还是老老实实回去用解析法。
嗯，运动学基础就聊到这儿。这些内容看着多，其实核心就三件事：DH参数怎么建、正运动学怎么乘、逆运动学怎么解。把这三件事搞明白，后面讲动力学和力矩控制时，你才不会觉得地基是虚的。