4、动力学建模（一）：刚体运动学方程、牛顿-欧拉方程、六自由度模型推导
各位同学好，我是你们的飞控算法讲师。今天咱们来啃一块硬骨头——动力学建模。说实话，这块内容我当年学的时候也头大，满屏的矩阵和偏微分，看着就劝退。但后来在实际项目中摔过几次跟头，才明白这些公式背后其实都是物理直觉。
咱们今天的目标很明确：搞懂刚体在三维空间里怎么动，以及怎么用数学描述它。说白了，就是给飞行器写一份「运动说明书」。
4.1 刚体运动学：位置、速度、姿态
先问大家一个问题：一个刚体在空间中有几个自由度？答案是六个——三个平动（前后、左右、上下），三个转动（俯仰、滚转、偏航）。这就是我们常说的六自由度（6-DOF）模型。
我个人习惯把运动学和动力学分开看。运动学只管「怎么动」，不管「为什么动」。就像你开车，运动学描述的是车速和方向盘转角的关系，至于发动机扭矩多大、轮胎抓地力如何，那是动力学的事。
核心公式：位置与速度的关系
ṗ = v （位置对时间的导数等于速度）
v̇ = a （速度对时间的导数等于加速度）嗯，这里要注意：在三维空间中，p 和 v 都是三维向量。我们通常把位置定义在地球坐标系（惯性系）中，而速度则要区分是体坐标系还是惯性系。我在项目中就吃过这个亏——把体坐标系下的速度直接当成惯性系速度去算位置，结果仿真出来的轨迹完全不对。
4.2 姿态描述：欧拉角与旋转矩阵
姿态描述是飞控里最绕的部分。常用的方法有三种：欧拉角、旋转矩阵、四元数。今天咱们先讲欧拉角，因为它最直观。
欧拉角就是三个角度：滚转角 φ（Roll）、俯仰角 θ（Pitch）、偏航角 ψ（Yaw）。你可以想象成：先绕 Z 轴转 ψ，再绕 Y 轴转 θ，最后绕 X 轴转 φ。这个顺序叫 ZYX 顺序，也是航空领域最常用的。
避坑指南：我曾经在代码里把欧拉角的旋转顺序搞反了，结果仿真出来的飞机像喝醉了酒一样乱转。后来花了整整两天才定位到问题。记住：旋转顺序不可交换，一定要和你的坐标系定义保持一致。
从体坐标系到惯性系的旋转矩阵 R 长这样：
R = Rz(ψ) * Ry(θ) * Rx(φ)
其中：
Rz(ψ) = [cosψ -sinψ 0; sinψ cosψ 0; 0 0 1]
Ry(θ) = [cosθ 0 sinθ; 0 1 0; -sinθ 0 cosθ]
Rx(φ) = [1 0 0; 0 cosφ -sinφ; 0 sinφ cosφ]有了旋转矩阵，我们就可以把体坐标系下的向量转换到惯性系。比如，机头方向在体坐标系中是 [1,0,0]ᵀ，在惯性系中就是 R * [1,0,0]ᵀ。
4.3 牛顿-欧拉方程：力与力矩的平衡
好，运动学讲完了，现在咱们来聊动力学。动力学回答的是「为什么动」——力产生加速度，力矩产生角加速度。
牛顿第二定律大家都会：F = m * a。但在三维空间中，力是矢量，加速度也是矢量。更关键的是，飞行器受到的力包括重力、推力、气动力，这些力作用在不同的方向上。
牛顿方程（平动动力学）
m * v̇_b = F_b - ω_b × (m * v_b)
其中：
v̇_b 是体坐标系下的加速度
F_b 是体坐标系下的合力
ω_b 是体坐标系下的角速度
× 表示叉乘为什么要减去 ω × (m v) 这一项？因为体坐标系本身在旋转。你想想看，如果飞机在滚转，即使外力不变，体坐标系下的速度方向也在变。这个叉乘项就是科里奥利力的体现。我在做高机动飞行仿真时，如果忽略这一项，仿真结果会明显偏离真实物理。
再看转动部分——欧拉方程：
J * ω̇_b = M_b - ω_b × (J * ω_b)
其中：
J 是惯性张量（3x3矩阵）
M_b 是体坐标系下的合力矩
ω̇_b 是角加速度惯性张量 J 描述了质量分布。对于四旋翼飞行器，由于结构对称，J 通常是对角矩阵：
J = [Ixx 0 0
0 Iyy 0
0 0 Izz]但如果是固定翼或者倾转旋翼机，非对角项就不能忽略了。我曾经帮一个团队调试倾转旋翼机的仿真模型，就是因为忽略了 J 的非对角项，导致横航向耦合完全不对。
4.4 六自由度模型：整合所有方程
现在我们把运动学和动力学拼在一起，得到完整的六自由度模型。这个模型包含 12 个状态量：
| 状态量 | 符号 | 维度 | 说明 |
|---|---|---|---|
| 位置 | p | 3×1 | 惯性系下的 x,y,z |
| 速度 | v | 3×1 | 体坐标系下的 u,v,w |
| 姿态 | Φ | 3×1 | 欧拉角 φ,θ,ψ |
| 角速度 | ω | 3×1 | 体坐标系下的 p,q,r |
完整的微分方程组如下：
ṗ = R * v_b （位置微分）
v̇_b = F_b/m - ω_b × v_b （速度微分）
Φ̇ = T * ω_b （姿态微分）
ω̇_b = J⁻¹ * (M_b - ω_b × J ω_b) （角速度微分）其中 T 是欧拉角速率到体角速度的转换矩阵：
T = [1 sinφ tanθ cosφ tanθ
0 cosφ -sinφ
0 sinφ/cosθ cosφ/cosθ]注意：当俯仰角 θ = ±90° 时，tanθ 和 1/cosθ 会趋于无穷大，这就是欧拉角的「万向锁」问题。在实际仿真中，如果飞行器要做垂直爬升或倒飞，建议改用四元数避免奇异。我在做特技飞行仿真时就吃过这个亏，仿真到一半直接崩了。
4.5 知识体系总览
为了让大家对本章内容有个整体把握，我画了一张图：
这张图把整个六自由度模型拆成了四个层次。从上往下看，六自由度模型由运动学和动力学两部分组成。运动学负责位置和姿态的微分关系，动力学负责速度和角速度的变化率。最底层是四个核心方程，它们共同构成了仿真引擎的数学基础。
4.6 在 MATLAB 中快速搭建
理论讲完了，咱们看看怎么在 MATLAB 里实现。我个人习惯用函数封装状态方程，这样便于调试和复用。
function dx = quadrotor_dynamics(t, x, u, params)
% 状态向量 x = [p; v; Phi; omega]
% 输入 u = [thrust; Mx; My; Mz]
% 解包状态
p = x(1:3); % 位置
v = x(4:6); % 体坐标系速度
Phi = x(7:9); % 欧拉角 [phi; theta; psi]
omega = x(10:12);% 体角速度 [p; q; r]
% 参数
m = params.m; % 质量
J = params.J; % 惯性张量
g = params.g; % 重力加速度
% 输入
thrust = u(1);
M = u(2:4);
% 旋转矩阵
phi = Phi(1); theta = Phi(2); psi = Phi(3);
R = rotation_matrix(phi, theta, psi);
% 重力在体坐标系下的分量
g_body = R' * [0; 0; -m*g];
% 合力（推力 + 重力）
F = [0; 0; thrust] + m * g_body;
% 牛顿方程
v_dot = F/m - cross(omega, v);
% 欧拉方程
omega_dot = J \ (M - cross(omega, J*omega));
% 位置微分
p_dot = R * v;
% 姿态微分
T = [1 sin(phi)*tan(theta) cos(phi)*tan(theta);
0 cos(phi) -sin(phi);
0 sin(phi)/cos(theta) cos(phi)/cos(theta)];
Phi_dot = T * omega;
% 组装输出
dx = [p_dot; v_dot; Phi_dot; omega_dot];
end这段代码虽然不长，但包含了六自由度模型的所有核心。你只需要提供初始状态和输入，用 ode45 或者 ode4（固定步长）就能跑起来。
小技巧：在调试阶段，我建议先用简单的悬停工况验证。给一个恒定的推力等于重力，力矩为零，看看飞机是不是能保持不动。如果位置和姿态都在漂，那肯定是方程写错了。我曾经用这个方法五分钟就找到了一个符号错误。
好了，今天的内容就到这儿。六自由度模型是飞控仿真的基石，后面的控制律设计、状态估计、轨迹规划全都建立在这个模型之上。希望大家回去后能亲手把这段代码跑一遍，看看不同输入下飞机的响应。动手做一遍，比看十遍书都管用。