Graph Search: Dijkstra and A*

The C-space $\mathcal{C}$ is continuous; computers need discrete graphs. The standard move is to overlay a regular grid: slice each DOF axis into cells of width $\epsilon$, giving $(1/\epsilon)^d$ nodes. Each node $n$ connects to its lattice neighbours (4-connected or 8-connected in 2-D; $2d$ or $3^d - 1$ in $d$-D) with edge weight equal to the Euclidean distance between centres.

Before adding a node or edge to the graph, run the collision checker. Only nodes in $\mathcal{C}_{\text{free}}$ are kept. The result is a finite, weighted, undirected graph $G = (V, E, w)$. Finding the shortest collision-free path from $q_s$ to $q_g$ is now exactly the single-source shortest path problem — and Dijkstra's algorithm solves it optimally. The catch: $|V|$ is exponential in $d$, so grid search only works up to about 4 DOF in practice.

Dijkstra maintains a priority queue (min-heap) of (cost, node) pairs and a distance map $g[n]$ — the best known cost to reach $n$ from the start. It expands the cheapest unvisited node and relaxes its neighbours:

$g[m] \leftarrow \min\bigl(g[m],\; g[n] + w(n, m)\bigr)$

Guarantees: Dijkstra is complete (finds a path if one exists in the finite graph) and optimal (the path has minimum total weight). Optimality relies on non-negative edge weights — satisfied for Euclidean grids. Complexity is $O((|V| + |E|) \log |V|)$ with a binary heap.

Problem for robotics: Dijkstra explores in expanding cost shells — roughly circular wavefronts around the start. It has no notion of direction toward the goal, so it wastes time exploring nodes that are far from $q_g$. In a 1000x1000 grid, Dijkstra may visit nearly every cell before reaching the goal in a corner. A* fixes this with a heuristic.

A* replaces Dijkstra's queue key $g(n)$ with:

$f(n) = g(n) + h(n)$

where $g(n)$ is the exact cost from start to $n$, and $h(n)$ is a heuristic estimate of the remaining cost to the goal. The node with the lowest $f$ is expanded next.

Admissibility is the key condition: $h(n) \leq h^*(n)$ for all $n$, where $h^*(n)$ is the true optimal remaining cost. An admissible heuristic never overestimates.

Consistency (monotonicity) is a stronger condition: $h(n) \leq w(n,m) + h(m)$ for every edge $(n,m)$. Consistency implies admissibility and ensures each node is expanded at most once — the standard assumption for A* on graphs.

Theorem: A* with a consistent heuristic is complete and optimal among algorithms that use the same heuristic. It expands no node with $f(n) < f^*$ (the optimal cost) and exactly the nodes needed along the optimal path.

The heuristic $h$ must be fast to compute and tight (close to $h^*$) to be useful. Common choices for grid/C-space planning:

For a robot arm in joint space, Euclidean joint-space distance is admissible only if the maximum Cartesian speed per unit joint motion is bounded. If your joint weights are wrong, your heuristic may be inadmissible — and optimality silently breaks. Always verify admissibility after changing robot geometry or units.

Greedy best-first search (GBFS) sorts the queue by $h(n)$ only — ignoring $g(n)$ entirely. It drives hard toward the goal heuristically and is often extremely fast. But it provides no optimality guarantee and is incomplete on graphs with cycles or dead ends.

Consider a narrow corridor: GBFS will rush down the corridor, hit a dead end, and get stuck — it has no memory of accumulated cost to guide backtracking intelligently. A* would have balanced $g$ (the cost of walking down the corridor) against $h$ (the hope at the end) and avoided the trap.

The A* spectrum:

• $h = 0$: Dijkstra (optimal, slow).
• $h = h^*$: perfectly informed A* (optimal, maximally fast).
• $h > h^*$ inadmissible: Weighted A* / GBFS (fast, suboptimal).

For real robots, Weighted A* with $w \in [1.1, 3]$ is a practical middle ground — you sacrifice a bounded factor of $w$ in path quality for significant speed gains.

A clean A* implementation on a 2-D occupancy grid illustrates every component:

import heapq
import math

def astar(grid, start, goal):
    """A* on a 2D grid. grid[r][c] = True means obstacle.
       Returns list of (r,c) or None."""
    rows, cols = len(grid), len(grid[0])
    def h(a, b):
        return math.hypot(a[0]-b[0], a[1]-b[1])  # Euclidean, admissible

    g = {start: 0.0}
    came_from = {start: None}
    heap = [(h(start, goal), start)]  # (f, node)

    while heap:
        _, n = heapq.heappop(heap)
        if n == goal:
            path = []
            while n is not None:
                path.append(n); n = came_from[n]
            return path[::-1]
        r, c = n
        for dr, dc in [(-1,0),(1,0),(0,-1),(0,1),
                       (-1,-1),(-1,1),(1,-1),(1,1)]:
            m = (r+dr, c+dc)
            if not (0<=m[0]<rows and 0<=m[1]<cols): continue
            if grid[m[0]][m[1]]: continue          # collision
            w = math.hypot(dr, dc)                 # 1.0 or sqrt(2)
            ng = g[n] + w
            if ng < g.get(m, float('inf')):
                g[m] = ng
                came_from[m] = n
                heapq.heappush(heap, (ng + h(m, goal), m))
    return None  # no path

Key details: diagonal moves cost $\sqrt{2}$, keeping Euclidean distance admissible. The heap may contain stale entries — the ng < g.get(m, inf) guard handles this without a separate closed set.

Choosing $\epsilon$ (cell size) is an engineering decision with hard tradeoffs:

Fine grid ($\epsilon$ small): more cells explored, higher memory, slower per query, but paths hug obstacles more tightly and kinematic feasibility is easier to post-process.

Coarse grid ($\epsilon$ large): fast, low memory, but may miss narrow passages entirely. A gap narrower than $\epsilon$ is invisible to the planner — this is resolution incompleteness. The planner declares no path exists when one does.

For 2-D mobile robots, $\epsilon = 5\text{–}10\,\text{cm}$ is standard. For 3-D manipulation, grids above 4 DOF at any useful resolution exceed memory limits. At this point:

1. Hierarchical planning — coarse grid for global structure, local sampling for details.
2. Anytime algorithms (ARA*, AD*) — return a suboptimal path immediately, improve over time.
3. Switch to sampling-based — PRM or RRT sidestep the grid entirely.

Grid search is not obsolete: for 2-D navigation it remains the most reliable approach, powering the global costmap in ROS navigation stacks worldwide.

A* is the gold standard for grid search, but several robotics scenarios break its assumptions:

1. Dynamic obstacles. The graph changes during planning. D* Lite (Koenig & Likhachev, 2002) replans incrementally, reusing prior search effort — the algorithm behind the Mars rovers.
2. Kinodynamic constraints. Edge costs depend on velocity state, not just position. A* on a position grid ignores dynamics; lattice planners add state $(q, \dot{q})$ to the node, exploding the graph size.
3. High DOF. Above 4–5 DOF, $|V|$ is astronomical. Sampling-based methods (next lesson) take over.
4. Non-holonomic robots. A car cannot move sideways; valid edges in the graph must respect turning radius. Dubins/Reeds-Shepp paths replace straight-line edges.

The lesson: A* is complete, optimal, and elegant — and also the wrong tool for about half of practical motion planning problems in robotics. Know its limits before reaching for it.