A Proximity Metric for Continuum Path Planning

1 Prior Work

Two early research efforts in this area may be seen as corner­ stones of the two basic methods employed.

Whitney introduced the first combinatorial algorithm for ma­ nipulator path planning [Whitney 1969). Although this algo­ rithm was only concerned with the planning of gripper motions in the plane, and only a few evenly spaced positions and orien­ tations were considered, the algorithm suiTered from problems of combinatorial explosion. The algorithm was formally verified to be correct.

Peiper's algorithm was not combinatorial, and was applied to the planning of motions for a full six degree-of- freedom robot manipulator [Peiper 1968). Although he reported qualitative suc­ cess, he was unable to verify correctness.

This basic tradeofT between verifiability and the size of tractable problems remains a fundamental issue among those who study path- planning today.

Perhaps the most successful combinatorial results have been built on the work of [Lozano- Perez 1981), who defined a scheme for exactly partitioning admissible points from inadmissible ones for polyhedral bodies in fixed relative orientation. Jarvis has reported a similar scheme which works for arbitrary classes of objects, and is based on constructing adjacency graphs among uniform- sized quanta in 3- space 1 [Jarvis 1984). Brooks later ex­ tended Lozano--Perez' work to apply to planar objects with vari­ able orientation through the use of a successive approximation technique [Brooks 1982), but was subsequently unable to extend it to three dimensions. [Schwartz 1982) reported a generaliza­ tion of this concept to apply to objects of arbitrary dimension bounded by algebraic surfaces (e. g. planes, cylinders, etc.) . Al­ though these results confirm~d earlier results showing the path­ planning problern tobe P-space complete [Reif 1979), they were of little practical interest since time complexity for discretization of the problern domain exceeded that of simple subdivision into hyperparallelopipeds of the smallest mesh size representable by standard computing hardware 2 [Buckley 1985) .

Two non--combinatorial algorithms stand out as significant . Loeff and Soni reported an algorithm for planning the motions of a planar, line- segment manipulator among circular forbid­ den zones [Loeff 1975). Khatib has implemented an algorithm in which simulated repulsive forces generated from a restricted set of object models were used to generate commanded positions for certain distinguished points on a manipulator [Khatib 1980) .

Another simpler algorithms was reported by Myers [Myers 1981). Although this algorithm was essentially one of hypoth­ esis and test, its computation times for solving a generat purpose path planning problern for a PUMA manipulator were on the same order of magnitude as those reported in [Brooks 1982) for the planar, free- body case.

These results suggest that practical algorithms for path­ planning will be heuristic in nature, and not formally verifiable. However, prior research has only begun to explore the question of what sort of heuristics may be used tobest advantage. Notably, excepting the algorithms of Loeff and Soni and that of Khatib, heuristics employed for collision avoidance in prior research have really been based only on whether or not a collision was detccted in following an hypothesized trajectory. In most cases the direc­ tion in which hypothetical impact occurred was not even taken into account. Often the occurrence of a collision at one point along an hypothesized trajectory was considered grounds for re­ jection of that entire trajectory. Local perturbation of offending portions of trajectories has hardly been explored.

2 The Proximal Relationship of Objects in Space

To a !arge extent, the simplicity of the heuristics employed in previous algorithms is due the fact that there doesn't really exist a good way of characterizing the proximal relationship between two rigid bodies or models. Conceptually, it is easy to determine whether or not an intersection between two such bodies occurs, given their relative position. However, if this information is in­ sufficient, then what? lt is often proposed that the actual point set ccrre~ponding to the intersection of rigid bodies be computed, but there have been no serious proposals made as to what might be dcne with all of this information if were it available.

1 This adjacency graphwas searched using dynamic programming, which that computations be performed for tach quantum in the space.

2 e. g. a single-precision ßoating- point number on something like a VAX

Practically, the only alternatives to hypothesize-and-test methods which have enjoyed any success at all are the relaxation methods. These methods were originally conceived to solve con­ strained non-linear optimization problems, and have been quite successfully applied. Loeff and Soni and Khatib took advantage of the fact that these algorithms work by continuously perturbing a state vector from some initial guess to a final optimum state to generate trajectories between known endpoints.

The main inconvenience with relaxation methods as they are almest always formulated is that all constraints must apply si­ multaneou&ly. This has to do partially with the formulation of the Kuhn-Thcker stationarity equations for an extremum, which are used as error equations to drive state perturbations, and par­ tially with long·-standing conventions of the discipline. This requirement is fundamentally incompatible with standard solid modelling practice, in which solid objects are represented as point sets defined by arbitrary Boolean functions of boundary predi­ cates.

For example, the set of points contained in the L-shaped pla­ nar region shown in Figure 1 may be described as those points which satisfy the following Boolean expression:

