3. 正运动学建模：D-H参数法、建立连杆坐标系、推导正运动学方程
正运动学，说白了就是：给定关节角度，算出末端在哪儿、朝哪个方向。
我刚开始做机器人时，觉得这步就是套公式。后来在产线上调试一个六轴协作臂，发现末端位置算出来总是差几毫米。查了两天，最后发现是连杆坐标系建反了。嗯，从那以后，我再也不敢小看D-H参数法了。
这一章，我们就来彻底搞定它。
3.1 D-H参数法：四个参数描述一个连杆
一个机器人手臂，说白了就是一堆连杆串在一起。每个连杆怎么描述？D-H法用了四个参数：
| 参数 | 符号 | 物理含义 | 我的理解 |
|---|---|---|---|
| 连杆长度 | ai-1 | 从zi-1到zi沿xi-1的距离 | 两个关节轴之间的最短距离 |
| 连杆扭角 | αi-1 | 从zi-1到zi绕xi-1的转角 | 两个关节轴之间的夹角 |
| 连杆偏距 | di | 从xi-1到xi沿zi的距离 | 沿当前关节轴的偏移 |
| 关节角 | θi | 从xi-1到xi绕zi的转角 | 就是我们要控制的关节角度 |
我的小技巧：记这四个参数，你就想成「先沿x走多远，再绕x转多少；再沿z走多远，再绕z转多少」。顺序别搞反，反了坐标系就乱了。
3.2 建立连杆坐标系：从基座到末端
坐标系怎么建？我总结了一套「三步法」，你照着做，基本不会错。
- 定z轴：每个关节的旋转轴就是zi轴。方向按右手定则。
- 定x轴：如果zi-1和zi不平行，xi-1沿它们的公垂线方向，从zi-1指向zi。如果平行，随便选一个方便的方向。
- 定原点：xi-1与zi的交点就是原点Oi-1。
避坑指南：我曾经在项目里犯过一个低级错误——把基座坐标系（{0}）的z0方向搞反了。结果后面所有关节的θ1都反号。记住：基座坐标系可以任意定，但一旦定了，后面所有坐标系都得跟着它走。
下面这张图展示了典型的六轴机器人连杆坐标系：
3.3 推导正运动学方程：从关节空间到笛卡尔空间
坐标系建好了，接下来就是算变换矩阵。每个相邻坐标系之间的变换，可以用一个4×4的齐次变换矩阵表示：
i-1T_i = Rot(x, α_{i-1}) · Trans(x, a_{i-1}) · Rot(z, θ_i) · Trans(z, d_i)展开后就是：
i-1T_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_1 · ¹T_2 · ²T_3 · ... · ⁿ⁻¹T_n
其中n是连杆数量（六轴机器人n=6）。
⁰T_n = ⁰T_1 · ¹T_2 · ²T_3 · ... · ⁿ⁻¹T_n
其中n是连杆数量（六轴机器人n=6）。
3.4 实战：一个两连杆机器人的正运动学
光讲理论太枯燥。我们拿一个最简单的两连杆平面机器人来算一遍。
假设：
- 连杆1长度L1，连杆2长度L2
- 关节角θ1和θ2
- 所有z轴都垂直于纸面（朝外）
D-H参数表：
| 连杆i | ai-1 | αi-1 | di | θi |
|---|---|---|---|---|
| 1 | 0 | 0 | 0 | θ1 |
| 2 | L1 | 0 | 0 | θ2 |
代入公式，得到：
⁰T_1 =
[ cosθ1 -sinθ1 0 0 ]
[ sinθ1 cosθ1 0 0 ]
[ 0 0 1 0 ]
[ 0 0 0 1 ]
¹T_2 =
[ cosθ2 -sinθ2 0 L1 ]
[ sinθ2 cosθ2 0 0 ]
[ 0 0 1 0 ]
[ 0 0 0 1 ]
⁰T_2 = ⁰T_1 · ¹T_2 =
[ cos(θ1+θ2) -sin(θ1+θ2) 0 L1·cosθ1 + L2·cos(θ1+θ2) ]
[ sin(θ1+θ2) cos(θ1+θ2) 0 L1·sinθ1 + L2·sin(θ1+θ2) ]
[ 0 0 1 0 ]
[ 0 0 0 1 ]末端位置就是：
x = L1·cosθ1 + L2·cos(θ1+θ2)
y = L1·sinθ1 + L2·sin(θ1+θ2)
验证一下：当θ1=0, θ2=0时，末端在(L1+L2, 0)。当θ1=90°, θ2=-90°时，末端在(0, L1)。你用手比划一下，是不是这个理？
3.5 常见坑与我的经验
- 坐标系方向搞反：我建议你每次建完坐标系，用手动转一下关节，看看末端运动方向对不对。不对就赶紧改。
- D-H参数符号搞错：a和α的下标是i-1，d和θ的下标是i。这个细节我至少被坑过三次。
- 连乘顺序：从基座到末端，是⁰T_1 · ¹T_2 · ...，不是反过来。矩阵乘法不满足交换律，顺序错了全完蛋。
我的血泪教训：有一次在项目里，我直接用别人的D-H参数表，没验证就往下算。结果仿真里末端位置飘到天上去。后来发现是那个参数表的坐标系定义和我用的库不一样。所以，拿到任何D-H参数，先手动验证一两个特殊位姿，再往下走。
正运动学是机器人学的基石。你把它搞透了，后面逆运动学、轨迹规划、动力学，都会轻松很多。这一章的内容，我建议你亲手算一遍，别光看。