Graph search: A*, Dijkstra, D*-lite

A robot in a 2D world needs to plan a path through a known map. Discretize the map into a grid, weight each cell by traversal cost, and you have a graph. Cheapest path through the graph = best path through the world. Three algorithms cover almost every real use: A*, Dijkstra, D*-lite. Two of them you've heard of; one of them is what your robot is actually running.

The grid abstraction

Map the world to a grid: each cell is a node; adjacent cells are connected by edges. Edge cost = how expensive it is to traverse from cell to cell (distance, terrain difficulty, proximity to obstacles).

Dijkstra: the truth

Dijkstra's algorithm finds the cheapest path from a start to every node in a weighted graph (or stops when it reaches a specified goal). The trick: maintain a priority queue of frontier nodes, always expanding the one with the lowest cost-so-far. Once a node is "finalized," its cost is provably the cheapest possible.

def dijkstra(graph, start, goal):
    pq = [(0, start)]              # (cost, node)
    came_from = {start: None}
    cost_so_far = {start: 0}

    while pq:
        cost, node = heapq.heappop(pq)
        if node == goal:
            return reconstruct(came_from, goal)
        for neighbor, edge_cost in graph.neighbors(node):
            new_cost = cost + edge_cost
            if neighbor not in cost_so_far or new_cost < cost_so_far[neighbor]:
                cost_so_far[neighbor] = new_cost
                came_from[neighbor] = node
                heapq.heappush(pq, (new_cost, neighbor))
    return None  # unreachable

Strengths: optimal, simple, well-understood.

Weakness: explores in all directions equally, even ones that obviously won't help. Slow on large maps.

A*: Dijkstra with a hint

A* adds a heuristic: an estimate of the remaining cost from any node to the goal. The priority becomes cost_so_far + heuristic. The frontier expands toward the goal first, so the search runs many times faster.

def a_star(graph, start, goal, heuristic):
    pq = [(heuristic(start, goal), 0, start)]
    came_from = {start: None}
    cost_so_far = {start: 0}

    while pq:
        _, cost, node = heapq.heappop(pq)
        if node == goal:
            return reconstruct(came_from, goal)
        for neighbor, edge_cost in graph.neighbors(node):
            new_cost = cost + edge_cost
            if neighbor not in cost_so_far or new_cost < cost_so_far[neighbor]:
                cost_so_far[neighbor] = new_cost
                came_from[neighbor] = node
                priority = new_cost + heuristic(neighbor, goal)
                heapq.heappush(pq, (priority, new_cost, neighbor))
    return None

For a 2D grid, the standard heuristic is the Euclidean (or Manhattan) distance to the goal. As long as the heuristic is admissible (never overestimates the true remaining cost), A* finds the optimal path.

Strengths: optimal, much faster than Dijkstra in practice.

Weakness: still has to re-search from scratch if the map changes. That's where D*-lite comes in.

D*-lite: the algorithm your robot is actually running

Real robots don't have static maps. Doors open, people walk in, dynamic obstacles appear. Re-running A* on every change wastes work — most of the previous search is still valid.

D*-lite (Koenig & Likhachev, 2002) reuses prior search effort. When the map changes, it only re-expands the affected nodes; everything else stays cached. Repeated re-planning is dramatically faster.

The algorithm is more involved (you maintain a "rhs" value per node and propagate inconsistencies), but the API is the same as A*: give it a start and goal, it returns a path. Internally it remembers state across calls.

This is what runs on most production mobile robots. Nav2's default global planner is a D*-style replanner. ROS Industrial uses it. So does most of the autonomy on TurtleBot, Spot, and many indoor delivery robots.

Heuristics that matter

• Euclidean: \sqrt{(x_1 - x_2)^2 + (y_1 - y_2)^2}. Admissible for any motion model.
• Manhattan: |x_1 - x_2| + |y_1 - y_2|. Admissible for 4-connected grids; tighter than Euclidean.
• Octile: for 8-connected grids (allows diagonal moves). Tighter than both Manhattan and Euclidean for 8-conn.
• Time-distance: min remaining time at max velocity. Useful for time-optimal planning.

Tighter heuristic = faster search, as long as it stays admissible. Inadmissible heuristics can be faster still but lose the optimality guarantee.

Costs that matter

In production code, edge costs aren't just distance. Layered costmaps add:

• Obstacle inflation: cells near obstacles are expensive but still passable. Keeps the path from hugging walls.
• Lane preferences: prefer certain corridors for AGVs in warehouses.
• Time-of-day costs: avoid the cafeteria during lunch.
• Sensor uncertainty: cells in poorly-mapped regions are expensive.

Nav2 lets you stack costmap layers and configure them via YAML.

When grid search isn't enough

• High-DOF planning: a 6-DOF arm has C-space dimension 6. Grid search doesn't scale (curse of dimensionality). Use sampling-based planning (RRT, PRM) — next lesson.
• Kinematic constraints: a car can't pivot in place. A grid path may include 90° turns the car can't execute. Use lattice planners (Dubins, hybrid A*) — encodes feasible motion primitives at each node.
• Optimal smooth trajectories: grid paths are jagged. Post-smoothing or trajectory optimization on top.

Common implementation pitfalls

• Diagonal movement cost. Moving diagonally costs \sqrt 2, not 1. Get this wrong and your paths bias toward diagonals.
• Closed-set hash collisions. Hashing grid cells as x \cdot W + y is fast; using strings is 5× slower.
• Heuristic units don't match cost units. If costs are in meters, heuristic must be in meters. If they're in seconds, heuristic in seconds.
• Tie-breaking in the priority queue. When f-values tie, prefer higher g-values (closer to goal). Without this, search explores plateaus needlessly.

Exercise

Implement A* on a 100×100 grid with random obstacles. Solve from one corner to the other. Then change one obstacle and re-solve with A*. Now implement a basic D*-lite that handles the obstacle change incrementally. Compare wall-clock times. The speedup on small maps is modest; on large dynamic environments it's 10–100×.

Next

Sampling-based planning — PRM and RRT — for when grid search hits the curse of dimensionality.