The path-planning problem requires to find some, possible shortest, path from a start to a goal location. This problem can be easily visualized in 2D as the problem of finding the shortest path on a map, for example to walk from your house to the nearest grocer. A complete solution to this problem will assure that the best solution can be found and that the algorithm can detect when no solution exists. Examples of complete solutions are Dijkstra’s algorithm and A*. Complete solutions are often unfeasible, however, when the possible state space is large. This is the case for robots with multiple degrees of freedom such as arms. In practice, most algorithms are only resolution complete as the state-space needs to be somewhat discretized for them to operate (e.g., into a grid) and some solutions might be missed as a function of the resolution of the discretization.

The goals of this lecture are

• to introduce sampling-based path-planning methods
• describe heuristics commonly used in planners

Sampling-based Path-Planning

Instead of evaluating all possible solutions or using a non-complete Jacobian-based inverse kinematic solution, sampling-based planners create possible paths by randomly adding points to a tree until some solution is found or time expires. As the probability to find a path approaches 1 when times go to infinity, sampling-based path planners are probabilistic complete. Prominent examples of sampling-based planners are Rapidly-exploring Random Trees (RRT) and Probabilistic Roadmaps (PRM). An example execution of RRT for an unknown goal, thereby reducing the path-planning problem to a search problem is shown below:

(c) Javed Hossein

This example illustrates well how a sampling-based planner can quickly explore a large portion of space and refines a solution as time goes on. Whereas RRT can be understood as growing a tree from a robot’s starting point until one of its branches hit a goal, probabilistic road-maps create a tree by randomly sampling points in the state-space, testing whether they are collision-free, connecting them with neighboring points using paths that reflect the kinematics of a robot, and then using classical graph shortest path algorithms to find shortest paths on the resulting structure. The advantage of this approach is clearly that such a probabilistic roadmap has to be created only once (assuming the environment is not changing) and can then be used for multiple queries. In contrast, RRT’s are known as single query path-planning algorithms.

In practice, the boundary between the different historic algorithms has become very diffuse, and single-query and multi-query variants of both RRT and PRM exist. It is important to note that there is no silver bullet algorithm/heuristic and even their parameter-sets are highly problem-specific. We will therefore limit our discussion on useful heuristics that are common to sampling-based planners.

Sampling-based Planning: Example

The screen-cast below illustrates the execution of different sampling based algorithm variants, illustrating the state-space, lazy-collision checking, Voronoi bias, bi-directional search and goal biasing.

Basic Algorithm

Let $\mathcal{X}$ be a $d$-dimensional state-space. This can either be the robot’s state given in terms of translation and rotations (6 dimensions), a subset thereof, or the joint space with one dimension per possible joint angle. Let $\mathcal{G} \subset \mathcal{X}$ be a  d-ball (d-dimensional sphere) in the state-space that is considered to be the goal, and $t$ the allowed time. A tree planner proceeds as follows

Tree=Init(X,start);
WHILE ElapsedTime() < t AND NoGoalFound(G) DO
newpoint = StateToExpandFrom(Tree);
newsegment = CreatePathToTree(newpoint);
Tree=Insert(Tree,newsegment);
ENDIF
ENDWHILE
return Tree

This process can be repeated on the resulting tree as long as time allows. This is known as an AnyTime algorithm. Given a suitable distance metric, the cost-to-goal can be stored at each node of the tree (much easier if growing the tree from the goal to start), which allows retrieving the shortest path easily. There are three key points in this algorithm

• Finding the next point to add to the tree (or discard) (StateToExpandFrom)
• Finding out where and how to connect this point to the tree taking into account the robot kinematics (CreatePathToTree)
•  Testing whether this path is suitable, i.e., collision-free

Finding the next point to add

A prominent method is to pick a random point in the state-space and connect it to the closest existing point in the tree. Other approaches put preferences on nodes with fewer out-degrees and chose a new point within its vicinity. Both approaches make it likely to quickly explore the entire state-space.

If there are constraints imposed on the robot’s path, for example the robot needs to hold a cup and therefore is not supposed to rotate its wrist, this dimension can simply be taken out of the state-space.

Once a possible path is found, this space can be reduced to the ellipsoid that bounds the maximal path-length. This ellipsoid can be constructed by mounting a wire of the maximum path length between start and goal and pushing it outward with a pen. All the area that can be reached with the pen constrained by the wire can contain a point that can possibly lead to a shorter path.

Connecting Points to the Tree

A new point is classically connected to the closest point already in the tree. This can be done by calculating the distance to all points already in the tree. This does not necessarily generate the shortest path, however. A recent improvement has been made by RRT*, which connects the point to the tree in a way that minimizes the overall path length. This can be done by considering all points in the tree within a d-ball from of fixed radius from the point to add and finding the point that minimizes the overall path length to the start.

Adding a point to the tree is also a good time to take into account the specific kinematics of a robot, for example a car. Here, a local planner can be used to generate a suitable trajectory that takes into account the orientation of the vehicle at each point in the tree.

Collision Checking

Efficient algorithms for testing collisions deserve a dedicated lecture. While the problem is intuitive in configuration-space planning in 2D (the robot reduces to a point) and can be solved using a simple point-in-polygon test, the problem is more involved for manipulators that are subject to self-collision.

As collision checking takes up to 90% of the execution time, a successful method to increase computational speed is “lazy collision evaluation”. Instead of checking every point for a possible collision, the algorithm first finds a suitable path. Only then, it checks every segment of this path for collisions. In case some segments are in collision, they are deleted and the algorithm goes on, but keeps the segments of the successful path that were collision-free.

Path Smoothing

As paths are randomly sampled, they will be most likely shakey and not optimal. Results can be drastically improved by running an additional algorithm that smoothes the path. This can be done by spline interpolation. Care needs to be taken, however, that the resulting paths are indeed collision free.

Summary and Outlook

Path planning is an ongoing research problem. Finding collision free paths for mechanisms with high degrees of freedom such as multiple arms operating in a common space, multi-robot systems, or systems involving dynamics (and therefore adding the derivatives of the state variables to the planning problem) might take unacceptably long to solve.

Although sampling-based path planners can drastically speed up the time to find some solution, there are not optimal and struggle with specific situations such as narrow passages. There is no “silver bullet” algorithm for solving all path planning problems and heuristics that lead to massive speed-up in one scenario might be detrimental in others. Also, algorithmic parameters are mostly ad-hoc and correctly tuning them to a specific environment might drastically increase performance.

Take-home lessons

• A sampling-based planning algorithm finds paths by sampling random points in the environment. Heuristics are used to maximize the exploration of space and bias the direction of search. This makes these algorithms fast, but neither optimal nor complete.
• As the resulting paths are random, multiple trials might lead to totally different results.
• There is no one-size-fits-all algorithm for a path-planning algorithm and care must be taken to select the right paradigm (single-query vs. multi-query), heuristics, and parameters.