第4章姿态解算算法

第4章：姿态解算算法：基于四元数的姿态更新、欧拉角解算、坐标系转换实战

各位同学，欢迎来到第四章。前面我们聊了传感器，聊了怎么把陀螺仪、加速度计的数据读出来。但光有原始数据没用，你得知道飞机现在“脸”朝哪边，对吧？

这一章，我们就来解决这个核心问题——姿态解算。说白了，就是通过算法，把传感器数据变成我们人能看懂的“俯仰角”、“横滚角”、“偏航角”。

我个人习惯把姿态解算比作“灵魂拷问”：陀螺仪说“你转了”，加速度计说“你歪了”，磁力计说“你方向偏了”。我们得综合这些“证词”，还原出最真实的姿态。嗯，这里面的门道不少，我们一步步来。

本章核心脉络： 坐标系定义 → 四元数基础 → 四元数更新姿态 → 欧拉角解算 → 坐标系转换实战

4.1 坐标系——你得先知道“北”在哪

做姿态解算，第一件事就是定坐标系。没有坐标系，你谈什么角度？

我们通常用两个坐标系：

导航坐标系（n系）：也叫地理坐标系。一般取“北-东-地”或者“东-北-天”。我习惯用“北-东-地”，因为和大多数惯性导航教材一致。
机体坐标系（b系）：固定在飞机上。原点在重心，x轴指向机头，y轴指向右翼，z轴指向下（符合右手定则）。

你想想看，姿态解算的本质，就是求机体坐标系相对于导航坐标系的旋转关系。这个关系，可以用欧拉角表示，也可以用四元数表示。

我的经验： 刚开始做飞控时，我总在坐标系正负号上栽跟头。后来我养成了一个习惯——在代码开头用注释把坐标系定义写清楚，比如：// NED: X-North, Y-East, Z-Down。这能省下大量调试时间。

4.2 四元数——姿态的“瑞士军刀”

为什么不用欧拉角直接算？因为欧拉角有万向锁问题，而且做插值、微分都不方便。四元数就没有这些毛病。

四元数可以看作一个“超复数”：

q = w + x*i + y*j + z*k

其中，w是实部，x、y、z是虚部。它满足：i² = j² = k² = ijk = -1。

一个单位四元数（模长为1）可以表示三维空间中的任意旋转。旋转轴为单位向量 (u_x, u_y, u_z)，旋转角度为 θ，那么对应的四元数为：

q = cos(θ/2) + sin(θ/2) * (u_x*i + u_y*j + u_z*k)

说白了，四元数就是用4个数，优雅地表达了“绕哪个轴转了多少度”。

核心性质： 单位四元数的逆等于它的共轭。即 q⁻¹ = q* = w - x*i - y*j - z*k。这个性质在坐标系转换时非常有用。

4.3 四元数更新姿态——核心算法

陀螺仪输出的是角速度 ω = [ω_x, ω_y, ω_z]ᵀ。我们要用这个角速度来更新四元数。

四元数的微分方程是：

dq/dt = 0.5 * q ⊗ ω

其中 ⊗ 表示四元数乘法，ω 要写成纯四元数形式 [0, ω_x, ω_y, ω_z]。

实际代码中，我们常用一阶龙格-库塔法来离散化更新：

// 四元数更新函数（一阶龙格-库塔法）
void quaternionUpdate(float q[4], float gx, float gy, float gz, float dt) {
    float q0 = q[0], q1 = q[1], q2 = q[2], q3 = q[3];
    
    // 计算四元数导数
    float dq0 = 0.5f * (-q1*gx - q2*gy - q3*gz);
    float dq1 = 0.5f * ( q0*gx + q2*gz - q3*gy);
    float dq2 = 0.5f * ( q0*gy - q1*gz + q3*gx);
    float dq3 = 0.5f * ( q0*gz + q1*gy - q2*gx);
    
    // 更新四元数
    q[0] += dq0 * dt;
    q[1] += dq1 * dt;
    q[2] += dq2 * dt;
    q[3] += dq3 * dt;
    
    // 归一化（防止累积误差导致模长偏离1）
    float norm = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
    q[0] /= norm;
    q[1] /= norm;
    q[2] /= norm;
    q[3] /= norm;
}

注意： 归一化这一步绝对不能省！我曾经在一个项目里忘了归一化，结果飞了30秒后姿态开始漂移，飞机直接翻了。嗯，从那以后我每次更新完四元数，第一件事就是归一化。

