2. 多旋翼动力学基础：坐标系定义、刚体运动学与动力学方程、控制效率矩阵推导

各位同学，欢迎来到多旋翼动力学这一讲。说实话，这部分内容是整个飞控系统的“地基”。你想想看，如果连无人机怎么动、受什么力都搞不清楚，那后面的故障诊断和容错控制就无从谈起了。我个人习惯，在开始任何算法设计之前，先把坐标系和基本方程理得清清楚楚，这样后面遇到问题才能快速定位。

2.1 坐标系定义：别让方向搞晕你

做飞控这么多年，我见过太多新手在坐标系上栽跟头。说白了，坐标系就是描述无人机运动的“参考系”。我们主要用两个：

• 地球坐标系（E系）：也叫惯性系，原点通常选在地面某点。X轴指向北，Y轴指向东，Z轴指向地心。这个坐标系是绝对的，用来描述无人机的位置和姿态。
• 机体坐标系（B系）：原点在无人机质心。X轴指向机头，Y轴指向右侧，Z轴指向下方。这个坐标系是跟着飞机一起转的，用来描述电机推力、陀螺仪测量值等。

嗯，这里要注意：两个坐标系之间的转换，全靠旋转矩阵。我记得有一次调试一个四旋翼，发现姿态解算总是有偏差，查了半天才发现是坐标系定义搞反了——把机体Z轴朝上了。这种低级错误，一旦飞起来就是炸机。

2.2 刚体运动学方程：位置和姿态怎么变

运动学只描述“怎么动”，不关心“为什么动”。说白了，就是位置和速度、姿态和角速度之间的关系。

位置运动学：

p_dot = v

其中 p 是位置向量，v 是速度向量。这个太简单了，没什么好说的。

姿态运动学：

这个稍微复杂一点。我们用四元数 q 来表示姿态，因为它没有万向节死锁问题。我在项目中遇到过，用欧拉角做大角度机动时，俯仰角到90度直接炸了——这就是死锁。从那以后，我所有飞控代码都用四元数。

q_dot = 0.5 * q ⊗ ω

其中 ω 是机体角速度，⊗ 表示四元数乘法。

2.3 刚体动力学方程：力与力矩的平衡

动力学才是核心。它告诉我们：无人机为什么会加速？为什么会旋转？

平动动力学：

m * v_dot = m * g + R * T + F_d

其中：

• m 是无人机质量
• g 是重力向量 [0, 0, g]^T
• T 是总推力向量（在机体坐标系下，通常只有 Z 轴分量）
• F_d 是空气阻力等外部扰动

转动动力学：

J * ω_dot = -ω × (J * ω) + τ + τ_d

其中：

• J 是转动惯量矩阵（3x3）
• τ 是控制力矩
• τ_d 是外部扰动力矩

你想想看，这个方程其实很直观：角加速度等于控制力矩减去陀螺效应力矩，再除以转动惯量。我刚开始做飞控时，总觉得陀螺效应项可以忽略，结果做高速偏航旋转时，飞机直接失控了。嗯，从那以后我再也不敢小看这个交叉项了。

2.4 控制效率矩阵推导：从电机转速到力和力矩

这部分是连接“控制指令”和“物理执行”的桥梁。说白了，就是告诉你：给四个电机发不同的转速，飞机到底会怎么动？

对于标准的 X 型四旋翼，电机布局如下：

电机编号	位置	旋转方向
1	右前	顺时针
2	左后	顺时针
3	左前	逆时针
4	右后	逆时针

每个电机产生两个效果：

• 推力：F_i = k_f * ω_i²，方向沿机体 Z 轴向上
• 反扭矩：M_i = k_m * ω_i²，方向与旋转方向相反

那么，总推力和三个轴的控制力矩可以写成：

[ T ]   [  k_f   k_f   k_f   k_f  ] [ ω₁² ]
[ τx ] = [ -l*k_f  l*k_f -l*k_f  l*k_f ] [ ω₂² ]
[ τy ]   [  l*k_f  l*k_f -l*k_f -l*k_f ] [ ω₃² ]
[ τz ]   [  k_m  -k_m  -k_m   k_m  ] [ ω₄² ]

这个 4x4 矩阵就是 控制效率矩阵，记作 G。其中 l 是电机到质心的距离。

有了 G 矩阵，我们就可以做控制分配了：

[ω₁², ω₂², ω₃², ω₄²]^T = G⁻¹ * [T, τx, τy, τz]^T

嗯，这里要注意：G 矩阵求逆的前提是满秩。如果某个电机坏了，G 矩阵就变成 4x3 了，这时候需要用伪逆或者重新分配策略。这部分我们在后面的容错控制章节会详细讲。

2.5 小结：动力学模型是飞控的“灵魂”

好了，这一讲的内容就到这里。我们梳理了：

• 坐标系定义：地球系和机体系，以及它们之间的转换
• 运动学方程：位置和姿态的变化率
• 动力学方程：力和力矩如何产生加速度
• 控制效率矩阵：从电机转速到控制力的映射

说实话，这些公式看起来枯燥，但它们是所有飞控算法的基础。我建议你亲手推导一遍，把每个符号的含义都搞清楚。下一讲我们会进入故障诊断部分，到时候你会发现，很多故障模式其实就藏在这些方程里。

有问题随时交流。我们下节课见。