Grid Pathfinding → GPS Navigation

Why Heuristic Search?

When you ask a maps app for the fastest route, it can’t try every possible path—there are too many. A* (pronounced “A-star”) narrows the search by combining what you’ve already spent (distance so far) with a smart guess of what’s left. It’s fast, optimal (with the right heuristic), and widely used in games, robotics, and GPS navigation.

You’ll build two working demos: (1) a grid pathfinder with obstacles; (2) a mini GPS-like route finder on a small road network.
You’ll learn how admissible/consistent heuristics guide A* to the goal without missing the optimal route, how to structure open/closed sets, and how to tune tie-breaking.

Theory Deep Dive

1. A* in one line

A* evaluates nodes using a combined score:
$f(n) = g(n) + h(n)$
where $g(n)$ is the cost from the start to $n$ and $h(n)$ is a heuristic estimate from $n$ to the goal. The node with the smallest $f$ is expanded next.

2. Relationship to Dijkstra and Greedy Best-First Search

Dijkstra’s Algorithm: set $h(n) = 0$ for all nodes ⇒ purely cost-so-far; optimal but can explore widely.

Greedy Best-First Search: set $g(n) = 0$ ⇒ purely heuristic; fast but not guaranteed optimal.

A* blends both: balances progress-so-far and goal-directed guidance.

3. Admissibility and Optimality

A heuristic $h$ is admissible if it never overestimates the true remaining cost $latex h^(n) $ : $latex \forall n,; h(n) \le h^(n). $
With an admissible heuristic on graphs with nonnegative edge costs, A* is optimal: the first time the goal is popped from the open set, the path found has minimal cost.

4. Consistency (Monotonicity)

A heuristic is consistent if for every edge $(n, n')$ with cost $c(n,n')$ :
$h(n) \le c(n,n') + h(n').$
Equivalently, the $f$ -values along any path are nondecreasing. Consistency implies admissibility and simplifies implementation: once a node is closed, its best path is final.

5. Heuristics you’ll use

Manhattan distance (4-connected grid):
$h(n) = |x_n - x_g| + |y_n - y_g|.$
Euclidean distance (continuous/8-connected):
$h(n) = \sqrt{(x_n - x_g)^2 + (y_n - y_g)^2}.$
Octile distance (8-connected with diagonal cost $\sqrt{2}$ ):
$h(n) = (\sqrt{2}-1)\cdot \min(dx,dy) + \max(dx,dy)$ where $dx = |x_n-x_g|,\ dy = |y_n-y_g|.$
Great-circle (Haversine) for geo-coordinates.

Choose the heuristic to be admissible and as tight as possible—tighter heuristics explore fewer nodes.

6. Complexity and effective branching factor

Worst-case time and space can be exponential in depth $d$ . A classic diagnostic is the effective branching factor $latex b^* $, which solves: $latex N + 1 = 1 + b^ + (b^)^2 + \cdots + (b^)^d, $
where $N$ is the total expanded nodes and $d$ is solution depth. Better heuristics lower $b^*$ .

7. Practical engineering notes

Open set: a min-priority queue keyed by $f(n)$ ; implement with a binary heap.

Closed set: nodes already expanded (final $g$ known if heuristic is consistent).

Tie-breaking: when $f$ ties, prefer larger $g$ (deeper) or smaller $h$ to bias toward goal-directed, smooth paths.

Weighted A*: $f(n) = g(n) + w,h(n)$ with $w>1$ yields faster, near-optimal routes (sacrifices optimality; often used in time-critical systems).

Variants: IDA* (memory-lean), D* Lite (dynamic environments), Theta* (any-angle), Jump Point Search (grid acceleration).

Toy Problem – Grid Pathfinding

We’ll solve shortest paths on a 2D grid with obstacles. Movements allowed: 8-directional (N, NE, E, SE, S, SW, W, NW). Costs: 1 for orthogonal, $\sqrt{2}$ for diagonals.

Dataset Snapshot (Representation)

Represent the grid as a matrix: 0 = free, 1 = obstacle. Example (10×10):

S 0 0 0 0 0 0 0 1 0
0 1 1 0 0 0 0 0 1 0
0 0 0 0 1 0 1 0 0 0
0 1 0 0 1 0 0 0 1 0
0 1 0 0 0 0 1 0 0 0
0 0 0 1 1 0 0 0 0 0
0 1 0 0 0 0 1 1 1 0
0 0 0 0 1 0 0 0 0 0
1 1 0 0 0 1 0 1 0 0
0 0 0 1 0 0 0 0 0 G

