A shortest path algorithm for a nonrotating object

IEEE TRANSACTIONS ON SYSTEMS, MAN, AND CYBERNETICS, VOL. 23, NO. 3, MAYIJUNE 1993

825

Correspondence A Shortest Path Algorithm for a Nonrotating Object Among Obstacles of Arbitrary Shapes P. L. Lin and Shyang Chang

Abstract-A shortest path algorithm for motion planning problem via mathematical morphology is proposed. The moving object and obstacles can be of arbitrary shapes of finite size. To speed up the computation of the free positions of a moving object, the idea of shape decomposition is developed. It is proven that the decompositionalways reduces the number of iterations. In addition, a simple geometric algorithm is proposed to check the existence of safe paths between the given initial and final positions. Once the existence condition is verified, a modified grass fire algorithm via the grayscale morphology is then used to compute the shortest path. Finally, examples of various descriptions of moving objects among obstacles are demonstrated.

I. INTRODUC~ION Motion planning problem has stimulated considerable interest in robotics and artificial intelligence [1]-[19]. The main objective of it is to plan a collision-free path for a moving object in a workspace populated with obstacles. Let B represent the moving object. The motion planning problem for B can be formulated as follows: Given the geometric descriptions of a moving object and obstacles in a bounded region, an initial position Z1 and a final position 2 2 of the moving object B, determine if there exists a continuous collisionfree path from 21 to 2 2 , and if so plan such a path [l].Up to now, several algorithms have been proposed to solve this kind of motion planning problem [l],[2]. For example, Lozano-Perez [3], [4] has used the concept of growing transformation on obstacles to simplify the consideration of the collision-free path planning about translational movement of a robot among polygonal obstacles. To improve the efficiency of the growing obstacles, Bhattacharya and Zorbas [13] have suggested a new fast algorithm to compute the collision boundaries of the obstacles grown by a convex polygonal robot. The essential idea of the paper is based on the property that any convex polygon can be expressed as a set sum of line segments and triangles. That is, the robot is first decomposed into a set sum of line segments and triangles. Then the process of growing obstacles by the robot are viewed as a sequence of steps where each step involves growing the partial grown collection of obstacles by a line segment or a triangle. A similar but more generalized solution by using Minkowski operations have also been suggested by Ghosh [ 141. However, these papers have made the assumption that the shapes of robot and obstacles are convex polygonal. Recently, Bajaj and Kim [15] have extended the procedure of growing obstacle to handle the more complex cases that both the boundaries of the moving object and obstacles are represented by segments of algebraic curves. But, the algebraic representations of obstacles and object are also complicated such that the computation of the compatible edges and vertices in their paper are time-consuming. Moreover, the cleanup phase for removing redundancies from its so-called convolution graph is also difficult. Manuscript received May 11, 1990; revised July 1, 1991, and June 26, 1992. The authors are with the Department of Electrical Engineering, National Tsing Hua University, Hsinhu Taiwan, Republic of China. IEEE Log Number 9205801.

Once the grown obstacles have been obtained, the motion planning problem is now translated into a simpler one where the moving object is a point. So far, there are several strategies to calculate a shortest path for a point object [3], [16]-[B]. The basic idea is to construct a visibility graph whose vertices are the initial and final configurations of the moving object together with points on the boundaries of grown obstacles. The edges of the graph represent the line segments joining vertices. Since the moving object and obstacles considered are all polygonal shapes, the shortest paths between two given points must be polygonal lines whose vertices are comers of the given polygonal obstacles. Based on this observation, the 2-D shortest path problem can be immediately reduced to the problem of a discrete graph search and solved in polynomial time. But, as illustrated in [16], if either the shapes of the moving object or obstacles in practice are not polygonal, the construction of the visibility graph will be very difficult. Recently, Lin and Chang [19] have extended the process of growing obstacles to the case that the shapes of the moving object and obstacles are arbitrary. However, the path planning solution is only suboptimal. In this paper, a shortest path problem for a nonrotating body amid obstacles of arbitrary shapes will be addressed. The main ideas are derived from the mathematical morphology. Furthermore, only two set operations of morphology, i.e., erosion and dilation are needed to justify if the moving object will collide with obstacles. In addition, some useful properties of mathematical morphology will enable us to develop techniques for shape decomposition so that the computational time of the free positions of the moving object will be much reduced. After the computation of the free positions of the moving object, the original motion planning problem will be transformed into a simpler one that allows us to treat the moving object as a point. A necessary and sufficient condition for the existence of a collision-free path between the given initial and final positions is also derived. To check the existence condition, a simple geometric algorithm is proposed. Once the existence condition Is verified, a modified grass fire algorithm via the grayscale morphology is then used to compute the shortest path. This paper is organized as follows. The mathematical morphology and its useful properties are presented in Section 11. The existence theorem for safe paths and the shape decomposition of the moving object are described in Section 111. In section IV, the geometric algorithm to check the existence condition and the shortest path algorithm are proposed. Experimental results and discussion are given in Section V. Finally, the conclusion will be given in Section VI. 11. MATHEMATICAL MORPHOLOGY

Mathematical morphology, developed by Matheron [20] and Serra [21], is a set algebra for the quantitative descriptions of geometric structures. In this theory, a signal is always viewed as a set in the space in which the problem is defined and then operated on by set-processing morphological operations. The two fundamental operations of morphology are “erosion” and “dilation.” Algebraically, these two operations are similar to the Minkowski addition and subtraction. However, from the geometric point of view, erosion and dilation are defined as set transformations that shrink or expand a set with respect to a structuring element. In the following, we use the compact set B of R” to denote a structuring element and A C R” the domain set that is operated by morphological operations. The

0018-9472/93$03.00 0 1993 IEEE

Authorized licensed use limited to: Akira Imada. Downloaded on May 15, 2009 at 21:26 from IEEE Xplore. Restrictions apply.

826

IEEE TRANSACTIONS ON SYSTEMS, MAN, AND CYBERNETICS, VOL. 23, NO. 3, MAY/JUNE 1993

The (5) and (6) follow immediately from the definitions of erosion and dilation. They will simplify the motion planning problem later on by allowing us to arbitrarily choose a point of B as the reference point as being located at the origin. As will be illustrated in later sections, this property will make the shape decomposition more easily. Property 2 4 a r a l l e l Composition:

@ 8

A CE (Bi U Bz) = (A CE B I )U (A @ Bz)

(74

A 8 (Bi U Bz) = (A e Bi) n (A e Bz). Algebraically, (7) and (8) tell us that to dilate or erode A with respect to a large structuring element B can be done by first decomposing the structuring element B piece by piece and then combining the intermediate results by union or intersection respectively. These two relations are very useful in the implementation. For example, 1) if the parallel architecture is available, then the computational time of erosion and dilation will be very much reduced. 2) Since A @ { z } = A, and A CE ( B I U B z ) = (A @ BI)U (A CE B z ) , it follows directly

