Distance estimation and collision prediction for on-line robotic motion planning

NASA-CR-192737

IECHNICAU REPORTS
.UNIVERSITY COLLECTION

TBHH1CAL REPORTS

/537S7

W --W.

■% ? . ■-, *• t, 4: I

23 ^±4 4JK .<* A** -kJ 1

^

O

HI

■-»
<\

I

m

z

V)

TO

y

c

3

ITl

r-

00^

2 O

O 2:

u

2

Z
•-< <
Q -J

uu a.

ui a z
u o

z z *«

< O K
»- M O ■
(O </) x

c

u

Q -J U

o

l

U I

o

O
QC

LU

z

c
w

o
a

w

V

TO

<

z

I

z
a

O

m

Center for Intelligent
Robotic Systems
for Space Exploration

Rensselaer Polytechnic Institute
Troy, New York 12180-3590

00

DISTANCE ESTIMATION AND

COLLISION PREDICTION FOR

ON-LINE ROBOTIC MOTION

PLANNING

by
K.J. Kyriakopoulos and G.N. Saridis

Rensselaer Polytechnic Institute

Electrical, Computer, and Systems Engineering

Troy, New York 12180-3590

January 1991
CIRSSE REPORT #25

02J91

DISTANCE ESTIMATION AND COLLISION PREDICTION
FOR ON-LINE ROBOTIC MOTION PLANNING

K.J.Kyriakopoulos and G.N.Saridis

NASA CIRSSE

Rensselaer Polytechnic Institute

Troy, NY 12180-3590, USA

Abstract

An efficient method for computing the minimum distance and predicting colli-
sions between moving objects is presented. This problem has been incorporated
[Kyriakopoulos 90b, 91] in the framework of an in-line motion planning algorithm to
satisfy collision avoidance between a robot and moving objects modeled as convex
polyhedra. In the beginning the deterministic problem, where the information about
the objects is assumed to be certain is examined. If instead of the Euclidean norm,
L\ or Zoo norms are used to represent distance, the problem becomes a linear pro-
gramming problem. The stochastic problem is formulated, where the uncertainty is
induced by sensing and the unknown dynamics of the moving obstacles. Two prob-
lems are considered: First, filtering of the minimum distance between the robot and
the moving object, at the present time. Second, prediction of the minimum distance
in the future, in order to predict possible collisions with the moving obstacles and
estimate the collision time.

Keywords: Collision Avoidance, Collision Prediction, Distance Functions, Ran-
dom Search.

1 INTRODUCTION

The trajectory planning problem for navigation of mobile robots or manipulation of
robotic arms has been traditionally treated for environments of stationary or deter-
ministically moving objects [Brooks 86, Chen 88, Kant 88, Khatib 85, Kyriakopoulos
88, Lozano-Perez 87, Lumelsky 87, Wu 88]. This is done because in order to satisfy
task constraints and optimize over certain criteria, all the available apriori informa-
tion has to be utilized, and therefore most of the planning has to have a global

1

character.

However, robots should be able to perform their tasks efficiently, not obstructed
by and not obstructing other moving robots, vehicles or mechanisms with which they
have loose communication. Therefore, local decision making strategies should be
included in the menu of the robotic motion planning algorithms.

The difference between the global and local motion planning strategies is mainly
found at the amount of processed information about the task and the environment
and at the considered time length of the plan. Both of these measures are significantly
larger for the global case which is bound to be off-line under the state of the art of
hardware.

An on-line strategy should then decrease the amount of information processing.
Therefore it should be based first on the already processed information which has ob-
tained the form of an off-line plan, and second on some additional local measurements
and estimates of measures related to the obstructing moving objects. Such measures
have been proposed [Kyriakopoulos 90, 90b, 91] to be the minimum distance be-
tween the robot and the moving obstacles, and the expected collision time under
the current plan.

The calculation of both the minimum distance and the expected collision time
has been investigated in the past [Canny 86, Gilbert 87, Gilbert 89, Kyriakopoulos
89] but in the case where full information about the description of the objects and
their motion is available. Unfortunately, this is not the case in realistic environments
where the information is based on measurements by a sensing system and the motion
of the objects is not known but just observed. The uncertainty introduced by both
factors should be included in the calculation procedures.

In this paper, the major contribution is the treatment of uncertainty introduced
by both the sensing process and the unknown attitude of the moving obstacles.The
distance estimation problem is posed as a filtering problem. Finally, the issue of colli-
sion time prediction is covered in depth. Such a process is computationally expensive.
If the L\ or L^ are used, then the distance problem is linear programming as opposed
to quadratic for the case of the Euclidean norm. Linear programming for problems
of moderate complexity, such as the one at hands, performs considerably better than
quadratic.

A statement of the problem in the deterministic case is given in section 2 in order
to introduce the framework under which analysis is going to be performed. Section 3
contains the problem formulation of the stochastic case and a proposed solution for

the minimum distance estimation. The collision time prediction is given in Section
4, while some example case with simulation results is presented in Section 5. Finally,
Section 6 includes comments on the obtained results, and some discussion on issues
that could constitute topics of future research.

2 DISTANCE FUNCTIONS - THE DETERMIN-
ISTIC CASE

2.1 Definitions

