Probabilistic Roadmaps (PRMs)

Most path planning algorithms construct a representation of ${\cal C}_{free}$ in the form of a graph. Let G(V,E) represent an undirected graph in which V is the set of vertices and E is the set of edges. Each vertex of V corresponds to a configuration in ${\cal C}_{free}$, and each edge in E corresponds to a collision-free path between a pair of configurations in ${\cal C}_{free}$.

There are typically two phases to path planning. The graph G is constructed in a preprocessing phase. In a query phase, G is used to solve a particular path planning question for a given q_init and q_goal.

The following algorithm builds a graph, G, in ${\cal C}_{free}$:

BUILD_ROADMAP(${\cal A},\O$)
 

1 G.init();

2 for i=1 to M

$$q \leftarrow$$ 3

4 G.add_vertex(q);

$$v \in$$ 5

6 if ((not G.same_component(q,v)) and CONNECT(q,v)) then

7 G.add_edge(q,v);

8 Return G;

The graph resulting graph will have M vertices. Each of the major components in described in more detail below. Sometimes it is preferable to replace the condition (not G.same_component(q,v)) with G.vertex_degree(q) < K, for some fixed K (e.g., K = 15). The same_component condition checks whether q and v are in the same connected component of G. If they are, then it is generally not necessary to attempt to connect q to v. However, it might be too costly to evaluate this condition each time, and one might prefer shorter paths. In this case, it would be appropriate to increase the density of edges in G. A limit K can be set on the maximum allowable degree for any vertex in G.

A random configuration in ${\cal C}_{free}$ is simply found by picking a configuration at random, and using a collision detection algorithm to test whether the configuration lies in ${\cal C}_{obs}$ or ${\cal C}_{free}$. If it lies in ${\cal C}_{obs}$, then a new random configurations are chosen, until one lies in ${\cal C}_{free}$. The running time of this algorithm will obviously be affected by the amount of freedom the robot has to move in the world. If most configurations are in collision, this algorithm will spend more time trying to find a free configuration. The algorithm is outlined below.

RAND_FREE_CONF()

$$q \leftarrow$$ 2

$$q \in {\cal C}_{free}$$ 3

4 Return q;

The following algorithm returns an ordered list of vertices in G(V,E), whose corresponding configurations are within a distance $\epsilon$ of q. The distance threshold, $\epsilon$, and the metric $\rho(q,v)$ are selected in advance.

NBHD(q,G)

1 Q.init();

$$v \in V$$ 2

$$\rho(q,v) < \epsilon$$ 3

4 Q.insert();

5 Return q;

Is it assumed that Q is a priority queue, with elements sorted in increasing order according to $\rho(q,v)$. This ordering will cause the roadmap building algorithm to attempt connections to closer vertices first, before proceeding to more difficult connections.

The CONNECT(q,v) algorithm is considered conceptually as a local planner. The simplest form of a local planner is one that simply connects q to v by a ``straight line'' in ${\cal C}_{free}$.This line is defined by performing interpolation between q and v. If this line intersects ${\cal C}_{obs}$, then an edge cannot be made in G to connect q and v. One way to ensure that the entire line lies in ${\cal C}_{free}$ is to perform collision detection incrementally along the line from q to v. In each step, the robot is moved a small amount, and the configuration is checked for collision. An incremental collision detection algorithm would be suitable for this phase of the algorithm. Alternatively, it may be preferable to precompute a bitmap for fast collision detection. If the steps are made small enough so that no point on ${\cal A}$ is displaced by more than some $\delta_1 \gt 0$, then one can guarantee that a collision not occur in any intermediate configurations if the robot is at least $\delta_2$away from the obstacles in the world. The robot displacement metric can be used to determine $\delta_1$, and the incremental collision detection algorithm can be used to determine $\delta_2$.

In the query phase, an initial configuration, q_init, and a goal configuration, q_goal, are given. To perform path planning, the first step is to pretend as if q_init and q_goal were chosen randomly for connection to G. The NBHD and CONNECT algorithms can be used to attempt connection. If these succeed in connecting q_init and q_goal to other vertices in G, a graph search is performed for a path that connects the vertex q_init to the vertex q_goal. The path in the graph corresponds directly to a path in ${\cal C}_{free}$, which is a solution to the query. Unfortunately, if this method fails, it cannot be determined conclusively whether a solution exists. One approach could be to add more random vertices to G, and then try again to solve the path planning problem.

In general, the best one can assure is that the probabilistic roadmap planning algorithm in probabilistically complete. This is a fairly weak statement, which simply implies that the probability of finding a solution (if one exists) converges to one, while the running time approaches infinity. This seems to guarantee that a solution will be found, but this guarantee could only be made ``after'' running the algorithm forever. Thus, the algorithm cannot conclude that a solution does not exist after a finite period of time.

It is very challenging to analyze the performance of randomized path planning algorithms. Among these algorithms, the strongest analysis thus far exists for the randomized roadmap approach. The concept is based on the notion of $\epsilon$-goodness. Consider cases in which the CONNECT algorithm will be unlikely to make a connection, even though a connection exists. The following examples are extremely difficult (especially the right one) because of the narrow corridor that links two portions of ${\cal C}_{free}$.

These are examples in a 2D configurations, and in higher dimensions, the problem can become even more difficult. Many planning problems involve moving a robot through an area with tight clearance. This will generally cause narrow channels to form in ${\cal C}_{free}$, which leads to a challenging planning problem for the randomized roadmap algorithm.

Let S(q) denote the set of all configurations that can be connected to q using the CONNECT algorithm. Intuitively, this can be considered as the set of all configurations that can be ``seen'' using line-of-sight visibility. The $\epsilon$-goodness is defined in terms of the following parameter,

$$\epsilon = \min_{q \in {\cal C}_{free}} \frac{\mu(S(q))}{\mu({\cal C}_{free})},$$

in which $\mu$ represents the volume (or more technically, the measure). Intuitively, $\epsilon$ represents the small fraction of ${\cal C}_{free}$ that is visible from any point. In terms of $\epsilon$and M (the number of vertices in G), a theorem has been established recently that yields the probability that a solution will be found. The main difficulties are that the $\epsilon$-goodness concept is very conservative (it uses worst-case analysis over all configurations), and $\epsilon$-goodness is defined in terms of the structure of ${\cal C}_{free}$, which cannot be computed efficiently. The result is interesting for gaining a better understanding of the algorithm, but the theorem is very difficult to use in a particular application to determine whether the algorithm will perform well.