A8a

Similarly, Ae{z} = A_, and Ae(B1UB2)= ( A e B i ) n ( A G B z ) ,

so

Fig. 1. Illustrations of the sets A 8 B and A @ B.

A e B = nA-b. symmetric set

B = {-b

:b E

B} =

u

{-b}.

(1)

b€B

We use B, to denote the translation of B by the vector z E R". Now we are ready to give the definitions of erosion and dilation. Definition 1 (Erosion): A 8 B = { z : B , A}. It is defined as the set of all points z such that the translation of B by z , B,, is contained in the original set A. We express the operation of erosion as the following

c

A e B = { z :B,

c A}

= { z : forevery b E B , b + z E A } .

(2)

Definition 2 (Dilation): A @ B = { z : B, n A # qb}. Dilation is a dual operation of erosion and is defined as follows: A B B = { z : B, n A

# qb}

= { z : B, fi A} = { z : forsomeaEAandbEB,z=a+b}

(9)

b€B

B of B with respect to the origin is defined as

These two altemative forms of erosion and dilation indicate that they can be accomplished by taking the intersections and unions of all the translations of A. When the bit-plane computer is used to implement the operations of morphology, these altemative definitions of erosion and dilation will be more suitable than the previous ones. Property 3: Serial composition: A 8 (BI@ B z ) = (A 8 BI)8 Bz A @ (Bi CE Bz) = (ACE Bi) CE Bz.

(10) (11)

Successively eroding (dilating) a set A first by B1 and then Bz is equivalent to eroding (dilating) A by the dilation set B1 @ Bz. If a pipeline computer is used to perform morphological operations, this technique becomes necessary when the size of structuring element is larger than the capability of input stage of the machine. Property 4: Local knowledge, Let M be a bounded mask and A an object that may be bigger than M. Since ( A n M) 8 B = ( A 0 B) n ( M e B )

(12)

(3)

we can obtain erosion of A inside the mask region M by considering only the original eroded A intersected by a new mask M 8B = M' . Notice that, as implied in the above definitions, only the logical AND and OR operations are required for us to implement the basic morphological set operations on conventional computers. However, in most practical applications, the signals are not binary values. A" @ B = ( A e B)' (4) To extend the power of morphology to these cases, the grayscale where A' denotes the complement of set A. morphology is invoked. In the following, several definitions and properties of the grayscale morphology defined on @ will be listed. Geometrically, (4) implies that eroding the object will be equivalent Definition 3: The umbra U ( Y )of set Y C R3 is defined by dilating the background. Moreover, some important algebraic propU ( Y )= { ( z , y , t ' ) : ( z , y , t ) E Y ; t ' 5 t } . erties of erosion and dilation that are very useful in our application will be stated in the following. All these properties can be derived For each function f defined on R2, the umbra of f is defined as from the definitions of erosion and dilation. Property 1: Translation invariance: where A fi B denotes that set A hits set B. Both the A 8 B and A @ B operations are illustrated in Fig. 1. Based on the definitions above, we can derive the duality relationship between erosion and dilation:

A

e B , = (A e B)-=

A @ B , = (A@B),.

(5) In other words, the umbra of f is a set consisting of the surface of (6)

f and everything below.

Authorized licensed use limited to: Akira Imada. Downloaded on May 15, 2009 at 21:26 from IEEE Xplore. Restrictions apply.

IEEE TRANSACIIONS ON SYSTEMS, MAN, AND CYBERNETICS, VOL. 23,

NO. 3, MAYIJUNE 1993

Definition 4: Let Q be a subset of R3 and P = { (x,y ) E R2 : for some t, (x,y,t) E Q} be the projection of Q on R2. The top surface of Q. denoted by T[Q] : P + R,is defined by

827

