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

各位同学，欢迎来到姿态解算的第一课。说实话，姿态解算是整个飞控系统的“灵魂”。你想想看，如果连飞机现在头朝哪、身子歪没歪都不知道，那还谈什么控制？

这一章我们不讲复杂的滤波算法，先把基础打牢。我会把坐标系、欧拉角、四元数、旋转矩阵这几个概念串起来讲。嗯，这里要注意，很多新手就是栽在这些基础概念上的。

4.1 坐标系定义：你得先知道“参照物”在哪

做飞控，首先得定义清楚两个坐标系。我习惯把它们比作“绝对参考系”和“自身参考系”。

4.1.1 地理坐标系（NED系）

也叫导航坐标系。说白了，就是地面上的一个固定坐标系。我们通常用NED（北-东-地）定义：

• X轴：指向正北
• Y轴：指向正东
• Z轴：指向地心（垂直向下）

为什么Z轴向下？因为加速度计测的是重力加速度，方向是向下的。这样定义，计算起来更直观。我在做第一个飞控原型时，把Z轴定义反了，结果姿态解算出来的角度全是反的，飞机一解锁就往地里钻……

4.1.2 机体坐标系（Body系）

这个坐标系是固定在飞机上的，跟着飞机一起动。定义如下：

• X轴：指向机头方向
• Y轴：指向飞机右侧（右翼方向）
• Z轴：指向飞机下方（垂直向下）

你想想看，这两个坐标系之间的相对关系，就是飞机的姿态。比如机头朝北、机身水平，那机体坐标系就和地理坐标系重合。一旦飞机倾斜了，两个坐标系之间就有了夹角。

4.2 姿态表示方法：三种武器，各有千秋

描述这个旋转关系，主要有三种方法。我一个个讲，你对比着看。

4.2.1 欧拉角：最直观，但有个大坑

欧拉角用三个角度来描述姿态：

• 横滚角（Roll, φ）：绕X轴旋转的角度。说白了就是飞机左右倾斜的程度。
• 俯仰角（Pitch, θ）：绕Y轴旋转的角度。就是飞机抬头低头的角度。
• 偏航角（Yaw, ψ）：绕Z轴旋转的角度。就是机头朝向。

欧拉角的好处是直观，人脑容易理解。你看到横滚角30度，就知道飞机向右倾斜了30度。但欧拉角有一个致命问题——万向锁（Gimbal Lock）。

4.2.2 旋转矩阵：数学上完美，但计算量太大

旋转矩阵是一个3x3的矩阵，它描述了从一个坐标系到另一个坐标系的线性变换。从欧拉角可以推导出旋转矩阵：

// 绕Z轴旋转（偏航）
Rz(ψ) = [cosψ  -sinψ  0]
        [sinψ   cosψ  0]
        [0      0     1]

// 绕Y轴旋转（俯仰）
Ry(θ) = [cosθ   0   sinθ]
        [0      1   0   ]
        [-sinθ  0   cosθ]

// 绕X轴旋转（横滚）
Rx(φ) = [1   0      0   ]
        [0   cosφ  -sinφ]
        [0   sinφ   cosφ]

// 完整旋转矩阵（Z-Y-X顺序）
R = Rz(ψ) * Ry(θ) * Rx(φ)

旋转矩阵的好处是：没有万向锁问题，数学性质好。但坏处也很明显——9个元素，每次更新都要做大量乘法和加法。在早期的8位MCU上，这简直是噩梦。我记得有一次用STM32F103做飞控，旋转矩阵更新一次要花掉好几毫秒，CPU都快被撑爆了。

4.2.3 四元数：飞控界的“最优解”

四元数是一个超复数，形式为 q = w + xi + yj + zk。它用4个参数来表示旋转。你不需要深究它的数学原理，只需要知道：

• 它没有万向锁问题
• 计算量比旋转矩阵小（只有4个元素）
• 插值平滑，适合做姿态融合

四元数和欧拉角的转换关系如下：

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

// 欧拉角转四元数
w = cos(φ/2)*cos(θ/2)*cos(ψ/2) + sin(φ/2)*sin(θ/2)*sin(ψ/2)
x = sin(φ/2)*cos(θ/2)*cos(ψ/2) - cos(φ/2)*sin(θ/2)*sin(ψ/2)
y = cos(φ/2)*sin(θ/2)*cos(ψ/2) + sin(φ/2)*cos(θ/2)*sin(ψ/2)
z = cos(φ/2)*cos(θ/2)*sin(ψ/2) - sin(φ/2)*sin(θ/2)*cos(ψ/2)

4.3 三种表示方法的对比

我把它们放在一起对比，你一看就明白了：

特性	欧拉角	旋转矩阵	四元数
参数数量	3个	9个	4个
直观性	★★★★★	★★	★
计算效率	高	低	中高
万向锁	有	无	无
插值平滑性	差	中	好
适用场景	人机交互、显示	理论推导	飞控内部运算

4.4 知识体系总览

为了让你对整个姿态解算的知识结构有个整体印象，我画了一张图：

从这张图你可以看到，整个姿态解算的起点是坐标系定义，然后通过旋转关系连接起来，最后用三种不同的数学工具来描述这个旋转。我个人建议你先把坐标系和欧拉角搞明白，因为这是最直观的。然后再去啃四元数，这是实际工程中最常用的。

---