trace0.m

本程序设计了5条典型的航迹发生器

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%                    航迹发生器
%
%  输入参数:
%  t          仿真时间
%  T          仿真步长
%  atti       横滚、俯仰、航向角（单位：度）
%  atti_rate  横滚速率、俯仰速率、航向角速率（单位：度/秒）
%  veloB      飞机运动速度——X右翼、Y机头、Z天向（单位：米/秒）
%  acceB      飞机运动加速度——X右翼、Y机头、Z天向（单位：米/秒/秒）
%  posi       航迹发生器初始位置经度、纬度、高度（单位：度、度、米）
%  veloN      飞机运动速度——X东向、Y北向、Z天向（单位：米/秒）
%  posiN      经度、纬度、高度（单位：度、度、米）        
%                           程序设计：吴玲  日期：2007/12/05
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%



%%%%%%%%仿真时间设置%%%%%%
t=0;
T=1;
TraceData=[];
t_stop=1850.0;

%%%%%%%%%%%%%%%%航迹发生器%%%%%%%%%%%%%%%%%
atti=zeros(3,1);     %横滚、俯仰、航向角（单位：度）
atti_rate=zeros(3,1);%横滚角速率、俯仰速率、航向速率（单位：度/秒）
veloB=zeros(3,1);    %飞机运动速度——X右翼、Y机头、Z天向（单位：米/秒）
acceB=zeros(3,1);    %飞机运动加速度——X右翼、Y机头、Z天向（单位：米/秒/秒）
posi=zeros(3,1);     %航迹发生器初始位置经度、纬度、高度（单位：度、度、米）
posi=[118.78;32.05;300.0];  

Re=6378137.0;  %地球半径（米） 
f=1/298.257;   %地球的椭圆率
posiN=posi;    %初始位置与航迹位置一致
long=posiN(1,1)*pi/180.0;lati=posiN(2,1)*pi/180.0;heig=posiN(3,1);
    %飞行器位置

Rm=Re*(1-2*f+3*f*sin(lati)*sin(lati));
Rn=Re*(1+f*sin(lati)*sin(lati));
    %地球曲率半径求解
atti(1,1)=0.0;
atti(2,1)=0.0;
atti(3,1)=90.0;  %初始航向角度（单位：度）

