📚 课程目录第1章建立飞行器六自由度非线性模型

1. 建立飞行器六自由度非线性模型

任务定义

建立某固定翼飞行器的六自由度非线性数学模型，包含刚体动力学（力方程与力矩方程）和运动学（位置方程与姿态方程），并给出标准大气环境下重力模型与简易气动系数模型。最终以MATLAB函数形式交付，输入为状态向量与控制向量，输出为状态导数，可直接用于后续数值积分仿真。

坐标系与状态定义

采用北-东-地（NED）惯性坐标系和机体坐标系。状态向量定义为：

X = [pn, pe, pd, u, v, w, phi, theta, psi, p, q, r]'

其中：pn, pe, pd为NED位置（m）；u, v, w为机体坐标系线速度（m/s）；phi, theta, psi为欧拉角（滚转、俯仰、偏航，rad）；p, q, r为机体角速度（rad/s）。控制向量为：

U = [delta_e, delta_a, delta_r, delta_t]'

依次为升降舵偏角（rad）、副翼偏角（rad）、方向舵偏角（rad）、油门（0~1无量纲）。

飞行器参数

本案例使用如下固定参数（可后续替换）：

m = 13.5;                % 质量 kg
g = 9.81;                % 重力加速度 m/s^2
S = 0.55;                % 机翼面积 m^2
b = 1.83;                % 翼展 m
cbar = 0.24;             % 平均气动弦长 m
rho = 1.225;             % 海平面空气密度 kg/m^3
Jx = 0.114; Jy = 0.132; Jz = 0.214; Jxz = 0.012;  % 惯量 kg·m^2

% 气动导数（简化线性模型）
C_L0 = 0.28; C_Lalpha = 4.21; C_Lq = 0.0; C_Lde = 0.33;
C_D0 = 0.03; C_Dalpha = 0.06; C_Dq = 0.0; C_Dde = 0.0;
C_Y0 = 0.0;  C_Ybeta = -0.48; C_Yp = 0.0; C_Yr = 0.0; C_Yda = 0.0; C_Ydr = 0.14;
C_l0 = 0.0;  C_lbeta = -0.092; C_lp = -0.42; C_lr = 0.0; C_lda = -0.18; C_ldr = 0.0;
C_m0 = 0.0;  C_malpha = -0.65; C_mq = -3.6; C_mde = -1.2;
C_n0 = 0.0;  C_nbeta = 0.052; C_np = 0.0; C_nr = -0.12; C_nda = 0.0; C_ndr = -0.062;

% 推力模型（简化：沿机体x轴）
T_max = 60;              % 最大推力 N

数学模型

力方程（机体坐标系）：


udot = r*v - q*w + (-m*g*sin(theta) + F_aero_x + T) / m;
vdot = p*w - r*u + ( m*g*cos(theta)*sin(phi) + F_aero_y) / m;
wdot = q*u - p*v + ( m*g*cos(theta)*cos(phi) + F_aero_z) / m;

力矩方程（机体坐标系）：


pdot = (Jxz*(Jx - Jy + Jz)*p*q + (Jy*Jz - Jz^2 - Jxz^2)*q*r + Jz*L + Jxz*N) / (Jx*Jz - Jxz^2);
qdot = ((Jz - Jx)*p*r - Jxz*(p^2 - r^2) + M) / Jy;
rdot = ((Jx^2 - Jx*Jy + Jxz^2)*p*q + Jxz*(Jy - Jx - Jz)*q*r + Jxz*L + Jx*N) / (Jx*Jz - Jxz^2);

位置运动学（NED）：


pn_dot = cos(theta)*cos(psi)*u + (sin(phi)*sin(theta)*cos(psi) - cos(phi)*sin(psi))*v + (cos(phi)*sin(theta)*cos(psi) + sin(phi)*sin(psi))*w;
pe_dot = cos(theta)*sin(psi)*u + (sin(phi)*sin(theta)*sin(psi) + cos(phi)*cos(psi))*v + (cos(phi)*sin(theta)*sin(psi) - sin(phi)*cos(psi))*w;
pd_dot = -sin(theta)*u + sin(phi)*cos(theta)*v + cos(phi)*cos(theta)*w;

姿态运动学：


phi_dot   = p + tan(theta)*(q*sin(phi) + r*cos(phi));
theta_dot = q*cos(phi) - r*sin(phi);
psi_dot   = (q*sin(phi) + r*cos(phi)) / cos(theta);

气动力与力矩计算：


V = sqrt(u^2 + v^2 + w^2);
alpha = atan2(w, u);
beta = asin(v / max(V, 1e-6));
qbar = 0.5 * rho * V^2;

