4、正运动学建模：D-H参数法、连杆坐标系建立、正运动学方程推导
各位同学，今天我们来聊聊正运动学建模。说实话，这是机器人控制里最基础、也最绕不开的一步。你想想看，要让机器人动起来，总得先知道它每个关节转多少度，末端执行器会跑到哪个位置吧？这就是正运动学要干的事。
我个人习惯用D-H参数法来做这件事。为什么？因为它标准化，不容易出错。我在项目里见过太多人自己瞎编坐标变换，结果调试时对不上，最后还得老老实实回来用D-H法重新算一遍。
4.1 D-H参数法：四个参数搞定一切
D-H参数法，说白了就是用四个参数来描述相邻两个连杆之间的相对位置和姿态。这四个参数分别是：
- 连杆长度 ai：沿着 xi 轴，从 zi-1 到 zi 的距离
- 连杆扭角 αi：绕着 xi 轴，从 zi-1 转到 zi 的角度
- 关节距离 di：沿着 zi-1 轴，从 xi-1 到 xi 的距离
- 关节转角 θi：绕着 zi-1 轴，从 xi-1 转到 xi 的角度
嗯，这里要注意：对于旋转关节，θi 是变量；对于移动关节，di 是变量。其他参数都是固定的，由机械结构决定。
核心思想：D-H参数法把复杂的空间变换拆成了四个简单的步骤，每一步只绕一个轴转或沿一个轴移。这样我们就能用统一的矩阵形式来表达任意两个连杆的关系。
4.2 连杆坐标系建立：别搞反了方向
建立坐标系是D-H法里最容易出错的地方。我曾经在调试一个六轴机器人时，就因为坐标系方向搞反了，花了整整两天才找到问题。
标准步骤是这样的：
- 找出每个关节的轴线，标出 zi-1 轴（沿关节 i 的轴线方向）
- 找出相邻两轴线的公垂线，标出 xi 轴（沿公垂线方向，从 zi-1 指向 zi）
- 用右手定则确定 yi 轴
- 基坐标系 {0} 固定不动，工具坐标系 {n} 在末端执行器上
这里有个避坑指南：当 zi-1 和 zi 平行时，公垂线不唯一。我建议你选一个让后续计算最方便的，比如让 xi 沿着从 zi-1 到 zi 的垂直方向。
我的小技巧：画坐标系时，先用不同颜色标出每个关节的 z 轴。这样一眼就能看出哪些轴平行、哪些轴相交，后面选 x 轴方向就轻松多了。
4.3 正运动学方程推导：从关节空间到操作空间
有了D-H参数和坐标系，我们就可以推导正运动学方程了。说白了，就是求从基坐标系到末端坐标系的变换矩阵 0Tn。
每个相邻连杆的变换矩阵是：
⎡ cosθᵢ -sinθᵢcosαᵢ sinθᵢsinαᵢ aᵢcosθᵢ ⎤
⎢ sinθᵢ cosθᵢcosαᵢ -cosθᵢsinαᵢ aᵢsinθᵢ ⎥
ᶤ⁻¹Tᵢ = ⎢ 0 sinαᵢ cosαᵢ dᵢ ⎥
⎣ 0 0 0 1 ⎦然后，把从第1个到第n个连杆的变换矩阵连乘起来：
⁰Tₙ = ⁰T₁ · ¹T₂ · ²T₃ · ... · ⁿ⁻¹Tₙ这个矩阵的左上角 3×3 部分就是末端姿态，右上角 3×1 部分就是末端位置。
注意：矩阵乘法不满足交换律，顺序绝对不能搞错！我见过有人把 ⁰T₁ 和 ¹T₂ 写反了，结果算出来的末端位置差了十万八千里。
4.4 知识体系总览
为了让你对整个流程有个直观印象，我画了张图：
4.5 实战举例：两连杆平面机械臂
光说不练假把式。我们拿一个最简单的两连杆平面机械臂来走一遍流程。
假设两个连杆长度分别是 L₁ 和 L₂，两个关节都是旋转关节。坐标系建立如下：
- 基坐标系 {0}：z₀ 垂直纸面向外，x₀ 水平向右
- 连杆1坐标系 {1}：z₁ 垂直纸面向外，x₁ 沿连杆1方向
- 连杆2坐标系 {2}：z₂ 垂直纸面向外，x₂ 沿连杆2方向
D-H参数表：
| 连杆 i | ai | αi | di | θi |
|---|---|---|---|---|
| 1 | L₁ | 0° | 0 | θ₁ |
| 2 | L₂ | 0° | 0 | θ₂ |
代入变换矩阵公式，得到：
⎡ cosθ₁ -sinθ₁ 0 L₁cosθ₁ ⎤
⁰T₁ = ⎢ sinθ₁ cosθ₁ 0 L₁sinθ₁ ⎥
⎢ 0 0 1 0 ⎥
⎣ 0 0 0 1 ⎦
⎡ cosθ₂ -sinθ₂ 0 L₂cosθ₂ ⎤
¹T₂ = ⎢ sinθ₂ cosθ₂ 0 L₂sinθ₂ ⎥
⎢ 0 0 1 0 ⎥
⎣ 0 0 0 1 ⎦连乘后得到末端位姿：
⁰T₂ = ⁰T₁ · ¹T₂
⎡ cos(θ₁+θ₂) -sin(θ₁+θ₂) 0 L₁cosθ₁ + L₂cos(θ₁+θ₂) ⎤
= ⎢ sin(θ₁+θ₂) cos(θ₁+θ₂) 0 L₁sinθ₁ + L₂sin(θ₁+θ₂) ⎥
⎢ 0 0 1 0 ⎥
⎣ 0 0 0 1 ⎦所以末端位置是：
x = L₁cosθ₁ + L₂cos(θ₁+θ₂)
y = L₁sinθ₁ + L₂sin(θ₁+θ₂)你看，结果很直观吧？就是两个向量相加。但如果没有D-H法，你很难保证这种推导在复杂机器人上还能不出错。
验证方法：算完后，把 θ₁=0, θ₂=0 代入，看看末端是不是在 (L₁+L₂, 0) 位置。如果是，说明你算对了。我每次算完都会做这个检查，省了不少调试时间。
4.6 常见错误与避坑
最后，我总结几个我在项目中踩过的坑：
- 坐标系方向搞反：特别是 z 轴方向，一定要和关节旋转方向符合右手定则
- D-H参数符号弄错：aᵢ 和 dᵢ 是有正负的，别想当然都取正
- 矩阵乘法顺序：从基座到末端，依次左乘
- 忘记验证：算完一定要找个特殊位姿验证一下
嗯，正运动学建模就讲到这里。记住，D-H参数法虽然看起来有点繁琐，但它是最可靠的方法。等你用熟了，就会发现它其实很优雅——四个参数，一个矩阵，连乘到底，万事大吉。