veloB(2,1)=0.0;  %飞机初始运动速度——机头（单位：米/秒）
while t<=t_stop
   if( t==0 )
       acceB(2,1)=0;                 %初始对准时间    
   elseif(t<=20)                     %加速滑跑
       acceB(2,1)=4.0;
   elseif(t<=24)                     %抬头
       atti_rate(2,1)=7.5;
       acceB(2,1)=5.0;            
   elseif (t<=84)                    %爬高
       atti_rate(2,1)=0.0;
   elseif (t<=88)                    %改平
       atti_rate(2,1)=-7.5;
       acceB(2,1)=0.0;
   elseif (t<=108)                   %加速飞行
       atti_rate(2,1)=0.0;
       acceB(2,1)=5.0;
   elseif (t<=111)                   %倾斜预转弯
       acceB(2,1)=0.0;
       atti_rate(1,1)=10.0;
   elseif (t<=141)                   %转弯
       atti_rate(1,1)=0.0;
       atti_rate(3,1)=-2.6;
   elseif (t<=144)                   %改平
       atti_rate(1,1)=-10.0;
       atti_rate(3,1)=0.0;
   elseif (t<=1744)                  %平直飞行
       att_rate(1,1)=0.0;
   elseif (t<=1764)                  %减速飞行
       acceB(2,1)=-5.0;
   elseif (t<=1770)                  %低头
       atti_rate(2,1)=-7.5;
   elseif (t<=1803)                  %俯冲
       atti_rate(2,1)=0.0;
   elseif (t<=1809)                  %改平
       atti_rate(2,1)=7.5;
   elseif (t<=1844)                  %减速滑行至停止
       atti_rate(2,1)=0.0;
       acceB(2,1)=-5.0;
   elseif (t<=1850)                  %静止
       acceB(2,1)=0.0;
   end
   t=t+T;
   t
   
   veloB(2,1)=veloB(2,1)+acceB(2,1)*T;    

   atti(1,1)=atti(1,1)+atti_rate(1,1)*T;
   atti(2,1)=atti(2,1)+atti_rate(2,1)*T;
   atti(3,1)=atti(3,1)+atti_rate(3,1)*T;
  
   roll=atti(1,1)*pi/180.0;pitch=atti(2,1)*pi/180.0;head=atti(3,1)*pi/180.0;

  %坐标系N(地理系)-->B(机体系)
  Cbn=[cos(roll)*cos(head)+sin(roll)*sin(pitch)*sin(head), -cos(roll)*sin(head)+sin(roll)*sin(pitch)*cos(head), -sin(roll)*cos(pitch);
       cos(pitch)*sin(head),                               cos(pitch)*cos(head),                                sin(pitch);
       sin(roll)*cos(head)-cos(roll)*sin(pitch)*sin(head), -sin(roll)*sin(head)-cos(roll)*sin(pitch)*cos(head), cos(roll)*cos(pitch)];

  veloN=Cbn'*veloB;
  Ve=veloN(1,1);Vn=veloN(2,1);Vu=veloN(3,1);
  V=sqrt(veloN(1,1)^2+veloN(2,1)^2+veloN(3,1)^2);

  heig=heig+T*Vu;
  lati=lati+T*(Vn/(Rm+heig));
  long=long-T*(Ve/((Rn+heig)*cos(lati)));

  posiN(1,1)=long*180.0/pi;       %单位：度
  posiN(2,1)=lati*180.0/pi;       %单位：度
  posiN(3,1)=heig;

  
  
  TraceData=[TraceData;t,posiN',V,veloN'];
end
fg_1=figure('Name','Trace','NumberTitle','off');
plot3(TraceData(:,2),TraceData(:,3),TraceData(:,4),'m',TraceData(1,2),TraceData(1,3),TraceData(1,4),'o',TraceData(1825,2),TraceData(1825,3),TraceData(1825,4),'o');
title('飞行航迹仿真(南京－>北京)');
text(TraceData(1,2),TraceData(1,3),TraceData(1,4),'南京');
text(TraceData(1825,2),TraceData(1825,3),TraceData(1825,4),'北京');
xlabel('经度（^{\circ}）');ylabel('纬度（^{\circ}）');zlabel('高度（m）');
grid;
fg_2=figure('Name','trace','NumberTitle','off');
subplot(3,1,1);plot(TraceData(:,1),TraceData(:,2));title('飞行器经、纬、高度仿真');ylabel('经度（^{\circ}）');grid;
subplot(3,1,2);plot(TraceData(:,1),TraceData(:,3));ylabel('纬度（^{\circ}）');grid;
subplot(3,1,3);plot(TraceData(:,1),TraceData(:,4));xlabel('t(sec)');ylabel('高度（m）');grid;
fg_3=figure('Name','Velo','NumberTitle','off');
plot(TraceData(:,1),TraceData(:,5));
title('飞行速度仿真');
xlabel('t(sec)');ylabel('速度（m/s）');grid;
fg_4=figure('Name','velo','NumberTitle','off');
subplot(3,1,1);plot(TraceData(:,1),TraceData(:,6));title('飞行器东、北、天速度仿真');ylabel('东向速度(m/s)');grid;
subplot(3,1,2);plot(TraceData(:,1),TraceData(:,7));ylabel('北向速度(m/s)');grid;
subplot(3,1,3);plot(TraceData(:,1),TraceData(:,8));xlabel('t(sec)');ylabel('天向速度(m/s)');grid;
%%%%%%%%%%%%%存储仿真数据%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
save trace0.dat TraceData -ASCII;    %存储航迹数据