正运动学建模：D-H参数法详解
正运动学，说白了就是给你一组关节角度，算出末端执行器在空间里到底在哪儿、朝哪个方向。这是机器人学里最基础、也最绕不开的一步。我刚开始做机器人时，觉得这玩意儿不就是矩阵乘来乘去嘛，后来真上手调机械臂才发现——坐标系没建对，后面全白干。
这一章我们重点啃D-H参数法。你把它理解成一套「标准化流程」就行：不管机器人长什么样，只要按这套规则建坐标系、填参数，就能用统一的数学框架算出末端位姿。
D-H参数法到底在干什么？
D-H参数法用四个参数来描述相邻两个连杆之间的相对位置和姿态：
- θᵢ（关节角）：绕Z轴旋转的角度
- dᵢ（连杆偏距）：沿Z轴平移的距离
- aᵢ（连杆长度）：沿X轴平移的距离
- αᵢ（连杆扭角）：绕X轴旋转的角度
这四个参数，就能唯一确定相邻坐标系之间的变换矩阵。我习惯把它记成「两次旋转加两次平移」——先绕Z转θ，再沿Z移d，然后沿X移a，最后绕X转α。顺序不能乱，乱了矩阵就不对了。
核心公式：相邻连杆变换矩阵
T = Rot(z, θ) · Trans(z, d) · Trans(x, a) · Rot(x, α)展开后就是4×4齐次变换矩阵，乘起来就能从基座一路算到末端。
标准D-H vs 改进D-H：到底差在哪儿？
这个问题我当年也迷糊过。两种方法都能用，但坐标系建法不一样，参数定义也不一样。我曾在项目里混用过两种，结果算出来的末端位置差了十几厘米——嗯，从那以后我再也不敢混着用了。
| 对比项 | 标准D-H | 改进D-H |
|---|---|---|
| 坐标系附着位置 | 附着在连杆i的输出端（关节i+1） | 附着在连杆i的输入端（关节i） |
| 变换顺序 | 先绕Z轴变换，再绕X轴变换 | 先绕X轴变换，再绕Z轴变换 |
| 参数定义 | θᵢ、dᵢ、aᵢ、αᵢ | θᵢ、dᵢ、aᵢ、αᵢ（但下标含义不同） |
| 常见教材 | Craig《机器人学导论》 | Siciliano《机器人学》 |
| 我的建议 | 适合串联臂、树形结构 | 适合并联机构、闭链结构 |
我曾经踩过的坑：用标准D-H建完坐标系，却用改进D-H的公式算矩阵。结果末端位置偏了，排查了整整两天才发现是参数下标对不上。记住——选了一种就别中途换。
建立连杆坐标系：手把手教你做
建坐标系这事儿，说白了就是「找轴、定原点、画箭头」。我总结了一套步骤，你照着做基本不会错：
- 找出所有关节轴，画出它们的轴线（Z轴方向）
- 确定相邻轴线的公垂线，或者找它们的交点
- 原点放在公垂线与关节轴的交点上
- X轴沿公垂线方向，从当前关节指向下一个关节
- Y轴由右手定则确定（Z×X = Y）
举个例子，一个六轴工业机器人，从基座开始，每个关节都按这套规则建坐标系。我习惯在纸上先画一遍，再写到代码里——手画一遍能发现很多细节问题。
小技巧：如果相邻关节轴平行，公垂线不唯一，取与前一坐标系X轴共线的方向。如果相邻关节轴相交，原点取在交点，X轴垂直于两轴所在平面。
SVG：D-H参数法知识体系
下面这张图把D-H参数法的核心逻辑串起来了。你从「输入关节角度」出发，经过「建坐标系→填参数→算矩阵→乘起来」，最后得到「末端位姿」。我每次做正运动学，脑子里过的就是这张图。
代码实现：从参数到矩阵
理论说完了，上代码。这是我常用的一个Python函数，输入D-H参数，输出4×4变换矩阵。你直接拿去用就行：
import numpy as np
def dh_transform(theta, d, a, alpha):
"""
标准D-H参数 -> 4x4齐次变换矩阵
theta: 关节角 (弧度)
d: 连杆偏距
a: 连杆长度
alpha: 连杆扭角 (弧度)
"""
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
# 示例：一个两连杆机械臂
T01 = dh_transform(np.pi/4, 0.2, 0.5, 0)
T12 = dh_transform(np.pi/3, 0.1, 0.4, 0)
T02 = T01 @ T12
print("末端位姿矩阵:\n", T02)我个人的习惯：把所有关节的D-H参数先写成一个表格，再用循环把矩阵乘起来。这样改参数时不用改代码，改表格就行。调试时也方便逐段打印检查。
避坑指南：我踩过的那些坑
- 坐标系方向搞反：Z轴方向没统一，导致旋转方向反了。我建议你画图时标清楚每个关节的正方向。
- 参数下标对不上：标准D-H的θ₁对应关节1，改进D-H的θ₁可能对应关节2。我建议你写代码时在注释里标明用的是哪种约定。
- 矩阵乘法顺序：从基座到末端是T01 * T12 * T23，不是反过来。我见过有人写反了，结果末端跑到机器人背后去了。
- 角度单位：Python的三角函数默认弧度，别传了度数进去。我建议你写个deg2rad的辅助函数。
嗯，正运动学建模就讲到这里。你先把D-H参数法的流程走一遍，坐标系建好，参数填对，矩阵算出来——后面做逆运动学和力控的时候，这些就是地基。地基稳了，楼才盖得高。