3、刚体变换与位姿表示：旋转矩阵、平移向量、齐次变换矩阵、欧拉角、四元数
各位，欢迎来到第三章。这一章是手眼标定的数学基础，说白了就是解决一个核心问题：如何精确描述一个物体在三维空间里的位置和朝向。
我刚开始做机器人项目时，总觉得这些数学工具是纸上谈兵。直到有一次，我亲手搭建的视觉抓取系统，明明坐标算对了，机械臂就是抓不到工件。折腾了两天，最后发现是旋转矩阵用错了顺序。嗯，从那以后，我再也不敢小看这些“基础概念”了。
今天我们就把这几个概念彻底讲透。你想想看，搞懂了它们，手眼标定的底层逻辑你就掌握了八成。
3.1 刚体变换：空间中的“搬家”与“转身”
先说说什么是刚体变换。一个刚体，比如你手里的一个零件，它在空间里移动时，形状和大小不会变。这种变换只包含两件事：平移（搬家）和旋转（转身）。
为什么手眼标定离不开它？因为相机看到的物体坐标，和机器人抓取时用的坐标，是两个不同的“家”。我们需要一个数学工具，把物体从一个坐标系“搬”到另一个坐标系。这就是刚体变换的使命。
核心公式：
空间中一点 P，在坐标系 A 中的坐标是 PA，在坐标系 B 中的坐标是 PB。它们的关系是：
PA = RAB · PB + tAB
其中 RAB 是旋转矩阵，tAB 是平移向量。
3.2 旋转矩阵：最可靠的“方向描述器”
旋转矩阵，我个人的理解是——它是描述旋转最“老实”的方式。为什么这么说？因为它有严格的数学约束。
一个 3×3 的旋转矩阵 R，必须满足两个条件：
- 正交性：RTR = I，即矩阵的逆等于它的转置。
- 行列式为 +1：保证是纯旋转，没有镜像。
我在项目中遇到过一个问题：用欧拉角算出来的旋转，在连续运动时出现了“万向锁”，导致机械臂突然卡死。后来我改用旋转矩阵做插值，问题就解决了。旋转矩阵虽然参数多（9个元素），但它没有奇异性，非常稳定。
实用技巧：
旋转矩阵的每一列，其实就是旋转后新坐标系的基向量在原坐标系中的投影。记住这一点，很多推导就变得直观了。
3.3 平移向量：简单但容易出错
平移向量 t 就是一个 3×1 的向量 [x, y, z]T，表示两个坐标系原点之间的位移。听起来很简单，对吧？
但这里有个坑：平移向量是在哪个坐标系下表示的？
我曾经犯过一个错误：把相机坐标系下的平移向量，直接当成机器人坐标系下的平移来用。结果标定出来的结果完全不对。记住，公式 PA = R·PB + t 中的 t，是从 B 系原点到 A 系原点的向量，在 A 系下的表示。
避坑指南：
我曾经在标定脚本里把平移向量的符号搞反了，导致标定结果收敛到一个错误值。检查了两天才发现。建议你在代码里显式注释：# t_AB: 从B系到A系的平移，在A系下表示。
3.4 齐次变换矩阵：把旋转和平移“打包”
每次都要写 PA = R·PB + t，有点麻烦。而且旋转和平移是分开的，不利于连续变换。于是就有了齐次变换矩阵。
它是一个 4×4 的矩阵：
| R t |
| 0 1 |其中 R 是 3×3 旋转矩阵，t 是 3×1 平移向量，最后一行是 [0, 0, 0, 1]。
用齐次坐标表示点 P = [x, y, z, 1]T，变换就变成了简单的矩阵乘法：
PA = TAB · PB
连续变换也只需要矩阵相乘：TAC = TAB · TBC。这就是手眼标定中“链式法则”的数学基础。
为什么叫“齐次”？
因为引入了第4维，把非线性变换（旋转+平移）变成了线性变换（矩阵乘法）。这在计算机视觉和机器人学里是标准做法。
3.5 欧拉角：直观但“有脾气”
欧拉角用三个角度（比如绕 X、Y、Z 轴的旋转角）来描述朝向。非常直观，一看就懂。
但它有三个问题：
- 万向锁：当中间轴旋转到 ±90° 时，会丢失一个自由度。
- 顺序依赖：XYZ、ZYX、ZYZ……不同顺序结果完全不同。
- 插值困难：在两个欧拉角之间做线性插值，得到的路径往往不是最短路径。
我记得有一次做视觉引导，用欧拉角给机械臂发姿态指令。结果在某个特定角度，机械臂突然做了一个大回环动作，差点撞到旁边的设备。后来查出来是万向锁导致的。从那以后，我在内部计算中只用旋转矩阵或四元数，只在人机交互界面才显示欧拉角。
| 表示方式 | 优点 | 缺点 |
|---|---|---|
| 旋转矩阵 | 无奇异性，适合计算 | 参数多，不直观 |
| 欧拉角 | 直观，参数少 | 万向锁，顺序依赖 |
| 四元数 | 无奇异性，适合插值 | 不够直观 |
3.6 四元数：工程师的“瑞士军刀”
四元数，听起来很玄乎，其实就是一个四维向量 [w, x, y, z]，满足 w² + x² + y² + z² = 1。它用“轴角”的方式描述旋转：绕某个单位向量 (x, y, z) 旋转 θ 角度，对应的四元数是 [cos(θ/2), sin(θ/2)·x, sin(θ/2)·y, sin(θ/2)·z]。
为什么我推荐在代码里用四元数？
- 无万向锁：这是最大的优势。
- 插值平滑：球面线性插值（SLERP）可以生成最短路径的旋转动画。
- 组合高效：两个四元数相乘就可以合成旋转，比矩阵乘法快。
你想想看，在手眼标定中，我们经常需要优化旋转参数。如果用旋转矩阵，9个参数还要满足正交约束，优化起来很麻烦。用四元数，只需要4个参数加一个归一化约束，简单多了。
代码示例：四元数与旋转矩阵互转（Python）
import numpy as np
from scipy.spatial.transform import Rotation as R
# 四元数 -> 旋转矩阵
quat = [0.707, 0.0, 0.707, 0.0] # [w, x, y, z]
rot_mat = R.from_quat(quat).as_matrix()
# 旋转矩阵 -> 四元数
rot_mat = np.eye(3)
quat = R.from_matrix(rot_mat).as_quat() # [x, y, z, w]3.7 知识体系总览
下面这张图，是我自己总结的位姿表示知识体系。你可以把它当作一个快速索引。
3.8 实战建议：怎么选？
说了这么多，到底该用哪种？我个人的经验是：
- 做标定计算：用齐次变换矩阵，方便链式相乘。
- 做优化求解：用四元数，参数少，无约束。
- 做数据存储：用旋转矩阵或四元数，避免欧拉角的歧义。
- 做调试显示：转成欧拉角，方便人眼理解。
重要提醒：
不同库对四元数元素顺序的定义可能不同。比如 SciPy 用的是 [x, y, z, w]，而 Eigen 用的是 [w, x, y, z]。我在项目里吃过这个亏，两个库互转时忘了调整顺序，结果标定结果全乱套了。建议你在代码开头就统一约定，并写清楚注释。
好了，这一章的内容就到这里。这些数学工具是手眼标定的“语言”，熟练使用它们，后面的章节你会走得很顺畅。