given initial position 21 and the destination position 2 2 . In the following discussion, we will assume that 21 and 2 2 belong to the set FP(B). If one of them is not a free position, then it is impossible to find a safe path connecting them. Lemma I : X 8 B = FP(B). 0 Proofi It is straightforward by definitions. Having defined the operations of taking a top surface of a set and Remark: The operation X e B can reduce the original motion the umbra of a function, we now define the grayscale dilation and planning problem for object B of arbitrary shapes to a simpler one erosion. Definition5: Let PI, P 2 C R2, f l : PI + R and f2 : P 2 + R. that can be treated as a point object moving in the region X e B. Theorem I : Given the initial position 21 and final position 2 2 The dilation of fi by f2 is denoted by f l @ f2 : PI @ PZ + R of the moving object B among a set of obstacles, both of arbitrary such that shapes, in a bounded region Y, then there exist safe paths between f1 @ f 2 = T[U[fl]@ U[f2]]. them if and only if 21 and 2 2 belong to a connected subset of X e B . Proof: 1)If there exists a safe path r between 21 to 2 2 , then by Definition 6: Let PI, 9 , fl, and f2 be defined as in Definition 5. The erosion of f l by fz is denoted by f l e f z : Pl e P2 + R definition every point of r belongs to the set FP(B), i.e., r c X 8 B by lemma 1. Since a safe path can be considered as a connected subset such that of X 8 B, so the first part of the proof is complete. 2) If 21 and 2 2 belong to a connected subset of X 8B , then by definition we can find a connected path r from 21 to 2 2 such that all points of I? are To implement the grayscale dilation and erosion, the following in this subset. That is, I? is a safe path. 0 two propositions will be useful. Notice that this theorem allows us to test the existence of a safe Proposition 1: Let f l , fz, PI, and PZbe defined as in Definition path for arbitrary shapes of moving object and obstacles. There are 5. Then two crucial steps in this theorem to check the existence of safe paths: 1) calculate the set X 8 B and 2) test if 21 and 2 2 belong to some connected subset of X 8 B. The procedure for testing if 21 and Proof: See [21], [22]. 0 2 2 belong to a connected subset of X 8 B will be discussed in Proposition 2: Let f 1, fz, PI, and P2 be defined as in Definition the next section. The procedure for calculating the set X e B in our system can be performed as follows. Three image planes are 5. Then used for 1) the free space X, 2) the structuring element B, and 3) the intermediate results respectively. According to (lo), the set X is shifted to x-b where b E B. Notice that B is a collection of Proof: See [21], [22]. 0 finite elements on image plane. Thus, only a finite number of shifted From the two propositions above, we observe that grayscale versions of x-b is required. The third plane holds the intermediate dilation and erosion have the same complexity as convolutions. results of the parallel logical AND operations of the shifted versions However, instead of taking the summation of products in convolution, of the image X-b. In other words, the number of parallel AND the maximum of sums or minimum of differences are performed re- operations needed for implementing erosion depends only on the size spectively. Furthermore, by using the definitions of grayscale erosion of structuring element B. In practice, only the boundary of B will and dilation, it is straightforward to extend the useful properties of be required. The following theorem illustrates this point. binary morphology to the grayscale morphology. Theorem 2: r in X 8 B is a safe path if and only if it is also a Remark: In practice, if the workspace considered is a subset of safe path in X 8 d B , where d B denotes the boundary of B. the Euclidean grid space Z",then only the grid points of R" will be Proof: 1) Since d B c B, it is clear that X e B C X e d B used in the morphological operations mentioned above. from the definition of erosion. So, if r is a safe path in X e B, then it implies that r C X e B c X 8dB. That is, r is a safe path in 111. EXISTENCETHEOREM AND SHAPE DECOMPOSITION X e d B . 2) Next, we will show that if I? is a safe path in X e dB, Recall that our objective is to find a shortest path that allows the then it is also a safe path in X e B. a) Assume that the sizes of the moving object B to be translated from Z1to 2 2 without colliding obstacles are large enough so that no obstacle can be enclosed by with any obstacles. Here the positions Z1 and 2 2 can be viewed the boundary d B . If r is a safe path in X e d B , then every point c as the positions taken by the reference point of B. In the following of r is a free position of dB. Since no obstacle is contained in B, discussion, let Y denote the workspace and X the points in Y, which the point x is also a free position of B , Le., x E X e B. b) There are not occupied by the obstacles. X will be called the free space. exist some obstacles whose sizes are small enough so that they can reside in the interior of B. If r is a safe path in X 8 d B , then for The complement of the set X in Y will be denoted by X " . Definition 7: The set of free positions of B, denoted by FP(B), any point z of r, the set dB, can not enclose any small obstacle in is defined as the collection of all those points in X at which the it. Otherwise, when d B moves from the initial position 21 to x, the reference point of B can occupy and does not collide with the boundary of B must cross over the small obstacle. This is contrary to the definition of a safe path. Hence, point z is a free position of obstacles. O Definition 8: When an object B can be translated without collid- B. In general, the geometric description of the structuring element B is ing any obstacles from the starting position to the final position along given before planning the shortest path, so the shape decomposition of a continuous path, then it is called a safe path. It is obvious that a path r is safe if the points of I? are contained B into pieces of even smaller structuring elements via morphological in the free positions of B. From the geometric point of view, the operations can be made in advance to speed up the calculation path r is safe if all the regions B,, Vz E r, do not intersect the of X e B. The remainder of this section will be devoted to region X'. We are now ready to state a necessary and sufficient the discussion of shape decomposition. These smaller pieces of condition that determines the existence of a safe path between the structuring elements will be called the "basis of Brrin the following

Authorized licensed use limited to: Akira Imada. Downloaded on May 15, 2009 at 21:26 from IEEE Xplore. Restrictions apply.

IEEE TRANSACTIONS ON SYSTEMS, MAN, AND CYBERNETICS, VOL. 23, NO. 3, MAYIJUNE 1993

828

discussion. Theoretically, there may exist many different bases for a given object B. The selection depends heavily on the system architecture available [24], [25]. For instance, as in our system, a bit plane type computer is used, then the ones that require fewer number of the parallel logical AND/OR computations will be selected. Since the moving objects considered in this paper are assumed to be of arbitrary shapes, it is very difficult to derive a systematic method to find the best decomposition [25]. However, if the shapes of the moving objects are parallelogram or parallelepiped, then the basis of B obtained by the proposed shape decomposition technique can be proved optimal by using the approach proposed in [%I. Later in this section, we will prove and demonstrate via examples that the shape decomposition techniques always reduce the iteration number of AND/OR operations. If the obstacles are irregular or the number is large, then decomposing the moving object will be better. If the number of obstacles is small and their shapes regular, then we can apply the shape decomposition to obstacles instead of the irregular moving object. Hence, without loss of generality, we will assume in the following that the shape decomposition technique is applied to the moving object B. Two cases will now be considered for the decomposition. Case 1: The structuring element B is decomposable, i.e., it can be directly decomposed into the set sum of smaller structuring elements B1, Bz, B3,...,Bn:

........... ........... ........... ........... ........... ........... ........... Fig. 3. Illustration of the decomposition technique.

+

B;(= Bk+l),i.e., the proof of n = k 1 case will be similar to the 0 case for n = 2. Thus, the theorem is proved. For example, as shown in Fig. 2, the rectangle can be decomposed into the set sum of two line segments. Obviously, this decomposition will reduce the number of parallel AND operations from 95 (= 8 x 12- 1)to 18 (= 11+7), that is, 80% of the computation time will be saved. Similarly, a square with side length n can be decomposed B = B1 @ BS 8 B3 @ ... @ B,. into the set sum of its two connected sides such that the number of AND operations needed for implementing erosion or dilation will be If the moving object B is a parallelogram or parallelepiped, then reduced from n2 - 1(= n x n - 1) to 2n - 1(= n n - 1). As illustrated in the example, if the moving object B is decomposthe optimal shape decomposition, in the sense that the minimal number of AND operations is needed for growing the obstacles, can able, then part of its boundary, i.e., two line segments will be adopted be proved by using the approach proposed in [24]. If the moving as the smaller structuring elements for calculating FP( B ) . Case 2: The structuring elements B is not decomposable. object B is a convex polygon, then it can be directly decomposed For example, as shown in Fig. 3, the object B can not be directly into a set sum of line segments and triangles [13], [23]. In addition to convex polygons, some irregular objects, such as S5 and S4 illustrated decomposed by using only the serial composition property. In this in Fig. 11, can also be directly decomposed into a set sum of curve case, the moving object B will be written as the union of disjoint subsets, Le., B = B1 U B2 U . . . U B,, where B, n B, = 4 for segments. Lemma 2: If B = HI@H2 @. ..@ H,, then there is an equivalent 1 5 i , j 5 n. Two situations may occur in this case. (a) Each subset B,, 1 5 z 5 n, is decomposable. If each B, is decomposition B = { z } @ B1 @ B2 @ . .. B,, where # B , = # H, and the origin o E B, Vi = 1,. n. Here # B , denotes the cardinal decomposable, then their shape decompositions B,, , as described in Case 1, can be calculated and in turn used in the computation of number of set B,. Proof: See [24]. 0 FP(B). Theorem 4: If the set B is equal to the union of disjoint decomposRemark: If a point b E B is chosen as a reference point of B, i.e., o E B, then it is easy to prove that the point z in lemma 2 will able sets B,, 1 5 i 5 n, then the number of parallel AND operations needed for the computation of F P ( B ) using the decomposition be equal to 0 . will beless thanwithout In the following theorem, we will show that the shape decompo- (Bll@...$Bl,)U...U(B,l$...B,,) sition techniques always reduce the iteration number of operations. the shape decomposition. 0 Proof: It is straightforward from Theorem 3. Theorem 3: Suppose the compact set B can be decomposed into For example, the moving object B shown in Fig. 3 will be the set sum of B1,B2,.... B,, Le., B = B1 @ B2 @ . @ B,, then the number of parallel AND operations needed for the computation of first divided into three smaller structuring elements based on the F P ( B )using B1 @ Bz @ . . @ B, will be less than the original one. parallel composition property. Then each smaller structuring element Proof: We will prove this theorem by mathematical induction. is further decomposed into even smaller structuring elements by using First consider the case n = 2. Based on the result of the serial composition property. It is clear that this representation of R lemma 2, we can assume that B , B1 and Bz all contain will make the number of the parallel logical AND operations needed the origin. Let B1 = { o , b l l , b 1 2 , . . . , b 1 ~ } and Bz = for the computation of F P ( B ) reduce from 699 (= 20 x 30 10 x 5 10 x 5 - 1) to 76 (= 48 13+ 13 2). The number 2 in the last ( 0 , b S 1 , b22,. ... bzp}. By the definition of dilation, B is a disjoint unionof { 0 , b l l , . . . , b l ~ , b 2 l , . . . , b 2 ~ } and{b: b = b l , + b 2 3 and parenthesis is the two AND operations for the intermediate results. b 4 B1 U B2,1 5 i 5 m 1 5 j 5 p}. Obviously, #B1 = m 1, Notice that only 11% of the original computational time is needed if #Bz = p + l , and # B 2 m + p + l . According to eq. (lo), it is clear the basis of B is adopted in the implementation. that the iteration number of parallel AND operations needed for the @) Some Bis are not decomposable. If some Bis cannot be calculation of F P ( B ) using B1 @ Ba will be less than that without directly decomposed, then their boundaries d B , can be adopted to shape decomposition. Now suppose it is true for n = k . Then for the reduce the iteration number of parallel AND operations. Theorem 5: Suppose B = B1 U B2 U ... U B,, where B,, 1 5 k 1 case, B = B1 @ BS@ ... @ Bk @ Bkl. Obviously, the set B can be viewed as the set sum of B$(=B1@ Bz @ . . . @ B k ) and i 5 n, are disjoint subsets. Without loss of generality, we can assume

+

...

..

+

+

+

+

+

Authorized licensed use limited to: Akira Imada. Downloaded on May 15, 2009 at 21:26 from IEEE Xplore. Restrictions apply.

+

+

IEEE TRANSACTIONS ON SYSTEMS, MAN, AND CYBERNETICS, VOL. 23, NO. 3, MAYIJUNE 1993

B1

B2

B3

&.& B2

B3=

B3

I $ /

(c)

Fig. 4. Illustration of the decomposition technique.

Fig. 5. (a) The original structuring element. @) The equivalent one of (a). (c) The shape decomposition of (b).

