A* Algorithm, in depth: designing the search space, with heading lattices, max-of-relaxation heuristics, hybrid A* keys and space-time planning

One search loop, any graph

The search loop does not change between problems. It takes a start state, a goal test, a successor function yielding (state, cost) pairs with non-negative costs, a heuristic and a key function. With an admissible, consistent heuristic and an exact key, the first time a goal state is popped its cost is optimal.

import heapq, math

def astar(start, is_goal, succ, h, key=lambda s: s):
    g, parent = {key(start): 0.0}, {key(start): None}
    heap, closed, expanded = [(h(start), 0.0, start)], set(), 0
    while heap:
        _, gs, s = heapq.heappop(heap)
        k = key(s)
        if k in closed:
            continue                      # stale entry (lazy deletion)
        if is_goal(s):
            path = [s]
            while parent[key(path[-1])] is not None:
                path.append(parent[key(path[-1])])
            return gs, path[::-1], expanded
        closed.add(k)
        expanded += 1
        for s2, cost in succ(s):
            k2, g2 = key(s2), gs + cost
            if k2 not in closed and g2 < g.get(k2, math.inf):
                g[k2], parent[k2] = g2, s
                heapq.heappush(heap, (g2 + h(s2), g2, s2))
    return None, None, expanded

Two lines carry the design: succ defines the graph and key defines what counts as the same node. Everything below is about choosing them.

What belongs in a state

The rule is the Markov property: a state must contain everything that affects which moves are legal from here on and what they cost. If two situations with the same position can have different futures, position alone is not a state. Typical additions:

Each addition multiplies the state space. A 64 by 64 map with 3,900 free cells and 8 headings has 31,200 states, and adding time makes it unbounded unless you cap the horizon. That growth is why the heuristic matters more here than on a plain grid.

Worked example: a heading lattice

The robot moves one cell per step in one of 8 headings. From heading h it can continue straight or turn by 45 degrees to h + 1 or h - 1 while moving; a turning step costs 1.5 times its length. Diagonal moves may not cut wall corners. The map is 64 by 64 with two long walls and a block in the middle; start (4, 4) facing east, goal (58, 58) facing north.

D = [(1, 0), (1, 1), (0, 1), (-1, 1), (-1, 0), (-1, -1), (0, -1), (1, -1)]

def lattice_succ(grid):
    def succ(s):
        x, y, hd = s
        for dh, mult in ((0, 1.0), (1, 1.5), (-1, 1.5)):
            h2 = (hd + dh) % 8
            dx, dy = D[h2]
            if free_step(grid, x, y, (dx, dy)):          # bounds, walls, no corner cutting
                yield (x + dx, y + dy, h2), math.hypot(dx, dy) * mult
    return succ

Each heuristic below is the exact cost of a relaxed problem, so each is admissible and consistent. Octile distance drops walls and heading. A 2D Dijkstra from the goal over the 8-connected grid with the same corner rule drops heading. A table computed once by reverse Dijkstra over the lattice with no obstacles drops walls; it depends only on the offset to the goal and the heading, so it can be reused for every query. The maximum of admissible, consistent heuristics is admissible and consistent too.

All exact heuristics find the same optimal cost. On this map the walls dominate, so the obstacle-aware 2D heuristic does most of the work, and the lattice table, which knows about turning, trims another 7 percent. Weighting by 1.5 cuts expansions thirtyfold for a path 2.7 percent longer, inside the guaranteed factor of 1.5.

Admissibility is easy to break when a heuristic is built by hand, so test it rather than argue it. Consistency is a local inequality, h(s) ≤ cost(s, s2) + h(s2) for every move, so sample a few thousand states, generate their successors and assert it; any violation is a bug. For the max heuristic above, all 89,136 lattice moves pass. Then run the exact search with h = 0 on a set of queries and require identical costs. Both checks run in seconds on maps like this one and belong in CI.

Coarse keys: the hybrid A* trade

Real vehicles have continuous poses, not 8 headings. Hybrid A*, described by Dolgov, Thrun, Montemerlo and Diebel for Stanford's entry in the DARPA Urban Challenge, keeps continuous (x, y, heading) states generated by driving short arcs, but closes them with a coarse key: the discretised cell. The first state expanded in a cell blocks every later state in it. It combines the same two relaxations as the table above, a non-holonomic heuristic without obstacles and a holonomic one with obstacles, and periodically tries an analytic curve straight to the goal.

