建立飞机的六自由度运动方程，并对飞机定常直线平飞状态进行配平

        是当年大三的专业课，但是当时其实完全不懂要干什么，做的还蛮糟糕的。现在上了研究生至少明白了 一些些，所以这两天把从前的作业又做了一下，当然还是有很多不懂的地方，期待进步吧。

一、飞机配平（定常直线平飞） 

        所谓配平，就是寻找一组飞机状态参数(飞机姿态角和速度、高度)和舵面偏度，使得飞

机的合外力 =0 ，合外力矩 =0。飞机定常直线飞行时，必然满足两个条件：升力 = 重力，推力 =阻力。也就是说配平的过程就是寻找合适的alpha、舵偏角和推力（纵向配平）使得飞机能够满足合外力为0的条件。也就是说配平的重点集中在了如何求飞机所受的气动力、重力以及推力，其中推力是直接由配平过程得到的。

二、飞机的六自由度方程

        建立六自由度方程的目的我认为就是为了在知道飞机所受的力和力矩的情况下，由这六个方程来推导出飞机的状态量如

Ve（飞机在地面系中的速度，不是u、v、w）

Xe（飞机相对起始点的位置，其中z分量*-1表示的就是飞行器所在高度）

姿态角、姿态角速度pqr、机体系中的速度（u，v，w）

等等。

所以六自由度的公式就是那个样子的，不需要进行特别分析（？）       

大概思路就是F=ma进行一系列的坐标变换得到对应值。

三、小扰动线性化

        小扰动线性化的前提条件是飞机在受扰动前处于平衡状态，平衡状态也就是配平状态，也就是说配平状态是进行飞机建模的基础？

然而在建立六自由度方程的时候其实是没有管飞行状态的，只是在小扰动线性化的过程中，将飞机的状态设置为了配平状态上受到小扰动。由于是平衡状态，所以V,W,p,q,r这些状态量都是0，在此之后进行其他的操作，这不是本文的重点。

四、配平过程

        根据给出的数据进行初始化：

clear;
clc;
%% 数据载入
%% 纵向静导数
data_s.alpha = deg2rad([-10 -8 -6 -4 -2 -1 0 1 2 3 4 5 6 7 8 10 13 15 18 20]);
data_xls_static(:,:)=[
    -0.30699 0.056399 0.006918 
    -0.12795 0.045023 -0.02105 
    0.065931 0.040349 -0.04321 
    0.260457 0.040256 -0.06409 
    0.45485 0.044127 -0.0835 
    0.552691 0.047827 -0.09351 
    0.64791 0.051832 -0.10338 
    0.743208 0.057095 -0.11199 
    0.837028 0.063162 -0.12182 
    0.930372 0.070135 -0.13053 
    1.02106 0.077888 -0.1373 
    1.10971 0.086527 -0.14547 
    1.19606 0.096105 -0.15235 
    1.28065 0.106448 -0.16249 
    1.36008 0.117507 -0.16918 
    1.50686 0.141826 -0.18368 
    1.63177 0.169244 -0.20312 
    1.73971 0.216285 -0.24708 
    1.727 0.28649 -0.34593 
    1.60298 0.379494 -0.42825];
data_s.CL=data_xls_static(:,1);
data_s.CD=data_xls_static(:,2);
data_s.Cm=data_xls_static(:,3);

%% 高度与密度速度
pi=3.141592653;
V = [25 50 75 25 50 75 25 50 75];
H = [50 50 50 1000 1000 1000 5000 5000 5000];
ro = [];
Q = [];
for i = 1:size(H,2)
    hight = H(i) ;  
    v = V(i);
    sim air_calculate.slx;
    ro(i) = rho;
    Q(i)= 0.5*rho*v*v;
end

需要注意角度与弧度的对应关系，不好搞错了。

 之后求解飞机所受的气动力：

由公式知道，飞机的升力系数可以被分解为CL = CL0+CL_alpha*alpha+CL_deltae*delta_e+CLq*q_bar+CL_alpha_dot*alpha_dot

其他同理，那么现在我们已知几个离散的alpha对应的CL，对这些数据进行插值，即可得到任意alpha角度下对应的CL，这个CL对应的其实是CL0+CL_alpha*alpha，又由于配平状态下很多力可以被忽略，所以L = QS（CL+CL_deltae*delta_e）,同理可得D。

