1. 建立固定翼飞机六自由度非线性模型

建立固定翼飞机六自由度非线性模型 学习卡片

任务定义

本案例的目标是根据给定的固定翼飞机气动数据，在Simulink中建立包含刚体六自由度运动方程的仿真模型。模型输入为舵面偏转角（升降舵、副翼、方向舵）和油门位置，输出为飞机的速度、姿态角、角速度和位置等状态量，为后续控制律设计提供被控对象模型。

气动数据与飞机参数

采用简化的短周期/长周期解耦气动模型，飞机参数与气动导数如下表所示（单位制：国际单位制，角度均以弧度计）：

参数符号数值参数符号数值
质量m1200 kg重力加速度g9.81 m/s²
机翼面积S16.2 m²平均气动弦长c̄1.5 m
翼展b10.2 m动压q̄0.5ρV²
空气密度ρ1.225 kg/m³惯性矩 Ixx1280 kg·m²
惯性矩 Iyy1820 kg·m²惯性矩 Izz2660 kg·m²
惯性积 Ixz0 kg·m²

气动导数（均为无量纲，α、β以弧度计）：

纵向导数数值横航向导数数值
CL00.28CYβ-0.56
CLα5.2CYp-0.12
CLq3.9CYr0.24
CLδe0.35CYδa0
CD00.03CYδr0.17
CDα0.21Clβ-0.072
CDq0Clp-0.41
CDδe0Clr0.11
Cm00.02Clδa0.19
Cmα-0.68Clδr0.015
Cmq-8.6Cnβ0.058
Cmδe-0.62Cnp-0.032
Cnr-0.11
Cnδa-0.012
Cnδr-0.072

推力模型：T = δth · Tmax，其中 Tmax = 3200 N，δth ∈ [0,1]。

六自由度刚体运动方程

采用机体坐标系下的十二状态方程。状态向量定义为：
位置（北东地坐标系）：pN, pE, pD；
速度（机体坐标系）：u, v, w；
姿态角（欧拉角）：φ, θ, ψ；
角速度（机体坐标系）：p, q, r。

动力学方程（机体坐标系）：

\[ \dot{u} = rv - qw + \frac{1}{m}(F_x + T_x) \]
\[ \dot{v} = pw - ru + \frac{1}{m}F_y \]
\[ \dot{w} = qu - pv + \frac{1}{m}F_z \]
\[ \dot{p} = \frac{I_{zz}L + I_{xz}N - (I_{xz}(I_{yy}-I_{xx}-I_{zz}))pq + (I_{zz}(I_{zz}-I_{yy})+I_{xz}^2)qr}{I_{xx}I_{zz}-I_{xz}^2} \]
\[ \dot{q} = \frac{M + (I_{zz}-I_{xx})pr + I_{xz}(r^2-p^2)}{I_{yy}} \]
\[ \dot{r} = \frac{I_{xz}L + I_{xx}N + (I_{xx}(I_{xx}-I_{yy})+I_{xz}^2)pq - I_{xz}(I_{xx}-I_{yy}+I_{zz})qr}{I_{xx}I_{zz}-I_{xz}^2} \]

运动学方程（采用四元数避免奇异性，此处为便于工程实现给出欧拉角速率形式，适用于非±90°俯仰角范围）：

\[ \dot{\phi} = p + q\sin\phi\tan\theta + r\cos\phi\tan\theta \]
\[ \dot{\theta} = q\cos\phi - r\sin\phi \]
\[ \dot{\psi} = \frac{q\sin\phi + r\cos\phi}{\cos\theta} \]
\[ \dot{p}_N = u\cos\theta\cos\psi + v(\sin\phi\sin\theta\cos\psi - \cos\phi\sin\psi) + w(\cos\phi\sin\theta\cos\psi + \sin\phi\sin\psi) \]
\[ \dot{p}_E = u\cos\theta\sin\psi + v(\sin\phi\sin\theta\sin\psi + \cos\phi\cos\psi) + w(\cos\phi\sin\theta\sin\psi - \sin\phi\cos\psi) \]
\[ \dot{p}_D = -u\sin\theta + v\sin\phi\cos\theta + w\cos\phi\cos\theta \]

气动力与力矩计算

迎角与侧滑角由速度分量计算：α = atan2(w, u)，β = asin(v/V)，其中 V = √(u²+v²+w²)。
动压 q̄ = 0.5ρV²。

气动力系数：

