PRM, in depth: the roadmap as a reusable asset, with Halton sampling, component-aware queries and local repair when obstacles appear

The model: an oracle, a graph and components

A query is a pair of configurations, start and goal. The planner never sees obstacle geometry directly. It asks a collision oracle two kinds of question: is this point free, and is this segment free? Segment checks are implemented as many point checks spaced at a resolution finer than the thinnest obstacle. In every real system those checks dominate the run time, so this article counts them rather than seconds.

Two properties make a roadmap reusable. First, the graph depends only on the static world, not on any query. Second, its connected components summarise reachability. If the start and goal attach to different components, no path exists on this roadmap, and you can say so without searching. That answer is "not found with these samples", not "impossible". PRM is only probabilistically complete: as the sample count grows, the chance of missing an existing path with positive clearance falls towards zero.

Architecture of a roadmap service

The build phase uses a k-d tree (see k-d trees) to find the k nearest neighbours of every sample in O(log n) expected time each, instead of a quadratic all-pairs scan. Each candidate edge is checked once, because an edge already validated from the other endpoint is skipped. Components are labelled at the end. Union-find would label them incrementally (see union-find), but the repair step below deletes edges, and union-find cannot undo a union, so a plain graph traversal is simpler here.

A tested implementation

The implementation works in the unit square with axis-aligned box obstacles. Only World is specific to that setting, so swapping in your robot's checker generalises the rest. The query search is A* (see A* in depth) with the goal as a virtual node -1. Without that node, the search would stop at the first attached roadmap node it pops and ignore the length of the final hop, so the path would not be the shortest one through the roadmap.

import heapq, math
from scipy.spatial import cKDTree

def halton(i, base):
    """i-th element (i >= 1) of the van der Corput sequence in `base`."""
    f, r = 1.0, 0.0
    while i > 0:
        f /= base
        r += f * (i % base)
        i //= base
    return r

def samples(n, kind, rng):
    if kind == "uniform":
        return [(rng.random(), rng.random()) for _ in range(n)]
    ox, oy = rng.random(), rng.random()          # random shift: Cranley-Patterson rotation
    return [((halton(i, 2) + ox) % 1.0, (halton(i, 3) + oy) % 1.0) for i in range(1, n + 1)]

class World:
    def __init__(self, rects, res=0.004):
        self.rects, self.res, self.checks = list(rects), res, 0
    def free(self, p):
        self.checks += 1
        return not any(x0 <= p[0] <= x1 and y0 <= p[1] <= y1 for x0, y0, x1, y1 in self.rects)
    def segment_free(self, a, b, rects=None):
        n = max(1, math.ceil(math.dist(a, b) / self.res))
        rs = self.rects if rects is None else rects
        for i in range(n + 1):
            t = i / n
            x, y = a[0] + t * (b[0] - a[0]), a[1] + t * (b[1] - a[1])
            self.checks += 1
            if any(x0 <= x <= x1 and y0 <= y <= y1 for x0, y0, x1, y1 in rs):
                return False
        return True