纵向配平时不需要考虑力矩，因此受力分析后有公式：

F(1) = T-m*g*sin(alpha*57.3)-D*cos(alpha*57.3)*cos(beta*57.3)+L*sin(alpha*57.3);

F(2) = m*g*cos(phi*57.3)*cos(alpha*57.3)-D*sin(alpha*57.3)*cos(beta*57.3)-L*cos(alpha*57.3);

求解方程让这两个力为0即可。

使用fsolve函数进行：

for i = 1:size(Q,2)
    x0 = [0,0,0];
    alpha = data_s.alpha(i);
    q = Q(i);
    result_x = fsolve(@myfun,x0);
    
    fprintf('高度%.0fm，速度%.0fm/s的配平值：\n',H(i),V(i))
    fprintf('迎角为%f 度 , 升降舵偏转为%f°, 推力为%f N\n',result_x(1)*57.3,result_x(2)/57.3,result_x(3));%迎角等于俯仰角 推力等于阻力
    
    result(i,1)=result_x(1)*57.3;%赋值Result第i列向量的第1个元素等于result_x(1)
    result(i,2)=result_x(2)/57.3;%赋值Result第i列向量的第2个元素等于result_x(2)
    result(i,3)=result_x(3);%赋值Result第i列向量的第3个元素等于result_x(3)
end

这里对于弧度和角度的概念还是有些模糊，结果不是特别好，需要进行改善。

myfun函数就是求解F的函数，具体如下：

function F = myfun(x)
global q
alpha=x(1)*57.3;
delta_e=x(2);
T=x(3);%推力
delta_r=0;
delta_a=0;
pi = 3.141592683;
% theta=alpha;%偏航角
beta=0;
p = 0;Q = 0;r = 0;
%%俯仰角不为0  滚转角为0   约束（组成代价函数） 控制量目前就alpha
phi=0;%滚转角
m = 25;
S = 0.8;
c = 0.26881;
b = 3;
g = 9.8;
cA=b^2/S;%平均气动弦长


Cy_beta = -0.00668;
Cn_beta = 0.00104;
Cl_beta = -0.00072;

%% 动导数
Cmq=-7.58;
Cm_alphadot=-1.64;
%横航向
Cnr = -0.04;
Clp = -0.62;
Clr = -0.01;
Cnp = 0.004;

%% 舵效全部都是角度
CLde = 0.00328/57.3;
CDde = 0.00018/57.3;
Cmde = -0.00842/57.3;

CYdr = 0.00242/57.3;
Cndr = -0.00061/57.3;
Cldr = -0.00004/57.3;

CYda = 0.00018/57.3;
Cnda = 0.00034/57.3;
Clda = -0.00393/57.3;
sim sim_F.slx;
L = ( CL + CLde*delta_e ) *q*S;%升力
M = ( Cm+ Cmde*delta_e )*q*S*cA;%俯仰力矩
D = ( CD + CDde*delta_e ) *q*S;%阻力 
Y = ( Cy_beta*beta + CYdr*delta_r + CYda*delta_a ) *q*S;%侧力
LM = ( Cl_beta*beta + Clda*delta_a + Cldr*delta_r)*q*S*b;%滚转力矩
N = ( Cn_beta*beta+Cnda*delta_a+Cndr*delta_r)*q*S*b;%偏航力矩
%% F就是函数本身，相当于fsolve函数干的事情就是要让F（x）=0，找到x使它成立。
F(1) = x(3)-m*g*sin(alpha*57.3)-D*cos(alpha*57.3)*cos(beta*57.3)+L*sin(alpha*57.3);
F(2) = m*g*cos(phi*57.3)*cos(alpha*57.3)-D*sin(alpha*57.3)*cos(beta*57.3)-L*cos(alpha*57.3);

在代码中调用了两个simulink模型，一个是大气密度求解模型，其实可以直接计算，但是参考了师兄的模型搭建，用了相同的模块进行尝试。

另一个sim_F.slx是用于进行插值计算的模块，调用后可以得到在不同alpha下的CL、CD、Cm值。

 

2023-3-23新增：

请教了一下师兄，发现插值问题不需要simulink也是可以的，使用interp1和interp2函数即可

形如：L = q*s*(interp1(data_s.alpha,data_s.CL,x(1))+CLde*x(2));

不知道是我搭的模型有问题还是怎么回事，飞机现在还是有些发散的状态，我再仔细研究一番。