3、运动学基础：正运动学与逆运动学概念、D-H参数法建模

各位同学，咱们今天聊点硬核的——运动学基础。说实话，我刚入行那会儿，觉得运动学就是一堆公式来回倒腾。后来在调试一个六轴机械臂时，发现算出来的位置和实际差了快两厘米，才意识到这东西真不能靠感觉。

运动学解决什么问题？说白了就是两件事：我知道每个关节转了多少度，想知道末端在哪儿（正运动学）；或者我知道末端要去哪儿，反推每个关节该转多少（逆运动学）。你想想看，机器人要抓个杯子，是不是就得先算清楚这个？

3.1 正运动学：从关节空间到笛卡尔空间

正运动学，就是已知关节角度，求末端位姿。公式长这样：

T = f(θ₁, θ₂, ..., θₙ)

其中 T 是 4×4 的齐次变换矩阵，包含了位置和姿态信息。θᵢ 是第 i 个关节的角度。

我记得第一次做正运动学推导时，用了整整三天。后来发现，其实核心就是矩阵连乘。每个关节对应一个变换矩阵，把这些矩阵按顺序乘起来，就得到了末端位姿。

举个例子，一个两连杆机械臂：

T = A₁ × A₂

其中：
A₁ = [cosθ₁  -sinθ₁  0  a₁cosθ₁]
     [sinθ₁   cosθ₁  0  a₁sinθ₁]
     [0       0      1  0      ]
     [0       0      0  1      ]

A₂ = [cosθ₂  -sinθ₂  0  a₂cosθ₂]
     [sinθ₂   cosθ₂  0  a₂sinθ₂]
     [0       0      1  0      ]
     [0       0      0  1      ]

乘出来就是末端位置：

x = a₁cosθ₁ + a₂cos(θ₁+θ₂)
y = a₁sinθ₁ + a₂sin(θ₁+θ₂)

嗯，这里要注意：正运动学计算量随关节数增加而爆炸。六轴机械臂的矩阵乘法，手算能算到怀疑人生。所以我建议用符号计算工具，比如 MATLAB 的 Symbolic Toolbox 或者 Python 的 SymPy。

3.2 逆运动学：从笛卡尔空间回到关节空间

逆运动学就麻烦多了。已知末端位姿，求关节角度。公式是：

θ₁, θ₂, ..., θₙ = f⁻¹(T)

为什么麻烦？因为 f⁻¹ 通常不是单值函数。一个末端位置，可能对应多种关节构型。比如六轴机械臂，同一个点可能有 8 组解。

逆运动学的求解方法主要有两种：

• 解析法：通过几何关系直接推导出公式。速度快，精度高，但只适用于特定构型。
• 数值法：用迭代逼近，比如牛顿-拉夫森法。通用性强，但可能不收敛或陷入局部最优。

我个人习惯是：能解析就解析，解析不了再用数值法。而且数值法一定要给初值，初值越接近真实解，收敛越快。

3.3 D-H参数法：建模的标准化工具

D-H参数法，全称 Denavit-Hartenberg 参数法。说白了，就是给每个关节建立坐标系，然后用四个参数描述相邻坐标系的关系。

这四个参数是：

参数	符号	含义
连杆长度	aᵢ	沿 xᵢ 轴，从 zᵢ 到 zᵢ₊₁ 的距离
连杆扭角	αᵢ	绕 xᵢ 轴，从 zᵢ 到 zᵢ₊₁ 的转角
连杆偏距	dᵢ	沿 zᵢ₋₁ 轴，从 xᵢ₋₁ 到 xᵢ 的距离
关节转角	θᵢ	绕 zᵢ₋₁ 轴，从 xᵢ₋₁ 到 xᵢ 的转角

有了这四个参数，相邻坐标系的变换矩阵就是：

Aᵢ = Rot(z, θᵢ) × Trans(z, dᵢ) × Trans(x, aᵢ) × Rot(x, αᵢ)

展开后：

Aᵢ = [cosθᵢ  -sinθᵢcosαᵢ   sinθᵢsinαᵢ   aᵢcosθᵢ]
     [sinθᵢ   cosθᵢcosαᵢ   -cosθᵢsinαᵢ  aᵢsinθᵢ]
     [0       sinαᵢ        cosαᵢ        dᵢ     ]
     [0       0            0            1      ]

你想想看，只要把每个关节的 D-H 参数填进表格，然后矩阵连乘，正运动学就出来了。是不是很优雅？

3.4 知识体系总览

下面这张图，是我自己总结的运动学知识框架。你看一眼，心里就有数了。

这张图把运动学的核心脉络理清楚了。正运动学用 D-H 参数法建模，逆运动学用解析法或数值法求解。两者最终都服务于机器人的轨迹规划和运动控制。

3.5 实战中的坑与经验

最后，分享几个我在项目中踩过的坑：

• 坐标系方向搞反：D-H 参数中 z 轴方向必须一致，否则矩阵乘出来全是错的。我建议画图时标清楚每个轴的方向。
• 奇异点处理：当两个关节轴平行时，D-H 参数中的 α 为 0，会导致自由度丢失。这时候需要特殊处理，比如加一个微小偏移。
• 逆运动学多解选择：同一个末端位置可能有多个解。我一般根据关节限位和能耗来选择最优解，比如选关节角度变化最小的那个。