3. 刚体变换数学基础：旋转矩阵、平移向量、齐次坐标、欧拉角与四元数

各位同学，欢迎来到第三讲。今天我们要啃一块硬骨头——刚体变换的数学基础。

说实话，我刚入行那会儿，觉得这些数学公式离工程很远。直到有一次，我标定完激光雷达和相机，发现投影出来的点云总是歪的。排查了三天，最后发现是旋转矩阵的乘法顺序搞反了。嗯，从那以后，我再也不敢小看这些基础了。

你想想看，自动驾驶里，传感器装在车上，车在动，障碍物也在动。我们要把不同坐标系下的数据对齐，靠的就是刚体变换。说白了，就是解决「这个东西在A坐标系下是(x,y,z)，在B坐标系下是多少？」的问题。

旋转矩阵，我习惯叫它「方向描述器」。它是一个3x3的正交矩阵，行列式为+1。

为什么用矩阵？因为计算机处理矩阵运算太方便了。你想想，一个3x3的矩阵乘上一个三维向量，结果还是三维向量。这不就是旋转吗？

举个最简单的例子，绕Z轴旋转θ角：

R_z(θ) = | cosθ  -sinθ  0 |
         | sinθ   cosθ  0 |
         | 0      0     1 |

我在项目中遇到过一个问题：两个旋转矩阵相乘，顺序不同结果完全不同。记住，R1 * R2 表示先做R2旋转，再做R1旋转。这个顺序千万别搞反。

⚠️ 我曾经踩过的坑： 旋转矩阵虽然是9个元素，但自由度只有3。这意味着它不是随便一个3x3矩阵都能当旋转矩阵用。必须满足正交且行列式为1。否则你算出来的坐标会「变形」。

平移就简单多了。一个三维向量 t = [tx, ty, tz]^T 就搞定了。

但这里有个细节：平移向量表示的是「从原坐标系原点指向新坐标系原点的向量」。方向别搞反了。

我记得有一次，同事把平移向量加反了方向，结果点云投影到图像上，整体偏移了2米。排查了半天，就是正负号的问题。

旋转用乘法，平移用加法。能不能统一成一种运算？

能。齐次坐标就是干这个的。

把一个三维点 (x, y, z) 变成四维 (x, y, z, 1)。然后构造一个4x4的变换矩阵：

T = | R   t |
    | 0   1 |

其中R是3x3旋转矩阵，t是3x1平移向量。

这样，一次矩阵乘法就完成了旋转+平移：

| x' |   | R   t | | x |
| y' | = |       | | y |
| z' |   |       | | z |
| 1  |   | 0   1 | | 1 |

  💡 我个人习惯： 在代码里，我永远用4x4齐次矩阵来存储外参。虽然多了一个维度，但代码逻辑统一，不容易出错。尤其是在做多传感器融合时，多个变换矩阵连乘，用齐次坐标简直不要太爽。

欧拉角用三个角度来描述旋转：偏航(Yaw)、俯仰(Pitch)、横滚(Roll)。

直观吧？太直观了。你一说「车头偏左30度」，大家都能理解。

但欧拉角有个致命问题——万向锁。

当俯仰角达到±90度时，偏航和横滚会耦合在一起，丢失一个自由度。这时候你无论怎么调偏航角，效果都跟横滚一样。

我在做无人机姿态估计时遇到过这个问题。飞机垂直爬升时，姿态解算突然就崩了。后来换成四元数，问题才解决。

四元数，说白了就是一个实部加三个虚部：q = w + xi + yj + zk。

为什么工程上爱用四元数？三个原因：

四元数旋转一个三维向量的公式：

p' = q * p * q^(-1)

其中p是纯四元数(0, x, y, z)，q是单位四元数。

🔧 我的建议： 在代码里，用四元数做旋转计算，用欧拉角做人机交互。内部计算用四元数，显示给用户看用欧拉角。这样既避免了万向锁，又保持了直观性。

下面这张图，是我梳理的刚体变换知识体系。你把它存下来，以后遇到坐标系变换的问题，对着这张图找对应的方法就行。

这张图把刚体变换拆成了三个部分：旋转、平移、齐次坐标。旋转又有三种表达方式。最终它们统一到刚体变换公式里，应用到传感器标定、点云投影等实际场景。

好了，这一讲的内容就到这里。数学基础打牢了，后面讲外参标定实战的时候，你才能理解每一步在干什么。

可可精品资料库