3. 多旋翼刚体运动学：位置、速度、加速度的微分关系，姿态运动学方程（四元数微分方程）

好，我们进入第三讲。这一讲的内容，说白了就是回答一个问题：无人机在三维空间里到底是怎么动的？

你可能会觉得，这还不简单？推油门它就往上飞呗。但作为飞控算法工程师，我们得把这种「感觉」翻译成数学语言。位置、速度、加速度之间是什么关系？姿态又是怎么随时间变化的？这些是MPC预测模型的基础。如果这一步搞错了，后面所有的控制都是空中楼阁。

我个人习惯把运动学分成两部分来看：线运动和角运动。咱们一个一个来。

3.1 线运动：位置、速度、加速度的微分链

先看最简单的部分。在惯性坐标系（通常我们取NED系，即北东地坐标系）下，多旋翼的位置、速度、加速度满足一个非常直接的微分关系：

p_dot = v
v_dot = a

嗯，就是速度是位置的导数，加速度是速度的导数。这看起来简单到不值一提，对吧？

但我在项目中遇到过一个问题：很多初学者会把「机体坐标系下的速度」和「惯性坐标系下的速度」搞混。

你想想看，MPC预测的是未来几秒内的位置轨迹。我们通常是在惯性坐标系下做预测的。所以，状态量里的速度 v，必须是惯性系下的速度。而机载的IMU（惯性测量单元）测量的是机体加速度，需要经过旋转矩阵转换到惯性系，才能用来更新状态。

所以，完整的线运动微分方程，在状态空间里长这样：

p_x_dot = v_x
p_y_dot = v_y
p_z_dot = v_z
v_x_dot = (R * [0, 0, T/m]^T + [0, 0, g]^T)_x
v_y_dot = (R * [0, 0, T/m]^T + [0, 0, g]^T)_y
v_z_dot = (R * [0, 0, T/m]^T + [0, 0, g]^T)_z

这里 R 是旋转矩阵，T 是总推力，m 是质量，g 是重力加速度。你看，加速度的表达式里，推力方向是由姿态 R 决定的。这就把线运动和角运动耦合起来了。

3.2 姿态运动学：四元数微分方程

接下来是重头戏——姿态运动学。多旋翼的姿态描述方式有很多：欧拉角、旋转矩阵、四元数。我个人强烈推荐在MPC里使用四元数。

为什么？因为欧拉角有万向锁问题，旋转矩阵有9个参数冗余。四元数只有4个参数，没有奇异性，而且插值方便。嗯，这里要注意，四元数虽然好，但必须归一化，否则会出大问题。

四元数描述的是从机体坐标系到惯性坐标系的旋转。它的微分方程是：

q_dot = 0.5 * q ⊗ ω

其中 q 是四元数，ω 是机体角速度（通常用 [p, q, r] 表示），⊗ 表示四元数乘法。

写成矩阵形式更直观：

[q0_dot]   [ 0  -p  -q  -r] [q0]
[q1_dot] = [ p   0   r  -q] [q1] * 0.5
[q2_dot]   [ q  -r   0   p] [q2]
[q3_dot]   [ r   q  -p   0] [q3]

这个方程，就是姿态预测的核心。在MPC里，我们每预测一步，都要用这个方程更新四元数。

3.3 从角速度到四元数：离散化实现

好，理论讲完了，咱们看看代码怎么写。假设我们有一个当前的四元数 q 和角速度 omega，时间步长 dt，预测下一时刻的四元数：

// 四元数微分方程离散化（一阶龙格-库塔法）
void quaternion_step(double q[4], double omega[3], double dt) {
    double q_dot[4];
    double p = omega[0], q_ = omega[1], r = omega[2];
    
    q_dot[0] = 0.5 * (-p * q[1] - q_ * q[2] - r * q[3]);
    q_dot[1] = 0.5 * ( p * q[0] + r * q[2] - q_ * q[3]);
    q_dot[2] = 0.5 * ( q_ * q[0] - r * q[1] + p * q[3]);
    q_dot[3] = 0.5 * ( r * q[0] + q_ * q[1] - p * q[2]);
    
    // 更新
    q[0] += q_dot[0] * dt;
    q[1] += q_dot[1] * dt;
    q[2] += q_dot[2] * dt;
    q[3] += q_dot[3] * dt;
    
    // 归一化！这一步绝对不能省
    double 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;
}

3.4 知识体系总览

为了让你对整个运动学模型有个全局概念，我画了一张图。它把线运动和角运动的关系、以及它们在MPC中的角色都串起来了。

3.5 工程中的注意事项

最后，我总结几个工程中容易忽略的点：

• 坐标系一致性： 所有状态量必须在同一个坐标系下定义。我习惯全部用NED系，包括位置、速度、加速度。角速度在机体系下测量，但四元数微分方程本身就是在机体系下定义的，所以没问题。
• 时间步长选择： MPC的预测步长通常比控制周期大。比如控制周期1ms，预测步长可能取10ms。这时候四元数微分方程的离散化精度就很重要了。我建议用二阶或解析方法。
• 数值稳定性： 四元数归一化是必须的，但不要每步都做。如果步长很小，可以每5-10步归一化一次，节省计算量。但如果你不确定，每步归一化最安全。
• 初始值： 四元数的初始值必须准确。如果初始姿态错了，后面预测的全是错的。我一般用加速度计和磁力计做初始化，或者用视觉/IMU融合的结果。

好，这一讲就到这里。运动学是基础中的基础，但也是最容易出错的地方。你把它吃透了，后面MPC的约束设计、代价函数设计才能站得住脚。