C_L = C_L0 + C_Lalpha*alpha + C_Lq*cbar*q/(2*V) + C_Lde*delta_e;
C_D = C_D0 + C_Dalpha*alpha + C_Dq*cbar*q/(2*V) + C_Dde*delta_e;
C_Y = C_Y0 + C_Ybeta*beta + C_Yp*b*p/(2*V) + C_Yr*b*r/(2*V) + C_Yda*delta_a + C_Ydr*delta_r;

C_l = C_l0 + C_lbeta*beta + C_lp*b*p/(2*V) + C_lr*b*r/(2*V) + C_lda*delta_a + C_ldr*delta_r;
C_m = C_m0 + C_malpha*alpha + C_mq*cbar*q/(2*V) + C_mde*delta_e;
C_n = C_n0 + C_nbeta*beta + C_np*b*p/(2*V) + C_nr*b*r/(2*V) + C_nda*delta_a + C_ndr*delta_r;

F_aero_x = qbar*S*(-C_D*cos(alpha) + C_L*sin(alpha));
F_aero_y = qbar*S*C_Y;
F_aero_z = qbar*S*(-C_L*cos(alpha) - C_D*sin(alpha));

L = qbar*S*b*C_l;
M = qbar*S*cbar*C_m;
N = qbar*S*b*C_n;

T = delta_t * T_max;

MATLAB实现


function Xdot = fixedwing6dof(X, U, params)
    % 状态解包
    pn = X(1); pe = X(2); pd = X(3);
    u  = X(4); v  = X(5); w  = X(6);
    phi = X(7); theta = X(8); psi = X(9);
    p  = X(10); q  = X(11); r  = X(12);
    
    delta_e = U(1); delta_a = U(2); delta_r = U(3); delta_t = U(4);
    
    % 参数解包
    m = params.m; g = params.g; S = params.S; b = params.b; cbar = params.cbar;
    rho = params.rho; Jx = params.Jx; Jy = params.Jy; Jz = params.Jz; Jxz = params.Jxz;
    T_max = params.T_max;
    
    % 气动系数
    C_L0=params.C_L0; C_Lalpha=params.C_Lalpha; C_Lq=params.C_Lq; C_Lde=params.C_Lde;
    C_D0=params.C_D0; C_Dalpha=params.C_Dalpha; C_Dq=params.C_Dq; C_Dde=params.C_Dde;
    C_Y0=params.C_Y0; C_Ybeta=params.C_Ybeta; C_Yp=params.C_Yp; C_Yr=params.C_Yr;
    C_Yda=params.C_Yda; C_Ydr=params.C_Ydr;
    C_l0=params.C_l0; C_lbeta=params.C_lbeta; C_lp=params.C_lp; C_lr=params.C_lr;
    C_lda=params.C_lda; C_ldr=params.C_ldr;
    C_m0=params.C_m0; C_malpha=params.C_malpha; C_mq=params.C_mq; C_mde=params.C_mde;
    C_n0=params.C_n0; C_nbeta=params.C_nbeta; C_np=params.C_np; C_nr=params.C_nr;
    C_nda=params.C_nda; C_ndr=params.C_ndr;
    
    % 计算气动力与力矩
    V = sqrt(u^2 + v^2 + w^2);
    if V < 1e-6
        V = 1e-6;
    end
    alpha = atan2(w, u);
    beta = asin(v / V);
    qbar = 0.5 * rho * V^2;
    
    C_L = C_L0 + C_Lalpha*alpha + C_Lq*cbar*q/(2*V) + C_Lde*delta_e;
    C_D = C_D0 + C_Dalpha*alpha + C_Dq*cbar*q/(2*V) + C_Dde*delta_e;
    C_Y = C_Y0 + C_Ybeta*beta + C_Yp*b*p/(2*V) + C_Yr*b*r/(2*V) + C_Yda*delta_a + C_Ydr*delta_r;
    
    C_l = C_l0 + C_lbeta*beta + C_lp*b*p/(2*V) + C_lr*b*r/(2*V) + C_lda*delta_a + C_ldr*delta_r;
    C_m = C_m0 + C_malpha*alpha + C_mq*cbar*q/(2*V) + C_mde*delta_e;
    C_n = C_n0 + C_nbeta*beta + C_np*b*p/(2*V) + C_nr*b*r/(2*V) + C_nda*delta_a + C_ndr*delta_r;
    
    F_aero_x = qbar*S*(-C_D*cos(alpha) + C_L*sin(alpha));
    F_aero_y = qbar*S*C_Y;
    F_aero_z = qbar*S*(-C_L*cos(alpha) - C_D*sin(alpha));
    
    L = qbar*S*b*C_l;
    M = qbar*S*cbar*C_m;
    N = qbar*S*b*C_n;
    
    T = delta_t * T_max;
    
    % 力方程
    udot = r*v - q*w + (-m*g*sin(theta) + F_aero_x + T) / m;
    vdot = p*w - r*u + ( m*g*cos(theta)*sin(phi) + F_aero_y) / m;
    wdot = q*u - p*v + ( m*g*cos(theta)*cos(phi) + F_aero_z) / m;
    
    % 力矩方程
    denom = Jx*Jz - Jxz^2;
    pdot = (Jxz*(Jx - Jy + Jz)*p*q + (Jy*Jz - Jz^2 - Jxz^2)*q*r + Jz*L + Jxz*N) / denom;
    qdot = ((Jz - Jx)*p*r - Jxz*(p^2 - r^2) + M) / Jy;
    rdot = ((Jx^2 - Jx*Jy + Jxz^2)*p*q + Jxz*(Jy - Jx - Jz)*q*r + Jxz*L + Jx*N) / denom;
    
    % 位置运动学
    pn_dot = cos(theta)*cos(psi)*u + (sin(phi)*sin(theta)*cos(psi) - cos(phi)*sin(psi))*v + (cos(phi)*sin(theta)*cos(psi) + sin(phi)*sin(psi))*w;
    pe_dot = cos(theta)*sin(psi)*u + (sin(phi)*sin(theta)*sin(psi) + cos(phi)*cos(psi))*v + (cos(phi)*sin(theta)*sin(psi) - sin(phi)*cos(psi))*w;
    pd_dot = -sin(theta)*u + sin(phi)*cos(theta)*v + cos(phi)*cos(theta)*w;
    
    % 姿态运动学
    phi_dot   = p + tan(theta)*(q*sin(phi) + r*cos(phi));
    theta_dot = q*cos(phi) - r*sin(phi);
    psi_dot   = (q*sin(phi) + r*cos(phi)) / cos(theta);
    
    Xdot = [pn_dot; pe_dot; pd_dot; udot; vdot; wdot; phi_dot; theta_dot; psi_dot; pdot; qdot; rdot];
