姿态解算基础：欧拉角、四元数、旋转矩阵、姿态更新算法
各位工程师朋友，今天我们来聊聊姿态解算。这是IMU融合系统的核心环节，说白了就是回答一个问题：传感器到底朝哪个方向？
我刚开始做惯导时，觉得姿态解算不就是算几个角度嘛。后来在无人机项目上摔过两次机，才明白这里面门道有多深。嗯，咱们一步步来。
一、欧拉角：最直观，但最危险
欧拉角用三个角度描述旋转：横滚（Roll）、俯仰（Pitch）、航向（Yaw）。你可以想象成飞机在空中转三下：先转航向，再抬头低头，最后左右倾斜。
核心公式：从机体坐标系到导航坐标系的旋转顺序通常为 Z-Y-X（航向→俯仰→横滚）
我在项目中遇到过一个问题：用欧拉角做姿态更新，飞机做大机动时突然角度跳变。这就是著名的万向锁（Gimbal Lock）——当俯仰角接近±90°时，航向和横滚无法区分。
避坑指南：我曾经在四轴飞行器上只用欧拉角做姿态控制，结果翻滚时角度直接飞掉。后来全部改用四元数，问题解决。记住：全角度运动千万别用欧拉角。
欧拉角的微分方程长这样：
// 欧拉角速率与角速度的关系
// 注意：当pitch接近90°时，分母cos(pitch)趋近于0
roll_dot = wx + wy*sin(roll)*tan(pitch) + wz*cos(roll)*tan(pitch)
pitch_dot = wy*cos(roll) - wz*sin(roll)
yaw_dot = wy*sin(roll)/cos(pitch) + wz*cos(roll)/cos(pitch)二、四元数：工程界的标准答案
四元数是什么？你可以把它理解成一个标量加一个三维向量，写成 q = [w, x, y, z]。它没有奇点，计算效率高，是IMU融合的事实标准。
我个人习惯用四元数做姿态更新，原因很简单：稳定、无奇点、好插值。你想想看，一个旋转用四个数表示，比三个数多了一个自由度，反而避免了万向锁。
实用技巧：四元数一定要归一化！我见过太多人忘记这步，结果姿态越算越偏。每次更新后记得做：q = q / norm(q)
四元数乘法表示旋转合成：
// 四元数乘法（Hamilton积）
q1 * q2 = [w1*w2 - x1*x2 - y1*y2 - z1*z2,
w1*x2 + x1*w2 + y1*z2 - z1*y2,
w1*y2 - x1*z2 + y1*w2 + z1*x2,
w1*z2 + x1*y2 - y1*x2 + z1*w2]三、旋转矩阵：数学上完美，工程上沉重
旋转矩阵是一个3×3的正交矩阵，行列式为+1。它把向量从一个坐标系变换到另一个坐标系。数学上非常优雅，但工程上……嗯，9个数要维护正交性，太累了。
我记得在嵌入式平台上跑旋转矩阵更新，每次都要做Gram-Schmidt正交化，CPU开销大得离谱。后来果断换成四元数，计算量直接砍半。
| 表示方法 | 参数数量 | 奇点 | 计算效率 | 工程推荐度 |
|---|---|---|---|---|
| 欧拉角 | 3 | 有（万向锁） | 高 | ★★（仅小角度） |
| 四元数 | 4 | 无 | 高 | ★★★★★ |
| 旋转矩阵 | 9 | 无 | 低 | ★★★ |
四、姿态更新算法：龙格库塔法
姿态更新，说白了就是解微分方程。陀螺仪输出角速度，我们要积分出角度。最简单的欧拉法误差太大，我一般用四阶龙格库塔法（RK4）。
为什么会这样？因为姿态微分方程是非线性的，欧拉法一步到位误差累积很快。RK4在每个时间步内做四次评估，精度高得多。
核心思想：RK4用四个斜率加权平均，代替欧拉法的单点斜率。公式如下：
// 四元数姿态更新的RK4实现
// q: 当前四元数, w: 角速度(rad/s), dt: 时间步长
void quaternion_update_rk4(float q[4], float wx, float wy, float wz, float dt) {
float k1[4], k2[4], k3[4], k4[4];
float q_temp[4];
// k1 = f(q, t)
quaternion_derivative(q, wx, wy, wz, k1);
// k2 = f(q + dt/2 * k1, t + dt/2)
for(int i=0; i<4; i++) q_temp[i] = q[i] + 0.5f*dt*k1[i];
quaternion_derivative(q_temp, wx, wy, wz, k2);
// k3 = f(q + dt/2 * k2, t + dt/2)
for(int i=0; i<4; i++) q_temp[i] = q[i] + 0.5f*dt*k2[i];
quaternion_derivative(q_temp, wx, wy, wz, k3);
// k4 = f(q + dt * k3, t + dt)
for(int i=0; i<4; i++) q_temp[i] = q[i] + dt*k3[i];
quaternion_derivative(q_temp, wx, wy, wz, k4);
// 加权平均更新
for(int i=0; i<4; i++) {
q[i] += dt/6.0f * (k1[i] + 2*k2[i] + 2*k3[i] + k4[i]);
}
// 归一化
float norm = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
for(int i=0; i<4; i++) q[i] /= norm;
}
// 四元数导数：dq/dt = 0.5 * q * w
void quaternion_derivative(float q[4], float wx, float wy, float wz, float dq[4]) {
dq[0] = 0.5f * (-q[1]*wx - q[2]*wy - q[3]*wz);
dq[1] = 0.5f * ( q[0]*wx + q[2]*wz - q[3]*wy);
dq[2] = 0.5f * ( q[0]*wy - q[1]*wz + q[3]*wx);
dq[3] = 0.5f * ( q[0]*wz + q[1]*wy - q[2]*wx);
}注意：RK4虽然精度高，但计算量是欧拉法的4倍。在低端MCU上（比如STM32F1），如果IMU更新频率1kHz，用RK4可能扛不住。我建议：200Hz以下用RK4，以上用二阶龙格库塔或欧拉法。
五、知识体系总览
下面这张图是我自己整理的姿态解算知识结构，帮你理清思路：
这张图把姿态解算分成三大块：表示方法、更新算法、关键问题。我个人建议你从四元数入手，配合RK4更新，这是最稳妥的方案。
实战建议：刚开始调姿态时，先用仿真数据验证算法。我习惯把陀螺仪数据录下来，在PC上跑MATLAB对比不同算法的误差。等算法没问题了，再烧到嵌入式板子上。这样能省下大量调试时间。
好了，姿态解算的基础就聊到这里。记住：四元数保平安，RK4提精度，归一化不能忘。下次我们聊怎么把加速度计和磁力计的数据融合进来，修正陀螺仪的漂移。