3、正运动学回顾：从关节空间到笛卡尔空间的映射，典型6轴机械臂正解推导

在逆运动学求解之前，我们必须先彻底理解正运动学。正运动学解决的是这样一个问题：给定机械臂各关节的角度（关节空间），求末端执行器的位置和姿态（笛卡尔空间）。逆运动学则是反过来的过程——给定末端目标位姿，反解关节角。如果正运动学理解不透彻，逆解时你将无法判断多解取舍、奇异位形和可达性。本章以典型6轴串联关节机械臂（如常见的ER3A-C60构型）为例，系统推导正运动学矩阵，并建立从关节空间到笛卡尔空间的完整映射框架。

上图展示了正运动学的核心链路：6个关节角作为输入，通过DH参数建立相邻坐标系变换，连乘得到末端位姿。下面我们逐步拆解这一过程。

3.1 核心原理：DH参数与齐次变换矩阵

正运动学的数学基础是Denavit-Hartenberg（DH）参数法。每个关节用4个参数描述：连杆长度aᵢ₋₁（沿Xᵢ₋₁轴从Zᵢ₋₁移动到Zᵢ的距离）、连杆扭转角αᵢ₋₁（绕Xᵢ₋₁轴从Zᵢ₋₁旋转到Zᵢ的角度）、连杆偏距dᵢ（沿Zᵢ轴从Xᵢ₋₁移动到Xᵢ的距离）、关节角θᵢ（绕Zᵢ轴从Xᵢ₋₁旋转到Xᵢ的角度）。对于旋转关节，θᵢ是变量，其余3个参数为常量。

相邻坐标系间的变换矩阵为：

Aᵢ = Rot(z, θᵢ) · Trans(z, dᵢ) · Trans(x, aᵢ₋₁) · Rot(x, αᵢ₋₁)

     | cosθᵢ  -sinθᵢ·cosαᵢ₋₁   sinθᵢ·sinαᵢ₋₁   aᵢ₋₁·cosθᵢ |
     | sinθᵢ   cosθᵢ·cosαᵢ₋₁  -cosθᵢ·sinαᵢ₋₁   aᵢ₋₁·sinθᵢ |
Aᵢ = | 0       sinαᵢ₋₁         cosαᵢ₋₁          dᵢ        |
     | 0       0               0                1         |

末端执行器相对于基座的总变换为：T₀₆ = A₁·A₂·A₃·A₄·A₅·A₆。这个4×4矩阵的左上3×3子矩阵是旋转矩阵R，右上3×1子矩阵是位置向量P。旋转矩阵的列向量分别代表末端坐标系在基坐标系中的X、Y、Z轴方向。

3.2 典型6轴机械臂DH参数表与坐标系建立

以常见的6轴关节型工业机械臂为例（如ER3A-C60构型），其DH参数表如下：

坐标系建立规则：Zᵢ轴沿关节i的旋转轴方向；Xᵢ轴沿Zᵢ和Zᵢ₊₁的公垂线方向，从Zᵢ指向Zᵢ₊₁；Yᵢ轴由右手定则确定。基坐标系{0}固定在底座，工具坐标系{6}固定在法兰盘末端。

3.3 正解推导过程：从A₁到T₀₆

我们逐步计算每个变换矩阵。以第1关节为例，a₀=0，α₀=0，d₁=330mm：

A₁ = | cosθ₁  -sinθ₁  0  0 |
     | sinθ₁   cosθ₁  0  0 |
     | 0       0      1  330 |
     | 0       0      0  1  |

第2关节，a₁=150mm，α₁=-90°，d₂=0：

A₂ = | cosθ₂  0  -sinθ₂  150·cosθ₂ |
     | sinθ₂  0   cosθ₂  150·sinθ₂ |
     | 0     -1   0      0        |
     | 0      0   0      1        |

第3关节，a₂=600mm，α₂=0°，d₃=0：

A₃ = | cosθ₃  -sinθ₃  0  600·cosθ₃ |
     | sinθ₃   cosθ₃  0  600·sinθ₃ |
     | 0       0      1  0        |
     | 0       0      0  1        |

第4关节，a₃=120mm，α₃=-90°，d₄=630mm：