The distance between two objects at the time instant t is defined [Gilbert 85] as

d(t) = min{||s,- - zj\\ : z,- € Ki(t), Zj € Kj{t)} (2.1)

where Ki(t), Kj(t) are compact sets of cartesian points representing the two objects.
Based on the above definition the necessary and sufficient condition so that no
collision occurs between the two objects would be

d° - d(t) < Vt

where d° > is a safety constant.

Sets Ki(t),Kj(t) are described by an equation of the form

K(t) = R(t).C + {T(t)} (2.2)

where

C C 3ft 3 , is a compact set and describes the shape of the object

R(t) € 9R 3 * 3 , is a rotation matrix

T{t) £ $1? is a translation vector.

The basic assumption adopted in this work [Kyriakopoulos 89] is that set C rep-
resenting the shape of the object is a convex polyhedron described by

C = {x/x€S8 3 9 A-i<6, A € » mx3 , b € » m }. (2.3)

A point x € 9R 3 after it is rotated and translated by R(t) and T{t) respectively, it
comes to a new point x' where

x' = R{t) ■ x + T{t).

Therefore K(t) since it comes from rotation and translation of points x € C can be
described as

K{t) = {x/x € 3F 3 A ■ R~ l (t) ■ x < b - A ■ Rr\t) ■ T{t)}.

2.2 Representations of distance

Based on definitions (2.1) and (2.3) and on equation (2.2), the problem to find the
minimum distance between two solids

C r = {x/x € ft 3 3 A r ■ x < b T , A r e £ mx3 , b r e » m }

C = {y/y€^BA -y<b , A, € # x3 ,&„ € »'}.

representing the robot and an obstacle respectively, is recasted to a mathematical
programming problem of the form:

d(t) = min x , y ||a'-j/||

s.t A r {t) • x(t) < b r (t) (2.4)

A {t) ■ y(t) < b {t)

where

b r (t) = b r -A r .R-^t).T r (t)

A (t) = A -R;\t) { - }

b (t) = b -A R; l (t).T (t)

and Rr(t),T r (t), R (t),T (t) are the rotation and translation matrices for the robot
and the obstacle describing their current configuration. In view of (2.4) and (2.5),
the distance between the solids C r and C can be viewed as a function of their
configuration [Gilbert 85] described as a quadruple P = (/2„(t),r <> (i),i2 r (<),T r (<)):

d = d{P) : V = 3^ x3 x ft 3 x 3J 3 * 3 x K 3 -+ £ + (2.6)

and therefore a domain V + of d can be defined as:

V + = {P e V : d(P) > 0} (2.7)

The norm used in mathematical program (2.4) was not explicitly defined here. It
can be any norm because norms in 3? n are equivalent. Although the Euclidean norm
is the most natural to our intuition, other norms such as || • ||i and || ■ ||oo can also

be used because of their computational efficiency. This comes from the fact that the
computation of the Euclidean norm requires the solution of a quadratic programming
problem, while this of || • ||i and || • ||oo requires solution of linear programming prob-
lems. An analysis of the above issues is presented in appendix A [Kyriakopoulos 89].
In the analysis to follow, the minimum distance is going to be associated with no
specific norm. In case that the kind of norm is significant then it will be indicated
with the form of a subscript e.g di,d 2 , d^.

3 DISTANCE FUNCTIONS - THE STOCHAS-
TIC CASE

3.1 Sources of uncertainty

From (2.4) and (2.5) becomes clear that the computation of the distance between
two objects requires accurate knowledge of the parameters describing the shape of
the objects C and C T and these describing the configuration of them as represented
by the quadruple P. Accurate knowledge of the solid models C and C r , and of the
configuration parameters Rr(t),T r (t) of the robot is assumed here. The uncertain
information considered here is the information about the configuration Ro(t),T (t) of
the moving obstacle C . Future work is going to incorporate uncertainties in a more
general fashion.

The uncertainty of the kinematic information of the moving obstacle C is partially
due to the way that it is extracted. For example, in a measurement process based
on a vision system the uncertainty is mainly introduced by two kinds of noises: first,
the image intensity noise from the environment, and second, the quantization noise
of the image array of the camera.

The most eminent source of kinematic uncertainty is the lack of knowledge about
the "attitude" of the moving obstacle. In other words the lack of knowledge about the
time evolution of its cartesian position and orientation and their first and second order
time derivatives. The assumption made here is continuity of position and orientation
and their velocities. The accelerations were assumed to be constant in time during
every sampling interval and their magnitude is going to be estimated.

3.2 Estimation of kinematic parameters

Naturally, the assumptions made about the models of uncertainty are necessary in
order to enable the creation of models of motion of the moving object, and the sensing
process. The models presented here are for the two dimensional case. Future work
is going to incorporate the full 3-D case which adds complexity just to the formulas.
On the other hand, the 2-D case perfectly serves the navigation problem considered
here.

The equations of motion of the moving obstacle are :

Px = V x py = V y 6 = Vg

v x = a x v y = a y vg = ag (3.1)

a x = a y = ag =

where

Px,Py,Pe define the cartesian position and orientation in 2-D

v x ,v y ,vg are the translational and rotational velocities and

a x ,a y , ag are the translational and rotational accelerations.

