正运动学：D-H参数法，建立人形机器人连杆坐标系并求解末端位姿
各位同学，欢迎来到第二章。今天咱们聊聊正运动学里最经典的工具——D-H参数法。
说实话，我刚入行那会儿，看到一堆连杆和关节，脑子里全是浆糊。后来带我的老工程师扔给我一句话：「别盯着机器人看，盯着坐标系看。」这句话我记到现在。D-H参数法，说白了就是给每个关节和连杆都贴上「身份证」，然后通过一套标准流程，算出末端执行器在空间里的精确位置和姿态。
为什么人形机器人需要D-H参数？
人形机器人有几十个自由度，腿、胳膊、躯干、脖子……每个关节都在动。你想想看，如果每个关节都单独算位置，那代码得写成什么样？
D-H参数法的好处在于：标准化。不管你是六轴机械臂，还是人形机器人的一条腿，只要按照四个参数（a、α、d、θ）来描述相邻连杆的关系，就能用统一的矩阵乘法算出末端位姿。
核心思想：把复杂的空间几何问题，转化为四个参数的代数运算。你只需要填参数，剩下的交给矩阵。
D-H参数的四个「身份证」
每个连杆坐标系之间的变换，由四个参数决定。我习惯把它们分成两组：
- 连杆长度 ai：沿 xi 轴，从 zi-1 到 zi 的距离。说白了就是两个关节轴之间的公垂线长度。
- 连杆扭角 αi：绕 xi 轴，从 zi-1 到 zi 的转角。这是两个关节轴之间的夹角。
- 关节偏距 di：沿 zi-1 轴，从 xi-1 到 xi 的距离。对于移动关节，这个值是变量。
- 关节转角 θi：绕 zi-1 轴，从 xi-1 到 xi 的转角。对于旋转关节，这个值是变量。
| 参数 | 符号 | 物理含义 | 关节类型 |
|---|---|---|---|
| 连杆长度 | ai | 两关节轴公垂线长度 | 固定值 |
| 连杆扭角 | αi | 两关节轴夹角 | 固定值 |
| 关节偏距 | di | 沿z轴偏移量 | 移动关节为变量 |
| 关节转角 | θi | 绕z轴旋转角 | 旋转关节为变量 |
我的小习惯：每次建立D-H表之前，我都会先画一张连杆坐标系的草图。哪怕只是随手画在草稿纸上，也能避免后面矩阵乘到一半发现符号搞反了。我曾经因为一个α的正负号没注意，调试了整整两天……
建立连杆坐标系的「三步法」
我个人总结了一套流程，每次用D-H参数都走这三步，基本不会出错：
- 找关节轴：画出每个关节的旋转轴（z轴）。人形机器人的髋关节、膝关节、踝关节，每个关节都有一条轴。
- 定坐标系原点：相邻两个z轴的交点，或者公垂线与z轴的交点，就是坐标系原点。
- 确定x轴方向：x轴沿着公垂线方向，从当前z轴指向下一个z轴。
嗯，这里要注意：人形机器人的腿部和手臂，关节排列方式不太一样。腿部通常是「髋-膝-踝」串联，而手臂是「肩-肘-腕」。但D-H参数法一视同仁，你只要按顺序标好坐标系就行。
从参数到矩阵：齐次变换
有了四个参数，相邻连杆的变换矩阵长这样：
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θ |
| 0 sinα cosα d |
| 0 0 0 1 |
这个矩阵，我建议你手推一遍。别偷懒。我当年就是对着书推了三遍，才真正理解每个元素为什么在那个位置。你想想看，如果连矩阵怎么来的都不清楚，后面做轨迹跟踪时，误差分析根本无从下手。
人形机器人腿部D-H参数示例
咱们拿人形机器人的左腿举个例。假设髋关节有3个自由度（横滚、俯仰、偏航），膝关节1个自由度（俯仰），踝关节2个自由度（横滚、俯仰）。
从骨盆到脚底，我习惯这样编号：
- 连杆0：骨盆（基座）
- 连杆1：髋关节横滚
- 连杆2：髋关节俯仰
- 连杆3：髋关节偏航
- 连杆4：膝关节俯仰
- 连杆5：踝关节俯仰
- 连杆6：踝关节横滚 → 脚底
对应的D-H参数表（部分示例）：
| 连杆 i | ai (mm) | αi (rad) | di (mm) | θi (变量) |
|---|---|---|---|---|
| 1 | 0 | -π/2 | 0 | q1 |
| 2 | Lthigh | 0 | 0 | q2 |
| 3 | 0 | -π/2 | 0 | q3 |
| 4 | Lshank | 0 | 0 | q4 |
避坑提醒：人形机器人的关节顺序和正负方向，不同厂家定义可能不同。我曾经在项目里直接套用别人的D-H表，结果仿真时膝盖反着弯……一定要对照你实际机器人的关节限位和安装方向，确认每个参数的正负号。
末端位姿求解：矩阵连乘
有了每个相邻连杆的变换矩阵 Ti，从基座到末端执行器的总变换就是：
T_0n = T_01 * T_12 * T_23 * ... * T_(n-1)n
这个矩阵里，左上角3×3是旋转矩阵，表示末端的姿态；右上角3×1是位置向量，表示末端在基座坐标系下的坐标。
举个例子，如果你想知道左脚在骨盆坐标系下的位置和朝向，就把从骨盆到脚底的所有T矩阵乘起来。代码实现很简单：
import numpy as np
def dh_transform(a, alpha, d, theta):
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
# 示例：计算左腿末端位姿
T_01 = dh_transform(0, -np.pi/2, 0, q1)
T_12 = dh_transform(L_thigh, 0, 0, q2)
T_23 = dh_transform(0, -np.pi/2, 0, q3)
T_34 = dh_transform(L_shank, 0, 0, q4)
T_04 = T_01 @ T_12 @ T_23 @ T_34
position = T_04[:3, 3]
rotation = T_04[:3, :3]
你看，代码就这么几行。但背后的坐标系建立和参数标定，才是真正花时间的地方。
知识体系总览
下面这张图，我把D-H参数法的核心逻辑画出来了。你可以把它当作一个「导航图」：
实际项目中的几点体会
最后，分享几个我在实际项目中踩过的坑：
- 坐标系方向一致性：人形机器人左右腿的坐标系，最好镜像对称。我见过一个团队，左腿用右手系，右腿用左手系，结果走路仿真时两条腿「打架」。
- 参数标定误差：D-H参数里的a和d，理论值和实际值可能有几毫米偏差。别小看这几毫米，在脚底累积起来，机器人站都站不稳。我建议做一次参数标定，用激光跟踪仪或者视觉系统实测。
- 奇异性问题：当相邻两个关节轴平行时，D-H参数会出现退化。人形机器人的膝关节和髋关节俯仰轴，如果完全平行，公垂线就不唯一了。这时候需要微调坐标系定义，或者改用改进型D-H参数。
一个小技巧：在写代码之前，先用纸笔算一遍末端位姿的解析表达式。哪怕只算到第三个关节，也能帮你验证矩阵乘法的中间结果。我每次做新机器人的运动学，都会先手算，再写代码，最后用仿真验证。三步下来，基本不会出大问题。
好了，这一章的内容就到这里。D-H参数法看起来是纯数学，但它是连接「关节角度」和「末端位姿」的桥梁。后面做轨迹规划、运动控制，都离不开这个基础。希望你能亲手建一个坐标系、填一张D-H表、写一段矩阵连乘的代码——做过一遍，才算真正学会。