that the first k B:s, 1 5 j 5 k < n, are decomposable subsets. Then, the iteration number of the parallel AND operations will be reduced if the shape decomposition of equivalent object B*, i.e., ( B 1 1 $ . . . $ B l , ) U . * . U ( B k l $‘Bkj)UbBk+l U a B k + Z U * . .uaB,, is used in the computation of F P ( B ) . Proof: It follows from Theorems 3 and 4. Now, let us consider another example. The structuring element B in Fig. 4(a) can not be written as the previous case. It is first transformed to an equivalent object as illustrated in Fig. 4(b). This transformation is justified by Theorem 2, i.e., whether the structuring element will contact with obstacles is completely determined by its outer boundary. Obviously, the equivalent object B* consists of three essential parts: a rectangle and two semicircles B2 and B3. The rectangle can in tum be decomposed into the set sum of two line segments. In addition, if the locations of the centers of BZ and B3 are denoted by p 2 and p3 respectively, then these two identical semicircles B2 and B3 can also be decomposed into the set sum of semicircle and the two points set { p z , p 3 } . Since the semicircle can not be decomposed furthermore, so their boundaries will be used to represent them according to Theorem 2. Fig. 4(c) indicates the final result of the equivalent representation of B. As expected, the parallel AND operations needed for the computation of F P ( B )will be much reduced.

Iv. VERIFICATION OF EXISTENCE CONDITION AND SHORTEST PATH ALGORITHM

Once the set F P ( B ) has been obtained by using the shape decomposition method, we have to check if the initial position 21 and the final position 2 2 are in the same connected subset of X 0 B. If exists, then we are interested in finding its shortest path. In the following, a simple geometric algorithm, called TESTCON, will be proposed to check if the points 21 and 2 2 are in a connected subset of X 8B. First, some definitions and notations will be introduced for the algorithm of this section. Referring to Fig. 5, let L denote the line segment from 21 to 2 2 . Let us imagine that a point is moving from 21 to 2 2 along L. The points where it hits the obstacles are called the H points. Similarly, the points where it leaves the obstacles to continue its motion toward 2 2 are called Q points.

{Hi,

4,

sAk,

e

a;

Algorithm TESTCON:

1) Draw a straight line L from the initial position position 2 2 .

2) If L doespot intersect with any obstacles, then it means that the shortest path L is found and stop. 3) If L intersects with the obstacles, then the coordinates of the H , points and Q; points, i = 1,2, ..., are sequentially recorded into an array. (In case that L intersects the obstacle tangentially, then the H point and Q point are considered as the same point and will be ignored in the following test.) 4) For each H,, determine if there exists Q j points, j > i , which is connected to the H ; point via the boundary of the associated obstacle. 5) Let H , = H , and Qz = Qt,Aifthe answer of step 4 is No!. Otherwise, let H , = H , and Qt = Qj where j is the maximal index of 0:s found in step 4 , i.e., the points H , and Q n t , i < n 5 j i 5 m < j , will be ignored (see Fig. 5). 6) For each pair of H , and Q;, 1 5 i 5 k, the points Qf} will divide the boundary of the i - t h obstacle 0, into two parts, say dOil and 8 0 , ~ . 7) Test if a point moving object can be moved from H,to Vi 1 5 i 5 k. along either d0,1 or 8 ) If the answer of step 7 is positive for each pair H , and Qt, then it means that the points 21 and 2 2 are in a connected subset of X B. Otherwise, we claim that there exists no collision-free path between 21 and 2 2 . Theorem 6: Let L, H;, Q;, aOt1, and d0,2 be defined as in TESTCON algorithm. Without loss of generality, we assume that there are k pairs of H , and Qz. Then a point moving object can be moved from H zto Q i along either aO;1 or 80;2, Vi 1 5 i 5 k, if and only if the points- 21 and 2 2 are in a connected subset of X 8B. Proof: 1) Let QO and Hk+l denote the positions 2 1 and ZZ respectively. By the definitions of L, H ; and Q;, it is clear that the line segments from from Q j to Hj+1, 0 5 j 5 IC, are all contained in the set X 8 B. That is, each pair of the points Q, and H , + I , 0 5 j 5 k, are in a connected subset of X €3 B. Now, if a point moving object can be moved from H , to Q c along either d 0 , l or i30;2, V i 1 5 i then it means that there exists a collision-free path from H; to Qs, that is, H;and Q;, V i 1 5 i 5 k , are located at the same connected subset of X B. Based on these observation, we get that Z,(= Qo) and 2 2 ( = H k + l ) are in a connected subset of X 8B . 2) Suppose there is a pair of Qt points, between which a point moving object cannot be safely moved from H , to Q t along either dO;1 or a 0 i 2 , as shown in Fig. 6. Obviously, this implies that the associated obstacle 0; will separate the set X @ B into

21

to the final

I

Authorized licensed use limited to: Akira Imada. Downloaded on May 15, 2009 at 21:26 from IEEE Xplore. Restrictions apply.

--

830

IEEE TRANSACTIONS ON SYSTEMS, MAN, AND CYBERNETICS, VOL. 23, NO. 3, MAY/JUNE 1993

Cl

P

f . a

~

~

(b) Fig. 6. Illustration of the TESTCON algorithm

two disjoint parts. One of them will contain H, and Z1. The other contains QEand 2 2 . In other words, 2 1 and 2 2 are not contained in a connected subset of X e B. 0 Once we have verified that 2 1 and 2 2 belong to a connected subset of X 8 B, the remaining task for our motion planning problem is to compute the shortest path between them. In this paper, a modified grass fire algorithm will be proposed. It can be stated as follows. Think of X 8 B as a meadow. W o different fires are lit simultaneously at the positions 2 1 and 2 2 respectively. Since 2 1 and 2 2 belong to the same connected subset of X e B, the firfronts will meet each other in finite time. To implement the modified grass fire algorithm, two grayscale functions are defined as follows (see Fig. 7). fl(X)

and

= 1 0