The above equations constitute a set of state equations, and can be written in a

matrix form:

X = A ■ X (3.2)

where X = \p x v x a x p y v y a y 9 vg ag],A 6 5R 9 * 9 .

The measurement model is described by an equation of the form:

z = h(X) + v (3.3)

where z 6 3? m is a vector of parameters of several geometric features of the object.
These parameters are extracted by the sensing process (e.g vision). The dependency
of z on X is given by the, nonlinear in general, function h(X). The corrupting noise v
is assumed to be white and Gaussian, and independent of X, TJ*e assumption about
the Gaussian property of the noise although not accurate, is attributed to the Central
Limit Theorem, because the noise under consideration is as an aggregation of many
independent sources of noise [Durrant 88]. The mean and the covariance matrix of
noise are respectively

E{v) =

C(v{, vj) = E(vi ■ vj) = Ri • Sij.

A variation of the Kalman filter proposed [Athans et.al 68] may well be used to
get a close to optimal estimate X of state vector X. The selection of this specific
filter was done because it is a second order filter which normally gives better results
in terms of bias errors. The utilization of Kalman filter provides, at every instant U :

• X(ti/U), the optimal estimate of state X based on measurements up to and
including i,-,and

• P(ti/ti) the error covariance matrix.

The prediction equations for the future, based on the information up to and in-
cluding £,-, are

X(tlU) = e A ^-X{UlU) t>U

P(t/U) = A ■ P{UlU) + P{UlU) -A T
The above Ricatti equation does not induce any kind of computational difficulty
because it is homogeneous and can be solved analytically.

3.3 Bounds of distance functions

From (2.6), it becomes evident the fact that the dependency of d on quadruple P =
{Ro(t), T (t), Rr (*), T r (t)) implies its dependence on the motion vector X. This comes
from the fact that R {i) and T Q (t) explicitly depend on X. In fact, in the 2-D case:

R.{t) = R (d(t)) =

cos6 —sinO
sinO cosd

(3.5)

T (t)=T ( Px (t),p y (t)) =

(3.6)

Px

Py

Assuming that R T (t) and T T {t) are well known at t , the distance d becomes a function
of the random variable A - , or :

d = d(X)

and as a matter of fact d is a random variable, too. From the discussion at the end
of section 2.2 it becomes obvious that d(X) is not given in a closed form, but it is
an outcome of a mathematical programming stage. Additional investigation [Clarke
83, Gilbert et.al 85] has shown that d although uniformly Lipschitz continuous, is
not differentiable everywhere on its domain V + defined by (2.7). However, V + can
be viewed as a subset of W 1 and by Rademachers theorem [Clarke 83], function

d(X) is differentiable almost every where (a.e), i.e except on a subset of points with
Lebesgue measure zero. From the geometry of the problem, we can easily see that
the set of points of nondifferentiability is not dense on SR* and therefore there exist
open neighborhoods where d(X) is differentiable everywhere and analytic. In the
discussion to follow, this is going to be mathematically more evident.

As a matter of fact, function d(X) can be approximated by a Taylor expansion
up to the second order (due to the Gaussian noise) around the expected value X of
X:

T dd(X) 1

dX + 2 {X X} dX>

d{X) * d(X) + (X-X)
Taking expected values in both sides of (3.7),

(X - X) (3.7)

) • tr(P(t))

(3.8)

where A min (-) denotes the minimum eigenvalue of a matrix.

In order to make possible the utilization of (3.8), the Hessian of d(X) has to be
explicitly found, in order to get the eigenvalues in closed form. This would be more
efficient for an on-line distance estimation scheme.

3.4 Derivatives of distance functions

The analysis that follows considers the 2-D case and is consistent with the develop-
ment of [Gilbert et.al 85]. The 3-D extension is straightforward.
Define r) a : V + x C x C T -f R + by:

T) a (P, W , W r ) = ||i?„ • tU„ + p - Rr ■ W r - p r \

where a defines the kind of used norm. Let

Case a = 2 : In this case

and

V x ^(P,^,^r) = (— ,0,0,-*, 0.0

12 V2

L y

= R ■ IU + p - Rr • W r - p r

T h (P,w ,w r ) = (Ll + Ll) 1/2

L * n

n L V n n ~ M * -L x + M y -L y

V2

,0,0) a

(3.9)

(3.10)

where

Figure 1: Singular Configuration

M x = w x ■ sind + w v • cos9
M x = w* ■ cosO — w v • sinB

(3.11)
(3.12)

the Hessian can be found by further differentiation and is given in appendix B with
its eigenvalues.

Based on [Clarke 75, Gilbert et.al 85] it can be proved that in the case that P is
nonsingular i.e when

W(P) = {(w ,w r ) eC xC T : T, 2 (P,w ,w r ) = d(P)}

is singleton, then

v x d(X) = v xn2 (p,w; 1 w;)

where

