29Planning a path around obstacles

Stand in the corridor of the house from the last module, at the left end, facing along it. The room you want is right below you: under a meter away as the crow flies, through the wall at your side. You do not walk into the wall. You set off along the corridor, turn in at the doorway some way down, and come back along the inside of the room. That module was about knowing where you are on a map. This one is about what you did just now without noticing: with the map in hand and your place on it, finding a way from here to there.

The same map, a different question

The map is the one from the last module, the house cut into small cells, each marked wall or floor; the cells are ten centimeters square. Where the rover stands is settled: this module takes the particle filter's answer as given. What is new is a goal, a cell it wants to reach, in the room below the corridor. Path planning is working out a route from where the robot is to where it wants to be, on the map, before it moves: a chain of floor cells, each touching the next, from the start cell to the goal.

Why plan, rather than head for the goal and swerve at walls? Because a robot that only reacts can be led into a corner and stay there, since every move out takes it further from the goal first. The map lets it look at the whole house before it turns a wheel.

A search that spreads

Here is a way to find the route with a pencil. Start at the rover's cell. Look at its neighbors, the cells above, below, left and right that are floor: each is one cell from the start. Write a 1 in each, and note which cell it was reached from. Now take the cell with the smallest number that has not yet been looked at, and look at its neighbors: any with no number yet gets this cell's number plus one, and a note of where it came from. Keep going, always the smallest number next.

The cells that have a number but have not yet been looked at are the frontier, the edge of the search, and the search grows by pushing it out from its smallest number. Because the smallest number always goes first, the edge grows evenly, like a ripple on a pond: a ring of 1s, then 2s, then 3s, bending round walls and pouring through doorways. The moment the goal is the cell taken from the frontier, stop. Follow its came-from notes back to the start: that chain is the path.

It is the shortest path on the grid, and the reason is the order. A cell only ever comes off the frontier when nothing on it has a smaller number, so by the time the goal comes off, every cell with a smaller number has already been looked at, and none of them led to the goal, or it would have come off sooner. This search has a name, Dijkstra's search, after the man who wrote it down, and A* is built on it.

The wasted work, and a guess that saves it

Watch the ripple and something is wrong: it spends as much effort behind the rover as in front, because a ripple has no idea where the goal is. On this house, finding the room below that way looks at about four hundred cells for a route of twenty-four.

The fix is a hint. From any cell the robot can measure the distance to the goal as the crow flies, straight through walls: a heuristic, a quick guess at how far there is still to go. Now instead of taking the frontier's smallest number, take the cell whose number plus its guess is smallest: the shortest distance so far, plus the best case for the rest. Cells toward the goal have small guesses and go first; cells away from it have big guesses and wait, often for ever. The ripple becomes a tongue reaching toward the goal. This is A*, said "A star", the search inside most robots that plan on a grid.

The guess can mislead, and the wall below the rover shows it. The cells nearest the goal as the crow flies are the ones against that wall, so the search goes there first. But each next cell along the wall has a bigger number than the last while its guess barely shrinks, so its sum climbs, until a waiting cell further along the corridor, away from the wall, has the smaller sum. The search moves there, slides right, finds the doorway, drops through and turns back to the goal: about a hundred and twenty cells instead of four hundred, and not a cell of extra path.

Why never a longer path? A straight line is the shortest thing there is, so the guess is never more than the true distance left. Now suppose a shorter route existed. Some cell on it is still waiting on the frontier, and its number plus its guess is at most that route's length, since the guess never overshoots the rest. The goal's own guess is zero, so its sum is the length of the route found, which is longer. The smaller sum goes first, so that waiting cell would have come off before the goal did. A guess that sometimes said too much would break this, pushing a cell on the shortest route to the back and letting the search finish through a worse one. Robots use the straight line because it is the best guess that is never too high.

Moves on this grid are up, down, left or right, so a slanting route is a staircase. A route to a cell three right and two down is three rights and two downs in some order, and every order is the same five moves: the staircase and the L round the outside tie. When routes tie, the planner takes the one with the fewest turns, because a rover turns slowly.

A search that throws darts

There is a second family of planners that does not spread at all. Picture a tree growing from the rover's cell. Throw a dart at the map, anywhere. Find the point of the tree nearest to where it landed and grow a new branch from there toward the dart, fifteen centimeters long. If it would cross a wall, throw it away. Throw again; a point nearest to several darts sprouts several branches, which makes this a tree. One dart in ten is aimed at the goal itself, so the tree leans that way; the rest land anywhere, so the tree fills the open space in every direction. When a branch tip comes within a cell of the goal with a clear line to it, follow that branch back to the root: that is the route. This is a rapidly exploring random tree, RRT for short, as the box calls it.

Its route is crooked, because the branches grew toward darts, not the goal, and it is not the shortest; the next run gives a different route. Narrow places are hard for it: a branch only gets through the doorway when a dart lands beyond it. So why use it? Because it never needs the grid. A rover's map is a few hundred cells. A robot arm's place is not a spot on a floor but the angles of all its joints at once, since those are what it can change, so its map is every combination of six angles, and a grid fine enough to plan on would have billions of cells, far too many to ripple over. A tree grown by darts looks only where they land, so it can find a way through a space no grid could cover.

The box

The house is the one from the last module, seen from above, the rover at the left end of the corridor. The green ring in the room below is the goal. Step does one look: a cell comes off the frontier and its neighbors join it. Search runs the search at thirty cells a second; Slow makes that seven or eight. Cells looked at are shaded faint orange, the frontier orange, the cell being looked at now a strong orange, and the path, once found, is a blue line, which the rover then drives. The line under the map counts the cells looked at and gives the path's length in meters.

The chooser labeled Planner switches between A* on the grid and RRT. The switch labeled Use the straight-line guess takes the guess away, turning A* back into Dijkstra's ripple. Drag the green ring to any floor cell, or click a cell to put a wall there or take one out; either restarts the search. Reset puts everything back.

ResetPlaySlowLabels

SearchStepPlannerA* on the gridRRT, a random treeUse the straight-line guess

Waiting for the first frame…

This is easier on a larger screen, but it works here too.

The goal and the walls are yours to move. Drag the green ring to the far right end of the corridor and the guess is at its best, a thin band of a few dozen cells where the ripple needs over five hundred. Wall up the three cells of the doorway the path went through and the search looks at every cell it can reach, about 630, and gives up: a tag on the map says there is no way to get here, which is as useful as a route, because a robot that cannot get there should say so rather than push at a wall.

What you saw

The ripple and the tongue found the same path, because both take cells off the frontier in an order that never misses a shorter route; the tongue did a third of the work, and where its guess was wrong it paid in cells, never in distance. The tree found a way too, by looking only where its darts fell.

What this is for

This is how a robot vacuum gets back to its dock and a warehouse robot threads between shelves: a map, a goal, a search spreading from the robot's cell with a guess pulling it toward the goal, and a path handed to the motors. Real planners also count a step into a cell near a wall as more than one, so the path keeps clear of walls, and they search again every second or two as the robot moves, in case the world has changed.

What the simulation leaves out

The rover here drives the path as if it were a point, and it is bigger than a cell, with the doorways thirty centimeters wide; a real planner grows every wall by the robot's own width before it searches, so that a path a cell from a wall is one the robot can drive. Every step here counts one, where a real map may count a step onto carpet or a ramp as two or three. And the map is a snapshot: a chair moved since it was drawn is not on it, so a planned path can run straight into something the rangefinders can see, and the robot must notice and plan again. What to do in the moment about a thing the map never knew is a different job, and it gets a module of its own.