{

c1

c2

z E X e B , z # & andz#Z2 ~ E ( X ~ B ) ~ z = 21 z = 2 2 , where c1 and c2 are integers and c1 > c2 > 1

fz(z) = 0 z E D, where D is the unit disk centered at origin. Conceptually, the two positive integers, c1 and c2 where c1 > c2 > 1, are used to represent the different fires initiated from 2 1 and 2 2 respectively. The grayscale function fl is similar to the indicator function of the set F P ( B ) , except that two larger integer values are now assigned to 2 1 and 2 2 . The second function f 2 is nothing but a zero function defined on a unit disk D. From the geometric point of view, the operation ( f l @ f2)(z) will find the maximum value from the unit circular region centered at z, denoted by D,, and assign it to the point z. This is clear from Proposition 1. In other words, the grayscale dilation f1 @ fz will enable the region D,,n (X e B) to change its value from 1 to c1, and simultaneously the region D,, n (X 8 B) from 1 to CZ. In Fig. 7(a), the functions fi and f 2 are defined in R and unit disk D respectively. Fig. 7@) indicates the regions D,,(= A:) and D,, (= A:) after the operation

@)

(p))

Fig. 7. Illustrationsof the operation f1 (R) €9 fz ( D )and ( f i (R)€9 [2 @f2 ( D ) where the former is denoted by dotted line and the latter by solid h e .

fl @ f2. Similarly, one more grayscale dilation, that is, ( f l @fz ) @ f 2 will change the values of the set A: -A: to c1 and the set A: - A: to c2, . . . ,etc. From the grass fire point of view, the sets A: and A: can be viewed as the regions reached by the two different tires initiated from the positions 2 1 and ZZ after one unit of time respectively. Similarly, the sets A: - A: and A: - A: can be viewed as the regions reached by the two different fires in the second unit time interval, etc. Since 2 1 and 2 2 belong to the same connected subset of X e B, the fronts of fire c1 and fire c2 will meet each other in finite time. The intermediate results of this algorithm, Le., the A: sets, will now be used to calculate the shortest path. Remarks: (1) The implementation of the grass fire algorithm via grayscale dilation is algebraically equivalent to the parallel logical OR operation, except that now two integers c1 and c2 are used to represent the logical 1’s. This fact indicates that the computational complexity will not be increased by introducing the grayscale morphology. (2) The modified grass fire algorithm is also suitable for checking the existence condition of the safe paths between 2 1 and 2 2 , although it is less efficient than the TESTCON algorithm. In our system, there are four image planes. As mentioned before, three of them are used to implement the morphological dilations. The fourth image plane P,which initially contains the set X 8 B is now used to keep the distance information. It works as follows. As explained in the last paragraph, the sets A: and A$ are the collections of points invaded by the fires after one unit of time, that is, the boundaries of sets A: and A: contain the points that are all one unit away from 2 1 and 2 2 respectively. The boundaries of sets A: and Ai are two units away from 2 1 and 2 2 respectively, etc. Based on this property, the points that correspond to the boundaries of sets Ai and Ai are marked with index number i in the image plane P. Once the implementation of the grass fire algorithm has been completed, the plane P will contain an “index function” that is defined on a subset of X e B and the functional value at each defined point is exactly equal to the distance away from 21or 2 2 . Since two different fires are lit

Authorized licensed use limited to: Akira Imada. Downloaded on May 15, 2009 at 21:26 from IEEE Xplore. Restrictions apply.

831

IEEE TRANSACTIONS ON SYSTEMS, MAN, AND CYBERNETICS, VOL. 23, NO. 3, MAYIJUNE 1993

........ .............. 4 ' . . . * . . .

............. ............. ............. ............. ............. ............. ............. 1 1 1 . 1 1

1

1

1

1

1

1

1

Fig. 8. The structuring element used to approximate the unit disk in implementation.

simultaneously at 21, 2 2 and the bounded set X aB is invaded in all directions with unit speed, so the two fronts of fires will meet at one or more points in finite time. One of these points, denoted by 2, will be chosen for the shortest path planning. Intuitively, we can think of the domain of the index function as the level curves and the position 2 as the mountain top, then the shortest path rl between 2 and 21 is the path that takes the direction of gradient. In other words, it starts from the position 21 and move toward 2 in the gradient direction of the level curve. Similarly for the shortest path r2 between 2 and 2 2 . We contend in the following theorem that the shortest path between Z1 and Z2 is exactly equal to the union of rl and l?z. Theorem 7: Assume there exist safe paths from 21 to 2 2 . Let 2, rl and r2 be defined as before, then the shortest path between 21 and 2 2 is equal to the union of l?l and I?*. Proof: Without loss of generality, let us assume that there is only one shortest path between 21 and 2 2 . Let R denote the collection of points that are equidistant from 21 and 2 2 in X 8 B. Clearly, 0 is nonempty by assumption. Obviously, the middle point q* of the shortest path between Z1 and 2 2 belongs to the set R.Moreover, the distance between q* and 21 (or 2 2 ) is shorter than the distance between any other point q E R and 21 (or 2 2 ) . Recall that in the grass fire algorithm, two fires initiated from 21 and 2 2 simultaneously invade the set X e B in all directions with unit speed. 2,by definition, is at the position where the fronts of two fires first meet. This implies that 2 E R. Now, if 2 # q*, then there exists another point q E R, which is closer to 21 than 2. Hence, the fronts of two fires will reach the point q before they reach the position 2. This is contradictory to our definition of 2. So we get 2 = q * . Moreover, the procedure for calculating the path rl(r2)using the gradients guarantees that it is the shortest path between 2 and 21(22). By uniqueness, l?l U l?z will be the shortest path. In case there are more than one shortest 0 path, the proof is similar. Before leaving this section, let us summarize the procedures of the proposed algorithm. Step 1: Decompose the shape of moving object B into a series of smaller structuring elements, Le., basis of B, via the binary morphological operations. Step 2: Define a binary function to indicate the free space X. Step 3: Calculate the free positions of the structuring element B , i.e. F P ( B ) ,by eroding the set X with the the basis of B obtained in Step 1.

Fig. 9. The configuration of the moving object and obstacles in example 1.

Fig. 10. The shortest path and the grown obstacles (shaded area) in example 1.

Step 4: Determine the existence of safe paths between 21 and 2 2 via the TESTCON algorithm. Step 5: Claim that no safe path exists if the answer of step 4 is No. Otherwise, go to next step. Step 6: Define a grayscale function on the set F P ( B ) with values 1 on it and the values c l , c2 to positions 21 and 2 2 respectively. Step 7: Apply the grass fire algorithm to compute one of the shortest paths between them.

v.

EXPERIMENTAL RESULTSAND DISCUSSION

In this section, two examples are given to demonstrate the novelty of the shortest path algorithm. All of our experiments are performed on a PC-AT and the series 151 Image processor of IMAGING Technology Inc. The clock speed of the system is 8 MHz. As mentioned in Section 111, the four image planes enable us to implement the morphological erosion and dilation operations according to (9) and (10). To speed up the computation of F P ( B ) ,the moving objects are all decomposed into small structuring elements in the way discussed in Section 111. The workspaces considered in our experiments are in a square region of 400x400 pixels. The structuring element within

Authorized licensed use limited to: Akira Imada. Downloaded on May 15, 2009 at 21:26 from IEEE Xplore. Restrictions apply.

832

IEEE TRANSACTIONS ON SYSTEMS, MAN, AND CYBERNETICS, VOL. 23, NO. 3, MAYIJUNE 1993

s2

s1

s3

SI

s5

=

s4

--$--$--$-a

s5

t

z h e 1 31

Fig. 11. The decomposition of the moving object in example 2.

Fig. 12. The configuration of the moving object and obstacle in example 2.

a 15x15 mask (see Fig. 8) will be used to approximate the unit disk in the grass fire algorithm. The implementation of the grass fire algorithm via grayscale dilation is algebraically equivalent to the parallel logical OR operation. The time needed to implement a parallel logical AND/OR operation for two shifted versions of the workspaces is about 100 msecs. Thus, the time needed to implement fl (R) @ f2 ( D ) operation is about 17.6 s. In addition, on the average, 1.2 s are also needed for us to record the distance information of the grass fire algorithm such that shortest paths from the given initial to the final positions can be easily calculated if the existence condition has been verified.

Fig. 13. The shortestpath and the grown obstacle (shaded area) in example 2.

Example I: The shape of the moving object B and its shape decomposition are shown in Fig. 3. As mentioned in Section 111, this representation of B will make the number of the parallel logical AND operations needed for the computation of F P ( B ) reduce from 699 to 76. Fig. 9 illustrates the configuration of the obstacles and the given initial and final positions of the moving object. The dot on the moving object represents the position of the selected reference point. To check if Z1 and 2 2 are in a connected subset of X 8 B , TESTCON algorithm is invoked. The checking time for this algorithm takes about 4 s. Then the grass fire algorithm is applied to calculate the shortest path. The time required is about 169(= (17.6 1.2) x 9) s. Referring to Fig. 10, the gray regions represent the set X- F P ( B ) . The blurred “rings” represent the intermediate results produced during the execution of the grass fire algorithm. The shortest path is denoted by the white curve. It is worth mentioning that because the diameter of selected “unit” disk is not large enough, so the shortest path derived from the grass fire algorithm may deviate slightly from the true one. This is the reason why the line segment near the position 2 2 is only nearly straight. Example2: The main purpose of this example is to show that our proposed algorithm can also be applied to solve a more general containment problem than that in [14]. The containment problem in [14] is to decide whether a polygonal object B can be translated to fit inside another polygonal object A, and if so then compute the set of safe positions where B can fit inside A. Obviously, our approach can extend the containment problem to the more general case of objects of arbitrary shapes. Furthermore, using our approach, we can solve the shortest path problem once a point in the safe positions is indicated. Now let the object A be the obstacle and we will try to find the safe positions surrounded by A and move B to the position 2 2 along a shortest path. As done in example 1, the moving object B is first decomposed into smaller structuring elements, as illustrated in Fig. 11. Similarly, this decomposition will reduce the number of parallel logical AND operations needed for calculating F P ( B ) from about 900 to 130. Fig. 12 is the configuration of the obstacle and the moving object. In this example, TESTCON algorithm takes about 4 s. The grass fire algorithm takes about 132(= (17.6 1.2) x 7) s. Referring to Fig. 13, the grown obstacle is represented by the union of gray and black regions. The whiter region inside the obstacle is the set of safe positions. The shortest path is displayed by the white curve in Fig. 13. Discussion: From the definitions in Section 11, we know that the morphological operations can be extended to handle the three-

Authorized licensed use limited to: Akira Imada. Downloaded on May 15, 2009 at 21:26 from IEEE Xplore. Restrictions apply.

+

+

IEEE TRANSACTIONS ON SYSTEMS, MAN, AND CYBERNETICS, VOL. 23,

NO. 3, MAYIJUNE 1993

dimensional (3-D) problems. In other words, if the motion planning problem considered is defined in a discrete 3-D Euclidean space Z 3 , a unit ball will be used to replace the unit disk so that the grass fire algorithm is still applicable in Z 3 . Similarly, the shape decomposition techniques can also be applied in Z 3 .Notice that since the TESTCON algorithm cannot be extended to handle the 3-D problem, the grass fire algorithm will be used to 1) check the existence condition of the safe paths and 2) calculate the shortest path if it exists. This capability of extension is very appealing. Moreover, instead of using algebraic equations, the idea of shape decomposition is proposed so that a) the growing obstacles procedure can be implemented very efficiently, b) there is no need to determine the boundary of the grown obstacles. Finally, the construction of the visibility graph, which is one of bottlenecks of the path planning problem [16], is not needed in our algorithm. More importantly, the extension to the arbitrary shapes of obstacles/object will make the motion planning problem more realistic than many existing ones.