( w ;,w' T )eW(P).

A typical case of singular configuration is demonstrated on figure 1.
Case fl = I-' In this case

■q x (P, w ,w T ) = |Z, r | + |L y |

(3.13)

and

Vxm(P, v ,u>r) = (^n(£ r ),0,0,^n(I y ),0,0,|£-,0,0) T (3.14)

where

^ = -(w x sgn(L x ) + vo»sgn(L y )) ■ sinO + {w x sgn(L y ) - v%sgn(L x )) • cos6 (3.15)

Similarly, the Hessian is very easy to obtain and is given in appendix B with its
eigenvalues.

In appendix C the following theorem is proved:
Theorem 3.1 :If

d x {P) = minfllc, - zjlU : z,- € #<(/>),*,- € Kj(P)}

77i(P, W , W r ) = \\R ■ W + p - Rr ■ W r - PrWi

and

W(P) ± {(w ,w r ) €C xC T : m (P,w ,w r ) = d^P)}

is singleton, then

v x d(x) = v xni (p,w' ,w;)

where

(w;,w' r )€W(P).

Case a = oo : In this case

T} 00 {P,w ,w r ) = max{\L x \,\L v \} (3.16)

and

^XVoo(P,W ,W r ) = <

iT

if J7oo(P, iu , iw r ) = \L X \

T

if 7?oo(P, U>0, U>r) = \Ly\

(3.17)

where

sgn(L x ), 0,0, 0,0.0, ^-,0,0
0,0,0, 3 gn(L y ),0.Q,2&,Q,Q

^oo _ J sgn(L x ) ■ {-w x cos - w v sin0} if ^(P, w , uv) = |£*| , 3 lg v

d0 1 sgn(Ly) ■ {w x cos 9 — w y sin 0} if ^(P, iy , u; r ) = \L X \

The Hessian is given in appendix B with its eigenvalues.

In appendix C the following corollary is proved using the theorem 3.1
Corollary 3.2 :If

<UP) = min{||s,- - ^|U : z t € #,(P), Zj € Kj(P)}

10

L = * = R ■ W + p - Rr • W T - p r

Tioo{P,w ,w r ) = HZHoo = max{|Z x |,|X,|}

and

W(L) = argmm{\L x \,\L y \}
W(P) ± {(w ,w T ) £C xC T : n<X) {P,w ,w T ) = <4o(J>)}
are singleton, with W(I) = {t} and W(P) = {«,<)} thea

3.5 Distance Estimation Algorithm

In summary, the steps of the distance estimation (prediction) algorithm are given:

• KALMAN FILTER provides X(t/ti), P{t/ti) t > *,- ( equation (3.4) )

• from a mathematical programming stage d^X) (or d\(X)) is obtained ( equa-
tion (2.4) )

• from one of (B.1),(B.2),(B.3) A min (^0±) is obtained.and

• from (3.8) a lower bound for E(d(X) ) is obtained.

4 PREDICTION OF COLLISION TIME

The prediction of collision time is going to be based on two kinds of information.
First, the future motion of the robot which is preplanned and therefore accurately
known. Second, the motion of the moving obstacle, as can be "optimally" predicted
by (3.4). It is worthwhile noticing the fact that distance, as a function of time, is
not necessarily differentiate. Additionally, the future time-evolution of d(t) is not
convex. Finally, we are not interested just in a local minimum of d(t), but in the first
time instant t c that d(t) becomes zero, or mathematically,

f c = inf {(*(*) = 0} (4.1)

where f, is the present time, and T is the time horizon of interest.

11

'■J

The characteristics of the above problem do not permit the use of stochastic
approximation methods. Additionally some methods that have recently appeared,
[Gilbert 89] although they give accurately the collision time based on the considered
information, they seem to have computational problems that do not enable their use
in on-line schemes. For those reasons, another method, the accelerated expanding
subinterval random search (AESRS) [Saridis 77] was utilized with modifications
to accommodate the problem. The convergence properties of this method have well
been investigated, and its performance experimentally verified. The basic idea of
AESRS is the following:

Obtain the lower bound d(t { ) from (3.8) and set d min = d(U). Split the interval
ft = [ti, ti + T] of concern in JV subintervals ft; C ft such that ft, n ft, = 0, i ^ j .
Define a probability density function (pdf ) p(g) over ft by:

N N

pie) = E = 0(*)p*tefc), Qk € ft*, E & = l ( 4 - 2 )

where each Pk(ek) is defined only on ft*. The pdf's Pk(Qk) can be chosen to be any
distribution, but in this work were chosen to have uniform distribution for practical
reasons. Initially fi(k) = (3 (k) = jf-

From (4.2) a random time instant t r € ft is obtained, and from (3.8) a lower
bound d(t r ) is obtained for the distance. If d(t T ) < d min then it is assumed to be a
"successful" iteration. The weights /?,•(&) are updated by a stochastic approximation
reinforcement algorithm, based on the frequency of "successful" iterations in the
particular k — th bin,

d(tr) {

f 0i(h) = fc_i(*) + 7*[1 - ft-i(*)]

- m,n ^ I m = *.--i(i) + 7.-A-iC;) j ¥> * (4-3)

, > d min =► 0i(k) = fc-xik) k = 1, .., AT
In the above equations 7; and 0o{k) must satisfy Dvoretzky's conditions for conver-

gence

Jim 7i = 0, Jim E 1i = °°t Jim E % ? < °°> IIA>( fc )ll < 00

t— OO T-T 1—00 . ,

therefore 7,- = 7 and j3 {k) = jj.

