Reactive navigation works locally but cannot reason about routes that require going away from the goal before coming back — exiting a room, navigating through a corridor, finding the only path through a maze. Global path planning requires a map and an algorithm for finding the shortest path through it. This chapter develops occupancy grid maps and the Dijkstra and A* algorithms.
An occupancy grid divides the environment into a regular array of cells. Each cell stores a probability (or log-odds) of being occupied. Cell $(i, j)$ covers the world rectangle $[x_j, x_j + r) \times [y_i, y_i + r)$ where $r$ is the cell resolution.
In Python, the grid is a 2-D NumPy array:
import numpy as np
class OccupancyGrid:
def __init__(self, width_m, height_m, resolution_m):
self.res = resolution_m
self.cols = int(width_m / resolution_m)
self.rows = int(height_m / resolution_m)
# 0 = free, 1 = occupied, 0.5 = unknown
self.grid = np.full((self.rows, self.cols), 0.5)
def world_to_cell(self, x, y):
col = int(x / self.res)
row = int(y / self.res)
return row, col
For path planning, a cell is treated as free if its occupancy is below a threshold (e.g., 0.3) and occupied if above another threshold (e.g., 0.7). Unknown cells are treated as occupied by a conservative planner, or as free by an optimistic one.
A robot with physical extent cannot treat obstacle cells as point obstacles; it would collide with the obstacle’s neighbours. The standard fix is to inflate obstacles: mark cells within the robot’s radius $r_{robot}$ of any occupied cell as also occupied. The path planner then treats the inflated map, which effectively shrinks the robot to a point.
from scipy.ndimage import binary_dilation, generate_binary_structure
def inflate(grid_binary, robot_radius_m, resolution_m):
radius_cells = int(robot_radius_m / resolution_m)
struct = generate_binary_structure(2, 1)
for _ in range(radius_cells):
grid_binary = binary_dilation(grid_binary, struct)
return grid_binary
For a robot radius of 0.12 m and cell resolution of 0.05 m, inflate by 2–3 cells.
After inflating the grid, the path-planning problem becomes: find a sequence of free cells from the start cell to the goal cell, minimising total cost (total path length, or some other metric). The grid is treated as a graph where each cell is a node and edges connect 4-connected (or 8-connected) neighbours with cost equal to the edge length (1.0 for cardinal directions, $\sqrt{2}$ for diagonals).
Dijkstra’s algorithm finds the shortest path from a single source to all reachable nodes in a non-negative-weight graph. Applied to a grid:
import heapq
def dijkstra(grid, start, goal):
costs = {start: 0.0}
parent = {start: None}
pq = [(0.0, start)]
while pq:
cost, node = heapq.heappop(pq)
if node == goal:
break
for neighbour, edge_cost in get_neighbours(grid, node):
new_cost = cost + edge_cost
if new_cost < costs.get(neighbour, float('inf')):
costs[neighbour] = new_cost
parent[neighbour] = node
heapq.heappush(pq, (new_cost, neighbour))
return reconstruct_path(parent, goal)
Dijkstra explores cells in order of increasing distance from the start, so it explores in circles. In a large grid, it expands many cells that are clearly in the wrong direction.
A* augments Dijkstra with a heuristic $h(n)$ that estimates the remaining cost from node $n$ to the goal. The priority queue sorts by $f(n) = g(n) + h(n)$, where $g(n)$ is the known cost from start to $n$.
If $h(n)$ is admissible (never overestimates the true remaining cost), A* finds the optimal path and expands far fewer cells than Dijkstra.
The standard admissible heuristic for a grid with 8-connectivity is the octile distance:
$h(n) = \max(\Delta c, \Delta r) + (\sqrt{2}-1)\min(\Delta c, \Delta r)$
| where $\Delta c = | c_n - c_{goal} | $, $\Delta r = | r_n - r_{goal} | $. |
def heuristic(a, b):
dc, dr = abs(a[0]-b[0]), abs(a[1]-b[1])
return max(dc, dr) + (1.414 - 1) * min(dc, dr)
Replace the priority in Dijkstra with (g + h, node) to get A. On a 200×200 grid with a simple room layout, A typically expands 5–20× fewer cells than Dijkstra.
Worked example: grid resolution 0.05 m, 4 m × 4 m room → 80×80 grid. Start at (10, 10), goal at (70, 70). Dijkstra expands ~4000 cells; A* expands ~800. Both find the same path; A* is faster by a factor of ~5.
A* on a grid produces a staircase path that changes direction only at 45° increments. This is kinematically unfriendly for a differential-drive robot. Two simple smoothing steps:
Shortcutting. Repeatedly check whether the segment from cell $i$ to cell $i+2$ is collision-free (ray-cast through the inflated grid). If so, remove cell $i+1$. Repeat until no more shortcuts can be made.
Spline fitting. Fit a cubic spline through the waypoints. The spline gives a smooth, differentiable path. Sample it at fixed arc-length intervals to get dense waypoints for trajectory following.
To execute a planned path, the robot iterates through waypoints, turning to face each waypoint and driving to it:
for wp in waypoints:
# Turn to face waypoint
target_heading = math.atan2(wp.y - pose.y, wp.x - pose.x)
turn_to(target_heading)
# Drive to waypoint
drive_to(wp, tolerance=0.05)
drive_to uses a heading PID controller to correct lateral drift while driving forward. The tolerance (0.05 m = 5 cm) determines how closely the robot must approach each waypoint before advancing.
Chapter 11 extends path planning to environments without a prior grid, using sampling-based methods that are more efficient in high-dimensional spaces and for non-holonomic robots.
Navigation: