Mobile Robot Navigation, step 5
Planning a path on the grid
Grow the walls by the rover's size, then find the shortest way through the maze: breadth-first, then A*, which heads for the goal.
From a map to a route
A grid is a graph: each free cell is a node, linked to its four neighbours (up, down, left, right), and each step costs 1. The best route is the path with the fewest steps.
Configuration space
The rover isn't a point: it's a disc 6.4 cm across. Grow every wall by the rover's radius plus a 1 cm margin, and the rover's centre can go anywhere that's still free. This grown map is the rover's configuration space: plan through it as if the rover were a dot. Here it leaves a 2 cm band down the middle of each corridor.
Three searches
- Breadth-first search (BFS) takes cells off a queue in the order it found them. It spreads out in rings, one step further each time, so it reaches the goal by a shortest path, searching every direction at once.
- Dijkstra's algorithm handles steps of different costs: a priority queue (a heap) always hands out the cell with the smallest cost so far, . When every step costs 1 it expands cells in the same order as BFS.
- A* orders the heap by , where guesses the cost still to go. The Manhattan distance counts the steps needed if nothing were in the way, so it never overestimates. A* then still returns a shortest path, but it heads for the goal and usually expands far fewer cells. In a maze, where walls keep sending it the long way round, it saves less.
Moving to 4 neighbours rather than 8 (no diagonals) keeps every step the same cost, so Manhattan distance is exact on an empty grid, and the follower rounds off the square corners.
g = {start: 0}; parent = {start: None}; heap = [(h(start), start)]
while heap:
pop (f, cell) with the smallest f
if cell was expanded before: skip it
expand it (record it); if it's the goal: stop
for each free neighbour n with g[cell] + 1 < g.get(n, inf):
g[n] = g[cell] + 1; parent[n] = cell; push (g[n] + h(n), n)
Your task
Write heuristic(cell, goal) and astar(grid, start, goal), which returns (path, expanded): the cells from start to goal (from walk_back), and every cell you expanded, in order. Cells are (row, col) tuples, and heapq.heappush and heapq.heappop keep the heap.
The program runs BFS and your A* across step 4's maze, shows both in the Images tab, and drives the A* path with the built-in follower. The grader also runs astar on 22 test grids: every path must be a shortest one, and A* must expand fewer cells than BFS.