Вы находитесь на странице: 1из 20

Algorithm 733: TOMP Fortran Modules

for Optimal Control Calculations


DIETER

KRAFT

Iachhochschule

Munchen

A great number of analysis and synthesis problems of modern processes can be written as state
and control constrained optimal control problems governed by ordinary differential equations
with multipoint boundary values. As the software tools for following this attractive approach are
still missing or can be used only by experts, the structure and usage of an easy-to-use software
package is described which efficiently solves the given problem Among its features are userorientation, applicability onpersonal computers and mainframes, and robustness with respect to
model changes and inaccurate starting values, It has been tested on a number of complex
engineering tasks, including aerospace and robotic trajectory planning.
Categories and Subject Descriptors: G.1.6 [Numerical Analysis]: Optimization; G,1.7 [Numerical Analysis]: Ordinary Differential Equations; G.4 [Mathematics
of Computing]: Mathematical Software; 1.2.9 [Artificial
Intelligence]:
Robotics
General Terms: Algorithms
Additional Key Words and Phrases: Boundary value problems, manipulators,
robotics, shooting method

optimal control,

1. INTRODUCTION
Within
nonlinear

the

last

decade

great

programming

progress

(NLP) :
subject

to general

and to lower

has

been

achieved

in

solving

the

problem

equality

and inequality

constraints

g7(x)

= o,

j=l,

gJ(x)

20,

j=me+l,

and upper

bounds
l,<

(1.1)

mi:n f( x )

(1.2)

. . .. m.,
. . ..m.

(1.3)

on the variables
i=l

X,<UL,

,. ...

(1.4)

n,

for a local minimum


by sequential
quadratic
programming
(SQP) algorithms.
f: R n + R and g : R + R m are assumed to be
The problem
functions
continuously
differentiable
and to have no specific structure.
A recent monoAuthors address: Fachhochschule Munchen, D-80335 Munchen 2, Dachauerstrasse 98b,
Germany
Permission to copy without fee all or part of this material is granted provided that the copies are
not made or distn buted for direct commercial advantage, the ACM copyright notice and the title
of the pub] ication and its date appear, and notice is given that copying is by permission of the
Association for Computing Machinery, To copy otherwise, or to republish, requires a fee and/or
specific permission.
01994 ACM 0098-3500/94/0900-0262 $03.50
ACM

TransactIons

on Mathematical

Software,

Vol

20, No

3, September

1994, Pages 262-281

TOMP Fortran Modules for Optimal Control Calculations


graph

[Gill

rithms

et al. 1991]

and their

Because
1983;

of the

Powell

calculating
nary
have

presents

numerical

of efficient

Schittkowski

optimal

overview

control

codes for

1986]

functions

it

1973; Kamm
and Johnson
problem
(NLP) the original
rnin

for dynamic

z(~),

(NLP)

to revisit
systems

algo-

[Gill

the

et al.

problem

described

of

by ordi-

programming.
Several authors
20 years [Brusch
and Schapelle

1973; Tabak and Kuo 1971]


optimal
control problem

J&(~,

UEU

on theoretical

solving

is time

differential
equations
via mathematical
pursued
this approach
within
the last

(OCP):

263

implementations.

availability

1978;

an excellent

~(~),~)~~

by transforming

+ @o(a, z(a)

into

b, z(b)),

(1.5)

.mER@

subject

to differential

constraints

i,(t)
boundary

@[(t, z(t),

conditions,

with

$,(a,
which

in practical

+,j~,
t~t(b,

state

In problem
variable

(OCP)

t = [a,

b]

1.

z(b))

appear

,. ...

separated

b)

characterizes

dimensional
problem

by

z(a))

O,

i=l,

. . ..ra.

(1.8)

Z(T))

= 0,

i=l,

. . .. r.,

(1.9)

z(b))

i=l,

. . ..r~.

O,

(1.10)

constraints
i=l,

are considered

The state

a multipoint
a vector

vector
(OCP),

(1.11)

. . ..s.

depending

R p is assumed

boundary

of time

of design

the

state

(1.7)

r,

as

on one independent

to be absolutely

uous; the control


u = U c R ~ is assumed
to be bounded
which practically
means piecewise
continuous.
The initial
(1.9) at some interior
and b may be fixed or free. Condition
controlled

(1.6)

7...7P,

i=l

O,

Zdt), %-) 20,

z(t),

functions
=:

b,

mostly

or control

