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

David Eagle

 

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