坐标变换与空间描述：旋转矩阵、四元数、欧拉角、坐标系对齐
各位同学，欢迎来到传感器融合的核心环节——坐标变换。说实话，我见过太多项目在融合阶段翻车，最后查出来都是坐标系没对齐。你想想看，IMU说往东，GPS说往北，两个数据一加，结果飞控直接抽风。嗯，今天我们就来彻底搞定这件事。
为什么我们需要多种旋转表示？
旋转在三维空间里有多种描述方式。每种方式都有它的脾气。我个人习惯在工程中混着用，因为不同场景下，它们的优劣太明显了。
- 欧拉角：直观，但容易遇到万向锁。我在做四轴飞行器姿态解算时，就因为这个锁，导致飞机在俯仰90度时突然翻滚失控。
- 旋转矩阵：数学上最干净，但9个参数冗余，而且每次更新都要保证正交性，计算量不小。
- 四元数：没有奇点，插值平滑，是传感器融合的标配。我建议你优先掌握它。
核心结论：欧拉角用来给人看，四元数用来给机器算，旋转矩阵用来做中间桥梁。
欧拉角：直观但危险
欧拉角用三个角度描述旋转：偏航（Yaw）、俯仰（Pitch）、滚转（Roll）。顺序很重要。我见过有人用ZYX顺序，有人用XYZ顺序，结果两个模块对不上，数据全乱套。
为什么会这样？因为旋转不满足交换律。先转X再转Y，和先转Y再转X，结果完全不同。
避坑指南：我曾经在一个无人机项目中，因为欧拉角顺序没统一，导致视觉SLAM和IMU的融合结果差了30度。后来花了整整两天才定位到问题。记住：团队内部必须统一旋转顺序，并在代码注释里写清楚。
欧拉角还有一个致命问题——万向锁。当俯仰角接近±90度时，偏航和滚转的旋转轴会重合，丢失一个自由度。说白了，就是你的姿态解算突然少了一个维度，飞控会瞬间失稳。
旋转矩阵：数学的基石
旋转矩阵是一个3x3的正交矩阵，行列式为+1。它可以把一个向量从一个坐标系变换到另一个坐标系。比如，把机体坐标系下的加速度，转换到世界坐标系下。
// 绕Z轴旋转θ角
Rz(θ) = [cosθ -sinθ 0]
[sinθ cosθ 0]
[0 0 1]
// 绕Y轴旋转φ角
Ry(φ) = [cosφ 0 sinφ]
[0 1 0 ]
[-sinφ 0 cosφ]
// 绕X轴旋转ψ角
Rx(ψ) = [1 0 0 ]
[0 cosψ -sinψ]
[0 sinψ cosψ]组合旋转就是矩阵乘法。注意顺序：先转的矩阵放在右边。我刚开始做时经常搞反，结果姿态越转越乱。
我的经验：在嵌入式平台上，直接做3x3矩阵乘法其实挺费时的。如果只是做一次变换还好，但传感器融合里每秒要算几百次，这时候四元数就香多了。
四元数：工程中的王者
四元数是一个超复数，形式为 q = w + xi + yj + zk，其中 w 是实部，x、y、z 是虚部。它用四个参数表示旋转，没有奇点，而且插值平滑。
我记得第一次接触四元数时，觉得这东西很抽象。但用多了你会发现，它其实就是把旋转轴和旋转角打包在一起。旋转轴是 (x, y, z) 方向，旋转角是 2*arccos(w)。
// 四元数乘法（用于组合旋转）
q1 * q2 = (w1*w2 - x1*x2 - y1*y2 - z1*z2) +
(w1*x2 + x1*w2 + y1*z2 - z1*y2)i +
(w1*y2 - x1*z2 + y1*w2 + z1*x2)j +
(w1*z2 + x1*y2 - y1*x2 + z1*w2)k
// 用四元数旋转向量 v
v' = q * v * q_conj你看，代码实现起来很规整，没有分支判断，没有三角函数调用。在STM32这类MCU上，四元数更新一次只需要几十个周期，比矩阵快得多。
关键点：四元数必须归一化。我见过有人忘了归一化，结果姿态越飘越远。每次更新后，记得做 q = q / norm(q)。
坐标系对齐：融合的灵魂
传感器融合的本质，就是把不同坐标系下的数据统一到一个坐标系里。常见的坐标系有：
- 世界坐标系：固定在地面，通常北东地（NED）或东北天（ENU）。
- 机体坐标系：固定在设备上，x轴朝前，y轴朝右，z轴朝下。
- 传感器坐标系：每个传感器有自己的安装朝向，比如IMU可能歪着焊在板子上。
我曾经在一个机器人项目中，IMU的安装角度偏了5度。一开始没在意，结果融合后的航向角误差越来越大。后来用标定板做了精确的外参标定，才把问题解决。
坐标系对齐的步骤很简单：
- 确定每个传感器的安装姿态（相对于机体坐标系）。
- 用旋转矩阵或四元数，把传感器数据转换到机体坐标系。
- 再用机体到世界的变换，把数据统一到世界坐标系。
注意：坐标系定义必须一致。比如，IMU的z轴朝上还是朝下？GPS的北向是磁北还是真北？这些细节不统一，融合结果就是垃圾。
三种表示的相互转换
在实际工程中，我们经常需要在三种表示之间切换。下面是我常用的转换公式：
| 转换方向 | 方法 |
|---|---|
| 欧拉角 → 四元数 | 分别计算三个轴的旋转四元数，然后相乘 |
| 四元数 → 旋转矩阵 | 用四元数元素构造3x3矩阵 |
| 旋转矩阵 → 欧拉角 | 从矩阵元素中提取角度，注意处理万向锁 |
| 四元数 → 欧拉角 | 先转矩阵，再提取角度，或者直接用公式 |
// 四元数转欧拉角（ZYX顺序）
roll = atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y))
pitch = asin(2*(w*y - z*x))
yaw = atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z))注意，这里用了 atan2 而不是 atan，因为 atan2 能处理四个象限的角度。我见过有人用 atan，结果角度在±180度边界上跳变。
知识体系总览
下面这张图是我自己总结的，把三种旋转表示的关系和转换路径画了出来。你保存下来，以后做融合时对照着看。
这张图里，实线箭头是常用转换方向，虚线是反向转换。你想想看，实际项目中，我们通常用四元数做融合计算，最后转成欧拉角输出给用户看。旋转矩阵则作为中间桥梁，用于坐标系对齐。
我的建议：刚开始学的时候，先把四元数和旋转矩阵的转换写熟。欧拉角可以晚点再深入，因为它在融合算法里用得少。但坐标系对齐一定要搞明白，这是所有融合的基础。
好了，坐标变换这部分就讲到这里。记住，工具是死的，思路是活的。多动手写代码，多在实际数据里验证，你很快就能找到感觉。