Capstone: the whole robot

Calibrate, estimate, localise, plan, follow, arrive, and report. One run, everything in it.

U12.7Learning, and the capstoneUniversity90 min

One run, and it needs most of the course in it.

The robot wakes at (30, 25) on a 200 by 200 mat, facing up it. A barrier runs across at y = 100 with a gap between x = 115 and x = 170. The target is the green square in the far left corner, past the barrier, and nothing about it is visible from the start. Tags 21 and 22 are on the far wall at (100, 195) and (40, 195). position() is not available: the robot has to work out where it is.

What it takes

The structure

from bugbot import *
import math
connect()

DT = 0.1
V_MAX, V_LAT = 20.0, 15.0

rates = [imu()[1] for i in range(100) if wait(DT) is None]
bias = sum(rates) / len(rates)
print("gyro bias:", round(bias, 3))

x, y, h = 30.0, 25.0, 0.0
set_cv("apriltag")

def step():
    global x, y, h
    vx, vy = flow()
    rate = imu()[1] - bias
    a = math.radians(h + 0.5 * rate * DT)
    x += (vx * math.cos(a) + vy * math.sin(a)) * DT
    y += (-vx * math.sin(a) + vy * math.cos(a)) * DT
    h += rate * DT
    plot("x", x)
    plot("y", y)
    wait(DT)

def go_to(tx, ty, tol=4.0):
    for tick in range(400):
        dx, dy = tx - x, ty - y
        gap = math.hypot(dx, dy)
        if gap < tol:
            break
        speed = max(6.0, min(13.0, 0.7 * gap))
        wx, wy = speed * dx / gap, speed * dy / gap
        a = math.radians(h)
        drive(100 * (wx * math.sin(a) + wy * math.cos(a)) / V_MAX,
              100 * (wx * math.cos(a) - wy * math.sin(a)) / V_LAT, 0)
        step()
    stop()
    step()

go_to(142.0, 70.0)
print("through the first leg, estimate", round(x, 1), round(y, 1))

Run this in the simulator

Ten seconds of standing still, not one. The precision of a mean improves only with the square root of the number of samples, and every degree per second of leftover bias becomes a degree of heading error per second of driving after that.

The fix

Both tags sit on the same wall, at y = 195, which makes the algebra short enough to do by hand. With ranges r1 to tag 21 at x1 and r2 to tag 22 at x2, subtracting the two circle equations removes y and the squares of x:

x = (x1^2 - x2^2 - r1^2 + r2^2) / (2 * (x1 - x2))
y = 195 - sqrt(r1^2 - (x - x1)^2)

The negative root, because the robot is below the wall. Average a few frames of each range before solving, and treat a frame where only one tag is visible as no fix at all rather than half of one.

Where you stop to take the fix matters. Both tags have to be inside the camera's view and inside its range, and the barrier must not be in the way, which rules out anywhere on the near side. Somewhere around (100, 150) sees both comfortably.

What a good submission looks like

Working is the floor, not the mark. A submission that would earn a good mark has:

• An estimator that runs every tick, not dead reckoning with a jump at the end.
• A stated frame. Mat coordinates, origin at the bottom left corner, heading 0 up the mat, and every number in the report in that frame.
• A plan that is visible in the code. A list of waypoints, with a sentence on why the route goes that way and what the clearance is.
• A fix that is checked before it is used. How far did the fix disagree with the estimate? Print it. A fix 40 cm from your estimate is a misread tag, not a revelation, and U6.6's gate is the right answer.
• Honest numbers at the end. The estimate, and how far out you believe it is.
• Evidence. Plots of the estimate, and a run repeated a few times with the spread reported. One successful run proves the program can pass, not that it works.

How it would be assessed

Most of the marks that get lost are in the last two rows, and both of them are decided before you write any code.

Task: the capstone

Reach the green target through the gap, without touching anything, and print gyro bias:, my x: and my y: from your own estimate. Plot x and y as you go. No position().

from bugbot import *
import math
connect()

DT = 0.1
V_MAX, V_LAT = 20.0, 15.0
START = (30.0, 25.0)
TAGS = {21: (100.0, 195.0), 22: (40.0, 195.0)}

Challenges

1. Add a gate on the fix: reject one that disagrees with the estimate by more than three times your uncertainty, and count the rejections.
2. Run it ten times and report the mean and worst final error. Which stage contributes most of it?
3. Remove the tag fix and see how far dead reckoning alone gets. Then work out what heading accuracy would have been needed to make up the difference.

Where to go next

The course stops here. The subject does not.

ROS 2 is the framework almost every robotics laboratory and most robotics companies build on. Nodes, topics, services, transforms, and a large set of existing packages: Nav2 for navigation, MoveIt for arms, and the tooling (rviz, rosbag, tf2) that makes a real system debuggable. Everything in this course exists there under a familiar name, and learning it is mostly learning where the parts live.

A real robot. Simulated sensors are polite. Real ones have connectors that come loose, timestamps that disagree, and failure modes nobody documented. Build something small, run it on a floor with people walking on it, and every lesson here will land again with more force.

The literature. Probabilistic Robotics (Thrun, Burgard and Fox) is the standard text for U4 to U9, and worth owning. Modern Robotics (Lynch and Park) is the standard for kinematics and dynamics. For the material in this module, Sutton and Barto's Reinforcement Learning is free online and readable from the first page. After that, read the conference proceedings: ICRA, IROS, RSS, CoRL. The field publishes quickly and much of the best work is on arXiv before it is anywhere else.

Keep the habits. Measure the machine rather than trusting the data sheet. Plot the signal before theorising about it. State the frame. Report the spread, not the best run. Write the simple thing, measure it, and only then make it clever. Those habits outlast every specific technique in this course.