http://ijr.sagepub.com
Research
The International Journal of Robotics
DOI: 10.1177/027836498500400308
1985; 4; 109 The International Journal of Robotics Research
Anthony A. Maciejewski and Charles A. Klein
Environments
Obstacle Avoidance for Kinematically Redundant Manipulators in Dynamically Varying
http://ijr.sagepub.com/cgi/content/abstract/4/3/109
The online version of this article can be found at:
Published by:
http://www.sagepublications.com
On behalf of:
Multimedia Archives
can be found at:The International Journal of Robotics Research Additional services and information for
http://ijr.sagepub.com/cgi/alerts Email Alerts:
http://ijr.sagepub.com/subscriptions Subscriptions:
http://www.sagepub.com/journalsReprints.navReprints:
http://www.sagepub.com/journalsPermissions.navPermissions:
© 1985 SAGE Publications. All rights reserved. Not for commercial use or unauthorized distribution.
at SAGE Publications on January 14, 2008 http://ijr.sagepub.comDownloaded from
109
Obstacle
Avoidance
for
Kinematically
Redundant
Manipulators
in
Dynamically
Varying
Environments
Anthony
A.
Maciejewski
Charles
A.
Klein
Department
of
Electrical
Engineering
The
Ohio
State
University
2015
Neil
Avenue
Columbus,
Ohio
43210
Abstract
The
vast
majority
of
work
to
date
concerned
with
obstacle
avoidance for
manipulators
has
dealt
with
task
descriptions
in
the form
ofpick-and-place
movements.
The
added flexibil-
ity
in
motion
control for
manipulators
possessing
redundant
degrees
offreedom
permits
the
consideration
of obstacle
avoidance
in
the
context
of
a
specified
end-effector
trajectory
as
the
task
description.
Such
a
task
definition
is
a
more
accurate
model for
such
tasks
as
spray
painting
or
arc
weld-
ing.
The
approach
presented
here
is
to
determine
the re-
quired joint
angle
rates for
the
manipulator
under
the
con-
straints
of
multiple
goals,
the
primary
goal
described
by
the
specified
end-effector
trajectory
and
secondary
goals
describ-
ing
the
obstacle
avoidance
criteria.
The
decomposition
of the
solution
into
a
particular
and
a
homogeneous
component
effectively
illustrates
the
priority
of the
multiple
goals
that
is
exact
end-effector
control
with
redundant
degrees
of freedom
maximizing
the
distance
to
obstacles.
An
efficient
numerical
implementation
of the
technique
permits
sufficiently
fast
cycle
times
to
deal
with
dynamic
environments.
1.
Introduction
Kinematically
redundant
manipulators
can
be
defined
as
manipulators
that
possess
more
than
the
required
six
degrees
of
freedom
to
arbitrarily
position
and
orient
their
end-effectors
in
space.
These
extra,
so-called
redundant
degrees
of
freedom
result
in
greater
dexter-
ity
and
flexibility
in
the
specification
of
motion
for
the
manipulator.
This
paper
presents
a
formulation
that
allows
the
use
of
these
redundant
degrees
of
freedom
so
that
a
manipulator
can
avoid
obstacles
in
the
work-
place
while
completing
a
specified
task.
Obstacles
are
defined
as
any
portion
of
an
object
with
which
contact
is
undesirable.
Clearly,
obstacle
avoidance
is
essential
for
satisfac-
tory
task
completion.
Current
manipulator
applica-
tions
typically
involve
removal
of
potential
obstacles
from
the
manipulator’s
workspace
and
the
use
of
fixed
motion
commands.
While
preventing
manipulator
collisions,
this
method
is
unduly
restrictive
because
of
its
inability
to
deal
with
unpredictable
or
dynamic
environments.
The
satisfaction
of
the
obstacle
avoid-
ance
criteria
for
such
environments
would
greatly
increase
the
autonomy
of
the
manipulator
and
result
in
a
wider
range
of
applications
that
could
benefit
from
the
advantages
of
automation.
This
motivation
has
resulted
in
a
number
of
various
approaches
to
the
resolution
of
the
obstacle
avoidance
problem
(Pieper
1968;
Loeff and
Soni
1975;
Udupa
1977;
Khatib
and
LeMaitre
1980;
Lozano-P6rez
1981 ).
A
basic
premise
applied
to
the
development
of
vir-
tually
all
of
these
approaches
has
been
that
the
task
can
be
defined
in
terms
of
a
known
initial
and
final
con-
figuration
for
the
end-effector.
Any
set
of
joint
mo-
tions
that
attains
the
goal
configuration
without
colli-
sion
is
considered
satisfactory
completion.
While
this
method
of
task
definition
is
well
suited
for
many
ap-
plications,
particularly
of
the
pick-and-place
variety,
it
© 1985 SAGE Publications. All rights reserved. Not for commercial use or unauthorized distribution.
at SAGE Publications on January 14, 2008 http://ijr.sagepub.comDownloaded from
110
does
not
characterize
those
tasks
that
require
a
speci-
fied
end-effector
trajectory
throughout
the
task,
such
as
spray
painting
or
arc
welding.
Implementation
of
these
obstacle
avoidance
schemes
for
such
tasks,
there-
fore,
is
at
best
awkward.
Since
most
tasks
can
be
de-
scribed
in
terms
of
desired
trajectories
for
a
workpiece,
the
basic
task
definition
used
here
will
be
that
of
a
specified
end-effector
velocity
(Paul
1979).
2.
Theory
The
end-effector
velocity,
described
by
the
six-dimen-
sional
vector
X,
is
related
to
the
joint
velocities,
de-
noted
by
the
n-dimensional
vector
0,
where n
is
the
number
of
degrees
of
freedom,
by
the
equation
where
J
is
the
Jacobian
matrix
(Paul
I 981 ).
In
the
case
of
redundant
manipulators,
the
inverse
of
the
Jaco-
bian
is
not
defined
because
J
is
rectangular.
Useful
so-
lutions
to
Eq.
(1),
however,
can
be
obtained
through
the
use
of
generalized
inverses
(Boullion
and
Odell
1971 ),
one
of
which
can
be
defined
as
the
best
approx-
imate
solution.
Definition.
The
vector
60
is
a
best
approximate
solution
of Eq.
(1)
if
for
all
vectors 0
either
where
II
II
denotes
the
Euclidean
norm.
The
best
approximate
solution
is
given
by
the
pseudo-
inverse
(Penrose
1956)
and
is
denoted
here
by
J+.
It
can
be
shown
(Greville
1959)
that
the
general
so-
lution
to
Eq.
(1)
is
given
by
where
I
is
an n
X ~t
identity
matrix
and
z
is
an
arbitrary
vector
in
0-space.
Thus
the
resultant
joint
angle
rates
can
be
decomposed
into
a
combination
of
the
least-
squares
solution
of
minimum
norm
(Whitney
1972)
plus
a
homogeneous
solution
created
by
the
action
of
a
projection
operator
(I -
J+J),
which
describes
the
redundancy
of
the
system,
mapping
an
arbitrary
B
into
the
null
space
of
the
transformation.
By
applying
var-
ious
functions
of 0
to
compute
the
vector
z,
reconfig-
uration
of
the
manipulator
can
be
obtained
to
achieve
some
desirable
secondary
criterion
under
the
con-
straint
of
the
specified
end-effector
velocity
(Liegeois
1977;
Ribble
1982;
Klein
and
Huang
1983;
Trevelyan,
Kovesi, and Ong
1984).
Recently,
Yoshikawa
(~1984)
has
implemented
an
obstacle
avoidance
scheme
in
which
the
vector
z
is
a
specified
velocity
toward
a
safe-i.e.,
collision-free-
joint
space
vector
that
is
predetermined
with
a
priori
knowledge
of
the
workspace.
The
homogeneous
solu-
tion
is
thus
used
to
reconfigure
the
manipulator
to
be
as
close
as
possible
to
the
safe
vector
while
tracking
the
desired
trajectory.
In
the
case
of
dynamic
environ-
ments,
however,
no
universal
safe
joint
space
vector
exists,
and
the
current
state
of
the
environment
must
be
used
in
the
motion
planning
to
obtain
effective
obstacle
avoidance.
The
obstacle
avoidance
approach
presented
here
is
to
identify
for
each
period
in
time
the
point
on
the
manipulator
that
is
closest
to
an
obstacle,
termed
the
obstacle
avoidance
point,
and
assign
to
it
a
desired
velocity
component
in
a
direction
that
is
directly
away
from
the
obstacle
surface.
Thus
the
primary
goal
of
specified
end-effector
velocity
and
the
secondary
goal
of
obstacle
avoidance
(see
Fig.
1 )
are
described
by
the
equations
where
Je
=
end-effector
Jacobian,
Jo
=
obstacle
avoidance
point
Jacobian,
i,
=
specified
end-effector
velocity,
and
Xo
=
specified
obstacle
avoidance
point
velocity.
One
possible
way
to
find
a
common
solution
to
both
Eqs.
(3)
and
(4)
would
be
to
adjoin
the
two
© 1985 SAGE Publications. All rights reserved. Not for commercial use or unauthorized distribution.
at SAGE Publications on January 14, 2008 http://ijr.sagepub.comDownloaded from
111
Fig.
J.
Planar
manipulator
illustrating
the
primary
goal
of end-effector
velocity
speci-
fication
and
the
secondary
goal
of
maximizing
the
distance
to
the
obstacle.
matrices
and
the
right-hand
sides
into
a
single
matrix
equation:
If
the
new
matrix
has
the
rank
n,
then
mathematically
the
system
has
been
fully
specified
and
is
no
longer
redundant.
If
the
matrix
in
Eq.
(5)
is
not
square
or
of
full
rank,
then
obviously
its
pseudoinverse
could
be
applied
to
give
a
best
approximate
solution.
However,
for
this
application
it
is
not
desirable
to
treat
end-ef-
fector
and
obstacle
velocities
in
the
same
way.
Instead
a
method
is
described
that
first
satisfies
the
primary
command
of
end-effector
velocity
and
then
uses
the
system’s
redundancy
to
best
match
the
secondary
command
of
obstacle
velocity.
The
set
of
solutions
to
exactly
satisfy
the
primary
goal
can
be
given
in
the
form
of
Eq.
(2).
Substituting
this
solution
into
Eq.
(4)
yields
which
can
now
be
solved
for
the
desired
homogeneous
solution.
Since
the
degree
of
available
redundancy
may
not
be
sufficient
to
exactly
achieve
the
secondary
goal,
a
solution
that
increases
the
minimum
obstacle
distance
is
provided
by
the
pseudoinverse,
given
by
This
equation
is
identical
to
the
one
obtained
by
Han-
afusa,
Yoshikawa,
and
Nakamura
{ 1981 )
for
two
goals
of
different
priority.
This
result
is
substituted
back
into
Eq.
(2)
to
provide
the
desired
solution
to
both
goals
under
the
constraints
imposed
by
the
number
of
available
degrees
of
freedom,
yielding
This
solution
can
be
simplified
to
since
the
projection
operator
is
both
hermitian
and
idempotent.
This
result
is
proved
in
the
Appendix.
Each
of
the
terms
in
the
preceding
formulation
has
an
easily
visualized
physical
interpretation.
As
stated
previously,
the
pseudoinverse
solution
(Je Xe)
of
the
desired
end-effector
trajectory
composes
the
particular
portion
of
the
solution
that
in
the
redundant
case
guarantees
the
exact
desired
end-effector
velocity
with
the
minimum
joint
velocity
norm.
The
added
homo-
geneous
solution
sacrifices
the
minimum
norm
solu-
tion
to
satisfy
a
different
goal,
that
of
obstacle
avoid-
ance.
The
matrix
composed
of
the
obstacle
Jacobian
times
the
projection
operator,
Jo(I -
JeJe},
represents
the
degrees
of
freedom
available
to
move
the
obstacle
point
while
creating
no
motion
at
the
end-effector.
This
matrix
is
used
to
transform
the
desired
obstacle
point
motion
from
Cartesian
obstacle
velocity
space
into
the
best
available
solution
in
joint
velocity
space
again
through
the
use
of
the
pseudoinverse.
The
vector
describing
the
desired
obstacle
point
motion
is
com-
posed
of
the
commanded
motion,
i.,
obtained
from
environmental
information,
modified
by
subtracting
the
motion
caused
at
the
obstacle
point
due
to
satis-
faction
of
the
end-effector
velocity
constraint
(JaJe~c~).
3.
Implementation
I
The
preceding
obstacle
avoidance
scheme
has
been
implemented
in
both
two-dimensional
and
fully
gen-
© 1985 SAGE Publications. All rights reserved. Not for commercial use or unauthorized distribution.
at SAGE Publications on January 14, 2008 http://ijr.sagepub.comDownloaded from
112
eral
three-dimensional
computer
graphics
simulations
on
a
PDP
11/70
computer
provided
with
a
Hewlett-
Packard
1350
vector
graphics
display.
The
primary
goal,
a
specified
end-effector
trajectory,
is
entered
either
through
a
file
or
interactively
with
a
joystick.
The
secondary
goal
information,
obstacle
avoidance
point
and
velocity,
is
determined
from
a
mathematical
description
of
the
world.
In
a
physical
implementa-
tion,
the
secondary
goal
information
would
be
ob-
tained
from
sensory
range-finding
devices
(Espiau
and
Andre
1984).
Details
of
the
implementation
can
be
obtained
from
Maciejewski
(1984).
In
implementing
Eq.
(9)
several
types
of
decisions
must
be
made,
one
of
which
involves
the
rank
of
the
matrices
for
which
the
pseudoinverse
is
computed.
One
of
the
aesthetic
attractions
of
pseudoinverse
for-
mulations
is
that
they
are
valid
independently
of
the
rank
of
the matrix.
If
a
matrix
has
full
rank,
then
the
pseudoinverse
yields
finite
values
as
weights
for
the
input
singular
vectors.
If
a
matrix
becomes
singular,
then
the
pseudoinverse
yields
a
zero
coefficient
for
the
direction
of
the
input
singular
vector
in
the
null
space.
A
simple
example
demonstrates
that
these
two
cases
cannot
blend
smoothly
into
each
other.
The
matrix
A
is
its
own
pseudoinverse:
However,
the
matrix
B
has
a
pseudoinverse
that
di-
verges
from
A~:
as
the
parameter
E
makes
B
approach
A.
Therefore,
the
main
problem
in
a
pseudoinverse
formulation
is
not
at
a
singularity
when
the
pseudoinverse
formula-
tion
assigns
a
zero
component
in
the
missing
degree
of
freedom
but
near
a
singularity
where
a
very
large
com-
ponent
may
be
applied.
The
solution
involves
evaluat-
ing
the
effective
rank
of
B
and
treating
it
as
singular
whenever
it
would
yield
unacceptably
larger
answers
(Noble
1975).
Since
this
is
used
in
a
real-time
con-
troller
for
a
physical
system,
limits
must
be
based
on
physical
speed
limitations
rather
than
loss
of
signifi-
cance
as
would
apply
in
a
numerical
analysis
situation.
However,
independent
of
the
value
of
the
threshold
of
rank,
there
will
be
a
discontinuity
when
the
change
of
rank
is
noted.
If
assumptions
regarding
the
rank
of
a
matrix
can
be
made,
then
the
computation
of
the
pseudoinverse
can
be
simplified.
Unless
the
specified
task
is
unachiev-
able,
the
end-effector
Jacobian
is
of
full
rank,
and
its
pseudoinverse
need
not
be
explicitly
calculated.
The
approach
used
here
is
to
solve,
by
using
Gaussian
elimination,
for
fl,
a
six-element
vector,
and
y,
a
6
X r~
matrix,
in
the
equation
where
*
denotes
the
complex
conjugate
transpose.
It
is
then
easily
shown
(Klein
and
Huang
1983)
that
the
desired
quantities
J+i
and
Je Je
are
obtained
from
The
elements
of
the
Jacobians
are
efficiently
computed
by
using
the
screw
axis
formulation
(Waldron
1982).
On
the
other
hand,
the
3
X
n
matrix
[Jo(I
-
Je J~)]
cannot
always
be
assumed
to
be
of
full
rank,
and
therefore
the
Gaussian
elimination
approach
used
above
is
not
applicable.
In
this
case
the
pseudoinverse
is
explicitly
calculated
using
the
recursive
method
presented
by
Greville
(1960).
Within
this
algorithm
one
implicitly
makes
decisions
regarding
the
rank
in
terms
of
evaluating
the
linear
independence
of
matrix
columns.
Therefore,
a
sufficiently
large
threshold
must
be
applied
to
limit
high
velocities
near
singularities.
Although
theoretically
this
technique
works
as
well
on
a
matrix
as
on
its
transpose,
numerically
it
is
very
important
to
transpose
this
matrix
so
that
Greville’s
algorithm
is
applied
to
an n
X
3 - order
matrix
(n
>
3).
Since
this
algorithm
operates
on
columns,
the
correct
decision
for
rank
is
much
more
easily
applied
to
3,
rather
than
n,
columns.
Proper
decision
making
regarding
rank
is
especially
important
since
the
system
tends
to
move
such
that
either
the
obstacle
point
cannot
move
any
farther
without
changing
the
end-effector
or
the
system
finds
a
new
point
to
become
the
obstacle
point.
Besides
© 1985 SAGE Publications. All rights reserved. Not for commercial use or unauthorized distribution.
at SAGE Publications on January 14, 2008 http://ijr.sagepub.comDownloaded from