class Roadmap:
    def __init__(self, world, k=10):
        self.w, self.k = world, k

    def build(self, pts):
        self.nodes = [p for p in pts if self.w.free(p)]
        self.adj = [dict() for _ in self.nodes]
        self.alive = [True] * len(self.nodes)
        self.tree = cKDTree(self.nodes)
        _, nbrs = self.tree.query(self.nodes, k=min(self.k + 1, len(self.nodes)))
        for i, row in enumerate(nbrs):
            for j in map(int, row[1:]):
                if j in self.adj[i]:
                    continue                      # edge already validated from j's side
                if self.w.segment_free(self.nodes[i], self.nodes[j]):
                    self.adj[i][j] = self.adj[j][i] = math.dist(self.nodes[i], self.nodes[j])
        self._label_components()

    def _label_components(self):
        self.comp = [-1 if a else -2 for a in self.alive]    # -2 marks a dead node
        c = 0
        for s in range(len(self.nodes)):
            if self.comp[s] != -1:
                continue
            stack, self.comp[s] = [s], c
            while stack:
                u = stack.pop()
                for v in self.adj[u]:
                    if self.comp[v] < 0:
                        self.comp[v] = c
                        stack.append(v)
            c += 1
        self.n_components = c

    def attach(self, q):
        """Indices of up to k live roadmap nodes visible from q."""
        _, idx = self.tree.query(q, k=min(self.k, len(self.nodes)))
        return [j for j in map(int, idx)
                if self.comp[j] >= 0 and self.w.segment_free(q, self.nodes[j])]

    def query(self, s, g):
        if not (self.w.free(s) and self.w.free(g)):
            return None
        S, G = self.attach(s), self.attach(g)
        shared = {self.comp[j] for j in S} & {self.comp[j] for j in G}
        if not shared:
            return None                           # answered without a graph search
        goal_set = {j for j in G if self.comp[j] in shared}
        h = lambda j: math.dist(self.nodes[j], g)
        dist, prev, pq = {}, {}, []
        for j in S:
            if self.comp[j] in shared:
                dist[j], prev[j] = math.dist(s, self.nodes[j]), None
                heapq.heappush(pq, (dist[j] + h(j), dist[j], j))
        while pq:
            _, d, u = heapq.heappop(pq)
            if d > dist.get(u, math.inf):
                continue
            if u == -1:                           # popped the goal itself: done
                path, u = [g], prev[-1]
                while u is not None:
                    path.append(self.nodes[u])
                    u = prev[u]
                return [s] + path[::-1]
            nbrs = list(self.adj[u].items())
            if u in goal_set:
                nbrs.append((-1, math.dist(self.nodes[u], g)))
            for v, w in nbrs:
                if d + w < dist.get(v, math.inf):
                    dist[v], prev[v] = d + w, u
                    heapq.heappush(pq, (d + w + (0 if v == -1 else h(v)), d + w, v))
        return None

    def add_obstacle(self, rect):
        """Invalidate only what the new box can touch; return edges re-checked."""
        x0, y0, x1, y1 = rect
        self.w.rects.append(rect)
        rechecked = 0
        for i, p in enumerate(self.nodes):
            if self.alive[i] and x0 <= p[0] <= x1 and y0 <= p[1] <= y1:
                for j in list(self.adj[i]):
                    del self.adj[j][i]
                self.adj[i].clear()
                self.alive[i] = False             # node now inside an obstacle
        for i, p in enumerate(self.nodes):
            for j in [j for j in self.adj[i] if j > i]:
                q = self.nodes[j]
                if max(p[0], q[0]) < x0 or min(p[0], q[0]) > x1 or \
                   max(p[1], q[1]) < y0 or min(p[1], q[1]) > y1:
                    continue                      # bounding boxes disjoint: cannot hit
                rechecked += 1
                if not self.w.segment_free(p, q, rects=[rect]):
                    del self.adj[i][j], self.adj[j][i]
        self._label_components()
        return rechecked

Sampling: Halton versus uniform, measured

Independent uniform samples clump and leave holes. A Halton sequence pairs van der Corput sequences in coprime bases (here 2 and 3). Every prefix of it covers the square evenly, which keeps its dispersion, the radius of the largest empty ball, small. A deterministic sequence gives only one roadmap per map. Adding a random shift modulo 1, the Cranley-Patterson rotation, gives independent trials that are still evenly spread. LaValle's Planning Algorithms (chapter 5) develops the dispersion argument properly.

The test map has a wall at x = 0.48 to 0.52 with a gap from y = 0.47 to 0.50, so the passage is 0.03 wide, plus two box obstacles. Its 34 fixed queries all run from the left side of the wall to the right side. Each configuration was built 20 times (seeds 0 to 19) with k = 10, and a run counts as a success only if it answered all 34:

