3. 正运动学：D-H参数法建模与连杆坐标系建立
正运动学，说白了就是：给定六个关节角度，算出末端在空间的位置和姿态。这是机器人控制的"基本功"，也是后面做轨迹规划、动力学分析的基础。
我个人习惯用D-H参数法来做这件事。为什么？因为它把复杂的空间几何问题，变成了四个参数的矩阵连乘。你想想看，一个六轴机械臂，六个关节，每个关节四个参数，总共24个参数就能完整描述整个臂的几何关系——这比用欧拉角硬算要优雅得多。
3.1 连杆坐标系建立规则
先说说坐标系怎么建。我记得刚入行时，师傅跟我说："坐标系建对了，后面全是矩阵乘法；建错了，调试三天都找不出问题。"这话一点不假。
D-H法建立连杆坐标系，有四个核心规则：
- Z轴：沿着关节轴线方向。旋转关节就是旋转轴，移动关节就是移动方向。
- X轴：沿着相邻两Z轴的公垂线，从当前关节指向下一个关节。
- Y轴：由右手定则确定，Z×X得到Y。
- 原点：Z轴与公垂线的交点。
关键点：坐标系{i}固定在连杆i上，随连杆i一起运动。坐标系{i}的原点位于关节i+1的轴线上。
嗯，这里要注意一个容易混淆的地方：坐标系编号和关节编号是错位的。关节i连接连杆i-1和连杆i，而坐标系{i}是固定在连杆i上的。我刚开始学的时候，这个错位让我画图画错了好几次。
3.2 D-H四参数定义
有了坐标系，接下来就是四个参数。每个参数都有明确的几何意义：
| 参数 | 符号 | 物理含义 | 我的理解 |
|---|---|---|---|
| 连杆长度 | ai | Zi到Zi+1沿Xi的距离 | 说白了就是"杆子有多长" |
| 连杆扭角 | αi | Zi到Zi+1绕Xi的转角 | "杆子拧了多少度" |
| 关节距离 | di | Xi-1到Xi沿Zi的距离 | "关节之间的偏置" |
| 关节转角 | θi | Xi-1到Xi绕Zi的转角 | "关节转了多少度" |
对于旋转关节，θi是变量，其他三个是常量。对于移动关节，di是变量。六轴机械臂通常全是旋转关节，所以只有θ1到θ6是变量。
我的经验：参数a和α描述的是"杆子本身的几何"，而d和θ描述的是"关节连接关系"。把这两个概念分开，参数就不容易搞混。
3.3 六轴机械臂D-H参数表
以典型的六轴工业机械臂为例（比如常见的ER系列），D-H参数表长这样：
| 关节i | ai (mm) | αi (°) | di (mm) | θi (°) |
|---|---|---|---|---|
| 1 | 0 | 0 | d1 | θ1 |
| 2 | a2 | -90 | 0 | θ2 |
| 3 | a3 | 0 | 0 | θ3 |
| 4 | 0 | -90 | d4 | θ4 |
| 5 | 0 | 90 | 0 | θ5 |
| 6 | 0 | -90 | d6 | θ6 |
注意看，关节1的a1=0，α1=0，说明基座到第一个关节没有偏置。关节2的α2=-90°，表示第二个关节轴线相对于第一个关节扭转了90度——这就是为什么大多数六轴臂的"肩膀"是弯的。
避坑指南：我曾经在项目里把α的正负号搞反了，结果仿真时机械臂末端轨迹完全不对。后来花了整整一天排查，才发现是符号问题。记住：α的正负由右手定则决定，绕X轴逆时针为正。
3.4 正运动学方程推导
有了参数表，接下来就是矩阵连乘。每个关节对应一个齐次变换矩阵：
T_i = Rot(z, θ_i) * Trans(z, d_i) * Trans(x, a_i) * Rot(x, α_i)
展开成4×4矩阵形式：
T_i = [cosθ_i -sinθ_i·cosα_i sinθ_i·sinα_i a_i·cosθ_i]
[sinθ_i cosθ_i·cosα_i -cosθ_i·sinα_i a_i·sinθ_i]
[0 sinα_i cosα_i d_i ]
[0 0 0 1 ]
这个矩阵就是相邻两个坐标系之间的变换关系。从基座到末端，总共六个矩阵连乘：
T_06 = T_01 * T_12 * T_23 * T_34 * T_45 * T_56
结果是一个4×4的齐次变换矩阵：
T_06 = [R_06 P_06]
[0 0 0 1 ]
其中R_06是3×3的旋转矩阵，表示末端的姿态；P_06是3×1的位置向量，表示末端在基坐标系下的坐标。
你想想看，六个矩阵连乘，每个矩阵里都是sin和cos的组合，展开后会有几十项。手算？不现实。我一般用MATLAB的符号工具箱或者Python的SymPy来做符号推导，然后生成C代码烧到控制器里。
3.5 正运动学计算流程
实际工程中，正运动学的计算流程是这样的：
- 输入：六个关节角度θ1~θ6
- 查表：从D-H参数表中取出对应的ai、αi、di
- 构造矩阵：对每个关节，代入θi计算Ti-1,i
- 连乘：T_06 = T_01 × T_12 × T_23 × T_34 × T_45 × T_56
- 输出：从T_06中提取位置P_06和姿态R_06
工程实现要点：
- 矩阵乘法用循环实现，不要手写6次乘法——容易出错且不好维护
- sin/cos计算用查表或快速近似，减少实时计算开销
- 结果矩阵的最后一列前三个元素就是末端位置，单位与D-H参数一致
3.6 正运动学验证
写完代码怎么验证对不对？我有个习惯：先做零位验证。把所有关节角度设为零，看末端位置是不是在机械臂的"零位姿态"。
比如，对于上面那张参数表，当所有θ=0时：
- 关节1和关节2的轴线共面
- 关节3水平伸出
- 关节4、5、6形成一条直线
末端位置应该是：Px = a2 + a3 + d6，Py = 0，Pz = d1 - d4。如果算出来不对，那肯定是参数或者矩阵写错了。
我的调试技巧：先只算前三个关节（位置部分），验证末端位置对不对。位置对了再算后三个关节（姿态部分）。分步验证，问题定位快得多。
3.7 正运动学知识体系
下面这张图总结了正运动学的核心逻辑：
从这张图可以看得很清楚：正运动学就是一条从"关节角度"到"末端位姿"的单向通道。输入是六个角度，经过D-H参数和矩阵连乘，输出位置和姿态。这个通道是确定的、唯一的——给定一组关节角度，末端位姿是唯一确定的。
这也是为什么正运动学比逆运动学简单。逆运动学可能有多个解、甚至无解，但正运动学永远只有一个答案。嗯，这就是它的"确定性"优势。
3.8 代码实现示例
最后，给出一段Python代码，实现六轴机械臂的正运动学计算。这段代码我在好几个项目里复用过，稍微改改参数表就能用：
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(joint_angles, dh_params):
"""
正运动学计算
joint_angles: [θ1, θ2, θ3, θ4, θ5, θ6]
dh_params: [[a1, α1, d1], [a2, α2, d2], ...]
"""
T = np.eye(4)
for i in range(6):
a, alpha, d = dh_params[i]
theta = joint_angles[i]
T_i = dh_transform(a, alpha, d, theta)
T = T @ T_i
return T
# 示例：D-H参数表（a单位mm，α单位rad）
dh_params = [
[0, 0, d1], # 关节1
[a2, -np.pi/2, 0 ], # 关节2
[a3, 0, 0 ], # 关节3
[0, -np.pi/2, d4], # 关节4
[0, np.pi/2, 0 ], # 关节5
[0, -np.pi/2, d6] # 关节6
]
# 零位验证
joints_zero = [0, 0, 0, 0, 0, 0]
T_06 = forward_kinematics(joints_zero, dh_params)
print("末端位置：", T_06[:3, 3])
print("末端姿态：\n", T_06[:3, :3])
这段代码的核心就两个函数：dh_transform构造单个关节的矩阵，forward_kinematics做连乘。你想想看，整个正运动学，本质上就是这么简单——24个参数，6次矩阵乘法，搞定。
但简单归简单，实际工程中坑不少。比如角度单位是弧度还是度？D-H参数是标准型还是改进型？这些细节不注意，算出来的结果就是错的。我建议你拿到一个机械臂，先用零位验证把代码跑通，再去做轨迹规划。