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

各位同学，欢迎来到第三讲。

说实话，姿态解算是整个飞控系统的灵魂。你想想看，无人机在天上飞，它得知道自己「脑袋」朝哪、「身子」歪没歪。这个「自知之明」，就是靠姿态解算给的。

我当年刚入行时，被欧拉角和四元数绕得晕头转向。后来踩了不少坑，才慢慢理清楚。今天我就把这些经验掰开揉碎，讲给你听。

3.1 坐标系定义：先把「家」安好

做姿态解算，第一件事就是定坐标系。坐标系没定好，后面全是糊涂账。

我们常用两套坐标系：

• 地理坐标系（n系）：也叫导航坐标系。通常取「北-东-地」或者「北-东-天」。我个人习惯用「北-东-地」，因为和惯导教材一致。X轴指北，Y轴指东，Z轴指向地心。
• 机体坐标系（b系）：固定在无人机上。X轴指向机头，Y轴指向右翼，Z轴指向机身下方（符合右手定则）。

嗯，这里要注意：两个坐标系的原点重合，都在无人机的重心。我们做的所有姿态解算，本质上就是求这两个坐标系之间的旋转关系。

3.2 欧拉角：最直观，但最危险

欧拉角是三个角度：横滚角（Roll, φ）、俯仰角（Pitch, θ）、偏航角（Yaw, ψ）。

说白了，就是绕三个轴依次转三次。顺序很重要！我们常用的是「Z-Y-X」顺序：先绕Z轴转ψ（偏航），再绕Y轴转θ（俯仰），最后绕X轴转φ（横滚）。

欧拉角的优点很明显：直观，三个数就能描述姿态。但缺点也很致命：万向锁（Gimbal Lock）。当俯仰角接近±90°时，横滚和偏航会耦合，丢失一个自由度。你想想看，无人机做筋斗动作时，俯仰角刚好经过90°，这时候欧拉角就失效了。

3.3 旋转矩阵：数学上完美，计算上沉重

旋转矩阵是一个3×3的正交矩阵，记作 C_n^b（从n系到b系的旋转矩阵）。

用欧拉角可以构建旋转矩阵：

C_n^b = 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]

旋转矩阵的好处是：没有奇点，数学上完美。而且向量旋转、坐标系变换都可以直接用矩阵乘法搞定。

但坏处是：9个参数，计算量大。在嵌入式MCU上，每次更新都要做9次乘加运算，还要做正交化修正（因为数值误差会导致矩阵不再正交）。

3.4 四元数：飞控界的「瑞士军刀」

四元数是个超复数，形式为：q = q0 + q1·i + q2·j + q3·k，其中 i² = j² = k² = i·j·k = -1。

你别被数学吓到。在飞控里，我们只把它当成一个4维单位向量来用：

q = [q0, q1, q2, q3]ᵀ，且 q0² + q1² + q2² + q3² = 1

四元数最大的优点是：紧凑、无奇点、计算快。4个参数，没有万向锁，而且更新时只需要做四元数乘法（4次乘加），比旋转矩阵快一倍以上。

从欧拉角转四元数：

q0 = cos(φ/2)·cos(θ/2)·cos(ψ/2) + sin(φ/2)·sin(θ/2)·sin(ψ/2)
q1 = sin(φ/2)·cos(θ/2)·cos(ψ/2) - cos(φ/2)·sin(θ/2)·sin(ψ/2)
q2 = cos(φ/2)·sin(θ/2)·cos(ψ/2) + sin(φ/2)·cos(θ/2)·sin(ψ/2)
q3 = cos(φ/2)·cos(θ/2)·sin(ψ/2) - sin(φ/2)·sin(θ/2)·cos(ψ/2)

从四元数转欧拉角：

φ = atan2(2·(q0·q1 + q2·q3), 1 - 2·(q1² + q2²))
θ = asin(2·(q0·q2 - q3·q1))
ψ = atan2(2·(q0·q3 + q1·q2), 1 - 2·(q2² + q3²))

3.5 它们之间的转换关系

这三种表示方法可以互相转换。我整理了一张表，你收藏好：

转换方向	方法	注意事项
欧拉角 → 旋转矩阵	按顺序乘三个基本旋转矩阵	注意旋转顺序（Z-Y-X）
欧拉角 → 四元数	用半角公式计算	注意角度单位（弧度）
旋转矩阵 → 四元数	从矩阵元素反解	注意数值稳定性
四元数 → 旋转矩阵	用四元数元素构造	确保四元数已归一化
四元数 → 欧拉角	用atan2和asin	俯仰角接近±90°时注意

3.6 实际工程中的选择建议

说了这么多，到底怎么选？我给你一个实战清单：

• 姿态递推（IMU积分）：用四元数。效率高，无奇点。
• 坐标变换（比如把加速度转到导航系）：用旋转矩阵。直观，好调试。
• 人机交互（显示给飞手）：用欧拉角。飞手只看角度，不看四元数。
• 数据存储（比如记录飞行日志）：用四元数。4个float，比9个float省空间。

好了，这一讲就到这里。下一讲我们开始讲「姿态解算的核心算法：互补滤波与卡尔曼滤波」。到时候我会手把手带你写代码。下课！