The definition of t c in (4.1) suggests that the following amendments to the above
algorithm should be done to achieve better performance.

A) Consider figure 2. The present time is U = 2sec and the time horizon of
interest is 7\ = 6sec. A histogram of JV = 12 bins represents pk{Qk), while the dashed

12

20
18
16

1 14.
* 12-

■a.

ID-
S'
6-
4-
2-

8

I

.s

~\-

3!

1

■a.

4 5

time (sec)

Figure 2: <f(t/t» = 2) and probability histogram for ft = [2, 8]

20

18

16

14
12
10
8
6
4
2

•V

4 5

time (sec)

I

Figure 3: d(t/ti = 2) and probability histogram for ft = [2, 7]

13

1

.9

■o

20

18

16

14

12

10

8

6

4

2

- [•

! 1 1

• N _ : !

}

-4--

—

— f-

I

V J

j

■j r-

\ : \

, {

\ :

V

, —

\

i

t -
» ■

1

t
1
1

;

i

\

1 ;
(
»
r
t

*

*^

/

4 5

time (sec)

Figure 4: d(t/ti = 2) and probability histogram for H = [2,6] at time t = 3~.

line is d(t/ti = 2) t £ [2,U + 7*]. The lower bound of the predicted distance d(t/U)
is computed using (3.8) not everywhere but only at random instants t T produced by
Pk(Qk)-

Assume that a trial time instant t r = 7 sec is randomly produced by the probability
of (4.2). Then d(t r /ti) = 0. Then a candidate collision time £ c = t r = 7 has been
found and the search interval Q is modified to Cl — [i,- = 2, i c = 7]. Then the new
search interval is being divided in N subintervals and therefore a finer grid is obtained
(figure 3). Since the grid becomes finer and finer the uniform distribution for Pk{Sk)
is satisfactory. The shape of the distribution of /3(k) k = 1, .., N in the new interval,
is similar to this of the previous distribution providing the search with "memory".

B) Consider figure 4. The present time is t = 3", all the information processing
has been done based on sensing info at t, = 2sec and the time horizon of interest is
Th, = 4sec. A histogram of N = 8 bins represents Pk{Qk), while the dashed line is
d(tlU = 2)te [2,t t + T h ].

When a new measurement d(t i+ i = 3) arrives (figure 5), the distribution of fi(k)
for the bins that available from the previous search (i.e [3,6] in figure 4), is not
changed because due to the Lipschitz continuity of d(-) w.r.t. <f>, small changes of
4>{tilti) produce, bounded changes of d(t r /ti). The distribution over [<,-,i l+ i] (i.e of

14

20
18
16
14

a n

I

10-
8
6
4
2

! 1 1 — 1 ; ! 1 !

" \ I ~™-J»-»*»» J - r . ;

! ! T

■ : ; ;

-. \

■- *

I | |

" ! i "

//

*

4 5

Time (sec)

Figure 5: d(t/ti = 2) (•••), d(t/ti = 3) ( ) and probability histogram for fi = [3, 7]

at time t = 3 + .

time interval [2,3] in figure 4) is uniformly distributed along the new extension (i.e
time interval [6,7] in figure 5).

In this search d\ was used, as measure of distance, because it is computationally
faster, while it gives the same collision time as </ 2 -

Finally an issue of interest is the selection of the time horizon T of search. It
should be based on a number of criteria depending on safety and performance issues.
In this work a reasonable and safe selection was considered:

T = n
where n is a positive integer.

V =

d(t i+l ) - d(ti)\

5 SIMULATION RESULTS

The case study considers a mobile robot endowed with a vision and motion control
system and a moving obstacle (figure 6). The vision system "traces" the edges of
the moving polyhedron representing the obstacle, and the parameters of their math-
ematical description, constituted the measurement vector z of equation (3.3). The

15

Figure 6: Environment with a mobile robot and a moving obstacle

measurement noise v was assumed to be white, Gaussian, with zero mean and diag-
onal covariance matrix with diagonal elements equal to 0.1.

The Kalman filter output behaved well especially in the straight line motions. The
lower bounds of E(di(X)) calculated by (3.8), are plotted in figure 7(a), while the
error is plotted in figure 7(b). As expected, the lower bound underestimates and is
close enough to the actual distance. The maximum absolute error is 7cm in a range
of actual distances of 20m.

The collision time was calculated using the modified AESRS that was outlined in
the previous chapter. As a metric, d\{t) was used. Therefore using a SUN 3-150 we
were able to perform 30 iterations/sec. At time instants t = 4, 5, 6 sec the collision
time was found to be t c = 7, 6.8, 6.69 sec respectively, while the actual collision time
is t c = 6.63 sec.

6 DISCUSSION - FUTURE RESEARCH

A theoretical analysis of the distance estimation and collision time prediction prob-
lems between moving polyhedra was presented along with simulation results. The

16

i

20
18
16

14
12
10

8

6

4-

2-

3 4 5

Time (sec)

0.01

Figure 7: (a) Lower Bound of E{d 2 (X(t))) (b) error E{d 2 (X{t))) - d 2 {X{t))

IT

whole analysis was based on an uncertainty model including sensing and attitude un-
certainty. The computational efficiency resulting from the use of L\ and £„, metrics
and the heuristic amendments to AESRS was verified with a simulation study. Future
research should include application of the above results in real time systems. Such
an effort is currently in progress in the NASA-CIRSSE Laboratories of RPI.

The above analysis should be extended for the case of nonconvex objects that can
be decomposed to a number of convex ones. Further investigation is also needed for
the calculation of the collision time. Although the modified AESRS performed well, it
really makes big use of CPU. Therefore, either some parallelizable scheme of AESRS
should be developed.

Decision schemes for collision avoidance, using the information calculated here in
the framework described in the introduction have been reported [Kyriakopoulos 90b,
91]. Further applications are under development and results are going to be reported
in the near future.

APPENDIX A

In (2.4) the distance computation problem between two convex polyhedra was
stated as the mathematical programming problem:

d = min r , y \\x - y\\ a
s.t A T • x < b r
A y <b

where integer a defines the kind of sought distance, x, y € R 3 , Ar € & mx3 ,6r € 3£ m ,
A € 3£'* 3 , b 6 3?'. Integers m,l are the numbers of the sides of the polyhedra
representing the robot and the obstacle respectively.

If we set z - [x T y T ] T , L = [bj b*} T , D = [I 3x3 -hx^l {I is the identity matrix),

A r mx 3

0/ x3 A,

then the above problem is written in the standard mathematical programming form

d = min- \\D • z

M =

(A.l)
s.t M z<L

Mathematical programming problem (A.l) is equivalent to

d = min z z T • Q • z
s.t M ■:< L

18

with Q = D T • D € SB 6 " 6 , and becomes a quadratic programming problem. Q is
positive semidefinite. Therefore more than one minimizing vectors may exist, with
all of them corresponding to the same minimum.

a = 1

If a new vector of unknowns w € 3R 3 is defined

w { = \Di • -| = |x,- - yi\

with Di the i-th row of D, then the mathematical programming problem (A.l) is
equivalent to

d = min z , w £?=i w i
s.t Wi>D,-z i = 1,2,3
Wi>-Di-z i = 1,2,3
M-z <L
and becomes a linear programming problem of 9 variables and m + 1 + 6 constraints.
More than one minimizing vectors may exist, with all of them corresponding to the
same minimum.
a = oo
If a new unknown w € 3? is introduced, such that

w = max \Di • z\ = max |x,- — yA

with Di the i-th row of D, then the mathematical programming problem (A.l) is
equivalent to

d — min z ,u, w
s.t w>Di-z z = 1,2,3
w > -Di ■ z i = 1,2,3
M-z<L
and becomes a linear programming problem of 7 variables and m + 1 + 6 constraints.
More than one minimizing vectors may exist, with all of them corresponding to the
same minimum.
APPENDIX B

In this appendix, the derivation of the eigenvalues of the Hessian of the distance
function is presented for two cases of norms.
a = 2 : If G = M x L y + M y L x then direct differentiation of (3.10) gives:

d 2 V2(X) _ 1

L — Li x Ly — LyCr

-L x L y Ll L X G

-L y G L X G G{G - ril)

19

Setting T = -Gr}$ and B = rt\{G - 1) - G 2 ,

,aV*h _ -l?-(i? 2 -4*r)*

(5-1)

q = 1 : Direct differentiation of (3.13) in conjunction with (3.15) gives:

d 2 Vi(X)

dX 2

%fr\

where

^L = -( w * o sgn{L x ) + wlsgn(L y )) ■ cos9 - (w* sgn(L v ) - w»sgn(L x )) ■ sin9

and therefore

'Pmm

av

a = oo : Direct differentiation of (3.16) in conjunction with (3.18) gives:

Aoo(*)

dX 2

a 2

where

de 2

and therefore

sgn(L x ) ■ {w x sin 9 - u>» cos 9} Voo(P, ^o, u>r) = \L X \
sgn(Ly) ■ {-u;*sin0 - u;Jcos0} ^(P,^,^) = |I*|

d 2 rU*h _,_,„ d 2 ^

A m ,„( Qg 2

-)=rm„{0,— -}

(5.2)

(£.3)

APPENDIX C

Some definitions, theorems and lemmas that are going to be utilized in the proof
of the theorem are presented here. Most of those results are based on [Clarke75]
[Clarke83] [Gilb85].

Consider / : O -*■ 3ft, be locally Lipschitz, where O C 3ft n . We say that / admits
a Frechet derivative V/ at x 6 O if

,. f{x + 8x)-f(x)

lim

5—0

11**1

=<Vf,6x >

20

where <,> denotes the inner product. The usual (one-sided) Directional deriva-
tive of / at x in the direction v is

,,, , ,. /(.c + tv) - /(g)
/ (x; v) = hm

The Generalized gradient of / at x is

df(x) - co{w : V/(xj) -* w, Vf(xi) exists , Xi -* x}

where co denotes the convex hull. Finally the Generalized directional derivative
f°(x; v) is defined as:

f o< ^ v f(x + tv)-f(x)
f(x;v)= hm sup

y — ► x

i 1

Definition C-l: A function f(x) is said to be regular at x provided:
(i) /'(x; v) exists Vu.
(n)f(x ] v) = f°(x;v)Vv.

Proposition C-2: Let / be Lipschitz near x then / is regular at x.

Proof: The proof is given in [Clarke83] (proposition 2.3.6).|.

Proposition C-3: Let / : X -*■ ft, with X finite dimensional. Then df is upper
semicontinuous Vx € X.

Proof: The proof is given in [Clarke83]( proposition 2.1.5).|.

The following theorem is key to our development

Theorem C-4: Let U be a sequentially compact space, and let g : 3£ n x U — ► &
have the following properties:

(a) g(x,u) is upper semicontinuous in (x, u).

(b) g is locally Lipschitz in x, uniformly for u in U.

(c) g°{x,u; •) = g' x (x,u; •), the derivatives being w.r.t x.

(d) d x g(x,u) is upper semicontinuous in (x.u).
If we let f(x) = max u€U {g(x,u)} then :

(1) / is locally Lipschitz.
(2)-/'(x;u) exists.

(3) f'{x;v) = f°(x;v) = max{C • v : C € tf*<7(x,u),tx € M{x) }, where Af(x) = {u €

£/:<,(*,")=/(*)}■

(4) d/(x) = co{d r #(x, it) : u € A/(x)}. {co{-} denotes the convex hull of a set).
Proof: The proof is given in [Clarke"5]( Theorem 2.1) J.

21

Finally, the following proposition is the last piece of the proof under development.

Lemma C-5: The following are equivalent:
(a) df{x) = {C}, is a singleton, (b) V/(.r) exists, V/(z) = C, and V/ ia continuous
at x relative to the set upon it exists.

Proof: The proof is given in [Clarke75] (Proposition 1.13).|.

Lemma C-6: The following results are true for

!h(P t W , W r ) = \\R ■Wo+Pc-Rr-Wr- p r \\l

a.)T]i(P,w ,w r ) is continuous and convex on its domain.

b)i7i is Locally Lipschitz.

c)t)° p (P, w , w r ; •) = v' lP {P, w„ w r ; •)

