三维空间刚体运动（上）：旋转矩阵、旋转向量、欧拉角、四元数
各位同学，欢迎来到《激光雷达惯导组合里程计实现》的第一章。
做SLAM这么多年，我最大的感触就是：搞懂旋转，你就搞懂了一半的里程计。为什么这么说？你想想看，机器人每走一步，都在做一件事——估计自己从哪来到哪去。这个「从哪到哪」，本质上就是刚体在三维空间里的旋转加平移。
今天我们先啃旋转这块硬骨头。别怕，我会用最接地气的方式，把这四种旋转表示法给你讲透。
核心观点：旋转矩阵、旋转向量、欧拉角、四元数，这四种工具描述的是同一件事——三维空间中的旋转。它们各有脾气，选对了事半功倍，选错了调试到哭。
1. 旋转矩阵：最直观，但也最啰嗦
旋转矩阵是个3x3的方阵。它的每一列，代表旋转后的坐标轴在原坐标系下的投影。
我记得刚入行时，导师让我手推一个旋转矩阵。我吭哧吭哧算了半天，结果符号搞反了。后来养成个习惯：先画图，再列式，最后写代码。
绕X轴旋转θ角的矩阵长这样：
R_x(θ) = [1, 0, 0;
0, cosθ, -sinθ;
0, sinθ, cosθ]绕Y轴：
R_y(θ) = [ cosθ, 0, sinθ;
0, 1, 0;
-sinθ, 0, cosθ]绕Z轴：
R_z(θ) = [cosθ, -sinθ, 0;
sinθ, cosθ, 0;
0, 0, 1]避坑指南：我曾经在项目里直接用欧拉角累加去更新姿态，结果跑了几分钟就飘到天上去了。旋转矩阵的乘法不满足交换律，先绕X再绕Y，和先绕Y再绕X，结果完全不同。千万别想当然。
旋转矩阵有3个重要性质：
- 正交性：R^T * R = I，逆矩阵就是转置矩阵
- 行列式为+1：这是刚体旋转，不是镜像
- 9个元素，6个约束：说白了，真正自由的只有3个参数
为什么说它啰嗦？因为你要存9个浮点数，但真正有用的信息只有3个自由度。这就好比用一卡车去运一袋米——浪费。
2. 旋转向量：最紧凑，但小心奇异
旋转向量，也叫轴角表示。一个向量，方向代表旋转轴，模长代表旋转角度。
公式很简单：
θ = ||r|| // 旋转角度
u = r / ||r|| // 单位旋转轴从旋转向量到旋转矩阵，用罗德里格斯公式：
R = cosθ * I + (1 - cosθ) * u*u^T + sinθ * u^其中u^是u的反对称矩阵。
个人经验：我在做视觉惯性里程计初始化时，经常用旋转向量来做优化变量。因为它只有3个参数，没有冗余约束，优化起来收敛快。但要注意，当θ接近0时，数值稳定性会变差。我一般会加个阈值判断，θ小于1e-6时直接近似处理。
3. 欧拉角：最符合直觉，但小心万向锁
欧拉角就是三个角度：横滚角（roll）、俯仰角（pitch）、偏航角（yaw）。
说白了，就是告诉飞机：你先转个航向，再抬个头，最后侧个身。
| 名称 | 旋转轴 | 范围 | 典型应用 |
|---|---|---|---|
| 偏航角 yaw | Z轴 | [-π, π] | 航向 |
| 俯仰角 pitch | Y轴 | [-π/2, π/2] | 抬头低头 |
| 横滚角 roll | X轴 | [-π, π] | 侧倾 |
万向锁问题：当pitch接近±90°时，yaw和roll的旋转轴会重合，丢失一个自由度。我曾经在无人机项目里用欧拉角做姿态控制，结果飞机做筋斗动作时直接失控。嗯，从那以后，只要涉及大角度机动，我坚决不用欧拉角。
4. 四元数：SLAM界的宠儿
四元数是个超复数：q = w + xi + yj + zk，其中w是实部，x,y,z是虚部。
为什么SLAM里都用它？三个字：无奇异、易插值、计算快。
单位四元数表示旋转：
q = [cos(θ/2), u_x * sin(θ/2), u_y * sin(θ/2), u_z * sin(θ/2)]四元数乘法：
q1 * q2 = [w1*w2 - v1·v2, w1*v2 + w2*v1 + v1×v2]关键点：四元数乘法不满足交换律。q1 * q2 和 q2 * q1 表示不同的旋转顺序。我习惯用Hamilton约定，即先左乘后右乘。这个约定一定要在团队里统一，否则联调时你会疯掉。
用四元数旋转一个三维点p：
p' = q * p * q_conj其中p_conj是q的共轭四元数。
我的习惯：在代码里，我永远用四元数做内部运算，只在最后输出时转成欧拉角给人看。这样既避免了万向锁，又方便调试。记住：四元数用于计算，欧拉角用于显示。
5. 四种表示法的对比
| 表示法 | 参数数量 | 优点 | 缺点 | 推荐场景 |
|---|---|---|---|---|
| 旋转矩阵 | 9 | 直观，易组合 | 冗余，效率低 | 理论推导 |
| 旋转向量 | 3 | 紧凑，无约束 | 有奇异点 | 优化求解 |
| 欧拉角 | 3 | 符合直觉 | 万向锁 | 人机交互 |
| 四元数 | 4 | 无奇异，易插值 | 不够直观 | SLAM核心 |
6. 知识体系总览
下面这张图，是我自己梳理的旋转表示法知识结构。你可以把它当作学习地图：
这张图把四种表示法的核心特点、优缺点、推荐场景都串起来了。你可以把它当作学习地图，每次遇到旋转相关的问题，先想想该用哪种工具。
最后说一句：别试图一次性记住所有公式。我在实际项目中，每次用到旋转矩阵时都会打开笔记本看一眼。真正重要的是理解每种表示法的适用场景和局限性。工具是死的，人是活的。
公众号:蓝海资料掘金营，微信deep3321