Halton with 250 samples was as reliable as uniform with 1,000 to 2,000 on this map. The uniform column is noisy at 20 runs (500 scored lower than 250), but the gap between the columns is consistent. The cause is the gap in the wall. A run fails when no sample lands in the 0.03-wide passage, or when samples land there but their k nearest neighbours do not reach across it. Even coverage makes both less likely. Low-dispersion sampling is not a fix for truly narrow passages, though. For those, combine it with the bridge and Gaussian samplers from the companion article.

Worked example: what a query costs

With seed 0, Halton sampling and 2,000 samples, 1,764 samples were free. They formed 9,673 edges in one component, and the build cost 95,037 point checks. Then 200 random free start-goal pairs (seed 1) were all solved, at an average of 170 point checks per query. In other words, the build cost about as much as 560 queries. That ratio is the economics of PRM. If you will ask fewer queries than that in the roadmap's lifetime, a single-query planner such as RRT is cheaper.

Per-query cost has three parts. Two point checks validate start and goal. Up to 2k segment checks attach them, and these dominate. The search needs no collision checks, because every edge was validated at build time. The component test turns the worst case, an unreachable goal, into the cheapest one; without it, A* would exhaust the start's whole component before giving up.

Repairing the roadmap when the world changes

Rebuilding after every world change throws the amortisation away. A box obstacle can only invalidate two things. Nodes inside it die, together with their edges. Edges that pass through it are deleted, and only edges whose bounding box overlaps the new box can pass through it. So add_obstacle re-checks just those edges, against the new box alone, because the edge already avoided every old obstacle.

The new box was (0.20, 0.55) to (0.40, 0.75). It killed 82 nodes, and the edge count fell from 9,673 to 9,160. The bounding-box filter left only 4 edges to re-check, at a cost of 32 point checks. A full rebuild with the same samples cost 91,724. Of the 200 earlier queries, 177 still had free endpoints. All 177 were solved, and every returned segment passed an independent check against the updated world. Real robots need a broad phase (bounding-volume hierarchies or a spatial hash over edges) rather than a linear scan, but the principle is the same: repair cost scales with the change, not with the roadmap.

Removing an obstacle leaves freed space with no samples in it: add fresh samples there, connect them with the same k-nearest rule, and relabel.

Operational guidance

• Persist the roadmap with its provenance: store nodes, edges, sampler seed, k, the checker resolution and a hash of the world model, and refuse to load a roadmap whose world hash does not match.
• Count collision checks in production metrics: track checks per query and per build. A jump in checks per query usually means the start or goal sit in cluttered regions where attachment fails.
• Keep it sparse when memory or latency matter: visibility-based PRM (Simeon, Laumond and Nissoux) keeps only guard and connector nodes, and SPARS-style planners (OMPL ships SPARS and SPARStwo) trade a bounded path-quality loss for far smaller graphs.
• Smooth after the search: shortcut waypoint pairs with fresh segment checks before handing the path to a controller.

Failure modes

• Tunnelling: a segment resolution coarser than the thinnest obstacle lets edges pass through walls. In joint space, account for lever arms: a small joint step moves the tool tip much further.
• Stale roadmaps: serving paths from a roadmap built for an older world is a safety bug. Gate every load on the world hash, and re-validate the final path before execution as a last line of defence.
• False "no path": different components mean "not connected yet". Log failed queries and grow the roadmap near them before reporting failure.
• Disconnected components that should merge: k-nearest connection with a small k can leave clusters that never link up. Check the component count after every build, and alarm if it exceeds the count you expect for the map.

Trade-offs

What to do next

1. Run the code above on the wall map and reproduce the Halton versus uniform table, then narrow the gap to 0.015 and watch both columns fall.
2. Wrap your own collision checker in the World interface and count point checks per build and per query before you tune anything.
3. Estimate your query volume. If it is below the build-to-query cost ratio you measure, use RRT-Connect instead.
4. Add a world hash to your roadmap file format and refuse mismatched loads.
5. Implement obstacle insertion with a bounding-box filter, then test it: after every change, compare query answers with a fresh rebuild on the same samples.
6. Read the motion planning deep dive for narrow-passage samplers and PRM* before moving to a high-dimensional arm.