Code covered by the BSD License  

Highlights from
Aerospace Trajectory Optimization Using Direct Transcription and Collocation

Aerospace Trajectory Optimization Using Direct Transcription and Collocation

by

 

06 Nov 2012 (Updated )

Demonstrates the solution of an aerospace trajectory optimization problem.

dto_trap.m
% dto_trap.m    June 22, 2013

% trajectory optimization using direct transciption and collocation

% trapezoid collocation, Chebyshev-Gauss-Lobatto
% node placement, and SNOPT NLP algorithm

% Bryson-Ho example - maximize final radial distance

% Orbital Mechanics with MATLAB

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear all;

global nc_total nc_defect ndiffeq nnodes nlp_state ncv

global nlp_control tarray acc beta

pi2 = 2.0 * pi;

% conversion factor - radians to degrees

rtd = 180.0 / pi;

% astronomical unit (kilometers)

aunit = 149597870.691;

% gravitational constant of the sun (km**3/sec**2)

xmu = 132712441933.0;

% time conversion factor

tcf = sqrt(aunit^3 / xmu) / 86400.0;

% number of differential equations

ndiffeq = 3;

% number of control variables

ncv = 1;

% number of discretization nodes

nnodes = 50;

% number of state nlp variables

nlp_state = ndiffeq * nnodes;

% number of control nlp variables

nlp_control = ncv * nnodes;

% total number of nlp variables

nlpv = nlp_state + nlp_control;

% number of state vector defect equality constraints

nc_defect = nlp_state - ndiffeq;

% number of auxilary equality constraints (boundary conditions)

nc_aux = 2;

% total number of equality constraints

nc_total = nc_defect + nc_aux;

% initial mass (kilograms)

mass0 = 4535.9;

% propellant flow rate (kilograms/day)

mdot = 5.85;

% normalized propellant flow rate

beta = (mdot / mass0) * tcf;

% thrust (newtons)

thrmag = 3.781;

% normalized thrust acceleration

