%---------飞机飞行状态4的配平攻角、升降舵偏角、油门开度---------% Result_alpha = resultalfaelevator(8,1); Result_elevator = resultalfaelevator(8,2); Result_Throtte = resultalfaelevator(8,3);
r2dalpha = Result_alpha; %配平攻角 alpha=r2dalpha/r2d;
delta_e = Result_elevator; %配平的舵偏角 Throtte = Result_Throtte; %配平的油门值
%---------配平时的侧滑角为零，副翼和方向舵均为零，姿态角速度为零---------% beta = 0; r2dbeta=0;
delta_a=0; delta_r=0; p=0; q=0; r=0;
%%--------------计算配平基础上的小扰动的气动导数-------------%% %---------计算阻尼导数----------%
Cx_q = interp1(given_Alpha,given_Cxq,r2dalpha,'spline'); Cy_r = interp1(given_Alpha,given_Cyr,r2dalpha,'spline'); Cy_p = interp1(given_Alpha,given_Cyp,r2dalpha,'spline'); Cz_q = interp1(given_Alpha,given_Czq,r2dalpha,'spline'); Cl_r = interp1(given_Alpha,given_Clr,r2dalpha,'spline'); Cl_p = interp1(given_Alpha,given_Clp,r2dalpha,'spline'); Cm_q = interp1(given_Alpha,given_Cmq,r2dalpha,'spline'); Cn_r = interp1(given_Alpha,given_Cnr,r2dalpha,'spline'); Cn_p = interp1(given_Alpha,given_Cnp,r2dalpha,'spline');
%---------计算对舵偏角的导数----------%
Cx_delta_e=(interp2(given_Elevator,given_Alpha,given_Cx,delta_e+0.0001,r2dalpha,'spline')-interp2(given_Elevator,given_Alpha,given_Cx,delta_e-0.0001,r2dalpha,'spline'))/0.0002;
Cm_delta_e=(interp2(given_Elevator,given_Alpha,given_Cm,delta_e+0.0001,r2dalpha,'spline')-interp2(given_Elevator,given_Alpha,given_Cm,delta_e-0.0001,r2dalpha,'spline'))/0.0002;
Cl_delta_a=interp2(given_delta_beta,given_Alpha,given_Cl_delta_a,r2dbeta,r2dalpha,'spline'); Cl_delta_r=interp2(given_delta_beta,given_Alpha,given_Cl_delta_r,r2dbeta,r2dalpha,'spline'); Cn_delta_a=interp2(given_delta_beta,given_Alpha,given_Cn_delta_a,r2dbeta,r2dalpha,'spline'); Cn_delta_r=interp2(given_delta_beta,given_Alpha,given_Cn_delta_r,r2dbeta,r2dalpha,'spline');
%---------计算对攻角和侧滑角的导数----------%
Cx0=interp2(given_Elevator,given_Alpha,given_Cx,delta_e,r2dalpha,'spline');
Cz0=interp1(given_Alpha,given_Cz,r2dalpha,'spline')*(1-beta^2)-0.19*(delta_e/25.0); Cm0=interp2(given_Elevator,given_Alpha,given_Cm,delta_e,r2dalpha,'spline');
Cx_a=(interp2(given_Elevator,given_Alpha,given_Cx,delta_e,r2dalpha+0.0001,'spline')-interp2(given_Elevator,given_Alpha,given_Cx,delta_e,r2dalpha-0.0001,'spline'))/0.0002;
Cz_a=(interp1(given_Alpha,given_Cz,r2dalpha+0.0001,'spline')-interp1(given_Alpha,given_Cz,r2dalpha-0.0001,'spline'))/0.0002;
Cm_a=(interp2(given_Elevator,given_Alpha,given_Cm,delta_e,r2dalpha+0.0001,'spline')-interp2(given_Elevator,given_Alpha,given_Cm,delta_e,r2dalpha-0.0001,'spline'))/0.0002; Cl_b = (sign(r2dbeta+0.0001)*interp2(given_beta,given_Alpha,given_Cl,abs(r2dbeta+0.0001),r2dalpha,'spline')-sign(r2dbeta-0.0001)*interp2(given_beta,given_Alpha,given_Cl,abs(r2dbeta-0.0001),r2dalpha,'spline'))/0.0002; Cn_b = (sign(r2dbeta+0.001)*interp2(given_beta,given_Alpha,given_Cn,abs(r2dbeta+0.001),r2dalpha,'spline')-(sign(r2dbeta-0.001)*interp2(given_beta,given_Alpha,given_Cn,abs(r2dbeta-0.001),r2dalpha,'spline')))/0.0002;
%---------计算轴向力对油门开度的导数----------%
Interp_throtte_000=interp2(given_height,given_mah,given_throtte_000,H/0.3048,Mah,'spline')*(14.5939*0.3048); Interp_throtte_077=interp2(given_height,given_mah,given_throtte_077,H/0.3048,Mah,'spline')*(14.5939*0.3048); Interp_throtte_100=interp2(given_height,given_mah,given_throtte_100,H/0.3048,Mah,'spline')*(14.5939*0.3048); matrix_interp=[Interp_throtte_000 Interp_throtte_077 Interp_throtte_100]; throtte_value=[0 0.77 1];
Cx_delta_T = ((interp1(throtte_value,matrix_interp,Throtte+0.0001))-(interp1(throtte_value,matrix_interp,Throtte-0.0001)))/0.0002;
%------------计算轴向气动力X的气动导数-------------------% Cw = m*g/(0.5*rou*V1^2*S); Xu = -rou*V1*S*Cw*sin(alpha); Xw = 0.5*rou*V1*S*Cx_a*r2d; Xq = 0.25*rou*V1*S*c_*Cx_q;
XdeltaE = 0.5*rou*V1^2*S*Cx_delta_e; XdeltaT = Cx_delta_T;
%------------计算法向气动力Z的气动导数-------------------% Zu = -rou*S*V1*Cw*cos(alpha); Zw = 0.5*rou*V1*S*Cz_a*r2d; Zq = 0.25*rou*V1*S*c_*Cz_q; ZdeltaT = 0;
ZdeltaE = 0.5*rou*V1^2*S*(-0.19/25);
%------------计算俯仰力矩M的气动导数-------------------% Mu = rou*V1*S*c_*Cm0;
Mw = 0.5*rou*V1*S*c_*Cm_a*r2d; Mq = 0.25*rou*V1*S*c_^2*Cm_q; MdeltaT = 0;
MdeltaE = 0.5*rou*V1^2*S*c_*Cm_delta_e;
%------------计算侧向力Y的气动导数-------------------% Yv = 0.5*rou*V1*S*(-0.02)*r2d; Yp = 0.25*rou*V1*S*b*Cy_p; Yr = 0.25*rou*V1*S*b*Cy_r;
YdeltaA = 0.5*rou*V1^2*S*0.021/20; YdeltaR = 0.5*rou*V1^2*S*0.086/30;
%------------计算滚转力矩L的气动导数-------------------% Lv = 0.5*rou*V1*S*b*Cl_b*r2d; Lp = 0.25*rou*V1*S*b^2*Cl_p; Lr = 0.25*rou*V1*S*b^2*Cl_r;
LdeltaA = 0.5*rou*V1^2*S*b*Cl_delta_a; LdeltaR = 0.5*rou*V1^2*S*b*Cl_delta_r;
%------------计算偏航力矩N的气动导数-------------------% Nv = 0.5*rou*V1*S*b*Cn_b*r2d; Np = 0.25*rou*V1*S*b^2*Cn_p; Nr = 0.25*rou*V1*S*b^2*Cn_r;
NdeltaA = 0.5*rou*V1^2*S*b*Cn_delta_a; NdeltaR = 0.5*rou*V1^2*S*b*Cn_delta_r;
5.2.2 求小扰动线性化方程
%%---------计算纵向小扰动线性化方程矩阵的各元素----------%%
ZA11 = Xu/m; ZA12 = Xw/m; ZA13 = Xq/m; ZA14 = -g*cos(alpha); ZA21 = Zu/m; ZA22 = Zw/m; ZA23 = Zq/m+V1; ZA24 = -g*sin(alpha); ZA31 = Mu/Iy; ZA32 = Mw/Iy; ZA33 = Mq/Iy; ZA34 = 0; ZA41 = 0; ZA42 = 0; ZA43 = 1; ZA44 = 0;
ZB11 = XdeltaT/m; ZB12 = XdeltaE/m; ZB21 = ZdeltaT/m; ZB22 = ZdeltaE/m; ZB31 = MdeltaT/Iy; ZB32 = MdeltaE/Iy; ZB41 = 0; ZB42 = 0;
ZA =[ ZA11 ZA12 ZA13 ZA14; ZA21 ZA22 ZA23 ZA24; ZA31 ZA32 ZA33 ZA34; ZA41 ZA42 ZA43 ZA44]
ZB=[ZB11 ZB12; ZB21 ZB22; ZB31 ZB32; ZB41 ZB42]
%%---------计算横侧向小扰动线性化方程矩阵的各元素----------%%
HA11 = Yv/m; HA12 = Yp/m; HA13 = Yr/m-V1; HA21 = (Iz*Lv+Ixz*Nv)/(Ix*Iz-Ixz^2); HA22 = (Iz*Lp+Ixz*Np)/(Ix*Iz-Ixz^2); HA23 = (Iz*Lr+Ixz*Nr)/(Ix*Iz-Ixz^2); HA24 = 0; HA31 = (Ixz*Lv+Ix*Nv)/(Ix*Iz-Ixz^2); HA32 = (Ixz*Lp+Ix*Np)/(Ix*Iz-Ixz^2); HA33 = (Ixz*Lr+Ix*Nr)/(Ix*Iz-Ixz^2); HA34 = 0;
HA41 = 0; HA42 = 1; HA43 = tan(alpha); HA14 = g*cos(alpha); HA44 = 0;
HB11 = YdeltaA/m; HB12 = YdeltaR/m;
HB21 = (Iz*LdeltaA+Ixz*NdeltaA)/(Ix*Iz-Ixz^2); HB22 = (Iz*LdeltaR+Ixz*NdeltaR)/(Ix*Iz-Ixz^2); HB31 = (Ixz*LdeltaA+Ix*NdeltaA)/(Ix*Iz-Ixz^2); HB32 = (Ixz*LdeltaR+Ix*NdeltaR)/(Ix*Iz-Ixz^2); HB41 = 0; HB42 = 0;
HA=[HA11 HA12 HA13 HA14; HA21 HA22 HA23 HA24; HA31 HA32 HA33 HA34; HA41 HA42 HA43 HA44] HB=[HB11 HB12; HB21 HB22; HB31 HB32; HB41 HB42]
%%---------计算小扰动线性化方程的特征值和特征矩阵----------%% [z1,lambdaz1]=eig(ZA) [h1,lambdah1]=eig(HA)
5.3 全量方程求时域响应
全量方程示意图如下所示：
5.3.1 计算力和力矩子函数
function [X, Y, Z, L, M, N] = fcn(delta_e, delta_r, delta_a, delta_T, state)
you = state(1); v = state(2); w = state(3); p = state(4); q = state(5); r = state(6); fai = state(7); theta = state(8); psi = state(9); ze = state(12);
H = 1000 - ze;
V = sqrt(you^2 + v^2 + w^2);
%地球半径% R = 6356766;
%-------飞机的常数（国际单位制）---------%
c_= 3.4503;S = 27.8709;b = 9.144;r2d=57.29577951;
Vv = sqrt(you^2+v^2+w^2);
alpha = atan(w/you);
beta = asin(v/Vv);
r2dalpha=r2d*alpha; r2dbeta=r2d*beta;
%-------飞机的惯性数据（国际单位制）---------%
Ix = 12874.8446; Iy = 75673.6077; Iz = 85552.0953; Ixz = 1331.4130; Isum=Ix*Iz-Ixz^2;
%气动参数
given_Alpha = [-10 -5 0 5 10 15 20 25 30 35 40 45]; given_Elevator = [-24 -12 0 12 24];
given_Cz =[0.7700 0.2410 -0.1000 -0.4160 -0.7310 -1.0530 -1.3660 -1.6460 -1.9170 -2.1200 -2.2480 -2.2290]; given_Cx = [-0.099 -0.048 -0.022 -0.04 -0.083 -0.081 -0.038 -0.02 -0.038 -0.073 -0.081 -0.04 -0.021 -0.039 -0.076 -0.063 -0.021 -0.004 -0.025 -0.072 -0.025 0.016 0.032 0.006 -0.046 0.044 0.083 0.094 0.062 0.012 0.097 0.127 0.128 0.087 0.024 0.113 0.137 0.13 0.085 0.025 0.145 0.162 0.154 0.1 0.043 0.167 0.177 0.161 0.11 0.053 0.174 0.179 0.155 0.104 0.047 0.166 0.167 0.138 0.091 0.04]; given_Cm = [0.205 0.081 -0.046 -0.174 -0.259 0.168 0.077 -0.02 -0.145 -0.202 0.186 0.107 -0.009 -0.121 -0.184 0.196 0.11 -0.005 -0.127 -0.193 0.213 0.11 -0.006 -0.129 -0.199 0.251 0.141 0.01 -0.102 -0.15 0.245 0.127 0.006 -0.097 -0.16 0.238 0.119 -0.001 -0.113 -0.167 0.252 0.133 0.014 -0.087 -0.104 0.231 0.108 0 -0.084 -0.076 0.198 0.081 -0.013 -0.069 -0.041 0.192 0.093 0.032 -0.006 -0.005]; given_Cl = [0 -0.001 -0.003 -0.001 0 0.07 0.009 0 -0.004 -0.009 -0.01 -0.01 -0.01 -0.011 0 -0.008 -0.017 -0.02 -0.022 -0.023 -0.023 0 -0.012 -0.024 -0.03 -0.034 -0.034 -0.037 0 -0.016 -0.03 -0.039 -0.047 -0.049 -0.05 0 -0.019 -0.034 -0.044 -0.046 -0.046 -0.047 0 -0.02 -0.04 -0.05 -0.059 -0.068 -0.074 0 -0.02 -0.037 -0.049 -0.061 -0.071 -0.079 0 -0.015 -0.016 -0.023 -0.033 -0.06 -0.091 0 -0.008 -0.002 -0.006 -0.036 -0.058 -0.076 0 -0.013 -0.1 -0.014 -0.035 -0.062 -0.077 0 -0.015 -0.19 -0.027 -0.035 -0.059 -0.076]; given_Cn = [0 0.018 0.038 0.056 0.064 0.074 0.079 0 0.019 0.042 0.057 0.077 0.086 0.09 0 0.018 0.042 0.059 0.076 0.093 0.106 0 0.019 0.042 0.058 0.074 0.089 0.106 0 0.019 0.043 0.058 0.073 0.08 0.096 0 0.018 0.039 0.053 0.057 0.062 0.08 0 0.013 0.03 0.032 0.029 0.049 0.068 0 0.007 0.017 0.012 0.007 0.022 0.03 0 0.004 0.004 0.002 0.012 0.028 0.064 0 -0.014 -0.035 -0.046 -0.034 -0.012 0.015 0 -0.017 -0.047 -0.071 -0.065 -0.002 0.011 0 -0.033 -0.057 -0.073 -0.041 -0.013 -0.001];
given_Cl_delta_a = [-0.041 -0.041 -0.042 -0.04 -0.043 -0.044 -0.043 -0.052 -0.053 -0.053 -0.052 -0.049 -0.048 -0.049 -0.053 -0.053 -0.052 -0.051 -0.048 -0.048 -0.047 -0.056 -0.053 -0.051 -0.052 -0.049 -0.047 -0.045 -0.05 -0.05 -0.049 -0.048 -0.043 -0.042 -0.042 -0.056 -0.051 -0.049 -0.048 -0.042 -0.041 -0.037 -0.082 -0.066 -0.043 -0.042 -0.042 -0.02 -0.003 -0.059 -0.043 -0.035 -0.037 -0.036 -0.028 -0.013 -0.042 -0.038 -0.026 -0.031 -0.025 -0.013 -0.01 -0.038 -0.027 -0.016 -0.026 -0.021 -0.014 -0.003 -0.027 -0.023 -0.018 -0.017 -0.016 -0.011 -0.007
-0.017 -0.016 -0.014 -0.012 -0.011 -0.01 -0.008];
given_Cl_delta_r = [0.005 0.007 0.013 0.018 0.015 0.021 0.023 0.017 0.016 0.013 0.015 0.014 0.011 0.01 0.014 0.014 0.011 0.015 0.013 0.01 0.011 0.01 0.014 0.012 0.014 0.013 0.011 0.011 -0.005 0.013 0.011 0.014 0.012 0.01 0.011 0.009 0.009 0.009 0.014 0.011 0.009 0.01 0.019 0.012 0.008 0.014 0.011 0.008 0.008 0.005 0.005 0.005 0.015 0.01 0.01 0.01 0 0 -0.002 0.013 0.008 0.006 0.006 -0.005 0.004 0.005 0.011 0.008 0.005 0.014 -0.011 0.009 0.003 0.006 0.007 0 0.02 0.008 0.007 0.005 0.001 0.003 0.001 0 ]; given_Cn_delta_a = [0.001 0.002 -0.006 -0.011 -0.015 -0.024 -0.022 -0.027 -0.014 -0.008 -0.011 -0.015 -0.01 0.002 -0.017 -0.016 -0.006 -0.01 -0.014 -0.004 -0.003 -0.013 -0.016 -0.006 -0.009 -0.012 -0.002 -0.005 -0.012 -0.014 -0.005 -0.008 -0.011 -0.001 -0.003 -0.016 -0.019 -0.008 -0.006 -0.008 0.003 -0.001 0.001 -0.021 -0.005 0 -0.002 0.014 -0.009 0.017 0.002 0.007 0.004 0.002 0.006 -0.009 0.011 0.012 0.004 0.007 0.006 -0.001 -0.001 0.017 0.015 0.007 0.1 0.012 0.004 0.003 0.008 0.015 0.006 0.004 0.011 0.004 -0.002 0.016 0.011 0.006 0.1 0.011 0.006 0.001];
given_Cn_delta_r = [-0.018 -0.028 -0.037 -0.048 -0.043 -0.052 -0.062 -0.052 -0.051 -0.041 -0.045 -0.044 -0.034 -0.034 -0.052 -0.043 -0.038 -0.045 -0.041 -0.036 -0.027 -0.052 -0.046 -0.04 -0.045 -0.041 -0.036 -0.028 -0.054 -0.045 -0.04 -0.044 -0.04 -0.035 -0.027 -0.049 -0.049 -0.038 -0.045 -0.038 -0.028 -0.027 -0.059 -0.057 -0.037 -0.047 -0.034 -0.024 -0.023 -0.051 -0.052 -0.03 -0.048 -0.035 -0.023 -0.023 -0.03 -0.03 -0.027 -0.049 -0.035 -0.02 -0.019 -0.037 -0.033 -0.024 -0.045 -0.029 -0.016 -0.009 -0.026 -0.03 -0.019 -0.033 -0.022 -0.01 -0.025 -0.013 -0.008 -0.013 -0.016 -0.009 -0.014 -0.01];
given_throtte_000=[1060 670 880 1140 1500 1860 635 425 690 1010 1330 1700 60 25 345 755 1130 1525 -1020 -710 -300 350 910 1360 -2700 -1900 -1300 -247 600 1100 -3600 -1400 -595 -342 -200 700]; given_throtte_077=[12680 9150 6200 3950 2450 1400 12680 9150 6313 4040 2470 1400 12610 9312 6610 4290 2600 1560 12640 9839 7090 4660 2840 1660 12390 10176 7750 5320 3250 1930 11680 9848 8050 6100 3800 2310];
given_throtte_100=[20000 15000 10800 7000 4000 2500 21420 15700 11225 7323 4435 2600 22700 16860 12250 8154 5000 2835 24240 18910 13760 9285 5700 3215 26070 21075 15975 11115 6860 3950 28886 23319 18300 13484 8642 5057];
given_Cxq = [-0.267 -0.11 0.308 1.34 2.08 2.91 2.76 2.05 1.5 1.49 1.83 1.21];
given_Cyr = [0.882 0.852 0.876 0.958 0.962 0.974 0.819 0.483 0.59 1.21 -0.493 -1.04]; given_Cyp = [-0.108 -0.108 -0.188 0.11 0.258 0.226 -0.344 0.362 0.611 0.529 0.298 -2.27]; given_Czq = [-8.8 -25.8 -28.9 -31.4 -31.2 -30.7 -27.7 -28.2 -29 -29.8 -38.3 -35.3]; given_Clr = [-0.126 -0.126 0.063 0.113 0.208 0.23 0.319 0.437 0.68 0.1 0.447 -0.33]; given_Clp = [-0.36 -0.359 -0.443 -0.42 -0.383 -0.375 -0.329 -0.294 -0.23 -0.21 -0.12 -0.1]; given_Cmq = [-7.21 -0.54 -5.23 -5.26 -6.11 -6.64 -5.69 -6 -6.2 -6.4 -6.6 -6];
given_Cnr = [-0.38 -0.363 -0.378 -0.386 -0.37 -0.453 -0.55 -0.582 -0.595 -0.637 -1.02 -0.804]; given_Cnp = [0.061 0.052 0.052 -0.012 -0.013 -0.024 0.05 0.15 0.13 0.158 0.24 0.15];
given_delta_beta = [-30 -20 -10 0 10 20 30]; given_beta = [0 5 10 15 20 25 30];
%%---------发动机推力插值，（英美单位制）-------------%% given_height=[0 10000 20000 30000 40000 50000]; given_mah=[0 0.2 0.4 0.6 0.8 1]';