Direct Approaches for Solving Optimal Control

Problems in MATLAB
Paul Williams
RMIT University, Melbourne, Australia
Introduction
Direct methods for solving trajectory optimization problems have become extremely
popular. One of the reasons for the popularity is the ease and speed with which difficult
problems can be solved. In this brief set of notes, some simple discretization techniques
will be utilized to obtain direct solutions to optimal control problems. These solutions
will be obtained using the MATLAB Optimization Toolbox. The Lagrange multipliers
obtained from the discrete solution can be used as initial guesses for refining the solution
with an indirect method. This will be illustrated with an example.
Optimal Control Problem
In this report, we will consider problems in the standard Bolza form. The central
problem is to determine the state and control pair { x(t ), u(t ) } and possibly the final time
t f that minimize the cost functional
J = M [ x (t f ), t f ] +

tf

[ L ( x (t ), u(t ), t )] dt

(1)

where t \ , x \n , u \m , M : \n \ \ , and L : \n \m \ \ .
The states and controls are subject to the dynamical constraints
x (t ) = f [ x (t ), u(t ), t ], t [ t0 , t f ]

(2)

0 [ x(t0 ), t0 ] = 0
f [ x (t f ), t f ] = 0

(3)
(4)

and boundary conditions

where 0 \ p and f \q with p n and q n , and the path constraints

g [ x(t ), u(t ), t ] 0,

g \r

(5)

Discretized Problem
One way of solving the optimal control problem defined in the previous section is to
apply a numerical discretization procedure to the problem, and then to employ parameter
optimization techniques by way of a nonlinear programming (NLP) algorithm. In this set
of notes, the NLP is a sequential quadratic programming (SQP) algorithm implemented
in the MATLAB Optimization Toolbox. There are a large number of discretization
techniques available. However, many of these are not straightforward to implement, so
only some simple methods are considered.
Trapezoidal Method
The decision variables for the trapezoidal rule are the states and controls at the N + 1
node points, i.e., z = { x 0 , u0 ,..., x N , uN } . The constraints are formulated as
xk xk +1 + h2k [ f ( xk , uk , tk ) + f ( xk +1, uk +1, tk +1 ) ] = 0,

k = 0,..., N 1

The cost function is approximated as

N 1
h
J M ( x N , tN ) + k { L ( xk , uk , tk ) + L ( xk +1, uk +1,tk +1 )}
k =0 2

(6)

(7)

Hermite-Simpson Method
The decision variables for the Hermite Simpson rule are the states and controls at the N +
1 node points. The constraints are formulated by first using Hermite interpolation
x + xk +1 hk
yk = k
+ [ f ( xk , uk , tk ) f ( xk +1, uk +1, tk +1 ) ]
(8)
2
8
followed by Simpson quadrature to satisfy the state equations
h
xk xk +1 + k f ( xk , uk , tk ) + 4 fc ( yk , vk , tk + 12 ) + f ( xk +1, uk +1, tk +1 ) = 0,
6

k = 0,..., N 1

(9)
where vk are the controls associated with the interval midpoint. Two cases are
distinguished: 1) vk is free, 2) vk is determined by linear interpolation in the interval. In
the example that follows, the center control is determined via linear interpolation.
The cost function is approximated as
N 1
h
J M ( x N , tN ) + k { L ( xk , uk , tk ) + 4L ( yk , vk , tk + 12 ) + L ( xk +1, uk +1,tk +1 )} (10)
k =0 6
Numerical Example
The following is a numerical example (Breakwell problem), which has an analytical
solution.

Cost Function

J =

1
2

0 u 2dt

(11)

State Equations
x1 = x 2,

(12)

x 2 = u
Boundary Conditions
x1(0) = 0,

x1(1) = 0

x 2(0) = 1,

x 2(1) = 1

(13)

Path Constraints
x1(t ) 0.1

(14)

Initial Guess
x1(t ) = x 2(t ) = u(t ) = 0 for all node points
Some Necessary Conditions:
The D-form of the Lagrangian of the Hamiltonian for this problem is given by

L = 12 u 2 + 1x 2 + 2u + ( x1 0.1 )

(15)

The necessary conditions for optimality are given by

1 =

(16)

2 = 1

(17)

H
= u + 2 = 0,
u

u = 2

(18)

The boundary conditions are

{ 1(0), 2(0)} = { 0(1), 0(2) }

(19)

(2)
{ 1(1), 2(1) } = { (1)
f , f }

(20)

This problem has the following analytic solution

J = 40 / 9
3