wherc each Boolean expression P; corresponds to a point lying on the proper side of the associated line 5 , as indicated by the direc­ tion of the normal arrows drawn in the figure. If g;(x) = 0 were the equation of boundary line P;, then the Boolean expression P; might be g;(x) $ 0. Only the constraints in one or the other of the two disjuncts are necessary to qualify a point as being part of the shaded rcgion. Further, for points lying in one of the arms of the "eil~ it is not pouible for all of the constraints to apply simultaneously. A similar argument may be made with respect to points outside of the shaded region. The developers of prior algorithms for path-planning in which relaxation methods wcre used were very aware of this problem, and in fact the limitations which they placed on their algorithms stemmed directly from it.

8 a.n instance of a. bounda.ry ma.nifold

In this paper is described a new metric for characterizing the proximal relationship between two convex rigid bodies which ad­ dresses this issue and others. This mctric may be related to an algorithm intended exclusively for detecting intersections among convex bodies published in [Comba 1968].

The use of the Comba method actually involves the numer­ ical solution of an unconstrained minimization problern itself, which can be time-consuming. The quantity to be minimized is a "pseudo-constraint function~ defined as follows:

Let the nc boundary predicates (those which define a/1 of the objects under consideration) be defined by

where the g; are convex functions of their arguments 4 · Then, the Comba constraint function G(x) is defined by the three equations:

where t, c are small, positive constants.

For t non-zero, each of the v; functions is always positive. However, when the corresponding g; is negative, then that v; tends toward zero. In particular, when a g; = 0, then v; = t . When V = t, then the term a in equation 4 is zero. Therefore, when a/1 of the g; are zero, then V= O(t). When a g; is negative, then its corresponding v; = O(t 2 ). When t is small, t 2 < t, and the contribution of a v; corresponding to a negative g; will be less. Therefore, except in instances where the bodies in question are just touching, if

then G· < 0 Cerresponds to a condition of intersection, G· > 0 eorresponds to non-intcrsection, and the x corresponding to a a· near zero are in a gray area. The constant c in the equation for G helps minimize the problems associated with this gray area. For example, it may be used to create a safety buffer areund the obstacles being tested.

The Comba function is meaningful only at its minimum, and then only when compared against the threshold of zero. Trying to assign other meaning to the value of the function taken at its minimum is hampered by the influence of the t and c parameters, which were introduced to insure continuous differentiability of the function as an aid to finding its minimum.

The Comba function is undefined at any point x satisfying all constraints if t = 0. However, for the non-intersecting case, if both t and c are set equal to zero, then the Comba function becomes:

If the number of convex bodies being tested is restricted to 2, and there is only one g; function per body, corresponding to g;(x) =