\[ C_L = C_{L0} + C_{Lα}α + C_{Lq}\frac{q\bar{c}}{2V} + C_{Lδe}δ_e \]
\[ C_D = C_{D0} + C_{Dα}α + C_{Dq}\frac{q\bar{c}}{2V} + C_{Dδe}δ_e \]
\[ C_Y = C_{Yβ}β + C_{Yp}\frac{pb}{2V} + C_{Yr}\frac{rb}{2V} + C_{Yδa}δ_a + C_{Yδr}δ_r \]

气动力矩系数：

\[ C_l = C_{lβ}β + C_{lp}\frac{pb}{2V} + C_{lr}\frac{rb}{2V} + C_{lδa}δ_a + C_{lδr}δ_r \]
\[ C_m = C_{m0} + C_{mα}α + C_{mq}\frac{q\bar{c}}{2V} + C_{mδe}δ_e \]
\[ C_n = C_{nβ}β + C_{np}\frac{pb}{2V} + C_{nr}\frac{rb}{2V} + C_{nδa}δ_a + C_{nδr}δ_r \]

气动力与力矩（机体坐标系，升力方向为 -zbody，阻力方向为 -xbody，侧力沿 +ybody）：

\[ F_x^{aero} = -q̄S C_D \cosα + q̄S C_L \sinα \]
\[ F_z^{aero} = -q̄S C_D \sinα - q̄S C_L \cosα \]
\[ F_y^{aero} = q̄S C_Y \]
\[ L = q̄Sb C_l, \quad M = q̄S\bar{c} C_m, \quad N = q̄Sb C_n \]

重力在机体坐标系的分量：

\[ F_x^{grav} = -mg\sinθ \]
\[ F_y^{grav} = mg\sinφ\cosθ \]
\[ F_z^{grav} = mg\cosφ\cosθ \]

Simulink模型实现

模型采用MATLAB Function模块实现核心微分方程，整体结构如下图所示。模型文件名为 fixed_wing_6dof.slx。

输入 δe (升降舵) δa (副翼) δr (方向舵) δth (油门) 4×1 向量 气动系数与力/力矩 CL, CD, CY Cl, Cm, Cn → F_aero, M_aero 重力分量 F_grav(φ,θ) 推力模型 T = δth · Tmax 六自由度运动方程 状态: [u v w p q r φ θ ψ pN pE pD] dX/dt = f(X, F, M) 含四元数/欧拉角转换 积分器: 12维状态 初始条件: 配平状态 输出 V, α, β φ, θ, ψ p, q, r pN, pE, pD 用于控制律 设计反馈 δe,δa,δr δth 状态反馈 (V, α, β, p, q, r) 固定翼六自由度非线性Simulink模型结构

模型内部使用一个MATLAB Function模块（名为 six_dof_dynamics）实现上述全部微分方程与气动计算。该模块的输入为：u_vec（4×1控制输入）、X（12×1状态向量）、param（结构体，包含飞机参数与气动导数）。输出为状态导数 dX（12×1）。

MATLAB Function核心代码

在Simulink模型中，双击MATLAB Function模块，输入以下代码。该代码完整实现了前述气动模型与运动方程：

function dX = six_dof_dynamics(u_vec, X, param)
% u_vec: [delta_e; delta_a; delta_r; delta_th]
% X: [u; v; w; p; q; r; phi; theta; psi; pN; pE; pD]
% param: 结构体，包含飞机参数与气动导数

% 解析控制输入
de = u_vec(1);  % 升降舵 (rad)
da = u_vec(2);  % 副翼 (rad)
dr = u_vec(3);  % 方向舵 (rad)
dth = u_vec(4); % 油门 [0,1]

% 解析状态
u = X(1); v = X(2); w = X(3);
p = X(4); q = X(5); r = X(6);
phi = X(7); theta = X(8); psi = X(9);
pN = X(10); pE = X(11); pD = X(12);

% 飞机参数
m = param.m; g = param.g;
S = param.S; b = param.b; cbar = param.cbar;
rho = param.rho;
Ixx = param.Ixx; Iyy = param.Iyy; Izz = param.Izz; Ixz = param.Ixz;
Tmax = param.Tmax;

