4. The Probabilistic Roadmap Planner

4.2.1. Definitions

For the following algorithm description, let R= ( N,E ) be the roadmap graph. N is the set of nodes that correspond to configurations in C_free. E is the set of edges connecting the nodes from N. D: C x C -> |R⁺₀ is a pseudo-metric (i.e. symmetrical and non-degenerate, the triangle inequation is not required) that defines a distance between any 2 configurations.

4.2.2. Roadmap Construction

The algorithm used to construct the roadmap is the following (pseudo-code):

1.    N:={};
2.    E:={};

      repeat
3.       q:=random configuration from C;
4.       if q is free (i.e. no collision) then
         begin
5.          add q to N;
6.          choose subset Nq of N with candidate neighbours for q

7.          for all q' in Nq sorted ascending by D(q',q) do
            begin
8.             if local planner can connect q with q'
9.             then add (q,q') to E
            end;
         end;
10.    until limit is reached;
       

The algorithm is simple and straightforward. Begininning with an empty roadmap (steps 1 and 2), it repeatedly adds nodes to the roadmap until a certain limit is reached (step 10). This limit has to be chosen based on the application. One possibility would be to impose a limit on the size of the roadmap. Another possibility would be to terminate the algorithm after a specified time has elapsed. Both methods don't take into account that depending on the given environment more time/nodes might be required for creating an adequate roadmap. With more knowledge about the structure of C and C_free more useful limits can be chosen. The choice of limit is not crucial as the algorithm is incremental and thus can be called again anytime should it turn out that not enough nodes have been created.

More important is the choice made for step 6. The algorithm attempts to connect the new configuration q to all configurations from N_q. N_q therefore has an influence on how large and how connected the roadmap will be. The more nodes are considered as candidates for a connection the more well-connected will the roadmap be. However, the preprocessing time is dominated by the time spent in the inner-most loop which is proportional to the size of N_q. Furthermore, more connections mean more edges in the graph which may slow down query processing later as a larger graph has to be searched. The choice of N_q will be discussed in more detail later together with other implementation details.

The most important part in the roadmap construction algorithm (and also the query phase described later) is played by the local planner used in step 8. This sub-planner within the roadmap planner is used to compute paths between 2 configurations within the roadmap. Unlike the roadmap planner, this local planner is simple and not very powerful. An example for a possible local planner is a straight line planner that tries to connect the configurations q and q' with a straight line within C-space. It is obvious that this planner will fail if an obstacle is present between q and q'. For that reason the local planner is not very useful on its own. Together with an algorithm to find a way within the roadmap graph, however, the local planner can be used to find very complex paths around arbitrary obstacles. The general idea is that the roadmap graph describes the global structure of C_free while the local planner is responsible for the details. Only the local planner actually computes the intermediate configurations of the final path. The roadmap is just used to define waypoints which the local planner is able to connect.

4.2.3. Roadmap Refinement

Depending on how many nodes were created in the roadmap construction step and on how complex the environment the robot is moving in is, the roadmap constructed may need to be improved before it is adequate. The biggest difficulty for this algorithm is finding ways through narrow passages. Because the planner picks a sample at random, the probability is small that the sample lies within a narrow passage and in addition to this, the narrower the passage is, the more likely is a collision of the random configuration for the robot with the walls of the narrow passage. As a consequence, the constructed roadmap may contain several components that are not connected within the graph, even though there is a connection (said narrow passage) between the parts of C_free the 2 graph components represent. Furthermore, even if everything is adequately connected, it may be desirable to add more nodes in regions of C_free that only contain few nodes after roadmap construction. This adds alternative routes which is important in situations where several robots move within the same environment so that some paths might be temporarily blocked by other robots. Likewise it may be desirable to remove redundant nodes from the graph. If there is a large empty region in C_free where a robot can move freely from one configuration to another in a straight line, it would be a waste of space to store many nodes for this region.

Only a few general suggestions will be given here regarding the improvement step. When a specific application is given, more effective strategies can possibly be found. One way to improve the roadmap is to add nodes in difficult regions, especially if this allows connecting previously unconnected components within the graph. Again a probabilistic method can be used. An effective way of adding nodes and connections to the roadmap that the roadmap construction step did not (and possibly could not) create is to use random-bounce walks. A configuration q is picked at random and from this starting point a path is generated by walking into a random direction till an obstacle is hit. On collision a different direction is chosen at random, and so on. This generates paths that have the potential of crossing difficult regions that the local planner failed at, like moving around a corner in a narrow tunnel. If it is possible to assign every node q in the graph a certain weight w(q) so that w(q) is high if q lies in a region where planning is hard for the local planner, it is usually a good idea to choose nodes with a high value for w(q) with a higher probability than those with lower weights. That way, random-bounce paths are not needlessly generated in regions where the local planner is sufficient. This is important because, unlike the paths generated by the local planner which can be regenerated any time by running the local planner again, the paths generated by random-bounce walks have to be store explicitly in the roadmap. Just inserting an edge is not sufficient. An effective way of storing a random-bounce path is to store the seed for the random number generator used. Once the new node q' that is the endpoint of the random-bounce walk, together with the path connecting it to q, has been stored in the roadmap, it should be attempted to connect q' with components previously unconnected to that of node q. There is the chance that the random-bounce walk has cleared a difficult obstacle and reached a point from where the local planner is able to connect to previously unreachable nodes. Connections should be attempted with nodes close to q' from components not connected to that of q.

If the weight function mentioned in the previous paragraph is available, there is another way of improving the roadmap. Again a node q from a difficult region is chosen at random but instead of doing a random-bounce walk, another configuration q' is just picked at random with a certain maximum distance from q. The local planner is used to connect q and q' as in the roadmap construction step. New roadmap nodes will thus be generated in difficult regions without the need to compute and store random-bounce paths. Note, however, that this method is not as powerful as the random-bounce method which is even able to find paths that the local planner is conceptually unable to find.

Another way to improve the roadmap is to choose to configurations q and q' from yet unconnected components and attempt to connect them using a very powerful planner if one is available to find a connection between q and q' (if one exists). That way a powerful planner, even if is too slow for regular use, can still be used productively by improving the roadmap for the fast probabilistic roadmap planner.

Sometimes it may even be advisable to add nodes manually. One configuration added in the middle of a narrow tunnel with the proper orientation of the robot for crossing the tunnel may be enough to allow the planner to make a connection through that tunnel. If possible, manual additions of nodes should already occur before the initial roadmap construction step as this may decrease the number of failed configurations tried and thus increase connectivity and speed of the roadmap construction.

4.4.1. Retracting Configurations onto the Medial Axis

The planner described in [2] is based on the medial axis or general Voronoi diagram. The medial axis is the set of all points that have the same distance to at least 2 obstacle walls. It is obvious that these points are at a maximum distance to the blocked part of C-space. A path on the medial axis that goes through a tunnel, for instance, would have the robot move exactly in the middle of the tunnel. Even though the medial axis has a lower dimension than C-space itself (e.g. for a 2D C-space the medial axis is just a network of 1 dimensional lines) it is still a complete representation with regards to roadmap planning in the sense that for any roadmap constructed using points outside the medial axis there exists a roadmap that just contains points from the medial axis but still allows a path to be found for any (q_start,q_goal) pair that the original roadmap could find a path for. It is assumed that the straight line planner is used as local planner.

Unfortunately the medial axis is not easy to compute. However, it turns out that a given configuration can be retracted onto the medial axis without computing it explicitly. The only geometric primitive required is a method to find the obstacle point that is closest to the configuration q to be retracted. Note that although I will still use the term configuration, this is actually just the location component of the configuration and of C-space. Only this component will be adjusted. Likewise the medial axis referred to here is not the medial axis of C-space. It's the medial axis of C-space projected onto the location component.

If a random configuration q is picked during the roadmap construction phase, it is usually not on the medial axis. This means that it has exactly one well-defined closest point p on an obstacle. This point is computed. To retract q onto the medial axis, we move q away from p on a straight line to a location q' with the property that it is as far away from p as from another point p' on a different obstacle. q' is now on the medial axis. This works for any configuration q in C_free. If q is a collision configuration, i.e. it is inside an obstacle it must first be moved into C_free. This is easily achieved by moving it towards instead of away from p until it has passed p by a little delta.

Using the above method it is possible to use even random configurations that are not in C_free as a basis for finding roadmap nodes. If it is desired that all paths have maximum distance to obstacles, this method should be applied to all nodes prior to entering them in the roadmap. It can be proven that this method significantly improves the number of configurations in the roadmap that lie in narrow areas of C_free. If only this effect is desired it is sufficient to only retract non-free configurations during roadmap construction and to use free configurations as is.

4.4.2. Growing The Robot Into Position

A different method for improving the number of configurations in narrow spaces is what could be called blow-up method. The only geometric primitive being used is the computation of collision points between the robot and obstacles. This information is usually available as a side-effect from the collision test that is always necessary for roadmap construction. The general idea is that roadmap construction is done as described in the basic algorithm but instead of throwing away non-free configurations in step 4, it is first attempted to find a near-by configuration that is free. In order to do this the robot is reduced to a single point (the reference point of the robot also used to describe the robot's position). This point is less likely to be within an obstacle (often it is even possible to avoid generating random configurations where the robot's reference point is inside an obstacle altogether). Now the robot's size is increased using a binary search-like method. First the robot is blown up to half its size. If it still collides, a quarter of its size is attempted. If the half-sized robot does not collide 3/4 are attempted, and so on until the maximum size is found that is collision free. As a next step the robot is enlarged just a little. This way it is likely to only collide in very few places. Using those points as reference it is easy to rotate and/or move the robot away from these collision points. If this is not possible without generating a new collision the attempt is aborted and the configuration is discared. If the attempt was successful the moved robot is enlarged further towards its natural size. If more collisions are encountered the above process is repeated until either the robot has reached its real size or the configuration is discarded.

The idea behind this method is to have the robot move into a collision-free position while being blown up just as a ballon would do if inflated close to a wall. It can improve the number of nodes in narrow passages because it allows more configurations to be used that would otherwise be discarded. Because it also allows modifying the rotation component of a configuration to make it collision-free it is especially suitable for situations where collisions are frequently caused by a misoriented robot, as would be the case when moving a long stick-like robot through a tunnel. In the latter case most configurations generated at random within the tunnel would only collide because the robot is not aligned with the tunnel's direction. This is something the blow-up algorithm should be able to fix easily. As mentioned above I have not tested this method for applicability.

4.5.1. Saving Space in the Roadmap

As the dimension of C-space corresponds to the robot's dof, the size of the necessary roadmap graph grows, too. This is because every entry in the roadmap graph only corresponds to exactly one possible combination of dof values. This means that as more dimensions are added, more configurations must be sampled to have a representative set of samples. However, very often several nodes can be joined together. Take a car in an empty parking lot for example. In almost every location of the parking lot the car can point into any direction without colliding. It would be a waste of space to save 100 nodes in the roadmap with (almost) the same location, only differing in orientation. It is better to modify the algorithm to check several orientations for every random location picked during roadmap construction and to store information like "angles 0 to 270 degrees free" in the node. This also improves the paths found during query phase as the car may move more smoothly, not being forced to go exactly through fixed orientations dictated by the waypoints. This approach can be taken a step further by not only storing several free configurations in one node but by testing several possible connections when adding an edge to the roadmap graph. Such an extended edge would contain information like "location 1 with orientations between 0 to 90 degrees can be connected to location 2 with orientations 90-180 degrees". This way a single edge can store many possible movements from location 1 to location 2 that differ in orientation. Again this allows for smoother paths to be chosen.

4.5.2. Choosing the Local Planner

When choosing the local planner there are basically 2 main questions to be answered. The first is the question whether the planner should be deterministic or use a probabilistic approach (like the random-bounce walk). The second question is how powerful the planner should be, considering that more powerful planners take more time to compute. Experimental results have shown that fast and deterministic planners are best. Speed is more important than power because the running time of the local planner is the main component in the running time of the preprocessing algorithm as well as the query algorithm. Handling the complexities of the environment is better left to the roadmap. The planner should be deterministic so that no additional information is necessary to reconstruct paths between nodes in the roadmap.

4.5.3. Choosing the Candidate Neighbours

In step 6 of the preprocessing phase a set of candidate neighbours is chosen for which connection with the random configuration will be attempted. Because the local planner is likely to fail for configurations far away from each other the set of candiate neighbours N_q should be chosen near the location of q which is also the reason why the nodes in N_q are used in the order of ascending distance from q in step 7. If the length and shape of the paths found during query phase is not important an optimization that can be done is to discard, once a succesfull connection between q and q' has been established, all those nodes from N_q within the same connected component as q'. Otherwise there are redundant connections to this component.

4.5.4. Choosing the Distance Function

The distance function used to find configurations close to another configuration should reflect the local planners ability to connect 2 given nodes. The distance of 2 nodes that the planner is likely to fail for should be high. If the local planner being used is the straight line planner, the Euclidean distance serves this purpose well.

4.5.5. Data Structures that Support the Planner

For the performance of the planner as described in this article, the proper choice of data structures is crucial. When the candidate neighbours for q are chosen during roadmap construction and when q'_start and q'_goal are to be found in the query phase it is important to be able to quickly find those nodes in the roadmap that are close to q, q_start and and q_goal respectively. Searching the whole graph for those nodes is extremely inefficient. An index structure specialized on next neighbour queries or window queries like an R* tree is advisable.

4.5.6. Improving Paths

The paths returned in the query phase may be twisted and overly long, especially if no redundant nodes were added in the construction phase by not attempting connections to the same component twice. For that reason smoothing and short-cutting the returned path is often required. One possible way is to try and skip waypoints. This is done by trying if the local planner can find a path between waypoint i and waypoint i+2, thus skipping waypoint i+1. A way to smooth the path for a rigid body robot would be to compute some kind of spline using the waypoints as orientation, instead of connecting them with the local planner. Care must be taken to ensure that this spline path does not collide with an obstacle.

4.5.7. Splitting Responsibilities

When implementing this algorithm it is best to keep the local planner, the global (roadmap) planner and the structure used to store the graph separate so that they can be easily exchanged depending on application. In an object-oriented language this naturally done by having separate classes for all three that only communicate via interfaces. Although generating a random configuration appears in the preprocessing phase of the global planner it should be implemented as part of the local planner because the local planner always has to know the robot whereas this knowledge would have to be added to the global planner in order to find a random configuration, which would destroy the application-independence of the global planner. If a separate class is used for collision detection it is also possible to give this class responsibility for generating the random free configurations. This would also be a natural choice as this class also has knowledge about the robot so that adding this function to its interface would decrease its flexibility. Of course the above is only one possible realization. There are many more but for a flexible design it is important that the different parts of the algorithm do not know more about the application than is strictly required.