Path planning on a grid with A*

A mobile robot that knows where it is still has to decide how to get somewhere else without hitting anything. The simplest useful answer is to cut the floor into square cells, mark the cells that contain obstacles, and search for the cheapest chain of free cells from start to goal. This lesson is for readers who are comfortable with Python dictionaries and want to understand the search that sits underneath many mobile robot planners. You will write A* in about 40 lines of pure Python, using only heapq from the standard library, and measure how much work a good heuristic saves compared with Dijkstra’s algorithm.

Occupancy grids

An occupancy grid divides the plane into cells of a fixed size, chosen to suit the robot and its sensors, and stores for each cell whether it is free, occupied or unknown. Mapping systems build these grids from lidar scans and store a probability of occupancy per cell. For planning we threshold that probability into a yes or no. In this lesson the grid is a string, # for occupied and . for free, so you can see the map in the code.

Two details matter on a real robot. First, the robot is not a point. Planners grow every obstacle by the robot’s radius (inflation) so that a path through free cells keeps the robot’s body clear. Second, a grid path is a sequence of cell centres with sharp corners. A differential-drive robot can follow it by turning on the spot at each corner, but most systems smooth the path or hand it to a local controller that respects the robot’s dynamics.

Neighbours and step costs

The search needs to know which cells are reachable in one move from a given cell, and what the move costs.

With 4-connectivity, a diagonal displacement of 3 cells by 3 cells costs 6 moves. With 8-connectivity it costs 3 diagonal moves, or about 4.24, which is the straight-line distance. Eight neighbours give shorter, less jagged paths at the price of more neighbours to check per cell.

The rule about corners matters on a real robot. Without it, a diagonal move could pass between two occupied cells that touch at a corner, a gap the robot cannot fit through.

Dijkstra and A*

Dijkstra’s algorithm (1959) finds the cheapest path in a graph with non-negative edge costs. It keeps a priority queue of frontier cells ordered by g, the cost of the best known path from the start. It repeatedly removes the cheapest cell, marks it as finished (expanded), and offers each neighbour a path through it. The first time the goal comes off the queue, its cost is optimal. Dijkstra explores in rings of equal cost around the start, in every direction, whether or not that direction leads towards the goal.

A* (Hart, Nilsson and Raphael, 1968) orders the queue by f = g + h instead, where h is a heuristic estimate of the remaining cost from a cell to the goal. Cells that look closer to the goal come off the queue earlier, so the search leans towards the goal. With h equal to zero for every cell, A* is exactly Dijkstra.

A* still returns an optimal path if h is admissible: it never overestimates the true remaining cost. A stronger property, consistency, says h drops by at most the step cost along any move. A consistent heuristic means a cell, once expanded, never needs to be expanded again. All the heuristics below are consistent for the move sets we pair them with.

Heuristic	Formula for offsets dx, dy	Admissible for
Zero	0	Anything (this is Dijkstra)
Manhattan	dx + dy	4-connected grids only
Euclidean	square root of (dx² + dy²)	4- and 8-connected grids, and continuous space
Octile	max(dx, dy) + (√2 - 1) min(dx, dy)	4- and 8-connected grids

On an empty 4-connected grid Manhattan distance is the exact cost, and on an empty 8-connected grid octile distance is. A heuristic that equals the true cost would lead the search straight to the goal with almost no wasted expansions. Obstacles make the true cost larger than any of these, which is where the search does extra work.

A* in pure Python

The map is 11 by 24 cells, with a C-shaped wall that opens towards the start. A goal-seeking search heads into the cup first and has to back out, which makes the difference between heuristics easy to see. The planner uses heapq for the queue. Python’s heap cannot decrease a key, so instead of updating a queued cell we push a new copy with the lower cost and skip stale copies when they come off the heap. A counter breaks ties between equal f values so that the heap never has to compare two cells.

import heapq
import math

