Grid and lattice planning

Most mobile robots plan on grids. The robot's map, built by the SLAM methods of ROB-340, is already an occupancy grid; turning it into a planning graph needs only a decision about which cells are traversable and what each step costs. Grid planning is resolution complete, simple to implement, fast in two dimensions and easy to update as the map changes, which is why it dominates indoor navigation. Its weaknesses are equally well known: paths constrained to grid directions, which zigzag and hug walls, and no notion of what motions the robot can actually execute. This lesson covers how to build a good planning grid from a map, how to get smoother paths from it, and how state lattices and Hybrid A* extend grid search to robots, such as cars, that cannot turn on the spot.

From occupancy grid to cost map

An occupancy grid stores, for each cell, the probability that it is occupied. A planner needs a cost per cell instead. The usual construction, used in ROS 2 Nav2's costmap layers, marks cells above an occupancy threshold as lethal obstacles, then inflates them. Every cell within the robot's inscribed radius of an obstacle is marked as an inscribed obstacle: if the robot's center were there, some part of it would certainly collide. Beyond that, cost decays with distance out to an inflation radius, giving the planner a reason to stay away from obstacles when it has room. Unknown cells get a separate cost chosen by policy. A common decay is exponential:

where d is the distance to the nearest obstacle, \( r_{\text{in}} \) is the inscribed radius and α sets how fast the cost falls. The distance to the nearest obstacle for every cell is computed in one pass with a brushfire (breadth-first) search from the obstacles or with a distance transform, so building the cost map is linear in the number of cells. Layered cost maps combine several sources: a static layer from the saved map, an obstacle layer from live sensor data, and inflation on top. The navigation stack itself, with its layers and plugins, is covered in ROB-340 lesson 11; here the focus is on what the planner does with the costs.

Grid resolution and connectivity

Choosing the cell size is a trade-off. Smaller cells represent narrow gaps and precise obstacle shapes but multiply the number of nodes by four each time the size halves, and they make the map noisier. Indoor robots commonly use 5 cm cells; outdoor vehicles 10 to 50 cm. Connectivity matters too. On a 4-connected grid, the shortest path between two points is a staircase whose length can exceed the straight-line distance by up to 41 percent. On an 8-connected grid the worst case is about 8 percent, at an angle of 22.5°. A 16-connected grid, adding moves like two cells across and one up, cuts this to about 3 percent. Even then, grid paths change direction only at multiples of the allowed angles, and they tend to graze the corners of obstacles where the grid is tight.

Smoother paths: post-processing and any-angle search

Two families of techniques remove grid artifacts. Post-processing takes the grid path and shortens it. The simplest method is line-of-sight shortcutting: walk along the path and, whenever a later waypoint is directly visible from the current one, delete the waypoints in between. The result is a sequence of straight segments, much like a visibility-graph path. Smoothing with splines or a gradient method (lesson 9) then rounds the corners. Any-angle search algorithms instead build line of sight into the search. Theta* (Nash, Daniel, Koenig and Felner, 2007) is A* in which, when a node is relaxed, it checks whether the node's parent's parent can see the neighbor directly; if so, it links them with a straight segment. Theta* paths are close to true shortest paths and need little smoothing. Field D* applies a similar idea to incremental replanning by interpolating costs along cell edges.

import math
# Line-of-sight shortcutting of an 8-connected grid path (rows, cols), with a free 10 x 10 area and one blocked cell
blocked = {(3, 4), (4, 4), (5, 4)}
path = [(0,0),(1,1),(2,2),(2,3),(1,4),(1,5),(2,6),(3,7),(4,8),(5,9),(6,9),(7,9)]
def visible(a, b, steps=50):
    for k in range(steps + 1):
        t = k / steps
        r, c = a[0] + t * (b[0] - a[0]), a[1] + t * (b[1] - a[1])
        if (round(r), round(c)) in blocked: return False
    return True
def length(p): return sum(math.dist(p[i], p[i + 1]) for i in range(len(p) - 1))
short = [path[0]]; i = 0
while i < len(path) - 1:
    j = len(path) - 1
    while j > i + 1 and not visible(path[i], path[j]): j -= 1
    short.append(path[j]); i = j
print("grid path: %d waypoints, length %.2f cells" % (len(path), length(path)))
print("shortcut:  %d waypoints, length %.2f cells" % (len(short), length(short)))
print("kept:", short)

grid path: 12 waypoints, length 13.90 cells
shortcut:  3 waypoints, length 12.30 cells
kept: [(0, 0), (5, 9), (7, 9)]

State lattices for car-like robots

A grid over (x, y) cannot represent a robot that must respect its heading and turning radius. Adding a heading dimension gives a three-dimensional grid over (x, y, θ), but joining neighboring cells with straight steps still ignores the turning constraint. State lattices, developed by Pivtoraiko, Knepper and Kelly around 2009, solve this by building the graph from motion primitives: short, dynamically feasible trajectories that start and end exactly on lattice states. For example, with headings discretized into 16 directions, each heading has a set of primitives that move forward a few cells and end at the same or an adjacent heading, plus reverse and turning primitives if the robot allows them. Because every primitive is feasible and they join exactly at lattice states, any path through the lattice is executable. The primitives are computed offline, usually by solving a small boundary-value problem, together with the cells they sweep, so collision checking a primitive at search time means checking a precomputed footprint.

Hybrid A*

Hybrid A*, used by Stanford's Junior in the 2007 DARPA Urban Challenge and described by Dolgov, Thrun, Montemerlo and Diebel, combines a grid with continuous states. Each search node holds a continuous pose (x, y, θ), but nodes are associated with discretized cells, and only one node is kept per cell (the cheapest). Expanding a node applies a few steering commands, such as full left, straight and full right, forward and in reverse, for a fixed arc length, producing new continuous poses. Because states are continuous, paths are drivable without lattice alignment, at the cost of losing resolution completeness: pruning by cell may discard a node that was needed. Two heuristics are combined, taking the maximum: a nonholonomic heuristic that ignores obstacles (the Reeds-Shepp distance) and a holonomic heuristic that ignores turning (a 2-D Dijkstra distance on the obstacle map). At regular intervals, the planner also tries an analytic expansion, connecting the current node directly to the goal with a Reeds-Shepp curve; if that curve is collision-free, the search ends early with an exact final approach. The resulting path is then smoothed by optimization. Hybrid A* remains a standard choice for parking and for car-like robots in unstructured spaces.