A* Pathfinding Algorithm

How does Google Maps find the best route in under a second, across millions of roads? Stripping this question into its basic elements, the question becomes, how to find the shortest path in a graph from the start position to the end position. If one was to approach this problem by trying every path, the number of paths grow as $\mathcal{O}(b^d)$, where $b$ is the branching factor (average number of branches from each node) and $d$ is the depth. The brute force method scales extremely poorly so another solution must be used.

Dijkstra’s algorithm improves on the brute force method enormously: its running time grows polynomially with the size of the graph instead of exponentially. Dijkstra always expands the cheapest node so far. The problem with Dijkstra is that it expands the search in every direction including roads that lead away from the destination. Dijkstra can be beaten with algorithms with sense of direction.

The idea behind the A* algorithm

A* assigns every node a single score:

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

where

• $g(n)$: the exact cost from start to the current node $n$. This is known.
• $h(n)$: the estimated cost from the current node $n$ to the goal. This is a heuristic (educated guess).

After A* computes the score of each frontier node, the algorithm will always expand the node with the lowest $f(n)$. Minimizing both the cost of getting to the current node $g(n)$ and the estimate of arriving at the goal $h(n)$ makes the algorithm seek the shortest path from start point to the end point.

For a more thorough study of A*, please refer to [1].

Implementation

Heuristic

A common heuristic is the Manhattan distance:

$$h(n) = |x_1 - x_2| + |y_1 - y_2|$$

The geometric interpretation of this is amount of grid cells that need to be navigated to go from one point to another without moving diagonally.

def heuristic(a: tuple[int, int], b: tuple[int, int]) -> int:
    return abs(a[0] - b[0]) + abs(a[1] - b[1])

Main loop

The open set is a min-heap keyed on $f(n)$. At each iteration the node with the lowest $f$ is popped. Then check if the goal has been reached. Then the third step is to expand the neighbours:

import heapq

def astar(
    grid: list[list[int]],
    start: tuple[int, int],
    goal: tuple[int, int]
) -> dict[tuple[int, int], tuple[int, int]]:
    rows, cols = len(grid), len(grid[0])

    open_set: list[tuple[int, tuple[int, int]]] = []
    heapq.heappush(open_set, (0, start))

    came_from: dict[tuple[int, int], tuple[int, int]] = {}
    g_score: dict[tuple[int, int], int] = {start: 0}

    while open_set:
        _, current = heapq.heappop(open_set)

        if current == goal:
            break

        r, c = current
        for dr, dc in [(-1,0),(1,0),(0,-1),(0,1)]:
            neighbour: tuple[int, int] = (r + dr, c + dc)
            nr, nc = neighbour

            if not (0 <= nr < rows and 0 <= nc < cols):
                continue
            if grid[nr][nc] == 1:
                continue

            tentative_g = g_score[current] + 1

            if tentative_g < g_score.get(neighbour, float('inf')):
                came_from[neighbour] = current
                g_score[neighbour] = tentative_g
                f = tentative_g + heuristic(neighbour, goal)
                heapq.heappush(open_set, (f, neighbour))

    return came_from

Path reconstruction

def reconstruct_path(
    came_from: dict[tuple[int, int], tuple[int, int]],
    start: tuple[int, int],
    goal: tuple[int, int]
) -> list[tuple[int, int]]:
    path: list[tuple[int, int]] = []
    node = goal
    while node != start:
        path.append(node)
        node = came_from[node]
    path.append(start)
    path.reverse()
    return path

Running the algorithm on a 2D grid

grid = [
    [0, 0, 0, 1, 0],
    [1, 1, 0, 1, 0],
    [0, 0, 0, 0, 0],
    [0, 1, 1, 1, 0],
    [0, 0, 0, 0, 0],
]
start, goal = (0, 0), (4, 4)

came_from = astar(grid, start, goal)
path = reconstruct_path(came_from, start, goal)
print(path)

[(0, 0), (0, 1), (0, 2), (1, 2), (2, 2), (2, 3), (2, 4), (3, 4), (4, 4)]

Weighted A*

A useful, greedier A* variant is weighted A*. The variant inflates the heuristic by a weight $w > 1$:

$$f(n) = g(n) + w \cdot h(n)$$

The benefit of using the weight is that fewer nodes are expanded because the search prioritizes nodes that are close to the end. The trade-off is that the path found can cost up to $w$ times the optimal cost (when $h$ is admissible).

Demo

Start with Dijkstra and then swap to A* with the Manhattan heuristic. Also, test the weight slider to make the algorithm faster with the expense of accuracy.

• Blue-green cells: open set, discovered but not yet expanded.
• Orange cell: node currently being expanded.
• Dark green cells: closed set, fully expanded.
• Yellow path: the solution once the algorithm reaches the end node.
• Drag S or E to reposition start and goal. Paint walls by clicking and dragging on empty cells.

Discussion

A* works well in applications where a cost function and an admissible heuristic (a heuristic that never overestimates the distance to the end) can be defined. Examples of these types of application are GPS routing, NPC path finding in games and network routing.

One problem with A* is that it doesn’t factor walls into the heuristic when using Manhattan distance, so it can expand many nodes that lead into dead ends. A heuristic built from precomputed distances accounts for obstacles, at the cost of preprocessing. Jump Point Search (JPS) speeds up search differently: on uniform-cost grids it skips over symmetric paths, so far fewer nodes are expanded.

References