Here S is start, G is goal.

Step 1: Imports and helpers

Minimal standard-library imports; we’ll also define a small tie-break counter.

import heapq
import math
from itertools import count

We’ll use heapq for the priority queue, math for square roots, and count() to break ties in the heap.

Step 2: Grid, start/goal, and neighbors

Define the grid and how to enumerate valid moves.

# 0 = free, 1 = obstacle
GRID = [
    [0,0,0,0,0,0,0,0,1,0],
    [0,1,1,0,0,0,0,0,1,0],
    [0,0,0,0,1,0,1,0,0,0],
    [0,1,0,0,1,0,0,0,1,0],
    [0,1,0,0,0,0,1,0,0,0],
    [0,0,0,1,1,0,0,0,0,0],
    [0,1,0,0,0,0,1,1,1,0],
    [0,0,0,0,1,0,0,0,0,0],
    [1,1,0,0,0,1,0,1,0,0],
    [0,0,0,1,0,0,0,0,0,0],
]
START = (0, 0)
GOAL = (9, 9)

# 8-connected moves: (dx, dy, cost)
MOVES = [
    (-1, 0, 1.0), (1, 0, 1.0), (0, -1, 1.0), (0, 1, 1.0),
    (-1, -1, math.sqrt(2)), (-1, 1, math.sqrt(2)),
    ( 1, -1, math.sqrt(2)), ( 1, 1, math.sqrt(2)),
]

H, W = len(GRID), len(GRID[0])

def in_bounds(x, y):
    return 0 <= x < H and 0 <= y < W

def passable(x, y):
    return GRID[x][y] == 0

def neighbors(x, y):
    for dx, dy, cost in MOVES:
        nx, ny = x + dx, y + dy
        if in_bounds(nx, ny) and passable(nx, ny):
            yield (nx, ny), cost

Defines the map, start/goal, and a generator to iterate over valid neighbors with movement costs.

Step 3: Heuristic (Octile distance)

Good for 8-connected grids.

def octile(a, b):
    (x1, y1), (x2, y2) = a, b
    dx, dy = abs(x1 - x2), abs(y1 - y2)
    return (math.sqrt(2) - 1) * min(dx, dy) + max(dx, dy)

Admissible and typically tighter than Manhattan in 8-connected grids.

4. A core*

Open set (min-heap), g/f scores, came_from for path reconstruction, and a tie-breaker.

def astar(start, goal, hfn):
    open_heap = []
    tie = count()

    g = {start: 0.0}
    f = {start: hfn(start, goal)}
    came_from = {}

    heapq.heappush(open_heap, (f[start], next(tie), start))
    open_set = {start}
    closed_set = set()

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

    if current == goal:
        # reconstruct
        path = [current]
        while current in came_from:
            current = came_from[current]
            path.append(current)
        path.reverse()
        return path, g[path[-1]]

    closed_set.add(current)
    cx, cy = current

    for (nx, ny), step_cost in neighbors(cx, cy):
        neigh = (nx, ny)
        if neigh in closed_set:
            continue

        tentative_g = g[current] + step_cost
        if tentative_g < g.get(neigh, float('inf')):
            came_from[neigh] = current
            g[neigh] = tentative_g
            f[neigh] = tentative_g + hfn(neigh, goal)
            if neigh not in open_set:
                heapq.heappush(open_heap, (f[neigh], next(tie), neigh))
                open_set.add(neigh)

    return None, float('inf')

Classic A loop using a heap. Consistent heuristics guarantee that once a node is closed, its best path is fixed.*

5. Run and pretty-print

Overlay the path on the grid for clarity.

path, cost = astar(START, GOAL, octile)

# Render
grid_chars = [['#' if c==1 else '.' for c in row] for row in GRID]
if path:
    for (x, y) in path:
        if (x, y) not in (START, GOAL):
            grid_chars[x][y] = '*'
    sx, sy = START; gx, gy = GOAL
    grid_chars[sx][sy] = 'S'; grid_chars[gx][gy] = 'G'


for row in grid_chars:
    print(' '.join(row))
print(f"Path length (cost): {cost:.3f}")

Shows start S, goal G, obstacles #, free cells ., and the path *, with the precise path cost.

Quick Reference: Full Code

import heapq, math
from itertools import count

