4、正运动学（Forward Kinematics）：D-H参数法建模、连杆坐标系建立、正运动学方程推导、典型六轴机械臂正解计算

4.1 D-H参数法核心原理

D-H参数法由Denavit和Hartenberg于1955年提出，其核心思想是在机械臂的每个连杆上固接一个坐标系，通过四个参数描述相邻连杆坐标系之间的相对位姿关系。这四个参数分别是：连杆长度a_i-1（沿X_i-1轴从Z_i-1移动到Z_i的距离）、连杆扭转角α_i-1（绕X_i-1轴从Z_i-1旋转到Z_i的角度）、连杆偏距d_i（沿Z_i轴从X_i-1移动到X_i的距离）、关节角θ_i（绕Z_i轴从X_i-1旋转到X_i的角度）。其中，θ_i为旋转关节的变量，d_i为移动关节的变量。

D-H参数法的优势在于：它将复杂的空间几何关系转化为四个标量参数，使得机械臂的运动学建模变得系统化和程序化。无论是串联机械臂、并联机构还是移动机器人，只要满足D-H建模的约束条件（相邻关节轴线要么相交、要么平行、要么垂直），都可以使用该方法建立运动学模型。对于六轴工业机器人，D-H参数法几乎是行业标准，几乎所有主流机器人厂商（如ABB、KUKA、FANUC）的控制器内部都采用D-H模型进行运动学解算。

4.2 连杆坐标系建立与D-H参数提取

建立连杆坐标系是D-H建模的第一步，也是最容易出错的一步。标准D-H法（Craig版本）的坐标系建立规则如下：

1. 确定关节轴线：找出所有关节的旋转轴线，记为Z_i轴方向（右手定则）。
2. 确定原点位置：对于关节i，其坐标系原点位于Z_i轴与Z_i+1轴的公垂线与Z_i轴的交点处。
3. 确定X轴方向：X_i轴沿Z_i轴与Z_i+1轴的公垂线方向，从关节i指向关节i+1。
4. 确定Y轴方向：由右手定则确定，Y_i = Z_i × X_i。
5. 基座坐标系{0}：固定不动，Z₀沿关节1轴线方向，通常与基座底面重合。
6. 末端坐标系{n}：通常与最后一个关节的坐标系重合，或根据工具（Tool）进行偏移。

以典型的六轴垂直关节型工业机器人为例（如KUKA KR系列、FANUC M-20iA），其D-H参数表通常如下：

注意：不同厂商的D-H参数定义可能存在差异（标准D-H vs 改进D-H），在建模时必须明确采用哪种约定。标准D-H（Classic）将参数标注在连杆前端，改进D-H（Modified）标注在连杆后端。对于同一台机器人，两种方法得到的参数表不同，但最终的正运动学结果一致。

4.3 正运动学方程推导

得到D-H参数表后，相邻坐标系{i-1}到{i}的齐次变换矩阵为：

T_i^(i-1) = 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         |

将每个关节的变换矩阵依次相乘，即可得到末端坐标系相对于基座坐标系的位姿：

T_6^0 = T_1^0 · T_2^1 · T_3^2 · T_4^3 · T_5^4 · T_6^5

其中，T_6^0的左上角3×3子矩阵表示末端的姿态（旋转矩阵R），右上角3×1子矩阵表示末端的位置（平移向量P）。在实际工程中，通常将旋转矩阵转换为欧拉角（如ZYX或ZYZ）或四元数，以便于控制器使用。

4.4 典型六轴机械臂正解计算案例

下面以一个具体的六轴机械臂为例，演示完整的正解计算过程。假设某型号机器人D-H参数如下：

• d₁ = 400mm, a₁ = 200mm, a₂ = 600mm, d₄ = 500mm, d₆ = 150mm
• α₁ = -90°, α₄ = -90°, α₅ = 90°, α₆ = -90°
• 初始关节角：θ₁ = 0°, θ₂ = -90°, θ₃ = 0°, θ₄ = 0°, θ₅ = 0°, θ₆ = 0°

第一步：计算各关节的变换矩阵。以关节1为例（θ₁=0°, α₀=0°, a₀=0, d₁=400）：

T_1^0 = | 1   0    0   0 |
        | 0   1    0   0 |
        | 0   0    1  400|
        | 0   0    0   1 |

关节2（θ₂=-90°, α₁=-90°, a₁=200, d₂=0）：

T_2^1 = | cos(-90°)  -sin(-90°)·cos(-90°)   sin(-90°)·sin(-90°)   200·cos(-90°) |
        | sin(-90°)   cos(-90°)·cos(-90°)  -cos(-90°)·sin(-90°)   200·sin(-90°) |
        | 0           sin(-90°)              cos(-90°)              0            |
        | 0           0                      0                      1            |

       = | 0   0   -1  -200 |
         | -1  0    0   0   |
         | 0   1    0   0   |
         | 0   0    0   1   |

同理计算T_3^2、T_4^3、T_5^4、T_6^5，然后依次相乘得到T_6^0。最终计算结果为：

T_6^0 = | 0   0   1   800 |
        | 0  -1   0     0 |
        | 1   0   0   400 |
        | 0   0   0     1 |

即末端位置为(800, 0, 400)mm，姿态为：X轴指向基座Z方向，Y轴指向基座-Y方向，Z轴指向基座X方向。这个结果可以通过几何直觉验证：当机械臂处于“直立”状态时，末端应位于基座正上方偏前的位置。

4.5 正运动学的工程验证与仿真

在实际工程中，正运动学计算完成后必须进行验证。常用的验证方法包括：

1. 零点位姿验证：将所有关节角置零，检查末端位姿是否与机械臂的机械零点一致。
2. 单轴运动验证：只移动一个关节，观察末端轨迹是否为一个圆弧（旋转关节）或直线（移动关节）。
3. 已知位姿反推：选取几个特殊位姿（如机械臂完全展开、完全折叠），用几何方法计算末端位置，与正解结果对比。
4. 与示教器对比：在真实机器人上示教几个点，记录关节角，用正解计算末端位姿，与示教器显示的位姿对比。
5. 仿真软件验证：使用RoboDK、MATLAB Robotics Toolbox或CoppeliaSim导入机器人模型，对比仿真结果。

以MATLAB Robotics Toolbox为例，验证上述计算：

% 定义D-H参数（标准D-H）
L(1) = Link([0 400 0 0]);
L(2) = Link([0 0 200 -pi/2]);
L(3) = Link([0 0 600 0]);
L(4) = Link([0 500 0 -pi/2]);
L(5) = Link([0 0 0 pi/2]);
L(6) = Link([0 150 0 -pi/2]);

robot = SerialLink(L, 'name', '6DOF');
q = [0 -pi/2 0 0 0 0];  % 关节角
T = robot.fkine(q);      % 正运动学计算
disp(T);

运行结果应与手算结果一致。如果存在偏差，应逐项检查每个T_i^(i-1)矩阵的计算过程。

4.6 总结与常见误区复盘

正运动学是机器人学中最基础、最核心的内容之一。D-H参数法提供了一套标准化的建模流程，但实际应用中仍存在诸多容易出错的地方。以下是本章的要点总结：

正运动学虽然相对简单，但它是逆运动学、轨迹规划、动力学控制的基础。只有建立精确的正运动学模型，后续的算法才能可靠运行。建议读者在掌握本章内容后，尝试用Python或MATLAB编写一个完整的正运动学求解器，并针对不同型号的机器人进行参数化测试，这将极大提升对D-H建模的理解深度。