📚 课程目录第4章正运动学（Forward Kinematics）

4. 正运动学（Forward Kinematics）

正运动学解决的是这样一个核心问题：给定机械臂各关节的关节变量（旋转角度或平移距离），求解末端执行器在基坐标系下的位置和姿态。它是机械臂运动控制、轨迹规划与路径规划的基础——无论是笛卡尔空间轨迹插值，还是关节空间速度规划，最终都需要通过正运动学将关节状态映射到操作空间。D-H（Denavit-Hartenberg）参数法是目前工业机器人和学术研究中最通用的运动学建模方法，它用四个参数（a、α、d、θ）描述相邻连杆之间的几何关系，从而将复杂的空间几何变换转化为一系列齐次变换矩阵的连乘。

D-H参数法的核心思想是将相邻连杆坐标系之间的变换分解为四个基本子变换，每个子变换只依赖一个参数。这四个参数的定义如下：a_i（连杆长度）是沿X_i轴从Z_i-1移动到Z_i的距离；α_i（连杆扭转角）是绕X_i轴从Z_i-1旋转到Z_i的角度；d_i（连杆偏距）是沿Z_i-1轴从X_i-1移动到X_i的距离；θ_i（关节角）是绕Z_i-1轴从X_i-1旋转到X_i的角度。对于旋转关节，θ_i是变量；对于移动关节，d_i是变量。

4.1 建立连杆坐标系的规则与步骤

建立正确的连杆坐标系是D-H参数法成功应用的前提。坐标系建立遵循以下规则：首先找出所有关节轴，并画出关节轴的延长线；然后确定各连杆的公共法线——对于相邻关节轴i-1和i，其公垂线或交点决定了连杆坐标系{i-1}的原点和X轴方向。具体步骤如下：

确定Z轴：坐标系{i}的Z_i轴与关节轴i重合，方向任意但需保持一致。
确定原点：坐标系{i}的原点位于Z_i与Z_i-1的公垂线与Z_i的交点处；若两轴相交，则原点取在交点处。
确定X轴：X_i沿公垂线方向由关节i指向关节i+1；若Z_i-1与Z_i平行，则X_i可任意选择使参数简化。
确定Y轴：由右手定则确定Y_i = Z_i × X_i。
基坐标系：基坐标系{0}固定不动，Z₀沿关节1轴线方向，当关节变量为0时，坐标系{0}与{1}重合。

经验提醒：在建立坐标系时，尽量使相邻坐标系的X轴方向一致或平行，这样可以简化D-H参数，使多个参数为0或90°的倍数，减少计算量。例如，对于常见的6轴工业机器人，通常选择使α参数为±90°或0°，这样变换矩阵中的三角函数值会变得非常简单。

4.2 相邻连杆变换矩阵的推导

有了D-H参数和坐标系定义后，相邻坐标系{i-1}到{i}的齐次变换矩阵^i-1T_i可以通过四个基本变换的连乘得到：先绕Z_i-1轴旋转θ_i，再沿Z_i-1轴平移d_i，然后沿X_i轴平移a_i，最后绕X_i轴旋转α_i。其标准矩阵形式为：

i-1T_i = Rot(z, θ_i) · Trans(z, d_i) · Trans(x, a_i) · Rot(x, α_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          ]

这个4×4矩阵包含了旋转子矩阵R_3×3和平移向量P_3×1。旋转子矩阵描述了坐标系{i}相对于{i-1}的姿态，平移向量描述了坐标系{i}原点在{i-1}中的位置。整个矩阵的推导过程体现了D-H参数法的精髓：将复杂的空间运动分解为绕轴旋转和沿轴平移的简单组合。

4.3 正运动学计算实例：三轴机械臂末端位姿求解

为了深入理解D-H参数法的实际应用，我们以一个三轴机械臂为例进行完整计算。该机械臂结构为：基座高度d₁=0.3m，连杆长度a₂=0.5m，a₃=0.4m，末端工具长度d₄=0.1m。各关节角度给定为θ₁=30°，θ₂=45°，θ₃=60°。D-H参数表如下：