GRID = [
    [0,0,0,0,0,0,0,0,1,0],
    [0,1,1,0,0,0,0,0,1,0],
    [0,0,0,0,1,0,1,0,0,0],
    [0,1,0,0,1,0,0,0,1,0],
    [0,1,0,0,0,0,1,0,0,0],
    [0,0,0,1,1,0,0,0,0,0],
    [0,1,0,0,0,0,1,1,1,0],
    [0,0,0,0,1,0,0,0,0,0],
    [1,1,0,0,0,1,0,1,0,0],
    [0,0,0,1,0,0,0,0,0,0],
]
START, GOAL = (0,0), (9,9)
MOVES = [(-1,0,1.0),(1,0,1.0),(0,-1,1.0),(0,1,1.0),
         (-1,-1,math.sqrt(2)),(-1,1,math.sqrt(2)),(1,-1,math.sqrt(2)),(1,1,math.sqrt(2))]
H, W = len(GRID), len(GRID[0])

def in_bounds(x,y): return 0<=x<H and 0<=y<W

def passable(x,y): return GRID[x][y]==0

def neighbors(x,y):
    for dx,dy,c in MOVES:
        nx, ny = x+dx, y+dy
        if in_bounds(nx,ny) and passable(nx,ny):
            yield (nx,ny), c

def octile(a,b):
    (x1,y1),(x2,y2) = a,b
    dx,dy = abs(x1-x2), abs(y1-y2)
    return (math.sqrt(2)-1)*min(dx,dy) + max(dx,dy)

def astar(start, goal, hfn):
    open_heap, tie = [], count()
    g, f, came = {start:0.0}, {start:hfn(start,goal)}, {}
    heapq.heappush(open_heap,(f[start], next(tie), start))
    open_set, closed = {start}, set()
    while open_heap:
        _,_,cur = heapq.heappop(open_heap)
        open_set.discard(cur)
        if cur==goal:
            path=[cur]
            while cur in came:
                cur=came[cur]; path.append(cur)
            return list(reversed(path)), g[goal]
        closed.add(cur)
        cx,cy = cur
        for (nx,ny), c in neighbors(cx,cy):
            if (nx,ny) in closed: continue
            tg = g[cur] + c
            if tg < g.get((nx,ny), float('inf')):
                came[(nx,ny)] = cur
                g[(nx,ny)] = tg
                f[(nx,ny)] = tg + hfn((nx,ny), goal)
                if (nx,ny) not in open_set:
                    heapq.heappush(open_heap,(f[(nx,ny)], next(tie), (nx,ny)))
                    open_set.add((nx,ny))
    return None, float('inf')

path, cost = astar(START, GOAL, octile)
chars = [['#' if c==1 else '.' for c in row] for row in GRID]
if path:
    for x,y in path:
        if (x,y) not in (START,GOAL): chars[x][y]='*'
    sx,sy=START; gx,gy=GOAL
    chars[sx][sy]='S'; chars[gx][gy]='G'
for row in chars: print(' '.join(row))
print(f"Path length (cost): {cost:.3f}")

Real‑World Application — Mini GPS on Small Road Graph

We’ll simulate a tiny road network using latitude/longitude coordinates. The heuristic will be great-circle distance (Haversine), which is admissible because roads cannot be straighter than the Earth’s surface line between two points.

Data Snapshot

A small directed graph with nodes (id → lat, lon) and weighted edges (approx road lengths in meters). Example (rough, illustrative coordinates around a city center):

Nodes (id: (lat, lon))
A: (14.5995, 120.9842)
B: (14.6030, 120.9880)
C: (14.6048, 120.9820)
D: (14.6065, 120.9900)
E: (14.6100, 120.9860)


Edges (u -> v : distance_m)
A->B: 550 A->C: 700
B->D: 450 B->E: 800
C->E: 600
D->E: 400 C->B: 650

Step 1: Imports and geo helpers

import heapq, math
from itertools import count

# Haversine distance in meters
R = 6371000.0  # Earth radius (m)

def haversine(p, q):
    lat1, lon1 = map(math.radians, p)
    lat2, lon2 = map(math.radians, q)
    dlat = lat2 - lat1
    dlon = lon2 - lon1
    a = math.sin(dlat/2)**2 + math.cos(lat1)*math.cos(lat2)*math.sin(dlon/2)**2
    c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
    return R * c

Great-circle distance never overestimates true road distance ⇒ admissible heuristic.

Step 2: Graph definition