(21)


3
0.1 1 1 10t ,

x1 = 0.1,

10 [ 1 t ]
0.1 1 1
3

2
1 10t ,

x 2 = 0,

10 [ 1 t ] 2
1
,
3

20
10t

1
,
3
3
u = 0,

20
10 [ 1 t ]
1
,

3
3

200

,
9
1 = 0,

200
,

9

0 t 0.3
0.3 t 0.7

) ,
3

(22)

0.7 t 1

0 t 0.3
0.3 t 0.7

(23)

0.7 t 1
0 t 0.3
0.3 t 0.7

(24)

0.7 t 1

0 t 0.3
0.3 t 0.7

(25)

0.7 t 1

Solving the Problem in MATLAB

To solve this simple problem in MATLAB using the Optimization Toolbox requires
implementing the discretization for use with the fmincon function.
In the
implementation discussed here, 3 files are necessary. The first file is a simple calling
script that defines the parameters necessary for calling fmincon. The additional two files
define the equality constraints and the cost function.

Trapezoidal Method

Filename: funcon_trap.m
% Example implementation of the trapezoidal method
function [cin,c,cinJac,cJac] = funcon(x)
global N;
global h;
% No inequality constraints
cin=[]; cinJac=[];
% Do some reassignment
y(1,:) = x(1:(N+1))';
y(2,:) = x((N+1)+1:2*(N+1))';
u(1,:) = x(2*(N+1)+1:3*(N+1))';
% Evaluate the state equations
f = states(y,u);
% Equality Constraints
zeta(:,1:N) = y(:,1:N)-y(:,2:N+1)+h/2*(f(:,1:N)+f(:,2:N+1));
c(1:N) = zeta(1,:);
c(N+1:2*N) = zeta(2,:);
% Constrain BCs
c(2*N+1) = y(1,1)-0;
c(2*N+2) = y(2,1)-1;
c(2*N+3) = y(1,N+1)-0;
c(2*N+4) = y(2,N+1)+1;
% Now determine Jacobian of Constraints
cJac = [];
function ydot = states(y,u)
ydot(1,:) = y(2,:);
ydot(2,:) = u(1,:);

Filename: funobj_trap.m
function [objf,objgrd,H] = funobj(x)
global N;
global h;
% y(1,:) = x(1:(N+1))';
% y(2,:) = x((N+1)+1:2*(N+1))';
u(1,:) = x(2*(N+1)+1:3*(N+1))';
% Define the cost function
M = 0;
L = 0.5*u(1,:).^2;
integral = h/2*sum(L(1:N)+L(2:N+1));
objf
= M+integral;
objgrd = []; H=[];

Hermite-Simpson Method

Filename: funcon_simpson.m
% Example implementation of the Hermite-Simpson method
function [cin,c,cinJac,cJac] = funcon(x)
global N;
global h;
% No inequality constraints
cin=[]; cinJac=[];
% Do some reassignment
y(1,:) = x(1:(N+1))';
y(2,:) = x((N+1)+1:2*(N+1))';
u(1,:) = x(2*(N+1)+1:3*(N+1))';
% Evaluate the state equations
f = states(y,u);
% Do Hermite interpolation
yc = 0.5*(y(:,1:N)+y(:,2:N+1))+0.125*h*(f(:,1:N)-f(:,2:N+1));
% Equality Constraints
zeta(:,1:N) = y(:,1:N)-y(:,2:N+1)+h/6*(f(:,1:N)+ ...
4*states(yc,0.5*u(:,1:N)+0.5*u(:,2:N+1))+f(:,2:N+1));
c(1:N)
= zeta(1,:);
c(N+1:2*N) = zeta(2,:);
% Constrain BCs
c(2*N+1) = y(1,1)-0;
c(2*N+2) = y(2,1)-1;
c(2*N+3) = y(1,N+1)-0;
c(2*N+4) = y(2,N+1)+1;
% Now determine Jacobian of Constraints
cJac = [];
function ydot = states(y,u)
ydot(1,:) = y(2,:);
ydot(2,:) = u(1,:);

Filename: funobj_simpson.m
function [objf,objgrd,H] = funobj(x)
global N;
global h;
% y(1,:) = x(1:(N+1))';
% y(2,:) = x((N+1)+1:2*(N+1))';
u(1,:) = x(2*(N+1)+1:3*(N+1))';
% Define the cost function
M = 0;
L = 0.5*u(1,:).^2;
Lc = 0.5*(0.5*u(1,1:N)+0.5*u(1,2:N+1)).^2;
integral = h/6*sum(L(1:N)+4*Lc(1:N)+L(2:N+1));
objf
= M+integral;
objgrd = []; H=[];

