Robotic Path Planning: PRM and PRM*

Building a infrastructure for path planning

In my previous article, I discussed two path planning algorithms often used in robotics. The algorithms aimed to solve the problem that I mentioned last week:

The robotic path planning problem is a classic. A robot, with certain dimensions, is attempting to navigate between point A and point B while avoiding the set of all obstacles, Cobs . The robot is able to move through the open area, Cfree , which is not necessarily discretized.

The Rapidly-exploring random tree (RRT) family of algorithms solved this problems by building a graph from a starting position and aiming to land a randomly generated point in the goal region. Various methods were used to connect points and optimize the path.

A robot using PRM to generate a circle

The RRT algorithm was advantageous to find possible paths from a given point. Yet, each time a new location was given, an entirely new graph had to be generated. Repeatedly generating a new graph to find a single path is arguably inefficient, especially if a relatively large area is being explored. An alternative approach that addresses this deficiency is the Probabilistic Roadmap (PRM) algorithm.

Instead of generating a fresh graph with each desired path, PRM builds a single roadmap that aims to cover a good portion of the Cfree area. A shortest path planning algorithm is then used to navigate across the roadmap. The point of PRM is important to note. PRM itself does not find a path, but builds a practical graph for traveling through a region. Once the roadmap is constructed a desired search algorithm — Dijkstra’s algorithm, A*, Bellman-Ford — is used to determine an optimal path.

This article aims to give a basic summary of PRM and PRM*. I will avoid going into proofs and mathematical details, as this article is targeted to people who are simply interested in implementation and results. For a robust understanding, I recommend the curious look into the original work of Dr. Karaman and Dr. Frazzoli.

PRM

PRM, similarly to RRT, generates a limited number of random points within a given area. As I mentioned in the previous article, there are numerous methods of generating (La Valle, 195) random points. In the examples shown here, Python’s pseudo random number generator is used. While decent results are produced, the biases of the random generator are fairly apparent in the resulting PRM graphs.

Path Generated using PRM (not so smooth)

After a new node is generated, the PRM algorithm clusters nodes into connected components. The connected component the node belongs to needs to be stored. To perform clustering, all nodes within a fixed radius of the randomly generated node are collected. These neighboring nodes are ordered by increasing distance (or the desired metric). Looping through the ordered neighboring nodes, check If the randomly generated node is not in the same cluster as the examined node. The new node is added to the cluster if the condition is satisfied. A node can, and should, belong to multiple clusters.

PRM Pseudo Code G(V,E) = Null //Initialize a graph as empty

limit = n //number of nodes to make graph out of

Rad = r //radius of neighborhoods

For itr in 0...limit:

Xnew = RandomPosition()

Xnearest = Near(G(V,E),Xnew,Rad) //find all nodes within a Rad

Xnearest = sort(Xnearest) //sort by increasing distance

For node in Xnearest:

if not ConnectedComp(Xnew,node) and not Obstacle(Xnew,node):

G(V,E) += {Xnew,node} //add edge and node to graph

Xnew.comp += node.comp//add Xnew to connected component

Return G(V,E)

The dimension of the radius is given by the user and it’s value defines the structure of the roadmap. Some of the formed clusters will from circular blobs that have this radius. The chosen radius will also impact speed and performance of the generated roadmap. A larger radius will involve parsing through more neighbors and determining their cluster relationship.

Fixed radius clusters

Another parameter the user determines is the number of nodes. The PRM algorithm ends once the number of desired nodes is generated. Having to generate too many nodes can prolong the time it takes to develop the roadmap. More time will be dedicated to examine regions of neighbors.

Disconnected roadmap

Having too few nodes can result in a fractured graph. Clusters, especially in high density obstacle regions, can become disconnected from the remainder of the road map. Paths generated on a sparsely formed road map can be more irregular as well. Fewer nodes decreases possible routes when using a shortest path planning algorithm.

The result of a PRM algorithm

Clusters create zig-zag paths

PRM*

Similarly to RRT*, PRM* makes two modifications to the default PRM algorithm. First, the optimized roadmap making algorithm does not use a fixed radius. Instead, the radius used to find neighbors varies by the number of nodes that have already been placed (for those intimidated by math, feel free to skip to after the graph). I will not describe how this function is derived, but will briefly explain its purpose:

The radius, labeled Rprm* is determined by the number of nodes, n , the number of dimensions, d , and a constant based on characteristics of the space being used, 𝛾prm* . The 𝛾 constant, is also determined by µ(Xfree) , the total area of free space in the area of path planning, and 𝛇d , the volume of a unit sphere in the dimension being used (in 2D, this is the area of a unit circle).

log(n)/n from Wolfram Alpha

While there is a ton of scary math here, the main function we care about is the log(n)/n term. Considering how this function behaves, we expect the size of the radius to be fairly large (ideally covering the whole space) when there are few nodes and shrink as more nodes are added.

Just applying the radius modification, we can observe the varying cluster sizes in the generated roadmap. The benefit of this modification now is that there are more straight roads when a path is generated. The earlier generated edges can be considered major highway systems that assist with the celeritous and efficient navigation across the free area, while the later generated nodes are smaller neighborhood roads that allow for access into nooks.

The nodes with numerous edges, were planted early

The second modification made to PRM removes the whole cluster system. When new nodes are added, their relation to neighboring nodes is not limited by other nodes they have already been added to. Instead, any node that falls within the radius value becomes connected.

The dense graph, yeah…hard to read, but look at that path!

This alteration, considerably increases the computational complexity of the algorithm. PRM* makes the roadmap into a dense graph, meaning that the number of edges is close to the number of maximum possible edges in the graph. The benefit of such a graph is that generated paths will be incredibly smooth. Part of the jaggedness in default PRM is due to clustering. Occasionally, it can be faster to move between a node and the closest node in its cluster. Yet, PRM forces paths to move through a central node first.

PRM* Pseudo Code G(V,E) = Null //Initialize a graph as empty

limit = n //number of nodes to make graph out of

For itr in 0...limit:

Xnew = RandomPosition()

Rad = yprm*(log(itr)/(itr))^(1/d)

Xnearest = Near(G(V,E),Xnew,Rad) //find all nodes within a Rad

For node in Xnearest:

if not Obstacle(Xnew,node):

G(V,E) += {Xnew,node}connected component

Return G(V,E)

Both PRM and PRM* are useful for situations in which a static environment exists. If obstacles are expected to stay stationary, the same roadmap can be used multiple times. If pre-processing time is not an issue, PRM* can produced roadmaps for cleaner paths. Both of these algorithms do suffer in non-static environments, such as in the examples shown where a robot arm has to move a block. Once the obstacle has been moved, the roadmap needs to be reconstructed. High density obstacle regions should also be of concern, as fragmented roadmaps will lead to incomplete or poor paths.

A path generated by PRM*