Академический Документы
Профессиональный Документы
Культура Документы
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,
TransactIons
on Mathematical
Software,
Vol
20, No
3, September
[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
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
(1.6)
7...7P,
i=l
O,
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,
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
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)
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
1994
1993].
value
problem
of TOMP.
input
and
The software
(OCP);
2. DIRECT
value
first
represent
2.1
therefore
they
problem
explicit
step
into
Control
solution,
is
described
material
algorithms
test
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
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
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
of the interval
each breakpoint
which
the
A summary
A, is defined
(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)
parameters
if they
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
of the
and Watts
1976] are
methods
are summarespective
software
is
is less than
ra
are included
(2.7)
. . ..p.
to (1.8)
O,
i=l,
0,
2.2.2
which
differential
(1.6)
fined,
A,J.
Approximation
is represented
constraints
coinciding
722 +
trajectory
of simplified
nq + qm +
< is uniquely
p
de-
i(a),
b, {(b))
=f(x)
(2.10)
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
= b}
1994
(2.13)
on a communi(2.14)
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
Transactions
on Mathematical
model
by the adjoint
equations
eliminated
because of the
is changed,
Software,
Vol
which
is very
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
compensated
of Section
functions,
is solved
TOMP
SLSQP
by a large
flexibility.
2.2.1 is integrated
e.g., Equation
by sequential
linear
least
In d-TOMP
squares
the initial
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
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
(2.10,
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. 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
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
1994
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
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,
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
using
10_ ~.
t: = 2.0. The
been i 2 m, respectively,
in the unconstrained
TOMP
accuracy
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
of iterations
solution
expanded
entry
for
to
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
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
in
ACM
Transactmns
on Mathematical
Software,
three
arms
and three
1994
rotors
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.
272
D. Kraft
Table I.
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
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
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
1994
the
3.0
2.01<
f32(t)
< +2.01,
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
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
algorithm
and treats
which
is to
R3M2SI.
Test Results.
A few results
of the
simulation
are given
in Figure
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,
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
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
1
for the constrained
robot
3, L = Okg,
275
m
Comparing
strates
ably
these
drastic
raise
results
time
gains
with
the productivity
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,
contain
A possible
consider-
SUBROUTINES
subroutines:
example
The last
II demon-
could
to be solved
has to write
in Table
which
manipulators.
control
calculations.
Included
also are
multirobot
configurations
where
optimal
boundary
[1990]
OUTPUT,
BRACH,
no executable
than
which
statements,
the states
in
VBRACH,
if the
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
usER iNTEILFACE
*
c
c
c
c
c
TC,
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
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
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
1994
AND
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
c
c
c
INNPUT/OUTPUT :
------------
ZO INITIAL
STATE, DOUBLE PRECISION VECTOR
ZI FINAL STATE, 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,
1994.
278
D. Kraft
A.3
Cost Function
and ConstraintsCofttinuecf
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
(MODE.EQ.NT)
THEN
F=U(NC+NP)
C(i)=z(l)-zl(l)
END IF
END
ACM
Transactions
on Mathematical
Software,
1994
Output
279
Routine
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
c
c
c
c
OUTPUT:
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
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,
1994
280
D. Kraft
CRAIG+, J.J,
1989.
Robottcs-Mechanics
FEHLBERC+, E, 1969,
welten-Kontrolle,
Runge-Kutta
Computzng4,
93106,
program
and
~assische
Control.
Addison-Wesley,
Formeln
ftinfter
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
Schritt-
5, 259-283.
Users
SOL
Mass
Ordnungmit
Thedesign
Math,
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.
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.
problems
m ing.
1974.
mild
collocation.
Lecture
N.J.
AND TURK,
DFVLR-Mitt.
dmect
Eds.
control
Program
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,
1994
optimal
Eds
control
Birkhauser,
methods.
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
Oper.
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,
1994