A₄ = | cosθ₄  0  -sinθ₄  120·cosθ₄ |
     | sinθ₄  0   cosθ₄  120·sinθ₄ |
     | 0     -1   0      630      |
     | 0      0   0      1        |

第5关节，a₄=0，α₄=90°，d₅=0：

A₅ = | cosθ₅  0  sinθ₅  0 |
     | sinθ₅  0  -cosθ₅ 0 |
     | 0      1  0      0 |
     | 0      0  0      1 |

第6关节，a₅=0，α₅=-90°，d₆=75mm：

A₆ = | cosθ₆  -sinθ₆  0  0 |
     | sinθ₆   cosθ₆  0  0 |
     | 0       0      1  75 |
     | 0       0      0  1  |

将A₁到A₆连乘，得到T₀₆。由于矩阵乘法不满足交换律，顺序必须严格保持。实际工程中，我们通常用符号计算工具（如MATLAB Symbolic、Python SymPy）展开，避免手算错误。展开后，位置分量Px、Py、Pz是6个关节角的非线性函数，例如Px的一般形式为：

Px = cosθ₁·(a₁ + a₂·cosθ₂ + a₃·cos(θ₂+θ₃) + d₄·sin(θ₂+θ₃)) 
     + d₆·(sinθ₁·sinθ₄·sinθ₅ + cosθ₁·cosθ₄·sinθ₅·cos(θ₂+θ₃) 
     + cosθ₁·cosθ₅·sin(θ₂+θ₃))

这个表达式虽然复杂，但结构清晰：前3项是腕部中心位置，后3项是工具偏移量。

3.4 案例拆解：用Python验证正解

假设机械臂处于零位（所有关节角为0°），我们验证正解是否正确。零位时：

θ₁=θ₂=θ₃=θ₄=θ₅=θ₆=0°

代入DH参数：
A₁(0°) = | 1  0  0  0 |
         | 0  1  0  0 |
         | 0  0  1  330 |
         | 0  0  0  1  |

A₂(0°) = | 1  0  0  150 |
         | 0  0  1  0  |
         | 0 -1  0  0  |
         | 0  0  0  1  |

A₃(0°) = | 1  0  0  600 |
         | 0  1  0  0  |
         | 0  0  1  0  |
         | 0  0  0  1  |

A₄(0°) = | 1  0  0  120 |
         | 0  0  1  0  |
         | 0 -1  0  630 |
         | 0  0  0  1  |

A₅(0°) = | 1  0  0  0 |
         | 0  0 -1  0 |
         | 0  1  0  0 |
         | 0  0  0  1 |

A₆(0°) = | 1  0  0  0 |
         | 0  1  0  0 |
         | 0  0  1  75 |
         | 0  0  0  1  |

连乘后得到：

T₀₆ = | 1  0  0  870 |
      | 0  1  0  0  |
      | 0  0  1  405 |
      | 0  0  0  1  |

即末端位置为(870, 0, 405)mm，姿态为单位矩阵（与基座姿态一致）。这个结果符合物理直觉：零位时机械臂完全伸直，X方向总长为150+600+120=870mm，Z方向为330+75=405mm。

3.5 常见误区与总结复盘

误区一：混淆标准DH和改进DH。两种约定下，a和α的定义位置不同，导致同样的DH参数表得到完全不同的正解。解决方法是始终在代码注释中明确标注DH类型。

误区二：忽略关节角单位。三角函数在C/C++/Python中默认使用弧度，而机器人示教器通常显示角度。如果直接混用，会产生灾难性错误。建议在程序入口统一转换为弧度。

误区三：矩阵连乘顺序错误。T₀₆ = A₁·A₂·A₃·A₄·A₅·A₆，不能写成A₆·A₅·...·A₁。矩阵乘法不满足交换律，顺序颠倒会导致末端位置完全错误。

误区四：忽视工具坐标系变换。正解得到的是法兰盘中心位姿，实际加工点（如焊枪末端）还需要乘以工具变换矩阵T_tool。很多初学者直接用法兰盘位姿做逆解，导致实际执行时末端偏差。

误区五：奇异位形下正解仍然有效，但逆解会出问题。当腕部中心与关节4、6轴线重合时（θ₅=0°），正解矩阵仍然正确，但逆解时会出现无穷多解。理解这一点有助于后续处理逆解奇异问题。