2. 无人机动力学模型：四旋翼飞行原理、坐标系与姿态表示

好，我们正式开始第二讲。

上一章我们聊了PID控制器的基本框架，但有个问题你想想看——PID控制的对象是谁？是无人机本身。如果你不了解无人机怎么飞的，那调参就是瞎调。我个人习惯是，先搞懂物理模型，再写代码。这样出了问题，你才知道是算法错了，还是模型错了。

2.1 四旋翼飞行原理：它到底怎么动起来的？

四旋翼无人机，说白了就是四个电机带着四个桨叶转。但为什么它能前后左右飞？这里有个关键点：力矩平衡。

四个电机分成两组：

• 1号、3号电机：顺时针旋转（CW）
• 2号、4号电机：逆时针旋转（CCW）

为什么要这样设计？因为电机旋转会产生反扭矩。如果四个电机同向转，机身会跟着打转。我刚开始做飞控时，第一次试飞就遇到这个问题——飞机上天后疯狂自旋，差点炸机。后来才意识到，反扭矩没抵消掉。

四旋翼的基本运动分为四种：

1. 垂直升降：四个电机同时增加或减少转速。升力大于重力就上升，反之下降。
2. 俯仰（前后运动）：1号和2号电机加速，3号和4号电机减速。机身向前倾斜，产生水平分力。
3. 横滚（左右运动）：1号和4号电机加速，2号和3号电机减速。机身向一侧倾斜。
4. 偏航（旋转）：两组对角电机转速差产生偏航力矩。比如1号和3号加速，2号和4号减速，机身就会顺时针旋转。

嗯，这里要注意：实际飞行中，电机响应有延迟，不是你说加速它就立刻加速。这个延迟在PID调参时是个大坑，我们后面会专门讲。

2.2 坐标系：你的参考系选对了吗？

搞飞控，坐标系是绕不开的。我见过不少新手，姿态解算出来数据乱跳，最后发现是坐标系搞反了。

我们通常用两个坐标系：

2.2.1 地球坐标系（NED系）

也叫惯性坐标系。原点在地面某点，X轴指向北，Y轴指向东，Z轴指向地心。说白了就是「大地参考系」。这个坐标系用来描述无人机的位置和速度。

2.2.2 机体坐标系（Body系）

原点在无人机重心，X轴指向机头，Y轴指向右侧，Z轴指向下方。这个坐标系用来描述无人机的姿态和角速度。

为什么需要两个坐标系？因为传感器数据是在机体坐标系下测量的，但导航和控制需要在地球坐标系下计算。这就涉及到坐标变换。

2.3 姿态表示：欧拉角、旋转矩阵与四元数

姿态表示是飞控的核心。你想想看，你要控制无人机保持水平，首先得知道它现在是什么姿态。常用的表示方法有三种：

2.3.1 欧拉角

欧拉角就是三个角度：俯仰角（θ）、横滚角（φ）、偏航角（ψ）。

• 俯仰角：机头上下摆动，范围 -90° 到 +90°
• 横滚角：机身左右倾斜，范围 -180° 到 +180°
• 偏航角：机头指向，范围 0° 到 360°

欧拉角直观，但有个致命问题——万向锁。当俯仰角接近 ±90° 时，横滚和偏航会耦合，丢失一个自由度。我在做特技飞行时遇到过这个问题，飞机翻过来后姿态解算直接崩了。

2.3.2 旋转矩阵

旋转矩阵是一个 3x3 的矩阵，用来描述两个坐标系之间的旋转关系。从机体坐标系到地球坐标系的旋转矩阵 R 可以表示为：

R = Rz(ψ) * Ry(θ) * Rx(φ)

其中：
Rx(φ) = [1, 0, 0; 0, cosφ, -sinφ; 0, sinφ, cosφ]
Ry(θ) = [cosθ, 0, sinθ; 0, 1, 0; -sinθ, 0, cosθ]
Rz(ψ) = [cosψ, -sinψ, 0; sinψ, cosψ, 0; 0, 0, 1]

旋转矩阵没有万向锁问题，但计算量大，而且需要正交化处理，否则误差会累积。

2.3.3 四元数

四元数是我个人最推荐的方式。它用四个数表示旋转：q = [w, x, y, z]，其中 w 是实部，x、y、z 是虚部。

四元数的优点：

• 没有万向锁
• 计算量小，只需要乘法和加法
• 容易插值，适合平滑过渡

四元数转欧拉角的公式：

φ = atan2(2*(w*x + y*z), 1 - 2*(x² + y²))
θ = asin(2*(w*y - z*x))
ψ = atan2(2*(w*z + x*y), 1 - 2*(y² + z²))

2.4 动力学方程：从力到运动的桥梁

有了坐标系和姿态表示，我们就能写出无人机的动力学方程了。这里我直接给出简化模型：

平动方程：

m * a = F_gravity + F_thrust + F_drag

其中：
m 是无人机质量
a 是加速度向量
F_gravity = [0, 0, -mg] 重力
F_thrust = R * [0, 0, T] 升力，T是总升力
F_drag 是空气阻力，通常忽略不计

转动方程：

J * α = M_thrust - ω × (J * ω)

其中：
J 是转动惯量矩阵
α 是角加速度向量
M_thrust 是电机产生的力矩
ω 是角速度向量
ω × (J * ω) 是陀螺力矩

嗯，这里要注意：陀螺力矩在高速旋转时影响很大。我做过一个高速偏航测试，飞机差点失控，就是因为没考虑陀螺效应。

2.5 代码实现：从模型到C语言

理论讲完了，我们看看代码怎么写。以下是一个简化的姿态更新函数：

// 四元数姿态更新
void attitude_update(float gx, float gy, float gz, float dt) {
    // gx, gy, gz 是陀螺仪角速度，单位 rad/s
    // dt 是采样时间间隔
    
    float q0 = q[0], q1 = q[1], q2 = q[2], q3 = q[3];
    
    // 四元数微分方程
    float dq0 = 0.5f * (-q1*gx - q2*gy - q3*gz);
    float dq1 = 0.5f * ( q0*gx + q2*gz - q3*gy);
    float dq2 = 0.5f * ( q0*gy - q1*gz + q3*gx);
    float dq3 = 0.5f * ( q0*gz + q1*gy - q2*gx);
    
    // 更新四元数
    q[0] += dq0 * dt;
    q[1] += dq1 * dt;
    q[2] += dq2 * dt;
    q[3] += dq3 * dt;
    
    // 归一化
    float 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;
}

2.6 本章小结

这一章我们讲了：

• 四旋翼的飞行原理：力矩平衡和四种基本运动
• 坐标系：地球坐标系和机体坐标系
• 姿态表示：欧拉角、旋转矩阵、四元数
• 动力学方程：平动和转动
• 代码实现：四元数姿态更新

下一章我们会讲传感器数据融合，也就是怎么把陀螺仪、加速度计、磁力计的数据融合成准确的姿态。到时候我会分享一个我踩过的坑——加速度计数据在剧烈运动时完全不能用，差点让我怀疑人生。

好，今天就到这里。有问题随时交流。