坐标系与姿态表示：欧拉角、四元数、旋转矩阵、坐标系转换实战
各位同学，欢迎来到第二章。今天咱们聊聊飞行器控制里最基础、也最容易踩坑的东西——坐标系与姿态表示。
说实话，我刚开始做飞控那会儿，觉得这玩意儿不就是几个角度嘛，有啥好学的？结果第一次试飞，飞机在天上翻了个跟头，差点砸到人。后来一查，原来是欧拉角万向锁了。嗯，从那以后，我再也不敢小看姿态表示了。
2.1 为什么需要坐标系？
你想想看，飞行器在天上飞，它得知道自己“头朝哪”、“肚子朝哪”。这些信息不能凭空说，得有个参考系。
我习惯把坐标系分成两类：
- 惯性坐标系（地球坐标系）：固定在地面上，北东地或者东北天。说白了，就是“大地告诉你哪是北”。
- 机体坐标系：固定在飞机上，机头朝前、右翼朝右、肚子朝下。飞机怎么动，它就怎么动。
这两个坐标系之间的转换，就是姿态表示要干的事。
核心思想：所有控制算法都在机体坐标系里算，但最终要映射到地球坐标系去执行。搞反了，飞机就乱飞。
2.2 欧拉角：最直观，但最危险
欧拉角就是三个角度：滚转角（Roll）、俯仰角（Pitch）、偏航角（Yaw）。
我在项目中遇到过一个问题：用欧拉角做姿态控制，飞机在大角度俯仰时突然失控。为什么？因为欧拉角有万向锁问题。
万向锁：当俯仰角接近±90°时，滚转和偏航的旋转轴会重合，丢失一个自由度。这时候你给滚转指令，飞机可能偏航；给偏航指令，飞机可能滚转。说白了，就是“你指挥不动它了”。
欧拉角的优点很明显：直观，人脑好理解。但缺点也很致命：
- 万向锁问题
- 插值不连续（两个姿态之间不能直接线性插值）
- 计算有奇点
所以我个人建议：人机交互用欧拉角，内部计算别用。
2.3 旋转矩阵：数学上完美，计算上沉重
旋转矩阵是一个3×3的正交矩阵，它能把一个向量从一个坐标系转到另一个坐标系。
比如，从机体坐标系到地球坐标系的旋转矩阵 R，满足：
v_earth = R * v_body旋转矩阵的好处是：
- 没有奇点
- 可以连续旋转
- 数学性质好（正交、可逆）
但坏处也很明显：
- 9个参数，冗余大
- 每次更新都要做矩阵乘法，计算量大
- 长时间运行会累积误差，需要正交化修正
我记得有一次在嵌入式平台上跑旋转矩阵更新，CPU占用率直接飙到80%。后来换成四元数，降到20%。所以，嵌入式系统里，旋转矩阵不是首选。
2.4 四元数：工程界的宠儿
四元数是个超复数，形式是 q = w + xi + yj + zk，其中 w 是实部，x、y、z 是虚部。
它表示绕一个单位轴 (x, y, z) 旋转 θ 角度：
q = [cos(θ/2), x*sin(θ/2), y*sin(θ/2), z*sin(θ/2)]四元数的优点：
- 4个参数，比旋转矩阵少一半多
- 没有奇点
- 插值平滑（球面线性插值）
- 计算效率高
我的习惯：所有IMU数据融合（比如Mahony、Madgwick算法）都用四元数。只在最后输出给上层时，才转成欧拉角给人看。
四元数也有坑：
- 必须归一化（模长为1），否则姿态会漂移
- 人脑不直观，调试时得转成欧拉角看
- 两个四元数相乘的顺序不能搞反（q1 * q2 ≠ q2 * q1）
2.5 三种表示方式的转换
实际工程中，这三种表示方式经常需要互相转换。我整理了一个对照表：
| 转换方向 | 公式/方法 | 注意事项 |
|---|---|---|
| 欧拉角 → 旋转矩阵 | 三个基本旋转矩阵相乘 | 旋转顺序很重要（常见ZYX） |
| 旋转矩阵 → 欧拉角 | 从矩阵元素反解角度 | 注意万向锁时的处理 |
| 欧拉角 → 四元数 | 半角公式组合 | 注意角度单位（弧度/度） |
| 四元数 → 旋转矩阵 | 直接代入公式 | 确保四元数已归一化 |
| 旋转矩阵 → 四元数 | 从矩阵迹反解 | 注意数值稳定性 |
| 四元数 → 欧拉角 | 先转矩阵，再转欧拉角 | 或者直接用公式 |
下面给一段C++代码，展示四元数转欧拉角的实现：
// 四元数转欧拉角（ZYX顺序）
void QuaternionToEuler(double qw, double qx, double qy, double qz,
double& roll, double& pitch, double& yaw) {
// 计算俯仰角（注意万向锁）
double sinr_cosp = 2.0 * (qw * qx + qy * qz);
double cosr_cosp = 1.0 - 2.0 * (qx * qx + qy * qy);
roll = atan2(sinr_cosp, cosr_cosp);
// 俯仰角
double sinp = 2.0 * (qw * qy - qz * qx);
if (fabs(sinp) >= 1.0) {
pitch = copysign(M_PI / 2.0, sinp); // 万向锁处理
} else {
pitch = asin(sinp);
}
// 偏航角
double siny_cosp = 2.0 * (qw * qz + qx * qy);
double cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz);
yaw = atan2(siny_cosp, cosy_cosp);
}我曾经踩过的坑：在四元数转欧拉角时，忘记处理俯仰角接近±90°的情况。结果飞机在做筋斗动作时，偏航角直接跳变180°，差点失控。所以，万向锁处理不是可选项，是必选项。
2.6 坐标系转换实战
实战中，最常见的需求是：把机体坐标系下的加速度、角速度转换到地球坐标系下。
比如，你要做位置控制，需要知道飞机在地球坐标系下的加速度。但IMU测到的是机体坐标系下的加速度。怎么办？
步骤很简单：
- 从IMU获取四元数姿态
- 用四元数构建旋转矩阵
- 用旋转矩阵把加速度从机体转到地球
代码示例（Python）：
import numpy as np
def quaternion_to_rotation_matrix(q):
"""四元数转旋转矩阵"""
w, x, y, z = q
R = np.array([
[1 - 2*y*y - 2*z*z, 2*x*y - 2*w*z, 2*x*z + 2*w*y],
[2*x*y + 2*w*z, 1 - 2*x*x - 2*z*z, 2*y*z - 2*w*x],
[2*x*z - 2*w*y, 2*y*z + 2*w*x, 1 - 2*x*x - 2*y*y]
])
return R
def transform_acceleration(acc_body, q):
"""把机体加速度转到地球坐标系"""
R = quaternion_to_rotation_matrix(q)
acc_earth = R @ acc_body
return acc_earth
# 示例
q = [0.707, 0.0, 0.707, 0.0] # 绕Y轴旋转90度
acc_body = np.array([0.0, 0.0, -9.8]) # 机体Z轴向下
acc_earth = transform_acceleration(acc_body, q)
print(f"地球坐标系加速度: {acc_earth}")小技巧：实际工程中，我习惯把四元数、旋转矩阵、欧拉角的转换封装成一个类。这样代码复用率高，也不容易出错。
2.7 本章知识体系
下面这张图，是我自己总结的坐标系与姿态表示的知识结构。你看一眼，心里就有数了。
这张图把本章的核心逻辑串起来了：从坐标系出发，到三种姿态表示，再到它们之间的转换，最后落到实战应用。你照着这个思路学，不会乱。
2.8 避坑指南
最后，我把自己这些年踩过的坑总结一下，你记好了：
- 四元数一定要归一化：每次更新后检查模长，偏离1.0就重新归一化。否则姿态会慢慢漂移。
- 欧拉角别用在内部计算：只做人机交互。内部计算一律用四元数。
- 旋转顺序要统一：团队里统一用ZYX顺序，别今天ZYX明天XYZ，代码会乱成一锅粥。
- 角度单位要小心：IMU输出一般是弧度，但人习惯看度。转换时别搞混。
- 万向锁处理不能省：哪怕你的飞机不做大角度机动，也要处理。万一测试时有人手贱呢？
一句话总结：欧拉角给人看，四元数给机器算，旋转矩阵做桥梁。记住这个原则，你的姿态系统就不会出大问题。
好了，这一章就到这里。坐标系和姿态表示是飞行器控制的基石，你把它搞透了，后面的控制算法学起来就轻松多了。
公众号:蓝海资料掘金营，微信deep3321