% 气动导数
CL0=param.CL0; CLa=param.CLa; CLq=param.CLq; CLde=param.CLde;
CD0=param.CD0; CDa=param.CDa; CDq=param.CDq; CDde=param.CDde;
Cm0=param.Cm0; Cma=param.Cma; Cmq=param.Cmq; Cmde=param.Cmde;
CYb=param.CYb; CYp=param.CYp; CYr=param.CYr; CYda=param.CYda; CYdr=param.CYdr;
Clb=param.Clb; Clp=param.Clp; Clr=param.Clr; Clda=param.Clda; Cldr=param.Cldr;
Cnb=param.Cnb; Cnp=param.Cnp; Cnr=param.Cnr; Cnda=param.Cnda; Cndr=param.Cndr;

% 速度与气动角
V = sqrt(u^2 + v^2 + w^2);
alpha = atan2(w, u);
beta = asin(v / V);

% 动压
qbar = 0.5 * rho * V^2;

% 无量纲角速度
if V > 1e-6
    p_hat = p * b / (2 * V);
    q_hat = q * cbar / (2 * V);
    r_hat = r * b / (2 * V);
else
    p_hat = 0; q_hat = 0; r_hat = 0;
end

% 气动力系数
CL = CL0 + CLa*alpha + CLq*q_hat + CLde*de;
CD = CD0 + CDa*alpha + CDq*q_hat + CDde*de;
CY = CYb*beta + CYp*p_hat + CYr*r_hat + CYda*da + CYdr*dr;

% 气动力矩系数
Cl = Clb*beta + Clp*p_hat + Clr*r_hat + Clda*da + Cldr*dr;
Cm = Cm0 + Cma*alpha + Cmq*q_hat + Cmde*de;
Cn = Cnb*beta + Cnp*p_hat + Cnr*r_hat + Cnda*da + Cndr*dr;

% 气动力（机体坐标系）
Fx_aero = -qbar*S*CD*cos(alpha) + qbar*S*CL*sin(alpha);
Fz_aero = -qbar*S*CD*sin(alpha) - qbar*S*CL*cos(alpha);
Fy_aero = qbar*S*CY;

% 气动力矩
L_aero = qbar*S*b*Cl;
M_aero = qbar*S*cbar*Cm;
N_aero = qbar*S*b*Cn;

% 重力（机体坐标系）
Fx_grav = -m*g*sin(theta);
Fy_grav = m*g*sin(phi)*cos(theta);
Fz_grav = m*g*cos(phi)*cos(theta);

% 推力（沿机体x轴）
Fx_thrust = dth * Tmax;

% 合力与合力矩
Fx = Fx_aero + Fx_grav + Fx_thrust;
Fy = Fy_aero + Fy_grav;
Fz = Fz_aero + Fz_grav;
L = L_aero;
M = M_aero;
N = N_aero;

% 线速度导数
du = r*v - q*w + Fx/m;
dv = p*w - r*u + Fy/m;
dw = q*u - p*v + Fz/m;

% 角速度导数（Ixz=0简化）
dp = (L + (Iyy - Izz)*q*r) / Ixx;
dq = (M + (Izz - Ixx)*p*r) / Iyy;
dr_ang = (N + (Ixx - Iyy)*p*q) / Izz;

% 欧拉角速率
if abs(cos(theta)) < 1e-6
    cos(theta); % 接近奇异，实际飞行中应使用四元数
end
dphi = p + q*sin(phi)*tan(theta) + r*cos(phi)*tan(theta);
dtheta = q*cos(phi) - r*sin(phi);
dpsi = (q*sin(phi) + r*cos(phi)) / cos(theta);

% 位置导数（北东地坐标系）
dpN = u*cos(theta)*cos(psi) + v*(sin(phi)*sin(theta)*cos(psi) - cos(phi)*sin(psi)) + ...
      w*(cos(phi)*sin(theta)*cos(psi) + sin(phi)*sin(psi));
dpE = u*cos(theta)*sin(psi) + v*(sin(phi)*sin(theta)*sin(psi) + cos(phi)*cos(psi)) + ...
      w*(cos(phi)*sin(theta)*sin(psi) - sin(phi)*cos(psi));
dpD = -u*sin(theta) + v*sin(phi)*cos(theta) + w*cos(phi)*cos(theta);

% 组装输出
dX = [du; dv; dw; dp; dq; dr_ang; dphi; dtheta; dpsi; dpN; dpE; dpD];
end

模型参数初始化脚本

在Simulink模型回调或单独脚本 init_6dof.m 中定义参数结构体与初始状态：

