2、正运动学求解：基于DH参数的连杆变换推导、MATLAB/Python符号计算实现
正运动学，说白了就是：给定关节角度，算出末端位姿。
我刚开始接触机器人时，觉得这玩意儿挺玄乎。后来做项目多了才发现，它其实就是一串矩阵乘法。你给每个关节一个变换矩阵，乘起来就完了。嗯，道理是这么个道理，但真正落地时，坑也不少。
2.1 DH参数：机器人建模的“通用语言”
DH参数法，全称Denavit-Hartenberg参数法。它用四个参数来描述相邻两个连杆之间的相对位置和姿态。
我个人习惯把DH参数分成两类：
- 标准DH（Standard DH）：坐标系建在连杆的前一个关节上。用得最多，但容易搞混。
- 改进DH（Modified DH）：坐标系建在连杆的后一个关节上。我个人觉得更直观，尤其在处理树形结构时。
不管哪种，四个参数都是：
| 参数 | 符号 | 含义 |
|---|---|---|
| 连杆长度 | ai-1 | 沿Xi-1轴，从Zi-1到Zi的距离 |
| 连杆扭角 | αi-1 | 绕Xi-1轴，从Zi-1到Zi的转角 |
| 连杆偏距 | di | 沿Zi轴，从Xi-1到Xi的距离 |
| 关节转角 | θi | 绕Zi轴，从Xi-1到Xi的转角 |
我的小技巧： 画图时，先把Z轴画出来（关节轴线方向），再画X轴（公垂线方向）。Y轴用右手定则补上。这样不容易乱。
2.2 连杆变换矩阵：从关节到末端的“传送带”
有了DH参数，相邻连杆的变换矩阵就是固定的。标准DH的变换矩阵长这样：
i-1T_i = Rot_x(α_{i-1}) · Trans_x(a_{i-1}) · Rot_z(θ_i) · Trans_z(d_i)展开成4x4矩阵：
| cosθ_i -sinθ_i 0 a_{i-1} |
| sinθ_i·cosα_{i-1} cosθ_i·cosα_{i-1} -sinα_{i-1} -d_i·sinα_{i-1} |
| sinθ_i·sinα_{i-1} cosθ_i·sinα_{i-1} cosα_{i-1} d_i·cosα_{i-1} |
| 0 0 0 1 |你想想看，这个矩阵里只有θi是变量（旋转关节），其他都是常数。所以正运动学本质上就是：把每个关节的θi代进去，然后连乘。
注意： 矩阵乘法不满足交换律！顺序错了，结果全错。我曾经在调试一个六轴机器人时，把第3和第4关节的变换顺序搞反了，末端位置差了十几厘米。查了整整两天才找到问题。
2.3 符号计算：让MATLAB和Python帮你干活
手算6个关节的矩阵乘法？别傻了。我建议你用符号计算工具。
2.3.1 MATLAB实现
MATLAB的Symbolic Math Toolbox很好用。我个人习惯用syms定义符号变量，然后用simplify化简。
% 定义符号变量
syms theta1 theta2 theta3 theta4 theta5 theta6
syms a1 a2 a3 a4 a5 a6 d1 d2 d3 d4 d5 d6 alpha1 alpha2 alpha3 alpha4 alpha5 alpha6
% 定义DH变换函数
function T = dh_transform(a, alpha, d, theta)
T = [cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta);
sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta);
0, sin(alpha), cos(alpha), d;
0, 0, 0, 1];
end
% 计算各连杆变换
T01 = dh_transform(a1, alpha1, d1, theta1);
T12 = dh_transform(a2, alpha2, d2, theta2);
T23 = dh_transform(a3, alpha3, d3, theta3);
T34 = dh_transform(a4, alpha4, d4, theta4);
T45 = dh_transform(a5, alpha5, d5, theta5);
T56 = dh_transform(a6, alpha6, d6, theta6);
% 正运动学：末端位姿
T06 = T01 * T12 * T23 * T34 * T45 * T56;
T06_simplified = simplify(T06);
% 提取位置和姿态
position = T06_simplified(1:3, 4);
rotation = T06_simplified(1:3, 1:3);
关键点： 用
simplify化简后，表达式会短很多。但有时候化简过头反而看不懂，我一般会保留中间结果，方便调试。
2.3.2 Python实现
Python里我用SymPy库。语法和MATLAB很像，但更灵活。
import sympy as sp
# 定义符号变量
theta1, theta2, theta3 = sp.symbols('theta1 theta2 theta3')
a1, a2, a3 = sp.symbols('a1 a2 a3')
d1, d2, d3 = sp.symbols('d1 d2 d3')
alpha1, alpha2, alpha3 = sp.symbols('alpha1 alpha2 alpha3')
def dh_transform(a, alpha, d, theta):
return sp.Matrix([
[sp.cos(theta), -sp.sin(theta)*sp.cos(alpha), sp.sin(theta)*sp.sin(alpha), a*sp.cos(theta)],
[sp.sin(theta), sp.cos(theta)*sp.cos(alpha), -sp.cos(theta)*sp.sin(alpha), a*sp.sin(theta)],
[0, sp.sin(alpha), sp.cos(alpha), d],
[0, 0, 0, 1]
])
# 计算变换矩阵
T01 = dh_transform(a1, alpha1, d1, theta1)
T12 = dh_transform(a2, alpha2, d2, theta2)
T23 = dh_transform(a3, alpha3, d3, theta3)
# 正运动学
T03 = T01 * T12 * T23
sp.simplify(T03)
避坑指南： 我曾经用Python算一个7自由度机器人的正运动学，结果表达式长达几十页。后来发现是没做
trigsimp（三角化简）。加上之后，表达式缩到了半页。记住：符号计算一定要化简。
2.4 实战：一个六轴机器人的正运动学
拿一个典型的六轴工业机器人举例。它的DH参数表如下：
| 关节i | ai-1 (mm) | αi-1 (rad) | di (mm) | θi (变量) |
|---|---|---|---|---|
| 1 | 0 | 0 | 400 | θ1 |
| 2 | 250 | -π/2 | 0 | θ2 |
| 3 | 900 | 0 | 0 | θ3 |
| 4 | 150 | -π/2 | 900 | θ4 |
| 5 | 0 | π/2 | 0 | θ5 |
| 6 | 0 | -π/2 | 200 | θ6 |
代入符号计算，得到末端位置：
px = cos(θ1)*(a2*cos(θ2) + a3*cos(θ2+θ3) + d4*sin(θ2+θ3) + d6*(sin(θ5)*sin(θ2+θ3) + cos(θ5)*cos(θ2+θ3)*cos(θ4)))
py = sin(θ1)*(a2*cos(θ2) + a3*cos(θ2+θ3) + d4*sin(θ2+θ3) + d6*(sin(θ5)*sin(θ2+θ3) + cos(θ5)*cos(θ2+θ3)*cos(θ4)))
pz = d1 - a2*sin(θ2) - a3*sin(θ2+θ3) + d4*cos(θ2+θ3) + d6*(sin(θ5)*cos(θ2+θ3) - cos(θ5)*sin(θ2+θ3)*cos(θ4))
看着挺长，对吧？但这就是正运动学的核心。你给一组关节角度，就能算出末端位置。
我的经验： 实际项目中，我不会每次都从头算符号表达式。我会把符号计算的结果保存成函数，然后直接调用。这样既快又不容易出错。
2.5 验证：用数值算例检查
算完了怎么知道对不对？拿一组已知的关节角度，手算一个简单情况来验证。
比如，让所有关节角度为0：
θ1=0, θ2=0, θ3=0, θ4=0, θ5=0, θ6=0代入上面的表达式，得到：
px = a2 + a3 + d6
py = 0
pz = d1 + d4这个结果符合直觉：机器人完全伸直，末端在X轴正方向，Z轴高度为d1+d4。
注意： 如果验证结果不对，先检查DH参数表。我遇到过好几次，参数表抄错了，导致后面全白算。记住：Garbage in, garbage out。
2.6 总结
正运动学不难，就是矩阵乘法。但要做好，得注意三点：
- DH参数要准：画图、标参数、反复核对。
- 符号计算要化简：不然表达式长得吓人。
- 数值验证不能省：一组简单的关节角度，手算验证。
下一章，我们聊逆运动学。那才是真正让人头疼的地方。不过别担心，先把正运动学吃透，后面就好办了。