Section 30.2: Graph search: BFS, Dijkstra, A*

Graph search is a contract between the map discretization and the motion the robot can actually execute.

A Local Planner With Commitment Issues

Problem First

Hand a robot a perfect map and tell it to reach the far corner, and it still cannot move a single wheel. A map says what space exists, not how to cross it. The gap between those two is where every stalled, lost, or circling robot lives. Graph search bridges that gap: it ties a static map representation to a state variable, a cost model, and the next concrete robot action, exactly as Figure 30.2.1 illustrates. A robot has a map, a current position, and a goal. The map is not a path. It represents what space exists; it does not prescribe how to move through it. Graph search converts that static map into an ordered sequence of waypoints the robot can follow. Without this step, the map is inert data.

The three algorithms in this section represent a progression of assumptions about the world. Breadth-first search (BFS) suits maps where all moves cost the same, such as a uniform grid where every step is identical. BFS matters in embodied AI because violating its equal-cost assumption causes silent failures. A robot that treats a steep ramp identically to flat floor will choose paths that are geometrically short but physically dangerous. BFS cannot represent the extra energy or tip-over risk a ramp carries. Mechanically, BFS maintains a queue of nodes to visit. It dequeues the front node, marks it visited, and enqueues all unvisited neighbors at the back. This first-in-first-out discipline explores every node at distance k before it examines any node at distance k+1. That is why BFS returns the fewest-hops path when all edges are equal. Dijkstra's algorithm relaxes that assumption. It finds the cheapest path when edges carry different costs, such as when mud costs more to traverse than pavement. A* adds one more ingredient: an admissible heuristic, an estimate of remaining cost that never overestimates the true remaining cost. This lets the search skip large portions of the map that point in the wrong direction. On a 1,000-by-1,000 warehouse grid, BFS examines roughly 500,000 cells to confirm the shortest path; A* with a Euclidean heuristic examines fewer than 3,000, because a single optimistic number eliminates entire regions before they are touched. Each step in this progression trades a simpler assumption for better real-world applicability.

A* is correct when the heuristic is admissible, meaning it never overestimates the true remaining cost. Consistency gives an even stronger property: once a node is expanded, its best cost is fixed and will not improve later. Those guarantees matter when the graph stands in for physical space.

Take a 5-by-5 grid where a warehouse robot travels from cell (0,0) to cell (4,4). BFS fans out in rings, touching up to 25 cells before it finds the goal. Dijkstra with uniform cost does the same work but also handles a variant where the center row costs 3 to cross a slope. A* with a Euclidean heuristic expands roughly 9 cells, skipping the left and bottom edges because the estimate steers it diagonally toward the goal, and Figure 30.2.2 below traces how its priority key is computed. All three return the same path on this open grid; they differ only in cells examined, and therefore in planning time. The frontier, where the frontier is the set of discovered but not-yet-expanded nodes waiting in the priority queue, is what each algorithm's ordering policy actually controls.

Formal Model

The hand-traced expansion above followed one rule at every step, expand the node with the smallest f, and that single rule is what the formal model now makes precise.

Most navigation methods can be read as constrained search or optimization:

$$ f(n)=g(n)+h(n),\quad 0\le h(n)\le h^*(n) $$

The cost term is graph distance or risk-weighted distance; constraints are traversable cells, obstacle inflation (padding each obstacle's footprint outward on the map by roughly the robot's radius, so a path that is graph-valid is also collision-safe for a robot with physical extent), motion connectivity, and heuristic admissibility.

Worked Diagnostic

Code Fragment 1 isolates graph-search invariants: frontier update, predecessor map (where the predecessor map is the lookup table from each visited node back to the node it was reached from, used to reconstruct the final path once the goal is popped), path cost, and heuristic behavior. The point is to verify the path object before handing it to a controller.

# Compute A* priorities for three frontier nodes.
# The admissible heuristic focuses search without changing cost units.
frontier = [
    {"node": "A", "g": 4.0, "h": 6.0},
    {"node": "B", "g": 7.0, "h": 2.0},
    {"node": "C", "g": 5.0, "h": 5.0},
]
ranked = sorted((n["g"] + n["h"], n["node"]) for n in frontier)
print(ranked)
print(f"expand={ranked[0][1]}")

Expected output interpretation. The frontier printout shows that node B is expanded first because its combined score \(f=g+h\) is smallest, even though it is not the node with the smallest traveled cost alone. Nodes A and C tie at 10.0, which is a useful reminder that tie-breaking policy can still influence search order and runtime even when optimality is unchanged.

Tool Workflow

Once the frontier and heuristic invariants above behave correctly, the next question is which production tools already implement them, so you build only the test harness and not the planner.

Keep the tiny graph-search implementation as a test for admissibility, cost accumulation, and predecessor reconstruction. Use Nav2 global planners or OMPL when maps and robot constraints get large.

A path that is optimal on a graph but physically impossible for the robot is not a plan; it is a broken promise from the model to the machine.