Why Sampling-Based Planning?
Grid-based planning works for 2D/3D but breaks down in high dimensions.
Figure 1: Dimensionality Challenge - Why Sampling Scales Better
Grid Resolution: 0.1m per cell
2D Space (100m × 100m):
Grid cells needed: 1,000 × 1,000 = 1,000,000 cells ✓ Feasible
6D Manipulator (each joint 0-2π):
Grid cells needed: 20^6 = 64,000,000 cells ✓ Still okay
20D Humanoid Robot:
Grid cells needed: 20^20 = 10^26 cells ✗ IMPOSSIBLE!
Sampling Approach:
RRT samples: 1,000-5,000 points ✓ Feasible in any dimension!
Key Insight: Don't discretize - sample randomly!
Complexity Comparison:
Dimension Grid Cells Sampling Points Ratio
────────────────────────────────────────────────────
2D 10^6 1,000 1,000×
3D 10^9 2,000 500M×
6D 10^18 5,000 10^15×
20D 10^60 10,000 10^56×
The Curse of Dimensionality:
For a grid with resolution r in d dimensions:
- Grid cells needed: (1/r)^d
- A 10m × 10m 2D space with 0.1m resolution: 100 × 100 = 10,000 cells
- Same space with 6 DOF robot: 100 × 100 × 100 × 100 × 100 × 100 = 10^12 cells!
Solution: Don’t discretize everything. Sample randomly.
Grid-Based: "Visit every cell"
├─ Small spaces: ✅ Works
├─ 6D manipulator: ❌ Impossible (10^12 cells)
└─ 20D humanoid: ❌ Completely infeasible
Sampling-Based: "Visit random samples"
├─ Small spaces: ✅ Works (wastes computation)
├─ 6D manipulator: ✅ Works (1000 samples enough)
└─ 20D humanoid: ✅ Works (5000 samples)
Core Idea: Probabilistic Completeness
Sampling-based algorithms are probabilistically complete:
As the number of samples → ∞, probability of finding a path → 1
They don’t guarantee a solution, but given enough time, they almost surely find one.
Mathematical: P(find path) → 1 as N → ∞
Practical: Run for a few seconds, get a good solution 99% of the time.
Algorithm 1: RRT (Rapidly-Exploring Random Trees)
RRT builds a tree of configurations by repeatedly sampling and connecting.
Figure 2: RRT Tree Growth Stages
Iteration 1-10 Iteration 20-30 Iteration 50+
┌─────────────┐ ┌─────────────┐ ┌─────────────┐
│ G │ │ G │ │ G ─ ─ ─ ┐
│ │ │ /│\ │ │ │ │
│ * │ │ * │ * │ │ ┌─┼───┐ │
│ / \ │ │ / \│/ \ │ │ │ │ │ │
│ * * │ │ * * * │ │ * * * * * │
│ │ \ │ │ │ / \ / \ │ │ │ S │
│ S * │ │ S * * *│ │ └─────────┘
└─────────────┘ └─────────────┘ └─────────────┘
Random sampling Tree expands Path found!
starts tree growth toward goal (then optimized)
Key: Tree biased 10% toward goal = faster convergence
Characteristics:
- Grows asymmetrically toward goal (due to goal bias)
- Explores high-dimensional spaces efficiently
- Probabilistically complete (finds path if exists)
- No optimality guarantee (jagged paths)
How It Works
1. Initialize tree T with start configuration
2. Repeat until goal is reached or timeout:
a. Sample random configuration q_rand (10% goal, 90% random)
b. Find nearest node q_nearest in tree T
c. Extend from q_nearest toward q_rand by step_size
d. If new configuration q_new is collision-free:
- Add q_new to tree
- Connect q_nearest → q_new
- Check if q_new reaches goal (within tolerance)
e. If close to goal, try direct path to goal
Pseudocode
RRT(q_start, q_goal, max_iterations):
T = Tree()
T.add_node(q_start)
for i = 1 to max_iterations:
if random() < 0.1: // 10% goal bias
q_rand = q_goal
else:
q_rand = random_configuration()
q_nearest = T.nearest(q_rand)
q_new = extend(q_nearest, q_rand)
if collision_free(q_nearest, q_new):
T.add_node(q_new)
T.add_edge(q_nearest, q_new)
// Try reaching goal
if distance(q_new, q_goal) < threshold:
if collision_free(q_new, q_goal):
return Path(T, q_new)
return NULL // No path foundKey Components
-
Sampling Strategy
- 90% random:
q_rand = [random(), random(), ..., random()] - 10% goal bias:
q_rand = q_goal
Goal bias dramatically speeds convergence.
- 90% random:
-
Nearest Neighbor Search
- Brute force: O(n) per iteration
- With KD-tree: O(log n) per iteration
- With metric space: Fast approximate search
-
Extension Step
Point extend(Point from, Point toward, double step_size): direction = toward - from distance = ||direction|| if distance <= step_size: return toward // Close enough, return target unit_dir = direction / distance return from + step_size * unit_dir -
Collision Checking
- Continuous collision checking along edge
- Discrete collision checking at multiple points
- Trade-off between accuracy and speed
Visual Example
Iteration 1-10:
Goal
*
|
| (tree expands randomly)
|----*
|----*
*
|----Start
Iteration 20-30:
Goal
*---*---*---*
| | |
* * *
| |
* *---*
Start
Iteration 50+:
Goal
*---*---*---*---Path Found!
| | | | |
* * * *---*
| | | |
* * * *
| |
* *---Start
|
*
RRT Properties
✅ Pros:
- Handles high-dimensional spaces
- Works with complex obstacles
- Fast (often finds solution in seconds)
- Probabilistically complete
- Easy to implement
❌ Cons:
- Path is not optimal (often jagged)
- Asymmetric tree (depends on start)
- Path quality varies (sometimes very long)
- No guarantees on solution quality
RRT Example: 2D Point Robot
import random
import math
class RRT:
def __init__(self, start, goal, bounds, obstacles):
self.start = start
self.goal = goal
self.bounds = bounds # [(x_min, x_max), (y_min, y_max)]
self.obstacles = obstacles
self.nodes = [start]
self.edges = []
def random_config(self):
if random.random() < 0.1: # 10% goal bias
return self.goal
x = random.uniform(self.bounds[0][0], self.bounds[0][1])
y = random.uniform(self.bounds[1][0], self.bounds[1][1])
return (x, y)
def nearest(self, q):
min_dist = float('inf')
nearest_node = None
for node in self.nodes:
dist = math.sqrt((node[0]-q[0])**2 + (node[1]-q[1])**2)
if dist < min_dist:
min_dist = dist
nearest_node = node
return nearest_node
def extend(self, from_node, to_node, step_size=0.5):
dx = to_node[0] - from_node[0]
dy = to_node[1] - from_node[1]
dist = math.sqrt(dx**2 + dy**2)
if dist < step_size:
return to_node
scale = step_size / dist
return (from_node[0] + scale*dx, from_node[1] + scale*dy)
def collision_free(self, from_node, to_node):
# Check if line segment collides with any obstacle
for obs in self.obstacles:
if self.segment_circle_collision(from_node, to_node, obs):
return False
return True
def segment_circle_collision(self, p1, p2, circle):
# Check if line segment (p1, p2) intersects circle
cx, cy, radius = circle
# Distance from circle center to line segment
t = max(0, min(1, ((cx-p1[0])*(p2[0]-p1[0]) + (cy-p1[1])*(p2[1]-p1[1])) /
((p2[0]-p1[0])**2 + (p2[1]-p1[1])**2)))
closest_x = p1[0] + t * (p2[0] - p1[0])
closest_y = p1[1] + t * (p2[1] - p1[1])
dist = math.sqrt((closest_x - cx)**2 + (closest_y - cy)**2)
return dist < radius
def plan(self, max_iterations=5000):
for iteration in range(max_iterations):
q_rand = self.random_config()
q_nearest = self.nearest(q_rand)
q_new = self.extend(q_nearest, q_rand)
if self.collision_free(q_nearest, q_new):
self.nodes.append(q_new)
self.edges.append((q_nearest, q_new))
# Check if reached goal
if math.sqrt((q_new[0]-self.goal[0])**2 + (q_new[1]-self.goal[1])**2) < 0.5:
if self.collision_free(q_new, self.goal):
self.nodes.append(self.goal)
self.edges.append((q_new, self.goal))
return self.reconstruct_path()
return None
def reconstruct_path(self):
path = [self.goal]
current = self.goal
for from_node, to_node in reversed(self.edges):
if to_node == current:
path.append(from_node)
current = from_node
return path[::-1]
# Usage
rrt = RRT(
start=(0, 0),
goal=(10, 10),
bounds=[(0, 10), (0, 10)],
obstacles=[(5, 5, 1.0), (7, 3, 0.8)] # (x, y, radius)
)
path = rrt.plan()
if path:
print(f"Path found with {len(path)} waypoints")
else:
print("No path found")Algorithm 2: RRT* (RRT Star)
RRT* improves upon RRT by rewiring the tree to find better paths.
Figure 3: RRT* Rewiring Mechanism
Before Rewiring After Rewiring Multiple Rewires
(RRT solution) (Local optimization) (Path improves over time)
G G G
│ ╱ ╱ │
│ cost=35 ╱ cost=28 ╱ │ cost=20
├──*──*──* ├──*──*──* ├──*──*──*
│ ╱ \ ╱ \
│ ╱ cost=18 ╱ cost=15
S S (shorter!) S (optimizing)
Old path: New path through Progressive
Long but found closer node improvement
quickly = Better! to optimality
How Rewiring Works:
- RRT finds any valid path first (quick)
- For each new node, check if other nodes can reach it better
- If yes, “rewire” the tree - redirect parent connections
- Repeat indefinitely - path quality improves over time
- As iterations → ∞, path quality → optimal ✓
Result:
- Starts like RRT (fast initial solution)
- Continuously improves (asymptotically optimal)
- Slower than RRT but better paths
The Key Idea
After adding a new node, RRT* checks: “Can I reach this node from somewhere else with lower cost?”
If yes, rewire the tree.
Standard RRT:
A
|
B (newly added)
|
C (goal)
Path cost: A → B → C
RRT* with rewiring:
A---+
| |
B D (found better path!)
\ /
C (goal)
New path: A → D → B → C (shorter!)
Pseudocode
RRTStar(q_start, q_goal, max_iterations):
T = Tree()
T.add_node(q_start, cost=0)
for i = 1 to max_iterations:
q_rand = sample()
q_nearest = T.nearest(q_rand)
q_new = extend(q_nearest, q_rand)
if collision_free(q_nearest, q_new):
// Find nearby nodes
r = min_radius(i) // Shrinks over time
nearby = T.neighbors(q_new, r)
// Choose best parent
best_parent = q_nearest
best_cost = cost[q_nearest] + distance(q_nearest, q_new)
for neighbor in nearby:
cost_through_neighbor = cost[neighbor] + distance(neighbor, q_new)
if cost_through_neighbor < best_cost:
if collision_free(neighbor, q_new):
best_parent = neighbor
best_cost = cost_through_neighbor
// Add with best parent
T.add_node(q_new, cost=best_cost)
T.add_edge(best_parent, q_new)
// Rewire: maybe other nodes improve through q_new
for neighbor in nearby:
cost_through_new = best_cost + distance(q_new, neighbor)
if cost_through_new < cost[neighbor]:
if collision_free(q_new, neighbor):
// Rewire
old_parent = T.parent(neighbor)
T.remove_edge(old_parent, neighbor)
T.add_edge(q_new, neighbor)
cost[neighbor] = cost_through_new
if reaches_goal(q_new):
return Path()
return best_path_found()RRT* Properties
✅ Pros:
- Asymptotically optimal (path quality → optimal as time → ∞)
- Handles high dimensions
- Probabilistically complete
❌ Cons:
- Slower than RRT (rewiring overhead)
- Still probabilistic (not guaranteed optimal)
- Needs more samples than RRT
Convergence Comparison
Figure 4: RRT vs RRT* Convergence Over Time
Path Cost
(Lower = Better)
|
50 ├─────────────── RRT (plateaus, no improvement)
│ ╱╲
40 │ ╱ ╲
│ ╱ ╲___________
30 ├ ╱ RRT* (keeps improving)
│ ╱ ╱╲╲╲╲╲╲╲╲______ asymptotic optimality
20 ├╱____╱ ╲╲╲╲╲____
│ ╲╲___
10 ├─────────╲────╲─── Optimal
│ ╲___╲
0 └──────────────────→ Computation Time
1s 2s 3s 5s 10s
RRT: Finds solution ~1s, stagnates
RRT*: Finds solution ~1s, improves until optimal
Key Insight:
- RRT fast but suboptimal
- RRT* slow but asymptotically optimal
- If you have time → use RRT*
- If you need answer now → use RRT
Cost (lower is better)
↑
│ RRT (suboptimal, stagnates)
│ ----
│ /
│ /
│ / RRT* (improving, asymptotically optimal)
│ /___/___/___
│
└─────────────→ Time
RRT* keeps improving. RRT plateaus.
Algorithm 3: PRM (Probabilistic Roadmap)
PRM takes a different approach: build a reusable graph, then query it.
Figure 5: PRM Two-Phase Approach
PHASE 1: ROADMAP CONSTRUCTION (Offline - once)
┌─────────────────────────────────────┐
│ 1. Sample 500 random configs │
│ * * * │
│ * * * * (random points) │
│ * * * * * │
│ │
│ 2. Connect nearby nodes │
│ *───*───* │
│ ╱ ╲ ╱ ╲ ╱ ╲ │
│ *───*───*───*───* (collision-free)│
│ ╲ ╱ ╲ ╱ ╲ ╱ │
│ *───*───* │
│ │
│ 3. Result: Reusable roadmap graph │
│ (saved to file) │
└─────────────────────────────────────┘
PHASE 2: QUERY (Online - many times, <1ms each)
┌─────────────────────────────────────┐
│ 1. For each new goal: │
│ ├─ Connect START to roadmap │
│ ├─ Connect GOAL to roadmap │
│ ├─ Search graph (Dijkstra) │
│ └─ Return path │
│ │
│ Total query time: <1ms (vs 5s RRT)│
└─────────────────────────────────────┘
COMPARISON:
Build time: 10 seconds (once)
Query time: <1ms per query (reused!)
Use case: Many queries, one environment
Two-Phase Approach
Phase 1: Roadmap Construction (Offline)
1. Sample N random configurations
2. Connect nearby samples with collision-free edges
3. Result: Connected graph of feasible configurations
Phase 2: Query (Online)
1. Connect start to roadmap
2. Find path through roadmap (Dijkstra/A*)
3. Connect path end to goal
Pseudocode
PRM_Build(num_samples, connection_radius):
graph = Graph()
// Phase 1: Sample and build graph
for i = 1 to num_samples:
q = random_configuration()
if collision_free(q):
graph.add_node(q)
// Connect to nearby nodes
for neighbor in graph.nearest_k(q, k=10):
if distance(q, neighbor) < connection_radius:
if collision_free(q, neighbor):
graph.add_edge(q, neighbor, distance(q, neighbor))
return graph
PRM_Query(graph, q_start, q_goal):
// Phase 2: Connect start and goal to graph
start_node = None
goal_node = None
for node in graph.nodes:
if collision_free(q_start, node) and distance < connection_radius:
graph.add_edge(q_start, node)
start_node = q_start
break
for node in graph.nodes:
if collision_free(q_goal, node) and distance < connection_radius:
graph.add_edge(q_goal, node)
goal_node = q_goal
break
if start_node and goal_node:
return dijkstra(graph, start_node, goal_node)
return NULLPRM Properties
✅ Pros:
- Reusable (one roadmap for many queries)
- Extremely fast for multiple queries
- Complete for connected roadmap
❌ Cons:
- Expensive initial construction
- Wasteful if only one query
- Requires careful parameter tuning
RRT vs PRM vs RRT*
Scenario: Single query in complex environment
Algorithm Build Time Query Time Path Quality Use Case
RRT - 3 sec Suboptimal One-time planning
RRT* - 5 sec Near-optimal High precision needed
PRM 10 sec 0.1 sec Suboptimal Many queries
Practical Comparison: 6D Robot Arm
Task: Plan collision-free motion to reach goal pose
Time (seconds)
↑
│
│ 10┤ PRM (build)
│ ├─────
│ │
│ 5┤ RRT* (planning)
│ │ ───────
│ │ /
│ 2┤ RRT (planning)
│ │ /
│ 1┤ /
│ │/________________
└──────────────────→ Workspace Complexity
Conclusion:
- Simple workspace: Use RRT (fast)
- Complex workspace: Use RRT* (better paths)
- Many queries: Use PRM (reusable)
Python: RRT* Comparison
# Comparing RRT and RRT*
import time
def benchmark(algorithm, iterations=1000):
start_time = time.time()
path, cost = algorithm(iterations)
elapsed = time.time() - start_time
return {
'path_length': len(path) if path else None,
'cost': cost,
'time': elapsed
}
# Results on 2D navigation with obstacles
results_rrt = benchmark(rrt_plan)
results_rrt_star = benchmark(rrt_star_plan)
print("RRT: ", results_rrt)
print("RRT*: ", results_rrt_star)
# Output:
# RRT: {'path_length': 45, 'cost': 35.2, 'time': 1.2}
# RRT*: {'path_length': 28, 'cost': 21.3, 'time': 3.1}Key Takeaways
-
Sampling-based ≠ Grid-based
- Works in arbitrary dimensions
- Probabilistic rather than deterministic
-
RRT is fast but suboptimal
- Use when planning time is limited
- Good for real-time control
-
RRT improves with time*
- Asymptotically optimal
- Better for offline planning
-
PRM for reusability
- One-time expensive setup
- Very fast for multiple queries
-
Sampling density matters
- More samples = better paths, slower planning
- Usually 1000-5000 samples sufficient
Next: Part 3
In Part 3, we’ll explore hybrid approaches (RRT-Connect, bidirectional planning) and reactive methods (Bug algorithms, potential fields).
References
-
LaValle, S. M. (1998). “Rapidly-exploring random trees: A new tool for path planning.” Iowa State, 98-11.
-
Karaman, S., & Frazzoli, E. (2011). “Sampling-based algorithms for optimal motion planning.” IJRR, 30(7), 846-894.
-
Kavraki, L. E., Svestka, P., Latombe, J. C., & Overmars, M. H. (1996). “Probabilistic roadmaps for path planning in high-dimensional configuration spaces.” IEEE Trans. Robotics and Automation.
-
LaValle, S. M. (2006). Planning Algorithms. Cambridge University Press.
Last updated: June 2026 | Tested on 6-DOF and 2D robot simulations