(/курсовая/Optimal Control C++ Orginal/ INITSTATE.С++ Source)
#include "Aircraft.h"
// инициализация начального состояния самолета
void INITSTAE(double X[], double U[])
{
double V, Alpha, THETA, Beta;
double Bal[6]={0.0};
// инициализация начального состояния системы
for (int i=0;i<N;i++)
X[i] = 0.0;
// инициализация параметров для функции балансировки
VTR = 80.0;
W1xTR = 0.0;
W1yTR = 0.0;
W1zTR = 0.0;
BETATR = 0.0;
GAMTR = 0.0;
THETATR = -3.0 / GR;
HTR = 15.9;
// балансировка
BALANCING(Bal);
U[1] = Bal[0];
U[2] = Bal[1];
U[3] = Bal[3];
U[4] = Bal[4];
V=VTR;
Alpha=Bal[2];
THETA = THETATR;
Beta = BETATR;
// вычисление параметров начального состояния
X[1]=V*cos(Beta)*cos(Alpha);
X[2]=-V*cos(Beta)*sin(Alpha);
X[3]=V*sin(Beta);
X[9]=Alpha+THETA;
X[11]=HTR;
// начальное состояние остальных параметров
X[4] = 0;
X[5] = 0;
X[6] = 0;
X[7] = 0;
X[8] = 0;
X[10] = 0;
X[12] = 0;
}
2. Моделирование движения ЛА по глиссаде на языке MATLAB (SIMULINK)
На основе программ, составленных на языке С++ были созданы программы на языке программирования MATLAB, а также сделан проект в SIMULINK для моделирования режима «посадка».
Схема и описание блоков
Рис.1 Схема для моделирования режима “посадка”
Программы, которые использовались для моделирования:
Программа FX.
Она находится в блоке MATLAB Function. Эта программа реализует уравнения движения ЛА.
function DX = fx(X,U,Q)
GR = 57.29578;
R00 = 0.125;
G = 9.81;
sinG=sin(X(7));
cosG=cos(X(7));
sinP=sin(X(8));
cosP=cos(X(8));
sinT=sin(X(9));
cosT=cos(X(9));
tanT=tan(X(9));
Delta=(((288.16-0.0066*X(11))/288.16)^4.255);
A=340.0-0.004*X(11);
if (X(11)>=1.1e04)
A=295.0;
end
Vwx1=X(1);
Vwy1=X(2);
Vwz1=X(3);
Vw=sqrt((Vwx1)^2+(Vwy1)^2+(Vwz1)^2);
Alpha=-atan(Vwy1/Vwx1);
Beta=asin(Vwz1/Vw);
AL=Alpha*GR+Q(53);
BE=Beta*GR;
sinAL=sin(Alpha);
cosAL=cos(Alpha);
OMGx=X(4)*cosAL-X(5)*sinAL;
OMGy=X(4)*sinAL+X(5)*cosAL;
OMGz=X(6);
Cy=Q(7)+Q(8)*AL+Q(9)*U(2)+Q(10)*Q(52);
Cx=Q(11)+Q(12)*Cy+Q(13)*(Cy)^2+(Q(14)+Q(15)*AL+Q(16)*(AL)^2)*U(2)+
(Q(17)+Q(18)*AL+Q(19)*(AL)^2)*Q(52);
Cz=Q(27)*BE+Q(28)*U(3);
F=0.5*R00*Delta*(Vw)^2*Q(49)*G;
P1=Cx*F;
P2=Cy*F;
P3=Cz*F;
Px1=P1*cosAL-P2*sinAL;
Py1=P1*sinAL+P2*cosAL;
Pz1=P3;
dVx1=(1.0/Q(1))*(U(1)*cos(Q(2))-Px1)+X(6)*X(2)-X(5)*X(3)-G*sinT;
dVy1=(1.0/Q(1))*(U(1)*sin(Q(2))+Py1)+X(4)*X(3)-X(6)*X(1)- G*cosT*cosG;
dVz1=Pz1/Q(1)-X(4)*X(2)+X(5)*X(1)+G*cosT*sinG;
dVwx1=dVx1;
dVwy1=dVy1;
dVwz1=dVz1;
dAL=-(dVwy1*Vwx1-dVwx1*Vwy1)/((Vwx1)^2+(Vwy1)^2);
dBE=(dVwz1*((Vwx1)^2+(Vwy1)^2+(Vwz1)^2)-Vwz1*(Vwx1*dVwx1+Vwy1*dVwy1+Vwz1*dVwz1))/(sqrt((Vwx1)^2+(Vwy1)^2)*
((Vwx1)^2+(Vwy1)^2+(Vwz1)^2));
AMx=(Q(29)+Q(30)*AL)*U(3)+(Q(31)+Q(32)*AL)*BE+Q(33)*U(4)+(Q(34)+Q(35)*AL+Q(36)*(AL)^2)*OMGx*Q(50)/(2.0*Vw)+(Q(37)+Q(38)*AL)*OMGy*Q(50)/(2.0*Vw);
AMy=Q(39)*BE+Q(40)*U(3)+(Q(41)+Q(42)*AL+Q(43)*(AL)^2)*OMGx*Q(50)/(2.0*Vw)+(Q(44)+Q(45)*AL+Q(46)*(AL)^2)*OMGy*Q(50)/(2.0*Vw)+Q(47)*dBE*Q(50)/ (2.0*Vw);
AMz=Q(20)+Q(21)*AL+Q(22)*(AL)^2+Q(23)*U(2)+Q(24)*Q(52)+Q(25)*OMGz* Q(51)/Vw+Q(26)*dAL*Q(51)/Vw+(Q(48)-25.0)*Cy*0.01;
P4=AMx*F*Q(50);
P5=AMy*F*Q(50);
P6=AMz*F*Q(51);
PMx1=P4*cosAL+P5*sinAL;
PMy1=-P4*sinAL+P5*cosAL;
PMz1=P6;
dx1 = dVx1;
dx2 = dVy1;
dx3 = dVz1;
dx4 = (1.0/(Q(3)*Q(4)-(Q(6)^2)))*(Q(4)*PMx1+Q(6)*PMy1-Q(6)*(Q(3)+Q(4)-Q(5))*X(4)*X(6)+((Q(4)^2)-Q(4)*Q(5)+(Q(6)^2))*X(5)*X(6));
dx5 = (1.0/(Q(3)*Q(4)-(Q(6)^2)))*(Q(6)*PMx1+Q(3)*PMy1-((Q(3)^2)-Q(3)*Q(5)+(Q(6)^2))*X(4)*X(6)+Q(6)*(Q(3)+Q(4)-Q(5))*X(5)*X(6));
dx6 = (1.0/Q(5))*(PMz1-(Q(4)-Q(3))*X(4)*X(5)-Q(6)*((X(5)^2)-(X(4)^2)));
dx7 = X(4)-(X(5)*cosG-X(6)*sinG)*tanT;
dx8 = (1.0/cosT)*(X(5)*cosG-X(6)*sinG);
dx9 = X(6)*cosG+X(5)*sinG;
dx10 = X(1)*cosP*cosT-X(2)*(cosP*sinT*cosG-sinP*sinG)+X(3)*(sinP*cosG+cosP*sinT*sinG);
dx11 = X(1)*sinT+X(2)*cosT*cosG-X(3)*cosT*sinG;
dx12=-X(1)*sinP*cosT+X(2)*(cosP*sinG+sinP*sinT*cosG)+X(3)*
(cosP*cosG-sinP*sinT*sinG);
DX = [dx1,dx2,dx3,dx4,dx5,dx6,dx7,dx8,dx9,dx10,dx11,dx12];
Программа INITSTATE.
Она расположена в папке курсовая, файл INITSTATE. В этой программе находятся все начальные значения самолета для режима “посадка”.
function [X,U] = INITSTATE()
X0=[78.346332 -16.181852 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.151319 0.000000 15.900000 0.000000];
[BAL,J]=BALANCING();
U=[BAL(1);BAL(2);BAL(4);BAL(5)];
X=X0;
V=80;
Alpha=BAL(3);
THETA=-3.0/57.29578;
Beta=0.0;
//Вычисление пармаетров начального состояния
X(1)=V*cos(Beta)*cos(Alpha);
X(2)=-V*cos(Beta)*sin(Alpha);
X(3)=V*sin(Beta);
X(9)=Alpha+THETA;
X(11)=100;
% началное состояние остальных параметров
X(4)=0.0;
X(5)=0.0;
X(6)=0.0;
X(7)=0.0;
X(8)=0.0;
X(10)=0.0;
X(12)=0.0;
end
Программа FCT.
Она расположена в папке курсовая, файл FCT и реализует вычисление суммы квадратов правых частей уравнений для балансировки в критерии выбора угла атаки, тяги и руля высоты из условия стационарного движения ЛА.
function [RST] = FCT(Bal)
Q=[1.700E05 5.000E00 8.400E06 3.000E07 2.300E07 -0.700E06 0.000E00 0.093E00 0.006E00 0.145E-01 0.586E-01 -0.518E-01 0.876E-01 -1.920E-04 8.110E-05 0.000E00 -2.330E-03 5.020E-04 -1.340E-05 0.515E-01 -3.215E-02 5.300E-04 -0.185E-01 -0.465E-01 -1.290E01 -5.000E00 -0.152E-01 -0.344E-02 -0.064E-02 5.200E-05 -0.145E-02 -1.700E-04 -0.120E-02 -0.430E00 -3.833E-03 1.133E-03 -0.075E00 -8.250E-03 -0.280E-02 -0.002E00 0.020E00 0.215E-01 -1.300E-03 -0.300E00 8.330E-04 8.700E-05 0.000E00 2.500E01 0.330E03 4.806E01 7.570E00 -3.9026E00 3.000E00];
G=9.81;
GR=57.29578;
R00=0.125;
VTR = 80.0;
W1xTR = 0.0;
W1yTR = 0.0;
W1zTR = 0.0;
BETATR = 0.0;
GAMTR = 0.0;
THETATR = -3.0 / GR;
HTR = 100;
SIGF=[0.0,0.0,0.0,0.0,0.0,0.0];
RST=0.0;
DRGR=Bal(1)*10^5;
ETA=Bal(2);
Alpha=Bal(3);
DNAPRV=Bal(4);
DELERON=Bal(5);
Beta=BETATR;
TETTR=Alpha+THETATR;
AL=Alpha*GR+Q(53);
BE=Beta*GR;
V=VTR;
Vwx1=V*cos(Beta)*cos(Alpha);
Vwy1=-V*cos(Beta)*sin(Alpha);
Vwz1=V*sin(Beta);
%вычисление
sinG=sin(GAMTR);
cosG=cos(GAMTR);
sinT=sin(TETTR);
cosT=cos(TETTR);
Delta=((288.16-0.0066*HTR)/288.16)^4.255;
A=340.0-0.004*HTR;
if (HTR>=1.1E4)
A=295.0;
end
sinAL=sin(Alpha);
cosAL=cos(Alpha);
OMGx=W1xTR*cosAL-W1yTR*sinAL;
OMGy=W1xTR*sinAL+W1yTR*cosAL;
OMGz=W1zTR;
Cy=Q(7)+Q(8)*AL+Q(9)*ETA+Q(10)*Q(52);
Cx=Q(11)+Q(12)*Cy+Q(13)*(Cy)^2+(Q(14)+Q(15)*AL+Q(16)*(AL)^2)*ETA+(Q(17)+Q(18)*AL+Q(19)*(AL)^2)*Q(52);
Cz=Q(27)*BE+Q(28)*DNAPRV;
F=0.5*R00*Delta*(V)^2*Q(49)*G;
P1=Cx*F;
P2=Cy*F;
P3=Cz*F;
Px1=P1*cosAL-P2*sinAL;
Py1=P1*sinAL+P2*cosAL;
Pz1=P3;
dVx1=(1.0/Q(1))*(DRGR*cos(Q(2))-Px1)+W1zTR*Vwy1-W1yTR*Vwz1-G*sinT;
dVy1=(1.0/Q(1))*(DRGR*sin(Q(2))+Py1)+W1xTR*Vwz1-W1zTR*Vwx1-G*cosT*cosG;
dVz1=Pz1/Q(1)-W1xTR*Vwy1+W1yTR*Vwx1+G*cosT*sinG;
SIGF(1)=dVx1*Q(1);
SIGF(2)=dVy1*Q(1);
SIGF(3)=dVz1*Q(1);
dVwx1=dVx1;
dVwy1=dVy1;
dVwz1=dVz1;
dAL=-(dVwy1*Vwx1-dVwx1*Vwy1)/((Vwx1)^2+(Vwy1)^2);
dBE=(dVwz1*((Vwx1)^2+(Vwy1)^2+(Vwz1)^2)-Vwz1*(Vwx1*dVwx1+Vwy1*dVwy1+Vwz1*dVwz1)/(sqrt((Vwx1)^2+(Vwy1)^2))*((Vwx1)^2+(Vwy1)^2+(Vwz1)^2));
AMx=(Q(29)+Q(30)*AL)*DNAPRV+(Q(31)+Q(32)*AL)*BE+Q(33)*DELERON+(Q(34)+Q(35)*AL+Q(36)*(AL)^2)*OMGx*Q(50)/(2.0*V)+(Q(37)+Q(38)*AL)*OMGy*Q(50)/(2.0*V);
AMy=Q(39)*BE+Q(40)*DNAPRV+(Q(41)+Q(42)*AL+Q(43)*(AL)^2)*OMGx*Q(50)/(2.0*V)+(Q(44)+Q(45)*AL+Q(46)*(AL)^2)*OMGy*Q(50)/(2.0*V)+Q(47)*dBE*Q(50)/(2.0*V);
AMz=Q(20)+Q(21)*AL+Q(22)*(AL)^2+Q(23)*ETA+Q(24)*Q(52)+Q(25)*OMGz*Q(51)/V+Q(26)*dAL*Q(51)/V+(Q(48)-25.0)*Cy*0.01;
P4=AMx*F*Q(50);
P5=AMy*F*Q(50);
P6=AMz*F*Q(51);
PMx1=P4*cosAL+P5*sinAL;
PMy1=-P4*sinAL+P5*cosAL;
PMz1=P6;
SIGF(4)=(Q(4)*PMx1+Q(6)*PMy1-Q(6)*(Q(3)+Q(4)-Q(5))*W1xTR*W1zTR+((Q(4)^2)-Q(4)*Q(5)+(Q(6)^2))*W1yTR*W1zTR);
SIGF(5)=(Q(6)*PMx1+Q(3)*PMy1-((Q(3)^2)-Q(3)*Q(5)+(Q(6)^2))*W1xTR*W1zTR+Q(6)*(Q(3)+Q(4)-Q(5))*W1yTR*W1zTR);
SIGF(6)=(PMz1-(Q(4)-Q(3))*W1xTR*W1yTR-Q(6)*((W1yTR)^2-(W1xTR)^2));
RST=(SIGF(1))^2+(SIGF(2))^2+(SIGF(3))^2+(SIGF(6))^2;
end
Программа BALANCING.
Она расположена в папке курсовая, файл BALANCING и реализует минимизацию функции балансировки.
function [BAL,J] = BALANCING()
f=@(BAL)FCT(BAL);
BAL0=[0.0,0.0,0.0,0.0,0.0];
% options=optimset('fminunc');
% options=optimset(options,'Display','iter','PlotFcns',@optimplotfval);
[BAL,J]=fminunc(f,BAL0);
BAL(1)=BAL(1)*10^5;
end
Для того, чтобы найденные значения балансировки включить в проект симулинк необходимо сделать следующие шаги:
l создать скрипт initsimulation в котором написать следующее:
[X0,U0] = INITSTATE();
l далее необходимо данный скрипт привязать к симулинк, для этого нажать file - model properties - model properties - выбрать callbacks - InitFcn и выбрать файл initsimulation
Далее в симулинке с помощью констант задаются значения для U и X (см. рис.1).