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

Print it: worksheet (PDF) · answers (PDF) · as a web page

Guide: PID control explained

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.

Three proportional loops, for x, y and heading, feeding one drive() call through the inverse kinematicsex = TX − x× 0.7clamp ±13wxey = TY − y× 0.7clamp ±13wyeh = TH − heading(wrapped)× 2clamp ±70spininversekinematicsworld torobot framefwdlatdrive(fwd,lat,spin)the robotposition(): x, yheading()every tick
The three loops in the cell. Each error goes through its own gain and clamp. The position loops ask for a velocity in the world, and the inverse kinematics turns it into fwd and lat using the heading, so heading() is needed twice: by the heading loop, and by the inverse kinematics, fresh every tick.

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 before the loop and using it all the way through is the bug. Here the robot turns 128 degrees in the first two seconds, so a heading read at the start aims every later move the wrong way: the robot drives away from the target and into the edge of the mat. Read it every tick.

The robot's path across the mat with the heading read every tick, and with it read once before the loopleft edge of the mattargetstartheading read once:into the edgeheading readevery tick
The small arrows show which way the robot faces, once a second: it turns 128 degrees in the first two seconds. Read the heading every tick and the path runs straight to the target. Read it once at the start and every move is aimed by the old heading, so the robot heads off to the left and reaches the edge of the mat at 4.8 s.

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, and the translation drags the heading too. Mild here; on an aircraft this is the whole subject of coupled dynamics.

They should finish together. In the cell below the heading is done in about 2 s and the position takes about 9. For the seven seconds between, the drive drags the nose round from 94 degrees to 83, and nothing pulls it back, because 2.0 times 7 degrees asks for only 12 percent of spin, inside the dead band. 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(220):
    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

It parks about 4 cm from the target after about 9 s, facing 83. That is inside 6 cm and 8 degrees, the heading only just.

The x and y errors, and the heading error, over twenty two seconds of parking051015200306090cmeyexwithin 6 cm at 8 s05101520-120-600time, secondseh, degreeswithin 8° at 2 sshaded: spin under 15 %, no turningdrifts to 7°
The heading is inside 8 degrees after 2 s, the position inside 6 cm only after 8. In between, the drive drags the nose round, and any heading error under 9 degrees asks for less than 15 percent of spin, so nothing pulls it back: it ends 7 degrees out.

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. Steer by odometry() instead of position() and heading(). It counts from where the robot started, facing 0, so turn it into the mat's frame first: this robot starts facing 220. How far off is the parking now, and how far would it be after a long run?

What comes next

That last challenge is the whole of U6. Over one short park, dead reckoning only puts the robot a couple of centimetres further out, but it drifts further the longer it runs. 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.