M5-T11: A* and RRT path planning

Learning objectives

After this lesson, you should be able to:

• Implement A* on an occupancy grid with an admissible heuristic and explain optimality.
• Describe how RRT and RRT* build a tree and what “probabilistically complete” and “asymptotically optimal” mean.
• Choose between search-based and sampling-based planning for a given map and dimension.
• Explain why a planner path must be smoothed and time-parameterised before flying.

Conceptual explanation

Established A* (Hart, Nilsson and Raphael, 1968) and RRT (LaValle, 1998) are classical. RRT* is from Karaman and Frazzoli (2011). Statements about guarantees assume the stated conditions (admissible heuristic; a static map; an unbounded sample count for asymptotic results).

A* searches a graph by expanding the node with the smallest $f(n)=g(n)+h(n)$, where $g$ is the cost from the start and $h$ is a heuristic estimate of the cost to the goal. If $h$ never overestimates the true remaining cost (admissible) A* returns an optimal path on that graph; if it is also consistent, each node is expanded at most once.

RRT grows a tree from the start: sample a random state, find the nearest tree node, extend a short step toward the sample, keep the new node if the segment is collision-free. It is probabilistically complete: if a path exists the probability of finding one tends to 1 as samples increase. The path is typically jagged and non-optimal. RRT* rewires nearby nodes to reduce cost and is asymptotically optimal: the path cost converges to the optimum as samples tend to infinity.

Engineering intuition

A* is flood-filling with a compass: it explores outward but biases toward the goal, so open space is cheap. Its cost grows with the number of grid cells, so it suits 2D or 2.5D maps with metre-scale cells. In 3D with centimetre-scale cells the cell count explodes; the grid must be coarse, hierarchical or local.

RRT is throwing darts and connecting them: it does not care how big the space is, and handles high dimension (position plus velocity, or a manipulator’s joints) far better than grids do. The price is randomness: two runs differ, and the first path found is rarely good. For flight, that means a post-processing step (shortcutting, smoothing) and a time budget that cuts the search off.

Planners return paths in a map, and the map has errors. The planner should keep a safety margin from obstacles (inflate the map by the vehicle radius plus an allowance for localisation and tracking error) so that the path is flyable, not merely collision-free in an ideal world.

Mathematical formulation

On an 8-connected grid with cell size 1 the octile distance is an admissible, consistent heuristic; for Euclidean costs the straight-line distance is admissible:

RRT* chooses the best parent among nodes within a connection radius $r_{n}$ that shrinks slowly with $n$ samples; the published choice for asymptotic optimality is

Weighted A* uses $f=g+\varepsilon \,h$ with $\varepsilon >1$: it is faster but only $\varepsilon$-suboptimal (the path cost is at most $\varepsilon$ times the optimum). Anytime variants start with a large $\varepsilon$ and reduce it while time remains.

$g$(n)

cost of the best known path from the start to $n$.

$h$(n)

heuristic estimate of the remaining cost.

$d$

dimension of the planning space.

$\varepsilon$

inflation factor of weighted A*.

Architecture and data flow

Mapping (occupancy grid, ESDF or point cloud) → obstacle inflation → global planner (A* or RRT*) → path shortcutting → trajectory generation (see the trajectory-generation lesson) → tracking controller. For a UAV flying in unknown space the loop repeats: a local map is updated, collision of the current trajectory is checked, and the planner re-runs from the current state. The map’s age and the localisation drift both enter the safety margin.

Real-world UAV applications

• Global route planning in a known 2D or 2.5D map of a site.
• Local replanning around newly detected obstacles in a 3D occupancy map.
• Planning through cluttered environments such as warehouses and forests (sampling-based planning with a short time budget).
• Coverage and inspection path planning.

Implementation example

This runs as shown. A* on a 40 × 40 occupancy grid with a wall and a gap, 8-connected moves, octile heuristic. The test confirms that the path is collision-free and that its cost matches an independent Dijkstra search (A* with $h=0$), which verifies optimality of the admissible heuristic on this map.

import heapq, math

N = 40
blocked = {(20, y) for y in range(N) if y != 28}               # a wall at x = 20 with a gap at y = 28
blocked |= {(x, 10) for x in range(5, 18)}                      # a shorter wall
def neighbours(c):
    for dx in (-1, 0, 1):
        for dy in (-1, 0, 1):
            if (dx or dy):
                n = (c[0] + dx, c[1] + dy)
                if 0 <= n[0] < N and 0 <= n[1] < N and n not in blocked:
                    if dx and dy and ((c[0] + dx, c[1]) in blocked or (c[0], c[1] + dy) in blocked): continue   # no corner cutting
                    yield n, math.hypot(dx, dy)
def octile(a, b):
    dx, dy = abs(a[0] - b[0]), abs(a[1] - b[1]); return (math.sqrt(2) - 1) * min(dx, dy) + max(dx, dy)
def search(start, goal, use_h=True):
    openq = [(0.0, start)]; g = {start: 0.0}; parent = {}; expanded = 0
    while openq:
        f, c = heapq.heappop(openq)
        if f > g[c] + (octile(c, goal) if use_h else 0.0) + 1e-9: continue           # stale entry
        expanded += 1
        if c == goal:
            path = [c]
            while c in parent: c = parent[c]; path.append(c)
            return path[::-1], g[goal], expanded
        for n, w in neighbours(c):
            if g[c] + w < g.get(n, 1e18):
                g[n] = g[c] + w; parent[n] = c
                heapq.heappush(openq, (g[n] + (octile(n, goal) if use_h else 0.0), n))
    return None, None, expanded
start, goal = (2, 2), (37, 37)
path, cost, ex_a = search(start, goal, True)
path0, cost0, ex_d = search(start, goal, False)
print(f"A*: cost {cost:.2f}, {ex_a} expansions   Dijkstra: cost {cost0:.2f}, {ex_d} expansions")
assert all(c not in blocked for c in path) and abs(cost - cost0) < 1e-9 and ex_a < ex_d

For an RRT*, use a tested library rather than writing your own: the Open Motion Planning Library (OMPL) provides RRT, RRT*, Informed RRT* and many others with a common interface.

Common engineering mistakes

Failure modes

Debugging approach

1. Visualise the map, inflated map, expanded nodes and path together.
2. Test the planner on a map where the optimal path is known by hand.
3. Compare with Dijkstra to verify optimality of A* implementations.
4. Check segment (not only node) collision tests and the frame of the map against the vehicle pose.

Practical exercises

1. Replace the octile heuristic with a heuristic multiplied by 3 and report cost and expansions (weighted A*).
2. Add a cost layer that penalises cells within 2 cells of an obstacle and observe the path move away from walls.
3. Implement an RRT in 2D with a 0.5 step and compare the first path found to the A* path.
4. Inflate the map by the vehicle radius and show a gap that becomes impassable.

Interview questions

Write an answer in your own words, then reveal the model answer. Nothing is sent anywhere.

References and further reading

Primary papers, official documentation and standards. Each claim above that is specific to a tool or version points here.

• Hart, P. E., Nilsson, N. J. & Raphael, B. (1968). A formal basis for the heuristic determination of minimum cost paths. IEEE Trans. Systems Science and Cybernetics
• Karaman, S. & Frazzoli, E. (2011). Sampling-based algorithms for optimal motion planning. IJRR 30(7)
• OMPL: the Open Motion Planning Library
• LaValle, S. M. Planning Algorithms (free online book)

What’s next