d)5p7?i(P, w ,w r ) is upper-semi-continuous.

Proof: (a) and (b) can be easily derived from the definition of 171.

(c) From C-2 t)\ is regular, and from definition C-l QED.

(d) The domain of ^ can be viewed as a subset of R n . From proposition C-3 the
sought result is obvious. |.

Proof of Theorem 3.1 : Lemma C-6 in conjunction to theorem C-4 gives the fol-
lowing results for function

di(P) = min{T)i(P,w .u! r ) : w € C ,w r € C r }

l)di is locally Lipschitz.

2)di'{P] P') exists.

Z)d x \P\F) = min{< Vpr tl {P,w ,w r ),P' > {w ,w r ) 6 W(P)} where

W{P) = {(w ,w r ) eC xC r : m {P,w ,w r ) = d 1 {P)}

±)dd x {P) = co{< Vprj^P.w^WrlP' >. (u;,,u> r ) € W{P)}.

From lemma C-5 the following statements are shown to be equivalent:
ddx(P) = {£},singleton; Vd x (P) exists; Vd x (P) = £.

Result

When the configuration of C and C r is such that there is only one pair of points
(w},w*) minimizing di(P) (i.e when W[P) is singleton) then according to the above
results V P d x {P) = V^»h(P, w' , w' r ). Q.E.D*

