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

## See other formats

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