Journal of Intelligent and Robotic Systems 21: 29–50, 1998. c 1998 Kluwer Academic Publishers. Printed in the Netherlands.

29

Robot Path Planning Using Fluid Model Z. X. LI and T. D. BUI Department of Computer Science, Concordia University, 1455 de Maisonneuve West, Montr´eal, H3G 1M8, Canada. (Received: 15 October 1996; in final form: 21 May 1997) Abstract. This paper presents a numerical potential function for point-robot path planning in configuration space based on the theory of fluid mechanics. Ideal fluid is first simulated using Poisson’s equation and heuristic path planning algorithms are established by comparisons of the velocity potentials. Several computational techniques are experimented and compared. A bitmap collision detection technique is proposed for non-point robots. This fluid model creates an environment which is not only free of local minima but also beneficial for navigation control. Key words: robot path planning, fluid model, numerical potential method, Poisson’s equation

Introduction Khatib [4] led the field of numerical potential for robot path planning by using a charge distribution model. Subsequently, a variety of potential functions similar to his scheme (i.e., two artificial forces one attractive the other repulsive) are used in [1, 5, 11, 12]. More recently, Connolly and Burns [2] reported a potential field model satisfying Laplace’s equation together with Dirichlet boundary conditions. The solutions to Laplace’s equation are composed of harmonic functions. Since harmonic functions satisfy the min-max principle, spontaneous creation of local minima within the region is impossible if Laplace’s equation is used. Connolly and Burns proved that every critical point in the concerned domain must be an isolated saddle point, from which streamlines may be easily found. Diffusion equation which is closely related to Laplace’s equation is also used in [13, 15]. In this paper, a numerical potential technique is proposed, in which Poisson’s equation is first used to simulate ideal fluid in a predefined domain, then velocity potentials on the streamlines are analyzed and used for robot global path planning. Since Poisson’s equation is a member of the Laplace family and its solutions are harmonic except at those singular points, i.e., sources and sinks, the advantages that a harmonic function has are preserved. Because sources and sinks can be conveniently used for the starting positions of robots and their destinations respectively, no superposition such as stated in [2] is further needed for path planning. Also, a bitmap collision detection technique is proposed for metric space.

VTEX(P) PIPS No.: 141641 MATHKAP JINT1386.tex; 18/11/1997; 11:45; v.7; p.1

30

Z. X. LI AND T. D. BUI

1. Simulation of Ideal Fluid Let Ω be a closed domain and subdivided into a grid of (M − 1) × (M − 1) small rectangles. Two interior sources (S + , S − ) are initialized in Ω. Assume S + has positive volume flow rate (i.e., the source) and S − has negative volume flow rate (i.e., the sink). In order to satisfy the conservation law in Ω, we define R(S + ) = −R(S − ),

(1)

where R(P ) represents the volume flow rate at point P . Hence, we have (see [7, 10]) Z

b dV = 0, R

(2)

Ω

P

n b = where R i=1 R(Si ), n is the number of sources in Ω and V stands for volume of the flow. Equation (2) states that the net flow rate in Ω is zero. This compatibility condition ensures a solution in Ω. Let φ denote the velocity potential. From theoretical fluid mechanics (see [9]), φ is defined as

−φ =

Z

B

qs ds,

(3)

A

where qs is the velocity along AB . Hence ∂φ (4) . ∂s In an ideal flow without sources and sinks, φ satisfies Laplace’s equation (see [3]). However, in Ω defined above, φ satisfies Poisson’s equation, i.e., qs = −

∆φ = −f,

(5)

or in two dimensions ∂2φ ∂2φ + 2 = −f, ∂x2 ∂y

where f (x, y) =

   −b  

+b 0

(6)

if (x, y) = S + , if (x, y) = S − , otherwise,

(7)

where b is a positive constant, typically assigned to 1. Since no flux is going in or out of the boundary of Ω (denoted by ∂Ω), we impose the Neumann boundary condition on its boundaries (see [9, 14]), i.e., ∂φ = 0 on ∂Ω, ∂n

(8)

JINT1386.tex; 18/11/1997; 11:45; v.7; p.2

ROBOT PATH PLANNING USING FLUID MODEL

31

Figure 1. The flow map in a working domain with three polygon-shaped obstacles.

Figure 2. Equipotential contours in the configuration space of Ω in Figure 1. The brightest spot is S + and the darkest is S − .

where n is the normal vector along the boundaries. Equation (8) indicates that the normal velocity at the boundary is zero. Figure 1 shows an ideal flow in a closed space generated by the numerical solutions. The arrows show both the flow directions and relative magnitudes of the potential gradients at each rectangular region. Figure 2 shows the equipotential contours of the flow in Figure 1. Figure 3 shows a 3D image of Ω, in which the magnitudes of the velocity potentials are indicated by the height of the surfaces.