NODES = {
    'A': (14.5995, 120.9842),
    'B': (14.6030, 120.9880),
    'C': (14.6048, 120.9820),
    'D': (14.6065, 120.9900),
    'E': (14.6100, 120.9860),
}

GRAPH = {
    'A': {'B': 550, 'C': 700},
    'B': {'D': 450, 'E': 800},
    'C': {'E': 600, 'B': 650},
    'D': {'E': 400},
    'E': {},
}

Nodes include approximate lat/lon; edges store road distances in meters (directed for illustration).

Step 3: A* on a general graph with Haversine heuristic

def astar_graph(start, goal):
    h = lambda n: haversine(NODES[n], NODES[goal])

    open_heap, tie = [], count()
    g, f, came = {start: 0.0}, {start: h(start)}, {}
    heapq.heappush(open_heap, (f[start], next(tie), start))
    open_set, closed = {start}, set()

    while open_heap:
        _, _, cur = heapq.heappop(open_heap)
        open_set.discard(cur)
        if cur == goal:
            path = [cur]
            while cur in came:
                cur = came[cur]
                path.append(cur)
            path.reverse()
            return path, g[goal]
        closed.add(cur)
        for nbr, w in GRAPH[cur].items():
            if nbr in closed:
                continue
            tg = g[cur] + w
            if tg < g.get(nbr, float('inf')):
                came[nbr] = cur
                g[nbr] = tg
                f[nbr] = tg + h(nbr)
                if nbr not in open_set:
                    heapq.heappush(open_heap, (f[nbr], next(tie), nbr))
                    open_set.add(nbr)
    return None, float('inf')

Same A* structure; cost is edge length, heuristic is Haversine to the target node.*

Step 4: Run and print route

route, meters = astar_graph('A', 'E')
print(' → '.join(route))
print(f"Total length: {meters:.1f} m")

Outputs the best route and its computed length.

Quick Reference: Full GPS-like Code

import heapq, math
from itertools import count

R = 6371000.0

def haversine(p, q):
    lat1, lon1 = map(math.radians, p)
    lat2, lon2 = map(math.radians, q)
    dlat, dlon = lat2-lat1, lon2-lon1
    a = math.sin(dlat/2)**2 + math.cos(lat1)*math.cos(lat2)*math.sin(dlon/2)**2
    return R * 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))

NODES = {
    'A': (14.5995, 120.9842),
    'B': (14.6030, 120.9880),
    'C': (14.6048, 120.9820),
    'D': (14.6065, 120.9900),
    'E': (14.6100, 120.9860),
}
GRAPH = {
    'A': {'B': 550, 'C': 700},
    'B': {'D': 450, 'E': 800},
    'C': {'E': 600, 'B': 650},
    'D': {'E': 400},
    'E': {},
}

def astar_graph(start, goal):
    h = lambda n: haversine(NODES[n], NODES[goal])
    open_heap, tie = [], count()
    g, f, came = {start:0.0}, {start:h(start)}, {}
    heapq.heappush(open_heap,(f[start], next(tie), start))
    open_set, closed = {start}, set()
    while open_heap:
        _,_,cur = heapq.heappop(open_heap)
        open_set.discard(cur)
        if cur==goal:
            path=[cur]
            while cur in came: cur=came[cur]; path.append(cur)
            return list(reversed(path)), g[goal]
        closed.add(cur)
        for nbr, w in GRAPH[cur].items():
            if nbr in closed: continue
            tg = g[cur] + w
            if tg < g.get(nbr, float('inf')):
                came[nbr] = cur
                g[nbr] = tg
                f[nbr] = tg + h(nbr)
                if nbr not in open_set:
                    heapq.heappush(open_heap,(f[nbr], next(tie), nbr))
                    open_set.add(nbr)
    return None, float('inf')

route, meters = astar_graph('A','E')
print(' → '.join(route))
print(f"Total length: {meters:.1f} m")

Strengths & Limitations

Strengths

Optimal with admissible, consistent heuristics and nonnegative edge costs.
Fast in practice: prunes exploration with a good heuristic; widely used in real-time systems.
General: works on grids, road networks, game maps, robot occupancy grids.

Limitations

Memory-hungry: maintains open/closed sets; can be large on big maps.
Heuristic design required: poor heuristics degrade into Dijkstra-like behavior.
Dynamic changes: vanilla A* recomputes from scratch when the map changes (use D* Lite or similar for dynamic environments).