Example Execution of fmincon: opt.m

clear all
global N;
global h;
N=50;
t0 = 0; tf = 1;
h=(tf-t0)/N;

Defines the number of nodes

step-size

% Initial Guess
for ii=1:N+1
x1(ii) = 0;
x2(ii) = 0;
u(ii) = 0;
end
x(1:N+1) = x1;
x((N+1)+1:2*(N+1)) = x2;
x(2*(N+1)+1:3*(N+1)) = u;

State constraint

x1lb = -inf; x1ub = 0.1;

x2lb = -inf; x2ub = inf;
ulb = -inf; uub = inf;
xlb(1:N+1)
= x1lb;
xlb((N+1)+1:2*(N+1))
= x2lb;
xlb(2*(N+1)+1:3*(N+1)) = ulb;
xub(1:N+1)
= x1ub;
xub((N+1)+1:2*(N+1))
= x2ub;
xub(2*(N+1)+1:3*(N+1)) = uub;
x0 = x;
A=[];
b=[];
Aeq=[];
beq=[];
lb(1:length(xlb))=xlb; ub(1:length(xub))=xub;

Change simpson to trap for

trapezoidal rule

funobj = 'funobj_simpson';
funcon = 'funcon_simpson';

options=[];
options =
optimset(options,'GradObj','off','GradConstr','off','display','off','La
rgeScale','off');
tic, [x,f,inform,output,lambda,g,H] =
fmincon(funobj,x0,A,b,Aeq,beq,lb,ub,funcon,options); toc;

For example, after executing the above script on a Pentium M laptop running MATLAB
7.0 returns,
>> opt
Elapsed time is 3.045000 seconds.

In other words, the solution time is on the order of 3 seconds with relatively bad guesses
and full finite differencing of the Jacobian. Faster solutions can be obtained with sparse
finite differences, better guesses, and better solvers, such as SNOPT.
Extracting Costate Information

An important property of the trapezoidal and Hermite-Simpson method is that discrete

approximations to the costates can be obtained from the Lagrange multipliers returned by
the NLP. The function fmincon returns a structure for the Lagrange multipliers as
follows:
>> lambda
lambda =
lower:
upper:
eqlin:
eqnonlin:
ineqlin:
ineqnonlin:

costates are found here

[153x1 double]
[153x1 double]
[0x1 double]
[104x1 double]
[0x1 double]
[0x1 double]

The costates correspond to the equality constraints for the state equations and boundary
conditions. However, for both the trapezoidal and Hermite-Simpson methods, the
multipliers correspond to the costate approximations at the center of an interval, rather
than at the nodes. To extract the results, the following can be used:
t = [t0:h:tf];
Initial costate 1
x1 = x(1:(N+1))';
x2 = x((N+1)+1:2*(N+1))';
u = x(2*(N+1)+1:3*(N+1))';
tlam = [t0,[h/2:h:tf-h/2],tf];
lambda1 = [-lambda.eqnonlin(2*N+1),lambda.eqnonlin(1:(N))', ...
lambda.eqnonlin(2*N+3)];
lambda2 = [-lambda.eqnonlin(2*N+2),lambda.eqnonlin(N+1:2*N)', ...
lambda.eqnonlin(2*N+4)];

Initial costate 2

For the current example, the states and costates obtained from the above executions are
plotted in the following figures.

10

0.1
0.08

x1

0.06
0.04
0.02
0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1
Exact Solution
Discrete Solution

x2

0.5
0
-0.5

-1

0.1

0.2

0.3

0.4

0.5
0.6
Time (sec)

0.7

0.8

Figure 1: State Solutions to Breakwell Problem

11

0.9

-1

-2

-3

-4

-5

-6

Exact Solution
Discrete Solution

-7

0.1

0.2

0.3

0.4

0.5
0.6
Time (sec)

0.7

0.8

Figure 2: Optimal Control Input for Breakwell Problem

12

0.9

30
20

10
0
-10
-20
-30

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

0.9

8
Exact Solution

Discrete Solution

4
2
0
-2

0.1

0.2

0.3

0.4

0.5
0.6
Time (sec)

0.7

0.8

Figure 3: Discrete Costates for Breakwell Problem

Notice how the discrete costates can be used to validate the solutions by comparing the
necessary conditions for optimality with the Lagrange multipliers obtained from the NLP.

13