3. 刚体变换与位姿表示：旋转矩阵、平移向量、齐次变换矩阵、欧拉角、四元数
各位工程师朋友，今天我们来聊聊手眼标定里最基础、也最绕不开的一块内容——刚体变换与位姿表示。说实话，我当年刚入行时，被这些矩阵和角度搞得晕头转向。后来在产线上调试机器人，才真正体会到：搞不懂位姿表示，标定精度就是空中楼阁。
你想想看，机器人末端抓着一个工件，相机拍到了它。这两个坐标系之间怎么对齐？靠的就是刚体变换。说白了，就是描述一个物体在三维空间里怎么转、怎么移。
核心概念：刚体变换 = 旋转 + 平移。物体形状不变，只是位置和姿态变了。
3.1 旋转矩阵：最直观的旋转表示
旋转矩阵是个3x3的方阵。它的每一列，代表旋转后的新坐标系在原来坐标系中的投影。我记得第一次在项目里用旋转矩阵，是在一个六轴机器人上做手眼标定。当时我手动计算了相机和机器人末端的旋转关系，结果发现矩阵不正交——嗯，这里要注意，旋转矩阵必须是正交矩阵，行列式为+1。
为什么要求正交？因为旋转不改变向量的长度。如果矩阵不正交，旋转后向量长度变了，那就不是纯旋转了，里面混进了缩放。
// 绕Z轴旋转θ角的旋转矩阵
R_z(θ) = [
[cosθ, -sinθ, 0],
[sinθ, cosθ, 0],
[0, 0, 1]
]我在项目中遇到过一个问题：用旋转矩阵做连续旋转时，多次乘法后数值误差累积，矩阵不再正交。解决办法是定期做正交化处理，或者改用四元数。
个人习惯：我一般用旋转矩阵做中间计算，最终存储和传输用四元数。这样既保证精度，又节省空间。
3.2 平移向量：简单但容易出错
平移向量就是个3x1的列向量，表示从原坐标系原点指向新坐标系原点的矢量。听起来简单吧？但实际标定时，平移向量的方向经常搞反。
我曾经在一条汽车产线上调试视觉引导抓取，机器人总是抓偏。查了两天，最后发现是平移向量的符号反了。相机坐标系下的偏移，到了机器人坐标系下要取反。这个坑，我踩过，希望你别踩。
注意：平移向量依赖于旋转。先旋转，再平移，顺序不能乱。齐次变换矩阵就是把这个顺序固定下来。
3.3 齐次变换矩阵：把旋转和平移打包
齐次变换矩阵是个4x4的矩阵。左上角3x3是旋转矩阵，右上角3x1是平移向量，最后一行是[0,0,0,1]。为什么要加这一行？为了能用矩阵乘法统一处理旋转和平移。
T = [
[R(3x3), t(3x1)],
[0 0 0, 1 ]
]你想想看，如果没有齐次坐标，你要先做旋转矩阵乘法，再加平移向量。有了齐次变换矩阵，一次乘法搞定。在机器人运动学里，串联多个关节的变换，就是一连串的齐次矩阵相乘。
我个人习惯在代码里用4x4矩阵存储所有位姿数据。虽然多了一行一列，但代码逻辑统一，不容易出错。
3.4 欧拉角：直观但小心万向锁
欧拉角用三个角度表示旋转：绕X轴转（滚转）、绕Y轴转（俯仰）、绕Z轴转（偏航）。听起来很直观，对吧？但实际用起来，有个大坑——万向锁。
什么是万向锁？当俯仰角达到±90度时，滚转和偏航的旋转轴重合，丢失了一个自由度。我在做无人机云台控制时遇到过这个问题。云台在俯仰接近90度时，航向控制突然失灵，画面乱转。
避坑指南：如果你需要全姿态控制，别用欧拉角。用四元数或者旋转矩阵。欧拉角只适合小角度变化或者人机交互界面显示。
欧拉角的另一个问题是：旋转顺序不同，结果不同。常见的顺序有ZYX、XYZ等。做手眼标定时，一定要确认你用的库是哪种顺序。我见过两个团队用不同顺序算出来的结果对不上，吵了半天。
3.5 四元数：高效且无奇点
四元数是个超复数，形式为 q = w + xi + yj + zk，其中w是实部，x,y,z是虚部。它用四个数表示旋转，没有万向锁问题，插值平滑，计算效率高。
我记得第一次接触四元数是在做视觉SLAM时。当时觉得这东西很抽象，后来发现它其实就是绕着某个轴转某个角度。轴角表示法：绕单位向量(u,v,w)旋转θ角，对应的四元数是：
q = [cos(θ/2), u*sin(θ/2), v*sin(θ/2), w*sin(θ/2)]四元数乘法可以组合旋转，而且不会像旋转矩阵那样累积误差。我在手眼标定项目中，最终存储的标定结果都用四元数。为什么？因为传输时只需要4个浮点数，比9个数的旋转矩阵省一半多。
实用技巧：四元数一定要归一化。非单位四元数会导致旋转后向量长度变化。我写了个断言函数，每次创建四元数后自动检查模长是否为1。
3.6 各种表示方法的转换
实际项目中，你经常需要在不同表示方法之间切换。比如相机标定输出的是旋转矩阵，机器人控制器用的是欧拉角，而你要用四元数做插值。
| 转换方向 | 常用方法 | 注意事项 |
|---|---|---|
| 旋转矩阵 → 欧拉角 | 根据矩阵元素反解角度 | 注意万向锁，检查分母是否为零 |
| 旋转矩阵 → 四元数 | 从矩阵迹计算 | 数值稳定性，大角度时用不同公式 |
| 四元数 → 旋转矩阵 | 直接代入公式 | 确保四元数已归一化 |
| 欧拉角 → 四元数 | 三个旋转分别转四元数再相乘 | 注意旋转顺序 |
我建议你在代码里封装一个转换工具类，把所有转换函数写在一起。这样调试时方便，也避免不同模块用不同表示法导致混乱。
3.7 知识体系总览
下面这张图是我自己整理的刚体变换知识结构。你可以看到，所有表示方法最终都服务于同一个目标：描述三维空间中的位姿。
从这张图你可以看到，五种表示方法各有优劣。旋转矩阵适合数学推导，齐次变换矩阵适合串联计算，欧拉角适合人机交互，四元数适合存储和插值。平移向量最简单，但方向最容易搞反。
在实际手眼标定项目中，我通常这样用：标定过程中用齐次变换矩阵做计算，标定结果用四元数+平移向量存储，调试时用欧拉角查看。这样各取所长，效率最高。
总结一下：刚体变换是手眼标定的数学基础。旋转矩阵、平移向量、齐次变换矩阵、欧拉角、四元数，这五种表示法你都得会用。不用背公式，但要知道它们怎么转换、各自有什么坑。我在项目里吃过亏的地方，上面都标出来了，希望能帮你少走弯路。
公众号:蓝海资料掘金营，微信deep3321