📚 课程目录第3章刚体变换与位姿表示

3. 刚体变换与位姿表示：旋转矩阵、平移向量、齐次变换矩阵、欧拉角、四元数

好，咱们进入第三章。这一章是手眼标定的数学地基，说白了就是搞清楚「机器人怎么描述自己在哪、朝哪看」。你想想看，如果连位置和姿态都说不清楚，那标定就无从谈起。

我个人习惯把刚体变换拆成两个部分：旋转和平移。旋转描述「朝向」，平移描述「位置」。两者合在一起，就是一个完整的位姿。

3.1 旋转矩阵：最直观的旋转表示

旋转矩阵是个 3×3 的方阵，它的每一列代表旋转后的坐标轴在原坐标系下的投影。我记得刚学的时候总觉得这东西抽象，后来在项目里用多了才发现，它其实就是个「坐标转换器」。

举个例子，一个点 p 在世界坐标系下的坐标是 (x, y, z)，如果相机旋转了，那这个点在相机坐标系下的坐标就是 p' = R · p。这里的 R 就是旋转矩阵。

旋转矩阵的性质：

正交矩阵：R^T = R^-1，逆矩阵等于转置
行列式为 +1：保证是纯旋转，没有缩放或镜像
9 个元素，但只有 3 个自由度（欧拉角就是这 3 个自由度的另一种表达）

我的经验：在实际代码里，千万别手动算旋转矩阵的逆。直接用 R.T 就行，又快又准。我曾经见过有人用 np.linalg.inv(R)，数值误差大不说，还慢。

3.2 平移向量：简单但容易踩坑

平移向量就是个 3×1 的列向量，表示从原点到目标点的位移。听起来很简单对吧？但这里有个坑——平移的方向。

假设相机在世界坐标系下的位置是 t，那世界坐标系下的点 p_w 转换到相机坐标系就是：

p_c = R · p_w + t

注意这里的 t 是「世界原点在相机坐标系下的坐标」，而不是「相机在世界坐标系下的坐标」。这两个概念刚好相反，我刚开始做标定时就被这个绕晕过。

避坑指南：我曾经在标定项目里把平移向量搞反了，结果机械臂抓取时差了 5 厘米。后来养成了习惯：每次写完变换代码，先用一个已知点验证一下，确保方向正确。

3.3 齐次变换矩阵：把旋转和平移打包

旋转矩阵和平移向量分开用有点麻烦，尤其是做连续变换的时候。比如从 A 到 B，再从 B 到 C，你得先旋转再平移，写起来啰嗦。齐次变换矩阵就是来解决这个问题的。

它是个 4×4 的矩阵：

T = | R   t |
    | 0   1 |

其中 R 是 3×3 旋转矩阵，t 是 3×1 平移向量，最后一行是 [0, 0, 0, 1]。

用齐次坐标表示点 p = (x, y, z, 1)，变换就变成了简单的矩阵乘法：

p' = T · p

连续变换就是矩阵连乘：T_AC = T_AB · T_BC。这个在手眼标定里太常用了，因为我们要把相机坐标系、机械臂基坐标系、工具坐标系串起来。

齐次变换矩阵的逆：

T_inv = | R.T   -R.T · t |
         | 0      1        |

不用求逆矩阵，直接用这个公式算，效率高很多。

3.4 欧拉角：直观但小心万向锁

欧拉角用三个角度表示旋转：绕 X 轴转（滚转 Roll）、绕 Y 轴转（俯仰 Pitch）、绕 Z 轴转（偏航 Yaw）。顺序不同，结果也不同。常用的顺序有 ZYX、XYZ 等。

欧拉角顺序	常见应用场景	特点
ZYX	无人机、机器人	先偏航，再俯仰，最后滚转
XYZ	相机姿态	直观，但容易万向锁
ZYZ	机械臂运动学	避免万向锁的一种方式

万向锁：当第二个旋转角为 ±90° 时，第一个和第三个旋转轴会重合，丢失一个自由度。我在做机械臂路径规划时遇到过这个问题，当时机械臂突然「卡住」了，排查了半天才发现是欧拉角导致的。后来我改用四元数，再也没出过这个问题。

3.5 四元数：SLAM 和机器人的首选

四元数是个超复数，形式为 q = w + xi + yj + zk，其中 w 是实部，(x, y, z) 是虚部。它用 4 个参数表示旋转，没有万向锁，插值平滑，计算效率高。

我个人在视觉 SLAM 项目里几乎只用四元数。原因很简单：

无奇点：不会出现万向锁
插值方便：球面线性插值（SLERP）可以生成平滑的旋转路径
组合高效：两个四元数相乘就是旋转的复合

四元数到旋转矩阵的转换公式：

R = | 1-2(y²+z²)   2(xy-wz)     2(xz+wy)   |
    | 2(xy+wz)     1-2(x²+z²)   2(yz-wx)   |
    | 2(xz-wy)     2(yz+wx)     1-2(x²+y²) |

实用建议：在 Python 里用 scipy.spatial.transform.Rotation 模块，可以轻松在四元数、旋转矩阵、欧拉角之间转换。别自己手写转换函数，容易出错。

from scipy.spatial.transform import Rotation as R

# 四元数转旋转矩阵
q = [0.707, 0.0, 0.707, 0.0]  # w, x, y, z
rot = R.from_quat(q)
matrix = rot.as_matrix()

# 旋转矩阵转欧拉角
euler = rot.as_euler('zyx', degrees=True)

3.6 知识体系总览

下面这张图把本章的核心知识点串起来了。你可以看到，旋转矩阵是基础，欧拉角和四元数是它的两种变体，平移向量负责位置，齐次变换矩阵把两者合为一体。

3.7 实际项目中的选择建议

不同的场景适合不同的表示方式。我根据自己的项目经验，整理了一个选择指南：

场景	推荐表示	原因
手眼标定计算	齐次变换矩阵	方便连乘和求逆，公式简洁
视觉 SLAM 优化	四元数	无奇点，适合非线性优化
人机交互界面	欧拉角	直观，容易理解
机械臂运动学	齐次变换矩阵 + 四元数	正运动学用矩阵，插值用四元数

核心要点：不管用哪种表示，最终在代码里都要能互相转换。我建议你写一个工具类，封装好旋转矩阵、欧拉角、四元数、齐次变换矩阵之间的转换函数。这样在标定流程里，随时可以切换到最方便的表示方式。

嗯，这一章的内容就到这里。刚体变换是手眼标定的数学基础，后面的章节会反复用到这些概念。你先把旋转矩阵和齐次变换矩阵练熟，后面学标定算法时会轻松很多。