RSD-TR-7-84 DISTANCE FUNCTIONS AND THEIR APPLICATION TO ROBOT PATH PLANNING IN THE PRESENCE OF OBSTACLES r ~.''.' *... Elmer G. Gilbert D'aniel. W. Johnson Department of Electrical Engineering and Computer Science The University of Michigan Ann Arbor, Michigan 48109-1109 September 1984 CENTER FOR ROBOTICS AND INTEGRATED MANUFACTURING Robot Systems Division COLLEGE OF ENGINEERING TIE UNIVERSITY OF MICHIGAN U i VV?,:r - ANN ARBOR, MICHIGAN 48109-1109 /j3 ip X:,:,,',, ~~ J'k y

,57.

RSD-TR-7-84 TABLE OF CONTENTS 1. Introduction......................................................................................... 3 2. Path Planning as a Problem of Optimal Control................................ 6 3. Properties of Distance Functions......................................................... 12 4. Numerical Solution of the Optimal Control Problem.......................... 19 5. Numerical Solution of the Example..................................................... 25 6. Conclusion................................................................................... 32 7. Appendix............................................................................................. 32 8. References............................................................................................ 34 ii

RSD-TR-7-84 DISTANCE FUNCTIONS AND THEIR APPLICATION TO ROBOT PATH PLANNING IN THE PRESENCE OF OBSTACLES' ABSTRACT An approach to robotic path planning, which allows optimization of useful performance indices in the presence of obstacles, is given. The main idea is to express obstacle avoidance in terms of the distances between potentially colliding parts. Mathematical properties of the distance functions are studied and it is seen that various types of derivatives of the distance functions are easily characterized. The results lead to a general formulation of path planning problems and suggest numerical procedures for their solution. A simple numerical example involving a 3-degree of freedom cartesian manipulator is described.'This work was supported by the Air Force Office of Scientific Research, AF Systems Command. USAF under grant F 49620-82-C-0089. Distance Functions 2

