姿态解算基础：欧拉角与四元数、旋转矩阵、坐标系定义

各位同学，欢迎来到飞控参数深度解析的第二讲。

今天我们要聊的，是整个飞控系统里最基础、也最容易让人头晕的部分——姿态解算。说白了，就是回答一个问题：飞机现在到底朝哪个方向？

我刚开始做飞控时，觉得这玩意儿不就是算个角度嘛，有什么难的？结果第一次试飞，飞机在天上疯狂自旋，差点炸机。后来才发现，是坐标系搞反了，欧拉角万向锁没处理好。嗯，从那以后，我再也不敢小看这一章了。

1. 坐标系定义：先把“东南西北”说清楚

做飞控，第一件事就是定规矩——坐标系。没有统一的坐标系，你算出来的角度就是鸡同鸭讲。

1.1 NED坐标系（北东地）

NED，全称North-East-Down。这是飞控里最常用的全局参考系。

• X轴：指向正北（North）
• Y轴：指向正东（East）
• Z轴：指向地心（Down）

注意这个Z轴是向下的。为什么？因为重力加速度是向下的，这样定义后，加速度计读出的重力分量就是正的。我见过不少新手把Z轴朝上定义，结果加速度数据符号搞反，姿态解算直接崩掉。

1.2 机体坐标系（Body Frame）

机体坐标系是固定在飞机上的，跟着飞机一起转。

• X轴：指向机头方向（前进方向）
• Y轴：指向飞机右侧（右翼方向）
• Z轴：指向飞机下方（符合右手定则）

这两个坐标系之间的转换关系，就是姿态解算要干的事。

坐标系	X轴	Y轴	Z轴	典型应用
NED	北	东	地	GPS导航、航点规划
机体	机头	右翼	机腹	IMU数据、姿态控制

2. 欧拉角：最直观，但最危险

欧拉角是描述姿态最直观的方式。三个角度：横滚（Roll）、俯仰（Pitch）、偏航（Yaw）。

• Roll（φ）：绕X轴旋转，范围±180°
• Pitch（θ）：绕Y轴旋转，范围±90°
• Yaw（ψ）：绕Z轴旋转，范围±180°

为什么说它危险？因为万向锁（Gimbal Lock）。当Pitch接近±90°时，Roll和Yaw会失去一个自由度，导致姿态解算失效。

所以，欧拉角适合人机交互（比如地面站显示），但不适合内部计算。内部计算，我们得用四元数。

3. 四元数：数学上的“瑞士军刀”

四元数听起来高大上，其实就是一个四维复数：q = w + xi + yj + zk，其中w是实部，x、y、z是虚部。

它的好处是：无万向锁、计算效率高、适合插值。

四元数表示旋转的公式很简单：

// 绕单位向量 (x, y, z) 旋转 θ 角度
q.w = cos(θ/2)
q.x = x * sin(θ/2)
q.y = y * sin(θ/2)
q.z = z * sin(θ/2)

你想想看，一个四元数就能表示任意轴任意角度的旋转，比欧拉角三个角度要优雅得多。

3.1 四元数与欧拉角的转换

实际项目中，我们经常需要在两者之间来回切换。下面是我常用的转换代码：

// 四元数 → 欧拉角
float roll  = atan2(2*(q.w*q.x + q.y*q.z), 1 - 2*(q.x*q.x + q.y*q.y));
float pitch = asin(2*(q.w*q.y - q.z*q.x));
float yaw   = atan2(2*(q.w*q.z + q.x*q.y), 1 - 2*(q.y*q.y + q.z*q.z));

// 欧拉角 → 四元数
float cy = cos(yaw * 0.5);
float sy = sin(yaw * 0.5);
float cp = cos(pitch * 0.5);
float sp = sin(pitch * 0.5);
float cr = cos(roll * 0.5);
float sr = sin(roll * 0.5);

q.w = cr * cp * cy + sr * sp * sy;
q.x = sr * cp * cy - cr * sp * sy;
q.y = cr * sp * cy + sr * cp * sy;
q.z = cr * cp * sy - sr * sp * cy;

4. 旋转矩阵：连接两个世界的桥梁

旋转矩阵是一个3x3的矩阵，能把机体坐标系下的向量转换到NED坐标系下（反之亦然）。

从四元数到旋转矩阵的公式：

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²) |

有了这个矩阵，你就可以把机体上的加速度、磁场等数据转换到NED坐标系下，进行导航计算。

举个例子：

// 机体加速度 → NED加速度
Vector3f body_accel = {ax, ay, az};  // IMU读取
Vector3f ned_accel = R * body_accel; // 旋转矩阵乘法

我个人习惯把旋转矩阵的计算封装成一个函数，输入四元数，输出矩阵。这样代码干净，也方便调试。

5. 知识体系总览

下面这张图，是我自己总结的本章知识结构。你可以把它当作一个“地图”，随时回来对照。

6. 总结与避坑指南

好了，这一章的内容就到这里。我帮你把重点再拎一下：

• 坐标系：NED和机体，搞反了会炸机
• 欧拉角：直观但小心万向锁，别在Pitch接近±90°时用它做计算
• 四元数：内部计算的首选，没有奇点
• 旋转矩阵：坐标系转换的桥梁，代码里封装好复用