3、刚体变换与位姿表示：旋转矩阵、欧拉角、四元数、齐次变换矩阵
做手眼标定，说白了就是在算两个东西：旋转和平移。
你想想看，机器人末端抓着一个相机，相机看到的物体在相机坐标系里有个位置。我们要把这个位置换算到机器人基座标系里去。这中间经过了多少次坐标变换？少说三四次。每次变换都涉及旋转加平移。
所以，怎么表示旋转？怎么把旋转和平移打包成一个整体去算？这就是本章要解决的核心问题。
3.1 旋转矩阵：最直观，但最啰嗦
旋转矩阵是一个 3×3 的正交矩阵，行列式为 +1。它描述了一个坐标系相对于另一个坐标系的姿态。
核心性质：
- RT = R-1（正交性）
- det(R) = +1（右手系）
- 9 个元素，但只有 3 个自由度
我在项目中遇到过一个问题：用旋转矩阵做连续变换时，矩阵乘法会累积浮点误差。时间一长，矩阵就不再正交了。你拿它去旋转一个点，结果会变形。
我的习惯：每次做完旋转矩阵乘法，都做一次正交化修正。代码就两行：
U, _, Vt = np.linalg.svd(R)
R_corrected = U @ Vt这能保证 R 永远是正经的旋转矩阵。
旋转矩阵的好处是直观，没有歧义。坏处是参数太多，9 个数存 3 个自由度，冗余。而且人眼根本看不懂——你给我一个 3×3 的矩阵，你能一眼看出它转了 30° 还是 45°？反正我不能。
3.2 欧拉角：人看得懂，但小心万向锁
欧拉角用三个角度表示旋转，比如绕 X 轴转 α，绕 Y 轴转 β，绕 Z 轴转 γ。常见的顺序有 ZYX、ZYZ、XYZ 等。
| 欧拉角顺序 | 常见应用场景 | 特点 |
|---|---|---|
| ZYX | 机器人运动学 | 偏航-俯仰-滚转，直观 |
| ZYZ | 手眼标定常用 | 避免某些奇异性 |
| XYZ | 相机姿态 | 固定轴顺序 |
万向锁（Gimbal Lock）：当第二个旋转角为 ±90° 时，第一个和第三个旋转轴重合，丢失一个自由度。
我曾经在调试一个六轴机器人时，用欧拉角做插值，结果在某个姿态附近机器人突然卡住不动了。查了半天，万向锁。从那以后，只要涉及插值或优化，我坚决不用欧拉角。
欧拉角的优点是参数少（3 个），人可读。缺点是：
- 有歧义（同样的旋转，不同顺序结果不同）
- 有奇异性（万向锁）
- 不适合插值
3.3 四元数：工业界的最爱
四元数用 4 个数表示旋转：一个实部 + 三个虚部。写成 q = w + xi + yj + zk，其中 w² + x² + y² + z² = 1。
为什么工业界爱用四元数？
- 无奇异性——不存在万向锁
- 插值平滑——球面线性插值（SLERP）做姿态过渡非常自然
- 计算高效——比旋转矩阵少 5 个乘法
四元数 vs 旋转矩阵：
| 对比项 | 四元数 | 旋转矩阵 |
|---|---|---|
| 参数个数 | 4 | 9 |
| 约束条件 | 单位模长 | 正交 + 行列式=1 |
| 插值 | SLERP 平滑 | 困难 |
| 组合旋转 | 乘法（非交换） | 乘法（非交换） |
| 人可读性 | 差 | 差 |
我个人习惯在手眼标定的内部计算中全部用四元数。只在输入输出时转成欧拉角给人看。
四元数转欧拉角的坑：不同库的转换公式可能不一样。Eigen 和 SciPy 的转换结果差一个符号是常有的事。我建议你写一个测试用例，拿一个已知旋转验证一下。
3.4 齐次变换矩阵：把旋转和平移打包
齐次变换矩阵是一个 4×4 的矩阵：
T = | R t |
| 0 1 |其中 R 是 3×3 旋转矩阵，t 是 3×1 平移向量。
为什么要用齐次坐标？
这个问题我问过不少新人。答案往往是「为了把平移和旋转统一成矩阵乘法」。对，但不全对。
更深层的原因是：在欧氏空间中，平移不是线性变换。你想想看，一个点 p 先旋转再平移：
p' = R·p + t
这不是线性变换——因为有加法。但如果你把 p 写成齐次坐标 [x, y, z, 1]ᵀ，那么：
p' = T·p
就变成了纯粹的矩阵乘法。这意味着你可以把多个变换串联成一个矩阵：
T_total = T₁ · T₂ · T₃ · ...
这在手眼标定中太重要了。你想想，相机到标定板、标定板到机器人末端、末端到基座……这一串变换，最终就是一个 4×4 矩阵的事。
齐次坐标的另一个好处：可以表示无穷远点。当 w=0 时，[x, y, z, 0]ᵀ 表示方向向量，平移对它无效。这在计算机视觉中非常有用。
3.5 知识结构图
下面这张图总结了四种位姿表示方法的关系和适用场景：
3.6 实战建议
说了这么多理论，给几条实在的建议：
- 内部计算用四元数——避免万向锁，插值方便
- 输入输出用欧拉角——给人看的，记得注明旋转顺序
- 验证用旋转矩阵——正交性检查可以快速发现数值问题
- 串联变换用齐次矩阵——一个 4×4 矩阵搞定一切
避坑指南：我曾经在标定脚本里混用了不同顺序的欧拉角，结果标定出来的外参完全不对。查了两天才发现是 SciPy 的 from_euler 默认顺序是 'xyz'，而我用的机器人 SDK 是 'zyx'。从那以后，我所有代码里都显式写明顺序参数，绝不依赖默认值。
嗯，关于位姿表示就聊这么多。这些概念是手眼标定的数学基础，后面所有章节都会用到。你先把这几种表示方法之间的转换写熟练了，后面会轻松很多。