%% init_6dof.m - 初始化固定翼六自由度模型参数
param.m = 1200;          % 质量 kg
param.g = 9.81;          % 重力加速度 m/s^2
param.S = 16.2;          % 机翼面积 m^2
param.b = 10.2;          % 翼展 m
param.cbar = 1.5;        % 平均气动弦长 m
param.rho = 1.225;       % 空气密度 kg/m^3
param.Ixx = 1280;        % kg·m^2
param.Iyy = 1820;
param.Izz = 2660;
param.Ixz = 0;
param.Tmax = 3200;       % 最大推力 N

% 纵向气动导数
param.CL0 = 0.28;   param.CLa = 5.2;    param.CLq = 3.9;    param.CLde = 0.35;
param.CD0 = 0.03;   param.CDa = 0.21;   param.CDq = 0;      param.CDde = 0;
param.Cm0 = 0.02;   param.Cma = -0.68;  param.Cmq = -8.6;   param.Cmde = -0.62;

% 横航向气动导数
param.CYb = -0.56;  param.CYp = -0.12;  param.CYr = 0.24;
param.CYda = 0;     param.CYdr = 0.17;
param.Clb = -0.072; param.Clp = -0.41;  param.Clr = 0.11;
param.Clda = 0.19;  param.Cldr = 0.015;
param.Cnb = 0.058;  param.Cnp = -0.032; param.Cnr = -0.11;
param.Cnda = -0.012; param.Cndr = -0.072;

% 初始配平状态（平飞，速度50 m/s，高度1000m）
V0 = 50; alpha0 = 0.05; theta0 = alpha0;  % 小迎角近似
u0 = V0*cos(alpha0); w0 = V0*sin(alpha0);
X0 = [u0; 0; w0; 0; 0; 0; 0; theta0; 0; 0; 0; -1000];

% 控制输入初始值（配平升降舵）
de0 = -(param.Cm0 + param.Cma*alpha0)/param.Cmde;
u0_vec = [de0; 0; 0; 0.3];  % 油门30%

Simulink模型搭建步骤

按以下步骤在Simulink中搭建模型：

  1. 新建模型，命名为 fixed_wing_6dof.slx。
  2. 添加 Constant 模块（4×1向量，值设为 u0_vec），作为控制输入。
  3. 添加 Integrator 模块，初始条件设为 X0（12×1向量），输出为状态向量。
  4. 添加 MATLAB Function 模块，输入连接 Constant 输出和 Integrator 输出，代码粘贴前述函数。
  5. 将 MATLAB Function 的输出连接到 Integrator 的输入，形成闭环。
  6. 添加 Demux 模块将状态向量分解，再通过 Bus Creator 或 Mux 输出所需信号（V、α、β、姿态角等）。
  7. 在模型回调（Model Properties → Callbacks → InitFcn）中运行 init_6dof 脚本，确保参数在工作区中可用。

验证方法与判定标准

完成模型搭建后，按以下步骤验证模型正确性：

  1. 配平验证：设置控制输入为 u0_vec，初始状态为 X0，仿真时间30秒。观察速度V、高度pD和俯仰角θ是否保持近似恒定。若模型正确，V的变化应小于0.5 m/s，高度变化应小于10 m，θ变化应小于0.01 rad。这是开环配平状态的合理性检验。
  2. 纵向扰动响应：在t=5s时给升降舵一个阶跃增量 Δδe = -0.02 rad（升降舵上偏），持续2秒后归零。观察俯仰角速率q和迎角α的响应：应呈现典型的短周期振荡（周期约1~3秒），随后趋于稳定或发散（取决于飞机静稳定性）。若响应发散极快（如1秒内数值超过10⁴），说明模型存在符号错误或积分步长问题。
  3. 横航向扰动响应：在t=5s时给方向舵阶跃 Δδr = 0.05 rad，持续3秒。观察滚转角φ和偏航角ψ的变化，应呈现荷兰滚模态特征（滚转与偏航耦合振荡）。
  4. 能量守恒检查：在无控制输入且初始状态为配平状态时，总能量（动能+势能）的变化率应等于推力功率减去阻力功率。可在模型中添加计算模块验证，误差应在5%以内。

判定标准：上述验证中，若配平状态能保持稳定（V漂移小于1 m/s，高度漂移小于20 m），且扰动响应呈现符合飞机气动特性的物理行为（而非数值发散或明显违背物理规律），则判定模型建立正确，可用于后续控制律设计。

← 上一章 📖 返回目录 下一章 →