4. 机器人机构运动学建模

多体动力学分析的第一步，是把真实机械臂抽象成可计算的连杆—关节链。运动学建模解决的核心问题是：给定各关节变量，末端执行器在基坐标系中的位姿是什么（正运动学）；反过来，给定末端目标位姿，各关节应取什么值（逆运动学）。本节以标准 D-H 约定为主线，把连杆坐标系、D-H 参数、齐次变换、正逆解串成一条可操作的建模路径，并用一个六自由度机械臂实例完整走一遍。

一、连杆坐标系与 D-H 参数的定义

机械臂由若干刚性连杆通过关节串联而成。为了用统一方式描述相邻连杆的相对位置，Denavit 与 Hartenberg 提出在每个连杆上固连一个坐标系，并用四个参数描述相邻坐标系之间的变换。标准 D-H 约定中，坐标系 {i} 固连在连杆 i 上，其 zi 轴沿关节 i+1 的轴线方向，原点位于 zi 与 zi-1 的公垂线交点处，xi 轴沿公垂线由 zi-1 指向 zi，yi 由右手定则确定。

四个 D-H 参数含义如下：

• 连杆长度 a_i：沿 xi 轴，从 zi-1 到 zi 的距离；
• 连杆扭角 α_i：绕 xi 轴，从 zi-1 转到 zi 的角度；
• 关节偏置 d_i：沿 zi-1 轴，从 xi-1 到 xi 的距离；
• 关节转角 θ_i：绕 zi-1 轴，从 xi-1 转到 xi 的角度。

对转动关节，θi 是变量，其余三个为常量；对移动关节，di 是变量。这一区分决定了后续正运动学中哪些量随关节运动变化。

二、齐次变换与相邻连杆变换矩阵

相邻坐标系 {i-1} 到 {i} 的变换可分解为四次基本运动：绕 zi-1 转 θi、沿 zi-1 移 di、沿 xi 移 ai、绕 xi 转 αi。写成齐次变换矩阵为：

        | cθi  -sθi·cαi   sθi·sαi   ai·cθi |
i-1Ti = | sθi   cθi·cαi  -cθi·sαi   ai·sθi |
        |  0      sαi       cαi       di    |
        |  0       0         0         1    |

其中 c 表示 cos，s 表示 sin。该矩阵同时包含旋转与平移信息，是正运动学递推的基本单元。基座到末端的总变换为各相邻变换连乘：

0Tn = 0T1 · 1T2 · 2T3 · … · n-1Tn

连乘顺序不可交换，必须严格按连杆编号从基座向末端依次相乘。

三、正运动学求解流程

正运动学是“已知关节变量求末端位姿”，流程清晰且无多解问题：

1. 建立各连杆坐标系，列出 D-H 参数表；
2. 代入每个关节的当前变量值，写出各 i-1Ti；
3. 从 0T1 开始依次右乘，得到 0Tn；
4. 从 0Tn 中提取位置向量 [px, py, pz]T 和旋转矩阵，旋转矩阵可进一步转换为欧拉角或四元数。

正解结果唯一，是后续动力学中计算雅可比、惯性力和重力的基础。

四、逆运动学求解思路

逆运动学是“已知末端位姿求关节变量”，难度显著高于正解，主要因为：解可能不存在（目标超出工作空间）、可能多解（肘上/肘下、腕翻转）、也可能无穷多解（冗余机械臂）。常用方法有三类：

• 解析法：利用几何关系分离变量，适合满足 Pieper 准则（三轴相交或平行）的六自由度臂，可得到封闭解；
• 几何法：把空间问题投影到平面，用三角形关系求关节角，直观但依赖构型；
• 数值法：用雅可比矩阵迭代逼近，如牛顿—拉夫森法，通用性强但需初值且可能不收敛。

工程中通常优先解析法，因为它计算快、可枚举全部解，再按“关节限位、避障、能耗最小”等准则选一组最优解。

五、典型六自由度机械臂建模实例

以一台常见的六轴串联机械臂为例，采用标准 D-H 参数（示例参数，用于教学演示，非实测值）：

取一组关节角 θ = [0°, 30°, -45°, 0°, 60°, 0°]，按前述矩阵连乘可算得末端位置约为 (0.62 m, 0.00 m, 0.75 m)（示例计算结果，用于说明流程）。若把该位姿作为逆解输入，解析法可得到多组关节角，其中一组与输入一致，其余为肘上/腕翻转构型。实际使用时需结合关节限位和奇异位形判断，例如当 θ5 ≈ 0° 时腕部奇异，逆解退化，应避免在该区域做高速插补。

把上述流程落到代码层面，正运动学可用一个循环完成矩阵连乘，逆运动学则按几何关系逐段求解。建模完成后，得到的 0Tn 与雅可比矩阵将直接进入多体动力学方程，用于计算关节力矩、惯性耦合和重力补偿。因此，运动学建模的坐标系定义、参数符号和连乘顺序必须与后续动力学模块严格一致，任何一处约定错位都会在动力学结果中被放大。