第2章飞行器运动模型

2、飞行器运动模型：刚体运动学方程、四元数与欧拉角、IMU测量模型

好，咱们进入正题。这一章要聊的是飞行器的运动模型。说白了，就是搞清楚飞机在天上到底是怎么动的，以及我们怎么用数学去描述它。

我个人习惯，做控制算法之前，先把模型吃透。模型不对，后面滤波和控制全是白搭。你想想看，你连飞机怎么转、怎么加速都没搞明白，怎么去估计它的状态？

飞行器在空中的运动，可以拆成两部分：平动和转动。平动就是位置的变化，转动就是姿态的变化。

我们先看平动。在惯性坐标系下，牛顿第二定律告诉我们：

m * a = F

这里的 a 是加速度，F 是合外力。但注意，这个加速度是在惯性系下看的。而我们实际测量加速度，通常是在机体坐标系下。所以需要转换。

我建议你记住这个核心方程：

位置微分：  p_dot = v
速度微分：  v_dot = (1/m) * R * F_b + g

其中：

再看转动。转动用角速度来描述。角速度 ω 和姿态的变化率之间，存在一个几何关系。这个关系，就是运动学方程的核心。

嗯，这里要注意：转动运动学方程的形式，取决于你用什么姿态表示方法。用欧拉角是一种写法，用四元数是另一种写法。我们后面会细说。

核心要点：刚体运动学方程，本质上是把「力」和「力矩」映射到「位置、速度、姿态」的变化率上。这是状态估计的「过程模型」。

说到姿态表示，就绕不开欧拉角和四元数。我在项目中遇到过不少因为姿态表示选错而踩坑的案例。

欧拉角很直观。就是绕三个轴转三次：偏航（Yaw）、俯仰（Pitch）、滚转（Roll）。

但欧拉角有个致命问题——万向锁。当俯仰角接近 ±90° 时，偏航和滚转的旋转轴会重合，丢失一个自由度。你想想看，飞机如果垂直向上飞，这时候你分不清是偏航在转还是滚转在转。

所以，我建议在控制算法内部，尽量用四元数。

四元数是一个超复数：

q = q0 + q1*i + q2*j + q3*k

它没有万向锁问题，而且插值平滑，计算效率也高。唯一的缺点是不太直观。你没法一眼看出飞机头朝哪。

两者之间的转换关系，我列个表给你：

转换方向	公式
欧拉角 → 四元数	q = q_yaw(ψ) ⊗ q_pitch(θ) ⊗ q_roll(φ)
四元数 → 欧拉角	φ = atan2(2(q0q1+q2q3), 1-2(q1²+q2²)) θ = asin(2(q0q2-q3q1)) ψ = atan2(2(q0q3+q1q2), 1-2(q2²+q3²))

我的习惯：在状态估计器内部，全程用四元数。只在输出给上层控制或显示时，才转成欧拉角。这样既避免了万向锁，又方便人理解。

IMU（惯性测量单元）是状态估计的眼睛。它包含加速度计和陀螺仪。

加速度计测量的是比力（specific force），也就是物体受到的合力减去重力。在机体坐标系下：

a_meas = a_body - g_body + bias_a + noise_a

注意！加速度计测的不是纯运动加速度。它测的是「你感觉到的加速度」。自由落体时，加速度计读数是 0。为什么？因为失重了，你感觉不到力。

陀螺仪测量的是角速度：

ω_meas = ω_true + bias_ω + noise_ω

陀螺仪的问题在于零偏（bias）会随时间漂移。我曾经在一个项目中，陀螺仪刚上电时零偏很小，飞了十分钟后漂了 0.5°/s。如果不补偿，姿态估计会越偏越远。

避坑指南：我曾经因为忽略了加速度计和陀螺仪的时间同步问题，导致融合出来的姿态在高机动时出现明显滞后。后来加了硬件时间戳和插值对齐，才解决。IMU数据的时间戳对齐，是工程实现中非常容易被忽视的一环。

总结一下IMU测量模型的几个关键点：

好了，这一章的内容就这些。运动模型是状态估计的基石。把刚体运动学、姿态表示、IMU测量模型搞清楚了，后面做卡尔曼滤波才能有的放矢。

下一章，我们会把这些模型用到实际的滤波算法中。到时候你会发现，前面这些看似枯燥的公式，其实都是宝贝。