4、正运动学求解：基于D-H参数的机器人正运动学方程推导与计算
4.1 什么是正运动学？
正运动学，说白了就是：已知关节角度，求末端位姿。
你给机器人每个关节一个角度值，比如关节1转30°，关节2转45°，关节3转-60°……然后我问你：机械臂末端在空间里到底在哪儿？朝向如何？
正运动学就是回答这个问题的数学工具。
我刚开始做机器人控制时，总觉得这玩意儿太理论。直到有一次调试六轴机械臂，发现末端位置怎么算都不对，最后发现是D-H参数表里一个符号写反了……嗯，从那以后我再也不敢小看正运动学了。
4.2 D-H参数法：标准流程
D-H参数法有四个核心参数，每个关节对应一组：
| 参数 | 符号 | 含义 |
|---|---|---|
| 连杆长度 | ai-1 | 沿xi-1轴，从zi-1到zi的距离 |
| 连杆扭转角 | αi-1 | 绕xi-1轴，从zi-1到zi的转角 |
| 连杆偏距 | di | 沿zi轴，从xi-1到xi的距离 |
| 关节角 | θi | 绕zi轴，从xi-1到xi的转角 |
我个人习惯用标准D-H法（也叫Craig法）。为什么？因为坐标系定义更清晰，不容易搞混。
4.3 相邻连杆的变换矩阵
每个相邻连杆之间的变换，可以拆成四个步骤：
- 绕xi-1轴旋转αi-1
- 沿xi-1轴平移ai-1
- 绕zi轴旋转θi
- 沿zi轴平移di
写成齐次变换矩阵就是：
i-1T_i = Rot(x, αi-1) · Trans(x, ai-1) · Rot(z, θi) · Trans(z, di)展开后：
i-1T_i =
[ cosθi -sinθi 0 ai-1 ]
[ sinθicosαi-1 cosθicosαi-1 -sinαi-1 -disinαi-1 ]
[ sinθisinαi-1 cosθisinαi-1 cosαi-1 dicosαi-1 ]
[ 0 0 0 1 ]
小技巧： 我建议你把这个矩阵背下来。不是死记硬背，而是理解每个元素的位置含义。左上角3×3是旋转矩阵，第四列前三行是位置向量。
4.4 正运动学方程：从基座到末端
对于n自由度的机器人，正运动学方程就是：
0T_n = 0T_1 · 1T_2 · 2T_3 · … · n-1T_n说白了，就是把每个关节的变换矩阵连乘起来。
举个例子，一个简单的2自由度平面机械臂：
- 关节1：旋转，连杆长度a0 = L1
- 关节2：旋转，连杆长度a1 = L2
D-H参数表：
| i | ai-1 | αi-1 | di | θi |
|---|---|---|---|---|
| 1 | 0 | 0 | 0 | θ1 |
| 2 | L1 | 0 | 0 | θ2 |
代入矩阵公式，得到：
0T_2 = 0T_1 · 1T_2
最终末端位置：
x = L1·cosθ1 + L2·cos(θ1+θ2)
y = L1·sinθ1 + L2·sin(θ1+θ2)你看，这就是正运动学的结果——一个关于关节角度的函数。
4.5 知识体系结构图
下面这张图帮你理清正运动学的整体逻辑：
4.6 避坑指南
⚠️ 我曾经踩过的坑：
- 坐标系方向搞反： z轴方向必须与关节旋转轴一致。我有一回把z轴方向设反了，结果末端位置直接飞到了机器人背后。
- a和d搞混： a是沿x轴的距离，d是沿z轴的距离。别问我怎么知道的……
- 角度单位： 代码里一定要统一用弧度！我见过太多人把角度当弧度传进去，结果末端位置完全不对。
核心要点：
正运动学其实就是一个从关节空间到笛卡尔空间的映射。你给它关节角度，它给你末端位姿。这个映射是唯一的、确定的，不存在多解问题。
记住：正运动学永远有唯一解。如果你算出来多个解，那一定是D-H参数或者矩阵乘法出错了。
4.7 实际项目中的建议
我在做六轴协作机器人项目时，总结了几条经验：
- 先画坐标系图： 别急着写代码。先在纸上把每个关节的坐标系画出来，标清楚z轴方向。
- 验证每个变换矩阵： 每算出一个i-1T_i，就检查一下它的旋转部分是不是正交矩阵（行列式=1）。
- 用已知位姿验证： 比如所有关节角为0时，末端应该在什么位置？先用手算出来，再跟代码结果对比。
嗯，正运动学就讲到这里。你想想看，其实核心就是四个参数、一个矩阵、连乘到底。掌握了这个，后面学逆运动学就轻松多了。