Project: park it

Three loops at once: x, y and heading, all settling together in a tight box.

U5.7Feedback controlUniversity45 min

Do this lesson in the simulator

Three controllers at once, all running every tick, all settling together.

The robot starts at an awkward heading and has to end at a particular point, facing a particular way, within 6 cm and 8 degrees, and stay there.

The structure

each tick:
    ex = target_x - x            error in the world
    ey = target_y - y
    eh = wrapped(target_h - heading)

    wx = clamp(Kp_pos * ex)      the velocity we want, in the world
    wy = clamp(Kp_pos * ey)
    spin = clamp(Kp_head * eh)

    inverse kinematics: (wx, wy) with the heading we have now -> fwd, lat
    drive(fwd, lat, spin)

Everything in this course so far, in eight lines. The inverse kinematics from U2, the proportional control from U5.2, and the clamping from U5.6.

What makes it interesting

The frames move under you. The heading is being controlled at the same time as the position, so the rotation matrix changes every tick. Reading the heading once at the top and using it all the way through is the bug, and it produces a robot that curves in slowly instead of going straight.

The three loops interact. Rotating while translating leaks into the translation, so the position errors move because of the heading controller, not only because of themselves. Mild here; on an aircraft this is the whole subject of coupled dynamics.

They should finish together. If the heading settles in one second and the position takes ten, the robot sits there twitching its nose for nine seconds. Match the gains so that both errors go to zero at about the same time, which is a question of scaling: 90 degrees of heading error and 90 cm of position error are not the same size of problem, and their gains should not be equal.

from bugbot import *
import math
connect()

V_MAX, V_LAT, W_MAX = 20.0, 15.0, 120.0
TX, TY, TH = 80.0, 90.0, 90.0

def wrapped(a):
    return (a + 180) % 360 - 180

for tick in range(120):
    x, y = position()
    wx = max(-13, min(13, 0.7 * (TX - x)))
    wy = max(-13, min(13, 0.7 * (TY - y)))
    spin = max(-70, min(70, 2.0 * wrapped(TH - heading())))
    a = math.radians(heading())
    drive(100 * (wx * math.sin(a) + wy * math.cos(a)) / V_MAX,
          100 * (wx * math.cos(a) - wy * math.sin(a)) / V_LAT,
          100 * spin / W_MAX)
    plot("ex", TX - x)
    plot("ey", TY - y)
    plot("eh", wrapped(TH - heading()))
    wait(0.1)
stop()
print("parked at", position(), round(heading()))

Run this in the simulator

Task: park it

Stop within 6 cm of the target and within 8 degrees of heading 90, and be settled there by twenty seconds. The target is 80 across and 90 up from where the robot starts.

from bugbot import *
import math
connect()

V_MAX, V_LAT, W_MAX = 20.0, 15.0, 120.0
TX, TY, TH = 80.0, 90.0, 90.0

Challenges

  1. Plot all three errors and adjust the gains until they reach zero together.
  2. Add a dead band so the robot stops rather than trembling on the spot.
  3. Replace position() and heading() with odometry(). How far off is the parking now, and what would fix it?

What comes next

That last challenge is the whole of U6. The controller is fine; what it needs is an estimate of the state that is better than dead reckoning and more stable than a raw measurement. That is what a filter is for, and it is the next module.