Motion Planning, in depth: configuration space, probabilistic roadmaps, narrow passages, PRM* and lazy PRM, with tested code and measured success rates

Configuration space and the collision oracle

A configuration is one complete setting of the robot's degrees of freedom: (x, y) for a point robot, (x, y, theta) for a planar body, or six or seven joint angles for an arm. Configuration space (C-space) is the space of all configurations. Its dimension d equals the number of degrees of freedom. Obstacles in the workspace map to forbidden regions of C-space, but those regions are almost never computed explicitly. For an arm they are curved high-dimensional shapes. Sampling planners therefore treat a collision checker as an oracle. Given a configuration, it answers free or not. Given two configurations, it answers whether the straight segment between them is free, by checking points along it at a fixed resolution.

This changes what completeness means. Exact combinatorial planners such as visibility graphs and cell decompositions are complete, but their cost explodes with d. Grid search with A* is resolution-complete, and a grid needs a number of cells exponential in d. PRM is probabilistically complete. If a path with some clearance exists, the chance of failing to find it goes to zero as the sample count grows. It does not prove that a path does not exist.

The algorithm: learn, then query

Learn phase. Draw n configurations and keep the free ones. Connect each to its k nearest neighbours whenever the local planner, a straight line in C-space, passes the segment check. Query phase. Attach the start and goal to their nearest roadmap nodes using the same check, then run Dijkstra or A* over edge lengths. If the start and goal land in different connected components, the query fails, and a union-find over the roadmap reports that in O(1) per query.

A tested planner

The planner below works on a 2D C-space with rectangular obstacles. Only World is specific to that space, so replacing it with your robot's checker generalises the rest. Segments are checked in bisection order (midpoint first, then quarter points, and so on), because a collision in the middle of an edge is found sooner that way.

import heapq, math, random

class World:
    def __init__(self, rects, size=10.0, res=0.05):
        self.rects, self.size, self.res = rects, size, res
        self.point_checks = 0

    def free(self, q):
        self.point_checks += 1
        x, y = q
        if not (0 <= x <= self.size and 0 <= y <= self.size):
            return False
        return not any(x0 <= x <= x1 and y0 <= y <= y1 for x0, y0, x1, y1 in self.rects)

    def segment_free(self, a, b):
        n = max(1, math.ceil(math.dist(a, b) / self.res))
        order, stack = [], [(0, n)]
        while stack:                                   # bisection order
            lo, hi = stack.pop()
            if hi - lo < 2:
                continue
            m = (lo + hi) // 2
            order.append(m)
            stack += [(lo, m), (m, hi)]
        for i in order + [n]:
            t = i / n
            if not self.free((a[0] + t * (b[0] - a[0]), a[1] + t * (b[1] - a[1]))):
                return False
        return True

def sample_uniform(w, rng):
    while True:
        q = (rng.uniform(0, w.size), rng.uniform(0, w.size))
        if w.free(q):
            return q

def sample_bridge(w, rng, sigma=0.5):
    while True:                  # both ends blocked, midpoint free: a narrow passage
        a = (rng.uniform(0, w.size), rng.uniform(0, w.size))
        if w.free(a):
            continue
        b = (rng.gauss(a[0], sigma), rng.gauss(a[1], sigma))
        if w.free(b):
            continue
        m = ((a[0] + b[0]) / 2, (a[1] + b[1]) / 2)
        if w.free(m):
            return m

def knn(nodes, q, k):            # brute force; use a kd-tree beyond a few thousand nodes
    return heapq.nsmallest(k, range(len(nodes)), key=lambda j: math.dist(nodes[j], q))

