4. 旋转表示方法：旋转矩阵、欧拉角（RPY）、四元数、轴角表示及相互转换

说到旋转表示，这大概是刚入行时最容易踩坑的地方了。我记得第一次做多传感器融合，明明标定流程都走完了，点云投影到图像上就是歪的。折腾了两天才发现——是欧拉角的万向锁问题。从那以后，我对每种旋转表示法的脾气都摸得清清楚楚。

说白了，三维空间里的旋转，数学上有很多种描述方式。每种方式都有自己的长处和短板。你想想看，旋转矩阵直观但冗余，欧拉角好理解但会锁死，四元数计算稳但反直觉，轴角最自然但不好直接插值。咱们做工程，得知道什么时候该用哪个。

4.1 旋转矩阵

旋转矩阵是最基础的表示法。它是一个 3×3 的正交矩阵，行列式为 +1。用 R 表示，满足 RTR = I。

它的核心作用是把一个向量从一个坐标系变换到另一个坐标系。比如点 p 在坐标系 A 下的坐标是 pA，在坐标系 B 下的坐标是 pB，那么：

p_B = R_AB * p_A

这里 R_AB 表示从 A 到 B 的旋转矩阵。

旋转矩阵的性质：

  • 正交性：RTR = I，R-1 = RT
  • 行列式为 +1（纯旋转，不含反射）
  • 三个列向量互相正交，且都是单位向量
  • 9 个元素，但只有 3 个自由度

我在项目中一般不会直接用旋转矩阵做插值或优化。为什么？因为 9 个参数要满足 6 个约束方程，优化起来太麻烦。但旋转矩阵做坐标变换是最直接的，尤其在激光雷达点云变换时，我习惯用矩阵乘法批量处理。

4.2 欧拉角（RPY）

欧拉角用三个角度来描述旋转。最常用的是 RPY 顺序：先绕 X 轴转 Roll（横滚），再绕 Y 轴转 Pitch（俯仰），最后绕 Z 轴转 Yaw（偏航）。

嗯，这里要注意：欧拉角的顺序很重要。同样的三个角度，顺序不同，结果完全不同。我见过有人把 RPY 和 YPR 搞混，结果标定出来的外参全错了。

角度 符号 范围 物理含义
横滚 Roll (φ) [-π, π] 绕 X 轴旋转，像飞机侧倾
俯仰 Pitch (θ) [-π/2, π/2] 绕 Y 轴旋转，像飞机抬头低头
偏航 Yaw (ψ) [-π, π] 绕 Z 轴旋转，像飞机转向

万向锁（Gimbal Lock）： 当 Pitch = ±90° 时，Roll 和 Yaw 的旋转轴会重合，丢失一个自由度。我曾经在做车载激光雷达标定时，车辆停在坡道上，Pitch 刚好接近 90°，结果欧拉角表示直接崩了。从那以后，我只要涉及连续旋转或插值，一律用四元数。

欧拉角转旋转矩阵的公式（RPY 顺序）：

R = R_z(ψ) * R_y(θ) * R_x(φ)

其中：
R_x(φ) = [[1, 0, 0],
          [0, cosφ, -sinφ],
          [0, sinφ, cosφ]]

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

R_z(ψ) = [[cosψ, -sinψ, 0],
          [sinψ, cosψ, 0],
          [0, 0, 1]]

4.3 四元数

四元数是我个人最喜欢的旋转表示法。它用四个参数 q = (w, x, y, z) 来表示旋转，其中 w 是实部，x、y、z 是虚部。单位四元数满足 w² + x² + y² + z² = 1。

为什么我喜欢它？因为四元数没有万向锁，插值平滑（可以用 Slerp），而且计算效率高。在激光雷达 SLAM 和 IMU 融合中，我几乎只用四元数。

实用技巧： 四元数表示绕单位向量 (ux, uy, uz) 旋转 θ 角度：

q = (cos(θ/2), ux·sin(θ/2), uy·sin(θ/2), uz·sin(θ/2))

这个形式在做轴角转换时特别有用。

四元数乘法（表示旋转的复合）：

