第二章 坐标系与姿态表示：地球坐标系、机体坐标系、欧拉角、四元数、旋转矩阵
各位同学，欢迎来到第二章。这一章，我们得把「坐标系」和「姿态」这两个概念彻底聊透。说实话，我见过太多新手飞控工程师，代码写得飞起，结果飞机一上天就翻跟头，最后查来查去，发现是坐标系搞反了。嗯，这种坑，我自己也踩过。
你想想看，固定翼无人机在天上飞，它怎么知道自己「头朝哪」？怎么知道自己是平飞还是倒扣？这一切，都依赖于我们如何定义空间中的方向和旋转。说白了，就是给飞机装上一套「空间语言」。
2.1 地球坐标系：我们脚下的「绝对参考」
地球坐标系，也叫导航坐标系或惯性坐标系（近似）。它是个固定不动的坐标系，原点通常选在地球表面某点（比如起飞点）。
我个人习惯这样定义：
- X轴：指向正北（N）
- Y轴：指向正东（E）
- Z轴：指向地心（D），也就是向下
这就是经典的 NED（北-东-地） 坐标系。为什么Z轴要向下？因为这样符合右手定则，而且跟重力方向一致，算重力补偿时特别方便。
我在项目中遇到过一个问题：有次做长航时飞行，发现高度数据一直在漂。查了半天，原来是地球坐标系的原点没选好，导致重力模型计算有偏差。所以，原点选择一定要慎重，尤其是做惯性导航时。
2.2 机体坐标系：飞机自己的「身体语言」
机体坐标系是固定在飞机上的。想象你坐在驾驶舱里，你的前后左右就是机体坐标系。
标准定义如下：
- X轴（Roll轴）：指向机头前方
- Y轴（Pitch轴）：指向飞机右翼
- Z轴（Yaw轴）：指向飞机下方（符合右手定则）
你看，这个坐标系是跟着飞机一起转的。飞机滚转90度，它的Y轴就指向地面了。所以，传感器（比如IMU）测量到的加速度、角速度，都是基于机体坐标系的。
body_frame 或 bf 作为前缀来命名机体坐标系下的变量。比如 bf_accel、bf_gyro。这样一眼就能看出来数据是在哪个坐标系下。
2.3 欧拉角：最直观的姿态描述
欧拉角，说白了就是用三个角度来描述飞机的姿态：横滚角（Roll, φ）、俯仰角（Pitch, θ）、偏航角（Yaw, ψ）。
旋转顺序是：先偏航（绕Z轴），再俯仰（绕Y轴），最后横滚（绕X轴）。这个顺序很重要，搞反了姿态就全乱了。
| 角度 | 符号 | 范围 | 物理意义 |
|---|---|---|---|
| 横滚角 | φ | -180° ~ 180° | 飞机绕机体X轴的旋转 |
| 俯仰角 | θ | -90° ~ 90° | 飞机绕机体Y轴的旋转 |
| 偏航角 | ψ | -180° ~ 180° | 飞机绕机体Z轴的旋转 |
2.4 四元数：优雅的数学工具
四元数，听起来很玄乎，其实它就是四个数：q = [w, x, y, z]。其中 w 是实部，x, y, z 是虚部。
为什么用四元数？因为它完美解决了欧拉角的两个痛点：
- 无万向锁：任何姿态都能平滑表示
- 计算高效：插值和乘法都比旋转矩阵快
四元数表示旋转的公式很简单：
// 绕单位向量 (nx, ny, nz) 旋转 θ 角度
q.w = cos(θ/2)
q.x = nx * sin(θ/2)
q.y = ny * sin(θ/2)
q.z = nz * sin(θ/2)嗯，这里要注意：四元数一定要归一化！也就是 w² + x² + y² + z² = 1。否则你算出来的旋转就是错的。我在代码里每次更新完四元数，都会强制做一次归一化。
q = q1 * q2 表示先做 q2 旋转，再做 q1 旋转。注意顺序，跟矩阵乘法一样，不满足交换律。
2.5 旋转矩阵：连接两个世界的桥梁
旋转矩阵，就是把机体坐标系下的向量，转换到地球坐标系下（或者反过来）。它是一个3x3的正交矩阵，行列式为+1。
从机体坐标系到地球坐标系的旋转矩阵 R 可以这样表示（基于Z-Y-X欧拉角顺序）：
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φ]你想想看，有了这个矩阵，我们就能把IMU测到的加速度从机体坐标系转到地球坐标系，然后减去重力，就能得到飞机的真实加速度了。这就是惯性导航的基础。
2.6 四者之间的转换
在实际工程中，这四个表示方法经常需要互相转换。我整理了一个速查表：
| 转换方向 | 方法 | 注意事项 |
|---|---|---|
| 欧拉角 → 四元数 | 按Z-Y-X顺序连乘 | 注意角度单位（弧度/度） |
| 四元数 → 欧拉角 | 用atan2和asin反算 | 注意万向锁时的处理 |
| 四元数 → 旋转矩阵 | 直接代入公式 | 确保四元数已归一化 |
| 旋转矩阵 → 四元数 | 从矩阵元素反解 | 注意数值稳定性 |
我曾经在调试一个飞控时，发现姿态解算偶尔会跳变。查了两天，最后发现是四元数转欧拉角时，atan2的参数顺序写反了。这种低级错误，真的让人抓狂。所以，我建议你每次写完转换函数，都要用已知的数值去验证一下。
2.7 本章小结
这一章的内容，是飞控算法的「地基」。地球坐标系给了我们绝对参考，机体坐标系描述了飞机自身，欧拉角直观但有限制，四元数优雅且无奇点，旋转矩阵则是连接一切的桥梁。
我个人建议，你在写代码时：
- 姿态更新：用四元数
- 姿态显示：用欧拉角（方便人看）
- 向量变换：用旋转矩阵（方便计算）
下一章，我们会把这些姿态表示法用到增稳控制里。到时候你会发现，今天打下的基础，会帮你省下无数调试时间。