A* Path Planning on an Occupancy Grid

A robot planner answers a different question from an obstacle-avoidance controller. The planner asks, “Which free cells connect the start to the goal at lowest cost?” The controller asks, “What motion is safe right now?” A* is a graph-search algorithm that combines the cost already travelled with an estimate of the remaining cost, making it a practical first planner for a two-dimensional occupancy grid.

The route is only as safe as the grid and robot footprint supplied to it. A mathematically shortest line through free cell centres can still scrape a wall if obstacles are not inflated.

Turn the map into a graph

An occupancy grid divides space into equal cells. Each cell may be free, occupied, or unknown. For a basic planner, every traversable cell is a graph node connected to its neighbours.

With four-way movement, neighbours are north, east, south, and west, each with cost 1. With eight-way movement, add diagonals with cost √2. Multiplying by map resolution later converts grid distance to metres.

Decide how to treat unknown cells. A cautious indoor robot may consider them blocked; an exploration robot may allow them with extra cost. Make that a policy input, not an accidental comparison against the wrong numeric value.

Understand the three A* scores

For each node n, A* tracks:

• g(n): known cost from the start to n;
• h(n): heuristic estimate from n to the goal;
• f(n) = g(n) + h(n): estimated total route cost through n.

The open set holds discovered nodes that may be expanded. At each step, remove the node with the smallest f. If it is the goal, reconstruct the route by following parent links back to the start. Otherwise, relax each valid neighbour: if the new g is cheaper than its recorded cost, update its cost and parent.

The heuristic guides search without changing the cost already paid. With four-way movement and uniform cost, Manhattan distance is suitable:

h = |goalX - x| + |goalY - y|

For eight-way movement with diagonal cost √2, use octile distance. Euclidean distance also stays below or equal to the true path cost in open space. Overestimating can make search faster but sacrifices the optimal-route guarantee.

Implement the search with a priority queue

Python’s standard library is enough for a compact reference implementation:

from heapq import heappop, heappush

def astar(start, goal, neighbors, heuristic):
    frontier = [(heuristic(start, goal), 0, start)]
    came_from = {start: None}
    cost = {start: 0}

    while frontier:
        _, current_cost, current = heappop(frontier)
        if current_cost != cost.get(current):
            continue  # stale queue entry
        if current == goal:
            break

        for next_cell, step_cost in neighbors(current):
            new_cost = current_cost + step_cost
            if new_cost < cost.get(next_cell, float("inf")):
                cost[next_cell] = new_cost
                came_from[next_cell] = current
                heappush(frontier, (
                    new_cost + heuristic(next_cell, goal),
                    new_cost,
                    next_cell,
                ))

    if goal not in came_from:
        return None

    path, cell = [], goal
    while cell is not None:
        path.append(cell)
        cell = came_from[cell]
    return list(reversed(path))

Instead of trying to edit an existing heap entry, push the improved value and discard stale entries when popped. This keeps the implementation small and correct. On a memory-constrained controller, a fixed-size grid and indexed arrays avoid dynamic maps.

Prevent diagonal corner cutting

With eight-way movement, a diagonal neighbour should be rejected when either adjacent cardinal cell is occupied. Otherwise the planner can pass between two obstacles that touch at a corner—an impossible motion for a robot with non-zero width.

For a move from (x, y) to (x+1, y+1), verify that (x+1, y) and (x, y+1) are both traversable. Then apply footprint inflation as a separate safety layer.

Inflate obstacles for the real robot

Planning the robot centre through raw occupied cells treats the robot like a point. Inflate every obstacle by at least the robot’s inscribed radius plus a safety margin. Equivalently, expand obstacles and plan a point centre through the remaining free space.

A hard inflation radius marks nearby cells blocked. A graded cost field adds larger cost near walls, encouraging clearance without declaring every nearby cell impossible. The chosen radius must include body overhang, localization uncertainty, and tracking error.

Map resolution changes the number of cells: inflationCells = ceil(inflationMetres / resolution). Test the conversion explicitly. A 0.20 m radius is four cells at 0.05 m/cell, not 0.20 cells.

Add useful traversal costs

Uniform A* returns the shortest grid path. Robots often need the lowest cost path instead. Add cost for cells near obstacles, reverse motion, sharp turns, or rough terrain. Keep all edge costs non-negative, and keep the heuristic no larger than the remaining cost under that model if optimality matters.

Do not add a turn penalty without including orientation in the state. If arriving at the same cell facing east and north produces different future cost, (x, y) alone no longer describes the node; use (x, y, heading) or accept that the penalty is only a post-processing preference.

Validate the planner systematically

Start with tiny maps whose answer is obvious:

1. Empty grid: path should approach the straight-line optimum.
2. One wall with one gap: route must pass through the gap.
3. Blocked goal: return no path without looping.
4. Start equals goal: return the single start cell.
5. Diagonally touching obstacles: no corner cutting.
6. Narrow corridor smaller than the inflated footprint: no path.

For each returned path, assert that it starts and ends correctly, every step is adjacent, no cell is blocked, and total cost equals the sum of step costs. Visualize explored cells as well as the final route; excessive expansion often reveals a bad heuristic or units mismatch.

Diagnose common planner failures

• The route crosses a wall: occupancy indexing, row/column order, or diagonal validation is wrong.
• The route clips corners: obstacles were not inflated or diagonal corner cutting is allowed.
• A returns a longer route than expected:* heuristic or step-cost units disagree, or stale costs are accepted.
• Search consumes too much memory: grid resolution is unnecessarily fine or duplicate queue entries are never discarded.
• The robot cannot follow a valid path: the plan ignores footprint, turning radius, controller limits, or localization error.
• The path flickers after every sensor update: global planning is being restarted for small changes without cost hysteresis or replanning policy.

The Maze Solver Simulator demonstrates a related distance-field method for a small unknown maze. The Obstacle Avoidance Simulator shows why a global path still needs a local safety controller.