2、姿态控制系统基础：姿态描述方法

各位同学，咱们今天聊聊姿态描述。说白了，就是怎么告诉计算机——飞行器现在“脑袋”朝哪。

我刚开始做飞控那会儿，总觉得这玩意儿太理论。后来有一次，仿真跑出来飞机在天上乱翻，查了三天才发现是欧拉角奇异点的问题。嗯，从那以后我再也不敢小看姿态描述了。

2.1 欧拉角——最直观，但有个坑

欧拉角大家应该都熟悉。三个角度：滚转（Roll）、俯仰（Pitch）、偏航（Yaw）。

你想想看，这就像你坐在飞机里：

• 滚转：绕机身纵轴转，像侧身
• 俯仰：绕横轴转，抬头低头
• 偏航：绕竖轴转，左右看

但这里有个大坑——万向锁。当俯仰角接近±90°时，滚转和偏航就分不清了。我曾在某型无人机项目中遇到过，俯仰拉到85°以上，姿态解算直接发散。后来我强制限制了俯仰角范围，才稳住。

2.2 四元数——数学上完美，但抽象

四元数是什么？一个标量加三个矢量。写成 q = [q0, q1, q2, q3]ᵀ。

我个人习惯把四元数想象成“绕一个轴转一个角度”。这个轴是三维的，角度是标量。嗯，这样就好理解多了。

四元数最大的好处：没有奇异点，而且插值平滑。我在做卫星姿态仿真时，全用的四元数。欧拉角只用来给人看。

来个代码示例，四元数归一化——这步千万别忘：

// 四元数归一化，防止数值漂移
void quat_normalize(float q[4]) {
    float norm = sqrt(q[0]*q[0] + q[1]*q[1] + 
                      q[2]*q[2] + q[3]*q[3]);
    if (norm < 1e-10f) return;  // 防止除零
    for (int i = 0; i < 4; i++) {
        q[i] /= norm;
    }
}

2.3 姿态运动学方程

运动学方程，说白了就是“角度怎么随时间变化”。

对于欧拉角，方程长这样：

dot(phi)   = p + q*sin(phi)*tan(theta) + r*cos(phi)*tan(theta)
dot(theta) = q*cos(phi) - r*sin(phi)
dot(psi)   = (q*sin(phi) + r*cos(phi)) / cos(theta)

看到 tan(theta) 了吗？这就是万向锁的数学根源。theta 接近90°，tan 趋于无穷。

四元数的运动学方程就优雅多了：

dot(q) = 0.5 * q ⊗ ω

其中 ω 是角速度，⊗ 是四元数乘法。没有奇异点，没有三角函数，干净利落。

我建议你在 HIL 仿真中，内部运算全用四元数。只在最后输出给显示界面时，才转成欧拉角。

2.4 姿态动力学方程

动力学方程回答的是：“力矩怎么让角速度变化”。

核心公式就是欧拉方程：

I * dot(ω) + ω × (I * ω) = M

其中 I 是惯量矩阵，M 是外力矩。

这里有个容易忽略的点：惯量矩阵。我见过不少同学直接拿个对角阵就往上怼。实际上，真实飞行器的惯量矩阵往往有非对角项。尤其是带大载荷的无人机，耦合项能让你仿真结果和实测差30%。

来个完整的动力学仿真步进代码：

// 姿态动力学一步积分
void attitude_step(float I[3][3], float omega[3], 
                   float M[3], float dt) {
    float I_inv[3][3];
    // 计算惯量逆矩阵（实际项目中预先算好）
    mat3_inv(I, I_inv);
    
    // 计算 ω × (I * ω)
    float Iw[3];
    mat3_vec3_mul(I, omega, Iw);
    float cross[3];
    vec3_cross(omega, Iw, cross);
    
    // 角加速度
    float alpha[3];
    vec3_sub(M, cross, alpha);
    mat3_vec3_mul(I_inv, alpha, alpha);
    
    // 积分
    for (int i = 0; i < 3; i++) {
        omega[i] += alpha[i] * dt;
    }
}

2.5 三种方法的对比

方法	优点	缺点	我推荐的使用场景
欧拉角	直观，容易理解	有万向锁，插值困难	人机交互、数据显示
四元数	无奇异，计算快，可插值	不直观，需要转换	内部运算、控制律、HIL仿真
旋转矩阵	数值稳定	9个参数，冗余大	坐标变换、视觉SLAM

好了，这一章就到这里。下一章咱们聊聊传感器模型——怎么把真实的陀螺仪和加速度计“骗”进仿真环境里。