Sampling-based planning I: probabilistic roadmaps

Grid search breaks down above three or four dimensions because the number of cells explodes. An arm with six or seven joints, a humanoid with thirty, or a mobile manipulator with ten degrees of freedom cannot be planned on a grid. Sampling-based planners avoid building the configuration space altogether. They pick configurations at random, ask a collision checker whether each is free, and connect nearby free configurations with short collision-free motions. The resulting graph captures the connectivity of free space without ever representing its boundary. This idea, developed in the mid-1990s, transformed motion planning: problems that had been intractable became solvable in seconds, and sampling-based planners are now the default for robot arms.

This lesson introduces the probabilistic roadmap (PRM), the first widely used sampling-based planner, published by Kavraki, Švestka, Latombe and Overmars in 1996. It covers how the roadmap is built and queried, what the collision checks along each edge involve, how the guarantees work, why narrow passages are the method's main weakness and how sampling strategies address them. Lesson 7 covers the tree-based planners, RRT and its variants, which are better suited to single queries.

The idea of a roadmap

A PRM works in two phases. In the learning (construction) phase, it samples N configurations uniformly at random from C-space and keeps those that are collision-free as nodes. For each node, it then tries to connect it to nearby nodes, either its k nearest neighbors or all nodes within a radius r, using a local planner, usually a straight line in C-space. If every configuration along the line is free, the edge is added. In the query phase, the start and goal are connected to the roadmap in the same way, and a graph search (Dijkstra or A*, lessons 3 and 4) finds a path through it. Because the roadmap can be reused for many start-goal pairs in a static environment, PRM is called a multi-query planner. A robot arm working in a fixed cell, picking from the same bins all day, is the classic case.

Local planning and edge checking

Checking an edge means checking the continuous motion between two configurations, not just its endpoints. The usual method samples configurations along the segment at a resolution fine enough that no obstacle could fit between consecutive checks, then calls the collision checker for each. A common rule is to bound how far any point on the robot can move between checks. For an arm, the farthest-moving point is usually at the tool, and its displacement can be bounded from the joint changes and link lengths. Edge checking dominates the cost of PRM construction, so two tricks are standard. First, checking the midpoint first, then the quarter points, and so on (bisection order) finds collisions sooner on average than checking from one end. Second, lazy PRM (Bohlin and Kavraki, 2000) does not check edges during construction at all; it searches the unchecked roadmap, checks only the edges on the candidate path, removes any that collide and searches again. Since most edges are never on a candidate path, this saves much of the work.

Nearest neighbors

Connecting each sample requires finding its nearest neighbors under the C-space metric of lesson 2, with angles wrapped and rotation weighted against translation. A brute-force search over N nodes costs \( O(N) \) per query and \( O(N^2) \) overall, which becomes significant for large roadmaps. Spatial data structures such as k-d trees, or approximate methods such as the geometric near-neighbor access tree used in OMPL, reduce this to roughly logarithmic time per query. The choice between k-nearest and radius connection also affects the guarantees. Karaman and Frazzoli showed in 2011 that connecting each node to its k nearest neighbors with k growing as a constant times \( \log N \), or within a radius shrinking as \( (\log N / N)^{1/d} \) in d dimensions, makes the roadmap asymptotically optimal; this variant is called PRM*. With a fixed k, PRM finds paths but their quality does not converge to the optimum.

Probabilistic completeness

PRM cannot prove that no path exists, but if one does, the probability of failing to find it decays exponentially with the number of samples. The rate depends on how 'open' free space is. Kavraki and colleagues formalized this with ε-goodness and expansiveness: if every free configuration can see at least a fraction ε of free space, then the probability that a roadmap of N samples fails to connect two points in the same component falls roughly like \( e^{-\epsilon N} \) times a factor depending on the path. In open environments ε is large and a few hundred samples suffice; in cluttered ones with narrow corridors, ε is tiny and the required N grows accordingly.

import numpy as np, heapq
rng = np.random.default_rng(1)
# 2-D C-space [0,10] x [0,10]; a wall at x in [4.5, 5.5] with a gap of width w around y = 5
def free(p, w):
    x, y = p
    return not (4.5 <= x <= 5.5 and abs(y - 5) > w / 2)
def edge_free(a, b, w, step=0.05):
    n = int(np.ceil(np.linalg.norm(b - a) / step))
    return all(free(a + (b - a) * t, w) for t in np.linspace(0, 1, n + 1))
def prm(N, w, k=10):
    pts = [np.array([1.0, 5.0]), np.array([9.0, 5.0])]
    while len(pts) < N + 2:
        p = rng.uniform(0, 10, 2)
        if free(p, w): pts.append(p)
    P = np.array(pts); adj = {i: [] for i in range(len(P))}
    for i in range(len(P)):
        d = np.linalg.norm(P - P[i], axis=1)
        for j in np.argsort(d)[1:k + 1]:
            if edge_free(P[i], P[j], w): adj[i].append(j); adj[j].append(i)
    seen = {0}; stack = [0]
    while stack:
        u = stack.pop()
        for v in adj[u]:
            if v not in seen: seen.add(v); stack.append(v)
    return 1 in seen
for w in (2.0, 0.5, 0.2):
    for N in (100, 400):
        ok = sum(prm(N, w) for _ in range(20))
        print("gap %.1f, %3d samples: path found in %2d of 20 runs" % (w, N, ok))

gap 2.0, 100 samples: path found in 18 of 20 runs
gap 2.0, 400 samples: path found in 20 of 20 runs
gap 0.5, 100 samples: path found in  8 of 20 runs
gap 0.5, 400 samples: path found in 15 of 20 runs
gap 0.2, 100 samples: path found in  2 of 20 runs
gap 0.2, 400 samples: path found in  6 of 20 runs

Narrow passages

The narrow passage problem is the main weakness of uniform sampling. A corridor whose volume is a tiny fraction of C-space receives proportionally few samples, so the roadmap may fail to connect the regions it joins even when both sides are densely sampled. In high dimensions the effect is severe: a passage that is 1 percent of the width of each of six joint ranges occupies \( 10^{-12} \) of the volume in the worst case. Many sampling strategies concentrate samples where they help. Obstacle-based sampling (OBPRM) places samples near obstacle surfaces. Gaussian sampling draws pairs of points a short distance apart and keeps one only when the other is in collision, which puts samples near boundaries. The bridge test draws pairs whose endpoints are both in collision and keeps the free midpoint, which lands samples precisely inside narrow passages. Medial-axis sampling pushes samples toward the middle of free space for clearance. Each strategy improves performance on some problems and wastes effort on others, so planners often mix uniform sampling with one or more biased strategies.

When to use PRM

PRM shines when the environment is static and many queries will be made, because the roadmap amortizes its construction cost. It parallelizes easily, since samples and edges can be checked independently, and its roadmap can be stored and reused. For a single query in an environment that changes between queries, building a roadmap is wasteful, and the tree-based planners of lesson 7, which grow only toward the goal, are faster. Many arm planners in practice, including those in MoveIt 2 through OMPL, default to tree-based planners for this reason, while PRM and its variants remain the choice for repeated tasks in fixed work cells.