The most intuitive approach to motion planning: discretize space into a grid, then search for a path.

Discretization

Divide C-space into a regular grid. Each cell is either free or occupied. Now motion planning is graph search: nodes are cells, edges connect adjacent cells.

For a 2D robot (ignoring orientation), you get a 2D grid. 4-connectivity (up/down/left/right) or 8-connectivity (add diagonals) determines which neighbors each cell connects to.

The resolution tradeoff is fundamental:

  • Coarse grid: fast to search, but might miss narrow passages
  • Fine grid: captures detail, but exponentially more cells to search

A* Algorithm

A* finds the shortest path on a graph. It maintains two values for each node:

  • $g(n)$: cost from start to node $n$
  • $h(n)$: estimated cost from $n$ to goal (the heuristic)

It always expands the node with lowest $f(n) = g(n) + h(n)$.

std::vector<Cell> astar(const Grid& grid, Cell start, Cell goal) {
    std::priority_queue<Node, std::vector<Node>, std::greater<Node>> open;
    std::unordered_set<Cell> closed;
    std::unordered_map<Cell, Cell> came_from;
    std::unordered_map<Cell, double> g_score;
    
    g_score[start] = 0;
    open.push({start, heuristic(start, goal)});
    
    while (!open.empty()) {
        Cell current = open.top().cell;
        open.pop();
        
        if (current == goal) {
            return reconstructPath(came_from, current);
        }
        
        if (closed.count(current)) continue;
        closed.insert(current);
        
        for (Cell neighbor : grid.neighbors(current)) {
            if (closed.count(neighbor) || !grid.isFree(neighbor)) {
                continue;
            }
            
            double tentative_g = g_score[current] + cost(current, neighbor);
            
            if (!g_score.count(neighbor) || tentative_g < g_score[neighbor]) {
                came_from[neighbor] = current;
                g_score[neighbor] = tentative_g;
                double f = tentative_g + heuristic(neighbor, goal);
                open.push({neighbor, f});
            }
        }
    }
    
    return {};  // No path found
}

Choosing a Heuristic

For grid search, common heuristics:

  • Euclidean distance: $h(n) = \sqrt{(x_n - x_g)^2 + (y_n - y_g)^2}$
  • Manhattan distance: $h(n) = |x_n - x_g| + |y_n - y_g|$ (for 4-connectivity)
  • Diagonal distance: for 8-connectivity

The heuristic must be admissible (never overestimate) to guarantee optimal paths. Euclidean distance is admissible because the straight line is always the shortest.

Limitations

Grid-based planning works well for low-dimensional problems. But the number of cells grows exponentially with dimensions. A 3D grid (x, y, θ) with 100 cells per dimension has $10^6$ cells. Add time for dynamic obstacles and you’re in trouble.

Next: sampling-based methods that escape this curse of dimensionality.