q1 * q2 = (w1*w2 - x1*x2 - y1*y2 - z1*z2,
           w1*x2 + x1*w2 + y1*z2 - z1*y2,
           w1*y2 - x1*z2 + y1*w2 + z1*x2,
           w1*z2 + x1*y2 - y1*x2 + z1*w2)

用四元数旋转一个向量 v：

v' = q * v * q_conj

其中 q_conj = (w, -x, -y, -z) 是共轭四元数。

4.4 轴角表示

轴角表示是最符合直觉的：用一个单位向量 u 表示旋转轴，一个标量 θ 表示旋转角度。记作 (u, θ) 或直接用向量形式 θ·u。

轴角的好处是参数少（3 个），物理意义明确。但它有两个问题：一是当 θ 接近 0 时，轴的方向不稳定；二是不能直接做插值。

轴角转旋转矩阵（罗德里格斯公式）：

R = I + sinθ * [u]× + (1 - cosθ) * [u]ײ

其中 [u]× 是 u 的反对称矩阵：
[u]× = [[0, -u_z, u_y],
        [u_z, 0, -u_x],
        [-u_y, u_x, 0]]

4.5 相互转换

实际工程中，我们经常需要在不同表示法之间来回切换。下面这张图是我自己总结的转换关系：

旋转表示法转换关系图 旋转矩阵 R 欧拉角 (RPY) 四元数 q 轴角 (u, θ) 公式推导 特征值法 罗德里格斯 RPY公式 直接展开 罗德里格斯 转换公式 半角公式 工程建议： • 坐标变换计算 → 用旋转矩阵（批量处理快） • 人机交互/调试 → 用欧拉角（直观） • 插值/优化/SLAM → 用四元数（无万向锁） • 物理意义分析 → 用轴角（最自然）

4.6 关键转换公式

下面是我工作中最常用的几个转换，直接贴代码：

四元数 → 旋转矩阵

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²)]]

旋转矩阵 → 四元数

w = sqrt(1 + R[0][0] + R[1][1] + R[2][2]) / 2
x = (R[2][1] - R[1][2]) / (4*w)
y = (R[0][2] - R[2][0]) / (4*w)
z = (R[1][0] - R[0][1]) / (4*w)

数值稳定性： 当 w 接近 0 时，上面公式会出问题。我一般会判断 w 的大小，如果 w 太小，就改用其他三个元素的最大值做分母。具体实现时，可以比较 R 的迹，选择数值最稳定的分支。

欧拉角 → 四元数（RPY 顺序）

q = q_z(yaw) * q_y(pitch) * q_x(roll)

其中：
q_x(roll)  = (cos(roll/2), sin(roll/2), 0, 0)
q_y(pitch) = (cos(pitch/2), 0, sin(pitch/2), 0)
q_z(yaw)   = (cos(yaw/2), 0, 0, sin(yaw/2))

轴角 → 四元数

q = (cos(θ/2), u_x*sin(θ/2), u_y*sin(θ/2), u_z*sin(θ/2))

四元数 → 轴角

θ = 2 * acos(w)
u = (x, y, z) / sin(θ/2)

避坑指南： 我曾经在写四元数转轴角时，忘了处理 θ=0 的情况。结果 sin(0)=0，除零错误直接让程序崩溃。后来我加了一个判断：如果 w 接近 1，直接返回 θ=0，轴任意。

4.7 工程选型建议

说了这么多，到底该用哪个？我个人的经验是：

  • 做标定优化：用四元数。因为四元数只有 4 个参数加一个单位约束，优化起来比旋转矩阵的 9 个参数加 6 个约束简单得多。
  • 做点云变换：用旋转矩阵。因为矩阵乘法可以直接用 GPU 或 SIMD 加速，批量处理几百万个点很高效。
  • 做可视化调试：用欧拉角。人眼一看就懂，方便排查问题。
  • 做 IMU 积分：用四元数。因为 IMU 的角速度积分天然适合四元数更新。

你想想看，如果选错了表示法，轻则代码跑得慢，重则出现万向锁或者数值不稳定。所以每次开始一个新模块，我都会先想清楚：这个场景下，哪种旋转表示最合适？

← 上一章 📖 返回目录 下一章 →