matlab程序：最优制导律+最优固定时间制导律+滑模固定时间制导律

参考文献：Ebrahimi B, Bahrami M, Roshanian J. Optimal sliding-mode guidance with terminal velocity constraint for fixed-interval propulsive maneuvers[J]. Acta Astronautica, 2008, 62(10-11): 556-562.

主程序

clear;
close all;
global flag tf tb km kv epsilon g
flag=3;
% 1: Optimal guidance law (OGL)
% 2: Optimal fixed interval guidance law (OFIGL), tb=30
% 3: Silde mode fixed interval guidance law (SMGL), tb=30
tf=35; tb=30; km=6; kv=-2; epsilon=60;
g=[0,0,-9.8]';
rt0=[35,50,30]'*1e+3; vt0=[-500,-400,0]';
% rt0=[35,35,30]'*1e+3; vt0=[-500,-500,0]';
rm0=[0,0,0]'; vm0=[0,0,1000]';
state0=[rt0;vt0;rm0;vm0]; % 
tspan=[0,tf];
[tout,stateout]=ode45('re_paper_1_equ',tspan,state0);
zem=stateout(:,1:3)-stateout(:,7:9);
zev=stateout(:,4:6)-stateout(:,10:12);
am=[];
for i=2:size(tout,1)
    am=[am;norm((stateout(i,10:12)-stateout(i-1,10:12))/(tout(i)-tout(i-1)))];
end
am=[am;am(end,:)];
figure
plot3(stateout(:,1),stateout(:,2),stateout(:,3),'r','linewidth',2); hold on;
plot3(stateout(:,7),stateout(:,8),stateout(:,9),'b','linewidth',2); hold off;
grid on; 
switch (flag)
    case 1
        title('Optimal guidance law (OGL)');
    case 2
        title('Optimal fixed interval guidance law (OFIGL)');
    case 3
        title('Silde mode fixed interval guidance law (SMGL)');
end
figure
subplot(1,2,1); plot(tout,zem,'linewidth',2); grid on; legend('x','y','z'); title('ZEM');
subplot(1,2,2); plot(tout,zev,'linewidth',2); grid on; legend('vx','vy','vz'); title('ZEV');
figure

plot(tout,am,'linewidth',2); grid on;

子程序

function out = re_paper_1_equ(t,state)
global g tf km kv flag tb epsilon
tg=tf-t;
rt=state(1:3); vt=state(4:6);
rm=state(7:9); vm=state(10:12);
rmf=rt;
vmf=vt;
% zem=rmf-rm-vm*tg-tf*tg*g+(tf^2-t^2)/2*g;
f=@(x)(tf-x)*g; % integral, F1 for help
zem=rmf-rm-vm*tg-integral(f,t,tf,'arrayvalued',true);
zev=vmf-vm-tg*g;
omega=pi/3;
d=5*g*sin(omega*t); % disturbance or perturbation
dotrt=vt; dotvt=g+d;
dotrm=vm;
% calculate dotvm %
switch (flag)
    case 1 % Optimal guidance law （OGL)
        dotvm=kv/tg*zev+km/tg^2*zem;  % without tbg
    case 2 % Optimal fixed interval guidance law (OFIGL), tb=30
        if t>=tb
            dotvm=g;
        else
            tbg=tb-t;
            dotvm=((kv+km)*tbg-km*tg)/tbg^2*zev+km/tbg^2*zem; % with tbg
        end
    case 3 % Silde mode fixed interval guidance law (SMGL), tb=30
        if t>=tb
            dotvm=g;
        else
            tbg=tb-t; % t=30,tbg=0 ???
            n1=1+kv+km+sqrt((1+kv+km)^2-4*km)/2; % n1 and n2 are both right
            lambda=n1/tbg;
%             n2=1+kv+km-sqrt((1+kv+km)^2-4*km)/2;
%             lambda=n2/tbg;
            s=zem+(1/lambda-tg)*zev;
            dotvm=((kv+km)*tbg-km*tg)*zev/tbg^2 + km*zem/tbg^2 + epsilon/tbg*sign(s); % with tbg
        end
end
out=[dotrt;dotvt;dotrm;dotvm];