4. 正运动学：从关节空间到笛卡尔空间的映射
正运动学，说白了就是：已知每个关节的角度（或位移），求末端执行器在空间中的位置和姿态。
我刚开始接触机器人时，总觉得这问题很抽象。后来做项目才明白——你给机器人发指令，它关节转了多少，手到底在哪儿？这就是正运动学要回答的。它是所有运动控制的基础，也是你调试机器人时最先要算清楚的东西。
核心思想：正运动学 = 从关节变量 → 末端位姿的映射。不涉及力、速度、加速度，只关心几何关系。
4.1 正运动学问题描述
先明确一下我们要解决什么问题。
一个 n 自由度的串联机器人，有 n 个关节。每个关节有一个变量 qi（旋转关节是角度 θi，移动关节是位移 di）。
正运动学要计算的是：
- 位置：末端执行器原点在基坐标系下的坐标 (x, y, z)
- 姿态：末端执行器坐标系相对于基坐标系的旋转关系（通常用旋转矩阵 R 或欧拉角表示）
数学上，我们用一个 4×4 的齐次变换矩阵 T 来表示：
T = [ R p ]
[ 0 1 ]
其中：
R — 3×3 旋转矩阵（姿态）
p — 3×1 位置向量
所以正运动学就是：给定 q1, q2, ..., qn，求 T0n。
我的经验：做正运动学计算时，我习惯先把每个关节的坐标系画出来。坐标系画对了，矩阵就不会错。有一次我在项目里偷懒没画图，结果算出来的末端位置差了 20 厘米——查了半天，原来是相邻两个关节的 z 轴方向搞反了。
4.2 基于 D-H 法的正运动学求解
D-H 法（Denavit-Hartenberg）是解决正运动学最经典的方法。它用四个参数来描述相邻两个连杆之间的变换关系。
4.2.1 D-H 参数的定义
对于相邻的两个关节 i-1 和 i，我们定义四个参数：
| 参数 | 符号 | 含义 | 说明 |
|---|---|---|---|
| 连杆长度 | ai-1 | 沿 xi-1 轴，从 zi-1 到 zi 的距离 | 总是 ≥ 0 |
| 连杆扭角 | αi-1 | 绕 xi-1 轴，从 zi-1 到 zi 的转角 | 可正可负 |
| 连杆偏距 | di | 沿 zi 轴，从 xi-1 到 xi 的距离 | 旋转关节为常量 |
| 关节角 | θi | 绕 zi 轴，从 xi-1 到 xi 的转角 | 旋转关节为变量 |
有了这四个参数，相邻连杆的变换矩阵就是：
T(i-1→i) = Rot(x, αi-1) · Trans(x, ai-1) · Rot(z, θi) · Trans(z, di)
展开成 4×4 矩阵：
T = [ 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 ]
4.2.2 求解步骤
- 建立坐标系：按 D-H 规则给每个关节建立坐标系
- 填写 D-H 参数表：把 a, α, d, θ 填进去
- 计算单个变换矩阵：对每个关节 i，计算 Ti-1i
- 连乘得到总变换：T0n = T01 · T12 · ... · Tn-1n
注意：矩阵乘法不满足交换律！顺序不能错。我曾经在写代码时把乘法的顺序写反了，结果末端位置完全不对。调试了整整一个下午才发现——嗯，从那以后我每次写矩阵乘法都会在注释里标清楚顺序。
4.3 实例分析：PUMA 560
PUMA 560 是工业机器人史上的经典机型，6 个旋转关节。我们拿它来走一遍完整的正运动学求解流程。
4.3.1 建立坐标系
PUMA 560 的 D-H 坐标系建立如下（标准 D-H 约定）：
- 基座坐标系 {0}：z0 竖直向上
- 关节 1：绕 z0 旋转，连杆 1 水平
- 关节 2：绕 z1 旋转，连杆 2 水平
- 关节 3：绕 z2 旋转，连杆 3 水平
- 关节 4、5、6：腕部关节，构成球腕
4.3.2 D-H 参数表
| 关节 i | ai-1 (mm) | αi-1 (°) | di (mm) | θi (°) |
|---|---|---|---|---|
| 1 | 0 | 0 | 0 | θ1 |
| 2 | 431.8 | -90 | 149.09 | θ2 |
| 3 | 20.32 | 0 | 0 | θ3 |
| 4 | 0 | -90 | 433.07 | θ4 |
| 5 | 0 | 90 | 0 | θ5 |
| 6 | 0 | -90 | 56.25 | θ6 |
4.3.3 计算变换矩阵
以关节 1 到关节 2 为例：
a1 = 431.8 mm, α1 = -90°, d2 = 149.09 mm, θ2 为变量
T12 = [ cosθ2 -sinθ2 0 431.8 ]
[ 0 0 1 149.09 ]
[ -sinθ2 -cosθ2 0 0 ]
[ 0 0 0 1 ]
同理，计算出所有 6 个变换矩阵，然后连乘：
T06 = T01 · T12 · T23 · T34 · T45 · T56
最终得到的是一个 4×4 矩阵，里面包含了末端的位置 (px, py, pz) 和姿态（旋转矩阵 R）。
4.3.4 验证：一个具体位姿
假设所有关节角度为 0°（即机器人的"零位"），代入计算：
θ1=0, θ2=0, θ3=0, θ4=0, θ5=0, θ6=0
得到末端位置：
px = 431.8 + 20.32 = 452.12 mm
py = 0
pz = 149.09 + 433.07 + 56.25 = 638.41 mm
姿态矩阵为单位矩阵（末端坐标系与基坐标系同向）
这个结果和 PUMA 560 的实际机械结构完全吻合——手臂完全伸直，末端在 x 正方向 452mm，z 方向 638mm 处。
避坑指南：我曾经在验证时发现计算结果和实际测量差了 2mm。排查后发现是 D-H 参数表里 d6 的值写错了——手册上写的是 56.25mm，但我看成了 56.25cm。单位搞错，差了一个数量级。所以，拿到参数表第一件事：确认单位。
4.4 正运动学知识体系
下面这张图帮你理清正运动学的整体脉络：
4.5 小结
正运动学是机器人学的入门必修课。说白了，它就是一套从关节空间到笛卡尔空间的映射规则。
- D-H 法用四个参数优雅地描述了相邻连杆的关系
- 矩阵连乘是核心操作，顺序至关重要
- PUMA 560 的实例展示了完整的求解流程
我个人建议：刚开始学的时候，一定要手算一遍完整的 6 自由度机器人。哪怕算到后面矩阵展开有几十项，也别偷懒。我当年就是手算了三遍 PUMA 560，才真正理解了每个参数在矩阵里是怎么"跑"的。代码可以帮你算得快，但手算能帮你理解得深。