$t(t,

i=l

b,

r <

applications
ia,(a,

and (algebraic)

<

T, z(T),

z(a),

m-) = o,

u(t),

value

functions

parameters
equations

problem.
u,(t),

n,,

(1.6)

i =

and

The trajectory

i =
1,.

1, ...,

...

q..

relations

contin-

and measurable,
and final time a
time event ~ E (a,
q,

Note,
(1.8)

and
that

z(t)

is

a finite
within

represent

an

initial
value problem
(IVP) as a subproblem.
The problem
described
by Equations
( 1.5 1. 11) can be solved by an indirect
approach
by transforming
problem
(OCP) into a two-point
boundary
value
problem
via the necessary
optimality
conditions
of (OCP),
the maximum
principle
of Pontryagin
et al. [1964], making
use of the costate equations,

i(t) =

;@(t,z(t),

u(t),T) A(t),

(1.12)

equations
adjoined
to the state equations
(1.6). For the
a set of p differential
solution
of the resulting
boundary
value problem
mainly
three approaches
have been pursued
[Keller
1976], e.g., finite
difference
techniques
[Lentini
ACM

TransactIons

on Mathematical

Software,

Vol. 20, No. 3, September

1994.

264

D. Kraft

and Pereyra
1977], collocation
procedures
[Ascher
et al. 1981], and shooting
methods
[Grimm
et al. 1985]. Shooting
methods
have first been applied to the
solution
(1.6)

of optimal

control

costate

equations

and

forward
boundary
nonlinear
ized

by Bulirsch

(1.12)

are

by high

accuracy

method

and
These

approach

and

is the
the

necessity

can

[von

fast

convergence
and

overcome

and Bulirsch

near

region

solution

equations

mesh

the

Programming

(TOMP),

by

costate

methods

and by automatic

which

the

optimum.

numerical

equa-

or a hybrid

symbolic

tion of the costate equations


[Schopf and Deuflhard
1994].
This article presents
a software
package, Trajectory
Optimization
ematical

in

due to Newtons

of the

by homotopy

1992],

state

initial
conditions.
The
by solving
a system
of
The method
is character-

convergence

of formulation

be partially

Stryk

small

The

on a finite

estimated
satisfied
method.

(quadratic)

inherent

[1971].

integrated

direction
(from
t. to t b) with
and continuity
conditions
are
equations
by a modified
Newton

Disadvantageous
tions.

problems

generaby Math-

solutions

of problem

shooting
(OCP) may be calculated
by a direct
solution
of the costate equations
by transforming

approach.1
This avoids the
problem
(OCP) into problem

(NLP)

has the advantage

as described

in the sequel.

extremely
high flexibility
lations
which are rather
tions,

This

approach

of

of the software
with respect to problem
the rule than the exception
in industrial

reformuapplica-

and

a large
convergence
radius
together
near the optimum
due to the particular
important
feature
seldom known.
Among

in industrial

its disadvantages

it

is approximative

But

the approximations

discretization

with
superlinear
convergence
choice of the optimizer.
This

applications

as accurate

starting

rate
is an

values

are

are that

by its discrete

nature.

may be chosen

of the problem

arbitrarily

and thereby

accurate

on the available

depending
computer

on the
facilities.

The software
aims at industrial
applications
rather
than at academic
accuracy solutions.
It can be used as a starting
procedure
for the latter.
The software
is completely
written
in Fortran
and has originally

highbeen

developed
on a mainframe
computer
with Watfiv
and Fujitsu
test compilers.
It is also best suited for microcomputer
applications.
It has been tested in this
environment
using the Fortran
compilers
Watfor77
Version
3.1, MS-Fortran
Version
5.0, and Salford
Ftn77\486
Version
2.60, the latter
being a genuine
32-bit compiler.
In this article
the transformation
from (OCP) to (NLP)
is presented
together with an overview
of the software
organization
and usage. The necessary mathematical
tools, e.g., approximation
schemes, initial
value problem
solvers,
and nonlinear
programming
solvers,
are described
in the original
report [Kraft
1991]. Examples
are presented
to show the performance
and the

1A direct

collocation

is solved

by collocation

ACM

Transactions

approach

proceeds

instead

on Mathematical

similarly;

by a Runge-Kutta-type
Software,

the

difference
method

is that

the initial

[von

Stryk

Vol. 20, No, 3, September

1994

1993].

value

problem

TOMP Fortran Modules for Optimal Control Calculations


usage

of TOMP.

input

and

The software

(OCP);

2. DIRECT

value

first

represent

2.1

does not make

therefore

they

problem

explicit

step

into

Control

solution,

is

described

material

algorithms

test

use of the optimality

example.
conditions

here.

the following

steps:

and

in

the

is given

(1) a number

in the original

solver

for problem

of q time

grids

as partitions

x=~ ,,

[Kraft

remaining

steps

of the

ap-

1991].

(OCP),

represents

t,,a control

or knot
Ui(t,,

the

by a finite

infinite

dimensional

set of control

control

parameters

i=l

1, one for each control

the control

n,,

functions

u,(t)

,...,

function

parameter

j=l,...,

),

XU.

(2.1)

q,

u,(t),

XUL is associated
1

i=l,...,

q,

(2.2)

at the breakpoints,

and

each subinterval

L, = [%,>%,+117

j=l,...

of the partitions
A,, each control
interpolating
piecewise
polynomial
u,(t)
is defined

the vector

in Section
u(t)

,n, _l,

,i=l

3.1 of Kraft

is completely

m~f={xl,,

Xl,,

. . ..xqn

is approximated
v~ ( t) of a certain

by an
order
(2.4)

?...,q,

and uniquely
X12,...,

(2.3)

,q,

[1991].
defined

by the finite

is composed

_,, x ~n,} U {ml,


q

vector
(2.5)

xqnq_,, Xqnq}.

vector
of controlling
parameters
XU and the design parameters
m

x = (x;,

i=l,...

function
u,(t)
(pp) function

= uAjt),

Xu := {xl,,
The entire
parameters

report

< tin,_l < t,n, = b}>

of the interval

each breakpoint

which

the

A summary

A, is defined

Al := {a = t,, < t,, <

(3) within

both

mathematics.

Parameterization

To use an (NLP)

which

following;

of numerical

functions
u(t) have to be represented
This is accomplished
as follows:

Thus

of the subroutine

comprehensive

optimization.

basic

(2) with

description

but

are omitted

can be separated

(3) parameter

plied

a detailed

a simple

parameterization,

(2) initial

The

for

SHOOTING

shooting

(1) control

gives

parameters

package

for problem

Direct

The Appendix

output

265

mz,...,

of the

mqm}.

control

(2.6)

The total number


of controlling
parameters
sums up to n = dim(x)
= nl +
of the set of
nz + . . . + nq + qm.It should be noted that a and b are elements
controlling

parameters

if they

are free parameters.

ACM Transactions on Mathematical Software, Vol. 20, No. 3, Septembsr 1994.

266

D. Kraft

We

limit

the

partitions

A,

of I

to one for

each

control

function.

As we

require
noncoincident
breakpoints,
this means
that
except
for zero-order
pp-functions
the approximating
controls
u~, are continuous.
Piecewise
continuous
2.2

pp-functions
Problem

2.2.1
turn

require

breakpoints

or multiple

partitions

Initial

Value

With

Problem.
by

a finite

u(t)

number

approximated

by (2.4)

of parameters

XU, the

are solved

with

initial

values
Y.(a,

according
i(a))

in

Runge-Kutta-Fehlberg
class
preferred
as IVP-solvers
for
rized in Section 3.2 of Kraft

i=l

,...,a,

(2.8)

[Prince
1981; Shampine
efficiency
reasons.
These
[ 1991]. The usage of the

explained
in Appendix
C of Kraft
[1991].
If the number
of specified
initial
conditions
then

states
x:

p,

where

the unspecified

ra

unspecified

states

notation.
In this case there
r~ controlling
parameters.
Problem

Evaluation.

and it only

depends

/@O(t,

l(t),

states

dim(x)

For

given

on x. Thus

uA(t),

n)dt

methods

the

x the

number

of

in the set of parameters

here for reasons


nl

of the

and Watts
1976] are
methods
are summarespective
software
is

is less than

ra

are included

<,(a) are reindexed


are

(2.7)

. . ..p.

to (1.8)

O,

i=l,

0,

to z(t), and ~(a) = z(a).


where <(t)is an approximation
For nonstiff
or mildly
stiff problems
one-step discretization

2.2.2

which

differential

(1.6)

i,(t) o,(t, l(t), u~(t), m) =

fined,

A,J.

Approximation

is represented

constraints

coinciding

722 +

trajectory

of simplified

nq + qm +

< is uniquely

p
de-

the cost function


+ @,(a,

i(a),

b, {(b))

=f(x)

(2.10)

and the boundary


t~,(b,

conditions
r(b))

=g,(x)

i=l

= O,

4.(7, 1(7)) ==
g,(x)

= o,

(2.11)

,. ... b,

i=r~+l,

. . ..r~+

r.,

(2.12)

can be evaluated
with me = rT + rb.
To calculate
the state constraints
f,(t,
a pointwise
cation grid

approach
AC

((t),

u~(t),

is applied.

AC := {a =tc,
ACM

TransactIons

on Mathematical

m) 20,
Equation

<tc,

Software,

<

i=l,
(2.13)

. . . <tC,_,

. . ..s.

is evaluated
<tci

Vol. 20, No 3, September

= b}
1994

(2.13)
on a communi(2.14)

TOMP Fortran Modules for Optimal Control Calculations


pointwise

at certain

communication

g,(tc,,@cJ), r@),

knots

m) =:gme+k(x)
i=l,..

267

tCt

>0,

.,S,

j=l,..

.,t,

k=l,...

,sXt,

(2.15)

m, = m me
is m, = s x t. The
and the number
of inequality
constraints
communication
grid has to be chosen fine within
subintervals
in which the
constraints
are active;
that means
g ~e+z(~) = 0, with
1 ~ Imll, the set of
inequality
constraint
indices,
and it can be coarse elsewhere.
Another
possibility
to handle
algebraic
state constraints
is to define
s

auxiliary

differential

equations
constraint

equations

(2.7). These
violations

ip+,(t) together

with

the boundary

The trajectory

the equations

problem

the

state
of the

i=l,

(2.16)

. . ..s.

i=l

{P+,(b)

are then

formulated

as

= $+,(b)

20,

i=l

2.11,2.15,

or 2.18) approximate

= o,

(2.17)

,. .,, s.

>. ..> s,

(2.18)

Gradient

partial

(2.10,

(OCP)

as

Generation
methods

for solving

derivatives
respect

analytically,
For reasons
(3$D,
(X)
dxj

problem

(NLP).

SQP-based
with

with

a quadrature

m, = s.

Now

2.3

0,

and

gme+,(x)
with

together

determine

conditions

= 0,

constraints

are integrated

equations

((t), UA(t),w), O} =

min{ft(t,

(p+,(a)

which

auxiliary

to

all

of the

parameters

they must
of efficiency
9,(XJ

problem

dp( x)\dx

(NLP)

require

problem

x. As

the

the availability

functions
dq( x)/

c?x cannot

be calculated
by some numerical
we prefer forward
differences:

h) $P~(xJ)
?

i=l

p = {f,

,.. .,m+l,

of the

gl, ...,

difference

.j=l,...,

g~}

be expressed
scheme.

n,

(2.19)

with
h = max{~,

IXJ18},

(2.20)

8/10},

(2.21)

and
8 = max{~,

is the
machine
precision
and O is the user-required
accuracy.
The
where
accuracy of this first-order
approximation
to the gradient
is O(h). The choice
of the perturbation
(2.21) is motivated
by the analysis
of Horn [1989].

The possibility
to calculate
[Brusch
and Schapelle
1973;
inherent
inflexibility
often the case within

the partial
derivatives
Kraft
1985] has been

when the simulation


design phases.
ACM

Transactions

on Mathematical

model

by the adjoint
equations
eliminated
because of the
is changed,

Software,

Vol

which

is very

20, No. 3, September

1994.

268

D. Kraft

3. SOFTWARE
The software
and

the

ORGANIZATION
package

optimizer

communication
pendent
from
head

is more

value

problem

control

is split

into

which

exchange

two modules,
their

the simulator

d_TOMP

information

by

reverse

[Gill et al. 1979]. This means that both are completely


each other. The disadvantage
of a slightly
more calling
than

compensated

of Section

functions,

is solved

TOMP
SLSQP

by a large

flexibility.

2.2.1 is integrated

e.g., Equation

by sequential

(2. 19). In SLSQP

linear

least

In d-TOMP

for the nominal

squares

the initial

and the perturbed

problem

problems

indeover-

(NLP)

[Kraft

of Section

1988]

using

slightly
modified
NNLS
For a given parameter

of Lawson
and Hanson
[1974] as core routine.
vector x k, where x 0 has to be furnished
by the user,

the simulator

values

calculates

the constraints
introduced
into

g( x k ), and
the optimizer

of the cost function

x k + 1 or informs
the user that
simple but efficient
and flexible
The dashed
box in Figure
intermediate

trajectories

interactive

version

The user
one

Vf( x k ),

certain
stopping
criteria
are satisfied.
organization
is shown in Figure
1.
1 indicates
a possible
user interface;

can

be plotted.

Thus

one

can

easily

build

This
here
up

an

of the software.

has to write

in which

f( x k ), its gradient

their
Jacobi
matrix
Vg( x k ). These values
are
which either proposes a new parameter
vector

the

three

subroutines:

right-hand

sides

of the

differential

equations

(2.7)

are

implemented,
another

in which

evaluated,
a third

one in which

subroutine
The
Section

the cost function

and the constraints

(2.10,

2.11, 2.15) are

and

handling

with

the

output

an empty
of these

is sampled

subroutine

for trajectories;

this

may

be a

body.

subroutines

will

be explained

in

the

example

of

4.1 and in the Appendix.

4. EXAMPLES
To demonstrate

the

effectiveness

of the

solution

given:
a state-constrained
brachistochrone
planning
approach
for an industrial
robot.
4.1

State-Constrained

approach

problem

and

two
an

examples
optimal

are
path-

Brachistochrone

The first example


has often been treated
in the literature
[Bryson
and Ho
1969]. Here it serves for comparison
reasons and as an example
of how the
user subroutines
have to be written.
The subroutine
source listings
are given
in Appendix
A.
4.1.1

Mathematical

Model.

A bead

two points
A and B in a constant
f3(t) of the wire that will produce
points?
ACM

Transactions

on Mathematical

Software,

slides

on a frictionless

wire

between

gravitational
field, g. What is the shape
a minimum-time
path between
the two

Vol

20, No, 3, September

1994

TOMP Fortran Modules for Optimal Control Calculations

Input:

269

I
d_TOMP:
Input

Xk

output:

f(zk),

Vf(zk),

,.---...

--1----------

; User

Interface

: ---------

g(zk),

,..

Vg(xk)

......

EiIizzl
Fig.1.

Organization of TOMPwith

reverse communication.

.
The state

where

equations

ZI = x is the

distance,

and

~,(t)=

~,(t,

e(t),

22(t)

=z3(t)sin

0(t),

A3(t)

=gsin

horizontal

distance

the other

states

(l(t),

distance

of the

All initial

o,

bead,

conditions
i=l,

Z2 = y is its
are assumed

vertical

to be zero:

...,3.

is prescribed
zl(tB)

Both

are

=z3(t)cos

Z3 = u its velocity.

horizontal

u(t)),

Al(t)

zL(tA)
The final

z(t),

= 1.

are free.

The problem
is to find
time to reach B from A

a control

function

u(t)

= O(t)

that

minimizes

the

f=tBt~.
We also
following

solve this problem


for the
state inequality
constraint
C(z(t))

with

a = 0.4 and

= az~(t)

case where

Zz(t)

2 b,

the

bead

has

to satisfy

the

~~c[~A,t~],

b = 0.2.
ACM

Transactions

on Mathematical

Software,

Vol. 20, No. 3, September

1994.

270

D. Kraft

4.1.2

Test Results.

4.1.2.1

Nominal

high-order

Case.

This

Runge-Kutta

The tolerance
of the nonlinear
initial
estimates
have been
bounds
number

on (1(t) have
of iterations

example

method

has been

with

absolute

programming
@(t ) = O and

solved
and

with

relative

code has been

using

10_ ~.

set to 10 _G. The


upper
and lower

t: = 2.0. The

been i 2 m, respectively,
in the unconstrained

TOMP
accuracy

and zero for tB. The


10, that in the con-

and ten
case was

strained
case 23. The results are shown in the graphs of Figures
2 and 3. (The
unconstrained
trajectory
is dotted; the constrained
trajectory
is dashed.) The
z I( t) <0.65. The
case, tB = 1.795

constrained
trajectory
is active
within
the range
0.30<
minimum
time for t~ = O is t~ = 1.772 in the unconstrained
in the constrained
4.1.2.2

case.

Convergence

Tests.

Let

this

example

be taken

as a paradigm

for

the large convergence


region of TOMP as claimed
in Section 1. The numerical
experiment
includes
three different
starting
controls
for three estimates
of
the final time. The results
are summarized
in Table I. The columns
include
different
0 -values
for a given t~-value.
The entries
(i, j) in the table are the
number
true

of iterations

solution

expanded
entry

for

to

for the constrained


the

data

~ 10, the

(2, 1) breaks

convergence

down.

The

looping
in its trajectory
time (to tB = 2.013).
4.2

Optimal

Trajectories

near

solution

example
problem.

ence trajectories
are computed

to

All

4.1.2.1.

to the

global

This

entries

If

is at a local

the origin.

of a Manutec

The following
optimal
control

problem.

of paragraph

the

converge

bounds

on

minimum

for

minimum

and then

of course

the

prolongates

to the
d are
case in
has a

the final

r3 Robot

treats
the calculation
of robot
trajectories
Optimal
robot trajectories
are compared
with

for the path-planning


check the robot

problem
performance

as an
refer-

[Craig
1989; Pfeiffer
1987] or
within
its working
space

[Konzelmann
et al. 19891. The calculations
are exemplified
on a Manutec
r3
robot of Siemens,
whose mathematical
model has been developed
by Otter
and Turk
4.2.1
structure
form

[1988].
ikfathematical

Model.

is a multibody

The mathematical

system
M(0);

in the

=V(0,6)

model

configuration
+G(0)

of a robot
space

of the

+T.

with

chain

following

(4.1)

We restrict
the model to one of three degrees of freedom.
In this case the
positive
following
notion is valid for Equation
(4.1): M is a 3 X 3 symmetric,
definite
mass matrix;
d is a 3 X 1 matrix
of generalized
position
coordinates;
the 0, are the relative
angles between
arm i 1 and arm i; ~ and ~ are
3 x 1 matrices
of angle velocity
and angle acceleration;
V is a 3 X 1 matrix
the centrifugal
and Coriolis
terms;
G is a 3 x 1 matrix
of the gravitational
terms, and T is a 3 X 1 matrix
of the motor moments.
The following
assumptions
have been made.

of

The robot consists of one fixed basis body,


which motors and gears are integrated.

in

ACM

Transactmns

on Mathematical

Software,

three

arms

Vol. 20, No 3, September

and three

1994

rotors

TOMP Fortran Modules for Optimal Control Calculations


Brachistochrone:

271

State Variables

0.1

-0.2

-0.3

z
x

0.4

0.5

0.6

-0.7
0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

z(1) (t)
Fig. 2. State constrained brachistochrone: state variables.
Brachistochrone:

Control

Variables

1.6

1.4

1.2
,

\
,

1
z

\
.

3
2
b

0.8

.\
.,

unconstrained
\.\

\..\
..,,

0.6

0.4

..~.x,
-.

.....

-..

6-oni-iiain-e-a-----------...

.. -.,,

0.2

0
0

(
0.1

0.2

1
0.3

1
0.4

,
0.5

1
0.6

....

-.. ...

,
0.7

.. ... .

....
,

0.8

0.9

-.. ..>-

..
1

z(1) (t)
Fig.3.

State constrained brachistochrone: control variables.


ACM TransactIons on Mathematical Software, Vol. 20, No. 3, September 1994.

272

D. Kraft
Table I.

Number of Iterations for Different Estimates

The

arms

rotational
The

are

nonelastic

degree

gears

links

which

are

connected

by joints

with

one

and backlash

are

of freedom.

are assumed

as ideal;

their

elasticity,

friction,

neglected.
-The

dynamics

of the

that

the input

voltages

Further
moments

motors

and

gears

are transformed

are
into

also

neglected,

which

means

motor

moments

without

delay.

details
as well as all model parameters
(robot geometry,
masses,
of inertia,
etc.) can be found in Otter and Turk [1988] where also a

Fortran

procedure

angular

accelerations

R3M2 S1 is described

in which

; are computed

for

according

given

to the

0, 6, and

algorithms

T the

of Brandl

et al. [1986].
The
velocity

state

vector

of the

model

z = (6, ~), and the motor

u. The state

equations

2 = ~(t,

is

composed

moments

from

angle

T is included

z, u) are thus

given

and

angular

in the control

;O= (j,
:~=M-l(OIV(O,

The boundary

values

at to and

(32(to) = +0.3107,

i2(to) = o,

03(to)= + 1.7541,

&(to) = o,

el(t~) =

i,(tf)

0.1466,

= +0.7313,

values

define

been chosen because they


industrial
controller
RCM
to Kempkens
[1990].
ACM

Transactions

on Mathematical

= o,

= o,

(&(tf) = o,
&(tf) = o.

a wide-ranging

can be compared
with
and another
suboptimal
Software,

(4.3)

t~:
el(to)

t93(t~)= 1.8929,
boundary

+ T].

(31(tO) = +0.2286,

e2(tf)

These

(4.2)

ti) + G(o)

are given

vector

as follows:

overhead

motion.

They

have

test results
obtained
for an
path-planning
method due

Vol. 20, No. 3, September

1994

TOMP Fortran Modules for Optimal Control Calculations


All state variables
as well as all
the entire interval
to < t < t~:
2.97

the

3.0

< ii(t)< +3.0,

2.01<

f32(t)

< +2.01,

1.5

< i2(t)< +1.5,

2.86<

/33(t)
< +2.86,

5.2

< &Jt)

< +945,
index

1890<Tz(t)

counts

< +1890,

from

the

basis

to the

exterior.

The

robot

carries

a load

moments

values

from

the following

Brandl

the final

(4.3) is only

time

time

avoids

are given

in

takes

15.

reasons

written

the inversion

by the user as the actual

it is the interface

Angles

L in kg, which

~ = tf is to be minimized.

for formal

et al. [1986]

L<

the descriptor
form (4. 1) directly.
The state equations
are implemented
be given

< +540,

domain:
0<

AS cost function

within

< +5.2,

540<7~(t)

in Nm.

and

from

are constrained

< +2.97,

radian

Equation

variables

< 61(t)

945<T1(t)
where

control

273

in the following

subroutine

to the robot

explicitly.

of the mass

model

The

matrix,

procedure

for the formal

algorithm
and treats
which

is to

FIHS. By the same

R3M2SI.

SUBROUTINE ROBOT (t, Z, DZ, U, IU)


DOUBLE PRECISION t, Z(6), DZ(6), U(*)
DOUBLE PRECISION LOAD, Torque (3)
INTEGER
IU(*)
COMMON /CLOAD / LOAD
DOIO, I= 1,3
10DZ(I)=Z(I
+3)
CALL CONTRL (t, U, IU, Torque)
CALL R3M2SI (LOAD, Z, DZ, Torque, DZ(4))
RETURN
END
4.2.2

Test Results.

A few results

of the

simulation

are given

in Figure

for the robot the states of which are not constrained


and in Figure
5 for the
state-constrained
robot. An extensive
study is included
in Hartl
[1991].
The minimal
final time of tf= 0.599 seconds for the robot the states of
which
are unconstrained
is determined
by the maximal
motor moments
Tz
and T~, the latter
controlling
the robot in a bang-bang-type
structure.
This
behavior
can be expected,
as the controls
T enter the differential
equations
(4.1) linearly
[Bryson
and Ho 1969,
angular
velocities
of the corresponding
a long arc of their trajectories.
The minimal
final time of tf = 0.848

pp. 110 T117].


joints
19zand
seconds

Note that the maximal


19sare dissatisfied
along

for the state-constrained

robot

is determined
by the maximal
angular
velocity
of the third
joint
19~. This
constraint
is active within
the time interval
t ~ [0.2, O.7] while outside
this
interval
the corresponding
motor
moment
T~ is at its boundary.
Here the
angular
acceleration
is maximal.
ACM

Transactions

on Mathematical

Software,

Vol. 20, No. 3, September

1994.

274

D. Kraft
Joint

Angles
1

0-

------- .-.

..- ...-

--------

Motor
,
,
/
/
,
!
/

2000
1000

-------------

1 -

1000

... +
--- . . . . . 4,,
!/

-/
I

Moments
I
,
,
,
\
,

/
,,

2000 ---
o

5,

0.2

0.4

Joint
I

0.6

0.2

0.4

Velocities
I
line type I angle I a,velocity
solid

5
lo 1

_15L___J
o

0.4

0.2
Fig. 4.

States

Joint

2,

and controls

for the unconstrained

Angles

T,

robot

r3,

L = O kg

Motor Moments

1000
500

0
500

-1000
1

0.5

o ....................

I moment

81

b,

0.6

-It
2

0.5

m
line type

angle

a.velocity

moment

t
-e~

0.6

0.5

Fig 5. States and controls

1
for the constrained

robot

3, L = Okg,

ACM Transactions on Mathematical Software, Vol 20, No. 3, September 1994

TOMP Fortran Modules for Optimal Control Calculations


Table II.

275

Minimum Final Times for Different Robot Control

m
Comparing
strates
ably

these

drastic

raise

results

time

gains

with

the productivity

The given examples


in a highly
constrained

those

of Kempkens

of modern
demonstrate
environment

values

APPENDIX
The user
the

have

A. USER-SUPPLIED

brachistochrone

OBRACH.

three

problems
control

[Hartl

A.1

Main

SLSQP

of collision
avoidance
in
problems
with
multipoint

1991].

of Section

subroutine

may

RHS,

VALUES,

4.1 are named

contain

user does not want to plot other trajectories


which are automatically
stored in dTOMP.

A possible

consider-

SUBROUTINES
subroutines:

example

The last

II demon-

could

that the problem


of robot path planning
can be very efficiently
solved by optimal

to be solved

has to write

in Table

which

manipulators.

control
calculations.
Included
also are
multirobot
configurations
where
optimal
boundary

[1990]

of even 1009% by optimization

OUTPUT,
BRACH,

no executable
than

which

statements,

the states

in

VBRACH,
if the

and the controls,

Program
structure

of a main

and dTOMP

according

program
to Figure

with

reverse

1 is shown

communication

between

in the following

program

section.
PROGRAMTOMP
DECLARATIONS
EXTERNAL RK54 , BRACH, VBRACH, OBRACH

c
c
c

INITIAL

ESTIMATE OF CONTROLPARAMETERIZATION ~IX , X3 ETC.

10 cALL D_TOMP (RK54, BRACH, VBRACH, OBRAcH, IX, x,


*
Z, ZO, Zl, TOL, F, C, DF, DC, LC)
c
c
c
c
c

usER iNTEILFACE

*
c
c
c
c
c

TC,

CALL SLSQP (M, MEQ, LA, N, X, XL, XU, F, C, G, A,


ACC, ITER, MODE, W, L_W, JW, L_JW)
IF (ABs(MoDE) .EQ. I) Go TO 10

FINAL OUTPUT,

etc,

END
ACM

Transactions

on Mathematical

Software,

Vol

20, No 3, September

1994.

276
A.2

D. Krati

Right-Hand

Sides

of Differential

Constraints

sumouTIw
BRAcH (T,Z,DZ,U,IU)
THIS SUBROUTINE COMPUTESTHE VALUES OF THE RIGHT HAND SIDES
FOR THE BRACHISTOCHRONE
IT IS CALLED FROM SUBROUTINE SLV WHICH IS CALLED FROM D.TOMP

c
c
c

c
c
c
c
c
c
c
c

INPUT :
.T TIME, DOUBLE PRECISION SCALAR
Z STATE, DOUBLE PRECISION VECTOR
U CONTROL, DOUBLE PRECISION VECTOR
IU CONTROL, INTEGER VECTOR
THE PAIR {U,IU]
REPRESENTSTHE CONTROLPARAMETERIZATION
FOR FURTHER INFORMATION LOOK AT THE PARAMETERSOF D_TOMP

c
c
c

OUTPUT:
.--DZ STATE DERIVATIVE,
INTEGER
DOUBLE PRECISION
DOUBLE PRECISION
DATA

DOUBLE PRECISION VECTOR

IU(20)
T, Z(*),
DZ(*),
U(*)
G, THETA(l),
TF
G/i.ODO/

NC=IU(O1)
NP=IU(13)
TF=U(NC+NP)
CALL CONTRL(T,U,IU,THETA)
DZ(1)=TF*Z(3)*COS(THETA(1))
DZ(2)=TF*Z(3)*SIN(THETA(1)

DZ(3)=TF*G*SIN(THETA(1))
END

A.3

Cost

Function

and Constraints

suBRouTINE VBRACH (RHS,Iu,T,u,Tc,z,z0,2i,w,F,


C,MODE)
THIS SUBROUTINE COMPUTESTHE VALUES OF THE COST FUNCTION

CllNSTRAIUTS

c
c

VBRACH

ACM

MAY

TransactIons

IS

ALSO

FOR

THE

CALLED

AS

BE

CALLED

on Mathematical

BRACHISTOCHRONE
SUBROUTINE
IN

THE

Software,

USER

Vol

VALUES

IN

D_TOMP

INTERFACE

20, No, 3, September

1994

AND

TOMP Fortran Modules for Optimal Control Calculations


A.3 Cost
c
c
c
c
c
c
c
c
c
c
c
c
c
c
c

Function

277

and ConstraintsCorrtirruecf

INPUT :
.--RHS SUBROUTINE IN WHICH THE RIGHT HAND SIDES OF THE DIFFERENTIAL
CONSTRAINTS ARE CALCULATED (NOT USED IN OUR CASE)
T TIME, DOUBLE PRECISION SCALAR
TC TIME GRID FOR COMMUNICATION, DOUBLE PRECISION VECTOR
Z STATE, DOUBLE PRECISION VECTOR
U CONTROL, DOUBLE PRECISION VECTOR
IU CONTROL, INTEGER VECTOR
THE PAIR {U,IU}
REPRESENTSTHE CONTROLPARAMETERIZATION
FOR FURTHER INFORMATION LOOK AT THE PARAMETERSOF D.TOMP
MODE INDICATES STATUS OF INTEGRATION, INTEGER
MODE=O: SET INITIAL
CONDITIONS
MODE=NT: EVALUATE BOUNDARYVALUES
O<MODE<NT: EVALUATE STATE AND CONTROLCONSTRAINTS

c
c
c
c

OUTPUT:
-

c
c
c

F COST FUNCTION, DOUBLE PRECISION SCALAR


C CONSTRAINTS, DOUBLE PRECISION VECTOR
EQUALITY CONSTRAINTS (BOUNDARYVALUES) MUST BE SET FIRST

c
c
c

INNPUT/OUTPUT :
------------

ZO INITIAL
STATE, DOUBLE PRECISION VECTOR
ZI FINAL STATE, DOUBLE PRECISION VECTOR

W WORKING ARRAY, DOUBLE PRECISION VECTOR

INTEGER
DOUBLE PRECISION
EXTERNAL
SAVE
IF

(MODE .Eq.

ACM

IU(20),MODE
U(*),T,TC(*),ZO(*)
RHS
J

,Z1(*),Z(*),W(*),F,C(*)

0) THEN

Transactions

on Mathematical

Software,

Vol. 20, No. 3, September

1994.

278

D. Kraft

A.3

Cost Function

and ConstraintsCofttinuecf

C SET SOME PARAMETERS


NC=IU(O1)
NP=IU(13)
NT=IU(15)
NZ=IU(16)
M =IU(17)
ME=l
J=ME

C BOUNDARYVALUES
ZO(I)=O.ODO
ZO(2)=0.ODO
ZO(3)=0.ODO
ZI(I)=l.ODO
ZI(2)=0.ODO
Z1(3)=0.ODO
END IF

C STATE CONSTRAINTS
IF

(M.NE.ME) THEN
IF (MODE.GT.i .AND. MODE.LT.NT)

THEN

J=J+l

C(J)=O.2DO+0.4DO*Z(1)-Z(2)
END IF
END IF

C COST AND FINAL STATES


IF

(MODE.EQ.NT)

THEN

F=U(NC+NP)
C(i)=z(l)-zl(l)

END IF
END

ACM

Transactions

on Mathematical

Software,

Vol. 20, No 3. September

1994

TOMP Fortran Modules for Optimal Control Calculations


A.4

Output

279

Routine

SUBROUTINE OBRACH(T, INDEX, G, LONG,NO, Z, U,IU)


c

OUTPUT ROUTINE IS CALLED AS SUBROUTINE OUTPUT FROM D.TOMP

c
c
c
c
c
c
c

INPUT :
----T TIME, DOUBLE PRECISION SCALAR
K INTEGRATION INDEX SET BY D_TOMP BEGINNING FROM 1,
K IS SET TO K+i, EACH TIME OBRACHIS CALLED, INTEGEh
G REAL ARRAY OF DIMENSION (LONG, *)
STORES COLUMNWISESTATES AND CONTROLS

c
c
c
c
c

Z STATE, DOUBLE PRECISION VECTOR


U CONTROL, DOUBLE PRECISION VECTOR
IU CONTROL, INTEGER VECTOR
THE PAIR {U,IU}
REPRESENTSTHE CONTROLPARAMETERIZATION
FOR FURTHER INFORMATION LOOK AT THE PARAMETERSOF D.TOMP

c
c
c
c

OUTPUT:

G THE USER MAY ADD NO-(NU+NZ) COLUMNSTO G


AND NZ=IU(16)
WHERENU=IU(l)
INTEGER
K,LONG,N0,1U(20)
DOUBLE PRECISION T,Z(*),U(*),THETA(l)
REAL
G(LONG,*)

c
c
C USERS OUTPUT, E.G.
*
CALL CONTRL(T,U,IU,THETA)
*
WRITE(*, (8(1PD1O.2)))
T,
c
c
END

(Z(I),I=1,3),

THETA(i)

REFERENCES
ASCHER, U., CHRISTIANSON, J.) AND RUSSEL, R.D.

1981.

Collocation

software

for boundary-value

2 (June), 209222.
BRAN~L,H., JOHANNI,R., ANDOTTER,M. 1986. Avery efficient algorithm for the simulation of
of
robots and similar multibody systems without inversion of the mass matrix. In Proceedings
odes.

ACM

Trans.

Math.

IFAC/IFIP/IMACS
BRUSCH,

R.

problems

G.,
using

Sofku.

7,

International
AND SCHAPELLE,
nonlinear

BRYSON, A. E., mmHo,Y.

Symposium

R. H.

programming.
C. 1969.

1973.

on Theory
Solution

AL44J.

Applied

ofRobots.

of highly

constrained

optimal

control

11,135-136.

Optimal

Control.

Ginnand

Company, Waltham,

Mass.
BULIRSCH, R. 1971.
Die Mehrzielmethode
problemen
und Aufgaben
der optimalen

zurnumerischen
Losungvon
nichtlinearen
Steuerung.
Report
of Carl-Cranz-Gesellschaft.

RandwertOber-

pfaffenhofen
ACM

Transactions

on Mathematical

Software,

Vol. 20, No. 3, September

1994

280

D. Kraft

CRAIG+, J.J,

1989.

Robottcs-Mechanics

FEHLBERC+, E, 1969,
welten-Kontrolle,

Runge-Kutta

Computzng4,

93106,

GILL, P. E., MuRRAY,


Fortran

program

and

~assische

Control.

Addison-Wesley,

Formeln

ftinfter

W., PICKEN, S. M., AND WRIGHT, M.H.


library

foroptimizatlon

ACM

1979.

Trcms.

Package

Laboratory,

for

Stanford

Nonlinear

Umv,,

Programming.

Stanford,

Report

Softw,

GRIMM,

W.,

BERGER,

BNDSCO
HARTL,

1991,

within

Engineering,

OBERLE,

work

Mimchen,

IVurnericcd

1985,

Guide

83-12,

Linear

Benutzeranleitung

optimaler

and

space.

for SOL/NPSOL:

Systems

Optimization

Algebra

and

OptirntzatZon

fur

RechenproWamm

Steuerungsprobleme.

graphical

Diploma

evaluation

Thesis,

of the

das

DFVLR-Mitt.

performance

Fachhochschule

85-05,

of the

Miinchen,

Dept.

Koln.

Manutec

r3

of Mechanical

Germany

HORN, M. K. 1989,

Solution

8th IFAC

Workshop

In the

H.-J,

beschrankter

Numerical

its

ofa

Mass.

E.,

zur Losung

M.

robot

Reading,

andstructure

Calif

GILL, P. E., MURRAY, W., AND WRIGHT, M. H, 1991


Addison-Wesley,

Schritt-

5, 259-283.

Users

SOL

Mass

Ordnungmit

Thedesign

Math,

GILL, P E., MURRAY, W,, SAUNDERS, M., AND WRIGHT, M, H. 1983.


A Fortran

Reading,

undsiebter

of the optimal
on Control

control

problem

Appllcatlons

using

the software

of Nonlinear

package

Programnung

and

STOMP.
Optlmlz.a-

tlon.
KAMM, J. L., AND JOHNSON, I. L. 1973.
lifting

vehicle

KELLER,

H.

ascent

B.

1976.

Philadelphia,

Nonlinear

Automat~ca

Numerical

programming

approach

for optimizing

two-stage

9, 713-720.

SolutZon

of

Two

Point

Boundary

Value

Problems.

SIAM,

Pa,

K. 1990.

KEMPKENS,

Robotersysteme
by

Optimierung

der

Verfahrparameter

von

Industrleroboter-Steuerungen

6, 145-529

KONZELMANNN,
robots

to orbit.

J , BOCK, H, G., AND LONGMAN,

direct

methods,

In

R, W,

Proceechngs

1989.

of the

Time

AMA

optimal

GuLdance,

trajectories

Naurgatlon

of elbow

and

Control

Conference.
KRAFT,

D.

1991

TOMPFORTRAN

schrittsbericht

D.

KR.WT,

Koln,
KRAFT,

Nr

1988.

for

ag, Dusseldorf,

A Software

Package

Nonhnear

system

Optimal

Control

Calculations.

VDI-Fort-

Germany.

for Sequential

Quadratic

Programming.

DFVLR-FB

88-28,

Germany
D,

1987,

Bulirsch,

A.

Sciences,

95

Mlele,

C,

LENTINI,

K,

value

M.,

R.

problems

88-13,

with

J.

K.

Notes

In
in

into

Opttmal

Control

nonlinear

Schittkowski,

Solumg

S.
Koln,

An adaptive

1988

boundary

Control,

and

R.

Information

Least

programming

Ed. Springer,

Squares

problems.

Berhn,

Problems

Prentice-Hall,

The

optlmaler

POWELL, M. J. D. 1978.
In Numerical
Analysis,

finite

difference

layers.

DFVLR

SIAM

Models

solver

for nonlinear

J. Numer,

1 and

2 of

Anal
the

two

point

14, 91-111.

Manutec

r3

Robot,

Germany.

PFINFFER, F., AND REITHMEXER, E. 1987.


Theorte

problems

m ing.

1974.

mild

PONTRJAGIN, L. S., BOLTJANSKIJ,


matische

collocation.

Lecture

N.J.

AND TURK,

DFVLR-Mitt.

dmect

Eds.

control

Program

M., AND PEREYRA, V. 1977.

boundary

by

Well,

opt]mal

AND HANSON,

Cliffs,

analysm
H,

Berlin.

Mathematical

L,,

Englewood

Steer,

On converting

In Computational
LAWSON,

J.

Springer,

KRAFT, D. 1985.

OTTER,

Modules

254, VDI-Verl

Roboterdvnamzh.

V. G., GAMKRELIDZE,

Prozesse.

Oldenbourgh,

A fast algorithm
G, A, Watson,

Teubner,

Stuttgart,

R. V , AND MISCENKO,
Mtinchen,

E. F, 1964.

Mathe-

Germany.

for nonlinearly
constrained
optimization
Ed, Lecture
Notes m Mathematics,
vol.

calculations.
630, Springer,

Berhn.
PRINCE,

P. J , AND DORMAND,

Compat.

Appl.

SCHITTKOWSKI,
ming

calculator.

ACM

K, 1986.

problems.

SCHO~F, R.,
Basel,

Math.
Ann

NLPQL:
Oper.

AND DEUFLHARD,
In

J.

R.

1981.

High

order

embedded

Runge-Kutta

J,

A Fortran

subroutine

solving

constrained

nonlinear

program-

Res. 5, 485500.
P.

ComputatmnaZ

1994.

OCCAL,

OptLmal

Control,

a mixed

symbolic-numerical

R. Bulirsch

and

Kraft,

Switzerland.

TransactIons

formulae,

7, 67-75.

on Mathematical

Software,

Vol. 20, No. 3, September

1994

optimal
Eds

control

Birkhauser,

TOMP Fortran Modules for Optimal Control Calculations


SHAMPINE, L. F., AND WATTS, H. A. 1976.
Runge-Kutta

methods.

VON STRYK, O. 1993.


Optimal

Control

Sandia
Numerical

and

Variational

Practical

Laboratories
solution

solution

Report
of optimal

Calculus,

of ordinary

SAND
control

76-0585,
problems

differential

equations

Albuquerque,

N, Mex.

by direct

collocation.

281
by
In

R. Bulirsch, A. Miele, J. Steer, and K. H. Well, Eds.

Birkhauser, Basel, Germany.


VONSTRYK,O., ANDBULIRSCH,R. 1992. Direct and indirect methods for trajectory optimization.
Ann.

Oper.

Res. 37, 357-373.

TABAII, D., AND Kuo,


Englewood

Received

Cliffs,

December

B.C.

1971.

Optzmal

Control

by Mathematical

Progranlming.

Prentice-Hall,

N.J.

1991;

revised

ACM

June

Transactions

1993;

accepted

on Mathematical

June

1993

Software,

Vol. 20, No. 3, September

1994

Вам также может понравиться