acc = 0.001 * (thrmag / mass0) / (xmu / aunit^2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% initial and final times and states
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clc; home;

% initial simulation time

tinitial = 0.0;

% final simulation time

tfinal = 3.32;

% define state vector at initial time

xinitial(1) = 1.0;

xinitial(2) = 0.0;

xinitial(3) = 1.0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% create time values at nodes
% (Chebyshev-Gauss-Lobatto)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[tarray, w] = cgl(nnodes, tinitial, tfinal);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% upper and lower bounds for nlp state variables
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for i = 1:1:nlp_state
    
    xlb(i) = -10.0;

    xub(i) = +10.0;
    
end

% constrain initial boundary conditions
% to be the initial state vector

for i = 1:1:ndiffeq
    
    xlb(i) = xinitial(i);

    xub(i) = xinitial(i);
    
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% upper and lower bounds for nlp control variables
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for i = nlp_state + 1:1:nlpv
    
    xlb(i) = -pi;

    xub(i) = +pi;
    
end

xlb = xlb';

xub = xub';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% initial guess for nlp state variables
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

j = 0;

for i = 1:1:nlp_state
    
    j = j + 1;

    xg(i) = xinitial(j);

    if (j == ndiffeq)
       j = 0;
    end 
    
end

lgflag = 1;

if (lgflag == 1)
    
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   % compute linear initial guess for state variables
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

   % initial conditions

   xi1 = xinitial(1);

   xi2 = xinitial(2);

   xi3 = xinitial(3);
   
   % final conditions

   xf1 = 1.525;

   xf2 = 0.0;

   xf3 = sqrt(1.0 / xf1);
   
   % initial and final times

   ti = tinitial;

   tf = tfinal;

   % compute linear guesses
  
   xg1 = dto_guess(ti, tf, xi1, xf1, nnodes);

   xg2 = dto_guess(ti, tf, xi2, xf2, nnodes);

   xg3 = dto_guess(ti, tf, xi3, xf3, nnodes);
      
   % load initial guess array

   j = 1;

   for i = 1: nnodes
       
       xg(j) = xg1(i);

       xg(j + 1) = xg2(i);

       xg(j + 2) = xg3(i);
       
       j = j + 3;
       
   end
   
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% initial guess for nlp control variables
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for i = nlp_state + 1:1:nlpv
    
    xg(i) = 0.0;
    
end

% transpose initial guess

xg = xg';

% define lower and upper bounds on objective function

flow(1) = -10.0;

fupp(1) = +10.0;

% define lower and upper bounds on defects

for i = 1:1:nc_defect
    
    flow(i + 1) = 0.0;
    
    fupp(i + 1) = 0.0;
    
end

% define lower and upper bounds on final boundary conditions

for i = 1:1:nc_aux
    
    flow(i + nc_defect + 1) = 0.0;
    
    fupp(i + nc_defect + 1) = 0.0;
    
end

flow = flow';

fupp = fupp';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve direct transcription problem
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% set SNOPT file options

snprintfile('dto_trap.out');

snsummary('dto_trap.sum');
       
snscreen('on');

[x, f, inform, xmul, fmul] = snopt(xg, xlb, xub, flow, fupp, 'dtofunc_trap');

snprintfile('off');

snsummary('off');
                                
% print results

clc; home;

fprintf('\n\n         program dto_trap \n');

fprintf('\n    trajectory optimization using');

fprintf('\n direct transcription and collocation \n\n');

fprintf ('\ninitial state vector \n');

fprintf ('\n radius              = %12.8f \n', x(1));
fprintf ('\n radial velocity     = %12.8f \n', x(2));
fprintf ('\n transverse velocity = %12.8f \n', x(3));

fprintf ('\n\nfinal state vector \n');

fprintf ('\n radius               = %12.8f \n', x(nlp_state - 2));
fprintf ('\n radial velocity      = %12.8f \n', x(nlp_state - 1));
fprintf ('\n transverse velocity  = %12.8f \n\n', x(nlp_state));

%%%%%%%%%%%%%%%%%%%%%%%%%
% create trajectory plots
%%%%%%%%%%%%%%%%%%%%%%%%%

npts = 0;

j = 1;

for i = 1:1:nnodes
    
    npts = npts + 1;

    % simulation time

    xplot(i) = tarray(i) * tcf;

    % radius

    yplot2(i) = x(j);

    % radial velocity

    yplot3(i) = x(j + 1);

    % transverse velocity

    yplot4(i) = x(j + 2);

    j = j + 3;
    
end

% polar angle (radians)

theta = cumtrapz(tarray, yplot4./yplot2);

j = 0;

for i = nlp_state + 1:1:nlpv
    
    j = j + 1;
    
    % alpha (degrees)

    yplot1(j) = rtd * x(i);
    
end

% plot results
 
figure(1);

plot(xplot, yplot1, '-*');

title('Trajectory Optimization Using Direct Transcription & Collocation', 'FontSize', 16);

xlabel('simulation time (days)', 'FontSize', 12);

ylabel('thrust angle (degrees)', 'FontSize', 12);

grid;

print -depsc -tiff -r300 control.eps

figure(2);

plot(xplot, yplot2, '-*');

title('Trajectory Optimization Using Direct Transcription & Collocation', 'FontSize', 16);

xlabel('simulation time (days)', 'FontSize', 12);

ylabel('radial distance (au)', 'FontSize', 12);

grid;

print -depsc -tiff -r300 rdistance.eps

figure(3);

plot(xplot, yplot3, '-*');

title('Trajectory Optimization Using Direct Transcription & Collocation', 'FontSize', 16);

xlabel('simulation time (days)', 'FontSize', 12);

ylabel('radial velocity (au/day)', 'FontSize', 12);

grid;

print -depsc -tiff -r300 vradial.eps

figure(4);

plot(xplot, yplot4, '-*');

title('Trajectory Optimization Using Direct Transcription & Collocation', 'FontSize', 16);

xlabel('simulation time (days)', 'FontSize', 12);

ylabel('transverse velocity (au/day)', 'FontSize', 12);

grid;

print -depsc -tiff -r300 vtransverse.eps

%%%%%%%%%%%%%%%%%%%%%%%%%%%
% create trajectory display
%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
figure(5);

hold on;
    
axis([-1.7 1.7 -1.6 1.6]);
    
plot (0, 0, 'y*');

% create array of polar angles (radians)

t = 0: pi / 50.0: 2.0 * pi;

% plot Earth orbit

plot(xinitial(1) * sin(t), xinitial(1) * cos(t), 'Color', 'b');

% plot destination planet orbit

plot(xf1 * sin(t), xf1 * cos(t), 'Color', 'r');

% plot and label transfer orbit

plot(yplot2.* cos(theta), yplot2.*sin(theta), 'k-');
 
plot(yplot2(1) * cos(theta(1)), yplot2(1) * sin(theta(1)), 'ko');
 
plot(yplot2(end) * cos(theta(end)), yplot2(end) * sin(theta(end)), 'ko');

% label plot and axes

xlabel('X coordinate (AU)', 'FontSize', 12);

ylabel('Y coordinate (AU)', 'FontSize', 12);

title('Trajectory Optimization Using Direct Transcription & Collocation', 'FontSize', 16);

grid;

axis equal;

zoom on;

print -depsc -tiff -r300 trajectories.eps

Contact us