animation2.m

用Matlab实现的一个卫星自动导航模拟系统

function animation1(action)

cla reset

xa=0;ya=1;za=0;
xb=sqrt(3)/2;yb=-1/2;zb=0;
xc=-sqrt(3)/2;yc=-1/2;zc=0;             %%A,B,C坐标

xn=0;yn=0;zn=-1;                       %%初始卫星所对准的点N

a=str2num(get(findobj(gcf,'tag','h_e1'),'string'));
b=str2num(get(findobj(gcf,'tag','h_e2'),'string'));
c=str2num(get(findobj(gcf,'tag','h_e3'),'string'));
xp=a;yp=b;zp=c;                         %%要求卫星所对准的点P


alpha1=acos((xa*xp+ya*yp)/(sqrt(xa^2+ya^2)*sqrt(xp^2+yp^2)));   
alpha2=acos((xb*xp+yb*yp)/(sqrt(xb^2+yb^2)*sqrt(xp^2+yp^2)));   
alpha3=acos((xc*xp+yc*yp)/(sqrt(xc^2+yc^2)*sqrt(xp^2+yp^2)));   
 
A=[alpha1;
   alpha2;
   alpha3];

alpha=min(A,[],1);                   %%计算alpha

      
 beta=acos((xn*xp+yn*yp+zn*zp)/sqrt(xn^2+yn^2+zn^2)/sqrt(xp^2+yp^2+zp^2));          %%计算beta
 
 
 v=50;   %%喷气对卫星本身的相对速度,单位是m/s
 Q=0.004*v;   %%喷气喷量最大值,单位是Kg/s
 J1=30000;  %%卫星对于过其质心一轴的转动惯量,单位是Kg*m^2
 J2=20000;  %%卫星对于过其质心另一轴的转动惯量，单位是Kg*m^2
 r=1;   %%6个喷嘴到卫星转动轴的距离,单位是m
 l=10;   %%上下两组喷嘴平面之间的距离，单位是m 
 
 %-------------------------------------------------------------------------------------
aa=(cos(alpha)+cos(pi/3-alpha)+(sin(pi/3-alpha)-sin(alpha))*cos(pi/3+alpha)/sin(pi/3+alpha))*Q*v*l/J1;
Time=sqrt(beta/aa);         %%计算加速减速总时间    

%------------------------------------------------------------------------------------------

H0=plot3([0,0],[0,0],[-4,4],'m');                        %%画卫星实体

hold on

[h1x,h1y,h1z]=ellipsoid(0,0,0,0.5,0.5,0.5,40);                               
H1=[h1x,h1y,h1z];
surfl(h1x,h1y,h1z);
shading interp;

hold on

[h2x,h2y,h2z]=ellipsoid(0,0,0,0.75,0.75,0.25,40);                                      
H2=[h2x,h2y,h2z];
surfl(h2x,h2y,h2z);
shading flat;

hold on

H4=plot3([0,0],[0,1],[0,0],'r--',...
          [0,sqrt(3)/2],[0,-1/2],[0,0],'r--',[0,-sqrt(3)/2],[0,-1/2],[0,0],'r--');           %%画OA,OB,OC
      
hold on
 
H5=plot3([0,0],[0,1],[0.75,0.75],'b',...
         [0,sqrt(3)/2],[0,-1/2],[0.75,0.75],'b',[0,-sqrt(3)/2],[0,-1/2],[0.75,0.75],'b');              %%画OA1,OB1,OC1

hold on

H6=plot3([0,0],[0,1],[-0.75,-0.75],'b',...
         [0,sqrt(3)/2],[0,-1/2],[-0.75,-0.75],'b',[0,-sqrt(3)/2],[0,-1/2],[-0.75,-0.75],'b');          %%画OA2,OB2,OC2
     
     
H=get(gca,'children');                                                  %%获得当前图形对象的句柄

hold on

H3=plot3([-1.5,1.5],[0,0],[0,0],'k ',...
         [0,0],[-1.5,1.5],[0,0],'k',...
         [0,0],[0,0],[-2.5,2.5],'k');
 text(1.7,0,0,'x','fontsize',14);
 text(0,1.7,0,'y','fontsize',14);
 text(0,0,2.7,'z','fontsize',14);                                        %%画x,y,z轴

hold on
plot3(a,b,c,'r+'); 
text(a+0.1,b+0.1,c+0.1,'P','fontsize',14);                              %%标定P点
                          %%标定P点

 
N=30;
t=Time/N;

for k=1:N
    beta(k)=aa*(k^2*t^2-(k-1)^2*t^2)/2;
   rotate(H,[yn*zp-zn*yp,zn*xp-xn*zp,xn*yp-yn*xp],180/pi*beta(k),[0,0,0])            %%[yn*zp-zn*yp,xn*zp-zn*xp,xn*yp-yn*xp]是旋转的转轴
  pause(0.1)
end

for k=1:N
   beta(k)=aa*((N-k+1)^2*t^2-(N-k)^2*t^2)/2; 
   rotate(H,[yn*zp-zn*yp,zn*xp-xn*zp,xn*yp-yn*xp],180/pi*beta(k),[0,0,0])    
   pause(0.1)
end