Before continuing with the proof of corollary 3.2 the following lemma is proved:
Lemma C-7: Let f u f 2 : X -+ 3ft. with X finite dimensional. If /i,/ 2 are
continuous, convex and Lipschitz continuous then the following results are true for

•>9

function / : X —* 5R defined by

/(i) = raax{/ 1 (i),/ 2 (i)}

a)/ is continuous and convex on its domain,
b)/ is Locally Lipschitz.

<:)£(*;•)«£(*;•)

d)d x f(x) is upper- semi- continuous.

Proof:
(a) The coninuouity of / is obvious. Consider x € X such that x = A • xi + (1 — A) • x 2
where x 1? x 2 6 A". Then f(x) = f t (x) i € {1,2}, but since f, is convex,

/ (A ■ x x + (1 - A) • x 2 ) = /(x) = /,(*) < A • fi{ Xl ) + (1 - A) • /,(x 2 )

but since f(xj) > fi(xj) ij € {1,2},

/ (A • x x + (1 - A) • x 2 ) < A • /(xx) + (1 - A) • /(x 2 ).

Thus / is convex.

(b) Since /,- is Lipschitz then there exists constant c,- > such that

\Mx)-My)\<Ci\\x-y\\ x Vx,j/€X (C.l)

where || • ||x is the induced norm of space X. Assume that

/(*) = M*) (C-2)

f(y) = My) (C3)

if i = j then /,(j/) = /,(;/). Otherwise, fj(y) > fi{x) (from C.2). Therefore, in general

fi(v)-fj{v)<0 {CA)

Consider now,

