Probabilistic Harmonic-function-based Method for Robot Motion

Proceedings of the 2003 IEEE/RSJ Intl. Conference on Intelligent Robots and Systems Las Vegas, Nevada · October 2003

Probabilistic Harmonic-function-based Method for Robot Motion Planning ∗ Pedro I˜ niguez∗

Jan Rosell†

∗ Dept. Eng. Electr` onica El`ectrica i Autom` atica (URV) Av. Pa¨ısos Catalans 26, 43007 Tarragona, SPAIN † Institut d’Organitzaci´ o i Control de Sistemes Industrials (UPC), Diagonal 647, 08028 Barcelona, SPAIN e-mail: [email protected] [email protected]

Abstract

scheme for path planning has been proposed [1]. Following this approach, the probabilistic roadmap method (PRM, [9]) randomly samples the C-space and, with the aid of a local planner, generates a roadmap by connecting the nodes corresponding to free configurations with straight free paths. The initial and the goal configurations are connected to the roadmap and then the roadmap is searched for a feasible path between them. This method has been successfully applied to many degrees of freedom path planning problems. Several versions of PRM have been proposed in order to improve the efficiency and solve difficult path planning problems involving narrow passages. For instance, the increase of the number of samples on the border of the obstacles [4], or the consideration of the initial and goal configurations to bias the sampling in order not to explore the entire C-space [12]. Other PRM variants use the quasi-random sampling technique [5], producing low-dispersion samples that best covers free C-space, or propose a lazy-evaluation approach that delay collision checking until it is absolutely needed [3]. Several PRM variants have been integrated in a PRM meta-planner [7].

This paper presents a robot motion planning method, called PHM, that uses a random sampling scheme together with a potential-field approach based on harmonic functions. The combination of both results in an efficient path planner that is both resolution and probabilistic complete. On one hand, random sampling allows the use of the harmonic functions approach without the explicit knowledge of the robot’s Configuration Space. On the other hand, harmonic functions allow an intelligent sampling of Configuration Space by introducing a bias towards the more promising regions.

1

Introduction

Robot motion planning is devoted to the planning of a collision-free path for a robot (either a freeflying robot or an articulated manipulator) trough the obstacles in a workcell. Robot motion planning is usually done in the robot’s Configuration Space (C-space), were the robot is mapped to a point and the obstacles in the workspace are enlarged accordingly (C-obstacles). The C-space of a robot is the space defined by all of its configurations, a configuration being specified by the set of variables that determine the pose of the robot (position and orientation variables for the free-flying robot or the joint variables for an articulated manipulator). The dimension of the C-space is, therefore, equal to the number of degrees of freedom of the robot (e.g. equal to 6 for a free-flying robot that can both rotate and translate in the space, or also equal to 6 for the typical industrial manipulators). In these high dimensional C-spaces, the complete characterization of the C-obstacles is difficult, precluding the use of many motion planning approaches.

Other planning methods use an approximate decomposition of C-space that also avoids the analytical characterization of C-obstacles. For instance, potential field methods usually work on a regular grid where a potential function is computed, and whose gradient guides the path [2]. In this line, the use of potential field methods based on harmonic functions gives rise to practical, resolution-complete planners without local minima [6]. In order to efficiently compute harmonic functions in large C-spaces, a method based on a non-regular grid decomposition of C-space can be used [10]. The combination of the approach based on harmonic functions with a random sampling scheme has been previously proposed by the authors [11], such that the random sampling scheme allows the use of the harmonic functions approach without the

In order to avoid the complex analytical computation of the C-obstacle’s border, a random sampling ∗ This work was partially supported by the CICYT projects DPI2002-03540 and DPI2001-2202

0-7803-7860-1/03/$17.00 © 2003 IEEE

382

explicit knowledge of the whole C-space. The resulting planner is both resolution and probabilistic complete, and its performance is evaluated in [8]. A similar proposal is that of Yang et al. [13] that define PSfrag replacements a navigation function over a collection of spherical s0 balls that cover the free C-space. The balls are randomly generated and represented as a graph in such a way that neighbor balls do intersect. This paper further explores the combination of harmonic functions and random sampling. It proposes a random sampling scheme that uses harmonic functions to intelligently guide the sampling of Configuration Space with the introduction of a bias towards the more promising regions. The obtained method is called PHM: Probabilistic Harmonic-function-based Method.

An harmonic function φ on a domain Ω ⊂ 0 (cell 1 in Figure 2).

Find path (H, bini , bgoal ): Function that finds a path between the cell bini that contains the initial configuration and the cell bgoal that contains the goal configuration, following the negated gradient of the harmonic function H.

• Obstacle cell: If the whole cell is inscribed inside the circumference of radius dbk and dbk < 0 (cell 2 in Figure 2). • Partially free cell: If the cell is not inscribed inside the circumference of radius dbk (cell 3 in Figure 2).

Compute Distance (bk ): Function that computes distance dbk . Partition (bk , G): Function that computes Bk .

The following nomenclature and functions are used in the following subsections:

Random Sampling (S, F ): Function that randomly samples an element of the set S, using a cumulative function F defined over S.

G: Set of partially free cells (gray). B: Set of obstacle cells (black). W : Set of free cells (white).

3.2

bk : Cell being evaluated, of partition level l.

Path planning algorithm

The solution path is obtained in two steps. First, a sequence of free cells (channel) is found from the cell containing the initial configuration (bini ) to the cell containing the goal one (bgoal ). Then any path contained within the interior of the channel is a valid solution free path. The algorithm Path Planning shown in Figure 3 copes with the first step. It combines the random sampling of the algorithm Explore, detailed in Section 3.3, with the path planning performed using harmonic functions (since harmonic functions do not have local minima it is possible to reach bgoal from bini , if a free channel exists, by iteratively selecting the neighbor cell with the lowest value). Three harmonic functions are 0 computed: HW G and HW G are used to improve the cell sampling of algorithm Explore, and HW is used to find the solution channel. When no path is found (i.e. when the HW value of bini is the same as that of its neighbors), more samples are required and the algorithm restarts. This is done until a path is found or a maximum predefined number of cells has been explored. For the computational efficiency of the algorithm, the relaxation of the harmonic

Bk : Set of (l + 1)-cells obtained by partitioning bk . dbk : Minimum distance in C-space from the C-obstaclesto the configuration corresponding to the center of bk . Dbk : Distance from the center of bk to one of its vertices. HW : Harmonic function computed over W (i.e. considering gray and black cells as C-obstacles), fixing the goal cell at a low potential. HW G : Harmonic function computed over W ∪ G (i.e. considering black cells as C-obstacles), fixing the goal cell at a low potential. 0 HW G : Harmonic function computed over W ∪ G (i.e. considering black cells as C-obstacles), fixing the initial cell and the goal cell at a low potential.

Compute-HF (S,B): Function that computes the harmonic function over a set S of cells of a nonregular grid, considering the cells of a set B at a low potential. 384

0 Explore({G, W, B},HW G ,HW G)

Path Planning(bini , bgoal )

0 F ← Init Rand(HW G ,HW G)

G ← cells of the initial non-regular grid covering the whole C-space. B←∅

FOR s = 1 TO Smax

bk ← Random Sampling(G, F )

W ←∅

dbk ←Compute Distance(bk )

FOR t = 1 TO Tmax

IF |dbk | ≥ Dbk THEN

HW G ← Compute-HF(W ∪ G,{bgoal })

IF dbk > 0 THEN W ← bk

0 HW G ← Compute-HF(W ∪ G,{bgoal , bini })

ELSE B ← bk

0 {G, W, B} ← Explore({G, W, B},HW G ,HW G)

ELSE {G, F } ←Partition(bk , G)

HW ← Compute-HF(W ,{bgoal })

END FOR

p ← Find path(HW , bini , bgoal )

RETURN {G, W, B}

IF p 6= ∅ RETURN p

END

END FOR

Figure 4: Exploration algorithm.

RETURN ∅ END

• The HW G values of the solution channel increase monotonically from the lowest value of the goal cell to the highest value of the C-obstacle cells. Only those cells with a lower HW G value than the initial cell must be considered for the sampling, since those are the only cells that are candidates to be in the solution channel because the Find path function navigates following the negated gradient of the harmonic function.

Figure 3: Path planning algorithm. functions is done incrementally, i.e. considering as initial values those computed in the previous call to 0 the function Compute-HF. Also, HW G and HW G are computed with few iterations, since precise values are not required. 3.3

Let bk be a given cell of the grid, and bini and bgoal be the initial and goal cells, respectively. The probability to sample bk will be determined by a weight, ω(bk ), that takes into account the items detailed above. Assume that bk is an l-cell and let hW G (bk ) be the value of the harmonic function at bk normalized as follows: ( 0 if HW G (bk ) > HW G (bini ) 0 hW G (bk ) = HW G (bk )−HW G (bgoal ) otherwise HW G (bini )−HW G (bgoal ) (6)

Exploration algorithm

The exploration algorithm randomly samples from G a predefined number of cells (Smax ), classifies them as free cells or obstacle cells, when possible, or otherwise subdivides them (in this latter case, the generated subcells are classified as partially free cells and are stored in G to be explored later). The algorithm Explore is shown in Figure 4 and the details of the involved functions are described in Section 3.4. 3.4

Random sampling

Then, the weight ω(bk ) is expressed as:

The random sampling scheme proposed considers the following items:

ω(bk ) = (2−l )kv (hW G (bk ))kh

(7)

where kv ≥ 0 and kh ≥ 0 are used, respectively, to emphasize the sampling criterion based on the cell volume and the sampling criterion based on the harmonic function value.

• If the sample probability of a cell increases with its volume, then a rapid characterization of the C-obstacles border can be obtained, since the uncertainty of big partially free cells is elucidated earlier.

The sampling procedure is performed by using a cumulative function, F (bk ), defined as follows over the cells of G (Figure 7): X F (bk ) = ω(bi ) (8)

• If the sample probability of a cell increases as the 0 harmonic function value HW G decreases, then a quicker selection of the gray cells that are in a promising region can be obtained, since those cells that when considered as free cells are next to the goal cell or next to the initial cell have a 0 lower HW G value.

∀ω(bi )u that FFmax

hW G (bj ) = hW G (bk ) ω(bj ) = (2−(l+1) )kv (hW G (bj ))kh

Extract bk from G

Insert bj in G ordered by ω(bj )

Fmax = Fmax − ω(bk )

Find the cell br ∈ G with greatest F (br ) such that ω(br ) < ω(bj )

F (bj ) = F (bj ) − ω(bk ) ∀bj s.t. ω(bj ) > ω(bk ) RETURN bk

F (bj ) = F (br ) + ω(bj )

END

F (bi ) = F (bi ) + ω(bj ) ∀bi s.t. ω(bi ) > ω(bj ) Fmax = Fmax + ω(bj )

Figure 6: Random sampling algorithm.

END FOR F (b64)

RETURN {G,F }

F Fmax

PSfrag replacements

END

ω(b64) uFmax

Figure 5: Partition algorithm. expressed in (6), (7) and (8), respectively. Then, F (bk ) is updated in algorithm Random sampling since when a cell is sampled G decreases by one, and also in algorithm Partition since when a sampled cell is classified as partially free G increases by 2d .

F (b18)

The maximum value of F is called Fmax . When an l-cell bk is partitioned, the ω value of each generated (l + 1)-cell bj is computed by inheriting the hW G value from the parent cell bk . Then, the ω value is used to orderly insert the cell bj in G (from low to high values). Finally, the values of F are computed and that of Fmax updated, as shown in the algorithm Partition of Figure 5.

F (b5)

F (b22) F (b12)

b5

b22

b18

b64

bk

Figure 7: Random sampling using cumulative function F (bk ).

5

Summary

This paper has introduced PHM, a novel path planning method that follows a random sampling scheme combined with a potential-field approach based on harmonic functions. The use of harmonic functions has been demonstrated to be a very effective way of biasing the sampling procedure towards the regions of Configuration Space that are more promising, i.e. those that may contain the solution path. A non-regular grid decomposition of Configuration Space has been used, where harmonic functions are more efficiently computed. The obtained planner, which is resolution and probabilistic complete, has been illustrated on a 2D Configuration Space with narrow passages. Implementation to higher d.o.f. Configuration Spaces is currently under development.

Finally, the random sampling procedure is implemented by using the cumulative function F (bk ), as detailed in the algorithm Random sampling of Figure 6. Once a cell has been selected, F (bk ) is updated. As an example in Figure 7 a value of u = 0.73 results in the selection of cell b64 .

4

b12

An Example

As an example, Figures 8 to 11 show the C-space at different steps of the path planning algorithm. Figure 8 shows the initial non-regular grid composed of gray cells covering the whole C-space except bini and bgoal . Figure 9 shows the C-space after the first call to the algorithm Explore. Figure 10 shows the obtained solution channel from bini to bgoal (red cells). In this figure the partially free cells that were discarded for exploration in the last call to the algorithm Explore (i.e. those with with HW G (bk ) > HW G (bini )) are marked in blue. Although not necessary, further exploration of the C-space has been performed, giving rise to Figure 11.

References [1] J. Barraquand, L. Kavraki, J.-C. Latombe, T.-Y. Li, and P. Raghavan. A random sampling scheme for path planning. The Int. J. Robotics Research, 16(6):759–774, Dec. 1997.

386

PSfrag replacements

PSfrag replacements

Figure 8: Initial non-regular grid

PSfrag replacements

Figure 10: Solution channel from bini to bgoal

PSfrag replacements

Figure 9: C-space after the first call to Explore

Figure 11: C-space after several iterations

[2] J. Barraquand and J. C. Latombe. Robot motion planning: A distributed representation approach. Int. J. of Robotics Research, 10(6):628–649, 1991.

monic functions and random sampling. Technical Report IOC-DT-P-2003-05, Univ. Polit. Cat., 2003; Accepted to ISATP’03.

[3] R. Bohlin and L. E. Kavraki. Path planning using lazy PRM. In Proc. of the IEEE Int. Conf. on Robotics and Automation, volume 1, pages 521 –528, 2000. [4] V. Boor, M. H. Overmars, and A. F. van der Stappen. The gaussian sampling strategy for probabilistic roadmap planners. In Proc. of the IEEE Int. Conf. on Robotics and Automation, pages 1018–1023, 1999. [5] M. S. Branicky, S. La Valle, K. Olson, and L. Yang. Quasi-randomized path planning. In Proc. of the IEEE Int. Conf. on Robotics and Automation, pages 1481–1487, 2001. [6] C. I. Connolly, J. B. Burns, and R. Weiss. Path planning using Laplace’s equation. In Proc. of the IEEE Int. Conf. on Robotics & Automation, pages 2102–2106, 1990. [7] L. K. Dale and N. M. Amato. Probabilistic roadmaps - putting it all together. In Proc. of the IEEE Int. Conf. on Robotics and Automation, pages 1940–1947, 2002. [8] P. I˜ niguez and J. Rosell. A performance evaluation of PHM: A motion planner that combines har-

[9] L. E. Kavraki and J.-C. Latombe. Randomized preprocessing of configuration for fast path planning. In Proc. of the IEEE Int. Conf. on Robotics and Automation, volume 3, pages 2138–2145, 1994. [10] J. Rosell and P. I˜ niguez. A hierarchical and dynamic method to compute harmonic functions for constrained motion planning. In Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, pages 2334–2340, 2002. [11] J. Rosell and P. I˜ niguez. Combining harmonic functions and random sampling in robot motion planning. Technical Report IOC-DT-P-2002-26, UPC, 2002; Accepted to SYROCO’03. [12] G. S´ anchez and J.-C. Latombe. On delaying collision checking in PRM planning: application to multirobot coordination. The Int. J. Robotics Research, 21(1):5–26, Jan. 2002. [13] L. Yang and S. La Valle. A framework for planning feedback motion strategies based on random neighborood graphs. In Proc. of the IEEE Int. Conf. on Robotics and Automation, pages 544–549, 2000.

387