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

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

各位同学，咱们今天聊点硬核的。手眼标定，说白了就是算清楚相机和机器人手臂之间那个「别扭」的姿势——到底偏了多少、转了多少。这个姿势，在数学上就叫刚体变换。

我刚开始做标定那会儿，总觉得这东西不就是个矩阵乘法嘛，有啥好讲的？直到有一次在现场，机器人抓件总是差那么几毫米，查来查去才发现是旋转矩阵的表示方式搞混了。嗯，从那以后，我对位姿表示就再也不敢马虎了。

想象一下，你手里拿着一个杯子，从桌面位置 A 挪到位置 B。这个过程中，杯子本身没变形、没摔碎——这就是刚体。刚体变换就是描述这种「搬家」的数学工具，它包含两部分：旋转和平移。

核心公式：

空间中一点 P，在坐标系 {A} 下的坐标为 P_A，在坐标系 {B} 下的坐标为 P_B，那么：

P_A = R_AB · P_B + t_AB

其中 R_AB 是旋转矩阵，t_AB 是平移向量。

说白了，就是先转个方向，再挪个位置。我在项目中见过不少新手，把旋转和平移的顺序搞反了，结果算出来的位姿完全不对。记住：先旋转，后平移，这是铁律。

旋转矩阵是个 3×3 的方阵，它的每一列代表旋转后坐标系的基向量在原坐标系下的投影。听起来有点绕？你只要记住它的三个关键性质就行：

我个人习惯在代码里用旋转矩阵做中间计算，因为它加减乘除都方便。但你要是让我手写一个旋转矩阵……嗯，我宁愿用欧拉角。

避坑指南：我曾经在标定程序里直接用旋转矩阵做优化，结果因为正交性约束没加进去，算出来的矩阵行列式变成了 0.98，导致后续误差越传越大。后来我改用四元数做优化，最后再转回矩阵，问题就解决了。

平移向量就是个 3×1 的列向量 [x, y, z]^T，表示两个坐标系原点之间的偏移。听起来简单吧？但单位问题经常坑人。

我记得有一次帮客户排查问题，标定结果总是跳变。查了三天，最后发现相机内参用的是毫米，机器人给的位姿是米，差了 1000 倍。你说冤不冤？

每次都要写 R 和 t 太麻烦，能不能把它们合并成一个东西？当然可以——齐次变换矩阵 T 就是干这个的：

T = [ R   t ]
    [ 0   1 ]

其中：
R: 3×3 旋转矩阵
t: 3×1 平移向量
0: 1×3 零向量
1: 标量 1

这样一来，刚体变换就变成了一个矩阵乘法：P_A = T_AB · P_B，其中 P 是齐次坐标 [x, y, z, 1]^T。

你想想看，有了齐次矩阵，多个变换连起来就简单了：T_AC = T_AB · T_BC。这就是手眼标定里「链式法则」的基础。

欧拉角用三个角度（比如绕 X、Y、Z 轴的转角）来描述旋转。常见的顺序有 ZYX、ZYZ、XYZ 等。我一般用 ZYX 顺序，也就是先绕 Z 转（偏航），再绕 Y 转（俯仰），最后绕 X 转（滚转）。

万向锁警告：当第二个旋转角为 ±90° 时，第一个和第三个旋转轴会重合，丢失一个自由度。我在做机械臂标定时就遇到过这个问题——腕部关节接近奇异位形，欧拉角突然跳变，标定算法直接崩了。

所以我的建议是：欧拉角只用来给人看，别用来做计算。显示姿态、调试界面用欧拉角很方便，但内部运算还是用四元数或旋转矩阵更稳。

四元数是个超复数，形式为 q = w + xi + yj + zk，其中 w 是实部，(x, y, z) 是虚部。它用 4 个参数表示旋转，没有冗余，也没有奇点。

我特别喜欢四元数的两个特性：

不过四元数也有个坑：q 和 -q 表示同一个旋转。我在写优化算法时，经常需要加一个约束，确保四元数的实部为正，否则迭代过程会来回震荡。

实用技巧：从旋转矩阵转四元数时，注意数值稳定性。如果矩阵的迹接近 -1，要用特殊公式。OpenCV 的 Rodrigues() 函数可以帮你做旋转向量和旋转矩阵的互转，我经常用它做中间桥梁。

旋转向量用一个 3 维向量表示旋转：方向表示旋转轴，模长表示旋转角度。这就是著名的罗德里格斯公式的基础。

它的优点是：

缺点嘛，就是角度超过 π 时会有歧义。我在做手眼标定的非线性优化时，经常用旋转向量作为优化变量，因为它不需要额外的约束条件。

下面这张图总结了刚体变换与位姿表示的核心逻辑，帮你理清思路：

讲了这么多，最后给你几条我在项目中摸爬滚打总结出来的经验：