Chapter 11: Sampling-Based Planning — RRT and Variants

Grid-based planners discretise the configuration space into a fixed-resolution grid. For a planar robot $(x, y, \theta)$, even a coarse 3-D grid of $200 \times 200 \times 36$ cells has 1.44 million nodes — manageable, but expensive. For robots with more degrees of freedom (arm + base), grid search becomes infeasible. Sampling-based planners sidestep the grid by randomly sampling configurations and incrementally building a tree or roadmap. They are probabilistically complete: as the number of samples grows, the probability of finding a path if one exists approaches 1.

11.1 Rapidly-Exploring Random Trees (RRT)

The RRT algorithm builds a tree rooted at the start configuration. At each iteration:

  1. Sample a random configuration $\mathbf{q}{rand}$ (with probability $p{goal}$, bias toward the goal instead).
  2. Nearest node in the tree: find $\mathbf{q}{near}$ closest to $\mathbf{q}{rand}$ by Euclidean distance.
  3. Steer from $\mathbf{q}{near}$ toward $\mathbf{q}{rand}$ by at most $\Delta q$ (step size) to get $\mathbf{q}_{new}$.
  4. Collision check the segment from $\mathbf{q}{near}$ to $\mathbf{q}{new}$. If free, add $\mathbf{q}_{new}$ to the tree.
  5. Goal check: if $\mathbf{q}_{new}$ is within goal tolerance, reconstruct path.
import random, math

def rrt(start, goal, is_free, step=0.1, max_iter=5000, p_goal=0.1):
    tree = [start]
    parent = {start: None}
    for _ in range(max_iter):
        q_rand = goal if random.random() < p_goal else random_config()
        q_near = min(tree, key=lambda n: dist(n, q_rand))
        q_new  = steer(q_near, q_rand, step)
        if is_free(q_near, q_new):
            tree.append(q_new)
            parent[q_new] = q_near
            if dist(q_new, goal) < step:
                parent[goal] = q_new
                return reconstruct(parent, goal)
    return None   # no path found within budget

Worked example: in a 4 m × 4 m room with two rectangular obstacles, RRT with step size 0.1 m and 2000 iterations typically finds a path in under 100 ms on a laptop. The path is jagged (short straight segments) but avoids all obstacles.

11.2 RRT-Connect

RRT-Connect grows two trees simultaneously — one from the start, one from the goal — and tries to connect them at each iteration. This typically converges 5–10× faster than single-tree RRT because both trees explore the space and meet in the middle.

def rrt_connect(start, goal, is_free, step=0.1, max_iter=2000):
    tree_a, tree_b = [start], [goal]
    parent_a = {start: None}
    parent_b = {goal: None}
    for _ in range(max_iter):
        q_rand = random_config()
        q_new_a = extend(tree_a, parent_a, q_rand, step, is_free)
        if q_new_a:
            q_new_b = connect(tree_b, parent_b, q_new_a, step, is_free)
            if q_new_b == q_new_a:   # trees joined
                return merge_paths(parent_a, parent_b, q_new_a)
        tree_a, tree_b = tree_b, tree_a
        parent_a, parent_b = parent_b, parent_a
    return None

11.3 RRT* — Asymptotically Optimal RRT

Standard RRT finds a path but not necessarily the shortest one. RRT* adds two modifications:

  1. Near-set rewiring: when adding $\mathbf{q}{new}$, check all nodes within a radius $r$ (which decreases with sample density). For each nearby node $\mathbf{q}{near}$, if routing through $\mathbf{q}{new}$ gives a lower cost than $\mathbf{q}{near}$’s current cost, rewire.
  2. Cost tracking: maintain the cumulative cost from the start to each tree node.

As the number of samples $n \to \infty$, RRT* converges to the optimal path. In practice, RRT* finds a near-optimal path with 2–5× more iterations than RRT.

The near-set radius is chosen as: $r = \min\left(\gamma \cdot \sqrt{\log n / n},\ \Delta q\right)$

with $\gamma$ a constant depending on the space dimension.

11.4 Kinodynamic RRT for Non-Holonomic Robots

Standard RRT uses Euclidean distance for nearest-neighbour search and straight-line steering. A differential-drive robot cannot follow arbitrary straight-line segments; it must satisfy the non-holonomic constraint. Kinodynamic planning modifies RRT to work with the robot’s actual dynamics:

A simple practical approximation: use standard Euclidean RRT for the $(x, y)$ projection, then use the path smoother (Section 10.6) to convert waypoints into a smooth trajectory the robot can execute.

11.5 Probabilistic Roadmap (PRM)

PRM pre-computes a roadmap for a static environment, then uses graph search for individual queries. It is most useful when many queries will be made in the same environment:

  1. Build phase: sample $N$ random free configurations. For each, attempt to connect it to its $k$ nearest neighbours using straight-line segments. Store connected pairs as graph edges.
  2. Query phase: connect start and goal to the nearest roadmap nodes, then apply A* on the roadmap graph.

PRM amortises the sampling cost over many queries and is efficient for environments that change rarely.

11.6 Practical Considerations

Step size $\Delta q$: Too large → paths hug obstacles poorly; too small → many nodes needed. A good starting value: 10–20 cm (2–4 grid cells at 5 cm resolution).

Goal bias $p_{goal}$: 5–10% goal-biased samples speed convergence without sacrificing coverage.

Collision checking: the bottleneck in sampling-based planning. Use the inflated occupancy grid from Chapter 10: check each point along the segment at intervals of $\Delta q / 2$ to ensure the entire segment is free.

Replanning: when the environment changes (a new obstacle is detected), the tree must be partially invalidated. Dynamic RRT variants (D-RRT) handle this incrementally. For simple cases, just replan from scratch — if the grid is small and the planner is fast, this is practical in real time.

The combination of offline RRT planning and online reactive avoidance (Chapter 9) forms the backbone of the navigation pipeline in Chapter 15.

Navigation: