2. 机械臂运动学建模

运动学建模是机械臂动力学与控制联合仿真的第一块基石。它回答两个最基本的问题：给定各关节角度，末端执行器在哪里（正运动学）；给定末端目标位姿，各关节应该转到什么角度（逆运动学）。只有把这两个映射关系建立准确，后续的雅可比矩阵、动力学方程、轨迹规划和控制器设计才有可靠的几何基础。本节围绕连杆参数、DH 法、正运动学、逆运动学与工作空间分析展开，并以一个 6 自由度串联机械臂为教学对象贯穿说明。

一、连杆参数：把机械结构抽象成四个数

串联机械臂由一系列刚性连杆通过关节连接而成。为了用统一的数学语言描述相邻连杆之间的相对位置，需要为每个连杆定义一组几何参数。最常用的是 Denavit-Hartenberg（DH）参数，它用四个量描述相邻两个关节轴之间的空间关系：

• 连杆长度 a_i：沿关节 i 的轴线方向，从关节 i 到关节 i+1 的公垂线距离。
• 连杆扭角 α_i：绕关节 i 的轴线，将关节 i 的轴线转到与关节 i+1 轴线平行所需的转角。
• 关节偏置 d_i：沿关节 i 的轴线，从上一连杆公垂线到本连杆公垂线的距离。
• 关节转角 θ_i：绕关节 i 的轴线，从上一连杆公垂线到本连杆公垂线的转角。

对于旋转关节，θ_i 是变量，其余三个是常量；对于移动关节，d_i 是变量。这四个参数把复杂的机械结构压缩成一张参数表，是后续所有计算的数据源头。需要特别强调的是：DH 参数的定义依赖坐标系约定，一旦选定标准 DH 或改进 DH，全文和仿真模型必须保持一致，否则正逆运动学会出现系统性偏差。

二、DH 法：建立连杆坐标系与齐次变换

标准 DH 法的坐标系建立规则是：Z_i 轴与关节 i+1 的轴线重合，X_i 轴沿关节 i 与关节 i+1 的公垂线方向，原点位于公垂线与 Z_i 轴的交点。按此规则，相邻坐标系之间的齐次变换矩阵可以写成四个基本变换的乘积：

T_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 矩阵同时包含旋转和平移信息。把从基座到末端的 n 个变换依次相乘，就得到末端执行器在基坐标系下的位姿：T_0n = T₁·T₂·…·T_n。这一步就是正运动学的核心计算。

三、正运动学：从关节角到末端位姿

正运动学是“已知关节变量，求末端位姿”的过程，计算直接、无歧义，适合在仿真中实时求解。以一个典型的 6 自由度机械臂为例，假设其 DH 参数如下（示例参数，用于说明计算流程）：

关节 i	ai (mm)	αi (°)	di (mm)	θi 范围 (°)
1	0	90	150	-170 ~ 170
2	430	0	0	-120 ~ 120
3	40	90	0	-170 ~ 170
4	0	-90	430	-190 ~ 190
5	0	90	0	-120 ~ 120
6	0	0	80	-360 ~ 360

给定一组关节角，例如 θ = [0°, 30°, -45°, 0°, 60°, 0°]，按上述矩阵逐级相乘，即可得到末端位置和姿态。在仿真中，通常把这一计算封装成函数，输入为 6 维关节向量，输出为 4×4 位姿矩阵或位置加四元数。正运动学的验证方法是：将计算结果与三维模型中的末端标记点位置对比，若偏差在数值精度范围内，说明 DH 参数和变换顺序正确。

四、逆运动学：从目标位姿反解关节角

逆运动学是“已知末端目标位姿，求关节角”的过程，比正运动学复杂得多，主要难点在于：解可能不存在（目标超出工作空间）、可能多解（肘上/肘下、腕翻转等构型）、也可能无穷多解（冗余机械臂）。对于常见的 6 自由度非冗余机械臂，若满足 Pieper 条件（三个相邻关节轴交于一点或相互平行），可以得到解析解。

以腕部三轴交于一点的 6R 机械臂为例，逆解通常分三步：

1. 求腕心位置：从目标位姿中扣除末端工具长度，得到腕部中心点 P_w。
2. 解前三个关节：利用 P_w 的几何关系，解出 θ₁、θ₂、θ₃，通常存在多组解，需要根据关节限位和避障要求筛选。
3. 解后三个关节：由目标姿态矩阵和前三个关节角，解出腕部 θ₄、θ₅、θ₆，同样需要处理多解和奇异问题。

当解析解难以获得时，可采用数值法（如牛顿-拉夫逊迭代、雅可比伪逆迭代）。数值法通用性强，但需要初值、可能不收敛，且计算量较大。在联合仿真中，通常优先使用解析解以保证实时性，数值法作为备用或用于冗余机械臂。

五、工作空间分析：可达与灵活

工作空间是机械臂末端能够到达的所有位置的集合，分为两类：

• 可达工作空间：末端至少能以一种姿态到达的位置集合，通常由几何法或蒙特卡洛法绘制。
• 灵活工作空间：末端能够以任意姿态到达的位置集合，是可达工作空间的子集，对抓取和装配任务尤为重要。

在仿真中，常用蒙特卡洛法分析工作空间：在各关节范围内随机采样大量关节角组合，对每组做正运动学，记录末端位置，最后绘制点云或包络面。通过观察点云密度和边界，可以判断机械臂的有效作业区域、是否存在空洞以及关节限位对工作空间的影响。工作空间分析的结果直接指导基座安装位置、任务点选取和轨迹规划范围。

六、建模实践中的关键检查点

把上述理论落到联合仿真模型时，有几个容易出错的地方值得逐一核对。第一，DH 参数表的符号和顺序必须与坐标系建立规则严格对应，尤其是 α 的正负号，它决定相邻关节轴是“向上”还是“向下”倾斜。第二，关节零位定义要与实际机械臂的机械零位一致，否则仿真中的 θ=0 并不对应真实姿态。第三，逆运动学多解筛选要同时考虑关节限位、奇异回避和路径连续性，不能只取第一组解。第四，工作空间分析时采样数量要足够大（通常 10⁵～10⁶ 组），否则包络面会出现明显锯齿或空洞误判。

运动学建模完成后，机械臂的几何映射关系就确定了。下一步可以在此基础上推导雅可比矩阵，建立关节速度与末端速度的映射，并进一步引入质量、惯量和力/力矩，进入动力学建模阶段。运动学是动力学与控制联合仿真的入口，只有入口准确，后续的仿真结果才具有可信度。