Final Notes

You implemented A* twice: first on a grid using an octile heuristic, then on a geo-graph using the Haversine heuristic.

You learned how admissibility and consistency guarantee optimality and how to engineer performant A* with heaps, tie-breaking, and clear data structures. These patterns transfer directly to robotics navigation, games, and routing services.

Next Steps for You:

Acceleration techniques: Try Jump Point Search (for uniform-cost grids) and bidirectional A* to reduce expansions.

Dynamic replanning: Explore D*, D Lite*, or LPA* for environments where edges change or costs update online.

Any-angle paths: Implement Theta* to remove grid-induced zigzags.

Scale up with real data: Load a neighborhood from OpenStreetMap (e.g., via osmnx), run A*, and compare routes to your maps app.

References

[1] P. E. Hart, N. J. Nilsson, and B. Raphael, “A Formal Basis for the Heuristic Determination of Minimum Cost Paths,” IEEE Trans. Systems Science and Cybernetics, vol. 4, no. 2, pp. 100–107, 1968.

[2] E. W. Dijkstra, “A Note on Two Problems in Connexion with Graphs,” Numerische Mathematik, vol. 1, pp. 269–271, 1959.

[3] J. Pearl, Heuristics: Intelligent Search Strategies for Computer Problem Solving. Addison-Wesley, 1984.

[4] S. J. Russell and P. Norvig, Artificial Intelligence: A Modern Approach, 3rd ed. Prentice Hall, 2010.

[5] R. E. Korf, “Depth-First Iterative-Deepening: An Optimal Admissible Tree Search,” Artificial Intelligence, vol. 27, no. 1, pp. 97–109, 1985. (See also IDA* variants.)

[6] S. Koenig and M. Likhachev, “D* Lite,” in Proceedings of AAAI, 2002, pp. 476–483.

[7] A. Nash, K. Daniel, S. Koenig, and A. Felner, “Theta*: Any-Angle Path Planning on Grids,” in AAAI, 2007, pp. 1177–1183.

[8] D. Harabor and A. Grastien, “Online Graph Pruning for Pathfinding on Grid Maps,” in AAAI, 2011. (Jump Point Search.)

[9] I. Pohl, “Heuristic Search Viewed as Path Finding in a Graph,” Artificial Intelligence, vol. 1, no. 3–4, pp. 193–204, 1970. (Weighted A*.)

Heuristic Search (A*): Shortest Paths with Smarts

Why Heuristic Search?

Theory Deep Dive

1. A* in one line

2. Relationship to Dijkstra and Greedy Best-First Search

3. Admissibility and Optimality

4. Consistency (Monotonicity)

5. Heuristics you’ll use

6. Complexity and effective branching factor

7. Practical engineering notes

Toy Problem – Grid Pathfinding

Dataset Snapshot (Representation)

Step 1: Imports and helpers

Step 2: Grid, start/goal, and neighbors

Step 3: Heuristic (Octile distance)

4. A core*

5. Run and pretty-print

Quick Reference: Full Code

Real‑World Application — Mini GPS on Small Road Graph

Data Snapshot

Step 1: Imports and geo helpers

Step 2: Graph definition

Step 3: A* on a general graph with Haversine heuristic

Step 4: Run and print route

Quick Reference: Full GPS-like Code

Strengths & Limitations

Strengths

Limitations

Final Notes

References

Leave a comment Cancel reply

Heuristic Search (A*): Shortest Paths with Smarts

Why Heuristic Search?

Theory Deep Dive

1. A* in one line

2. Relationship to Dijkstra and Greedy Best-First Search

3. Admissibility and Optimality

4. Consistency (Monotonicity)

5. Heuristics you’ll use

6. Complexity and effective branching factor

7. Practical engineering notes

Toy Problem – Grid Pathfinding

Dataset Snapshot (Representation)

Step 1: Imports and helpers

Step 2: Grid, start/goal, and neighbors

Step 3: Heuristic (Octile distance)

4. A core*

5. Run and pretty-print

Quick Reference: Full Code

Real‑World Application — Mini GPS on Small Road Graph

Data Snapshot

Step 1: Imports and geo helpers

Step 2: Graph definition

Step 3: A* on a general graph with Haversine heuristic

Step 4: Run and print route

Quick Reference: Full GPS-like Code

Strengths & Limitations

Strengths

Limitations

Final Notes

References

Share this:

Leave a comment Cancel reply