第3章 正运动学：Denavit-Hartenberg (D-H) 参数法
正运动学，说白了就是：给定关节角度，求末端位姿。你想想看，机器人每个关节转了多少度，最后手爪在哪儿、朝向如何——这就是正运动学要回答的问题。我做了这么多年运动控制，几乎每个项目第一步都是把正运动学模型搭扎实。模型错了，后面什么轨迹、力控都是空中楼阁。
这一章我们聚焦 D-H 参数法。为什么选它？因为它是工业界和学术界最通用的“语言”。不管你是做六轴协作臂，还是 SCARA 机器人，D-H 参数都能帮你用一套标准流程建立连杆坐标系，然后推导出齐次变换矩阵。
3.1 D-H 参数法 —— 四个数字定天下
D-H 参数用四个值描述相邻连杆之间的相对位姿：连杆长度 aᵢ、连杆扭角 αᵢ、关节距离 dᵢ、关节转角 θᵢ。我个人习惯把 a 和 α 看作“连杆自身的几何”，d 和 θ 看作“关节运动变量”。对于旋转关节，θ 是变量；对于移动关节，d 是变量。记住这个区分，后面写代码不容易乱。
标准 D-H 参数定义（我常用的约定）：
- aᵢ —— 从 Zᵢ₋₁ 到 Zᵢ 沿 Xᵢ 的距离（连杆长度）
- αᵢ —— 从 Zᵢ₋₁ 到 Zᵢ 绕 Xᵢ 的转角（连杆扭角）
- dᵢ —— 从 Xᵢ₋₁ 到 Xᵢ 沿 Zᵢ₋₁ 的距离（关节偏置）
- θᵢ —— 从 Xᵢ₋₁ 到 Xᵢ 绕 Zᵢ₋₁ 的转角（关节角）
我在项目里见过不少工程师把 a 和 d 搞反，结果仿真时末端飞出去十万八千里。嗯，这里要注意：a 总是沿着 X 轴，d 总是沿着 Z 轴。你画一遍坐标系就记住了。
3.2 建立连杆坐标系 —— 步骤与口诀
建立坐标系是 D-H 法里最容易出错的一步。我自己的口诀是：“先定 Z 轴，再定 X 轴，Y 轴右手定”。具体来说：
- 找出所有关节轴，画出轴线。每个关节的 Z 轴与关节轴线重合。
- Zᵢ 轴指向关节 i+1 的运动方向（旋转轴或移动轴）。
- Xᵢ 轴沿 Zᵢ₋₁ 和 Zᵢ 的公垂线，方向从 Zᵢ₋₁ 指向 Zᵢ。
- 如果 Zᵢ₋₁ 和 Zᵢ 相交，Xᵢ 垂直于二者所在平面。
- Yᵢ 由右手法则确定：Yᵢ = Zᵢ × Xᵢ。
你想想看，只要 Z 轴和 X 轴定对了，Y 轴自动就出来了。我曾经在调试一台六轴机器人时，因为第三个关节的 Z 轴方向画反了，导致逆运动学死活算不对。后来重新按这个口诀捋了一遍，十分钟解决问题。
3.3 正运动学方程推导 —— 从参数到矩阵
有了 D-H 参数，相邻连杆的齐次变换矩阵就是标准公式。我一般直接记下面的形式，每次用的时候心里默念一遍：
# 相邻连杆变换矩阵 (标准D-H)
T_i = Rot(z, θᵢ) · Trans(z, dᵢ) · Trans(x, aᵢ) · Rot(x, αᵢ)
[ cosθᵢ -sinθᵢ·cosαᵢ sinθᵢ·sinαᵢ aᵢ·cosθᵢ ]
[ sinθᵢ cosθᵢ·cosαᵢ -cosθᵢ·sinαᵢ aᵢ·sinθᵢ ]
Tᵢ = [ 0 sinαᵢ cosαᵢ dᵢ ]
[ 0 0 0 1 ]把每个关节的 Tᵢ 连乘起来，就得到末端相对于基座的变换矩阵：T_base_to_end = T₁ · T₂ · T₃ · … · Tₙ。这就是正运动学方程。说白了，就是一连串矩阵乘法，但顺序千万不能错——矩阵乘法不满足交换律，我见过有人把 T₂ 和 T₃ 写反了，结果末端位置差了十几厘米。
3.4 实例：SCARA 机器人正解
SCARA 机器人有四个关节：三个旋转关节（J1, J2, J4）和一个移动关节（J3）。我们直接上 D-H 参数表。这个表是我从实际项目中整理出来的，你直接拿去用：
| 连杆 i | aᵢ (mm) | αᵢ (°) | dᵢ (mm) | θᵢ (°) |
|---|---|---|---|---|
| 1 | a₁ = 200 | 0 | d₁ = 350 | θ₁ (变量) |
| 2 | a₂ = 160 | 0 | 0 | θ₂ (变量) |
| 3 | 0 | 0 | d₃ (变量) | 0 |
| 4 | 0 | 0 | d₄ = 80 | θ₄ (变量) |
把参数代入标准矩阵，然后连乘。我习惯用 Python 的 sympy 做符号推导，这样不容易出错。下面是一个简化版的正解代码片段：
import sympy as sp
# 定义符号变量
θ1, θ2, θ4 = sp.symbols('θ1 θ2 θ4')
d3 = sp.symbols('d3')
a1, a2, d1, d4 = 200, 160, 350, 80
# 构建四个变换矩阵 (标准D-H)
T1 = sp.Matrix([[sp.cos(θ1), -sp.sin(θ1), 0, a1*sp.cos(θ1)],
[sp.sin(θ1), sp.cos(θ1), 0, a1*sp.sin(θ1)],
[0, 0, 1, d1],
[0, 0, 0, 1]])
T2 = sp.Matrix([[sp.cos(θ2), -sp.sin(θ2), 0, a2*sp.cos(θ2)],
[sp.sin(θ2), sp.cos(θ2), 0, a2*sp.sin(θ2)],
[0, 0, 1, 0],
[0, 0, 0, 1]])
T3 = sp.Matrix([[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, d3],
[0, 0, 0, 1]])
T4 = sp.Matrix([[sp.cos(θ4), -sp.sin(θ4), 0, 0],
[sp.sin(θ4), sp.cos(θ4), 0, 0],
[0, 0, 1, d4],
[0, 0, 0, 1]])
# 正运动学：末端位姿
T_end = T1 * T2 * T3 * T4
sp.simplify(T_end)
print(T_end) # 输出 4x4 齐次矩阵运行这段代码，你会得到末端位置 (x, y, z) 关于四个关节变量的表达式。比如 x 分量会包含 cos(θ1+θ2+θ4) 这样的项。你想想看，这就是正运动学的核心——用关节空间映射到操作空间。
SCARA 正解关键点：
- J1 和 J2 的轴线平行，所以 α₁ = α₂ = 0°。
- J3 是移动关节，d₃ 是变量，θ₃ 固定为 0。
- J4 的旋转不影响末端位置，只影响姿态（绕 Z 轴）。
- 末端高度由 d₁ + d₃ + d₄ 决定，非常简单直观。
我在现场调试 SCARA 时，经常用这个正解模型快速验证关节角度是否合理。比如给定一组 θ₁=30°, θ₂=-45°, d₃=50mm, θ₄=90°，手算一下末端坐标，再和示教器读数对比。如果对不上，十有八九是 D-H 参数符号搞反了。