Skip to main content

Full text of "Distance estimation and collision prediction for on-line robotic motion planning"

See other formats





W --W. 

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

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
















2 O 

O 2: 



•-< < 
Q -J 

uu a. 

ui a z 
u o 

z z *« 

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



Q -J U 



U I 
















Center for Intelligent 
Robotic Systems 
for Space Exploration 

Rensselaer Polytechnic Institute 
Troy, New York 12180-3590 






K.J. Kyriakopoulos and G.N. Saridis 

Rensselaer Polytechnic Institute 

Electrical, Computer, and Systems Engineering 

Troy, New York 12180-3590 

January 1991 



K.J.Kyriakopoulos and G.N.Saridis 


Rensselaer Polytechnic Institute 

Troy, NY 12180-3590, USA 


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. 


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 



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 

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 

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.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) 


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) 


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.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 

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 = 


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 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 


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




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 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 

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)) 


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 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 


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 


,0,0) a 




Figure 1: Singular Configuration 

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


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

Based on [Clarke 75, Gilbert 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;) 


( 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 | 



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


^ = -(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 

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 


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;) 


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

Case a = oo : In this case 

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


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


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


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



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)} 


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

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


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. 


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. 



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- 


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 



1 14. 
* 12- 










4 5 

time (sec) 

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






4 5 

time (sec) 


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















- [• 

! 1 1 

• N _ : ! 




— f- 


V J 


■j r- 

\ : \ 

, { 

\ : 


, — 



t - 
» ■ 






1 ; 




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 

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 



a n 



! 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)\ 


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 


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. 


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









3 4 5 

Time (sec) 


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


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. 


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 = 

s.t M z<L 

Mathematical programming problem (A.l) is equivalent to 

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


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 
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. 

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) 


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

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


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

d 2 Vi(X) 

dX 2 



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

and therefore 



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


dX 2 

a 2 


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,— -} 




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) 




=<Vf,6x > 


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. 


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) = £. 


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 


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. 

(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) 


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



\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 

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;). 


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


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, 



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- 

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 

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


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