GRID_TEXT = """
........................
........................
......##########........
...............#........
...............#........
..S............#....G...
...............#........
...............#........
......##########........
........................
........................
""".strip().splitlines()

grid = [[c == "#" for c in row] for row in GRID_TEXT]
rows, cols = len(grid), len(grid[0])
start = next((r, c) for r, row in enumerate(GRID_TEXT) for c, ch in enumerate(row) if ch == "S")
goal = next((r, c) for r, row in enumerate(GRID_TEXT) for c, ch in enumerate(row) if ch == "G")

MOVES_4 = [(-1, 0, 1.0), (1, 0, 1.0), (0, -1, 1.0), (0, 1, 1.0)]

def manhattan(a, b):
    return abs(a[0] - b[0]) + abs(a[1] - b[1])

def euclidean(a, b):
    return math.hypot(a[0] - b[0], a[1] - b[1])

def octile(a, b):
    dx, dy = abs(a[0] - b[0]), abs(a[1] - b[1])
    return max(dx, dy) + (math.sqrt(2.0) - 1.0) * min(dx, dy)

def zero(a, b):
    return 0.0          # A* with h = 0 is Dijkstra's algorithm

def astar(start, goal, moves, h):
    """Return (path, cost, nodes expanded). Cells are (row, col); True in grid means occupied."""
    g = {start: 0.0}
    parent = {start: None}
    open_heap = [(h(start, goal), 0, start)]   # (f, tie-break counter, cell)
    closed = set()
    counter = 0
    while open_heap:
        _, _, cell = heapq.heappop(open_heap)
        if cell in closed:
            continue                 # stale heap entry, a cheaper copy was already expanded
        closed.add(cell)
        if cell == goal:
            path = []
            while cell is not None:
                path.append(cell)
                cell = parent[cell]
            return path[::-1], g[goal], len(closed)
        for dr, dc, step in moves:
            nr, nc = cell[0] + dr, cell[1] + dc
            if not (0 <= nr < rows and 0 <= nc < cols) or grid[nr][nc]:
                continue
            new_g = g[cell] + step
            if new_g < g.get((nr, nc), math.inf):
                g[(nr, nc)] = new_g
                parent[(nr, nc)] = cell
                counter += 1
                heapq.heappush(open_heap, (new_g + h((nr, nc), goal), counter, (nr, nc)))
    return None, math.inf, len(closed)

def show(path):
    canvas = [list(row) for row in GRID_TEXT]
    for r, c in path[1:-1]:
        canvas[r][c] = "*"
    print("\n".join("".join(row) for row in canvas))

free_cells = sum(not occ for row in grid for occ in row)
print(f"grid {rows} x {cols}, free cells {free_cells}, start {start}, goal {goal}")
for name, h in [("zero", zero), ("Euclidean", euclidean), ("octile", octile), ("Manhattan", manhattan)]:
    path, cost, expanded = astar(start, goal, MOVES_4, h)
    print(f"4-connected, h = {name:9s}  cost {cost:5.1f}  path cells {len(path):3d}  expanded {expanded:3d}")
show(astar(start, goal, MOVES_4, manhattan)[0])

grid 11 x 24, free cells 239, start (5, 2), goal (5, 20)
4-connected, h = zero       cost  26.0  path cells  27  expanded 223
4-connected, h = Euclidean  cost  26.0  path cells  27  expanded 196
4-connected, h = octile     cost  26.0  path cells  27  expanded 190
4-connected, h = Manhattan  cost  26.0  path cells  27  expanded 158
........................
.....************.......
.....*##########*.......
.....*.........#*.......
.....*.........#*.......
..S***.........#****G...
...............#........
...............#........
......##########........
........................
........................

Every heuristic finds a path of cost 26, as it must, because all four are admissible on a 4-connected grid. They differ in effort. Dijkstra expands 223 of the 239 free cells: nearly the whole map, to find a path that uses 27 of them. Manhattan distance cuts that to 158. Euclidean and octile land in between, because on a 4-connected grid they underestimate more than Manhattan does. The tighter an admissible heuristic, the fewer cells A* expands.

