Skip to content
Tutorials Center
My learning

Topics

Lesson 20 of 38 ยท Path Planning

Grid Search: BFS and Dijkstra for Robot Paths

Plan robot paths on an occupancy grid with breadth-first search and Dijkstra's algorithm, handle terrain costs and diagonal moves, in runnable Python.

The simplest way to plan a path is to chop the world into square cells, mark each cell free or blocked, and search the resulting graph. Each free cell is a node, and neighboring free cells are joined by edges. Finding a route becomes a graph search problem, and graph search is something we know how to do well.

This lesson covers the two classic uninformed searches: breadth-first search (BFS), which finds the path with the fewest steps, and Dijkstra's algorithm, which finds the path with the lowest total cost when moves are not equally expensive. The next lesson adds a heuristic to make them faster.

Turning a map into a graph

An occupancy grid stores one value per cell: free or occupied. From a cell you can move to its 4 neighbors (up, down, left, right) or its 8 neighbors (including diagonals). Diagonal steps cost sqrt(2) times a straight step, about 1.414.

Python
grid = [
    "S..#....",
    ".#.#.##.",
    ".#...#..",
    ".####.#.",
    "......#G",
]

Here # is blocked, S the start and G the goal. Remember to inflate obstacles first when your robot has size.

BFS explores in rings. It visits every cell one step from the start, then every cell two steps away, and so on, using a queue (first in, first out). The first time it reaches the goal, it has used the fewest possible steps, because it would have found any shorter route in an earlier ring.

To recover the path, store for each cell the cell you came from (parent) and walk backwards from the goal.

Python BFS shortest path on a grid

Press Run (or Ctrl + Enter) to see the result here.

Program input ยท what you'd type while the program runs

Type the answers before you press Run, one per line. Each time the program reads a line (with input()), it gets the next line from this box.

BFS only counts steps. It treats a move across mud the same as a move across a smooth floor. When costs differ, you need Dijkstra.

Dijkstra's algorithm

Dijkstra keeps, for every cell, the cost so far (g), the best known total cost from the start. Instead of a plain queue it uses a priority queue and always expands the cell with the smallest g. When a cell is popped, its cost is final. With non-negative edge costs this guarantees the cheapest path.

flowchart TD
  A["Push start with g = 0"] --> B["Pop the cell with the smallest g"]
  B --> C{"Is it the goal?"}
  C -- yes --> D["Rebuild path from parents"]
  C -- no --> E["For each free neighbor: new_g = g + step cost"]
  E --> F{"new_g lower than the stored value?"}
  F -- yes --> G["Update g and parent, push neighbor"]
  F -- no --> B
  G --> B

Dijkstra with a priority queue. BFS is the special case where every step costs 1.

Costs can come from the terrain. Let digits on the map multiply the cost of entering a cell: 1 is normal floor, 5 is rough mud, and # is a wall. Allowing diagonal moves also makes the geometry more natural.

Python Dijkstra with terrain cost and diagonals

Press Run (or Ctrl + Enter) to see the result here.

Program input ยท what you'd type while the program runs

Type the answers before you press Run, one per line. Each time the program reads a line (with input()), it gets the next line from this box.

The robot detours around the mud box instead of taking the short line through it, because the cheap long way costs less than a short way through expensive cells.

Use the heap trick above ("push duplicates, skip stale entries with a done set"). It is simpler than updating priorities inside the heap and fast enough for large maps.

Costs for real robots

You can fold many preferences into the step cost:

  • Distance from walls. Add cost near inflated obstacles so paths keep clear of them (a "cost map").
  • Terrain. Grass, carpet, slopes, and mud get higher costs.
  • Turning. A path with many zig-zags is slow for a wheeled robot; penalize direction changes by searching in (x, y, heading).
  • Danger zones. Near people or edges, raise the cost sharply.

Dijkstra requires non-negative costs. A negative edge breaks the "popped means final" guarantee.

Complexity and when to stop using it

For a grid with V cells, BFS runs in O(V) and Dijkstra in O(V log V) with a heap. Both are complete and, with the right costs, optimal. Their weakness is that they spread in all directions, even away from the goal. On a 1000 by 1000 map that can mean a million expansions to cross a short corridor. A* search fixes this by preferring cells that look closer to the goal.

Check your understanding

Quick check ยท 4 questions

of 4 answered You scored / 4 ยท perfect!

  1. 1. BFS is guaranteed to give the shortest path when...

    BFS counts steps, so it is optimal only when every move has equal cost. With unequal costs use Dijkstra.
  2. 2. Which data structure does Dijkstra use to pick the next cell?

    Dijkstra always expands the cell with the smallest cost so far, which a priority queue (heap) provides efficiently.
  3. 3. What breaks Dijkstra's optimality guarantee?

    Dijkstra assumes that once a cell is popped no later path can be cheaper. A negative edge could make a later path cheaper.
  4. 4. What is the cost of one diagonal step if a straight step costs 1?

    A diagonal across a unit square has length the square root of 2, about 1.414.

Summary

  • A grid map becomes a graph: free cells are nodes, moves are edges.
  • BFS uses a queue and finds the path with the fewest steps.
  • Dijkstra uses a priority queue and finds the cheapest path for non-negative costs; BFS is the equal-cost special case.
  • Terrain, clearance, and turning can be folded into costs.
  • Both expand in every direction, which wastes work on big maps.

Next: A* Path Planning