1. 姿态控制基础：坐标系定义与欧拉角、四元数基础、刚体运动学方程、姿态表示方法对比

各位同学，咱们开始第一讲。姿态控制，说白了就是让飞行器知道自己“脑袋朝哪、身子正不正”。我做了这么多年飞控，见过太多因为坐标系搞混导致炸机的案例。所以这一章，咱们把地基打牢。

1.1 坐标系定义：你得有个“绝对参考”

飞控里最常用的两套坐标系：世界坐标系（NED）和机体坐标系（Body）。我个人习惯把世界坐标系想象成“大地上的北东地”，机体坐标系就是“机头朝哪、右翼朝哪、肚子朝哪”。

• NED坐标系：X轴指向北，Y轴指向东，Z轴指向地心。原点通常在地面起飞点。
• Body坐标系：X轴指向机头，Y轴指向右翼，Z轴指向机身下方。原点在飞行器质心。

嗯，这里要注意：两个坐标系之间的转换，是所有姿态算法的起点。你想想看，如果传感器读到的数据是机体坐标系下的，而你要控制的是世界坐标系下的姿态，中间差了啥？——旋转矩阵。

1.2 欧拉角：直观但“有坑”

欧拉角用三个角度描述姿态：滚转（Roll, φ）、俯仰（Pitch, θ）、偏航（Yaw, ψ）。直观吧？一看就懂。但为什么很多飞控工程师对它又爱又恨？

因为万向锁（Gimbal Lock）。当俯仰角接近±90°时，滚转和偏航会耦合，丢失一个自由度。说白了，你没法区分“是滚转还是偏航造成的旋转”。

1.3 四元数基础：飞控的“内功心法”

四元数是个超复数，形式为 q = w + xi + yj + zk，其中w是实部，x、y、z是虚部。它用四个参数表示三维旋转，没有万向锁，插值平滑，计算效率高。

为什么飞控里都用它？我给你列几个理由：

• 无奇点：无论怎么转，都不会出现万向锁。
• 易插值：球面线性插值（SLERP）可以平滑过渡姿态。
• 计算快：乘法只涉及加减乘除，没有三角函数。

来看一段四元数乘法的代码，这是姿态融合的核心操作：

// 四元数乘法：q = q1 * q2
void quaternion_multiply(float q1[4], float q2[4], float q_out[4]) {
    q_out[0] = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3];
    q_out[1] = q1[0]*q2[1] + q1[1]*q2[0] + q1[2]*q2[3] - q1[3]*q2[2];
    q_out[2] = q1[0]*q2[2] - q1[1]*q2[3] + q1[2]*q2[0] + q1[3]*q2[1];
    q_out[3] = q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1] + q1[3]*q2[0];
}

嗯，代码看着有点长，但实际在STM32或DSP上跑，也就几十个时钟周期。我个人习惯把四元数归一化放在每次更新后，防止累积误差导致模长偏离1。

1.4 刚体运动学方程：姿态怎么“动”起来

有了姿态表示，还得知道姿态怎么随时间变化。刚体运动学方程描述了角速度与姿态变化率的关系。

用四元数表示的运动学方程：

dq/dt = 0.5 * q ⊗ ω

其中ω是机体角速度（陀螺仪测量值），⊗表示四元数乘法。这个方程是姿态解算的核心，所有互补滤波、卡尔曼滤波都在用它做预测。

如果用欧拉角表示，方程会长这样：

φ_dot = p + q*sin(φ)*tan(θ) + r*cos(φ)*tan(θ)
θ_dot = q*cos(φ) - r*sin(φ)
ψ_dot = (q*sin(φ) + r*cos(φ)) / cos(θ)

看到没？当θ接近90°时，cos(θ)趋近于0，ψ_dot会爆炸。这就是万向锁的数学本质。

1.5 姿态表示方法对比：选对工具做对事

我把三种主流姿态表示方法放在一起对比，方便你选型：

你想想看，飞控里每秒要跑几百上千次姿态更新，选四元数几乎是必然的。旋转矩阵虽然无奇点，但9个参数冗余，计算量大；欧拉角虽然直观，但奇点致命。

1.6 本章知识体系总览

下面这张图总结了本章的核心逻辑：从坐标系出发，经过姿态表示，再到运动学方程，最后落地到工程选型。你可以把它当作飞控姿态部分的“地图”。

这张图把本章的知识点串起来了。你从坐标系出发，选择姿态表示方法，然后推导运动学方程，最后根据工程需求做选型。每一步都有坑，但只要你按这个框架走，就不会迷路。