/(*) - f(y) {C = ] M*) - f:(y) = /*(*) - My) + My) - Mv) ^ M x ) ~ Mv)

therefore

\f(x) - f(y)\ < \Mx) - My)\ iC < a\\x - y\\x i € {1, 2}

23

and

\f{x)-f(y)\<c\\x-y\\x <* = %*$*

therefore / is Lipschitz.

(c) & (d) are proved the same way as in lemma C-6 j.

Proof of Corollary 3.2 : We follow the same steps as in the theorem of Theorem
3.1.

From Lemma C-7, Theorem C-4 and Lemma C-5 we come to the conclusion that
when the configuration of C and C T is such that there is only one pair of points
{wl,w') minimizing d^P) (i.e when W(P) is singleton) then

v p <up) = v pnoo (P,w;, w ;). (c.5)

Similarly, when L(P, w" , w') = \L X L y ] T is such that only one of L s , L y (denote it
as f) maximizes tj(P) (i.e when W(£) is singleton) then

Vp7 7oo (P, w' , w' r ) = V P t{P, wl,w;). (C.6)

From (C.5,C6) the following result is obtained

v P d„(P) = v P r(P,w;,w;).

ACKNOWLEDGMENTS

This work was supported by NASA Grant NAGW-1333 1988. The first author
was partially supported by a fellowship of Alexander Onassis Foundation.

REFERENCES

M.Athans, R.Wishner, A.Bertolini,(1968) ^Suboptimal State Estimation for Continuous-
Time Nonlinear Systems from Discrete Noisy Measurements", IEEE Trans. Autom.
Contr., Vol. AC-13, No. 5, October 1968.

R.A.Brooks, (1986) "A Robust Layered Control System for a Mobile Robot", IEEE
Journ. of Robotics and Automation, Vol.RA 2, No. 1, March 1986
J.Canny, (1986) "Collision Detection for Moving Polyhedra ", IEEE-PAMI, Vol.
PAMI-8, 1986.

Y.C.Chen, M.Vidyasagar, (1988) "Optimal Trajectory Planning for Planar N-Link
Revolute Manipulators in the Presence of Obstacles", Proc IEEE 1988 Intern. Conf.
on Robotics and Automation.
F.H. Clarke, (1983) "Optimization and Nonsmooth Analysis". New York: Wiley,

24

1983.

F.H. Clarke, (1975) "Generalized gradients and applications", Trans. Amer. Math.
Soc, vol. 205,1975.

H.F. Durrant-Whyte, (1988) "Uncertain Geometry in Robotics", IEEE Joum. of
Robotics and Automation, Vol. RA 4, No.l., Febr. 1988

E.G.Gilbert, D.W.Johnson, (1985) "Distance Functions and their Application to
Robot Path Planning in the Presence of Obstacles", IEEE Journ. of Robotics and
Automation, Vol. RA 1, No.l., March 1985

E.G.Gilbert, D.W.Johnson and S.S.Keerthi (1988), "A Fast procedure for computing
the distance between complex objects in three-dimensional space", IEEE Journ. of
Robotics and Automation, Vol. RA 4,p.p. 193-203, March 1988
E.G. Gilbert, S.M.Hong, (1989) "A New Algorithm for Detecting the Collision of
Moving Objects", Proc. IEEE 1989 Intern. Conf. on Robotics and Automation.
K.Kant, S.Zucker, (1988) "Planning Collision- Free Trajectories in Time Varying En-
vironments: A Two Level Hierarchy", Proc. IEEE 1988 Intern. Conf. on Robotics
and Automation.

O.Khatib,(1985) "Real Time Obstacle Avoidance for Manipulators and Mobile Robots" ,
Proc. IEEE 1985 Intern. Conf. on Robotics and Automation.

K.J.Kyriakopoulos, G.N.Saridis, (1988) "Minimum Jerk Path Generation", Proc.
IEEE 1988 Intern. Conf. on Robotics and Automation.

K.J.Kyriakopoulos, G.N.Saridis, (1989) "An Efficient Minimum Distance and Colli-
sion Estimation Technique for On- Line Motion Planning of Robotic Manipulators",
NASA-CIRSSE Rept. No. 19, March 1989.

K.J.Kyriakopoulos (1990) "A Supervisory Control Strategy for Navigation of Mobile
Robots in Dynamic Environments", Ph.D. Thesis, February 1991, ECSE, RPI.
K.J.Kyriakopoulos, G.N.Saridis, (1990 b) "On- Line Motion Planning for Mobile Robots
in Non-stationary Environments", Proc. 1990 Intern. Symp. on Intelligent Control.
K.J.Kyriakopoulos, G.N.Saridis, (1991) "Collision Avoidance of Mobile Robots in
Nonstationary Environments", Proc. IEEE 1991 Intern. Conf. on Robotics and Au-
tomation.

T.Lozano-Perez, (1987) "A Simple Motion Planning Algorithm for General Robot
Manipulators", IEEE Journ. of Robotics and Automation, Vol. RA 3, No.3., June
1987

V.J.Lumelsky, (1987) "Effect of Kinematics on Motion Planning for Planar Robot
Arms Moving Amidst Unknown Obstacles'* , IEEE Journ. of Robotics and Automa-

25

tion, Vol. RA 3, June 1987

G.N.Saridis, (1977) "Expanding Subinterval Random Search for System Identifica-
tion and Control", IEEE Trans. Autom. Contr., pp 405-412, June 1977
C.Wu, C.Jou, (1988) "Design of a Controlled Spatial Curve Trajectory for Robot
Manipulators", Proc. of 27 Conf. on Decision and Control, Dec. 1988

26