1. 机器人运动学基础：坐标系与变换矩阵、齐次坐标、旋转矩阵与欧拉角、DH参数建模方法
各位同学，欢迎来到这门实战课的第一章。
说实话，我见过太多工程师一上来就调PID、写轨迹规划，结果机器人动起来完全不是那么回事。为什么？因为坐标系都没搞明白。你想想看，连“我在哪、我要去哪”都没定义清楚，控制算法写得再漂亮也是白搭。
这一章，我们就来把运动学的基础打牢。我个人习惯把这一章叫做“机器人的世界观”——你用什么语言描述机器人的位姿，决定了你后续所有工作的精度和效率。
核心观点：运动学是机器人控制的地基。地基不稳，楼盖得越高越危险。本章所有内容，都会在后续的标定、控制、规划章节反复用到。
1.1 坐标系与变换矩阵
先问一个问题：你如何告诉别人一个点在空间中的位置？
最简单的办法——给他一个坐标系。比如“以我的右肩为原点，向前是X，向右是Y，向上是Z，那个螺丝刀尖的位置是(0.3, 0.2, 0.5)米”。这就是坐标系的作用。
但在机器人里，事情没那么简单。机械臂有6个关节，每个关节都有自己的坐标系。基座有一个坐标系，末端执行器有一个坐标系，视觉相机有一个坐标系，工件台上还有一个坐标系。这些坐标系之间怎么互相转换？
答案就是变换矩阵。
一个4×4的变换矩阵长这样：
| R11 R12 R13 tx |
| R21 R22 R23 ty |
| R31 R32 R33 tz |
| 0 0 0 1 |左上角3×3是旋转矩阵R，描述姿态；右上角3×1是平移向量t，描述位置。最后一行永远是[0 0 0 1]，这是齐次坐标的固定格式。
我的经验：刚入行时我总记不住变换矩阵的乘法顺序。后来我给自己编了个口诀：“先旋转，再平移，矩阵左乘坐标系”。也就是说，如果你想把坐标系A下的点变换到坐标系B下，变换矩阵T_BA要左乘这个点的坐标向量。顺序搞反了，结果就是天差地别。
1.2 齐次坐标：为什么非要加那一行？
你可能觉得奇怪：明明3×3旋转矩阵加3×1平移向量就能描述变换了，为什么非要用4×4矩阵，还多一行[0 0 0 1]？
原因很简单：为了把旋转和平移统一成一个线性运算。
如果不加那一行，你要写：p' = R * p + t。这是一个线性变换加一个平移，不是纯线性变换。而加上齐次坐标后，变成：p'_h = T * p_h。一次矩阵乘法搞定，干净利落。
更重要的是，多个变换可以连乘：T_AC = T_AB * T_BC。这在机器人运动学里太常用了——从基座到关节1，关节1到关节2，...，一直乘到末端执行器。
注意：齐次坐标的尺度等价性。点(x, y, z, w)和点(x/w, y/w, z/w, 1)表示的是同一个三维点。w=0时表示方向向量（无穷远点）。我在做手眼标定时，曾经因为w值没归一化，导致标定结果完全错误，排查了整整两天。嗯，这个坑你们别踩。
1.3 旋转矩阵与欧拉角
旋转矩阵有9个元素，但自由度只有3个。说白了，它是个冗余表示。好处是计算方便，坏处是占内存、难理解。
所以工程上更常用欧拉角——用三个角度描述旋转。最常见的顺序是ZYX（先绕Z轴转γ，再绕Y轴转β，最后绕X轴转α），也叫RPY角（Roll滚转、Pitch俯仰、Yaw偏航）。
| 名称 | 旋转轴 | 符号 | 范围 |
|---|---|---|---|
| Yaw（偏航） | Z轴 | γ | [-π, π] |
| Pitch（俯仰） | Y轴 | β | [-π/2, π/2] |
| Roll（滚转） | X轴 | α | [-π, π] |
欧拉角有个著名的坑——万向锁。当Pitch角达到±90°时，Roll和Yaw的旋转轴会重合，丢失一个自由度。我记得有一次调试六轴机器人，末端姿态突然“卡住”了，怎么转都转不到目标位置。查了半天，就是万向锁在作怪。
我的建议：做插值和姿态规划时，尽量用四元数代替欧拉角。四元数没有万向锁问题，插值也更平滑。虽然理解起来有点抽象，但写代码时你会感谢它的。后面章节我们会专门讲四元数的实战用法。
1.4 DH参数建模方法
好了，现在我们知道怎么描述单个坐标系之间的变换了。但一个六轴机器人有六个关节，每个关节之间的变换关系怎么统一描述？
这就是DH参数（Denavit-Hartenberg）的用武之地。它用四个参数，就能唯一确定相邻两个关节坐标系之间的变换关系。
四个参数分别是：
- ai-1（连杆长度）：沿Xi-1轴，从Zi-1到Zi的距离
- αi-1（连杆扭转角）：绕Xi-1轴，从Zi-1到Zi的角度
- di（连杆偏距）：沿Zi轴，从Xi-1到Xi的距离
- θi（关节角）：绕Zi轴，从Xi-1到Xi的角度
对于旋转关节，θi是变量；对于移动关节，di是变量。其他三个参数由机械结构决定，是常数。
有了DH参数，相邻坐标系的变换矩阵就可以直接写出来：
T_i = RotX(α_{i-1}) * TransX(a_{i-1}) * RotZ(θ_i) * TransZ(d_i)展开成4×4矩阵就是：
| 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 |注意：DH参数有两种约定——标准DH和改进DH（也叫Craig DH）。区别在于坐标系附着的位置不同。标准DH把坐标系i附着在关节i上，改进DH附着在连杆i上。我建议你选一种用到底，混用的话，变换矩阵会完全错位。我个人习惯用改进DH，因为它在树状结构机器人（比如带分支的协作臂）中更直观。
1.5 知识体系总览
为了让你对本章内容有个整体印象，我画了一张结构图。它把坐标系、变换矩阵、齐次坐标、旋转表示和DH参数串在了一起——这就是机器人运动学的完整知识链条。
1.6 实战：给你的机器人建立DH参数表
光说不练假把式。我们拿一个常见的六轴工业机器人（比如类似KUKA KR6的构型）来实操一下。
第一步，按照改进DH的规则，给每个关节建立坐标系：
- 基座坐标系{0}：Z0沿关节1轴线向上
- 关节1坐标系{1}：Z1沿关节2轴线，X1由Z0和Z1的公垂线定义
- 以此类推，直到末端坐标系{6}
第二步，测量或从图纸读取四个DH参数。这里我给出一个典型值（单位：mm和度）：
| 关节 i | ai-1 (mm) | αi-1 (°) | di (mm) | θi (°) |
|---|---|---|---|---|
| 1 | 0 | 0 | 400 | θ1 |
| 2 | 250 | -90 | 0 | θ2 |
| 3 | 560 | 0 | 0 | θ3 |
| 4 | 125 | -90 | 515 | θ4 |
| 5 | 0 | 90 | 0 | θ5 |
| 6 | 0 | -90 | 90 | θ6 |
有了这张表，你就可以用前面给的公式，写出每个相邻关节的变换矩阵T1到T6。然后连乘：
T_06 = T_1 * T_2 * T_3 * T_4 * T_5 * T_6这个T_06就是末端执行器在基座坐标系下的位姿。给定六个关节角，你就能算出末端在哪、朝向哪——这就是正运动学。
一个小技巧：写代码验证时，先把所有关节角设成0。这时候机器人应该处于“零位”姿态。拿你的DH参数表手算一遍T_06，再和代码输出对比。如果对不上，99%是某个参数的正负号搞反了。我当年就因为这个，debug了整整一个下午。