flysimulation.m

基于空间飞行器的姿态仿真源程序

close all,clear,clc
global J1 J2 J3
J1=1200;
J2=2200;
J3=3100;
%---------
mu1=3;
mu2=3;
mu3=3;
lamda11=20;
lamda12=1/4*lamda11^2;
lamda21=20;
lamda22=1/4*lamda21^2;
lamda31=20;
lamda32=1/4*lamda31^2;
%--------
t0=0;
h=0.01;
tf=50;
tout=t0:h:tf;
%-------
sita1=0*pi/180;
sita2=0*pi/180;
sita3=0*pi/180;
w1=0*pi/180;
w2=0*pi/180;
w3=0*pi/180;
state=[sita1;sita2;sita3;w1;w2;w3];
stateout=[];
stateout=[stateout,state];
%-----------------
w1jifen=0;
w2jifen=0;
w3jifen=0;
%-------------
sita1cout=[];
sita2cout=[];
sita3cout=[];
for t=t0:h:tf
    sita1c=30*pi/180*sign(sin(2*pi/10*t));
    sita2c=30*pi/180*sign(sin(2*pi/10*t));
    sita3c=30*pi/180*sign(sin(2*pi/10*t));
    sita1cout=[sita1cout,sita1c];
    sita2cout=[sita2cout,sita2c];
    sita3cout=[sita3cout,sita3c];
    %--------------
    L1=-(J2-J3)*w2*w3-J1*(lamda11*w1+lamda12*w1jifen);
    L2=-(J3-J1)*w3*w1-J2*(lamda21*w2+lamda22*w2jifen);
    L3=-(J1-J2)*w1*w2-J3*(lamda31*w3+lamda32*w3jifen);
    control=[L1;L2;L3];
    %-------------------
    k1=h*stateequation(t,state,control);
    k2=h*stateequation(t+0.5*h,state+0.5*k1,control);
    k3=h*stateequation(t+0.5*h,state+0.5*k2,control);
    k4=h*stateequation(t+h,state+k3,control);
    state=state+1/6*(k1+2*k2+2*k3+k4);
    stateout=[stateout,state];
    %-------------------
    sita1=state(1);
    sita2=state(2);
    sita3=state(3);
    w1=state(4);
    w2=state(5);
    w3=state(6);
    %---------------
    w2c=-mu2*(sita2-sita2c)*cos(sita1)*cos(sita3)-mu3*(sita3-sita3c)*sin(sita1);
    w3c=mu2*(sita2-sita2c)*sin(sita1)*cos(sita3)-mu3*(sita3-sita3c)*cos(sita1);
    w1c=-mu1*(sita1-sita1c)+w2c*cos(sita1)*tan(sita3)-w3c*sin(sita1)*tan(sita3);
    w1jifen=w1jifen+(w1-w1c)*h;
    w2jifen=w2jifen+(w2-w2c)*h;
    w3jifen=w3jifen+(w3-w3c)*h;
end
%------------
tout=[tout,tout(end)];
sita1cout=[sita1cout,sita1cout(end)];
sita2cout=[sita2cout,sita2cout(end)];
sita3cout=[sita3cout,sita3cout(end)];
sita1out=stateout(1,:);
sita2out=stateout(2,:);
sita3out=stateout(3,:);
w1out=stateout(4,:);
w2out=stateout(5,:);
w3out=stateout(6,:);
%---------------
figure,
subplot(3,1,1),
plot(tout,sita1cout*180/pi,'r'),hold on
plot(tout,sita1out*180/pi,'b'),grid on
legend('command','response'),
subplot(3,1,2),
plot(tout,sita2cout*180/pi,'r'),hold on
plot(tout,sita2out*180/pi,'b'),grid on
legend('command','response'),
subplot(3,1,3),
plot(tout,sita3cout*180/pi,'r'),hold on
plot(tout,sita3out*180/pi,'b'),grid on
legend('command','response'),