mobile robots from scratch, part 15: path planning and navigation

The planner doesn’t use the map directly. The robot isn’t a point: it’s 36 cm across. So it plans on a costmap, a grid of 10 cm cells where each says how bad it is for the robot’s centre to be there.

Any cell closer to an obstacle than the robot’s radius, 18 cm, is lethal (dark): the robot would touch something. Beyond that, an inflated band (orange) costs less and less, out to 50 cm more, so paths prefer to keep away from walls without being forbidden from going near them:

\[c(d) = 0.9\, e^{-3 (d - r) / 0.5}.\]

Of the room’s 6,000 cells, 1,371 are lethal and 2,446 inflated. The robot is in the corner (blue); the goal is the red dot behind the wall.

Dijkstra’s algorithm finds the cheapest path by exploring outward from the start, always taking the cheapest unexplored cell next, until it reaches the goal. Choose it and scroll: it explores 3,905 cells, in every direction, including away from the goal.

A* adds a guess of how far each cell still is from the goal, its straight-line distance \(h\), and explores the cell with the smallest cost so far plus guess:

\[f = g + h.\]

It finds the same 10.20 m path after exploring 2,103 cells. Because the straight line is never longer than the real route, A* is still guaranteed to find the cheapest path.

Multiply the guess by 3 and the search rushes at the goal: 525 cells. But the guess now overestimates, the guarantee is gone, and the path it finds costs 10% more, squeezing past the box.

Each move costs its length times \(1 + w \cdot c\), where \(c\) is the cell’s cost and \(w\) says how much the planner cares. Slide it.

At 0, only length counts: the shortest path, 8.91 m, but it skims the box and the wall with no gap at all. One bump of the localisation, and the robot hits them. At 1, the path is 9.11 m and keeps 14 cm away. At 4, 10.20 m, and 47 cm: it goes down the middle of every gap. Beyond that, nothing changes much, since the path is already as far from things as the room allows.

A good plan is not the shortest one. It’s the shortest one the robot can follow safely, with the errors it has.

The path is a list of cells; the motors want speeds. The controller turns one into the other. Pure pursuit is the simplest that works: find the point on the path one lookahead distance ahead of the robot, and drive the circular arc that reaches it. For a point at \((x, y)\) in the robot’s frame, that arc has curvature

\[\kappa = \frac{2y}{x^2 + y^2},\]

so the robot turns at \(\omega = v\kappa\) (part 2’s kinematics give the wheel speeds).

With a lookahead of 0.4 m, the robot (blue) stays 3.8 cm from the path on average, 9.1 cm at worst, and arrives in 32.4 s. Slide it longer: at 1.2 m it looks so far ahead that it cuts the corners, 37.4 cm off at worst, which on this plan eats most of the gap the planner left. Shorter tracks tighter, but on a real robot, with delays and wheel slip, too short a lookahead makes it weave.

Maps go out of date. The robot is 4.25 m along its 10.20 m path when its lidar sees a chair that wasn’t there when the map was made. Press the button.

The lidar’s points go into the costmap as new obstacles, and the chair stands right where the old plan (red, dashed) went round the end of the wall. The planner runs again from where the robot is: a new path of 7.73 m, wider round the chair and between it and the pillar, 11.98 m in all. It found that by exploring 1,330 cells, in a few milliseconds here.

Real navigation replans like this all the time, about once a second, whether anything has changed or not.