Important Concepts

We first introduce the important concept of configuration space (Lozano-Perez 1983) in the context of serial robotic manipulators.

Definition 1 (Configuration space). A set of joint angles q = (θ1, . . ., θn), where n is the number of joints of the robot manipulator, is called a configuration. The set of all possible joint angles is called the configuration space and denoted C . For instance, C = [-π, π] n for a manipulator with n revolute joints. If a robot is in collision with the environment or with itself at configuration q, then q is called a colliding configuration. The subset of C consisting of all non-colliding configurations is called the free space and denoted Cfree.
It is assumed that the starting configuration qstart and the goal configuration qgoal are given. The problem thus consists in finding a path or a trajectory (see definition below) connecting qstart and qgoal and respecting the constraints.

Definition 2 (Path and trajectory). Formally, a path can be defined as a continuous function P

For a path connecting qstart and qgoal, one thus has

A path can be regarded as a geometric object devoid of any timing information. Next, a time parameterization of P is a strictly increasing function

with s(0) = 0 and s(T) = 1. The function s gives the position on the path for each time instant t.

The path P endowed with a time parameterization s becomes a trajectory П

with T being the duration of the trajectory. Note that the same path P can give rise to many different trajectories П.

Path Planning Under Geometric Constraints

There exists a large variety of approaches to path planning: combinatorial methods, potential field methods, sampling-based methods, etc.
Combinatorial methods make use of algebraic geometry and mathematical programming tools to provide complete algorithms to the path planning problems. “Complete” means that the algorithm can decide in finite time whether the problem has a solution and provide a solution when it exists. An example of a combinatorial method is the cell decomposition method: when the obstacles are polygonal, the free space Cfree can be decomposed into cells (e.g., triangular cells by a Delaunay triangulation). Solving the problem then boils down to finding a sequence of adjacent cells such that qstart and qgoal belong to respectively the first and the last cell of the sequence (see, e.g., LaValle 2006, Sect. 6.3.2). While combinatorial methods are valuable to obtain theoretical results (for instance, they allow proving that the general motion planning problem is NP complete), they are too slow to be used in practice, especially in high-dimensional problems. Another issue is that these methods require an explicit representation of the obstacles, which is very difficult to obtain in most practical problems.
The potential field method was introduced in (Khatib 1986). This method constructs a potential field which is high near the obstacles and low at the goal configuration. Steering towards the goal configuration while avoiding the obstacles can then be naturally achieved by letting the robot configuration evolve in that potential field. This method is interesting in that it allows real-time control. However, the issue of local minima – the robot may get trapped in the local minima of the potential field – prevents its use in highly cluttered environments.
Sampling-based methods are perhaps the most efficient and robust, hence probably the most widely used methods for path planning currently. The next two sections review these methods in detail.

Grid Search and Probabilistic Roadmap (PRM)

A first category of sampling-based methods requires building, in a preprocessing stage, a roadmap that captures the connectivity of Cfree. A roadmap is a graph G whose vertices are configurations of Cfree. Two vertices are connected in G only if it is possible to connect the two configurations by a path entirely contained in Cfree. Once such a roadmap is built, it is easy to answer the path planning problem: if (i) qstart can be connected to a (nearby) vertex u and (ii) qgoal can be connected to a (nearby) vertex v and (iii) v and v are in the same connected component of G (in the graph-theoretic sense), then there exists a collision-free path between qstart and qgoal (see Fig. 3).