def build_roadmap(w, n, k, rng, bridge_frac=0.0):
    nodes = [sample_bridge(w, rng) if rng.random() < bridge_frac else sample_uniform(w, rng)
             for _ in range(n)]
    adj, tried = {i: {} for i in range(n)}, set()
    for i, q in enumerate(nodes):
        for j in knn(nodes, q, k + 1):
            e = (min(i, j), max(i, j))
            if j == i or e in tried:
                continue
            tried.add(e)
            if w.segment_free(q, nodes[j]):
                adj[i][j] = adj[j][i] = math.dist(q, nodes[j])
    return nodes, adj

def astar(nodes, adj, s, g):
    h = lambda i: math.dist(nodes[i], nodes[g])
    best, parent, pq = {s: 0.0}, {s: None}, [(h(s), s)]
    while pq:
        f, u = heapq.heappop(pq)
        if u == g:
            path = []
            while u is not None:
                path.append(u)
                u = parent[u]
            return path[::-1], best[g]
        if f > best[u] + h(u) + 1e-12:
            continue                                   # stale entry
        for v, d in adj[u].items():
            if best[u] + d < best.get(v, math.inf):
                best[v], parent[v] = best[u] + d, u
                heapq.heappush(pq, (best[v] + h(v), v))
    return None, math.inf

def query(w, nodes, adj, start, goal, k):
    nodes = nodes + [start, goal]
    s, g = len(nodes) - 2, len(nodes) - 1
    adj = {i: dict(e) for i, e in adj.items()}
    adj[s], adj[g] = {}, {}
    for t in (s, g):
        for j in knn(nodes[:-2], nodes[t], k):
            if w.segment_free(nodes[t], nodes[j]):
                adj[t][j] = adj[j][t] = math.dist(nodes[t], nodes[j])
    return astar(nodes, adj, s, g)

The query copies the adjacency map so the roadmap stays unchanged between requests. A production planner would add and remove the two temporary vertices instead of copying.

Measured: narrow passages and the bridge test

The test map is a 10 x 10 square containing a wall 0.8 thick at x = 4.6 to 5.4. The wall spans the full height except for a gap centred at y = 5, and there are two rectangular obstacles off to the sides. The start is (1, 1) and the goal is (9, 9), so every path must go through the gap. Each cell in the table is the fraction of 25 seeds that found a path, with k = 10. The bridge column draws 30% of samples from the bridge test with sigma = 0.5.

This is the narrow-passage problem in numbers. The 0.2-wide gap covers 0.16 square units, under 0.2% of the free space, so 100 uniform samples are expected to put about 0.2 of a sample inside it. Uniform sampling does not find the passage. It waits for one. The bridge test of Hsu, Jiang, Reif and Sun (2003) inverts the logic. It keeps a sample only when the two ends of a short random segment are both in collision and its midpoint is free, which happens mostly inside passages. The Gaussian sampler of Boor, Overmars and van der Stappen (1999) is a close relative that keeps free samples lying near obstacle boundaries. Mix such samplers with uniform sampling rather than replacing it, because open regions still need coverage.

Connection rules and PRM*

The connection rule decides both cost and path quality. Too few neighbours fragments the roadmap. Too many multiplies collision checks. Karaman and Frazzoli (2011) showed that PRM with a fixed k is not asymptotically optimal: its paths do not converge to the shortest path as n grows. A fixed radius is optimal, but its neighbour count grows linearly with n. Their PRM* keeps O(log n) neighbours by shrinking the connection radius as gamma (log n / n)^(1/d), with gamma above a threshold that depends on the free-space volume. The k-nearest form uses k = k_PRM log n with k_PRM > e(1 + 1/d). For d = 2 that is about 4.08 ln n, which is 19 neighbours at n = 100, 29 at 1,000 and 38 at 10,000.

Measured on the map with the 1.0 gap, the straight line from start to goal is free, so the true optimum is 11.314. k-PRM* found paths of length 11.370 (n = 200, k = 22), 11.423 (n = 1,000, k = 29) and 11.363 (n = 3,000, k = 33). A fixed k = 6 failed at n = 200, then gave 13.785 and 12.269. Individual runs are noisy, but the gap between the two rules is not. Either way, shortcut the result: try to replace subpaths with straight checked segments before handing the path to a controller.

