Grid-Based Planning

Take a map. Divide it into a grid. Mark each cell as "free" or "blocked." Now find the shortest path from start to goal.

This is grid-based planning, and it's the foundation of most 2D robot navigation. The algorithm at its core? A* (A-star) — a search algorithm so elegant it's taught in every computer science course.

Occupancy Grids

An occupancy grid is a 2D array representing the world. Each cell stores a probability: "How certain am I that this space is occupied?"

    0   1   2   3   4   5
  ┌───┬───┬───┬───┬───┬───┐
0 │ 0 │ 0 │100│100│100│ 0 │   0 = free
  ├───┼───┼───┼───┼───┼───┤  100 = occupied
1 │ 0 │ 0 │100│ 0 │ 0 │ 0 │   50 = unknown
  ├───┼───┼───┼───┼───┼───┤
2 │ S │ 0 │100│ 0 │ 0 │ G │   S = start, G = goal
  ├───┼───┼───┼───┼───┼───┤
3 │ 0 │ 0 │ 0 │ 0 │ 0 │ 0 │
  └───┴───┴───┴───┴───┴───┘

Each cell might be 5cm × 5cm (high resolution) or 10cm × 10cm (coarser, faster). The grid resolution is a key trade-off:

For a 100m × 100m warehouse at 5cm resolution, you need 2000 × 2000 = 4 million cells. At 1 byte per cell, that's 4MB — totally manageable.

A* Search: The Big Idea

A* is a best-first search algorithm. It explores the grid cell-by-cell, always picking the most promising cell next.

The key insight: For each cell, estimate the total cost to reach the goal through that cell.

f(cell) = g(cell) + h(cell)

where:
  g(cell) = cost to reach this cell from start (known)
  h(cell) = estimated cost from this cell to goal (heuristic)
  f(cell) = estimated total cost (start → cell → goal)

A* always expands the cell with the lowest f(cell). This guarantees it finds the shortest path — as long as the heuristic h(cell) is admissible (never overestimates the true cost).

The A* Algorithm (Step-by-Step)

Let's walk through A* on the grid above.

Setup

1. Open set: cells to explore, sorted by f(cell). Start with just the start cell.
2. Closed set: cells already explored.
3. g-scores: cost to reach each cell from start (initialized to ∞, except start = 0).
4. Came-from map: tracks the path (which cell did we come from?).

Loop

while open_set is not empty:
    current = cell in open_set with lowest f(current)

    if current == goal:
        return reconstruct_path(came_from, current)

    remove current from open_set
    add current to closed_set

    for neighbor in neighbors_of(current):
        if neighbor in closed_set:
            continue  # already explored

        tentative_g = g[current] + cost(current, neighbor)

        if tentative_g < g[neighbor]:
            # This path to neighbor is better than any previous one
            came_from[neighbor] = current
            g[neighbor] = tentative_g
            f[neighbor] = g[neighbor] + h(neighbor)

            if neighbor not in open_set:
                add neighbor to open_set

Example Trace

Starting from S at (0, 2), goal G at (5, 2):

Step 1: Expand S. Neighbors: (1,2), (0,1), (0,3). Add to open set.

Step 2: Expand (1,2). Neighbors: (1,1), (1,3), (0,2) already closed. Add (1,1), (1,3).

Step 3: Notice (2,2) is blocked (occupancy = 100). Skip it.

Step 4: Eventually reach (5,2) = G. Reconstruct path from came_from map.

Result: Path is S → (1,2) → (1,1) → (2,1) → (3,1) → (4,1) → (4,2) → (5,2) = G.

The path goes around the wall.

Choosing a Heuristic

The heuristic h(cell) estimates the cost from cell to the goal. Common choices:

Rule: The heuristic must be admissible (never overestimate) and consistent (h(A) ≤ cost(A→B) + h(B)). Euclidean distance is always safe because it's the straight-line distance — the absolute minimum.

Cost Functions

The cost cost(current, neighbor) is usually just the distance:

• Straight move (4-way grid): cost = 1.0
• Diagonal move (8-way grid): cost = √2 ≈ 1.414

But you can penalize certain cells:

• Near obstacles: cost = 1.5 (stay away from walls)
• Rough terrain: cost = 2.0 (prefer smooth paths)
• Restricted zones: cost = 10.0 (avoid unless necessary)

This lets you express preferences beyond "shortest distance."

Implementation Notes

What's Next?

A* works great for 2D maps, but what about high-dimensional spaces like a 6-axis robot arm? Next, we'll explore sampling-based planning — algorithms like RRT that can plan in spaces where grids fail.