The path itself goes round the top of the wall. There are many paths of cost 26 on this map (any staircase with the same number of steps), and which one A* returns depends on the tie-breaking order. That is a property of grid search, not a bug.

Eight neighbours, four heuristics

The second block allows diagonal moves, refuses diagonal moves that cut a corner, and runs all four heuristics again. It then runs a second query with octile and Manhattan only.

import heapq
import math

GRID_TEXT = """
........................
........................
......##########........
...............#........
...............#........
..S............#....G...
...............#........
...............#........
......##########........
........................
........................
""".strip().splitlines()
grid = [[c == "#" for c in row] for row in GRID_TEXT]
rows, cols = len(grid), len(grid[0])
start, goal = (5, 2), (5, 20)
SQRT2 = math.sqrt(2.0)
MOVES_8 = [(-1, 0, 1.0), (1, 0, 1.0), (0, -1, 1.0), (0, 1, 1.0),
           (-1, -1, SQRT2), (-1, 1, SQRT2), (1, -1, SQRT2), (1, 1, SQRT2)]

def free(r, c):
    return 0 <= r < rows and 0 <= c < cols and not grid[r][c]

def neighbours(cell):
    r, c = cell
    for dr, dc, step in MOVES_8:
        if not free(r + dr, c + dc):
            continue
        if dr and dc and not (free(r + dr, c) and free(r, c + dc)):
            continue                     # no squeezing diagonally past a corner
        yield (r + dr, c + dc), step

def zero(a, b):
    return 0.0

def manhattan(a, b):
    return abs(a[0] - b[0]) + abs(a[1] - b[1])

def euclidean(a, b):
    return math.hypot(a[0] - b[0], a[1] - b[1])

def octile(a, b):
    dx, dy = abs(a[0] - b[0]), abs(a[1] - b[1])
    return max(dx, dy) + (SQRT2 - 1.0) * min(dx, dy)

def astar(start, goal, h):
    g, parent, closed, counter = {start: 0.0}, {start: None}, set(), 0
    open_heap = [(h(start, goal), 0, start)]
    while open_heap:
        _, _, cell = heapq.heappop(open_heap)
        if cell in closed:
            continue
        closed.add(cell)
        if cell == goal:
            path = []
            while cell is not None:
                path.append(cell)
                cell = parent[cell]
            return path[::-1], g[goal], len(closed)
        for nxt, step in neighbours(cell):
            new_g = g[cell] + step
            if new_g < g.get(nxt, math.inf):
                g[nxt], parent[nxt] = new_g, cell
                counter += 1
                heapq.heappush(open_heap, (new_g + h(nxt, goal), counter, nxt))
    return None, math.inf, len(closed)

results = {}
for name, h in [("zero", zero), ("Euclidean", euclidean), ("octile", octile), ("Manhattan", manhattan)]:
    path, cost, expanded = astar(start, goal, h)
    results[name] = path
    print(f"S to G, h = {name:9s}  cost {cost:6.3f}  expanded {expanded:3d}")

canvas = [list(row) for row in GRID_TEXT]
for r, c in results["octile"][1:-1]:
    canvas[r][c] = "*"
print("\n".join("".join(row) for row in canvas))

# A second query, from the top-left area to below the cup, shows Manhattan's overestimate biting.
for name, h in [("octile", octile), ("Manhattan", manhattan)]:
    path, cost, expanded = astar((1, 4), (9, 19), h)
    print(f"(1, 4) to (9, 19), h = {name:9s}  cost {cost:6.3f}  expanded {expanded:3d}")

S to G, h = zero       cost 21.899  expanded 212
S to G, h = Euclidean  cost 21.899  expanded 123
S to G, h = octile     cost 21.899  expanded 105
S to G, h = Manhattan  cost 21.899  expanded 108
........................
.....************.......
....*.##########.*......
...*...........#..*.....
...*...........#...*....
..S............#....G...
...............#........
...............#........
......##########........
........................
........................
(1, 4) to (9, 19), h = octile     cost 21.243  expanded 106
(1, 4) to (9, 19), h = Manhattan  cost 22.414  expanded  68

