2. 运动学基础（上）：空间描述与变换、齐次坐标、旋转矩阵、欧拉角与四元数
各位同学，欢迎来到运动学基础的上半部分。说实话，这部分内容是整个机械臂控制的「地基」。地基没打好，后面建多高的楼都悬。我见过太多新手，一上来就调PID、跑轨迹，结果机械臂乱转，最后发现是坐标系搞反了——嗯，这种坑我踩过不止一次。
2.1 空间描述：我们到底在说哪个「位置」？
先问个问题：你说「机械臂末端在点P」，这个点P相对于谁？
你想想看，如果我说「杯子在桌子左边」，那是以桌子为参考。如果我说「杯子在我右手边」，那就是以我为参考。同一个杯子，描述完全不同。
在机器人学里，我们管这个叫参考坐标系。每个位置和姿态，都必须明确是相对于哪个坐标系说的。
核心概念：
- 位置：用3×1的向量表示，比如 p = [x, y, z]^T
- 姿态：用3×3的旋转矩阵表示，描述坐标系的朝向
- 位姿：位置 + 姿态，合起来就是完整的空间描述
我个人习惯把坐标系想象成「一个带箭头的三脚架」。原点就是位置，三个箭头指向就是姿态。你在空间中移动这个三脚架，就是在做变换。
2.2 齐次坐标：为什么非要加个1？
很多初学者看到齐次坐标就头大：「好好的三维向量，为什么非要变成四维？」
我当年也这么想。直到我在项目中需要连续做旋转+平移，发现用3×3矩阵根本没法同时处理——旋转是乘法，平移是加法，混在一起写代码特别容易出错。
齐次坐标的妙处就在于：把平移也变成乘法。
具体做法很简单：
- 普通三维点 (x, y, z) → 齐次坐标 (x, y, z, 1)
- 普通三维向量 (dx, dy, dz) → 齐次坐标 (dx, dy, dz, 0)
注意这个区别：点最后是1，向量最后是0。为什么？因为向量没有位置，只有方向，平移对它没影响。
我的小技巧：写代码时，我习惯把齐次变换矩阵存成4×4的numpy数组。这样旋转和平移可以一次搞定，调试起来也方便。
2.3 旋转矩阵：最直观，但也是最笨重的
旋转矩阵 R 是一个3×3的正交矩阵，满足 R^T R = I，行列式为+1。说白了，它就是一个「不改变向量长度，只改变方向」的变换。
举个例子，绕Z轴旋转θ角：
R_z(θ) = [cosθ -sinθ 0]
[sinθ cosθ 0]
[0 0 1]绕X轴、Y轴类似，只是把sin和cos放到对应位置。
旋转矩阵的好处是直观，坏处是——太占地方。9个数，其实只有3个自由度。而且连续旋转时，矩阵乘法容易累积误差。我在做高精度标定时就吃过这个亏，旋转矩阵稍微不正交，后面整个运动学就崩了。
注意：旋转矩阵必须保持正交性。如果数值计算导致矩阵不再正交，需要用SVD或Gram-Schmidt重新正交化。我一般用 R = U @ V.T 来修正。
2.4 欧拉角：简单但容易「锁死」
欧拉角用三个角度来描述旋转，比如 Z-Y-X 顺序：先绕Z转α，再绕新Y转β，最后绕新X转γ。
这种表示法很符合直觉——你想想看，调整云台的时候不就是先水平转、再俯仰吗？
但欧拉角有个著名的坑：万向锁。
当β = ±90°时，第一次旋转和第三次旋转的轴会重合，丢失一个自由度。我在做六轴机械臂逆解时，曾经因为欧拉角万向锁导致轨迹规划突然跳变——机械臂「嗖」一下翻了个跟头，吓得我赶紧按了急停。
所以我的建议是：
- 人机交互界面用欧拉角（直观）
- 内部计算尽量别用（容易出问题）
2.5 四元数：工程师的救星
四元数 q = w + xi + yj + zk，其中 w 是实部，x,y,z 是虚部。它用4个数表示旋转，没有奇点，插值平滑。
说实话，我第一次看四元数也觉得玄乎。但用多了你会发现，它其实就是「绕一个单位轴 n 旋转 θ 角」的数学表达：
q = [cos(θ/2), n_x * sin(θ/2), n_y * sin(θ/2), n_z * sin(θ/2)]四元数的好处：
- 没有万向锁
- 插值平滑（球面线性插值，SLERP）
- 计算效率高（比矩阵快）
我在做机械臂平滑轨迹规划时，全部用四元数做姿态插值。效果比欧拉角好太多了，不会出现突然的跳变。
实用技巧：ROS和很多机器人库都用四元数。如果你需要把四元数转成旋转矩阵，公式是固定的。我一般直接用scipy的 Rotation.from_quat()，省得自己手写容易出错。
2.6 知识体系总览
下面这张图是我自己整理的，把本章的核心逻辑串起来了。你一看就明白：
2.7 实战建议：怎么选？
说了这么多，到底用哪个？我个人的经验是：
| 场景 | 推荐表示 | 原因 |
|---|---|---|
| 人机交互（输入角度） | 欧拉角 | 直观，操作员容易理解 |
| 内部运动学计算 | 旋转矩阵 | 数学性质好，适合推导 |
| 姿态插值/平滑 | 四元数 | 无奇点，插值平滑 |
| 坐标变换链 | 齐次矩阵 | 统一处理旋转+平移 |
我曾经在一个项目里，三种表示换来换去，最后自己都晕了。后来我定了个规矩：内部统一用齐次矩阵，只在界面层转成欧拉角。这样代码清晰，也不容易出bug。
本章小结：
- 空间描述离不开坐标系，先搞清楚「相对于谁」
- 齐次坐标把平移变成乘法，4×4矩阵是万能工具
- 旋转矩阵直观但冗余，欧拉角有万向锁，四元数最实用
- 实际项目中，我建议内部统一用齐次矩阵，界面用欧拉角
好了，运动学基础的上半部分就到这里。这些概念你消化好了，下一章我们开始讲正运动学——也就是怎么根据关节角度算出末端位姿。到时候你会发现，今天学的这些变换矩阵，全都要用上。