Code covered by the BSD License

# Aerospace Trajectory Optimization Using Direct Transcription and Collocation

### David Eagle (view profile)

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);

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;

yplot2(i) = x(j);

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

% transverse velocity

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

j = j + 3;

end

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);

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);

grid;

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

```