A* Path Planning: Guided Search on a Grid

February 15, 2024 · motion-planning

Imagine you are in a city and want to drive from your house to a coffee shop. You could explore every possible street — that is Dijkstra’s algorithm — but it is wasteful. Instead, you naturally favor roads that are heading towards the coffee shop. That is exactly what A* does: it is a guided search.

Full implementation: astarV3.cpp

The Core Idea

A* finds the shortest path by balancing two questions at every step:

• “How far have I already travelled?” — don’t ignore the cost already paid
• “How far do I still have to go?” — use a heuristic to prioritize promising directions

For every cell being considered, A* computes:

where $g(n)$ is the actual cost from start to this cell, $h(n)$ is the estimated cost from this cell to the goal (the heuristic), and $f(n)$ is the total estimated path cost through this cell. A* always expands the cell with the lowest $f(n)$ next.

The Heuristic $h(n)$

Two common choices on a grid:

Manhattan distance — for 4-connected grids (no diagonals):

Euclidean distance — for 8-connected grids (with diagonals):

The heuristic must never overestimate the true cost — if it does, A* can miss the optimal path. A heuristic with this property is called admissible.

Cost of Movement

Manhattan (4-connected)

Four neighbors, each costing 1 unit to enter. From position $(row, col)$:

Euclidean (8-connected)

Eight neighbors — cardinal moves cost 1, diagonal moves cost $\sqrt{2} \approx 1.41$:

The Algorithm

1. Create OPEN list (nodes to explore) and CLOSED list (nodes already explored)
2. Add START to OPEN
3. Loop while OPEN is not empty:
   a. Pick node with lowest f(n) from OPEN → call it CURRENT
   b. If CURRENT == GOAL → path found, reconstruct and return
   c. Move CURRENT from OPEN to CLOSED
   d. For each NEIGHBOR of CURRENT:
      - Skip if obstacle
      - Skip if already in CLOSED
      - Compute tentative g = g(CURRENT) + move cost
      - If NEIGHBOR already in OPEN with worse g → update g, h, f, parent
      - If NEIGHBOR not in OPEN → add it, set parent = CURRENT
4. If OPEN is empty and goal never reached → no path exists

Path Reconstruction

Once the goal is reached, we trace back through parent pointers to recover the path:

1. Create path list
2. step = goal
3. While step != start:
   - push step to path
   - step = parent[step]
4. Push start
5. Reverse

C++ Implementation Highlights

Displaying the Path on the Grid

void Astar::displayPathOnGrid(Grid& grid,
                               std::vector<std::pair<int,int>> path)
{
    auto display = displayGrid(grid);
    for (const auto& p : path)
        display[p.first][p.second] = '*';

    display[start.x][start.y] = 'S';
    display[goal.x][goal.y]   = 'G';

    for (char i = 0; i < grid.rows; ++i) {
        for (char j = 0; j < grid.cols; ++j)
            std::cout << display[i][j] << " ";
        std::cout << "\n";
    }
}

Grid Display Helper

std::vector<std::vector<char>> displayGrid(Grid& grid) {
    std::vector<std::vector<char>> display(
        grid.rows, std::vector<char>(grid.cols));

    for (int i = 0; i < grid.rows; ++i) {
        for (int j = 0; j < grid.cols; ++j) {
            if      (grid.cells[i][j] == -1) display[i][j] = '#'; // obstacle
            else if (grid.cells[i][j] ==  5) display[i][j] = '&'; // light terrain
            else if (grid.cells[i][j] == 15) display[i][j] = '%'; // heavy terrain
            else                             display[i][j] = '.'; // free
        }
    }
    return display;
}

Obstacles and Terrain Costs

Adding Obstacles

Set cell values to -1 and skip them during neighbor expansion:

void createObstacle(Grid& grid) {
    for (int i = 2; i < 6; ++i)
        for (int j = 3; j < 6; ++j)
            grid.cells[i][j] = -1;
}

Adding Terrain Costs (Hills / Mud)

In real navigation, not all paths are equal — a tarred road is easier than a muddy slope. Assign higher initial cell values to make those cells more costly to enter. A* naturally routes around them when a cheaper path exists:

void addHillTerrain(Grid& grid) {
    for (int i = 4; i < 7; ++i)
        for (int j = 6; j < 8; ++j)
            grid.cells[i][j] = 15;  // higher cost = harder terrain
}

The higher initial value is added into $g(n)$ when the cell is entered, so A* naturally routes around it when a cheaper path exists.

Sample Results

The figures below show two example runs. S is the start, G is the goal, * marks the planned path, # marks obstacles, and % marks high-cost terrain.

Summary

A* is the workhorse of grid-based path planning in robotics. Its combination of optimality guarantees and practical efficiency makes it the go-to choice before reaching for more sophisticated planners like RRT or PRM.

Code: astarV3.cpp

Next up: probabilistic planners — RRT and PRM for high-dimensional spaces where grid search becomes intractable.