VI. CONCLUSION A geometric algorithm for planning shortest paths in a workspace populated with obstacles of arbitrary shapes has been presented. To speed up the computation of the free positions of a moving object, the idea of shape decomposition has been developed. We have proved and demonstrated via examples that the shape decomposition always reduces the number of iterations. Moreover, a simple geometric algorithm TESTCON has also been proposed to check the existence of safe paths between these two points. In addition, a modified grass fire algorithm that two different fires are simultaneously lit at initial and final positions has been developed to compute the shortest path if it exists. The advantages of this algorithm are as follows. 1) It can be easily implemented by using only the grayscale dilation, which is equivalent to the parallel logical OR operation. 2) It can be extended to handle the 3-D motion planning problem. Finally, different examples have been given to illustrate that not only the shortest path but a more general type of containment problem can be solved via the same technique.

REFERENCES

833

C. O’Dunlaing, M. Sharir, and C. Yap, “Generalized Voronoi diagrams for a ladder-Part I: Topological analysis,” Comm. Pure Appl. Math., vol. 39, pp. 423-483, 1986. D. Leven and M. Sharir, “planning a purely translational motion for a convex object in two-dimensional space using generalized voronoi diagrams,” Discrete Comput. Geom., vol. 2, pp. 9-31, 1987. 0. Takahashi and R. J. Schilling, “Motion planning in a plane using generalized Voronoi diagrams,” IEEE Trans. Robotics Automat., vol. 5, pp. 143-150, Apr. 1989. B. K. Bhattacharya and J. Zorbas, “Solving the two-dimensional findpath problem using a line-triangle representation of the robot,” J.Algorithms, vol. 9, pp. 449469, 1988. P. K.Ghosh, “A Solution of polygon containment, spatial planning, and other related problems using Minkowski pperations,” CVGIP, vol. 49, pp. 1-35, 1990. C. L. Bajaj and M-S Kim, “Generation of configuration space obstdcles: The case of moving algebraic curves,’’ Algorithmica, vol. 4, pp. 157-172, 1989. J. Hershberger, “An O(n2) shortest path algorithm for a non-rotating convex body,” J. Algorithms, vol. 9, pp. 18-46, 1988. M. Sharir and A. Schorr, “on shortest paths in polyhedral spaces,” SIAM J . Comput., vol. 15, pp. 193-215, Feb. 1986. M. Sharir, “On shortest paths amidst convex polyhedra,” SIAM J. Comput., vol. 16, pp. 561-572, June 1987. P. L. Lin and S. Chang, “Collision-free path planning among obstacles of arbitrary shapes: An approach via mathematical morphology,” IEEE Trans. Pattern Anal. Machine Intell., submitted for publication. G. Matheron, Random Set and Integral Geometry. New York: Wiley, 1975. J. Serra, Image Analysis and Mathematical Morphology. New York: Academic, 1982. R. M. Haralick, S. R. Stemberg, and X. Zhuang, “Grayscale morphology,” in Proc. IEEE Conf Comput. vis. Pattern Recogn., Miami, FL, pp. 453-550, June 1986. P. K. Ghosh, “A mathematical model for shape description using minkowski operators,” CVGIP, vol. 44, pp. 239-269, 1988. X. Zhuang and R. M. Haralick, “Morphological structuring element decomposition,” CVGIP, vol. 35, pp. 370-382, 1986. 3. Xu, “Decomposition of convex polygonal morphological structuring elements into neighborhood subsets,” IEEE Trans. Pattern Anal. Machine Intell., vol. 13, pp. 153-162, Feb. 1991.

