2. 坐标系与姿态表示：欧拉角、四元数、旋转矩阵、坐标系转换、不同表示方法的优劣对比

2.1 为什么飞控离不开坐标系？

做飞控这么多年，我见过太多新手一上来就写PID，结果飞机在天上乱转。为什么？坐标系没搞明白。

说白了，飞控算法就是回答三个问题：我在哪？我要去哪？我怎么去？ 这三个问题都绕不开坐标系。你想想看，传感器给的数据是相对于机体的，但导航指令是相对于地面的。这两者之间怎么换算？这就是坐标系转换要干的事。

我个人习惯把坐标系分成两类：导航坐标系（比如NED：北东地）和机体坐标系（前右下）。导航坐标系是固定的，机体坐标系是跟着飞机转的。飞控的核心任务之一，就是实时算出这两个坐标系之间的旋转关系。

2.2 三种主流姿态表示方法

2.2.1 欧拉角

欧拉角是最直观的。三个角度：横滚（Roll）、俯仰（Pitch）、偏航（Yaw）。我在项目中第一次调飞控，用的就是欧拉角。为什么？因为好理解啊！

但欧拉角有个致命问题——万向锁。我记得有一次做固定翼飞控，俯仰拉到90度，横滚和偏航突然就分不清了。当时我盯着数据看了半天，还以为传感器坏了。后来才意识到，这就是万向锁。

欧拉角的优点：直观、物理意义明确、占用内存小（3个浮点数）。
缺点：有万向锁、三角函数运算慢、不便于插值。

2.2.2 旋转矩阵

旋转矩阵是一个3x3的正交矩阵。它没有万向锁问题，而且可以直接用于向量旋转。你想想看，一个向量左乘旋转矩阵，就完成了坐标系转换，多方便。

但旋转矩阵有个麻烦——正交性约束。数值计算中，由于浮点误差，旋转矩阵会慢慢偏离正交。我早期做惯性导航时，就吃过这个亏。解算出来的姿态越来越歪，最后发现是旋转矩阵不满足正交性了。

旋转矩阵的优点：无万向锁、可直接用于向量旋转、便于组合旋转。
缺点：9个参数冗余、需要正交化修正、计算量大。

2.2.3 四元数

四元数是我现在最常用的。为什么？因为它完美解决了欧拉角和旋转矩阵的问题。

四元数用4个参数表示旋转：q = [w, x, y, z]。它没有万向锁，没有冗余，而且插值平滑。我在做无人机轨迹规划时，需要让姿态平滑过渡，四元数的球面线性插值（SLERP）简直是神器。

四元数的优点：无万向锁、无冗余、计算效率高、便于插值。
缺点：不够直观、需要归一化、理解门槛高。

2.3 坐标系转换实战

在实际飞控中，最常见的转换是：机体坐标系 → 导航坐标系。比如加速度计测的是机体加速度，但我们要的是导航坐标系下的加速度。

假设我们有一个四元数 q 表示当前姿态，那么转换公式如下：

// 机体坐标系下的向量 v_body
// 导航坐标系下的向量 v_nav
// 四元数 q = [w, x, y, z]

// 方法1：直接用四元数旋转
v_nav = q * v_body * q_conj

// 方法2：先转成旋转矩阵再旋转
R = quat2rotm(q)
v_nav = R * v_body

我个人习惯用方法1，因为计算量更小。但如果你需要多次转换同一个向量，方法2更高效，因为旋转矩阵可以复用。

2.4 优劣对比总结

特性	欧拉角	旋转矩阵	四元数
参数数量	3	9	4
万向锁	有	无	无
直观性	高	低	低
计算效率	中	低	高
插值平滑性	差	中	好
数值稳定性	中	需正交化	需归一化
适用场景	人机交互	向量旋转	内部运算

2.5 知识体系结构图

下面这张图，是我自己总结的坐标系与姿态表示的知识体系。你看一眼就能明白各个概念之间的关系。

2.6 我的选择建议

如果你问我，在实际项目中到底该用哪种？我的答案是：看场景。

• 做姿态解算（AHRS/INS）：用四元数。卡尔曼滤波的状态量用四元数，没有万向锁，数值稳定。
• 做控制律设计：用欧拉角。因为PID控制器需要明确的物理量，横滚角误差就是横滚角误差，直观。
• 做可视化/地面站：用欧拉角。操作员需要看到飞机姿态，三个角度一目了然。
• 做轨迹规划/插值：用四元数。SLERP插值平滑，不会出现欧拉角的突变问题。