[Robotics] Motion Planning
Once a robot has a kinematic model and the ability to follow a prescribed trajectory through closed-loop control, a natural and central question remains: how should the trajectory itself be chosen? Given a start configuration and a goal configuration, with obstacles cluttering the workspace, we would like an automatic procedure that produces a continuous, collision-free motion connecting the two. This is the motion planning problem.
Motion planning sits at the intersection of geometry, topology, search theory, and probability. The single most important conceptual move it asks of us is to abandon the workspace picture and reason instead inside the configuration space $\mathcal{C}$, where each point is a complete description of the robot’s pose. In $\mathcal{C}$, the robot becomes a point and the obstacles become a (generally complicated) subset $\mathcal{C}\text{obs}$; the planning problem becomes that of finding a continuous curve from $q\text{start}$ to $q_\text{goal}$ that stays inside the free space $\mathcal{C}\text{free} = \mathcal{C} \setminus \mathcal{C}\text{obs}$.
This post is a tour through the algorithmic and geometric core of classical motion planning, drawing primarily on Chapter 10 of Lynch and Park and Chapter 12 of Siciliano et al.. We progress from the problem statement and the notion of $\mathcal{C}$-obstacles, through the three classes of completeness one can hope for, to the four families of planners that dominate practice: complete planners, grid-based search, potential fields, and the modern sampling-based workhorses (PRM, PRM*, RRT, RRT-Connect, RRT*).
The Path Planning Problem
We begin with a precise statement. Let $\mathcal{C} \subset \mathbb{R}^n$ (or more generally a smooth manifold) be the configuration space of a robot with $n$ degrees of freedom. Assume the obstacles in the workspace, together with joint-limit constraints, induce a closed subset $\mathcal{C}_\text{obs} \subset \mathcal{C}$. The complementary set
\[\mathcal{C}_\text{free} = \mathcal{C} \setminus \mathcal{C}_\text{obs}\]is the free configuration space. Given a start configuration $q_\text{start} \in \mathcal{C}\text{free}$ and a goal configuration $q\text{goal} \in \mathcal{C}_\text{free}$, the (geometric) path planning problem is to find a continuous map
\[\tau : [0, 1] \to \mathcal{C}_\text{free}, \qquad \tau(0) = q_\text{start}, \ \tau(1) = q_\text{goal},\]or to report that no such path exists. Lynch and Park call this the piano mover’s problem, emphasizing the purely geometric nature of the task: dynamics, time, and torque limits are deliberately set aside. Once a path $\tau$ is produced, a separate time-scaling procedure (the subject of the trajectory-generation post) turns it into a time-parameterized trajectory $q(t)$ executable by the robot.
Given $q_\text{start}, q_\text{goal} \in \mathcal{C}_\text{free}$, find a continuous map $\tau : [0, 1] \to \mathcal{C}_\text{free}$ with $\tau(0) = q_\text{start}$ and $\tau(1) = q_\text{goal}$, or certify that none exists.
A more general formulation, which Lynch and Park call the motion planning problem, asks for a time $T$ and control $u : [0, T] \to \mathcal{U}$ such that the state trajectory $x(t)$ obtained by integrating $\dot{x} = f(x, u)$ satisfies $q(x(t)) \in \mathcal{C}\text{free}$ for all $t$, $x(0) = x\text{start}$, and $x(T) = x_\text{goal}$. For a fully actuated kinematic system, with $\dot{q} = u$ and no dynamic constraints, the two problems coincide; for a car-like vehicle or a free-flying spacecraft they do not. We focus on the path planning version, with brief comments on the kinodynamic extension at the end.
The problem looks deceptively simple. In dimension two, with polygonal obstacles, the human eye solves it at a glance. The mathematical content lies entirely in the dimensionality of $\mathcal{C}$, the topological structure of $\mathcal{C}\text{free}$ (its connected components), and the fact that $\mathcal{C}\text{obs}$ is in general extraordinarily difficult to represent exactly. Almost every planner one encounters is a compromise between geometric fidelity and computational tractability.
Configuration Space and C-Obstacles
What is $\mathcal{C}$?
The configuration space $\mathcal{C}$ is the manifold of all kinematically admissible poses of the robot. Its dimension equals the number of degrees of freedom, and its topology is determined by the joint types. Some examples make this concrete.
- A point translating in the plane. $\mathcal{C} = \mathbb{R}^2$, with $q = (x, y)$.
- A rigid body translating and rotating in the plane. $\mathcal{C} = \mathbb{R}^2 \times S^1$, three-dimensional.
- A 2R planar arm. Each revolute joint contributes a circle, so $\mathcal{C} = S^1 \times S^1 = T^2$, a torus. The fact that $\theta_i = 0$ is identified with $\theta_i = 2\pi$ has real consequences for any planner: a straight-line path in joint coordinates that crosses the cut must “wrap around” the torus.
- A 6-DOF spatial open-chain arm. $\mathcal{C} = \mathbb{R}^3 \times SO(3)$ for the end-effector pose, or $T^6$ when parameterized by six revolute joint angles. The two manifolds are related by forward kinematics and share dimension $6$, but their global topology is very different.
- A floating rigid body in 3D. $\mathcal{C} = SE(3) = \mathbb{R}^3 \rtimes SO(3)$.
For most of what follows we silently assume $\mathcal{C} \subset \mathbb{R}^n$, which is sufficient for the algorithmic ideas. The non-Euclidean cases are handled by selecting a suitable distance metric and a local planner adapted to the manifold, as we shall remark in the sampling-based section.
C-Obstacles
Each workspace obstacle $\mathcal{O}_i \subset \mathcal{W}$ induces a corresponding subset of $\mathcal{C}$ called a C-obstacle:
\[\mathcal{C}\text{-obs}_i = \{ q \in \mathcal{C} : \mathcal{A}(q) \cap \mathcal{O}_i \neq \emptyset \},\]where $\mathcal{A}(q)$ denotes the volume occupied by the robot at configuration $q$. The full obstacle region and free space are
\[\mathcal{C}_\text{obs} = \bigcup_i \mathcal{C}\text{-obs}_i, \qquad \mathcal{C}_\text{free} = \mathcal{C} \setminus \mathcal{C}_\text{obs}.\]Joint limits are treated as C-obstacles as well: the set ${q : \theta_j < \theta_j^{\min} \text{ or } \theta_j > \theta_j^{\max}}$ is appended to $\mathcal{C}_\text{obs}$.
The key geometric fact is that $\mathcal{C}_\text{obs}$ can look very different from the workspace obstacles that generated it. Lynch and Park’s running example is illustrative. Consider a circular mobile robot of radius $r$ translating in the plane with a single polygonal obstacle $\mathcal{O}$. The corresponding C-obstacle is precisely the Minkowski sum $\mathcal{O} \oplus B(r)$, where $B(r)$ is a disk of radius $r$ centered at the origin; geometrically, it is the workspace obstacle “grown” by $r$ on every side, with rounded corners. The robot is then treated as a point in $\mathcal{C} = \mathbb{R}^2$, and the path planning problem reduces to finding a curve that avoids the grown obstacle.
A slightly more involved example is a polygonal robot translating in the plane with a polygonal obstacle. Sliding the robot around the boundary of the obstacle while keeping it just in contact traces out the boundary of the corresponding C-obstacle. The result is again a polygon, obtained by the Minkowski sum of the obstacle with the reflected robot (a classical result from computational geometry).
If the polygonal robot is additionally allowed to rotate, $\mathcal{C} = \mathbb{R}^2 \times S^1$ becomes three-dimensional, and the C-obstacle is a three-dimensional region whose horizontal slice at orientation $\theta$ is the two-dimensional C-obstacle for that fixed rotation. Even for this very simple system, an exact description of $\mathcal{C}\text{obs}$ already requires nontrivial bookkeeping. For a 6-DOF arm operating among arbitrary obstacles, an exact analytic description of $\mathcal{C}\text{obs}$ is hopelessly complicated, and in practice we do not even try to represent it. Instead, we rely on a collision-detection routine: a black-box procedure that, given a configuration $q$, returns whether $\mathcal{A}(q)$ intersects any obstacle.
Motion planners almost never have an explicit representation of $\mathcal{C}_\text{free}$. They access it only through a *collision oracle* that evaluates whether a sample configuration $q$ lies in $\mathcal{C}_\text{free}$, and (often) a *local planner* that checks whether a short connecting motion stays in $\mathcal{C}_\text{free}$. This black-box access is what makes high-dimensional motion planning tractable.
Distance and collision detection. Given a C-obstacle $\mathcal{B}$ and a configuration $q$, the signed distance $d(q, \mathcal{B})$ satisfies $d > 0$ for separation, $d = 0$ for contact, and $d < 0$ for penetration. Practical implementations include the Gilbert–Johnson–Keerthi (GJK) algorithm for distance between convex bodies, and conservative approximations such as bounding spheres (a robot represented by $k$ spheres and an obstacle by $\ell$ spheres yields $d(q, \mathcal{B}) = \min_{i,j} \lVert r_i(q) - b_j \rVert - R_i - B_j$). For a continuous path segment, one typically samples it at fine resolution and grows the robot slightly to certify that the swept volume is collision-free.
Completeness and Other Properties
A planner is described not only by what it computes but by the guarantees it offers. The most important classification is the type of completeness.
A motion planner is **complete** if, whenever a feasible path exists, it returns one in finite time, and whenever no feasible path exists, it terminates and reports failure.
Complete planners exist for limited problem classes (the most celebrated is Schwartz and Sharir’s exact cell decomposition for semi-algebraic obstacles, doubly exponential in the dimension of $\mathcal{C}$), but for almost all problems of practical interest, exact completeness is unattainable. Two weaker notions are central.
A planner is **resolution complete** if, given a discretization of $\mathcal{C}$ (e.g., a grid of resolution $h$), it is guaranteed to find a solution at that resolution whenever one exists.
A planner is **probabilistically complete** if the probability that it finds a solution, when one exists, tends to $1$ as the number of samples (or planning time) goes to infinity.
These categories are not mutually exclusive—a planner can be probabilistically complete but offer no resolution guarantee, or vice versa. Grid-based planners (Section: Grid Methods) are typically resolution complete; sampling-based planners (Section: Sampling-Based Planners) are typically probabilistically complete. A potential-field method, taken on its own, offers neither.
Two further properties round out the planner taxonomy:
- Single-query vs. multi-query. A single-query planner answers one specific $(q_\text{start}, q_\text{goal})$ pair, possibly very quickly. A multi-query planner invests up-front effort to build a reusable representation of $\mathcal{C}_\text{free}$ so that subsequent queries are cheap. RRTs are paradigmatically single-query; PRMs are multi-query.
- Asymptotic optimality. If a planner has a notion of path cost $J(\tau)$, it is asymptotically optimal if the cost of the returned path converges (almost surely) to the optimum as planning time $\to \infty$. Vanilla RRT is not asymptotically optimal: a suboptimal solution found early is never improved upon, because the tree’s edges are never deleted. RRT* and PRM* are designed precisely to recover this property.
Complete Planners and the Visibility Graph
Before introducing the heuristic methods that dominate practice, it is instructive to look at one situation in which a complete planner is both simple and useful: a polygonal robot translating in the plane among polygonal obstacles. Here the C-obstacles are themselves polygons, and one can construct a discrete object called the visibility graph that encodes optimal connectivity:
- Nodes: the start $q_\text{start}$, the goal $q_\text{goal}$, and all vertices of $\mathcal{C}_\text{obs}$.
- Edges: a pair of nodes is joined by an edge if and only if the open line segment between them does not intersect any C-obstacle.
The visibility graph satisfies two roadmap properties: every point in $\mathcal{C}\text{free}$ can reach the graph trivially (along straight segments), and within each connected component of $\mathcal{C}\text{free}$ the graph is itself connected. Crucially, in this special setting the shortest path from $q_\text{start}$ to $q_\text{goal}$ is either a single segment or a concatenation of edges of the visibility graph. Running Dijkstra’s algorithm on it therefore returns an exact shortest path. This gives a complete, optimal, polynomial-time planner—but only for the planar polygonal translational case.
For higher-dimensional or rotational problems, no analogous tractable exact representation exists, and we must turn to approximate methods.
Grid Methods
The most straightforward way to convert path planning into a finite combinatorial problem is to discretize $\mathcal{C}$.
Discretizing $\mathcal{C}$
Cover $\mathcal{C}$ with a regular grid of $k$ points along each of $n$ dimensions, yielding $k^n$ total grid points. Declare a grid point free if the corresponding configuration is collision-free, and otherwise discard it. The result is a graph whose nodes are free grid points and whose edges connect neighbors in $\mathcal{C}$.
A choice of neighbor must be made. For $n = 2$, one can use 4-connectivity (north, south, east, west) or 8-connectivity (including diagonals). Diagonal moves should be penalized by $\sqrt{2}$ when costs are Euclidean; integer-valued approximations $5$ and $7$ are sometimes used for efficiency. An edge from grid point $p$ to grid point $p’$ is admitted only if the local segment is collision-free.
Dijkstra and A*
Once we have a weighted graph, the shortest-path problem can be solved by classical search.
Dijkstra’s algorithm. Maintain a priority queue of nodes ordered by the best-known cost-to-come, and repeatedly expand the cheapest unprocessed node. The first time the goal is dequeued, its cost-to-come is optimal. The complexity is $O((V + E) \log V)$ with a binary heap.
A* search. Like Dijkstra, but the priority of a node $n$ is
\[f(n) = g(n) + h(n),\]where $g(n)$ is the known cost-to-come and $h(n)$ is a heuristic estimate of the cost-to-go from $n$ to the goal. If $h$ is admissible (never overestimates the true cost-to-go) then A* returns an optimal path. If in addition $h$ is consistent (satisfies the triangle inequality $h(n) \leq c(n, n’) + h(n’)$ for every edge), then no node need be re-expanded, and A* runs efficiently.
A heuristic $h(n)$ is *admissible* if $0 \leq h(n) \leq h^*(n)$, where $h^*(n)$ is the true cost-to-go to the nearest goal. It is *consistent* (monotone) if $h(n) \leq c(n, n') + h(n')$ for every neighbor $n'$ of $n$.
For path planning in $\mathcal{C} \subset \mathbb{R}^n$ with Euclidean edge costs, the straight-line Euclidean distance to the goal is admissible and consistent (it underestimates because obstacles can only lengthen the path). For grid graphs with 4-connectivity and unit edge costs, the Manhattan distance $\sum_i \lvert q_i - q_{\text{goal},i} \rvert / h$ is the appropriate consistent heuristic.
The A* pseudocode follows.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
A*(start, goal):
OPEN <- priority queue, push start with f = h(start)
CLOSED <- empty
g[start] <- 0
parent[start] <- None
while OPEN is not empty:
current <- node in OPEN with minimum f
if current in goal set:
return reconstruct_path(parent, current)
remove current from OPEN; add to CLOSED
for each neighbor nbr of current:
if nbr in CLOSED or edge (current, nbr) is in collision:
continue
tentative_g <- g[current] + cost(current, nbr)
if tentative_g < g[nbr]:
g[nbr] <- tentative_g
f[nbr] <- tentative_g + h(nbr)
parent[nbr] <- current
push or update nbr in OPEN
return FAILURE
If $h \equiv 0$ then A* reduces to Dijkstra; if all edge costs are equal then Dijkstra in turn reduces to breadth-first search. Multiplying the heuristic by a factor $\eta > 1$ gives the weighted A* family, which is no longer optimal but explores far fewer nodes and finds a solution within $\eta$ of the optimum.
If the heuristic $h$ is admissible, then A\* with non-decreasing priority returns a path whose cost equals the optimal cost $g^*$ from start to the goal set, provided one exists in the graph. If $h$ is additionally consistent, then when a node is first removed from OPEN its $g$-value already equals $g^*$ for that node.
A grid-based A* planner is resolution complete: it finds a path at the discretization scale whenever one exists. It is also resolution-optimal: it returns the shortest path subject to the allowed motions on the grid.
The Curse of Dimensionality
The principal weakness of grid methods is laid bare by simple arithmetic. With a resolution of $k = 100$ points per axis, the grid contains $k^n$ nodes: $10^6$ for $n = 3$, $10^{10}$ for $n = 5$, and $10^{14}$ for $n = 7$. Even storing the grid—let alone searching it—becomes prohibitive for a 6-DOF arm. Multi-resolution grids (refining only near obstacle boundaries) help somewhat, but the exponential blow-up cannot be hidden indefinitely. This is the curse that motivates the sampling-based methods of a later section.
Potential Field Methods
A radically different idea, due in its modern form to Khatib, is to circumvent global search entirely and instead drive the robot by a vector field constructed by hand. The goal point exerts an attractive virtual force that pulls the robot toward it, while obstacles exert repulsive forces that push it away. The net force is the negative gradient of a virtual potential function
\[\mathcal{P}(q) = \mathcal{P}_\text{goal}(q) + \sum_i \mathcal{P}_{\mathcal{B}_i}(q),\]and the robot is controlled by
\[F(q) = -\nabla \mathcal{P}(q).\]Attractive and Repulsive Components
A typical attractive potential is the quadratic bowl
\[\mathcal{P}_\text{goal}(q) = \tfrac{1}{2} (q - q_\text{goal})^\mathsf{T} K (q - q_\text{goal}),\]with positive-definite $K$, yielding an attractive force $F_\text{goal}(q) = K(q_\text{goal} - q)$ proportional to the displacement from the goal.
A standard repulsive potential, defined in terms of the distance $d(q, \mathcal{B}_i)$ to the $i$th C-obstacle, is
\[\mathcal{P}_{\mathcal{B}_i}(q) = \frac{k}{2\, d^2(q, \mathcal{B}_i)},\]valid outside the obstacle and yielding
\[F_{\mathcal{B}_i}(q) = \frac{k}{d^3(q, \mathcal{B}_i)} \nabla d(q, \mathcal{B}_i).\]To bound the influence of distant obstacles, one usually truncates the repulsion outside a distance $d_\text{range}$:
\[U_{\mathcal{B}_i}(q) = \begin{cases} \frac{k}{2} \left( \dfrac{d_\text{range} - d(q, \mathcal{B}_i)}{d_\text{range}\, d(q, \mathcal{B}_i)} \right)^2 & \text{if } d(q, \mathcal{B}_i) < d_\text{range}, \\ 0 & \text{otherwise.} \end{cases}\]The robot is then driven either by treating $F(q)$ as a commanded velocity, $\dot{q} = F(q)$, or as a force on a damped second-order system, $u = F(q) + B \dot{q}$ with positive-definite $B$ to dissipate energy.
The Local Minima Problem
Potential fields are seductive: the gradient is cheap to evaluate, the controller is reactive (it adapts to moving obstacles in real time), and the implementation is trivial. The fundamental drawback is that the sum of attractive and repulsive potentials is in general not a function with a single minimum. The robot can find itself in a local minimum of $\mathcal{P}$ where the attractive force is exactly balanced by repulsion, yet the goal is nowhere in sight. Saddle points are typically benign (a small perturbation breaks them); local minima are not.
For an arbitrary collection of obstacles in $\mathcal{C}$, the standard attractive-plus-repulsive potential $\mathcal{P}(q) = \mathcal{P}_\text{goal}(q) + \sum_i \mathcal{P}_{\mathcal{B}_i}(q)$ generally possesses critical points $q^* \neq q_\text{goal}$ with $\nabla \mathcal{P}(q^*) = 0$ that are local minima. Gradient descent initialized in their basin of attraction fails to reach the goal.
The classical counterexample is a U-shaped obstacle with the goal on the convex side: the robot is pulled into the cup of the U, where the repulsion from the three sides cancels the attraction to the goal, and it stops.
Navigation Functions
Koditschek and Rimon turned this difficulty into a research program. A navigation function $\varphi : \mathcal{C}\text{free} \to [0, 1]$ is a virtual potential whose only critical points are non-degenerate, and whose only minimum is at $q\text{goal}$. Concretely:
- $\varphi$ is smooth (at least twice differentiable);
- $\varphi$ takes the value $1$ on the boundary of every obstacle;
- $\varphi$ has a unique minimum at $q_\text{goal}$;
- $\varphi$ is a Morse function: every critical point has a non-degenerate Hessian.
Conditions (3) and (4) together guarantee that the saddle points form a set of measure zero, so that gradient descent from almost every initial condition converges to $q_\text{goal}$. Rimon and Koditschek constructed an explicit family of navigation functions for the sphere world—a unit $n$-ball with a finite collection of disjoint smaller spherical obstacles inside—and showed that, via a smooth diffeomorphism, the construction extends to the much broader class of star worlds, in which each obstacle is star-shaped (every boundary point is “visible” from a designated center).
Navigation functions are an elegant theoretical guarantee, but constructing them for general environments is hard, and in practice they are limited to specific geometries. Potential fields are thus typically used either as a heuristic component within a larger planner (e.g., as the cost-to-go estimate $h$ inside A*) or as a reactive controller layered atop a global plan.
Workspace Potential
A pragmatic difficulty with the formulation above is that the distance $d(q, \mathcal{B})$ to a C-obstacle, and especially its gradient, can be hard to compute. A workaround is the workspace potential: represent the obstacle boundary by a finite set of point obstacles $c_j \in \mathbb{R}^3$ and the robot by a set of control points $f_i(q) \in \mathbb{R}^3$. The potential between control point $i$ and obstacle point $j$ is then
\[\mathcal{P}'_{ij}(q) = \frac{k}{2 \lVert f_i(q) - c_j \rVert^2},\]and the workspace force $F’{ij}(q) = -\partial \mathcal{P}’{ij}/\partial q$ is mapped to a generalized force in $\mathcal{C}$ by the principle of virtual work,
\[F_{ij}(q) = J_i^\mathsf{T}(q) F'_{ij}(q),\]where $J_i(q) = \partial f_i / \partial q$ is the Jacobian of the control point. Summing over all $(i, j)$ pairs yields the full generalized force on the robot without ever computing $d(q, \mathcal{B})$ in $\mathcal{C}$.
Sampling-Based Planners
The decisive idea of the modern era is to give up on representing $\mathcal{C}\text{free}$ and instead probe it by *sampling*. Sampling-based planners maintain a finite data structure—a roadmap or a tree—in which nodes are configurations drawn from $\mathcal{C}\text{free}$ and edges are short, collision-free paths between them, produced by a simple local planner. The whole picture rests on three operations:
- A sampler that draws candidate configurations from $\mathcal{C}$ (and rejects those in collision).
- A collision check that decides whether a sample lies in $\mathcal{C}_\text{free}$.
- A local planner that attempts a short connecting path between two configurations and returns success if the path is collision-free.
The hope—and it is a robust one—is that even though we never compute $\mathcal{C}_\text{free}$ explicitly, a few thousand random samples are enough to capture its connectivity in dimensions where a grid would be hopeless.
Probabilistic Roadmap (PRM)
The PRM, due to Kavraki, Svestka, Latombe, and Overmars, is the canonical multi-query sampling-based planner. It separates the problem into a learning phase, executed once for a given environment, and a query phase, which can be repeated cheaply for many start/goal pairs.
Learning phase. Build a roadmap $R$ as an undirected graph:
1
2
3
4
5
6
7
8
9
10
11
12
PRM-Construct(N, k):
R <- empty graph
for i = 1 .. N:
q_i <- sample from C
if q_i in C_free:
add q_i as a node of R
for each node q in R:
Nbrs <- k nearest neighbors of q in R (or those within radius r)
for each q' in Nbrs:
if local_planner(q, q') succeeds:
add edge (q, q') to R
return R
The sampler typically draws uniformly from $\mathcal{C}$ (and rejects collisions), although many variants bias the distribution toward narrow passages to improve coverage. The local planner for an unconstrained kinematic system is usually a straight line, checked for collisions at fine resolution.
Query phase. Given $(q_\text{start}, q_\text{goal})$, attempt to connect each to its nearest neighbors in $R$ via the local planner. If both succeed, run A* on the augmented roadmap to extract a path.
Under mild assumptions on $\mathcal{C}_\text{free}$ (e.g., expansive or having positive clearance), a PRM with uniform sampling and a straight-line local planner has the property that the probability of failure, given that a solution exists, decays to zero as the number of samples $N \to \infty$.
The PRM is well suited to environments in which many queries are made against a static world—industrial cells, simulated factories, video game maps—because the costly learning phase is amortized.
PRM*: Asymptotic Optimality
Vanilla PRM is probabilistically complete but not asymptotically optimal: a fixed-$k$-nearest or fixed-radius connection rule may leave shorter homotopy classes permanently undetected. Karaman and Frazzoli showed that with the right choice of radius, PRM becomes asymptotically optimal.
In PRM*, after sampling $n$ nodes one connects each pair within a radius
\[r(n) = \gamma \left( \frac{\log n}{n} \right)^{1/d},\]where $d$ is the dimension of $\mathcal{C}$ and $\gamma$ is a constant that depends on the measure of $\mathcal{C}\text{free}$ and a tunable margin. With this radius the roadmap remains *sparse* (the number of edges grows like $n \log n$, not $n^2$) yet captures enough connectivity that the shortest path through the roadmap converges to the optimal path in $\mathcal{C}\text{free}$ as $n \to \infty$. The same paper introduced k-nearest PRM* with $k(n) = k_\text{PRM} \log n$, $k_\text{PRM} > e(1 + 1/d)$, with the same guarantee.
For PRM\* with connection radius $r(n) = \gamma (\log n / n)^{1/d}$, with $\gamma$ exceeding a threshold $\gamma^*$ depending on the geometry of $\mathcal{C}_\text{free}$, the cost of the shortest path on the roadmap converges almost surely to the optimal path cost as the number of samples $n \to \infty$.
Rapidly-Exploring Random Trees (RRT)
The RRT, introduced by LaValle and Kuffner, is the single-query counterpart to the PRM. Rather than building a global roadmap, RRT grows a tree $T$ from $q_\text{start}$ that incrementally biases its exploration toward unvisited regions of $\mathcal{C}$.
1
2
3
4
5
6
7
8
9
10
11
RRT(q_start, q_goal, N):
T <- tree containing only q_start
for i = 1 .. N:
q_rand <- sample from C (with small bias toward q_goal)
q_near <- nearest node in T to q_rand
q_new <- step from q_near toward q_rand by distance eta
if local_planner(q_near, q_new) succeeds:
add q_new to T with parent q_near
if q_new in B(q_goal, epsilon):
return SUCCESS, path from q_start to q_new
return FAILURE
The geometric intuition is striking. Each random sample $q_\text{rand}$ “pulls” the tree toward an unvisited region by promoting the nearest current node to be extended in that direction. Because uniformly random samples land more often in large empty regions than in small explored regions, the tree’s leaves expand preferentially into the unexplored part of $\mathcal{C}_\text{free}$, producing the characteristic Voronoi-biased fractal exploration pattern. Lynch and Park observe that a tree grown by extending from a uniformly chosen existing node fails to explore far from its origin—it is the nearest-node rule, applied to uniform target samples, that produces the rapid exploration.
Goal bias. A simple and effective heuristic is to set $q_\text{rand} = q_\text{goal}$ with probability $p_\text{goal} \in [0.05, 0.10]$ on each iteration. This injects a small attractive bias toward the goal while preserving the exploration property.
Step size $\eta$. A small $\eta$ produces dense, fine-grained trees that handle narrow passages but converge slowly; a large $\eta$ explores quickly but may miss thin corridors. Implementations often extend (single step) or connect (repeated steps until collision) toward $q_\text{rand}$, the latter yielding faster connectivity in open spaces.
For a uniform sampler in $\mathcal{C}$ and a straight-line local planner, the probability that the basic RRT fails to find a feasible path, given that one exists, decays exponentially in the number of iterations. The same holds for goal-biased variants with bias probability bounded away from $1$.
Bidirectional RRT (RRT-Connect)
A natural acceleration is to grow two trees, one rooted at $q_\text{start}$ and one at $q_\text{goal}$, and to attempt to connect them. In the RRT-Connect algorithm of Kuffner and LaValle, the two trees alternate roles each iteration: one of them samples $q_\text{rand}$ and extends toward it, then the other tree attempts to connect (greedily extend until collision or success) toward the newly added node. The algorithm terminates the moment the two trees meet.
The bidirectional variant has two practical advantages:
- It allows the goal to be specified as a single configuration $q_\text{goal}$, not just a goal region, since the backward tree handles the inversion.
- In typical cluttered environments the trees meet much sooner than a single forward tree would reach the goal, because each tree explores from its own end.
The price is a small bookkeeping issue: the local planner may not be able to connect the two trees exactly (especially for systems with motion constraints), in which case the trees are considered linked when sufficiently close, and the joining segment is patched by a smoothing routine.
RRT*: Asymptotic Optimality by Rewiring
Like the vanilla PRM, the vanilla RRT is not asymptotically optimal: an early, poor route from $q_\text{start}$ to a region of $\mathcal{C}$ is never improved upon, because edges in the tree are never deleted. Karaman and Frazzoli’s RRT* repairs this by rewiring the tree at every insertion.
The modification is local. When $q_\text{new}$ is about to be inserted, find the set $\mathcal{X}_\text{near}$ of tree nodes within a shrinking radius
\[r(n) = \min\!\Big(\gamma_\text{RRT*} \big(\log n / n\big)^{1/d},\ \eta\Big).\]Two passes are then performed:
- Best parent. Among all $x \in \mathcal{X}\text{near}$ such that the local planner from $x$ to $q\text{new}$ is collision-free, choose the one that minimizes the total cost $g(x) + c(x, q_\text{new})$ as the parent of $q_\text{new}$. (Vanilla RRT would simply use $q_\text{near}$.)
- Rewire. For each $x \in \mathcal{X}\text{near}$, check whether routing $x$ through $q\text{new}$ would lower its cost: i.e., whether $g(q_\text{new}) + c(q_\text{new}, x) < g(x)$. If so, and if the local planner succeeds, change $x$’s parent to $q_\text{new}$.
The rewiring step gradually replaces high-cost ancestral chains in the tree by lower-cost paths through more recently added nodes. The resulting tree always encodes the shortest known path from $q_\text{start}$ to each of its nodes.
With the connection radius $r(n) = \min(\gamma_\text{RRT*} (\log n / n)^{1/d}, \eta)$ for an appropriate constant $\gamma_\text{RRT*}$, the cost of the path found by RRT\* converges almost surely to the optimal cost as the number of samples $n \to \infty$. The planner is also probabilistically complete.
The price of asymptotic optimality is per-iteration cost: each insertion now requires a $k$-nearest neighbor query and up to $k$ collision checks against the local planner. In practice RRT* is the planner of choice when an initial feasible solution is needed quickly and one is willing to keep planning to improve it.
Multi-Query vs. Single-Query: When to Use What
The PRM and RRT families illustrate the central design trade-off in sampling-based planning.
- PRM (multi-query). Up-front investment in a global roadmap of $\mathcal{C}_\text{free}$; subsequent queries are cheap. Suitable when the environment is static and many planning queries will be made (manufacturing cells, animation, multi-step manipulation pipelines). Less well suited when only a single query is needed or when the environment changes between queries.
- RRT / RRT-Connect (single-query). No up-front cost; the tree grows on demand for the particular $(q_\text{start}, q_\text{goal})$ at hand. Suitable when the environment changes, when the problem is high-dimensional and a global roadmap would be wasteful, or when only one path is needed. Less well suited when many queries against the same world will be made—though in practice RRT is so cheap that re-running it is often fast enough.
- Optimality. Vanilla PRM and vanilla RRT find some feasible path but no guarantee of quality. PRM* and RRT* both deliver asymptotic optimality, at the cost of additional bookkeeping (logarithmic connection radius, rewiring).
A complementary axis is kinematic vs. dynamic. PRMs require an exact bidirectional local planner between arbitrary pairs of configurations, which is straightforward for unconstrained kinematic robots but hard (or impossible) for nonholonomic systems. RRTs only require a forward simulator from a node toward a sample, which is much more permissive. For this reason, RRT and its variants dominate planning for systems with motion constraints (cars, drones, dynamic arms).
Path Post-Processing
The paths produced by grid or sampling-based planners are typically jerky: grid paths zigzag along axis directions; RRT paths wander along the route happened upon by random sampling. Two post-processing stages are therefore standard.
Shortcut Smoothing
The simplest and most effective smoothing strategy is random shortcutting:
1
2
3
4
5
6
7
Smooth(path, K):
for k = 1 .. K:
pick two random times t1 < t2 along the current path
let p1 = path(t1), p2 = path(t2)
if local_planner(p1, p2) succeeds (collision-free):
replace the segment from t1 to t2 by the local plan
return path
Each successful shortcut replaces a portion of the path by a straight line (or a short local plan), which is necessarily at least as short. A small number of iterations typically removes most of the visual jaggedness. A divide-and-conquer variant repeatedly bisects the path and attempts a shortcut on each half, recursing on the segments that cannot be replaced.
Smoothing destroys none of the topological guarantees of the planner: it only operates on segments that pass collision checks.
Reparameterization into a Trajectory
The geometric path $\tau(s)$, $s \in [0, 1]$, returned by the planner has no notion of time. To execute it, we must reparameterize $s$ as a function of $t$, $s = s(t)$, choosing the function $s(t)$ to respect the robot’s velocity, acceleration, and torque limits. This is the subject of the trajectory-generation post: typical choices are trapezoidal velocity profiles, $S$-curves, polynomial via-point interpolation, or time-optimal scaling on the $(s, \dot{s})$ phase plane (Bobrow, Pfeiffer–Johanni, and successors). The clean separation between path planning in $\mathcal{C}_\text{free}$ and time scaling along the path is one of the most useful structural simplifications in the classical robotics pipeline.
Kinodynamic and Nonholonomic Extensions
We have, throughout, treated the path planning problem as purely geometric: the configuration variable $q$ may move in any direction in $\mathcal{C}_\text{free}$. Real systems often violate this. A wheeled mobile robot cannot move sideways; a fixed-wing aircraft cannot stop; a torque-limited arm cannot accelerate arbitrarily fast.
Such constraints can be folded into the planning problem in two ways.
- State-space planning. Augment the configuration $q$ with the velocity $\dot{q}$, giving a state $x = (q, \dot{q}) \in \mathcal{X}$, and plan in $\mathcal{X}\text{free}$ subject to the dynamics $\dot{x} = f(x, u)$. This is *kinodynamic planning*. Grid methods become time- and control-discretized search; RRTs sample $q\text{rand}$ in $\mathcal{C}$ but extend the tree by integrating the dynamics forward with a control $u$ chosen to drive the system toward $q_\text{rand}$.
- Local planners with steering functions. Replace the straight-line local planner in PRM/RRT by a steering function $\text{Steer}(q_1, q_2)$ that returns a short feasible motion from $q_1$ to $q_2$ respecting the system constraints. For a car-like robot, Reeds–Shepp curves (forward and reverse motion with minimum turning radius) are the classical choice; for a Dubins car (forward-only), Dubins curves. For nonlinear dynamics, the steering function may itself be the result of a small optimal-control problem.
A subtle issue with sampling-based kinodynamic planning is the metric: the Euclidean distance in $\mathcal{X}$ no longer captures the difficulty of moving between two states, because a state slightly to the side of the robot may be unreachable in any short time. Choosing a metric that approximates reachability (in the spirit of the LQR cost-to-go) is an active area of research, and the practical performance of RRT-style planners depends sensitively on it.
Summary
We collect the principal threads.
- The problem: given $q_\text{start}, q_\text{goal} \in \mathcal{C}\text{free}$, find a continuous path $\tau : [0, 1] \to \mathcal{C}\text{free}$ connecting them.
- The geometric setting: configuration space $\mathcal{C}$ (with topology determined by the joint types), free space $\mathcal{C}\text{free}$, and the obstacle region $\mathcal{C}\text{obs}$ obtained by lifting workspace obstacles to $\mathcal{C}$.
- Completeness comes in three strengths: complete (rare and expensive), resolution complete (grid methods), and probabilistically complete (sampling methods).
- Grid methods + A*. Resolution complete and resolution optimal, with admissible heuristics for guidance. Limited by the exponential growth of the grid in $\dim \mathcal{C}$.
- Potential fields. Cheap, reactive, fail at local minima. Navigation functions (Koditschek–Rimon) provide a principled remedy for star-shaped worlds.
- Sampling-based planners. The modern workhorse. PRM is multi-query and probabilistically complete; PRM* with radius $r(n) = \gamma (\log n / n)^{1/d}$ adds asymptotic optimality. RRT is single-query and probabilistically complete; RRT-Connect grows two trees for speed; RRT* recovers asymptotic optimality by rewiring.
- Post-processing. Shortcut smoothing removes jaggedness; reparameterization $s(t)$ turns the path into a trajectory respecting actuator limits.
- Beyond the geometric problem. Kinodynamic and nonholonomic systems are handled by planning in $\mathcal{X} = \mathcal{C} \times T\mathcal{C}$ with explicit dynamics, or by replacing the straight-line local planner with a steering function (Reeds–Shepp, Dubins, or LQR-based).
Motion planning is one of the most algorithmically rich subfields of robotics; nothing in this post should be read as the last word. The combination of sampling-based exploration with optimization-based local refinement—CHOMP, STOMP, TrajOpt—and the integration of learned heuristics into classical search constitute the active research frontier. But the conceptual edifice above—configuration space, completeness, A*, RRT, PRM—is the language in which every modern planner is described.
Reference
[1] Kevin M. Lynch and Frank C. Park. Modern Robotics: Mechanics, Planning, and Control. Cambridge University Press, 2017. Chapter 10, “Motion Planning.”
[2] Bruno Siciliano, Lorenzo Sciavicco, Luigi Villani, and Giuseppe Oriolo. Robotics: Modelling, Planning and Control. Springer, 2009. Chapter 12, “Motion Planning.”
[3] Richard M. Murray, Zexiang Li, and S. Shankar Sastry. A Mathematical Introduction to Robotic Manipulation. CRC Press, 1994. (Configuration space background.)
[4] Steven M. LaValle. Planning Algorithms. Cambridge University Press, 2006.
[5] Lydia E. Kavraki, Petr Svestka, Jean-Claude Latombe, and Mark H. Overmars. “Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces.” IEEE Transactions on Robotics and Automation, 12(4):566–580, 1996.
[6] Steven M. LaValle and James J. Kuffner. “Randomized Kinodynamic Planning.” International Journal of Robotics Research, 20(5):378–400, 2001.
[7] James J. Kuffner and Steven M. LaValle. “RRT-Connect: An Efficient Approach to Single-Query Path Planning.” Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), 2000.
[8] Sertac Karaman and Emilio Frazzoli. “Sampling-based Algorithms for Optimal Motion Planning.” International Journal of Robotics Research, 30(7):846–894, 2011.
[9] Oussama Khatib. “Real-Time Obstacle Avoidance for Manipulators and Mobile Robots.” International Journal of Robotics Research, 5(1):90–98, 1986.
[10] Elon Rimon and Daniel E. Koditschek. “Exact Robot Navigation Using Artificial Potential Functions.” IEEE Transactions on Robotics and Automation, 8(5):501–518, 1992.
[11] Peter E. Hart, Nils J. Nilsson, and Bertram Raphael. “A Formal Basis for the Heuristic Determination of Minimum Cost Paths.” IEEE Transactions on Systems Science and Cybernetics, 4(2):100–107, 1968.
Leave a comment