JINT1386.tex; 18/11/1997; 11:45; v.7; p.3

32

Z. X. LI AND T. D. BUI

Figure 3. A 3D mesh of φ after fluid simulation in Figure 1. The peak is S + and the lowest point is S − .

2. Properties of Fluid Model Two types of spaces exist in the working domain Ω. One is the interior bounded space B , i.e., the space occupied by obstacles. The other is the free space F , i.e., the space occupied by the fluid. Or formally, F ∪ B = Ω and F ∩ B = ∅.

(9)

Also we assume that S + ∈ F and S − ∈ F . 2.1. GENERIC PROPERTIES The working domain Ω computed by Equations (6) and (8) has the following generic properties: PROPERTY 1. No local minima exist in F . Local minimum is any point other than S − , the sink, that has the lowest value of φ in its surrounding region. Proof. Assume P, P (i, j) 6= S − , is a local minimum. Consider a small region S ∈ F around P , where φ(P ) is smaller than φ(Ni ), i = 0, 1, . . . (see Figure 4). Because P is the local minimum, from the theory of fluid mechanics, there exist flows from Ni to P . Let S ∗ ⊆ S and P ∈ S ∗ , then the fluid goes into S ∗ from N0 , N1 , . . . . However, neither fluid can flow outward from the boundary of S ∗ because P has the lowest potential, nor it can flow inward into P for P 6= S − . Therefore, the assumption contradicts the conservation law in S ∗ . 2 This property can also be proved by analyzing harmonic functions (see [2]). PROPERTY 2. No flat regions exist in F . Flat here means the fluid is static, and a flat region is a region where all the values of φ are equal. Proof. Let S0 ⊆ F be a flat region and P , N be arbitrary points in S0 , then φ(P ) = φ(N ). Assume S1 is not a flat region and is connected to S0 by AB ,

JINT1386.tex; 18/11/1997; 11:45; v.7; p.4

ROBOT PATH PLANNING USING FLUID MODEL

33

Figure 4. A local minimum centered at P .

Figure 5. A flat region S0 cannot exist if there is flow from P ∗ to P .