The diagonal moves shorten the path from 26 to 21.9 cells and the path now hugs the wall corners. Octile, the tightest admissible heuristic for this move set, expands the fewest cells (105). Euclidean is admissible but looser, at 123. Dijkstra still expands most of the map.

Manhattan is the interesting row. On an 8-connected grid it can overestimate: a single diagonal step costs 1.41, but Manhattan says 2. It is no longer admissible, so A* loses its guarantee. On the first query it happened to return the optimal cost with 108 expansions. On the second query, from cell (1, 4) to cell (9, 19), it expanded 68 cells instead of 106 and returned a path of cost 22.414 against the optimum of 21.243, about 5.5 % longer. An inadmissible heuristic trades path quality for speed. Some planners do this on purpose (weighted A* multiplies an admissible h by a factor above 1). Doing it by accident, by pairing the wrong heuristic with the move set, is a common bug.

What changes on a real robot

Production navigation stacks build on the same search but add several layers. The grid comes from a map built by SLAM and is updated with live sensor data. Obstacles are inflated by the robot’s radius plus a margin, and cells near obstacles get a higher cost rather than a hard ban, so paths keep a comfortable distance from walls. The cost of a cell can also encode terrain, slopes or keep-out zones.

Grid search has limits. The number of cells grows with the square of the map size at a fixed resolution, and with the power of the dimension for higher-dimensional problems. A 6-joint arm’s configuration space cannot be gridded at any useful resolution. Sampling-based planners such as RRT and PRM take over there, and the motion planning hub introduces them. Grid paths also ignore the robot’s motion constraints: a car-like robot cannot turn on the spot, so its planners search over poses that include the heading rather than over cells alone.

Exercise

1. Add an inflation step: before planning, mark every free cell within one cell (8-connected) of an obstacle as occupied. Re-run the 8-connected octile search and report the new cost and number of expansions. Why did the path move?
2. Make the planner prefer to stay away from walls without forbidding it: give each free cell next to an obstacle an extra cost of 2 when entering it. Which heuristics stay admissible?
3. Replace h with 1.5 * octile (weighted A*) on the second query and report cost and expansions. Is the cost within a factor of 1.5 of the optimum?

Summary

• An occupancy grid turns a map into a graph: free cells are nodes and allowed moves are edges with costs of 1 or √2.
• A* orders the search by g + h. With h equal to zero it is Dijkstra’s algorithm. With an admissible h it still returns an optimal path and expands fewer cells.
• Match the heuristic to the moves: Manhattan for 4-connected, octile for 8-connected. Euclidean is admissible for both but looser.
• On our map Dijkstra expanded 212 to 223 cells, and A* with the tightest admissible heuristic expanded 158 (4-connected) and 105 (8-connected).
• A heuristic that overestimates (Manhattan with diagonal moves) can return a longer path, here by 5.5 %.

Further reading

• Planning Algorithms, Steven M. LaValle, Cambridge University Press, 2006: chapter 2, “Discrete Planning”, covers forward search, Dijkstra and A* on graphs. The whole book is free online.
• A Formal Basis for the Heuristic Determination of Minimum Cost Paths, Hart, Nilsson and Raphael, IEEE Transactions on Systems Science and Cybernetics, 1968: the paper that introduced A* and proved that it finds a cheapest path when the heuristic never overestimates.
• A note on two problems in connexion with graphs, E. W. Dijkstra, Numerische Mathematik, 1959: the original shortest-path algorithm, in three pages.
• Modern Robotics, Lynch and Park, Cambridge University Press, 2017 (updated first edition, 2019 preprint): chapter 10, Motion Planning, is the place to go after grid search, for planning in the configuration space of arms as well as mobile robots.