关节i	a_i (m)	α_i (°)	d_i (m)	θ_i (°)
1	0	90	0.3	30
2	0.5	0	0	45
3	0.4	0	0	60
4（工具）	0	-90	0.1	0

首先计算各相邻变换矩阵。对于关节1，将θ₁=30°，α₁=90°，a₁=0，d₁=0.3代入标准矩阵：

0T_1 = [ 0.866  0     0.5   0    ]
       [ 0.5    0    -0.866 0    ]
       [ 0      1     0     0.3  ]
       [ 0      0     0     1    ]

对于关节2，θ₂=45°，α₂=0°，a₂=0.5，d₂=0：

1T_2 = [ 0.707  -0.707  0   0.354 ]
       [ 0.707   0.707  0   0.354 ]
       [ 0       0      1   0     ]
       [ 0       0      0   1     ]

对于关节3，θ₃=60°，α₃=0°，a₃=0.4，d₃=0：

2T_3 = [ 0.5   -0.866  0   0.2   ]
       [ 0.866  0.5    0   0.346 ]
       [ 0      0      1   0     ]
       [ 0      0      0   1     ]

对于工具坐标系，θ₄=0°，α₄=-90°，a₄=0，d₄=0.1：

3T_4 = [ 1   0   0   0   ]
       [ 0   0   1   0   ]
       [ 0  -1   0   0.1 ]
       [ 0   0   0   1   ]

将四个矩阵依次相乘得到末端位姿：

0T_4 = 0T_1 · 1T_2 · 2T_3 · 3T_4

     = [ 0.435  -0.866  0.25   0.612 ]
       [ 0.75    0.5    0.433  0.716 ]
       [ -0.5    0      0.866  0.3   ]
       [ 0       0      0      1     ]

因此，末端执行器在基坐标系下的位置为P = (0.612m, 0.716m, 0.3m)，姿态由旋转矩阵R表示。这个结果可以直接用于后续的轨迹规划——例如，若要在笛卡尔空间规划一条从当前末端位置到目标位置的直线轨迹，就需要反复调用正运动学将关节角转换为末端位置。

案例复盘：在上述计算中，我们注意到关节2和关节3的α参数为0°，这使得变换矩阵中的sinα项为0，cosα项为1，大大简化了矩阵乘法。实际工程中，大多数工业机器人的肩肘关节都设计为平行轴结构，正是为了利用这一简化。此外，工具坐标系的引入使得末端执行器的实际位姿可以独立于机械臂本体进行标定和补偿，这是机器人离线编程和仿真中常用的技巧。

4.4 常见误区与工程实践要点

在应用D-H参数法时，工程师常犯以下几类错误，需要特别注意：

坐标系方向选择不一致：Z轴方向选择随意导致α参数符号错误，进而影响整个变换矩阵。建议在建模初期就明确所有Z轴方向，并保持全局一致。
公垂线判断错误：当相邻关节轴平行时，公垂线不唯一；当两轴相交时，公垂线长度为0。这两种特殊情况需要特殊处理，否则a参数会出错。
矩阵乘法顺序错误：齐次变换矩阵的乘法不满足交换律，必须严格按照从基座到末端的顺序连乘。建议在代码中明确标注每个矩阵对应的坐标系对。
忽略工具坐标系：很多初学者只计算到最后一个关节，而忽略了末端工具的长度和姿态偏移。在实际应用中，工具坐标系变换必须包含在正运动学链中。

风险提示：在编写正运动学计算程序时，建议使用双精度浮点数，并定期进行数值验证——例如将各关节角置零，检查末端位姿是否与机械臂的物理零位一致。此外，对于奇异位形附近的计算，矩阵条件数会急剧增大，此时需要特别注意数值稳定性问题。

正运动学作为机器人学的基石，其重要性不仅体现在理论计算上，更在于它为后续的逆运动学求解、速度雅可比矩阵推导、动力学建模以及轨迹规划提供了统一的数学框架。掌握D-H参数法的精髓，意味着能够为任意结构的串联机械臂建立规范的运动学模型，这是从事机器人控制与自动化工程实践的核心能力之一。