3、旋转矩阵与欧拉角：旋转矩阵的数学定义、欧拉角（Roll/Pitch/Yaw）、旋转矩阵与欧拉角的相互转换
大家好，我是老K。今天咱们聊聊外参标定里最绕不开的两个概念——旋转矩阵和欧拉角。
说实话，我刚入行那会儿，看到一堆三角函数和矩阵乘法就头大。后来在项目里吃过几次亏，才真正搞明白它们是怎么回事。嗯，今天我把这些经验掰开了揉碎了讲给你听。
3.1 旋转矩阵的数学定义
旋转矩阵，说白了就是一个3x3的方阵。它描述了一个坐标系相对于另一个坐标系的朝向关系。
数学上，旋转矩阵有三个重要性质：
- 正交性：矩阵的每一行、每一列都是单位向量，且两两正交
- 行列式为+1：这保证了它是纯旋转，没有缩放或镜像
- 可逆性：旋转矩阵的逆等于它的转置
一个绕Z轴旋转θ角的旋转矩阵长这样：
R_z(θ) = [
[cosθ, -sinθ, 0],
[sinθ, cosθ, 0],
[0, 0, 1]
]绕X轴和Y轴的类似，只是把对应的行和列换成三角函数。
核心要点：旋转矩阵的每一列，其实就是原坐标系的基向量在新坐标系中的坐标表示。这个理解很重要，后面讲外参标定时会反复用到。
3.2 欧拉角：Roll、Pitch、Yaw
欧拉角是另一种描述旋转的方式。它用三个角度来表示：
- Roll（横滚角）：绕X轴旋转，就像飞机侧身
- Pitch（俯仰角）：绕Y轴旋转，就像飞机抬头低头
- Yaw（偏航角）：绕Z轴旋转，就像飞机左右转向
我个人习惯用RPY的顺序，也就是先绕Z轴转Yaw，再绕Y轴转Pitch，最后绕X轴转Roll。这个顺序在自动驾驶领域很常见。
小技巧：你可以把右手伸出来，拇指、食指、中指分别指向X、Y、Z轴方向。绕哪个轴转，哪个手指就指向旋转轴的正方向。我在项目里经常用这个手势跟同事沟通，比画图快多了。
3.3 旋转矩阵与欧拉角的相互转换
为什么要做转换？因为旋转矩阵适合计算，欧拉角适合人理解。你想想看，你给一个工程师说「旋转矩阵是0.707, -0.707, 0...」，他肯定一脸懵。但你说「偏航角45度」，他立马就懂了。
从欧拉角到旋转矩阵
假设我们按RPY顺序，先Yaw(ψ)、再Pitch(θ)、最后Roll(φ)：
R = R_z(ψ) · R_y(θ) · R_x(φ)展开后就是：
R = [
[cosψ·cosθ, cosψ·sinθ·sinφ - sinψ·cosφ, cosψ·sinθ·cosφ + sinψ·sinφ],
[sinψ·cosθ, sinψ·sinθ·sinφ + cosψ·cosφ, sinψ·sinθ·cosφ - cosψ·sinφ],
[-sinθ, cosθ·sinφ, cosθ·cosφ]
]看着复杂，但代码实现其实很简单。我在项目里都是直接调库，但建议你至少手推一遍，理解每个元素是怎么来的。
从旋转矩阵到欧拉角
反过来就有点坑了。从旋转矩阵提取欧拉角时，会遇到一个经典问题——万向锁。
避坑指南：当Pitch角接近±90度时，Roll和Yaw会变得无法区分。我曾经在一个标定项目里没处理这个边界情况，结果输出的欧拉角跳来跳去，排查了半天才发现是万向锁的问题。
提取公式如下（假设R已知）：
θ = -arcsin(R[2][0])
φ = atan2(R[2][1], R[2][2])
ψ = atan2(R[1][0], R[0][0])当θ接近±90度时，需要用不同的公式处理。具体来说：
- 如果θ ≈ 90度：φ = 0, ψ = atan2(R[0][1], R[1][1])
- 如果θ ≈ -90度：φ = 0, ψ = -atan2(R[0][1], R[1][1])
3.4 知识体系总览
下面这张图帮你理清本章的核心逻辑：
3.5 实际项目中的选择建议
我在做自动驾驶外参标定时，一般遵循这个原则：
- 存储和传输：用欧拉角，因为三个数比九个数的矩阵省空间
- 计算和优化：用旋转矩阵，因为矩阵乘法比三角函数运算稳定
- 可视化调试：用欧拉角，因为人能直观理解角度变化
个人经验：如果你在做标定优化，建议用旋转矩阵的9个元素作为优化变量，但加上正交性约束。我试过直接用欧拉角做优化，结果在接近万向锁的区域，优化器经常跑飞。后来改成用旋转矩阵加约束，问题就解决了。
好了，这一章的内容就到这里。旋转矩阵和欧拉角是外参标定的基本功，理解了它们，后面讲坐标系变换、标定算法时就会轻松很多。