explainer
Probabilistic roadmaps: reuse a graph for new routes
Build a probabilistic roadmap from collision-free samples, attach new start and goal queries, and search the same graph for routes. Explore neighbor counts, missed connections, and what a finite roadmap can prove.
What you will learn
- Build a graph whose nodes and complete edge motions pass collision checks.
- Explain how nearest-neighbor proposals become undirected roadmap edges.
- Attach different start and goal pairs without rebuilding the roadmap.
- Sum query connections and roadmap edges to calculate route length.
- Distinguish a missing graph route from an impossible physical motion.
Before you start
A robot can visit many destinations while the room stays the same. Repeating all its collision checks for each trip wastes useful work.
A probabilistic roadmap, or PRM, stores sampled free configurations and valid motions between them. Each new trip connects its start and goal to that graph, then searches for a route.
Build once and answer several queries
The method has two phases:
- Construction: sample free configurations and check local connections between nearby samples.
- Query: attach a chosen start and goal, then search the combined graph.
Kavraki, Švestka, Latombe and Overmars describe this division in their 1996 PRM paper. Its stored graph supports new queries in the same static workspace.
A roadmap can contain cycles and several disconnected components. An RRT grows a tree from a starting configuration. The roadmap here samples across the room before receiving any start or goal.
Reuse assumes the robot and obstacles stay fixed. If the robot grows or an obstacle moves, the old collision checks need revalidation. LaValle’s multiple-query discussion develops that fixed-model setting.
Define the robot and its free space
Use a disk of radius ρ = 0.2 m inside a 6 m × 6 m room. The default circular obstacle has center c = (3, 3) m and radius R = 1 m.
Represent the robot by its center q. It clears the obstacle when ‖q − c‖₂ > R + ρ = 1.2 m, and clears the walls when both coordinates lie strictly between 0.2 and 5.8 m. The configuration-space lesson explains how this point represents the whole disk.
Contact counts as collision. The implementation also rejects clearance at most 10⁻¹⁰ m to handle roundoff. This small numerical tolerance supplies no physical allowance for sensing or tracking errors.
Straight center motions work for this freely translating disk. A robot with joints or steering limits needs a suitable configuration distance and local motion method. Differential-drive kinematics shows how wheel speeds constrain a mobile robot's center motion and heading.
Keep collision-free sample centers
Draw candidate centers across the inset square. Check each candidate as a stationary disk and retain it only if it clears the walls and every obstacle. The accepted centers become roadmap nodes.
The slider sets the number of sample attempts. Invalid candidates still use an attempt, so 60 attempts can produce fewer than 60 nodes. The default seed produces 52 valid nodes and eight rejected samples.
The seed makes the example repeatable. Its unsigned 32-bit state follows s ← (1664525s + 1013904223) mod 2³², and each draw returns s/2³². Each attempt consumes two draws, one per coordinate, using coordinate = 0.2 + 5.6 × draw.
Changing only the query leaves those samples untouched. Increasing the attempt count with the same seed preserves the earlier sampled points, though rebuilding their neighbor connections can change old edges.
Choose neighbors and propose connections
Connecting every pair costs up to n(n − 1)/2 motion checks for n nodes. This experiment limits the proposals through a k-nearest-neighbor rule:
- For each node, sort all other nodes by Euclidean distance.
- Propose its first k neighbors, or all remaining nodes if fewer exist.
- Combine the proposals into distinct unordered pairs.
- Check each proposed pair once and keep it only if the motion is clear.
Either endpoint can propose a pair. The resulting graph is undirected because this disk can traverse a clear segment in either direction. A node can have more than k edges when other nodes select it.
Distance ties use accepted sample order. The checker processes distinct pairs in increasing endpoint-index order. A rejected pair gets no farther-away replacement, making the connection rule explicit and repeatable.
LaValle compares nearest-neighbor and radius choices. This lesson uses a batch of points with the stated fixed-k rule; PRM implementations can use other connection strategies.
Validate each complete edge motion
Two valid sample centers can have an obstacle between them. To test a proposed segment from a to b, find its closest point to each obstacle center c:
d = b − a
τ = min(1, max(0, ((c − a) · d)/(d · d)))
q_near = a + τd
clearance = ‖q_near − c‖₂ − (R + ρ)
When a = b, use a itself as the closest point. For a nonzero segment, the continuous collision check derives the projection and explains why it covers every position between the endpoints.
Both endpoints must clear the inset walls. The inset square is convex, so the straight segment between two points inside it stays inside it. Retain the edge only when every obstacle check also passes.
Store the edge weight as ‖b − a‖₂ meters. The graph now records motions the disk can follow under this geometric model.
Attach a start and goal to the graph
A query supplies a valid start S and goal G. The experiment adds a temporary copy of each endpoint and tries its k nearest roadmap nodes. It checks the complete connecting segments using the same local motion test.
It also checks the direct S-to-G segment, independent of k. A clear direct edge can answer a query even when the roadmap offers no useful connections. That shortcut explains the straight result in Along the left side.
Search the roadmap plus these temporary edges with Dijkstra’s algorithm. Every weight is a nonnegative distance, and the search stops when the goal leaves the priority queue. The route cost includes both endpoint attachments and every roadmap edge it follows.
Equal queue costs use node index, and an equal candidate leaves its current parent unchanged. Changing the query discards the old temporary edges and search records. The stored roadmap remains the same.
Calculate a route through four chosen nodes
For a hand calculation, choose four roadmap nodes around the default obstacle:
| Node | Center (m) |
|---|---|
| A | (1, 1) |
| B | (1, 5) |
| C | (5, 5) |
| D | (5, 1) |
With k = 2, each node proposes its two adjacent corners, each 4 m away. The diagonal distance is √32 ≈ 5.656854 m, so neither diagonal enters the candidate set. The union contains the four square sides.
Each side stays 2 m from the obstacle center at its closest point. Its minimum clearance is 2 − 1.2 = 0.8 m, so all four edges pass. All endpoints also clear the walls.
Set S = (1, 3) m and G = (5, 3) m. Their two nearest roadmap neighbors lie 2 m away. The upper route S → B → C → G costs 2 + 4 + 2 = 8 m, and the lower route has the same cost.
The direct segment crosses the obstacle center and fails with minimum clearance −1.2 m. Both 8 m routes minimize cost in this small graph. Other continuous routes through the room need not follow its four corners.
Reuse a sampled roadmap for a new route
The default room uses seed 7, 60 sample attempts and six neighbor proposals per node. Its 52 nodes and 188 edges support the middle query with an 8-segment route of 6.505341 m.
Choose Across the diagonal. The blue roadmap stays fixed while the query connections and returned route change. The new 9-segment route measures 7.683467 m; both this query and the middle query have blocked direct connections.
Choose Along the left side for a clear direct segment. Its route measures 4.8 m and uses one segment. Return to Across the middle, lower attempts to 20, and set neighbors to two: the search reports No route in roadmap.
Keep those 20 attempts and raise neighbors to six. The same 18 valid sample nodes now support a 6.304260 m route. The room never changed; the extra candidate connections changed what the graph could represent.
Read graph success and failure correctly
A returned route consists entirely of checked motions. Dijkstra makes it shortest in the current graph. The finite sample set and connection rule limit which physical motions that graph includes.
Increasing k on the same samples adds candidate pairs, so retained edges and valid endpoint connections cannot disappear. The graph's shortest route can stay the same or become shorter. The computation also performs more motion checks.
Increasing the sample-attempt count rebuilds the fixed-k neighbor lists. New nearby nodes can push an old connection out of those lists. The 20-attempt default-seed route of 6.304260 m becomes 6.505341 m at 60 attempts, illustrating why this implementation gives no promise of steady improvement as samples increase.
No route in roadmap means the current graph does not connect the query endpoints. Sparse samples, rejected connections or a narrow passage can cause that result even when physical motion is possible. LaValle describes this limitation of a failed query.
The Solid barrier consists of six inflated disks that overlap and join opposite walls. Geometry establishes that the middle query has no physical route there. The graph search itself only reports its graph failure.
The Narrow passage leaves a 0.20 m center gap between two inflated obstacles at x = 3 m. The middle query happens to pass straight through it, so the explicit direct check succeeds. The diagonal query must use other checked connections; a narrow passage need not make every query hard.
PRM* changes the connection rule as the graph grows to support an asymptotic optimality guarantee under its assumptions. OMPL distinguishes regular PRM from PRM*, and Karaman and Frazzoli give the analysis. This bounded fixed-k demonstration establishes neither asymptotic optimality nor probabilistic completeness.
A returned polyline can still have sharp corners. Path smoothing considers additional local connections, each of which needs its own collision check. Execution also requires a motion model and timing that match the real robot.
Reproduce roadmap reuse in Python
This Python 3 example uses only the standard library. It builds the room's roadmap once and applies three queries to it. The final case builds a separate roadmap for the barrier.
from math import hypot
from heapq import heappop, heappush
RHO, EPS = 0.2, 1e-10
def distance(a, b):
return hypot(a[0] - b[0], a[1] - b[1])
def clear(a, b, obstacles):
if not all(RHO + EPS < v < 6 - RHO - EPS for p in (a, b) for v in p):
return False
dx, dy = b[0] - a[0], b[1] - a[1]
length2 = dx * dx + dy * dy
for center, radius in obstacles:
t = 0 if length2 == 0 else max(0, min(1,
((center[0] - a[0]) * dx + (center[1] - a[1]) * dy) / length2))
closest = (a[0] + t * dx, a[1] + t * dy)
if distance(closest, center) - radius - RHO <= EPS:
return False
return True
def nearest(nodes, point, k, exclude=-1):
return sorted((i for i in range(len(nodes)) if i != exclude),
key=lambda i: (distance(nodes[i], point), i))[:k]
def build(obstacles, attempts=60, k=6, seed=7):
state = seed
def random():
nonlocal state
state = (1664525 * state + 1013904223) % 2**32
return state / 2**32
nodes = []
for _ in range(attempts):
point = (0.2 + 5.6 * random(), 0.2 + 5.6 * random())
if clear(point, point, obstacles):
nodes.append(point)
pairs = {tuple(sorted((i, j))) for i, point in enumerate(nodes)
for j in nearest(nodes, point, k, i)}
edges = [(i, j, distance(nodes[i], nodes[j])) for i, j in sorted(pairs)
if clear(nodes[i], nodes[j], obstacles)]
return nodes, edges
def query(roadmap, obstacles, start, goal, k=6):
base, edges = roadmap
nodes = base + [start, goal]
source, target = len(base), len(base) + 1
overlay = []
for endpoint in (source, target):
for i in nearest(base, nodes[endpoint], k):
if clear(base[i], nodes[endpoint], obstacles):
overlay.append((i, endpoint, distance(base[i], nodes[endpoint])))
if clear(start, goal, obstacles):
overlay.append((source, target, distance(start, goal)))
adjacency = [[] for _ in nodes]
for i, j, cost in edges + overlay:
adjacency[i].append((j, cost))
adjacency[j].append((i, cost))
costs = [float("inf")] * len(nodes)
costs[source] = 0.0
parents, heap = {}, [(0.0, source)]
while heap:
cost, node = heappop(heap)
if cost != costs[node]:
continue
if node == target:
path = [target]
while path[-1] != source:
path.append(parents[path[-1]])
return list(reversed(path)), cost
for neighbor, weight in adjacency[node]:
candidate = cost + weight
if candidate < costs[neighbor]:
costs[neighbor] = candidate
parents[neighbor] = node
heappush(heap, (candidate, neighbor))
return [], None
room = [((3, 3), 1)]
roadmap = build(room)
for label, start, goal in [
("middle", (0.6, 3), (5.4, 3)),
("diagonal", (0.6, 1), (5.4, 5)),
("left", (0.6, 0.6), (0.6, 5.4)),
]:
path, length = query(roadmap, room, start, goal)
result = "No route found" if length is None else f"{length:.6f} m"
print(f"{label}: nodes={len(roadmap[0])}, edges={len(roadmap[1])}, length={result}")
barrier = [((3, i + 0.5), 0.65) for i in range(6)]
blocked_map = build(barrier)
_, length = query(blocked_map, barrier, (0.6, 3), (5.4, 3))
print("barrier:", "No route found" if length is None else f"{length:.6f} m")
Expected output:
middle: nodes=52, edges=188, length=6.505341 m
diagonal: nodes=52, edges=188, length=7.683467 m
left: nodes=52, edges=188, length=4.800000 m
barrier: No route found
Use the same k for build and query when reproducing the widget's controls. The small implementation sorts neighbor lists and uses a heap for query search. Production planners can use spatial indexes to reduce neighbor-search work.
Try it yourself
1. Add diagonal proposals. Return to the four chosen nodes A, B, C and D. Increase k from two to three. How many distinct pairs become candidates, how many edges survive, and does the S-to-G route get shorter?
Show the connection-count solution
Each node now proposes the other three. Deduplicating gives 4 × 3 / 2 = six candidate pairs. The four square sides remain clear, while both diagonals cross the obstacle center and fail.
Four roadmap edges survive, so the shortest query route remains 8 m. The extra proposals spend two more motion checks without adding a usable edge in this example.
2. Diagnose a failed query. Use the room, seed 7, 20 attempts, two neighbors and the middle query. It finds no route. Raising neighbors to six finds a 6.304260 m route: what stayed fixed, what changed, and what can you conclude about the earlier result?
Show the roadmap-failure solution
The obstacle, robot, query endpoints and 18 valid sampled nodes stayed fixed. The neighbor rule added candidate pairs and expanded the endpoint attachment sets. The roadmap grew from 21 retained edges to 62.
The later route gives a checked physical connection in this model. The earlier failure therefore showed a missing connection in its limited graph. It could not establish that motion through the room was impossible.
Sources and further study
- Kavraki, Švestka, Latombe and Overmars, Probabilistic Roadmaps for Path Planning in High Dimensional Configuration Spaces (1996). The original two-phase method for repeated queries in static workspaces.
- Steven LaValle, Planning Algorithms, Section 5.6: Roadmap Methods for Multiple Queries. Preprocessing, neighbor selection, query attachment and failure interpretation.
- OMPL: Available Planners. PRM, PRM*, lazy validation and other planner variants.
- Karaman and Frazzoli, Sampling-based Algorithms for Optimal Motion Planning. The connection assumptions behind asymptotic cost guarantees.