4.4 欧拉角解算——从四元数到人眼可读

四元数虽然计算方便，但人眼看不懂。我们需要把它转换成欧拉角：俯仰角（Pitch）、横滚角（Roll）、偏航角（Yaw）。

假设我们使用Z-Y-X旋转顺序（先偏航，再俯仰，最后横滚），从四元数到欧拉角的转换公式如下：

// 从四元数解算欧拉角（弧度）
float roll  = atan2(2.0f*(q0*q1 + q2*q3), 1.0f - 2.0f*(q1*q1 + q2*q2));
float pitch = asin(2.0f*(q0*q2 - q3*q1));
float yaw   = atan2(2.0f*(q0*q3 + q1*q2), 1.0f - 2.0f*(q2*q2 + q3*q3));

这里要注意几点：

asin 的值域是 [-π/2, π/2]，所以俯仰角不会超过±90度。这符合物理限制——飞机一般不会垂直向上或向下。
atan2 的值域是 [-π, π]，能覆盖全范围。
当俯仰角接近±90度时，横滚和偏航会出现耦合（万向锁问题）。但在四元数表示下，我们只是解算时遇到这个问题，四元数本身没有万向锁。

避坑指南： 我曾经在解算偏航角时，发现角度在±180度附近来回跳变。后来我加了一个简单的“unwrap”处理：如果当前角度和上一时刻角度差超过180度，就加减360度补偿。这样偏航角就能平滑变化了。

4.5 坐标系转换实战——向量旋转

姿态解算的另一个重要应用是坐标系转换。比如，加速度计测量的是机体坐标系下的加速度，但我们要把它转换到导航坐标系下才能用于速度积分。

用四元数旋转一个向量 v_b（机体坐标系）到 v_n（导航坐标系）：

v_n = q ⊗ v_b ⊗ q⁻¹

其中 v_b 要写成纯四元数 [0, v_x, v_y, v_z]。

实际代码实现时，我们通常用旋转矩阵来避免四元数乘法的开销：

// 用四元数构建旋转矩阵，将向量从机体坐标系转到导航坐标系
void rotateBodyToNav(float q[4], float v_b[3], float v_n[3]) {
    float q0 = q[0], q1 = q[1], q2 = q[2], q3 = q[3];
    
    // 旋转矩阵元素
    float R11 = q0*q0 + q1*q1 - q2*q2 - q3*q3;
    float R12 = 2.0f*(q1*q2 - q0*q3);
    float R13 = 2.0f*(q1*q3 + q0*q2);
    float R21 = 2.0f*(q1*q2 + q0*q3);
    float R22 = q0*q0 - q1*q1 + q2*q2 - q3*q3;
    float R23 = 2.0f*(q2*q3 - q0*q1);
    float R31 = 2.0f*(q1*q3 - q0*q2);
    float R32 = 2.0f*(q2*q3 + q0*q1);
    float R33 = q0*q0 - q1*q1 - q2*q2 + q3*q3;
    
    // 应用旋转
    v_n[0] = R11*v_b[0] + R12*v_b[1] + R13*v_b[2];
    v_n[1] = R21*v_b[0] + R22*v_b[1] + R23*v_b[2];
    v_n[2] = R31*v_b[0] + R32*v_b[1] + R33*v_b[2];
}

反过来，从导航坐标系转到机体坐标系，只需要用旋转矩阵的转置（因为旋转矩阵是正交矩阵）。

实战要点： 在飞控代码中，坐标系转换通常放在传感器数据预处理阶段。比如，我先用陀螺仪更新四元数，然后用四元数把加速度计数据转到导航坐标系，再去做水平加速度补偿。这个流程我建议你固定下来，不要随意调换顺序。

4.6 本章小结

好了，这一章我们走完了姿态解算的完整链路：

定义好坐标系（n系和b系）
用四元数表示姿态
用陀螺仪数据更新四元数（一阶龙格-库塔法）
从四元数解算出欧拉角
用四元数做坐标系转换

你想想看，这其实就是飞控的“感知层”核心。没有准确的姿态，后面的控制算法全是空中楼阁。

我个人建议你动手写一个简单的姿态解算程序，用MPU6050的数据跑一跑。先别加滤波，看看纯陀螺仪积分出来的姿态能漂移多快——嗯，你会对“为什么要融合”有更深的体会。

下一章，我们就来聊这个“融合”——互补滤波和卡尔曼滤波，敬请期待。

公众号:蓝海资料掘金营，微信deep3321

📚 课程目录