6.1 Graph-based path planning

Graph representations of environments

Graph-based path planning lets autonomous robots convert a physical environment into a data structure they can reason about. Instead of dealing with raw sensor data or continuous space, the robot works with nodes (locations) and edges (connections between them). Once the environment is encoded as a graph, search algorithms can find optimal or near-optimal paths through it.

Different graph representations come with trade-offs in resolution, memory usage, and how well they fit a given environment. Choosing the right one matters a lot for performance.

Occupancy grid maps

Occupancy grids divide the environment into a uniform grid of cells. Each cell stores a binary value: occupied (obstacle) or free (navigable). This gives you a high-resolution, pixel-like view of the space.

• Work best in structured indoor environments with static obstacles (think: a room with furniture)
• Scale to 3D for multi-story buildings, though memory cost grows fast
• The finer the grid resolution, the more accurate the map, but the more cells the search algorithm has to process

Topological maps

Topological maps strip away geometric detail and focus on connectivity. Nodes represent distinct places or landmarks, and edges represent traversable paths between them.

• Much more compact than occupancy grids, which makes search faster
• Work well for large-scale environments with well-defined routes
• A city map is a natural example: intersections are nodes, roads are edges. Similarly, a factory floor might use workstations as nodes and pathways as edges.
• The trade-off is that you lose fine-grained spatial information, so local obstacle avoidance typically needs a separate system

Voronoi diagrams

Voronoi diagrams partition the free space based on proximity to obstacles. The Voronoi edges are the set of points equidistant from the two nearest obstacles, forming a natural roadmap that maximizes clearance.

• Particularly useful when you want the robot to stay as far from obstacles as possible
• Best suited for environments with sparse obstacles and open areas (warehouses, outdoor terrains)
• Less useful in tightly cluttered spaces where the Voronoi edges become dense and tangled

Visibility graphs

Visibility graphs connect the corners (vertices) of polygonal obstacles with edges wherever two corners have a direct line of sight.

• Tend to produce short paths because the robot can "cut corners" around obstacles
• Work well in environments with polygonal obstacles and clear sightlines (maze-like layouts, rooms with furniture)
• The number of edges can grow quickly as obstacles increase, since every pair of mutually visible vertices gets an edge

Graph search algorithms

Once you have a graph representation, you need an algorithm to find the best path through it. The algorithms below differ in what they guarantee (optimal vs. suboptimal), what they assume (static vs. dynamic environments), and how much computation they require.

Dijkstra's algorithm

Dijkstra's finds the shortest path from a single source node to every other node in a weighted graph.

1. Initialize the source node with cost 0 and all others with infinity.
2. Place all nodes in a priority queue ordered by current cost.
3. Extract the node with the lowest cost, and for each neighbor, check if traveling through the current node offers a cheaper path. If so, update the neighbor's cost.
4. Repeat until the queue is empty or you've reached your goal.

• Guarantees the optimal path as long as all edge weights are non-negative
• Time complexity: $O(E + V \log V)$ with a priority queue, where $E$ is edges and $V$ is vertices
• The main downside is that it explores outward uniformly in all directions, with no sense of where the goal is. This makes it slower than A* when you have a known goal location.

A search algorithm

A* improves on Dijkstra's by adding a heuristic function that estimates the remaining cost to the goal. This steers the search toward the goal instead of expanding blindly.

Each node gets a total cost: $f(n) = g(n) + h(n)$

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

The algorithm always expands the node with the lowest $f$-cost. If the heuristic is admissible (never overestimates), A* guarantees an optimal path. If it's also consistent, A* avoids re-expanding nodes, which improves efficiency.

Time complexity is the same as Dijkstra's in the worst case, $O(E + V \log V)$, but a good heuristic dramatically reduces the number of nodes actually explored.

D search algorithm

D* (and its lighter variant D* Lite) handles dynamic environments where edge weights can change after the initial search, such as when the robot discovers a new obstacle mid-traverse.

• Maintains the search tree from a previous plan and incrementally updates costs when changes are detected
• Avoids recomputing the entire path from scratch, which saves significant time
• Guarantees optimality even as the environment changes
• Overhead comes from maintaining and updating the priority queue as costs shift

This is the go-to algorithm for robots that must navigate while continuously receiving new sensor data.

Bidirectional search

Bidirectional search runs two simultaneous searches: one forward from the start and one backward from the goal. The path is found when the two frontiers meet.

• Cuts down the search space significantly, since two smaller searches cover less total area than one large search
• Can be combined with Dijkstra's or A* for the individual search directions
• Requires a reliable way to detect when the frontiers overlap and to stitch the two half-paths together
• Works best when both start and goal are known and the graph structure is reasonably symmetric

Heuristics for graph search

A heuristic is an estimate of how far a node is from the goal. The better the estimate, the fewer nodes A* needs to expand. But the heuristic must be chosen carefully to preserve optimality guarantees.

Manhattan distance

Sums the absolute differences in x and y coordinates:

$h = |x_1 - x_2| + |y_1 - y_2|$

This assumes movement is restricted to horizontal and vertical directions (no diagonals), making it a natural fit for grid-based maps. It's both admissible and consistent for 4-connected grids.

Example: From $(3, 4)$ to $(7, 2)$: $|3 - 7| + |4 - 2| = 4 + 2 = 6$

Euclidean distance

The straight-line distance between two points:

$h = \sqrt{(x_1 - x_2)^2 + (y_1 - y_2)^2}$

This assumes the robot can move in any direction. It's admissible in obstacle-free space, but in environments with obstacles the actual path will be longer than the straight line, so it still never overestimates.

