4、坐标系与姿态表示：地球坐标系、机体坐标系、欧拉角、旋转矩阵、四元数基础
做飞控这么多年，我越来越觉得坐标系和姿态表示是整个系统的「地基」。地基没打牢，后面什么控制算法、状态估计都是空中楼阁。今天咱们就把这块硬骨头啃下来。
4.1 地球坐标系：我们到底在哪儿？
先说说地球坐标系。说白了，就是给飞行器一个「绝对位置」的参考。
我个人习惯把地球坐标系想象成一个巨大的三维网格，原点在地球中心。X轴指向春分点，Z轴指向北极，Y轴按右手定则补齐。这个坐标系不随飞行器转动，所以叫「惯性系」——当然严格来说地球本身也在转，但飞控里我们通常把它当惯性系用。
嗯，这里要注意：实际工程中我们常用的是NED坐标系（北-东-地）。
- X轴：指向地理北
- Y轴：指向地理东
- Z轴：指向地心（向下）
为什么Z轴向下？因为这样跟重力方向一致，计算起来方便。我在做第一个四旋翼项目时，就因为搞反了Z轴方向，仿真里飞机直接「坠毁」了——后来才发现是坐标系符号搞反了。
4.2 机体坐标系：飞机自己的视角
机体坐标系就简单了——它跟着飞机走。原点在飞机重心，X轴指向机头，Y轴指向右翼，Z轴指向机腹（向下）。
你想想看，飞机上的传感器（IMU、磁力计）测量的数据，都是在这个坐标系下的。所以我们需要一个「桥梁」，把机体坐标系的数据转换到地球坐标系去。
这个桥梁，就是姿态表示。
4.3 欧拉角：最直观的姿态描述
欧拉角大家应该不陌生。三个角度：滚转角（φ）、俯仰角（θ）、偏航角（ψ）。
我记得刚入行时，觉得欧拉角太直观了，简直完美。直到有一次做特技飞行仿真，飞机翻了个跟头，欧拉角直接「炸了」——这就是著名的万向锁问题。
欧拉角的旋转顺序也很关键。飞控里最常用的是Z-Y-X顺序（先偏航，再俯仰，最后滚转）。顺序不同，结果完全不同。
| 旋转顺序 | 应用场景 | 注意事项 |
|---|---|---|
| Z-Y-X | 大多数飞控系统 | 避免俯仰角接近±90° |
| Z-X-Z | 某些机器人系统 | 万向锁问题更隐蔽 |
4.4 旋转矩阵：数学上的桥梁
旋转矩阵就是把欧拉角「翻译」成数学运算的工具。一个3×3的矩阵，乘以一个向量，就能完成坐标系转换。
从机体坐标系到地球坐标系的旋转矩阵长这样：
R = Rz(ψ) * Ry(θ) * Rx(φ)
其中：
Rz(ψ) = [cosψ -sinψ 0; sinψ cosψ 0; 0 0 1]
Ry(θ) = [cosθ 0 sinθ; 0 1 0; -sinθ 0 cosθ]
Rx(φ) = [1 0 0; 0 cosφ -sinφ; 0 sinφ cosφ]注意顺序！先乘Rx，再乘Ry，最后乘Rz。顺序错了，姿态就全错了。
angle2dcm(ψ, θ, φ, 'ZYX') 直接生成旋转矩阵，省得自己手算。但一定要搞清楚输入顺序和单位（弧度还是角度）。
4.5 四元数：工程上的最优解
四元数，说白了就是「没有万向锁的欧拉角」。它用四个数表示旋转：q = [w, x, y, z]，其中w是标量部分，(x,y,z)是向量部分。
为什么飞控里都用四元数？三个原因：
- 无万向锁：任意姿态都能平滑表示
- 计算效率高：比旋转矩阵少乘几次
- 插值平滑：做姿态平滑过渡时不会跳变
我记得第一次在STM32上实现四元数更新时，发现浮点运算太慢。后来改用一阶龙格-库塔法做离散化，才把更新频率跑到了1kHz以上。
四元数更新公式（离散化后）：
q_new = q_old + 0.5 * dt * Ω * q_old
其中 Ω = [0, -p, -q, -r;
p, 0, r, -q;
q, -r, 0, p;
r, q, -p, 0]
p, q, r 是陀螺仪测得的角速度q = q / norm(q)
4.6 三种表示方式的对比
| 表示方式 | 优点 | 缺点 | 工程建议 |
|---|---|---|---|
| 欧拉角 | 直观、易理解 | 万向锁、插值困难 | 仅用于人机交互显示 |
| 旋转矩阵 | 数学性质好、易推导 | 9个参数、计算量大 | 用于坐标变换计算 |
| 四元数 | 无奇点、效率高、易插值 | 不够直观 | 飞控核心算法首选 |
实际项目中，我通常这样搭配使用：
- 传感器数据融合 → 四元数
- 控制律计算 → 四元数转旋转矩阵
- 地面站显示 → 四元数转欧拉角
说白了，就是「内部用四元数，外部用欧拉角」。这样既保证了计算稳定性，又方便人看。
好了，坐标系和姿态表示这块，核心就是这些。记住一句话：地球坐标系是锚，机体坐标系是船，欧拉角是船上的旗，旋转矩阵是缆绳，四元数是舵。各有用处，缺一不可。