第一章 姿态控制基础：坐标系、欧拉角与四元数
各位同学好，我是老张。在飞控这行摸爬滚打了十几年，今天咱们来聊聊姿态控制里最基础、也最容易踩坑的部分——坐标系和姿态表示。
说实话，我见过太多新手一上来就调PID，结果飞机翻跟头都不知道为什么。其实啊，很多问题根源都在坐标系没搞明白。咱们先把地基打牢，后面才能盖高楼。
1.1 坐标系定义：你得知道飞机在哪儿、朝哪儿
姿态控制说白了就三件事：我在哪儿？我朝哪儿？我怎么过去？ 坐标系就是回答前两个问题的工具。
1.1.1 地理坐标系（NED系）
也叫导航坐标系，我习惯叫它“大地系”。
- X轴：指向正北（North）
- Y轴：指向正东（East）
- Z轴：指向地心（Down），也就是垂直向下
嗯，这里要注意：Z轴是向下的。为什么？因为大部分传感器（比如加速度计）测的是重力方向，向下为正方便计算。我在项目里见过有人把Z轴搞反了，结果高度控制直接炸机——血的教训。
1.1.2 机体坐标系（Body系）
这个坐标系是跟着飞机走的，我称之为“飞机自己的视角”。
- X轴：指向机头方向（前进方向）
- Y轴：指向飞机右侧（右翼方向）
- Z轴：指向飞机下方（符合右手定则）
你想想看，这两个坐标系之间怎么转换？这就是姿态估计要干的事。
核心要点：地理系是“绝对参考”，机体系是“相对参考”。所有控制指令最终都要从地理系转到机体系才能执行。
1.2 欧拉角与旋转矩阵：最直观的姿态表示
欧拉角，说白了就是三个角度：横滚（Roll）、俯仰（Pitch）、偏航（Yaw）。我刚开始学飞控时，觉得这玩意儿太直观了，飞机歪了多少一目了然。
但这里有个大坑——万向锁。我记得有一次调试固定翼，俯仰拉到90度时，横滚和偏航突然耦合了，飞机直接螺旋下坠。嗯，这就是万向锁的典型表现。
旋转矩阵：数学上的“翻译官”
从地理系到机体系的旋转，可以用三个基本旋转矩阵相乘得到：
// 绕Z轴旋转（偏航）
R_yaw = [cos(ψ) -sin(ψ) 0;
sin(ψ) cos(ψ) 0;
0 0 1]
// 绕Y轴旋转（俯仰）
R_pitch = [cos(θ) 0 sin(θ);
0 1 0;
-sin(θ) 0 cos(θ)]
// 绕X轴旋转（横滚）
R_roll = [1 0 0;
0 cos(φ) -sin(φ);
0 sin(φ) cos(φ)]
// 完整旋转矩阵：R = R_yaw * R_pitch * R_roll我个人习惯用Z-Y-X顺序，也就是先偏航、再俯仰、最后横滚。不同顺序结果不一样，这个一定要统一。
避坑指南：我曾经在代码里用了不同的旋转顺序，结果姿态解算和控制器对不上，查了三天bug才发现。建议团队内部统一标准，并在代码注释里写清楚。
1.3 四元数基础：解决万向锁的利器
四元数听起来高大上，其实就是一个四维复数：q = w + xi + yj + zk，其中w是实部，x、y、z是虚部。
为什么要用四元数？说白了就两个原因：
- 没有万向锁——这是最大的优势
- 计算效率高——只有乘法和加法，没有三角函数
我记得在PX4的代码里，姿态估计全部用四元数，只在最后输出控制量时才转成欧拉角。为什么？因为四元数插值平滑，不会出现角度跳变。
四元数基本运算
// 四元数乘法（用于旋转合成）
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
// 四元数归一化（必须做！）
q_norm = q / sqrt(w² + x² + y² + z²)
// 四元数转旋转矩阵
R = [1-2(y²+z²) 2(xy-wz) 2(xz+wy);
2(xy+wz) 1-2(x²+z²) 2(yz-wx);
2(xz-wy) 2(yz+wx) 1-2(x²+y²)]小技巧：每次更新四元数后一定要归一化，否则数值误差会累积，导致姿态发散。我习惯在IMU数据更新循环里加一个归一化步骤，养成肌肉记忆。
1.4 姿态表示方法对比：选哪个？
咱们来做个对比，方便你根据场景选择：
| 方法 | 优点 | 缺点 | 适用场景 |
|---|---|---|---|
| 欧拉角 | 直观、物理意义明确 | 万向锁、三角函数计算慢 | 人机交互、数据显示 |
| 旋转矩阵 | 无奇点、可逆 | 9个参数冗余、计算量大 | 坐标变换、视觉SLAM |
| 四元数 | 无奇点、计算快、易插值 | 不够直观、需归一化 | 姿态估计、控制律计算 |
我个人建议：内部计算用四元数，外部显示用欧拉角。这是业界主流做法，也是经过验证的最佳实践。
经验之谈：我在做无人机编队时，发现四元数还有一个好处——可以方便地做姿态平滑过渡。比如从悬停到前飞，用四元数球面线性插值（Slerp），飞机姿态变化非常丝滑，不会出现欧拉角那种突然抖动。
知识体系总览
下面这张图是我自己画的，把本章的核心逻辑串起来了。你一看就明白各个概念之间的关系：
从这张图可以看出来，坐标系是基础，姿态表示方法是工具，应用场景是目标。三者环环相扣，缺一不可。
好了，第一章的内容就到这里。记住：坐标系搞错，后面全白干。我当年刚入行时也在这上面栽过跟头，希望你能少走弯路。