2、坐标系与姿态表示：欧拉角、旋转矩阵、四元数
做无人机控制，第一件事不是调PID，而是搞清楚——飞机到底朝哪飞。
你可能觉得这问题很简单。但我在项目里见过太多人，上来就写代码，结果飞机在天上乱转，最后发现是坐标系搞反了。嗯，我也翻过这个车。
这一节，咱们把姿态表示这件事彻底讲透。你搞懂了它，后面的控制算法才有根基。
2.1 两个坐标系：机体系与导航系
先说坐标系。无人机控制里，我们主要跟两个坐标系打交道：
- 机体坐标系（Body Frame）：固定在飞机上。原点在重心，X轴朝机头，Y轴朝右翼，Z轴朝下（右手系）。
- 导航坐标系（Navigation Frame）：固定在地面。常用的是北东地（NED）坐标系：X轴指北，Y轴指东，Z轴指向地心。
为什么要区分？因为传感器数据是在机体系下测量的，而控制指令需要在导航系下规划。说白了，你得知道“飞机自己觉得它在转”和“地面看它在怎么转”之间的映射关系。
核心原则：IMU（加速度计、陀螺仪）输出在机体系，控制目标（比如悬停、航线）在导航系。姿态表示就是这两个坐标系之间的桥梁。
2.2 欧拉角：最直观，但小心“万向锁”
欧拉角是大多数人最先接触的姿态表示法。它用三个角度描述旋转：横滚（Roll，φ）、俯仰（Pitch，θ）、偏航（Yaw，ψ）。
旋转顺序很关键。我习惯用Z-Y-X顺序（先偏航，再俯仰，最后横滚），这也是航空领域的标准。
数学上，从导航系到机体系的旋转矩阵可以写成：
R = R_x(φ) * R_y(θ) * R_z(ψ)其中：
R_x(φ) = [[1, 0, 0],
[0, cosφ, -sinφ],
[0, sinφ, cosφ]]
R_y(θ) = [[cosθ, 0, sinθ],
[0, 1, 0],
[-sinθ, 0, cosθ]]
R_z(ψ) = [[cosψ, -sinψ, 0],
[sinψ, cosψ, 0],
[0, 0, 1]]我曾经在调试一款四旋翼时，发现飞机在俯仰90°时突然失控。查了半天，问题出在欧拉角上——万向锁。
当俯仰角接近±90°时，横滚和偏航的旋转轴会重合，丢失一个自由度。此时姿态解算会出奇异值，控制指令直接乱掉。
所以我的建议是：欧拉角只适合做可视化显示和人机交互。在控制算法内部，尽量别用它做连续运算。
2.3 旋转矩阵：通用但计算量大
旋转矩阵是一个3×3的正交矩阵，行列式为+1。它能把一个向量从一个坐标系变换到另一个坐标系。
比如，机体系下的向量 v_b 转到导航系：
v_n = R * v_b旋转矩阵的好处是：没有奇异值，没有万向锁。你可以任意组合旋转，结果永远正确。
但代价也很明显：9个元素，每次更新都要做大量三角函数运算。在早期的飞控芯片上，这可能是性能瓶颈。
我记得有一次在STM32F4上跑姿态解算，用旋转矩阵做预测更新，CPU占用率直接飙到70%。后来换成四元数，降到了20%。
小技巧：如果你用旋转矩阵做长时间积分，记得定期正交化。否则数值误差会让矩阵慢慢变成“非旋转”的，导致姿态漂移。
2.4 四元数：工程首选
四元数，说白了就是一个“带约束的复数升级版”。它用四个数表示旋转：
q = [q0, q1, q2, q3]其中 q0 是标量部分，[q1, q2, q3] 是矢量部分。约束条件：
q0² + q1² + q2² + q3² = 1为什么四元数在工程中这么受欢迎？三个原因：
- 无奇异值：不像欧拉角，四元数在所有姿态下都有效。
- 计算量小：只有4个元素，乘法比旋转矩阵快得多。
- 插值平滑：做姿态平滑过渡时，四元数的球面线性插值（SLERP）非常自然。
四元数乘法表示旋转复合：
q_result = q2 * q1注意顺序：先应用 q1，再应用 q2。这和旋转矩阵的乘法顺序一致。
从四元数到旋转矩阵的转换公式：
R = [[1-2(q2²+q3²), 2(q1q2-q0q3), 2(q1q3+q0q2)],
[2(q1q2+q0q3), 1-2(q1²+q3²), 2(q2q3-q0q1)],
[2(q1q3-q0q2), 2(q2q3+q0q1), 1-2(q1²+q2²)]]我个人习惯：在飞控代码里，所有姿态运算都用四元数。只在最后输出给地面站时，才转成欧拉角显示。
2.5 三种表示法的对比
| 特性 | 欧拉角 | 旋转矩阵 | 四元数 |
|---|---|---|---|
| 元素个数 | 3 | 9 | 4 |
| 有无奇异值 | 有（万向锁） | 无 | 无 |
| 计算量 | 低 | 高 | 中 |
| 直观性 | 高 | 低 | 中 |
| 插值平滑性 | 差 | 差 | 好 |
| 工程推荐度 | 仅显示 | 不推荐 | 强烈推荐 |
2.6 知识体系总览
下面这张图，帮你理清这一节的核心逻辑：
2.7 实际项目中的选择建议
你可能会问：那我到底该用哪个？
我的经验是：
- 如果你在写飞控核心算法（姿态解算、控制律）——用四元数。这是行业标准，PX4、ArduPilot 都在用。
- 如果你在做地面站或日志分析——用欧拉角显示。人眼对角度更敏感。
- 如果你在做仿真或视觉SLAM——旋转矩阵有时更方便，因为和相机投影矩阵直接关联。
一个小技巧：在代码里，我习惯把四元数作为“内部货币”，只在输入输出时兑换成欧拉角。这样既避免了万向锁，又保持了代码的可读性。
好了，坐标系和姿态表示就聊到这。你搞懂了这些，后面的控制算法才能落地。下一节我们聊动力学模型——飞机到底是怎么飞起来的。
公众号:蓝海资料掘金营，微信deep3321