end

参数初始化脚本


% init_params.m
params.m = 13.5; params.g = 9.81; params.S = 0.55; params.b = 1.83; params.cbar = 0.24;
params.rho = 1.225; params.Jx = 0.114; params.Jy = 0.132; params.Jz = 0.214; params.Jxz = 0.012;
params.T_max = 60;
params.C_L0=0.28; params.C_Lalpha=4.21; params.C_Lq=0.0; params.C_Lde=0.33;
params.C_D0=0.03; params.C_Dalpha=0.06; params.C_Dq=0.0; params.C_Dde=0.0;
params.C_Y0=0.0; params.C_Ybeta=-0.48; params.C_Yp=0.0; params.C_Yr=0.0; params.C_Yda=0.0; params.C_Ydr=0.14;
params.C_l0=0.0; params.C_lbeta=-0.092; params.C_lp=-0.42; params.C_lr=0.0; params.C_lda=-0.18; params.C_ldr=0.0;
params.C_m0=0.0; params.C_malpha=-0.65; params.C_mq=-3.6; params.C_mde=-1.2;
params.C_n0=0.0; params.C_nbeta=0.052; params.C_np=0.0; params.C_nr=-0.12; params.C_nda=0.0; params.C_ndr=-0.062;

验证方法

在MATLAB中运行以下测试脚本，观察状态导数是否与手算预期一致：


% test_model.m
init_params;
% 水平直线飞行初始条件（近似配平）
V0 = 25; alpha0 = 0.05; theta0 = alpha0;
X0 = [0; 0; -100; V0*cos(alpha0); 0; V0*sin(alpha0); 0; theta0; 0; 0; 0; 0];
% 配平升降舵（近似）
delta_e0 = -0.02; delta_t0 = 0.3;
U0 = [delta_e0; 0; 0; delta_t0];
Xdot = fixedwing6dof(X0, U0, params);
disp('状态导数:');
disp(Xdot);
% 检查平飞条件：udot≈0, wdot≈0, theta_dot≈0
assert(abs(Xdot(4)) < 0.5, 'udot过大');
assert(abs(Xdot(6)) < 0.5, 'wdot过大');
assert(abs(Xdot(8)) < 0.01, 'theta_dot过大');
disp('验证通过：近似配平状态下线加速度与俯仰角速率接近零。');

判定标准：在给定近似配平状态下，udot与wdot值小于0.5 m/s²，theta_dot值小于0.01 rad/s，即认为模型力与力矩方程实现正确。若偏差较大，检查气动系数符号与推力方向。

使用说明

将fixedwing6dof.m与init_params.m置于同一目录，在MATLAB中先运行init_params生成参数结构体，再调用fixedwing6dof(X0, U0, params)即可获得状态导数。该函数可直接作为ode45等积分器的微分方程函数，用于后续配平、线性化与仿真任务。注意模型在俯仰角接近±90°时姿态运动学存在奇异，本案例仅适用于常规飞行包线内。