三维坐标变换：欧拉角、旋转矩阵、四元数基础

说到三维坐标变换，这可能是感知融合里最绕不开的基础了。我刚开始做融合的时候，天天跟这几个概念打交道。说白了，我们感知到的每一个障碍物、每一帧点云，都离不开坐标系之间的来回倒腾。

今天咱们就聊聊三个最常用的旋转表达方式：欧拉角、旋转矩阵、四元数。嗯，它们各有各的脾气，也各有各的坑。

欧拉角：直观但容易翻车

欧拉角可能是最直观的旋转表达方式了。你想想看，一个物体在三维空间里怎么转？绕X轴转一下（横滚roll），绕Y轴转一下（俯仰pitch），绕Z轴转一下（航向yaw）。

我习惯用Z-Y-X的顺序，也就是先绕Z轴转yaw，再绕Y轴转pitch，最后绕X轴转roll。这个顺序在自动驾驶里很常见，因为yaw对应车头的朝向，pitch对应上下坡，roll对应侧倾。

但欧拉角有个大坑——万向锁。我在项目里就踩过这个坑。有一次做车辆姿态估计，pitch角接近90度的时候，roll和yaw突然就分不清了，姿态直接崩了。为什么会这样？因为当pitch=±90°时，roll和yaw的旋转轴重合了，自由度丢失了一个。

旋转矩阵：计算友好但冗余

旋转矩阵是个3×3的矩阵，它能把一个向量从一个坐标系变换到另一个坐标系。说白了，你有一个点P在坐标系A里的坐标，乘以旋转矩阵R，就能得到它在坐标系B里的坐标。

旋转矩阵有几个重要性质：

• 它是正交矩阵，即 R^T = R^-1
• 行列式为+1
• 三个列向量互相正交且模长为1

从欧拉角到旋转矩阵的转换公式长这样（Z-Y-X顺序）：

R = Rz(yaw) * Ry(pitch) * Rx(roll)

其中：
Rz(ψ) = [[cosψ, -sinψ, 0],
         [sinψ,  cosψ, 0],
         [0,      0,    1]]

Ry(θ) = [[cosθ,  0, sinθ],
         [0,     1,   0],
         [-sinθ, 0, cosθ]]

Rx(φ) = [[1,   0,    0],
         [0, cosφ, -sinφ],
         [0, sinφ,  cosφ]]

我个人习惯在代码里直接用旋转矩阵做点云变换，因为矩阵乘法效率高，而且可以串联多个变换。但旋转矩阵有9个参数，实际上只有3个自由度，冗余信息太多了。这就引出了四元数。

四元数：优雅且高效

四元数是个神奇的东西。我第一次接触它的时候觉得挺抽象，但用久了发现真香。四元数用4个参数表示旋转，没有万向锁，插值平滑，计算效率也高。

一个四元数可以写成：

q = w + xi + yj + zk

其中 w 是实部，x、y、z 是虚部
且满足 i² = j² = k² = ijk = -1

单位四元数（模长为1）可以用来表示旋转。给定一个旋转轴（单位向量）和旋转角度θ：

q = [cos(θ/2), sin(θ/2)*nx, sin(θ/2)*ny, sin(θ/2)*nz]

嗯，这里要注意，四元数的顺序在不同库里有不同约定。Eigen和ROS用的是[w, x, y, z]顺序，但有些库用[x, y, z, w]。我曾经因为这个顺序搞反，调试了一整天，最后发现是四元数顺序的问题。

三种表达方式的对比

实际项目中的选择建议

我在实际项目中一般这样用：

• 人机交互：用欧拉角，因为直观好理解。比如在可视化界面里显示车辆姿态。
• 内部计算：用四元数，避免万向锁，方便做姿态插值和滤波。
• 点云变换：用旋转矩阵，因为可以直接跟平移向量组合成齐次变换矩阵。

说白了，没有哪种表达方式是万能的。关键是要理解它们的优缺点，在合适的场景用合适的工具。

最后分享一个小技巧：在代码里写一个转换工具函数，把三种表达方式互相转换，然后写单元测试验证。这样不管后续用什么接口，都能保证转换正确。我每次新开项目都会先写这个工具，省了不少调试时间。

可可精品资料库