and P ∗ is an arbitrary point in S1 (see Figure 5). If φ(P ∗ ) 6= φ(P ), which is the usual case when S1 is not a flat region, from Equation (4), there must be flow between P and P ∗ . However, when the flow crosses AB into S0 , S0 is no longer a flat region. Therefore, the assumption is incorrect and φ(P ∗ ) = φ(P ). Then S1 has to be a flat region so as to keep S0 flat. In the same manner, we can prove that all regions directly or indirectly connected to S0 through the fluid are flat regions, then we have n [

Si = F.

(10)

i=0

However, F is not flat because of the initialization of Ω at Section 1.

2

Theoretically, the above proof is complete. However, because numerical representation of φ is machine dependent. Even if 64 bit floating point is used, the flat area may still occur. The remedy suggested in [2] cannot be used here. Instead, an escaping strategy is added in the global path planning when a flat region is encountered. The technique is simply to expand the available comparison region to its next neighborhood until differences are found. This is experimentally suit-

JINT1386.tex; 18/11/1997; 11:45; v.7; p.5

34

Z. X. LI AND T. D. BUI

able because flat regions can never occur at areas close to sources and sinks of the fluid simulation. PROPERTY 3. No circular flows exist in F . Proof. To maintain a circular flow, a concentric force must be present, which can be measured by circulation (Γ). From [9], ζ=

Γ ∂v ∂u = − , Area ∂x ∂y

(11)

where ζ is the vorticity. From Equation (4), we can get the components of qs : u=−

∂φ , ∂x

v=−

∂φ . ∂y

(12)

Substitute Equation (12) into Equation (11), we get 

ζ=

∂ −∂φ/∂y ∂x





−

∂ −∂φ/∂x ∂y



=−

∂2φ ∂2φ + = 0. ∂x∂y ∂x∂y

(13)

Equation (13) indicates that ζ must be zero. When ζ = 0, however, Γ = 0, hence no concentric force occurs. This is the property of ideal flow. 2 From the above three properties, the general mover’s problem for a point robot is easy to solve. THEOREM 1. If a solution for φ (see Equations (6) and (8)) exists in Ω, there exists a path (τ ) in F from S + to S − by simply following the decrement of the values of φ computed at the discrete points in Ω. Based on Theorem 1, it is convenient to initialize the position of robot at S + , and its destination at S − . 2.2. THE PATH τ Let Υ denote a finite set of paths which have the common starting point and common ending point. When a solution for φ is obtained, Υ is defined and Υ = {τ1 , τ2 , . . . , τK }.

(14)

A path τ (∈ Υ) is a concatenation of a finite number of elementary steps, i.e., τ = τ (0) ⊕ τ (1) ⊕ · · · ⊕ τ (N − 1),

(15)

where ⊕ means concatenation, τ () represents the elementary step composed of two adjacent points and an edge between them, and N is the number of grid points connected in τ . Due to the kinematic characteristics of fluid, the following property holds for τ .

JINT1386.tex; 18/11/1997; 11:45; v.7; p.6

35

ROBOT PATH PLANNING USING FLUID MODEL

PROPERTY 4. Let p(0) and p(N ) be the starting point and ending point of a path τ (∈ Υ) and p(m) be the connecting point between two consecutive elementary steps, m = 1, . . . , N − 1. We have the path configuration condition,











φ p(0) > φ p(i) > φ p(j) > φ p(N ) ,

(16)

where, i, j ∈ m and i < j . This condition indicates that any valid path must be a route along the descending values of φ. This condition prevents the occurrence of circular paths and greatly reduces the amount of redundant path searches. Now we have THEOREM 2. Let Property 4 hold, then any two paths in Υ undergo the same decrement of the velocity potentials regardless of the number of steps they take to reach the destination. Formally, let τi and τi0 be two paths in Υ, i 6= i0 , we have the total gradient G between the two extreme points in Υ G=

m X

gij =

n X

gi0 j 0 ,

(17)

j 0 =1

j=1

where m and n are the number of grid points connected in τi and τj , respectively, and gij denotes the gradient of φ at the j th elementary step in τi , i.e.,





gij |φ pi (j) − φ pi (j − 1) |.

(18)

The same formula as Equation (18) holds for gi0 j 0 . From Theorem 2, the shortest path in Υ is the one that uses fewest steps, or has the largest mean velocity potential gradient. 3. The Steepest Falling Method (SFM) By the path configuration defined in Equation (16), the number of searches for a shortest path is greatly reduced. However, searching every path requires extensive work and that is not economical. Based on Theorems 1 and 2, a heuristic algorithm in a local path planning is proposed. Let S be a small region centered at P , and N1 , N2 , . . . , Nn are its neighbors in a distance of an elementary step. The next step from P to its neighborhood is the neighbor Nnext with which the greatest gradient of φ is obtained. We have 



Nnext = D max (g(P, Ni )) ,

(19)

16i6n

where D is a function returning the selected point for the next step in the path, and


g(P, Ni ) = ξ ∗ φ(P (x, y)) − φ(Ni (x0 , y 0 )) ,

(20)

JINT1386.tex; 18/11/1997; 11:45; v.7; p.7

36

Z. X. LI AND T. D. BUI

Figure 6. A basic region for path selection with SFM.

where ξ=

  1√, 

2 , 2

if x0 = x or y 0 = y , (21)

otherwise.

Here, the distance factor ξ ensures the comparison is taken in the same length. Figure 6 shows such a small region S , the centered point P and its eight neighbors. This technique allows eight candidate directions for the next move. Because it always chooses the one with the greatest potential gradient (i.e., the largest falling in altitude), we call this technique the Steepest Falling Method, or SFM. 4. Flow Direction Correction In fact, SFM is similar to other fast descending algorithms such as depth-first method (see [6]). In general, SFM works well in finding a good (optimal or near optimal) path. However, in some special cases, for example, when S + is close to the boundary, the result sometimes is not satisfactory (see Figure 7). In ideal flow, the streamlines are radiated from the source and equipotential contours are radiative circles around it (see [9]), but when S + is close to the boundary, the flow pattern around it is no longer fully radiative. The speed of fluid is larger in the direction perpendicular to the boundary. This causes the irregular results shown in Figure 7. However, the speed as well as the velocity potential reduce drastically with the increase in distance from S + . When the impact from the source decreases, the impact from the sink becomes more apparent. This feature of the flow in Ω makes path correction possible. By examining the gradients of velocity potentials in a little larger area around S + , we may detect the direction in which the impact from S − is larger. Hence, the distance to it is shorter. This impact can be detected from successive ratios of the potential gradients. Obviously, this technique is still valid along the path. In our method, we call this

JINT1386.tex; 18/11/1997; 11:45; v.7; p.8

37

ROBOT PATH PLANNING USING FLUID MODEL

Figure 7. Path generated by SFM when S + is close to the boundary.

heuristic technique Flow Direction Correction (FDC), which is simply to stretch out the lines from the starting point to two or more candidate points in their own directions, and compare their potential gradients again until certain condition is satisfied. Assume P is a point, either at S + , or at the end of an elementary step (6= S − ). There are two subpaths τ1 and τ2 from P , which lead to N1 and N2 , respectively, and φ(N1 ) < φ(N2 ), i.e., g(P, N1 ) > g(P, N2 ). Let g1 = g(P, N1 ) and g2 = g(P, N2 ). N1 will be chosen provided g1 − g2 > ε,

where ε =

|φ(P )| . ρM

(22)

ε is a threshold of significant potential difference and it is related to the size of the grid in Ω and the value computed at P . More precisely, ε varies with the variation of φ. ρ is an adjustable coefficient. Increasing ρ reduces ε and hence decreases the frequency of invoking FDC. From experience, ρ is chosen from 1 to 5. If Equation (22) is not satisfied, FDC is invoked. Starting from Ni , i = 1, 2, in the above case, the next step is taken in the same direction as it comes from P (i, j), i.e., Nnext (x0 , y 0 ) ← N (x, y),

where 0

x =



x±1 x

if x 6= i, if x = i,

(23) 0

y =



y±1 y

if y 6= j , if y = j .

(24)

Equations (23) and (24) are repeated until the condition in Equation (22) is satisfied. However, the maximum number of stretching is controlled to prevent missing S − . This number should be proportional to M , the grid size in Ω. It is defined as bM/16c.

JINT1386.tex; 18/11/1997; 11:45; v.7; p.9

38

Z. X. LI AND T. D. BUI

Figure 8. Comparison between the path generated by SFM only and that generated with FDC. FDC is used for the shorter path.

Figure 9. Paths generated by SFM combined with FDC.

Figure 10. Paths generated by reusing the solution of the fluid simulation.

JINT1386.tex; 18/11/1997; 11:45; v.7; p.10

ROBOT PATH PLANNING USING FLUID MODEL

39

Figure 8 shows the comparison of the two paths generated from the same configuration. Figure 9 gives examples of paths generated by SFM and FDC. Also this mode has strong reusability. In a time-invariant working domain, if the goal configuration does not change, the simulation needs only to be done once and the potentials can be reused again and again. Figure 10 shows an example, in which one solution of the simulation is used to plan paths for three robots in different positions. 5. Robot Speed Control vs. Gradients From Equation (4), the velocity of an object can be calculated from the potential gradients. However, the result from Equation (4) cannot be directly applied to robot navigation because when R is constant, the narrower the channel, the faster the fluid flows. On the contrary, considering the physical constraints for a real robot, we should allow it to move slower in narrow channel and faster in open space. Also at the beginning, the speed should be increased gradually and at the end, the robot should slow down so that it can station itself accurately at the destination. These requirements can be inversely satisfied by the characteristics of ideal flow in Ω! Thus we can use the computed velocity potentials for the purpose of speed control of a real robot. For the speed v between two points, p and pprev , we can have


v p, pprev =

α α = , |φ(p) − φ(pprev )| g(p, pprev )

(25)

where α is the velocity control factor. It can be obtained from experiments. In reality, upper and lower bounds of the robot’s speed of navigation may be defined. 6. Simulation with Multigrid Method Efficiency of this model is of much concern because it involves solving a differential equation (e.g., Equation (6)). Like Laplace’s equation, Poisson’s equation is well suited to computation on massively parallel machines. We have experimented it on NEC SX3/44, a vector supercomputer. The results of computation is reported in [7]. We also solved the problem using the multigrid adaptive method (MAM), which turns out to be very fast. There are infinite number of solution for Equations (4)–(6). However, what we need is a relatively stabilized solution for Ω. Therefore, we can stop the computation at the first instant when such a state is obtained. The stopping condition is defined as


r k = φk+1 − φk ,

(26)

JINT1386.tex; 18/11/1997; 11:45; v.7; p.11

40

Z. X. LI AND T. D. BUI

Table I. Performance improvement by MAM over GSO M

r I.N

GSO T (sec.)

I.N

MAM T (sec.)

Speedup

64

1 × 10−3 5 × 10−4

273 574

8.6 18.5

22 88

1.1 4.0

7.82 4.63

128

1 × 10−3 5 × 10−4

83 319

23.4 83.8

10 41

2.8 8.3

8.36 10.10

256

1 × 10−3 5 × 10−4

81 160

171.0 304.6

8 12

16.1 18.4

10.62 16.55

where φk is the solution of φ at the kth iteration. A small threshold µ is chosen so that when krk < µ,

(27)

the computation terminates. Here k · k is the Euclidean norm. From experience, µ should be chosen between 1 × 10−3 and 5 × 10−4 to achieve a good solution for a grid not more than 256 × 256. In order to make the multigrid structure adaptive to Ω defined in Section 1, MAM contains four levels (Ωh , Ω2h , Ω4h and Ω8h ). Detailed report can be found in [8]. Table I shows the comparison of MAM with the overrelaxation method based on Gauss–Seidel (GSO). A GTX200 Silicon Computer is used for both algorithms. In the table, M is the grid size in Ω. I.N is the number of iterations. For MAM, I.N only shows the number of iterations at the finest level – Ωh . The other three levels are processed in the following order: Ω2h , Ω4h , Ω8h , Ω4h , Ω2h with 3 sweeps each. The relaxations on coarser grids are 1 to 4 times depending on the convergence rate. The results in Table I suggest that the performance of our method is as fast as other best methods in path planning when MAM is used. The two figures of Figure 9 (with a grid of 128 × 128) are generated in a little more than 10 seconds.

7. Bitmap Technique for Collision Detection In this section, a bitmap technique for collision detection is proposed for nonpoint robots. It is used for local motion planning in 2 dimensional environment. The robot is assumed to be rigid and capable of free translations and one rotation around Z -axis (i.e., 3 DOFs). Because in our model, the predefined domain is embedded in a grid structure, we find the bitmap technique an efficient and convenient way for collision detection.

JINT1386.tex; 18/11/1997; 11:45; v.7; p.12

ROBOT PATH PLANNING USING FLUID MODEL

41

Figure 11. A rectangular robot represented by (x, y, θ).

7.1. EXTRACTION OF THE BITMAP Let h be the unique length between any two adjacent grid nodes in Ω, and A be a rectangular rigid robot and a free-flying object in 3 DOFs. Its width is equal to 2ah and its length to 2bh (a, b > 0 and smaller than some predefined limit of the size of the robot). Let c(x, y) be the center of the robot. The configuration of A in Ω is denoted by A(x, y, θ). x, y are the domain coordinates in Ω and θ is the angle between the x-axis of Ω and the moving direction of A, which coincides with the spine line of A through c (see Figure 11). To satisfy a free rotation of the robot around c, the circular area around c with a radius r equal q to (ah)2 + (bh)2 must be free of obstacles (i.e., ∈ F ). After the initialization of Ω, each node contains information showing whether the spot is occupied or not. Since the coordinates of c are known, we can start from the corresponding node of c and extract information about those nodes which are within the range of r . Let N (x0 , y 0 ) be a node in Ω which is in the bitmap, provided q

(x0 − x)2 + (y 0 − x)2 6 r.

(28)

Each node is represented by a bit in the bitmap. If a node is occupied, a corresponding bit is assigned to 1, otherwise, to 0. After the extraction, a circular bitmap is constructed with only 1’s and 0’s as in Figure 12. Now the collision detection can be done through scanning the bitmap. 7.2. BITMAP SCANNING AND BIT ARRAYS Let M denote the bitmap. It is conveniently subdivided into four quadrants, QI , QII , QIII , QIV as in Figure 13. Scanning will be done for each quadrant separately in rows and in columns. For each quadrant, two bit arrays are defined, one for the rows and the other for the columns. Along the scanning process, the bit values are logically ORed and finally stored in bit arrays. Therefore, each bit in the array represents the union of the bits in that row or column. Examples of the

JINT1386.tex; 18/11/1997; 11:45; v.7; p.13

42

Z. X. LI AND T. D. BUI

Figure 12. The bitmap technique for obstacle detection.

Figure 13. The subdivision of the bitmap.

scanning and the resulted bit arrays in QII and QIV for the sample bitmap in Figure 12 are shown in Figure 14. Clearly, the bit arrays have the same length, because there are as many rows as there are columns. The number of bits in each row or column in M may vary, however, it can be calculated through Equation (28). Therefore, only one array is needed to store the exact number of bits in each q row or column. The maximum number of bits in both row and column is b (ah)2 + (bh)2 c. The array always starts from the center c, i.e., the first bit shows the row orqthe column where c is on. The first bit is the most significant bit and the (b (ah)2 + (bh)2 c)th bit is the least. The bitmap in each region will be processed in the same manner and its two bit arrays will be obtained which represent the results of horizontal and vertical scanning and union operation, respectively. After scanning, eight bit arrays are obtained. For example, in Figure 12, if we show in ascending order for quadrants, the row bit array first in a quadrant, the values of those bit arrays are 10000 00111 00110 00110, 00000 00000 11000 00111.

JINT1386.tex; 18/11/1997; 11:45; v.7; p.14

ROBOT PATH PLANNING USING FLUID MODEL

(a)

43

(b)

Figure 14. Scanning of the bitmap always starts from the center of the circle. Bit values in the scanning process are ORed and the results are stored in row or column bit arrays, respectively.

7.3. CASES ANALYSIS AND UNION OF QUADRANTS For simplicity, assume a = 0, i.e., A becomes a straight line. The loss of accuracy by the above assumption can be compensated to some extent, for example, through expansion of the obstacles in the configuration space by ah. Because the center of A resides at the center of M, there are four cases for the position of A: 1. 2. 3. 4.

Reside Reside Reside Reside

in QI and QIII , in QII and QIV , on the boundary of QI and QII or QIII and QIV (vertical), on the boundary of QI and QIV or QII and QIII (horizontal).

Obviously, the two diagonal quadrants are always used together. Hence, they can be regarded as one part. Let R0 be the union of QI and QIII and R1 of QII and QIV (see Figure 15). Then the number of cases is reduced to three. 1. Reside in R0 , 2. Reside in R1 , 3. Reside on the boundary of R0 and R1 . Here, we actually have two questions to answer. While A is in Ri , i = 0, 1, the question is whether it can freely rotate in Ri or out of Ri . While A is not in Ri , the question is whether A can rotate into R|i−1| and how far it can go. The two problems are often required to be solved together so as to decide the maximum allowable rotation of A in both clockwise and anti-clockwise. To further simplify the calculation, the two quadrants in Ri can be unionized as if one quadrant has been turned 180 degrees around the center. The union

JINT1386.tex; 18/11/1997; 11:45; v.7; p.15

44

Z. X. LI AND T. D. BUI

Figure 15. The two diagonal quadrants are together to decide the rotation. Therefore, M is divided into two regions: R0 and R1 .

(a)

(b)

Figure 16. The union of the two diagonal quadrants. (a) The original configuration in R0 . (b) The unionized quadrant, in which the maximum allowable rotation angle remains unchanged, i.e., γ = β.

operation can be either row-wise or column-wise. For example, we choose the row-wise union operation for R0 . Then QI = r11 + r12 + · · · + r1n

and QIII = r31 + r32 + · · · + r3n ,

(29)

n represents the bits in nth row of Q and n is the total number of row where rm m bit arrays in QI or QIII . Therefore, the union is

Q0 = r11 ∪ r31 + r12 ∪ r32 + · · · + r1n ∪ r3n .

(30)

The union operation should be done in the same order of the corresponding bit arrays. As we stated before, the order of the bit arrays is always from the center of M to the out-circle. The advantage is shown in Figure 16. In Figure 16a, two angles (α and β ) are calculated in QI and QIII respectively. After the union operation, only one angle (γ) needs to be calculated. Clearly γ = β , which is the maximum allowable angle to rotate into R0 anti-clockwise. This can be verified by that the union of the row or column bit arrays of QI and QIII is exactly equal

JINT1386.tex; 18/11/1997; 11:45; v.7; p.16

ROBOT PATH PLANNING USING FLUID MODEL

45

to the row or column bit array of Q0 . For example, in Figure 16a, the row and column bit arrays for QI contain 00111 and 10000 respectively, and for QIII , they are 00100 and 11000. Then we have the union of the bit arrays of QI and QIII : row 00111 column 10000 00100 11000 −−− −−− 00111 11000.

(31)

Clearly, the result is the same as the row and column bit arrays for Q0 (see Figure 16b). Now, for each region, i.e., R0 or R1 , we can have only two bit arrays, one for the row and the other for the column. 7.4. MAXIMUM ALLOWABLE ROTATION INTO Ri First, let us answer the second question raised in the above section, because it can be easily answered by analyzing the unionized bit arrays of Ri . We have the following steps to process the bit arrays: 1. If the value of either row or column bit array for Ri is zero (i.e., all are zero bits in the array), Ri is totally in F . Then, no further collision detection is needed. 2. The first bit in both bit arrays is called entry bit. As to R0 , if the first bit of its row bit array is 1, R0 cannot be entered anti-clockwise, and if the first bit of its column bit array is 1, R0 cannot be entered clockwise. The same as to R1 , but in reverse direction. For example, for R0 in Figure 12, the row bit array is 10000. Then this region cannot be entered anti-clockwise. However, its column bit array is 00111, it can be entered clockwise. 3. If the entry bit for Ri is zero, what is the maximum angle allowed to reach into Ri without collision? This can be easily calculated through two ways. (a) TANGENT METHOD. In Figure 17a, if the lengths of st and ct are known,       st uh u α = arctan = arctan = arctan , (32) ct vh v where u, v are positive integers. In fact, u and v can be obtained through counting the bit arrays. The counting always starts from the entry bit. Clearly, the entry bit for a bit array must be zero so that the counting can be applied. There are two methods of counting, denoted by m1 and m2. For m1, counting starts from the entry bit until a 1 is encountered, then record the count. For m2, the counting starts from the last bit of the bit array until a 1 is encountered, and then report the length from the entry bit to that 1 bit. In different regions, the method applied may be different. Table II shows the detail of the counting method for different regions in different directions. For example, in Figure 17a, the column bit array is 00110 and the row bit array is 00100. This region is allowed to enter from both sides. Now we

JINT1386.tex; 18/11/1997; 11:45; v.7; p.17

46

Z. X. LI AND T. D. BUI

Table II. The technique of calculating u and v in R0 and R1 Entry Region u v

(a)

Clockwise

Anti-clockwise

R0

R1

R0

R1

Array

Method

Array

Method

Array

Method

Array

Method

Col. Row

m1 m2

Row Col.

m1 m2

Col. Row

m2 m1

Row Col.

m2 m1

(b)

Figure 17. The tangent method and cosine method in calculation of α.

consider entering the region anti-clockwise. Counting the column bit array using m1, we get v = 2, and counting the row bit array using m2, we get u = 3. Now we  canapply the  tangent method to calculate α.  u 3 α = arctan = arctan ≈ 56.3. (33) v 2 Since α is known, the maximum anti-clockwise rotation angle β can be easily calculated according to what quadrant is under concern. (b) COSINE METHOD. Cosine method is used for calculation when the value n obtained by m2 is equal to or greater than length [n]. length [ ] records the number of bits in each row or each column in a quadrant. For example, in Figure 17b, u = 4 using m2 and length [4] = 3, so the cosine method is used. Then   cs α = arccos , (34) ct where q q cs = r = (ah)2 + (bh)2 = (bh)2 = bh = 5h, (35) r since a = 0 and b = 5 for the line robot. Let d = h , then     c 2 α = arccos = arccos ≈ 66.4, (36) d 5 where c is either u or v obtained by m1.

JINT1386.tex; 18/11/1997; 11:45; v.7; p.18

47

ROBOT PATH PLANNING USING FLUID MODEL

Figure 18. The calculation of maximum allowable rotation angles on Q0 of R0 . The allowable rotation angle is ang[2] − ang[1].

7.5. MAXIMUM ALLOWABLE ROTATION IN Ri From Section 7.4, we answered the question whether or not a line robot can rotate into a region it does not occupy, and if it can, how far (or with what angle) it can rotate. However, if the quadrants in which A occupies, are not entirely free of obstacles, then the question is what are the bounds for its rotation? The bounds cannot be calculated simply using bit operation as we have done in Section 7.4. A new strategy of calculating the maximum rotation angle is explained below. Let a bitmap M be extracted from Ω as stated in Section 7.1. Let Ri be in M where the line robot A(x, y, θ) is positioned with its center point c(x, y) at the center of M and an angle θ to the X -axis of Ω. If Ri is entirely in F , only the entry checking to R|i−1| is necessary. Assume part of Ri is in B . Then we have different strategy: 1. Scanning Q0 , the unionized quadrant of Ri , row by row (or column by column) and locate the critical points. Here critical points mean where the bit value changes from 0 to 1 or from 1 to 0. If by row, scanning starts from the Y -axis which crosses the center of M. If by column, starts from X -axis. Always scan the row or column which is closer to the center first. The scanning is done in a loop. When the bit is 1, store the coordinates (in row and column) in CritP , then ignore the continuous 1’s until a 0 is encountered or the end of the bit array. If 0 is met and not the end of the array, another coordinates (in row −1 and column) is stored in CritP . The scanning continues until all the bits array in Q0 are passed. Then a set of critical points are obtained. 2. Calculate the angles for each starting or ending point for the obstacles using 



CritP [i] · row ang[i] = arctan . CritP [i] · column

(37)

JINT1386.tex; 18/11/1997; 11:45; v.7; p.19

48

Z. X. LI AND T. D. BUI

Figure 19. Samples of paths for rigid robots with 3 DOFs in different environments. Collision detection is done by bitmap algorithm.

Then sort ang[ ] in ascending order. Finally, search ang[ ] for the location of θ and get ang[k] and ang[l] which are closely adjacent to θ in two sides such that

ang[k] < θ < ang[l].

(38)

Clearly, ang[k] and ang[l] are the two bounds for the rotation of A around its center in Ri (see Figure 18), i.e., the maximum anti-clockwise rotation is ang[l]−θ and the maximum clockwise rotation is ang[k] − θ . The total allowable rotation angle is ang[l] − ang[k]. If θ is smaller or larger than any element in ang[ ], only one bound is on the X or Y -axis in Q0 . In fact, this algorithm is valid for any shape of rigid objects provided they can be enclosed in a circle. The possible change for different shapes is the change of the way M is divided. For example, for an L-shaped robot, M can still be divided into two parts according to whether it is occupied by the robot. So the adjacent two quadrants now are combined into one region. The actual calculations (e.g., the tangent method or cosine method) may have to be changed too. The global path generated by SFM and FDC may act as important references for the moving directions of a non-point robot. In our experiments, it is used to guide the robot to its goal configuration and the robot tends to converge its movements to the closest path line. Problem of this technique is that it may fail in some situation where the areas between obstacles are narrow and composed of continuous turns. Figure 19 give some samples of paths for rectangular and triangular robots using the above strategy. Figure 20 shows a path for an L-shaped robot.

JINT1386.tex; 18/11/1997; 11:45; v.7; p.20

ROBOT PATH PLANNING USING FLUID MODEL

49

Figure 20. A path for an L-shaped robot generated by the fluid model and with the bitmap algorithm to deal with collision avoidance.

Conclusions Starting from the theories of fluid mechanics, this paper explains the fluid simulation strategy, proves the properties of the simulated model and defines path configurations. Problem in tracing the streamlines by simple SFM is reported and its remedy is proposed. For grid-embedded domains and suitability, bitmap collision detection is a simple and convenient technique in dealing with 2-dimensional path planning problems. Related to Laplace’s equation, this model contains the same good properties such as efficiency in path planning, ease of extension to arbitrary n-dimensional domains, and suitability for parallel computations, etc. Also it is a complete navigation function itself in view of point-robot path planning in configuration space. Acknowledgements This work was supported by research grants from the Natural Sciences and Engineering Research Council of Canada and the Fonds pour la Formation de Chercheurs et l’Aide a` la Recherche of Qu´ebec. References 1. Barraquand, B., Langlois, J., and Latombe, J.: Numerical potential field technique for robot path planning, Technical report, Dept. of Computer Science, Report No. STAN-CS-89-1285, Stanford University, 1989. 2. Connolly, C. I. and Burns, J. B.: Path planning using Laplace’s equation, in: Proceedings of IEEE International Conference on Robotics and Automation, 1990. 3. Hwang, Y. K. and Ahuja, N.: Path planning using a potential field representation, in: Proceedings of IEEE International Conference on Robotics and Automation, Philadelphia, 1988.

JINT1386.tex; 18/11/1997; 11:45; v.7; p.21

50

Z. X. LI AND T. D. BUI

4. Khatib, O.: Real-time obstacle avoidance for manipulators and mobile robots, in: IEEE International Conference on Robotics and Automation, March 1985, pp. 500–505. 5. Krogh, B. H.: A generalized potential field approach to obstacle avoidance control, in: Robotics Research: The Next Five Years and Beyond, Michigan, August 1984. 6. Latombe, J.-C.: Robot Motion Planning, Kluwer Academic Publishers, Boston, 1991. 7. Li, Z. X., Bui, T. D., and Tao, L.: Real time robot motion planning and navigation – Fluid model and supercomputing, in: Proceedings of Supercomputing Symposium’92, Montr´eal, June 1992. 8. Li, Z. X.: A Fluid model for robot path planning in time-invariant environment, Master Thesis, Department of Computer Science, Concordia University, Montr´eal, 1994. 9. Massey, B. S.: Mechanics of Fluid, Chapman and Hall, 6th ed., 1990. 10. McCormick, S. E.: Multilevel Adaptive Methods for Partial Differential Equations, Frontiers in Applied Mathematics, SIAM, Philadelphia, 1989. 11. Myers, J. K.: Multiarm collision avoidance using the potential field approach, in: Space Station Automation, SPIE, 1985. 12. Newman, W. S. and Hogan, N.: High speed robot control and obstacle avoidance using dynamic potential functions, Technical Report TR-86-042, Philips Laboratories, November 1986. 13. Ritter, H.: Self-organizing neural maps, Doctoral dissertation, TU M¨unchen, Germany, 1988. 14. Roy, D. N.: Applied Fluid Mechanics, Ellis Norwood Limited, Chichester, 1988. 15. Schmidt, G. and Neubauer, W.: High-speed robot path planning in time-varying environment employing a diffusion equation strategy, in: S. G. Tzafestas (ed.), Robotics Systems, 1992.

JINT1386.tex; 18/11/1997; 11:45; v.7; p.22