4. 串联机构运动学：D-H参数法、正运动学求解、逆运动学求解

各位工程师朋友，大家好。今天我们进入串联机构运动学这个核心章节。说实话，搞多自由度调整机构，运动学就是你的基本功。你设计的机构能不能动、怎么动、动到哪里去，全看这一章学得扎实不扎实。

我个人习惯把运动学分成两个问题：正运动学——给定关节角度，算末端位姿；逆运动学——给定末端位姿，反推关节角度。前者是唯一的，后者往往多解。嗯，这里面的门道，我们一个一个来拆。

4.1 D-H参数法：串联机构的通用语言

为什么要有D-H参数法？你想想看，一个六轴机械臂，六个连杆，六个关节，每个关节的轴线方向、连杆长度、偏距都不一样。如果没有一套统一的描述方法，你连数学模型都建不起来。

D-H参数法说白了就是用四个参数来描述相邻两个连杆之间的相对位置和姿态：

建立D-H坐标系时，我建议你记住这个口诀：Z轴是关节轴线，X轴是公垂线。先定Z轴，再定X轴，最后用右手定则补Y轴。我在项目中遇到过一位同事，坐标系建反了，算出来的末端位置差了十万八千里，排查了整整两天。

4.2 正运动学求解：从关节空间到操作空间

正运动学就是一连串的矩阵乘法。相邻连杆的变换矩阵长这样：

i-1T_i = Rot(X, α_i-1) · Trans(X, a_i-1) · Rot(Z, θ_i) · Trans(Z, d_i)

写成矩阵形式：
[ cosθ_i    -sinθ_i    0       a_i-1 ]
[ sinθ_i·cosα_i-1  cosθ_i·cosα_i-1  -sinα_i-1  -d_i·sinα_i-1 ]
[ sinθ_i·sinα_i-1  cosθ_i·sinα_i-1   cosα_i-1   d_i·cosα_i-1 ]
[ 0          0          0       1      ]

把每个关节的变换矩阵乘起来，就得到末端相对于基座的位姿：

⁰T_n = ⁰T₁ · ¹T₂ · ... · ⁿ⁻¹T_n

这里有个坑——矩阵乘法顺序不能错。我刚开始做的时候，习惯从基座往末端乘，也就是左乘。后来发现有些教材用右乘，差点把项目带偏。记住：从基座到末端，依次左乘。

4.3 逆运动学求解：从操作空间回到关节空间

逆运动学才是真正考验功力的地方。正运动学是唯一的，逆运动学却可能无解、有唯一解、有多解甚至无穷多解。为什么会这样？因为你的机构自由度、关节限位、连杆参数都会影响解的存在性和数量。

求解方法分两大类：解析法和数值法。

4.3.1 解析法

解析法就是通过代数或几何推导，直接写出关节角度的表达式。它的优点是快、准、稳，但只适用于特定构型的机构。比如常见的六轴工业机器人，如果满足Pieper准则（三个相邻关节轴线交于一点），就可以用解析法。

解析法的典型步骤：

1. 分离变量：把已知的末端位姿矩阵和未知的关节角度分开
2. 等式两边同乘逆矩阵：逐步消去未知量
3. 利用三角函数恒等式：比如用atan2代替arcsin，避免多值问题
4. 判断解的合理性：剔除超出关节限位的解

我记得有一次做精密对位平台，要求末端重复定位精度0.01mm。用解析法算逆解，一个周期只要0.5ms，完全满足实时性要求。如果是数值法，同样的精度可能要算5ms以上。

4.3.2 数值法

数值法不挑机构构型，通用性强。最常用的是牛顿-拉夫森法和雅可比伪逆法。基本思路是：先猜一组关节角度，算正运动学得到末端位姿，和期望位姿对比，然后根据误差修正关节角度，迭代直到收敛。

数值法的核心公式：

ΔX = J · Δθ
Δθ = J⁺ · ΔX

其中：
ΔX —— 末端位姿误差（6×1）
J  —— 雅可比矩阵（6×n）
J⁺ —— 雅可比伪逆矩阵（n×6）
Δθ —— 关节角度修正量（n×1）

数值法的优缺点很明显：

数值法有个经典问题——初值敏感。初值选得不好，可能收敛到错误的解，甚至不收敛。我一般先用解析法（如果机构满足条件）算一个近似解作为初值，再用数值法精调。这样既快又稳。

4.4 本章知识体系总览

下面这张图把串联机构运动学的核心逻辑串起来了。你顺着箭头看，就能理解从D-H参数到正逆解的全流程。

这张图把正运动学和逆运动学的关系、各自的子方法、以及最终的应用方向都画清楚了。你写代码或者做方案时，可以对照这张图，看看自己卡在哪一步。