The coarse key is a deliberate trade, and it gives up optimality and completeness. You can measure the effect on the lattice by keying on (x, y) only, ignoring heading: expansions fall from 5,262 to 1,986, but the path costs 162.746, 1.8 percent worse, because a cell first reached with the wrong heading is closed to the right one. On a map where the only route needs a cell to be crossed twice in different headings, the coarse search fails outright. Accept that only with a fallback, such as re-planning with a finer key.

Space-time A* for agents that share space

When other agents move, the state needs time. In prioritised planning, introduced as Cooperative A* by David Silver, agents plan one after another, and each finished path is written into a reservation table of (cell, time) entries plus the edges used at each time step, so a later agent can neither stand where another stands nor swap places with it. Waiting becomes a move.

MOVES = [(0, 0), (1, 0), (-1, 0), (0, 1), (0, -1)]       # (0, 0) is "wait"

def spacetime_succ(free, reserved_v, reserved_e, horizon):
    def succ(s):
        x, y, t = s
        if t >= horizon:
            return
        for dx, dy in MOVES:
            n = (x + dx, y + dy)
            if n not in free or (n, t + 1) in reserved_v:
                continue                                    # vertex conflict
            if (n, (x, y), t) in reserved_e:
                continue                                    # head-on swap
            yield (n[0], n[1], t + 1), 1.0
    return succ
# goal test: right cell AND no later reservation of it, so the agent can stay there

The corridor is the row y = 1 from x = 1 to x = 9, with a one-cell pocket at (7, 2). Agent 0 plans first from (1, 1) to (8, 1) and goes straight: cost 7, then it parks. Agent 1 goes from (9, 1) to (2, 1), head-on. With the key (x, y, t), A* expanded 27 states and found a cost-12 plan: it advances to (6, 1), waits once, backs into (7, 1), steps into the pocket at time 6 as agent 0 passes, and then runs the corridor. Unconstrained, the trip costs 7. Because each step costs 1 whether the agent moves or waits, several plans tie at 12 and the one returned looks odd; give waiting a slightly lower cost if you prefer plans that park instead of shuffle.

Now key the same search on (x, y), as a grid planner would. Waiting produces a state with an already-closed key, so it is discarded, and returning to a cell later is impossible. The search expanded 5 states and reported no path.

Prioritised planning is also incomplete, because it depends on the order. Let agent A go from (2, 1) to (3, 1) and agent B from (1, 1) to (8, 1). Planned first, A takes one step and parks in the corridor; B then expands 121 states up to the 60-step horizon and fails, because nothing can pass a parked agent. Reverse the order and B's plan costs 7, while A runs ahead of B into the pocket and back out to (3, 1) for a cost of 11. Production planners retry with other orders, or use conflict-based search, which branches on conflicts instead of fixing an order.

Failure modes

• Missing state component. Planning on (x, y) for a vehicle yields paths with turns it cannot make. Check every returned path against the real motion model.
• Inadmissible relaxation. A heuristic taken from a problem that is not truly easier, such as a 2D grid with a stricter corner rule than the lattice, or Manhattan distance where diagonal moves exist, overestimates and loses optimality without any error.
• Unbounded time. Space-time search without a horizon never terminates on an unsolvable query. Cap t, and size the cap from the longest reservation.
• Goal reached too early. Arriving at a goal cell that another agent passes through later produces a collision after arrival. Test for future reservations.
• Heuristic table on the wrong grid. A lattice table computed with different turn costs or headings is silently inadmissible. Rebuild it when motion parameters change.
• Memory blow-up. Exact keys over large state spaces fill memory. Measure expansions per query, and consider the memory-bounded variants or D* repair for repeated queries.

Trade-offs

Measure these on your own maps. The open list itself matters less than the state space: the priority queue deep dive covers heap choices once the expansion count is under control.

What to do next

1. Write down the state for your problem and justify each component with a move whose legality or cost depends on it.
2. Implement succ and key separately from the search loop, and test the successor function against the real motion model.
3. Build two relaxations that drop different constraints, combine them with max, and measure expansions for each alone and together, as in the table above.
4. If expansions are still too high, try a weighted heuristic and record the cost ratio; try a coarse key only with a fallback for failures.
5. For multiple agents, add time to the state, a horizon and a goal test that checks future reservations, then test a head-on corridor and a swapped priority order.
6. Keep a regression set of maps with known optimal costs and expansion counts in CI.