4. 正运动学（一）：Denavit-Hartenberg (D-H) 参数法、建立连杆坐标系
正运动学，说白了就是：给定关节角度，求末端位姿。
你给机器人每个关节一个角度值，它就能算出夹爪或末端执行器在空间里到底在哪儿、朝向如何。这听起来像是个几何问题，但实际做起来，你会发现——坐标系怎么建，直接决定了后面所有计算的复杂度。
我刚开始接触机器人学时，最头疼的就是这一步。明明看着一个六轴机械臂，却不知道从哪下手去描述它。后来我才明白，不是我不懂几何，而是缺少一套标准化的建模方法。
这套方法，就是 Denavit-Hartenberg 参数法，简称 D-H 法。
4.1 为什么需要 D-H 参数法？
一个机器人手臂，本质上是一串连杆通过关节串联起来的运动链。每个连杆都有自己的几何尺寸，每个关节都有自己的运动范围。
如果我们想用数学描述这个链，最直接的想法是：在每个关节处建一个坐标系，然后通过坐标变换把末端的位置算出来。
但问题来了——坐标系怎么建？
你可以随便建，但那样的话，每个人建的坐标系都不一样，变换矩阵也千奇百怪，代码没法复用，也没法对比。
D-H 法就是一套约定俗成的规则。它规定了：
- 坐标系怎么放
- 坐标轴怎么指
- 四个参数怎么取
只要大家都按这个规则来，同一个机器人，建出来的模型就是唯一的。
核心思想：D-H 法用四个参数（a, α, d, θ）就能完整描述相邻两个连杆之间的相对位置和姿态。这四个参数，就是机器人运动学的“基因”。
4.2 四个 D-H 参数的含义
我们先直观理解一下这四个参数，别急着背公式。
| 参数 | 符号 | 物理含义 | 我习惯叫它 |
|---|---|---|---|
| 连杆长度 | ai | 沿 Xi 轴，从 Zi-1 到 Zi 的距离 | “杆长” |
| 连杆扭角 | αi | 绕 Xi 轴，从 Zi-1 转到 Zi 的角度 | “扭转角” |
| 连杆偏距 | di | 沿 Zi-1 轴，从 Xi-1 到 Xi 的距离 | “偏置” |
| 关节角 | θi | 绕 Zi-1 轴，从 Xi-1 转到 Xi 的角度 | “关节变量” |
你可能会问：为什么是这四个？不是三个也不是五个？
嗯，这里有个小故事。我当年在实验室调一个六轴协作臂时，发现用欧拉角描述相邻连杆关系，需要六个参数，冗余且容易出奇异。而 D-H 法用四个参数就够，因为它巧妙地利用了连杆本身的几何约束——两个关节轴之间的公垂线，天然定义了 X 轴的方向。
我的经验：对于旋转关节，θ 是变量；对于移动关节，d 是变量。其他三个参数在机器人设计完成后就是固定值。写代码时，把固定参数写在配置文件里，变量在运行时传入，这样结构最清晰。
4.3 建立连杆坐标系的步骤
这部分是实操重点。我建议你拿一支笔、一张纸，边看边画。
假设我们有一个 n 自由度的机器人，连杆编号从 0（基座）到 n（末端）。
- 找出所有关节轴：画出每个关节的旋转轴或移动方向，标为 Zi-1 轴。
- 确定原点 Oi：Zi-1 和 Zi 的公垂线与 Zi 的交点，就是坐标系 {i} 的原点。
- 确定 Xi 轴：沿公垂线方向，从 Zi-1 指向 Zi。
- 确定 Yi 轴：右手定则，Yi = Zi × Xi。
- 处理特殊情况：
- 如果 Zi-1 和 Zi 平行：公垂线不唯一，取与前一公垂线共线的方向。
- 如果 Zi-1 和 Zi 相交：原点取交点，Xi 垂直于两轴所在平面。
我曾经踩过的坑：在建立第一个坐标系（基座坐标系 {0}）时，Z0 沿关节 1 的轴方向，但 X0 和 Y0 可以任意选择。我一开始随便选了一个方向，结果后面算出来的变换矩阵符号全反了。建议：基座坐标系尽量与世界坐标系对齐，这样后续调试时直觉上更容易理解。
4.4 从坐标系到变换矩阵
坐标系建好了，接下来就是数学活了。
从坐标系 {i-1} 到 {i} 的变换，可以分解为四个步骤：
- 绕 Zi-1 轴旋转 θi
- 沿 Zi-1 轴平移 di
- 沿 Xi 轴平移 ai
- 绕 Xi 轴旋转 αi
写成齐次变换矩阵就是：
T = 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 ]这个矩阵，就是 D-H 法的核心产出。你只要把四个参数代进去，就能得到相邻连杆的变换关系。
4.5 完整正运动学计算
有了相邻变换矩阵，正运动学就变成了一个连乘问题：
T_0n = T_01 · T_12 · T_23 · ... · T_(n-1)n其中 T_0n 就是末端坐标系相对于基座坐标系的位姿。
我写代码时，习惯用一个循环来实现：
import numpy as np
def dh_transform(a, alpha, d, theta):
"""计算单个D-H变换矩阵"""
ct = np.cos(theta)
st = np.sin(theta)
ca = np.cos(alpha)
sa = np.sin(alpha)
T = np.array([
[ct, -st*ca, st*sa, a*ct],
[st, ct*ca, -ct*sa, a*st],
[0, sa, ca, d ],
[0, 0, 0, 1 ]
])
return T
def forward_kinematics(dh_params):
"""
dh_params: 列表，每个元素为 (a, alpha, d, theta)
返回末端位姿矩阵
"""
T = np.eye(4)
for a, alpha, d, theta in dh_params:
T = T @ dh_transform(a, alpha, d, theta)
return T调试技巧：写完正运动学后，把每个关节角设为 0，看看末端位置是不是在机器人的“零位”姿态。如果不是，大概率是某个 D-H 参数的符号搞反了。我每次都会做这个检查，能省下大量排查时间。
4.6 知识体系总览
下面这张图，把本章的核心逻辑串起来了。你可以把它当作一个思维导图来用。
4.7 一个简单的例子：两连杆平面臂
光讲理论太干，我们来看个具体例子。
假设一个两连杆平面机械臂：
- 连杆 1 长度 L1，关节角 θ1
- 连杆 2 长度 L2，关节角 θ2
- 所有关节轴都平行（垂直于纸面）
按照 D-H 法，我们可以建立如下参数表：
| 连杆 i | ai | αi | di | θi |
|---|---|---|---|---|
| 1 | L1 | 0 | 0 | θ1 |
| 2 | L2 | 0 | 0 | θ2 |
代入变换矩阵公式，得到：
T_01 = [cosθ1 -sinθ1 0 L1·cosθ1]
[sinθ1 cosθ1 0 L1·sinθ1]
[0 0 1 0 ]
[0 0 0 1 ]
T_12 = [cosθ2 -sinθ2 0 L2·cosθ2]
[sinθ2 cosθ2 0 L2·sinθ2]
[0 0 1 0 ]
[0 0 0 1 ]
T_02 = T_01 · T_12
末端位置：
x = L1·cosθ1 + L2·cos(θ1+θ2)
y = L1·sinθ1 + L2·sin(θ1+θ2)这个结果，和你用几何法直接推导出来的完全一致。这就是 D-H 法的威力——它把几何直觉变成了可编程的代数流程。