Project: drive the route

A full route through waypoints, marked on how closely it was followed and how long it took.

U10.7Following a trajectoryUniversity50 min

Do this lesson in the simulator

A full route, four legs and three corners, driven once, cleanly, inside a time limit. Everything in the module is in it.

The route

The tape runs (30, 30) to (30, 150) to (120, 150) to (120, 60) to (170, 60). That is 350 cm of path with three right-angle corners, and a crate sitting beside the middle of it that the robot must not touch. The finish is the green zone around the far end.

You are marked on two things that pull against each other:

  • Accuracy. Within 10 cm of the tape for at least 85 percent of the run.
  • Economy and time. The distance you travel must be close to the length of the route, and the whole thing has to be inside 90 seconds.

Driving slowly and carefully passes the first and risks the second. Driving quickly fails the first at the corners. That tension is the project.

The structure

from bugbot import *
import math
connect()

PATH = [(30.0, 30.0), (30.0, 150.0), (120.0, 150.0), (120.0, 60.0), (170.0, 60.0)]
cum = [0.0]
for (ax, ay), (bx, by) in zip(PATH, PATH[1:]):
    cum.append(cum[-1] + math.hypot(bx - ax, by - ay))
print("route", round(cum[-1], 1), "cm in", len(PATH) - 1, "legs")
print("at 13 cm/s that is", round(cum[-1] / 13.0, 1), "s of driving before any corners")

Run this in the simulator

  1. The path. The waypoints, with a table of cumulative arc lengths beside them, as in U10.1.
  2. The projection. Nearest point on the path, its arc length, and how far off you are. U10.3.
  3. The look-ahead point. Arc length plus L, read back off the path. U10.4.
  4. The speed. A profile, not a constant: ramp up at the start, ease off for the last few centimetres, and reduce for the corners. U10.2.
  5. The command. Velocity vector at the look-ahead point, through the inverse kinematics, with a dead band inverse on the way out. U2 and U10.3.
  6. The plots. off path and speed, every tick.

Slowing down for what matters

Two rules are worth writing, and they cover most of the difference between a run that scrapes through and one that looks deliberate.

Slow down when you are off the path. Speed multiplied by something like max(0.45, 1 - off / 14). It is self correcting: drifting wide costs speed, which gives the correction time to work, which brings you back, which gives the speed back.

Slow down when the path bends. Look at the direction of the path at your projection and at the look-ahead point, and reduce the speed in proportion to the angle between them. That is the same preview idea as pure pursuit, applied to the throttle rather than the steering, and it is what makes the corners look intentional.

Where to look when it fails

In this order, because the order saves time.

  1. Is off path small on the straights? If not, it is the cross-track loop: gain, sign, or the dead band.
  2. Does it spike at the corners? That is the look-ahead cutting them. Reduce L, or slow down for the bend, or both.
  3. Does it weave on the straights? Look-ahead too small for the speed you are running.
  4. Did it run out of time? Cruise speed, or a speed rule so cautious that the robot never gets going.
  5. Did it touch the crate? The path was fine and the tracking was not. See 2.

Task: drive the route

Follow the tape from (30, 30) round to the green finish, within 10 cm of it for at least 85 percent of the run, without touching the crate, inside 90 seconds. Plot off path and speed, and print drove:, how far the robot actually travelled.

from bugbot import *
import math
connect()

DT = 0.1
START = (30.0, 30.0)
PATH = [(30.0, 30.0), (30.0, 150.0), (120.0, 150.0), (120.0, 60.0), (170.0, 60.0)]
CRUISE, LOOK = 13.0, 16.0

Challenges

  1. Round the corners in the path itself with short arcs, and follow that instead. Does the run get faster, tighter, or both?
  2. Run it with a fixed speed and no corner rule, and compare the worst off path value.
  3. Put the route on a clock as in U10.6, and report how many seconds behind its own plan the robot finished.

What comes next

This module trusted position() throughout, which is a lab overhead camera and not something a robot carries. U11 is the camera the robot does carry: the pinhole model, how a tag on the wall becomes a pose, and how a picture becomes regions worth driving towards.