Control of Cooperative Multiple Flexible Joint Robots

J. T. Schwartz and M. Sharir, “A survey of motion planning and related geometric algorithms,” Artificial Intell., vol. 37, pp. 157-169, 1988. G. T. Toussaint, Computational Geometry. New York North-Holland, 1985. T. Lozano-Perez and M. A. Wesley, “An algorithm for planning collision-free paths among polyhedral obstacles,” Comm.ACM, vol. 22, pp. 56C.570, Oct. 1979. T. Lozano-Perez, “ Automatic planning of manipulator transfer movements,” IEEE Trans. Syst., Man, Cybern., vol. 11, pp. 681498, 1981. J. T. Schwartz and M. Sharir, “On the piano movers’ problem-Part I: The case of a two-dimensional rigid polygonal body moving amidst polygonal harriers,” Comm.Pure Appl. Math. 36, pp. 345-398, 1983. R. A. Brooks, “Solving the find-path problem by good representation of free space,” IEEE Trans. Syst., Man, Cybern., vol. 13, pp. 190-197, 1983. J. Borenstein and Y. Koren, “Real-time obstacle avoidance for fast mobile robots,” IEEE Trans. Syst., Man, Cybern., vol. 19, pp. 1179-1187, 1989. D. Leven and M. Sharir, “An efficient and simple motion planning algorithm for a ladder moving in the two-dimensional space amidst polygonal barriers,” J. Algorithms, vol. 8, pp. 192-215, 1987. R. T. Chien, L. Zhang, and B. Zhang, “Planning collision-free paths for robotic arm among obstacles,” IEEE Trans. Pattern Anal. Machine Intell., vol. PAMI-6, pp. 91-96, Jan. 1984.

Shaheen Ahmad

Absfruct- The problem of controlling multiple flexible joint robots during cooperative manipulation of a rigidly grasped common load (denoted as the CMFJR problem) is addressed. The dynamic model of the CMFJR shows the interaction between the manipulators in the closed chain is analogous to a single manipulator interacting with a frictionless (algebraic constraint) surface, while in motion along the surface. It is assumed that each flexible joint robot is equipped with joint velocity and position sensors and also actuator mounted position and velocity sensors. It is shown that it is not necessary to employ force sensors mounted on the robot wrist in order to track a desired load trajectory and an internal force trajectory. The transient response of the position error and the internal force e m r can be arbitrarily assigned even though the force sensors are not employed.

Manuscript received May 4, 1991; revised August 14, 1992. The author is with the Real-Time Robot Control Laboratory, School of Electrical Engineering, Purdue University, West Lafayette, IN 47907. IEEE Log Number 9206192.

0018-9472/93$03.00 0 1993 IEEE

Authorized licensed use limited to: Akira Imada. Downloaded on May 15, 2009 at 21:26 from IEEE Xplore. Restrictions apply.