Methods for building the roadmap fall into two families: deterministic or probabilistic. A typical deterministic method is the Grid Search, where the configuration space C is sampled following a regular grid, as in Fig. 3a. A sampled configuration is rejected if it is not in Cfree. Next, one attempts to connect every pair of adjacent configurations (adjacent in the sense of the grid structure) to each other: if the straight segment connecting the two configurations is contained within Cfree, then the corresponding edge is added to the graph G.
In the Probabilistic Roadmap method (Kavraki et al. 1996), instead of following a regular grid, samples are taken at random in Cfree, see Fig. 3b. Since there is no a priori grid structure, several methods exist for choosing the pairs of vertices for which connection is attempted: for instance, one may attempt, for each vertex, to connect it to every vertices that are within a specified radius r from it.
Advantages of the Roadmap-Based Methods The strength of the roadmap-based methods (both deterministic and probabilistic) comes from the global/local decomposition – the difficult problem of path planning is solved at two scales: the local scale, where neighboring configurations (adjacent configurations in Grid Search, configurations within a small distance r of each other in the Probabilistic Roadmap) are connected by a simple straight segment; and the global scale, where the graph search takes care of the global, complex connectivity of the free space.
Note also that it is not necessary for these methods to have an explicit representation of Cfree: one only needs an oracle which says whether a given configuration is in Cfree. To check whether a straight segment is contained within Cfree, it suffices to call the oracle on every configuration (or, in practice, on sufficiently densely sampled configurations) along that segment.
These methods also possess nice theoretical guarantees. Specifically, it can be shown that the Grid Search is resolution complete, which means that if a solution exists, the algorithm will find it in finite time and for a sufficiently small grid size. Similarly, it can be shown that the Probabilistic Roadmap method is probabilistically complete, which means that, if a solution exists, the probability that the algorithm will find it converges to 1 as the number of sample points goes to infinity (LaValle et al. 2004). However, the converge rate of both methods is difficult to determine on practical problem instances.
Regarding the comparative performances of the deterministic and probabilistic approaches, it has been shown both theoretically and practically that randomness is not advantageous in terms of search time. However, it can be argued that probabilistic methods are easier to implement (LaValle et al. 2004).

Rapidly Exploring Random Trees (RRT)

The methods just discussed require building the entire roadmap in the preprocessing stage before being able to answer any query [a query being a pair (qstart qgoal) to be connected]. In applications where only a single or a limited number of queries are needed, it may not be worthy to build the whole roadmap. Single query methods, such as the Rapidly Exploring Random Trees (RRT), are much more suited for these applications (Lavalle et al. 1998).
Specifically, RRT iteratively builds a tree (see Algorithm 1), which is also a roadmap, but which has the property of exploring “optimally” the free space. The key lies in the EXTEND function, which selects the vertex in the tree that is the closest to the randomly sampled configuration (see Algorithm 2 and Fig. 4). Thus, the probability for a vertex in the tree to be selected is proportional to the size of its Voronoi region, causing the tree to grow preferably towards previously underexplored regions (Lavalle et al. 1998).

Note: STEER(qnear, q) attempts making a straight motion from qnear towards q. Three cases can happen: (i) q is within a given distance r of qnear and [qrand, q] is collision-free, then q is returned as the new vertex qnew; (ii) q is farther than a given distance r of qnear, and the segment of length r from qnear and in the direction of q is collision-free, then the end of that segment is returned as qnew (see Fig. 4); (iii) else, STEER returns failure
Finally, to find a path connecting qstart and qgoal, one can grow simultaneously two RRTs, one rooted at qstart and the other rooted at qgoal, and attempt to connect the two trees at each iteration. This algorithm is known as bidirectional RRT (Kuffner and Lavalle 2000). In practice, bidirectional RRT has proved to be easy to implement, yet extremely efficient and robust: it has been successfully applied to a large variety of robots and challenging environments.

Trajectory Planning Under Kinodynamic Constraints

As mentioned earlier, kinodynamic constraints involve higher-order derivatives of the configuration and cannot therefore be expressed in the configuration space.One approach to kinodynamic planning thus consists of transposing the path planning methods (such as PRM or RRT) to the state-space X, that is, the configuration space C augmented with velocity coordinates, where the kinodynamic constraints can be appropriately taken into account (Donald et al. 1993; LaValle and Kuffner 2001; Hsu et al. 2002). One drawback of this approach is that moving into the state space is associated with a twofold increase in the dimension of the search space: if C is of dimension n, then X is of dimension 2n. Since planning algorithms usually have complexities that scale exponentially with the dimension of the search space (Hsu et al. 2002), a twofold increase in the dimension may make state-space kinodynamic planning algorithms impractical even for relatively small values of n (Shiller and Dubowsky 1991).
A second approach avoids the complexity explosion by decoupling the problem: first, search for a path in the robot configuration space C under geometric constraints (using path planning methods discussed earlier) and, in a second step, find a time parameterization of that path that satisfies the kinodynamic constraints (Bobrow et al. 1985; Kuffner et al. 2002). The drawback here is that the path found in the first step may have no time parameterization at all that respects the kinodynamic constraints. This drawback may be addressed by introducing an Admissible Velocity Propagation scheme, which allows checking, at each PRM/RRT extension, the existence of an eventual admissible time parameterization (Pham et al. 2013).