Lazy PRM

Bohlin and Kavraki's lazy PRM (2000) skips edge checks while building. It connects neighbours unchecked, runs A*, checks only the edges on the returned path, deletes any that fail, and searches again. It reuses knn and astar from above:

def lazy_query(w, nodes, start, goal, k):
    nodes = nodes + [start, goal]
    n = len(nodes)
    adj = {i: {} for i in range(n)}
    for i in range(n):                                 # connect without checking
        for j in knn(nodes, nodes[i], k + 1):
            if j != i:
                adj[i][j] = adj[j][i] = math.dist(nodes[i], nodes[j])
    known_ok = set()
    while True:
        path, cost = astar(nodes, adj, n - 2, n - 1)
        if path is None:
            return None, math.inf
        bad = None
        for u, v in zip(path, path[1:]):               # check only this path
            e = (min(u, v), max(u, v))
            if e in known_ok:
                continue
            if w.segment_free(nodes[u], nodes[v]):
                known_ok.add(e)
            else:
                bad = (u, v)
                break
        if bad is None:
            return path, cost
        del adj[bad[0]][bad[1]], adj[bad[1]][bad[0]]

On the gap-1.0 map with 500 nodes and k = 10, eager construction tried 2,885 edges, of which 2,860 were valid, and made 35,440 point checks. Lazy PRM returned the same 11.794-length path after 345 point checks. Wall-clock time fell only from 419 ms to 181 ms, because in this toy the brute-force neighbour search costs more than the trivial rectangle tests. On a real arm, where one check means a mesh-distance query, the reduction in checks is what counts. Lazy evaluation pays most in open environments, where nearly every edge would pass anyway. In clutter it degrades into many re-searches.

Operational guidance

Profile before tuning. Collision checking and neighbour search take almost all of the time. Use a kd-tree or an approximate nearest-neighbour index, and a broad-phase collision check (bounding volumes) before the exact one. Set the edge resolution from the robot's thinnest feature and the largest joint-to-surface lever arm. A resolution coarser than the thinnest wall lets edges tunnel through it. Mature libraries such as OMPL ship PRM, PRM* and lazy variants behind one interface. Start there, and spend your effort on the state space, the distance metric and the checker. When obstacles move, either invalidate the affected edges lazily or switch to an incremental planner. LPA* repairs the search after edge costs change instead of starting over.

Failure modes

• Narrow passages. Success collapses with gap width, as the table shows. Add bridge or Gaussian samples, and report the measured success rate.
• Tunnelling. A segment-check resolution larger than the thinnest obstacle returns colliding paths that look valid.
• Bad metrics. Mixing metres with radians, or ignoring angle wrap-around, makes the nearest neighbours wrong and the roadmap sparse.
• Disconnected components. A query fails even though the roadmap is large. Count components with a union-find over the edges before blaming the search.
• Stale roadmaps. A roadmap built before an obstacle moved returns paths through it unless edges are re-validated.
• Kinodynamic limits. Straight C-space edges ignore velocity and turning limits. Car-like or dynamic robots need a steering-function local planner, or a tree planner.

Trade-offs

What to do next

1. Write down the C-space: its dimensions, joint limits, and a distance metric that weights angles and lengths sensibly.
2. Wrap your collision checker behind free(q) and segment_free(a, b), set the resolution from the thinnest obstacle, and count calls.
3. Build a uniform k-nearest roadmap, measure the success rate over many seeds on your hardest known scene, and check component counts.
4. Add bridge or Gaussian sampling if the success rate stalls, and switch to k = 4.08 ln n (for d = 2; use e(1 + 1/d) log n in general) if path quality matters.
5. Try lazy edge checking if collision checks dominate the profile, and add path shortcutting before control.
6. Move to OMPL or an equivalent for production, and keep your measured seeds as a regression test.