RSD-TR-7-84 1. Introduction The generation of robotic motion to accomplish a given task is often broken down into two steps: path planning - the off-line determination of time functions which specify a suitable time dependence of mechanism configuration variables such as manipulator joint angles, path tracking - accurate on-line implementation of the motion through feedback control. Other types of motion control may occur too. Examples include the use of a vision-directed control system for part acquisition at the beginning of a motion, and force control in an assembly operation at the end of a motion. See [1, 2] for more details and references. This paper is concerned with the path planning problem. The standard approach is to specify a geometric path (lacking time evolution) as a series of segments in end-effector space. Then smooth, time-dependent functions are fitted to these segments in the space of configuration variables. A crucial aspect of such path planning methods is the implementation of physical constraints. The most important of these are limits on the configuration variables and actuator inputs, and obstacle avoidance. There is a rather large literature on the computation of shortest geometric paths to avoid obstacles (see, e.g., (1, 3. 4, 51 and the literature discussed in these references). But the research to date has limitations in treating multiple-member mechanisms and objects in three dimensional space. The determination of motions to meet actuator constraints is difficult Distance Functions 3

RSD-TR-7-84 because of the complexity and nonlinearity of the equations of motion. The result has been a rather conservative approach to the fitting of time functions to geometric paths [1, 2]. Some attempts have been made to optimize motion, but they are in one way or another limited. See, for example [1, 6, 7, 8]. A few examples may give a better understanding of these issues. Figure 1 shows the first, a crane or cartesian manipulator. The payload K1 is translated and rotated in the plane. There are limits on the translations ql(t) and q2(t) and K2,...,K7 are the obstacles. There is no limit on the rotation q3(t). The task is to move the payload from position A to position B and minimize the energy consumed in the move. Determining an admissible motion as a sequence of translations and rotations such as Po is not difficult. But determining a motion which meets the control constraints and minimizes the energy is. The second, more complex, example is the interaction of two manipulators in a common workspace. Again, there are objects K1,K2,...: the moving parts (links) of the manipulators and the fixed obstacles in the workspace. During the motion of two manipulators, interference of the objects, including collisions between the moving parts of the two manipulators, must be avoided. Minimization of a suitable performance index, say the time of travel to transfer a part from the gripper of one manipulator to the gripper of the other, would tend to bring the manipulators together. Thus collision avoidance is likely to be an active constraint. The constraint is especially complex because there may be 4 Distance Functions

RSD-TR-7-84 many pairs of objects which can collide. P R X 7 N^<st / -- -. —S Cl I I K6 2, 4 _ I uetr. CD ~ LL - - 3 5 ~ "~^I^B^ ^0.00 2.00 4.00 6.00 8.00 10.00 12.00 14.00 16.00 18.00 20.00 QK1) - AXIS Figure 1. The Cartesian Manipulator with Payload K, and Obstacles K,-K7. The distances between potentially colliding parts, such as K1 and K2 in Figure 1, can be used to express the obstacle constraints. They must all be greater than zero if collision is to be avoided. This observation provides the central focus for this paper. We examine carefully the properties of the distances expressed as functions of the configuration variables. Under certain conDistance Functions 5

RSD-TR-7-84 ditions we find they are differentiable functions and have derivatives which can be expressed by reasonably simple formulas. These results are useful in path planning problems. They lead to an optimal-control formulation and suggest procedures for obtaining numerical solutions of quite general problems. The plan of the paper is as follows. In Section 2 the optimal control problem is given a precise statement and its connections to the examples are discussed. Section 3 contains the main mathematical results on the continuity and differentiability of distance functions. The results are expressed in a fairly general form and may have other applications such as on-line collision avoidance control schemes [9]. One approach to the numerical solution of the optimal control problem is outlined in Section 4. The problem of numerical solutions has many diverse and difficult aspects and our study of it has just begun. Thus the presentation in Section 4 should not be considered as final or completely authoritative. It does raise critical issues and gives some useful ideas for addressing them. Section 5 shows a few numerical results for the example of Figure 1. Section 6 contains some concluding remarks. This report is an amplification of results which first appeared in [10]. 2. Path Planning as a Problem of Optimal Control In this section we establish a general framework for path planning problems. It is an optimal control problem with special state constraints expressed 6 Distance Functions

RSD-TR-7-84 by distance functions. The following notations and definitions are used: xER " is a column with components x,AER mXn is an m by n matrix with elements A,In ER nXn is the identity matrix, the' denotes transpose, <x,x>=x' x is the inner product 1 on Rn,xl\=<z,x> 2 is the norm on R,x<0 means xi <0 for i=l,...,n. For S1S2CR\, xERn,AERnn let: S +S2={zt+ z:Z1ESli2ES2}, AS1={Az:zlES1}, and Sl={x} denote a singleton. Functions of several variables which have continuous partials of order up to order k are said to be Ck The optimal control problem involves the minimization of J = L, (x(0),x(r)) + fL(x(t),u(t))dt (2.1) subject to the constraints x(t) = f((t),u(t)), a.a.tE[O,a], (2.2) h((O),x(T))= 0, (2.3) g((t),u(t))<0, tE[0,r]. (2.4) Additional requirements are: O<r<a where r is either fixed or free and a is fixed; x(-)EX, where X denotes the set of absolutely continuous functions from [O,a] into R; U(')U, where U denotes the set of measurable, essentially Distance Functions 7

RSD-TR-7-84 bounded functions from [0, a] into RP. It is possible that the components of g are either functions of x only or u only. Thus (2.4) can implement both state and control constraints. The problem data satisfy Assumption A.1: Let n,p,N,N~ be positive integers. The functions L,:R XR n -R, L:Rn XR P -R, f:Rn XR P R n, h:Rn XR n - RN, g:R n XR p -RNR are C. To this rather general optimal control problem we add constraints corresponding to obstacle avoidance. These constraints require KI (x(t))nKj (x(t)) =, tE[O,t],(i,j)EI, (2.5) where the sets Ki (x)CR m describe the space occupied by parts of the robotic mechanism and its environment, I is the set of index pairs corresponding to the parts which must not collide and I is the empty set. The sets have the form K (x) = Ti(x)C; + {pi(z)}. (2.6) In applications m = 2 or 3. The sets C; characterize the shape of the (rigid) parts; the matrix Ti and the vector pi specify the rotation and translation of the parts due to robotic motion. In applications, the matrices T. are orthogonal and, therefore, nonsingular. It is convenient to describe the requirement (2.5) by the distance between the sets: 8 Distance Functions

RSD-TR-7-84 di( (x) = min{lzi -zy 1:z EKi (),z EKj (x)}. (2.7) To make sure (2.5) holds with some margin for error, we impose the conditions diV-di (x(t))<O, tE[0,r], (i,j)EI, (2.8) where the dij >0 are measures of the margin. The constraint data satisfy Assumption A.2: Let m and N, be positive integers and IC{1,...,N, }2. The sets C, CR m, i =,.., N, are compact. The functions p;:R n R m, T;:R n — R mXm i =,...,N, are C'. For each xER n, T; (x) is orthogonal, i= 1...,N. Thus the complete optimal control problem consists of minimizing J subject to (2.1) - (2.4) and (2.6) - (2.8). The problem data satisfy assumptions A.1 and A.2. As will be seen from the results of the next section, the properties of di depend on the properties of C, and CI. In particular, A.2 implies dii is Lipschitz continuous but not necessarily differentiable. The lack of differentiability does not upset most results on the existence of optimal solutions (see, e.g. [11]), but it does complicate the application of necessary conditions and numerical optimization techniques. As discussed in Section 3, if either Ci or Cj is strictly convex d6 is continuously differentiable on the domain of interest and the complications are avoided. In specific applications it is often possible to achieve the strict convexity without introducing significant modelling errors. Distance Functions 9

RSD-TR-7-84 For instance, K1 in Figure I can be approximated by the slightly bulged set shown by the dashed lines. Moreover, most nonconvex objects, such as K6UK7, can be represented satisfactorily by the union of convex sets. In this paper we restrict our attention to robotic mechanisms which satisfy dynamic equations of the form M(q(t))q(t) + F(q(t),q(t)) = H(q(t))u(t). (2.9) Here: q(t)ERP describes the configuration of the mechanism, M(q)ER PXP is a nonsingular inertia matrix, F(q,q) represents a variety of force terms including actuator damping, H(q)ERPXP is nonsingular, u(t)ERP is the actuator input. By choosing n = 2p and x = (q,q) it is clear (2.9) can be written in the form (2.2). Consider the formulation of the optimal control problem for the example of Figure 1. A fairly accurate dynamic model is q (t) + c q' (t) =h u (t), i = 1,2,3, (2.10) where the m are the inertias of the (essentially) uncoupled axes, the c,h' are parameters of DC motor drives, and the u' are motor control voltages. Limit constraints on q (t),q2(t) and q3(t), and on u (t),u (t),u3(t) are imposed by the choice of g. The beginning and ending points A,B give q(O) = q4,q(r) = qB and q(O)= q(r)= 0; these conditions determine h. The energy required to transfer the payload in a fixed time r is 10 Distance Functions

RSD-TR-7-84 r 3 J= f (u'(t)2-bi ui(t)q'(t))dt, (2.11) Oi - I where the bi >0 are motor parameters. Clearly, No = 7,1 = {(1,2),....(1,7)}, and m = 2. Only K is dependent on x;Ki = Ci for i = 2,...,7. Similar remarks apply to the second example of Section 1. The components of q are variables such as joint angles which describe the configuration of the two manipulators. Since the dynamics of the two manipulators are not coupled, the matrix and vector functions M(q),H(q),F(q,q) have a special, partly uncoupled structure. Of course, the system (2.9) is nonlinear and much more complex than (2.10). In addition, there are constraints of the form (2.3) and (2.4) to implement the desired starting and ending positions and limits on the configuration and control variables (joint torques or forces). For minimum time, r is free and Lo -0,L-1. The index set I is likely to have many elements, particularly if the various links are to be represented by the union of several convex sets. Our numerical approach produces a smooth approximation to the optimal q(t) and u(t). This is an advantage because they may be used to provide nominal inputs for the manipulator control system. Similarly, M(q(t)) and F(q(t),q(t)) are available and may be useful in implementing the feedback control law. Provided motions satisfying the constraints exist, both of the example problemis have optimal solutions. This follows from standard existence theorems. Distance Functions 11

RSD-TR-7-84 See [11], Chapter 9. 3. Properties of Distance Functions We now consider the distance between two sets in Rm which are the images of fixed compact sets under affine mappings. It will be shown that the dependence of the distance on the parameters of the mappings is Lipschitzian, but not necessarily differentiable in the Frechet sense. Clarke [12, 13] has developed a theory of derivatives for Lipschitz continuous functions and we apply it to obtain important properties of the dependence. Our results have obvious connections to the problem of the preceding section. To simplify the presentation, we abstract the notation of the previous section. For i = 1,2 let: Ci CR m be compact, pi ER m, T, R mXm, K;(T,,p;) = T C +{(pi } (3.1) = {z:z Ti w + pi wC }. The quadruple (T 1,p 1 T2,p 2) is written as P - (T1p, T2,p2)EP = (R mm XR m) (3.2) The distance between K1 and K2 is d(P) = min{lz 1-z2'z 1EK1( T1,p 1),z2EK2( T2,p2)}. (3.3) Clearly, the minimum exists and thus d:P-: R is defined by (3.3). 12 Distance Functions

RSD-TR-7-84 It is worth noting that the compactness of C1 and C2 seems to be essential. The example, C1 = {Z:z2> e-} CR2,C2 = {z:z2 -1}CR2, P= (12,0,12,0), shows that the minimum may not exist if Cl and C2 are merely closed. Replacing the "min" by "inf does not solve the problem but leads to another difficulty: d may be discontinuous. This can be seen by perturbing P in the example (for instance rotate C2 about the origin). Inner products and norms on Rmxm and P are introduced in the obvious way. For T,TER mm and PPFP: <T,T>= E Ti TPi, — = 1" (3.4) <PJ > = (<Tk,T > + <pk pk >). k 1,2 The inner product on R mXm has the property that for z,wER m <z',T> = z' Tw. (3.5) i 1 The norms are: i T\ = < T, T> 2,PI = <PP> 2 Theorem 3.1 There exists Mi>0 such that for all P,PEP Id(P) - d(P)I<MIP - P. (3.6) Proof. Clearly, Distance Functions 13

RSD-TR-7-84 d(P) = min{lTlw1 + Pj - T2W2 - P2I:WlECw2EGC2) <lTlwli + Pi - T2W2 - P2 + (TI-Tl)wl (3.7) +(-p) (T -( T2)- ) -(2 - p2){ for all wlEC1, w2EC2. Pick w,w2 to minimize ITliw + Pi- T2 21Then it follows that d(P)<d(P) + IIT, -TIIM, + I, - p (3.8) + IIT- T2I- M2 + I\p -p 2, where Ml =- max{Iwl:wECi },i = 1,2, and IITI is the spectral norm of T. Noting IITII<ITI, ITl<jPl,lpjl<ljPl and setting M=M1+M2+2 gives d(P) - d(P)<MIP-PI. Interchanging the roles of P and P gives d(P) - d(P)<MlP - PI and thus (3.6) follows. While d is uniformly Lipschitz it does not necessarily have a Frechet derivative (gradient) Vd. This can be seen from the sets Kl and K2 in Figure 1. If K is rotated clockwise from its indicated position about its center, the distance between K and K2 changes at a different rate than if it is rotated counterclockwise. The theory of Clarke [12, 13] does apply. Specifically, it will show that d has a generalized gradient Ad and a directional derivative D+d See the Appendix for definitions of the various derivatives and a summary of results appropriate to what follows. Let 14 Distance Functions

RSD-TR-7-84 P+ {PEP:d(P)>0}. (3.9) By the continuity of d it follows that P+ is an open set. Consider:P+ x c 1X C2-R defined by r(P,w,w2) = =T1w 1 + p - T2w2 - P21 (3.10) Then d(P) = min{q(P,w,w2) LECiw2EC2. (3.11) Note 7740 on its domain. Thus, q is differentiable with respect to P. Specifically, letting'(P,w1,w2) = t(P,w,w2)-1(Tlw + Pi - T2 2 - p2) (3.12) gives.(P, 1,w ) = (P,w,w2) WI (3.13) aTy 2L. (P,w,w2) = Yi (P,w1,w2). (3.14) apt By collecting together all the partial derivatives of t) with respect to the elements of P = (T1,p1,T2,p2) we can view Vp11(P,w1,w2) as an element of P. In particular, it is the quadruple VP?(P,W 1,W2) = (qW 1 -,, 2 - -, (3.15) - =,(P, 1, w 2). Distance Functions 15

RSD-TR-7-84 With these notations and those of the Appendix it is now possible to characterize the derivatives of d. Theorem 3.2 For all PEP+,dd(P) and D+d(P;') exist and are given by od(P) =co{(w',, 2',-y):' ='(Pw lw 2),(w, w 2)E (P)}, (3.16) D +d(P;P)=min{<Vp i(P,w lw 2) P>:( 1 2)E W(P)} (3.17).... (3.17) = min{-y(P,W 1,W2)' (T1w+p 1-T2w2-p2):(w 1,2)EW(P)} where W(P) = {(W1,W2)C1X C2:'e(P,wlw2) = d(P)}. (3.18) Proof. It is only necessary to note (3.11) and use Results 3 and 4 of the Appendix with the following substitutions: P —x,(wl, w2)+u,P+ -O, C1X C2-+U, jrl* - g,d- - f,P-+v. The required conditions on g and V, g are verified immediately from (3.10), (3.12), and (3.15). The second formula for D+d follows from (3.5) and (3.15). Theorem 3.3 Suppose PEP+ and W(P) = {(1(P),2(P))} singleton. (3.19) Then d has a gradient at P which is given by vd(P) = Vp n(P,Ppl)(P),V2(P)). (3.20) Moreover, for PEP 18 Distance Functions

RSD-TR-7-84 <Vd(P),P> = P(P,1(P),2( P)P))' (Tl1l(P) + Pi - T202(P) - P2) Proof. Use Result 1 of the Appendix with the same notation as used in the proof of Theorem 3.2. Some simplifications in the above results occur if convexity assumptions are added. Theorem 3.4 Suppose C1 and C2 are convex. Then for all PEP: (Cl) W(P) is convex, (C2)'(P,w 1,w2) is independent of (wl,w2) for (w1,w2)E WV(P). Proof. Result C1 follows because W(P) is the set of minimizers for a convex function (/(P,',)) on a convex set (C1XC2). It is clear that for (w1,w2)EIV(P),'(P,wl,w2) = z[l-1z where z is a minimizer of lzl on Kl(Tlp 1) - K2(T2,p2). Since there is only one element of minimum norm on a closed convex set, result C2 follows. Because of C2 it is possible to write y(P, W,1):W = 2 (P),(w,w2)E W(P). (3.22) From (3.15) with y=, (P) this shows that Vp ij(P,, ) is affine on C X C2. Hence (3.16) and (3.17) are simplified: od(P) = {(w 1',r-w2',-")'):r = - P(P),(w l,w2)E W(P)} (3.23) Distance Functions 17

RSD-TR-7-84 D+d(P;P) = min{'~ (P)' (Tw l+p -T2w2-P2):(WlW2)EW(P)} (3.24) Note D+d(P;P) is obtained by minimizing an affine function on the convex set w'(P) With still stronger conditions d is C1. Let PN {PEP+: T1,T2 are nonsingular }. (3.25) Since det T,i = 1,2, is continuous on R mXm it follows that PN is an open set. Theorem 3.5 Suppose the sets C1,C2 are both convex and at least one is strictly convex. Then: (SC1) W(P) is a singleton for all PEP+, (SC2) d restricted to PN is C1. Proof. Conclusion SC2 is a consequence of SC1 and Result 2 of the Appendix. Now consider the proof of SCI. For PEP+ it is easily shown that Vu,,,l(P,w 1,2) = - (-1)' Ti'(P, wl,w 2). (3.26) From this Vw (P,w 1,w 2)O0,i = 1,2, for PEPN and (wl,w2)ECCX C2. Since (wI,w2)EW(P) solves (3.11), (wi,w2)EW(P) implies wE boundary C,,i=1,2. Assume, contrary to SCI, that (wil,w2)7)(wl,w2) are both in W(P). It follows from Tl and T2 nonsingular and C2 that wl1$wl and W2$w2. From C1, Xk- + (1 - X)wi E boundary C;,i = 1,2 and 0<X<1. This contradicts the 18 Distance Functions

RSD-TR-7-84 assumption that either C or C2 is strictly convex. Extensions of the above results are possible. First, Clarke's theory may be applied to the characterization of d on all of P. This is because qr(P,w,w2) is Lipschitz at rq = 0, even though it doesn't have a Frechet derivative at r = 0. However, the details are more complex and of no value in the collision avoidance problem where we require d(P)>O. Second, if T2 and P2 are fixed, Theorems 3.1-3.5 still apply to dependence of d on Tl,pl, even though C2 is only closed. A more complex method of proof must be used and, again, the result has limited practical value. In the real world fixed obstacles are of finite extent. 4. Numerical Solution of the Optimal Control Problem Numerical solution of the optimal control problem (2.1)-(2.4), (2.6)-(2.8) is a complex task which raises a number of questions. These include: (1) approximation of the infinite dimensional system by a finite dimensional system, (2) imposition of the equality constraints (2.2) and (2.3), (3) treatment of the inequality constraints (2.4) and (2.8), (4) choice of the descent algorithm, (5) efficient calculation of the various functions and their derivatives, (6) determination of a good initial approximate solution which is feasible, (7) achievement of reasonable computational cost. It is not clear what algorithmic approach addresses these questions in the best way. In this section we present one approach which appears to have a number of advantages. It assumes the system dynamics are Distance Functions 19

RSD-TR-7-84 described by (2.9) and the function h is affine. This permits the solution of a wide class of path planning problems, including the examples discussed in Sections 1 and 2. To keep the notation reasonably simple and to emphasize the conceptual issues, the approach is described in general terms. Actual implementation of the approach involves attention to important, complex details. Questions (1) and (2) are resolved together in the following way. The state x(t) is approximated by restricting it to a linear subspace of X. Specifically, the approximation is K q(t,a) - a' o i (t) (4.1) i —1 where aER K and the O; are basis functions for the subspace. It is assumed that the Oi are at least twice differentiable. The most natural choice for the X; are piecewise polynomial splines of limited support [14]. The equality constraint (2.9), which is another way of writing (2.2), is satisfied by choosing u(t) equal to u(t,a) H-'(q(t,a))(M(q(t,a))q(t,a) + F(q(t,a),q(t,a))). (4.2) The functions q(t,c) and q(t,a) have obvious expressions in terms of i and P;, which are easily computed if the Xi are splines. Moreover, for mechanical manipulators there are efficient recursive procedures [15, 16] for evaluating the left side of (4.2). Specifying the finite dimensional approximations of q(t),q(t) and u(t) in the above way provides an exact solution of (2.0), avoiding numerical integration of (2.9). By defining x(t,a) = (q(t,a),q(t,a)) it is clear x(t,o),u(t,a) 20 Distance Functions

RSD-TR-7-84 solves (2.2). The integration of (2.1) still requires a numerical scheme, but a simple quadrature formula suffices. Similar ideas have appeared previously in the literature. See, e.g. [17]. Now consider the constraint (2.3). Since h is affine, h(x(O),x(r))= Hllq(O) + H12q(0) H21q() + H22q(r) + ho =, (4.3) where ho ER NhHiER N X. Setting q(t) = q(t,a) in (4.3) yields an affine constraint on a. Provided the 0i and Hij satisfy reasonable assumptions the constraint on a has a solution which can be expressed by a = HI + 7r, (4.4) where ZER K-N',7rER K, HER Kx(K-N). Thus (2.3) is satisfied by expressing a in terms of a by (4.4). Having obtained exact solutions of the (more troublesome) equality constraints, we accept approximate implementation of the inequality constraints. The approach is to use interior penalty functions [18, 19]. This causes the constraints (2.4), (2.8) to be satisfied, but increases J somewhat because the penalty functions force the solution away from the constraint boundaries. The penalty function approach requires the constraint set to have an interior. This is a physically reasonable condition for our problem. The penalized cost has the form Distance Functions 21

RSD-TR-7-84 J(a,#) = L (x(O,a),x(r,a)) + fL(x(t,a),u(t,a),p)dt (4.5) 0 where.mX U, p) = L (x, u) N, L(x,u,p) = L(x,u) + P E 3 (-ag (X,u)) i-i (4.6) + p1E j i(dij() - di). (ij)EI The functions /; and /;i have the following properties: /:(O,oo)-+R is C1,f(e)>O and (e (e)<O for all e,/(e) —oo as e-O. An example of such a Qe function is 3(e) = 3 (ee- + e -'1-2), 0< e <, /3(e) = O,e >, (4.7) where the numbers,/3>O measure respectively the point where the penalty becomes active and the strength of the penalty. The parameter p>0 is selected in the usual way to establish a trade-off between bad conditioning of J and acceptably small degradation of J. Finally, J must be integrated numerically. This gives J(cap) J(a,), where J(c,-l) = L, (x(O,),xz(r,T)) + ai L(x(ti,c),u(t;,c),P) (4.8) i=-o Here, the ti,ci determine the "time grid" and the method of numerical 22 Distance Functions

RSD-TR-7-84 integration. It is important that the ti be close together, particularly where the penalty terms are changing rapidly. Otherwise, the penalty terms in L may fail to enforce the inequalities (2.4), (2.8). On the other hand, the computational cost grows almost linearly with v. At a given t = ti only a few of the constraints (2.8) are likely to be active. Therefore, many calculations can be eliminated by having a simple scheme which omits evaluation of dii(x(t,&)) when the sets Ki and Kj are widely separated. The last step in the overall solution process is the numerical minimization of J(a) = J(Zl + Tr,p). From the preceding section it follows that J, which depends on di, (x), is Lipschitz but may not be Cl. The numerical minimization of such functions presents both theoretical and practical difficulties [20, 21, 22]. The resulting algorithms tend to be complicated, since they involve a smoothing of the generalized gradient in order to determine a direction of descent. For our problem it seems much simpler to carry out the "smoothing" directly by using the suggestion of Section 2: Assume the C;,i 1,...,lo, are convex and at least one of the sets C;,Cj is strictly convex for each (ij)CI. By Theorem 3.5 this causes the dfj(x) and hence J to be Cl. Then standard minimization algorithms, which have excellent convergence properties, may be used. To begin the minimization an initial choice of a must be made. Because of the interior penalty function, the corresponding initial path must satisfy strictly Distance Functions 23

RSD-TR-7-84 the inequalities (2.4) and (2.8). In many problems, an admissible path is evident from the geometry; alternatively, one may be generated algorithmically by the methods of [3, 4]. Once an admissible path is known, a can be determined by a spline fit of the path (this another advantage of doing the computations in terms of q(t) rather than u(t) ). For example, in Figure 1 there is an obvious sequence of translations and rotations which achieve a transfer from A to B. Efficient minimization algorithms also require the evaluation of VJ. Formulas for VJ are obtained by applying the chain rule of differentiation to the series of expressions which define J. We omit the details, but comment on the terms which are computationally most expensive: the derivatives of dii(x) and u(t,). Application of (3.21) gives 0 d1i _ Ti Ci () = I. * Zji 1-l( *- Z *)(:k (WI i')P aii ap ( p a Ti 4 +ere, ( (. * (7) t H ere,,1(z( * are the (unique) minimizers from (2.7) at expici and - * =- T-(x)(t * - pii_ )),l =( i,j. All the terms which show explicit dependence on x are rather simple functions of q and T1-1(x) = T' (7),l = i,j Thus, the greatest potential for computational expense is the solution of (2.7). There are a variety of approaches ranging from the use of quadratic program24 Distance Functions

RSD-TR-7-84 ming algorithms to the derivation of explicit solution formulas. Several of the approaches are being investigated for numerical efficiency, along with procedures for approximating convex sets by strictly convex sets. The most difficult computation in the differentiation of (4.2) with respect to a is the differentiation of M(q) and F(q,q) with respect to the components of q and q. For some systems, such as the one in Figure 1, the computation is easy. General manipulator dynamics offer a greater challenge. Some interesting methods for treating them have been developed and will be reported in a later paper. It is worth noting that the computations described in the preceding two paragraphs have exactly the same form at each ti. Moreover, the distance minimizing problem for each (ij)EI are independent of each other. Thus it would be possible to greatly speed the computations by using a computer with parallel processors. 5. Numerical Solution of the Example To test the ideas of Section 4 the system in Figure I was solved numerically for several cases. The following paragraphs give the details. The dynamics and cost are given by (2.10) and (2.11) with ci _ b= = hi = l,i = 1,2,3, and m1 = m2 = 1. There are six cases as slummarized in Table 1. The constraints (2.3) and (2.4) correspond to: Distance Functions 25

RSD-TR-7-84 q(0)=(2,2,0),q(r)=(18,2,0),q(0) = q(r) = 0,0< (t)<20,0<q2(t)<10. The payload K1 is.8 units wide and 2 units long. It is made strictly convex by bulging its sides slightly into circular arcs of radius 100. The distance minimizing problems are solved easily so that their solutions can be expressed explicitly by relatively simple formulas. The functions i (t) represent a cubic spline basis for q (t),q2(t),q3(t) specifically, the components of; (t) are normalized B-splines of order 4 [141 with the knots producing 40 equal intervals in [0,r. Thus K = 3X43 and dim - = 3X39 = 117. The /, and dij in (4.6) have the form (4.7) with F =.2 and 3 = 1. The remaining parameters in (4.6) are d -i.1 and t =.1 for all cases. The formula (4.8) was obtained from the composite Simpson's rule where the time grid partitions each spline knot interval into F equal sections, where F = 8. Points along the piecewise rectilinear path Po in Figure 1 were used to generate an initial spline for the optimization algorithm. A BFGS - type algorithm [23] was used to carry out the minimization. Typically, V O 0 was obtained in about 300-600 evaluations of J and VJ. Smaller values of /(/ = 10-3,10-5) were also tried in several cases. The result was a somewhat improved cost (approximately 10%) and a closer approach to the obstacles. The poorer conditioning of the penalized cost did not affect the number of iterations significantly, although convergence did require the use of a better initial path than Po ~ 26 Distance Functions

RSD-TR-7-84 The resulting paths for cases I through 6 are indicated in Figures 2 through 7. The energies of the initial and final paths as obtained in the computational process are given in Table 1. The optimization produces appreciable reductions in cost. This is no doubt due to the rather abrupt starting and stopping along the initial path. As expected, the optimal cost decreases as either r increases or m3 decreases. The use of Simpson's rule ensures that the actual cost, being a piecewise cubic polynomial in this example, is integrated exactly for all the above cases. However, the penalized portion of the cost is not, and the distance constraints (2.8) may be violated at values of t between grid points. This was observed to happen when F = 1. The paths obtained in [10] had this difficulty, although it was not detected at the time [10] was written. A plot of distance versus time for each object pair in the index set I is given in Figure 8 for case 1. Distance Functions 27

RSD-TR-7-84 TABLE 1. NUMERICAL RESULTS CASE T m3 INITIAL COST FINAL COST 1 20.0 0.25 214.75 3.8374 2 20.0 1.0 214.75 4.9214 3 20.0 4.0 214.75 5.6072 4 20.0 16.0 214.75 6.0235 5 40.0 0.25 26.84 0.5030 6 40.0 1.0 26.84 0.6397 x. -v V tt ~0.00 2.00 4.00 6.00 8.00.00 10.00 14. 00 16.00 18.00 20 0 Q(1) - AXIS Figure 2. Final Path for Case 1. 28 Distance Functions

RSD-TR-7-84 G) x. (0 0 0.00 2.00 4.00 6.00 8.00 10.00 12.00 14.00 16.00 18.00 20.00 Q(1) - AXIS Figure 3. Final Path for Case 2. Q (0 00 2.00 4. 00 e.00 8.00 1e.00 12.00 14. 001e. - 0 18.00 2 ee00 QC1) - AXIS Figure 4. Final Path for Case 3. Distance Functions 29

RSD-TR-7-84 0. yQ(1q - AXIS Figure 5. Final Path for Case 4. x. 0 /8 0 (eJ~.00 2.00 4.00 6.00 8.00 10.00 t12. 00 14.00 16.00 18.00 20 04 Q- 1 ) - AXIS Figure 6. Final Path for Case 5. Distance F 0(1) - AXIS Figure 6. Final Path for Case 5. 30 Distance Functions

RSD-TR-7-84 -0 90 2.00 4.00 6.00 8.00 I0.00 i2.00 14.00 16.00 18.00 20.00 Q(1) - AXIS Figure 7. Final Path for Case 6. ~Q (1,2) (1,6) (1,5) C'i.LJ (1,4) 6.0 ~ 4.700 ~00 ~ T12.00 ~ I.00 ~ 20.00 TIME Figure 8. Distances of Approach for Case 1. Distance Functions 31

RSD-TR-7-84 6. Conclusion In this paper we have formulated an optimal control problem which includes a great variety of path planning problems. Obstacle constraints are expressed in terms of the distances between potentially colliding parts. We have studied the properties of these functions and found that they are continuously differentiable and have simple gradient formulas provided "strictly convex" modelling of part shapes is used. An algorithmic approach to the optimal control problem has been proposed and has proven successful for a cartesian manipulator in 2-space. Manipulator problems in 3-space are much more complex than our example, but we feel that the ideas we have set forth will prove fruitful in their solution. The distance function approach may also be useful in finding admissible geometric paths; in this case the "dynamics" are very simple since they are integrators whose function is to produce smooth connecting paths. Unlike many of the presently known geometric approaches [1, 3, 4, 5], there is no conceptual problem in treating multi-member mechanisms in 3-space with this approach. 7. Appendix Here we summarize some results of Clarke [12, 13] on the differentiability of Lipschitz functions. In all of what follows 0 is an open subset of R n and If: O —R is locally Lipschitz. 32 Distance Functions

RSD-TR-7-84 It is known that f has a Frechet derivative (gradient) at almost all points of O, i.e., for such points x there exists vf(x)ER n with \6xl-l(f(x + 6x)-f(x)-< f(x),6x>)-*O,6x —O. (A. 1) The usual one-sided directional derivative at x is denoted by D +f(x;v) and, provided the following limit exists for all vER', is defined and characterized by -l(f(x + av) - f(x) - aD +f(x;v))-O,a1O. (A.2) The generalized gradient of f at x is Of(x) = co{ow:vf(x, )-w,vf(xi )exists,xz — x}, (A.3) where co denotes the convex hull. For all xEO the set Of(x)ER' is nonempty, compact, and convex. We now state the results which are needed in Section 3. Result 1 ([12], Prop. 1.13). The following statements are equivalent: af(x) = {I }, a singleton; Vf(x) exists; Vf(x) = ~. Result 2 ([12], Cor. of Prop. 1.13). The following statements are equivalent: f is C';of(x) is a singleton for each xEO The remaining results concern the function f(x) = max{g(x,u):U UV}, (A.4) where g:OxU — R and U is compact. It is assumed that g satisfies the following conditions (somewhat stronger than those in [12]): (1) g is continuous; Distance Functions 33

RSD-TR-7-84 (2) g is locally Lipschitz in x, uniformly for UEU; (3) the Frechet derivative of g with respect to x,V, g(x,u), exists and is continuous on OxU. Result 3. D+f(x;v) exists for all xEO. Moreover, D +f(x;v) = max{ <V, g(x,u),v>:uEM(x)}, (A.5) where I(x) = {uE\:g(x,u) = f(x)}. (A.6) Result 4. The generalized gradient exists and is given by af(x) = co{v, g(x,u):uEM(x)}. (A.7) These results follow from Theorem 2.1 of [12], with the trivial modification in the proof of the Theorem that 0 replaces R. 8. References [1] J.Y.S. Luh, "An anatomy of industrial robots and their controls," IEEE Trans. Automat. Contr., vol. AC-28, pp. 133-153, Feb. 1983. [2] R. P. Paul, Robot Manipulators: Mathematics, Programming and Control. Cambridge, MA: M.I.T. Press, 1981. [3] T. Lozano-Perez, "Spatial planning: a configuration space approach," IEEE Trans. Comput., vol. C-32, Feb. 1983. 34 Distance Functions

RSD-TR-7-84 [4] T. Lozano-Perez, "Automatic planning of manipulator transfer movements, IEEE Trans. Sys., Man, Cybern., vol. SMC-II, pp. 681-698, Oct. 1981. [5] J. Y. S. Luh, C. E. Campbell, "Minimum distance collision-free pathplanning for industrial robots with a prismatic joint," IEEE Trans. Automat. Contr., vol. AC-29, pp. 675-680, Aug. 1984. [6] M. Vukobratovic and M. Kircanski, "A method for optimal synthesis of manipulation robot trajectories," Trans. of the ASME, vol. 104, pp. 188-193, June 1982. [7] K. G. Shin and N. D. McKay, "Minimum-time control of robotic manipulators with geometric path constraints," to appear in IEEE Trans. Automat. Contr. [8] J. E. Bobrow, S. Dubowsky and J. S. Gibson, "On the optimal control of robotic manipulators with actuator constraints," in Proc. Automat. Contr. Conf., pp. 782-787, June 1983. [9] H. P. Moravec, "The Stanford cart and the CMU rover", Proc. of the IEEE, vol. 71, July 1983. [10] E. G. Gilbert and D. W. Johnson, "Distance functions and their application to robot path planning in the presence of obstacles," in Proc. Eighteenth Annual Conf. on Information Sciences and Systems, PrinceDistance Functions 35

RSD-TR-7-84 ton University, March 1984. [11] L. Cesari, Optimization Theory and Applications. New York: SpringerVerlag, 1983. [12] F. H. Clarke, "Generalized gradients and applications," Trans. American Mathematical Society, vol. 205, pp. 247-262, 1975. [13] F. H. Clarke, Optimization and Nonsmooth Analysis. New York: John Wiley & Sons, 1983. [14] L. L. Schumaker, Spline Functions: Basic Theory. New York: John Wiley & Sons, 1981. [15] J. Y. S. Luh, M. W. Walker, and R. P. C. Paul, "On-line computational scheme for mechanical manipulators", Trans. ASME, J. Dynamic Syst. AMeas. Contr., vol. 102, pp. 69-76, June 1980. [16] M. W. Walker and D. E. Orin, "Efficient dynamic computer simulation of robotic mechanisms," Proc. Joint Automat. Contr. Conf. 1981. [17] R. K. Mehra and R. E. Davis, "A generalized gradient method for optimal control problems with inequality constraints and singular arcs," IEEE Trans. Automat. Contr. Conf., vol. AC-17, pp. 69-79, Feb. 1972. [18] A. V. Fiacco and G. P. McCormick, Nonlinear Programming: Sequential Unconstrained Minimization Techniques New York: John Wiley & Sons, 1968. 36 Distance Functions

RSD-TR-7-84 [19] E. Polak, Computational Methods in Optimization, A Unified Approach. New York: Academic Press, 1971. [20] P. Wolfe, "A method of conjugate subgradients for minimizing nondifferentiable functions," Mathematical Programming Study 3, pp. 145173, 1975. [21] E. Polak, D. Q. Mayne and Y. Wardi, "On the extension of constrained optimization algorithms from differentiable to nondifferentiable problems," SIAM J. Contr. and Optimization, vol. 21, pp. 179-203, Mar. 1983. [22] E. Polak, D. Q. Mayne, "Algorithm models for nondifferentiable optimization", Proc. IEEE Conf. on Decision and Contr., pp. 934-939, Dec. 1983. [23] R. Fletcher, Practical Methods of Optimization, Unconstrained Optimization, vol 1. New York, John Wiley & Sons, 1980. Distance Functions 37