·The g; muot be continuouoly differentia.ble in order to employ numerica.l methods for minimiza.tion, such a.o Da.vidon- Fietcher-Poweii[Avriel 1976J .

d(x, B;), the convex distance function 5 (Rockafellar 1970), then the function g will be minimized anywhere on the line segment connecting the two closest points of bodies B1 and B2 · This segment is shown in Figure 2. More important, its value there will correspond to half the distance bctween these closest points, or in other words, half the minimum distance between bodies B1 and Bz 8

This specialized version of the Comba function has the advan­ tage that the generafing &egment corresponding to the locus of minimizing points is a function of only a single point on each of the bodies. This makes it easier to generate a derivative of this specialized Comba function than it is for the general case. On the other hand, this specialization effectively reimposes the restrictions that:

1. Each of the convex bodies involved be represented by a single, diffcrentiable manifold.
2. The bodies in question should not intersect .

These restrictions correspond almost exactly to those which Loeff and Soni, and subsequently Khatib found it necessary to impose in their algorithms. Loeff and Soni restricted their attentions to planar problems, in which the manipulatorlinks were modelled as line segments , and the obstacles "[did) not have sharp corners or sides" [Loeff 1975], and in which the inßuence function used to repel a moving from a fixed body was a decreasing function of the minimum distance between them. Khatib restricted his at­ tention to the interaction between obstacles whose surfaces could be modelled as single differentiable manifolds, and selected dis­ crcte points on a moving manipulator 7 · The minimum distance betwecn these points and obstacles was subsequently used in an inverse square potential function to simulate forces between the two bodies generating it.

In both of these cases, a clear effort was made to avoid having the two bodies in qucstion intersect. There is good reason for this - if the two bodies intersect, the minimum distance between them drops to 0 identically and abruptly, and remains that way for arbitrary intersections. This means that the gradient of the minimum distance function is identically zero, and can therefore provide no information which migbt be used to drive a relaxation algorithm.

An even more important Iimitation than that placed on the type of objects which can be modelled is the one which prevents the modified Comba algorithm from working if the objects be­ tween which constraints are computed intersect. Both the Lo­ eff/Soni and Khatib algorithms were subject to this Iimitation, which is a s·evere one because it forccs an evolutive algorithm for tre.jectory generation, in which one known state is sequentially perturbed towards another, as shown in Figure 3a, and no inad­ migsible states are ever entered. This approach will only work if the problern in question is free from false local minima. The limi­ tations of this type of algorithm have been aptly demonstrated in mobile base path planning research, e. g. (Chatila 1981), (Cahn 1975).

"corresponding to the minimum distance from a point :z: to all pointo in body 8;

6 The 4 factor io included so as to make the a term correspond to the inverse o( the transformation from g; to v;. lt's presence is not essential.

7 which he called "points submitted to a potential" !Khatib 19801

An alternate paradigm for path planning is shown in Figure 3b, in which an entire trajectory is hypothesized between the two known endpoints, and perturbed into admissibility, if it is not al­ ready. Such an approach depends on being able to deal effectively with inadmissible states (in this case intersections between the moving body and its obstacles) should they arise.

3 .6., the Minimum Directed Distance

The function ll., called the minimum directed distance between two arbitrary convex bodies, was developed in order to address these two issues. That is, it was developed to:

• be valid for bodies bounded using more than one boundary predicate.
• be valid even when the bodies in question intersect.

ll. also stems from the distance form of the Comba intersection form described earlier, hence its restriction to convex bodies. This restriction does not constitute much of a problem, since con­ vex decomposition can be accomplished "off- line". Algorithms exist for performing this decomposition automatically in the case of certain classes of objects, such as polyhedra [Chazelle 1980). It can also be done by band if necessary.

Although the merit of the ll. function stems in I arge part from the fact that it is valid between two arbitrary convex bodies , it is simplest to explain for the case in which one of the bodies is a point. Extension to the point body case follows from the same configuration space obstacle transformation described in [Lozano- Perez 1981).

3.1 Disjoint Case

Consider first the case in which the givens of the problem, a point x 0 and a set oC points oC a rigid, convex body C are fixed in

~elative position so that Cnxo = 0. Let C be defined cymbolically by

where nc is the nurober of constraints. The g; are concave func­ tions, which means that g;(x) ;:::: 0 is a convex set .

The minimum distance problern (DP) can be clefined as the problern of finding:

subject to

The stationary conditions for this problern are:

in which

This problern is a well- behaved, convex one.

Now, the following two-stage problern (SP) is introduced as an alternative to problern (DP) above.

subject to

and

Considering the two extremizations as separate problems, the objective functions of each are linear, and may be considered as simultaneously convex and concave. The constraints are concave, so each extremization represents a convex problem.

There are two sets of stationary conditions, one corresponding to each of the stages:

Jt is easy to eliminate ~. which yields

Comparing these stationary conditions with those of problern (DP), it is seen that they are equivalent if

From the definition of the Kuhn- Tucker stationary conditions, when a multiplier ß; is equal to 0, then that constraint is inactive. If ßo = 0, then the stationary conditions 11 dictate that x· = xo, where x· is an x which satisfies the stationary conditions. However, by definition, there is no x E C which can equal (be coincident with) x0 , since x0 and C are disjoint. Therefore, the unit magnitude constraint on e must be active in this case.

The activity of the unit magnitude constrair~t on ~ in turn implies that

and therefore

Further,if C, x· are extrema which satisfy the stationary condi­ tions, then

and

The equivalence of the two problems under conditions of non­ intersection has been fully established.

The solution of problern (SP) has a simple interpretation in terma of support functions, which is useful in developing intuition for the development which follows .

The indicator function of a convex set C is defined by

Its convex conjugate function is given by [Avriel 1976)

The x-extrem um portion of the problern (SP) is given by

subject to

or in other words

where the operator e denotes set subtraction. Set subtraction will be formally dcfmed later on, but in this case the operation simply involves subtracting x0 from each point in the convex set C. lts easy to sec that Xo '/. C # 0 '/. Ce xo.

Expressed in this form, the x- extremization may be given a simple graphical interpretation, as shown by the (necessarily pla­ nar) example in Figure 4. A support function with an argument of e (considered as a free vector) may be computed graphically by taking a hyperplane 8 with normal ~ and moving it so that it

8 & line in the planar case

Figure 6: Multiple Stationary Conditions for 6.

For problern (SP), the origin of the transformed problern will lie inside of C e {xo}. Therefore, i'(e, Ce {xo}) ~ 0 for all e E R". The e-minimization of problern (SP') is therefore solved by e = 0, which means s(xo; C) = 0 and x· = Xo , just as with (DP). Therefore, problern (SP) is again equivalent to (DP), and equally uninteresting.

However, the formulation of (SP) suggests a small modification which is not apparent in the problern (DP). Ir a problern (SP=) is written, which consists of finding

subject to

osculates against the set in question, with the normal pointing away as shown. Then the value of the support function is equal and to the dot product between a vector from the origin of the Co­ ordinate system to any point of the osculation. Of course, this value depends on the choice of the origin, but the transformation

turns the point x0 into the origin, and makes this method ap­ propriate for describing proximal relationships between xo and c.

The problern (SP') can be restated in light of the above as

subject to

For x0 , C disjoint, solving & simply involves finding the unit mag­ nitude direction vector which minimizes the support function of the transformed set. This corresponds to the identification of a point such as x! in figure 5, and its corresponding e. It is expressly pointed out that this is not equivalent to solving

Snch a solution would correspond to the point x6 in the same figure . The unit vector e points along the generafing 11egment from x! to xo.

3.2 Non-Disjoint Case

When xo E C 9 , the solution to the problern (DP) is uniformly zero, and therefore uninteresting.

0 corresponding to intersection of two bodies in the original problern

is defined, in which the only differente between it and problern (SP) is that the unit magnitude constraint on e is an equality constraint, its solutions assume interesting properties. To begin with, when x0 (/. C, the behavior of problems (SP) and (SP=) are the same.

However, the guaranteed activity of this constraint at the op­ timum means that e will always be of unit length, and parallel to the vector from x· to x0 · Therefore 6.(x0 ; C) will always have units of length. Because e i: 0, any x· will always lie on the boundary of C or

The:-efore, the magnitude of 6. will corresponds to the shortest normal distance to the boundary of C. As was the case with the Comba function, the sign of 6.. may be used as an intersection predicate:

When 6.. < 0 the problern (SP=) is no Ionger a convex pro­ gram, and is therefore much harder to solve. It is not hard to think of Situations in which there are several x· E ßC which satisfy the stationary conditions of (SP=); Figure 6 shows a cou­ ple of such cases. In the case where the point A is taken as xo, although there are multiple stationary conditions, there is a clear minimum distance to the boundary. In the case where Bis taken as xo, both stationary conditions may be used, since their corresponding distances are equal. B is called a conjugate point.

3.3 Properties of ~(x0 ; C)

These properties of .:l(x0 ; C) are established in [Buckley 1985] :

• .:l(xo; C), considered as a function of xo is convex.
• .:l(xo; C) is a continuous function.
• e. the unit vector parallel to the generating segment, is always contained in the subdifferential of .:l(x0 ; C), and may be used as a gradient.

An intui t ive feeling for the behavior of .:l as a point x0 passes near a body E may be seen in Figure 7. Dashed generating segments correspond to negative values of .:l, and the magnitude of .:l is proportional to the length of the generating segment. Note that the subgradient of .:l will be discontinuous.

3.4 Extension to the Body-Body Case

The derivation for the body- body case proceeds almost ex­ actly as wi th the point- body case, except that both ends of the generating segment are now free to move about in the minimiza­ tions. Details are given in [Buckley 1985]. Equivalence to the configuration space obstacle method described in [Lozano- Perez 1981] is also established there . .:l between two convex bodies is also a continuous function, but because the relative position and orientation of two bodes must be expressed in terms of at least one rotat ion group, it cannot be a convex function of its argu­ ments . An analytic expression which may always be used as the gradient of .:l has also been derived.

An intuitive feeling for the behavior of .:l as a reetangle moves near a hexagon may be seen in Figure 8. The encoding on the generating Segment is the same as with Figure 7.

4 Computational Issues

While .:l may be attractive theoretically, it is not Straightfor­ ward to compute for arbitrary convex bodies. However, if the bodies between which it is to be computed are polyhedral set3, then its computation becomes a combinatorial problem. Efficient ways of computing this function have been explored cxtensively [Buckley 1985]. Briefly, the complexity bounds O(n log n) in the planar case, where n i is the co:nbined number of vertices of the two polyhedral bodies for which .:l is computed. The correspond­ ing figure for spatial polyhedral bodies is O(n 2 logn) .

Implementational issues of the .:l function are currently be­ ing studied, and it is being incorporated into a free body path­ planning system. The performance of this system is being as­ sessed relative to a combinatorial system, that of [Brooks 1982].

5 Conclusion

A new function for characterizing the proximal relationship be­ tween two convex bodies has been developed, and its properties studied. This function makes it possible to implement path­ planning algorithms significantly different in capability from those heretofore reported.

6 Acknowledgement

The support of the Palo Alto VA Hospi tal in this research is gratefully acknowledged. Computational facilities and a super working environment were made available by the Fairebild Laba­ ratory for Artificial lntelligence Research (now apart of Schlum­ berger). Many thanks to Rod Brooks for providing the code for his combinatorial configuration space planning algorithm to serve as a benchmark.

[Author's address: Design Division. Stanford. Cal. 94305.]