Example: From $(3, 4)$ to $(7, 2)$: $\sqrt{(3 - 7)^2 + (4 - 2)^2} = \sqrt{16 + 4} \approx 4.47$

Diagonal distance

Accounts for diagonal movement on a grid by taking the maximum of the absolute coordinate differences:

$h = \max(|x_1 - x_2|, |y_1 - y_2|)$

This is more accurate than Manhattan distance on 8-connected grids (where diagonal moves are allowed), since a diagonal step covers both an x and a y unit simultaneously.

Example: From $(3, 4)$ to $(7, 2)$: $\max(|3 - 7|, |4 - 2|) = \max(4, 2) = 4$

Admissible vs. consistent heuristics

• Admissible: Never overestimates the true cost to the goal. A* with an admissible heuristic is guaranteed to find the optimal path.
• Consistent (also called monotone): Satisfies the triangle inequality: $h(n) \leq c(n, n') + h(n')$, where $c(n, n')$ is the edge cost between neighbors $n$ and $n'$.
• Every consistent heuristic is admissible, but not every admissible heuristic is consistent.
• With a consistent heuristic, A* never needs to re-expand a node, which makes it more efficient in practice.

Graph pruning techniques

Real-world environments can produce enormous graphs. Pruning techniques reduce graph size and complexity while keeping enough detail for effective navigation. This speeds up search and lowers memory usage.

Hierarchical graphs

Build a multi-level graph where higher levels are coarser abstractions and lower levels hold fine detail. The planner first searches the high level to identify a promising region, then refines the path at lower levels.

Example: In a multi-story building, the top level might have one node per floor. The next level down has one node per room. The bottom level is a full occupancy grid within each room.

Adaptive resolution grids

Instead of using a uniform grid everywhere, adjust cell size based on what's nearby. Use fine resolution near dense obstacles or critical areas, and coarse resolution in open space.

Example: In a warehouse, cells near shelving units are small (high detail), while cells in open corridors are large (low detail). This keeps the total number of cells manageable without sacrificing accuracy where it counts.

Quadtrees and octrees

These recursively subdivide space into four quadrants (2D quadtree) or eight octants (3D octree). Subdivision continues only in regions that contain a mix of free and occupied space. Homogeneous regions (all free or all occupied) stay as a single large cell.

• Very efficient for environments with non-uniform obstacle distributions
• A large outdoor map with a few buildings will compress well: open areas become large leaf nodes, while areas near buildings get subdivided finely

Real-time graph-based planning

Robots in the real world can't always wait for a perfect plan. They need to start moving quickly and adjust as new information arrives. Real-time planning methods address this by trading off plan quality against computation time.

Anytime planning algorithms

These produce an initial (possibly suboptimal) solution fast, then keep improving it if time allows. You can interrupt the algorithm at any point and still have a valid plan.

Example: Anytime Repairing A* (ARA*) starts with an inflated heuristic to find a quick solution, then gradually reduces the inflation factor to approach optimality.

Incremental search methods

When the environment changes locally, incremental methods update the existing search tree rather than recomputing from scratch. They reuse as much prior work as possible.

Example: Incremental A* detects which parts of the search tree are affected by a cost change and re-expands only those nodes.

Replanning strategies

Sometimes the current plan becomes invalid (a new obstacle blocks the path) or significantly suboptimal. Replanning strategies detect these situations and trigger a fresh or partial replan.

• The key design question is when to replan: too often wastes computation, too rarely means the robot follows bad plans.
• D* Lite is a common choice here, since it efficiently incorporates new obstacle information without a full restart.

Applications of graph-based planning

Indoor robot navigation

Robots in homes, offices, or hospitals typically use occupancy grid maps or topological maps paired with A* search. The environment is mostly static, so the main challenge is accurate mapping and efficient replanning when doors close or furniture moves.

Outdoor vehicle routing

Autonomous vehicles on road networks naturally fit topological maps, where intersections are nodes and road segments are edges. Dijkstra's or A* can plan routes while accounting for traffic, road type, and vehicle constraints like turning radius.

Multi-robot coordination

When multiple robots share a workspace, their paths must avoid collisions with each other, not just with obstacles. Graph-based planners can model this by searching a joint state space or by using centralized coordination that assigns non-conflicting paths to each robot.

Autonomous warehouse systems

Warehouses combine many of the techniques covered here. Adaptive resolution grids or quadtrees handle the structured-but-changing layout. Anytime planners let robots start moving quickly, and replanning strategies handle shifting inventory and congestion.

Limitations and challenges

High-dimensional state spaces

A mobile robot on a flat floor has a manageable 2D or 3D state space (x, y, heading). But a robotic arm with six joints lives in a 6D configuration space, and graph-based methods struggle here because the number of nodes grows exponentially with dimensions. Sampling-based planners (like RRT or PRM) are often used alongside or instead of graph search in these cases.

Dynamic and uncertain environments

Moving obstacles, uncertain sensor readings, and imprecise localization all complicate graph-based planning. The graph itself may be inaccurate or outdated. Probabilistic representations (like belief roadmaps) can model this uncertainty, but they add complexity.

Computational complexity

For large environments or high-dimensional spaces, even efficient algorithms like A* can be too slow for real-time use. Strategies to cope include hierarchical planning, pruning, parallel computation, and GPU acceleration.

Integration with other planning methods

Graph-based planning rarely works in isolation on a real robot. A common architecture uses a graph-based planner for global path planning (getting from room A to room B) and a local planner (sampling-based or optimization-based) for real-time obstacle avoidance along the way. Designing these layers to work together smoothly is an ongoing engineering challenge.