Cross-track error
Splitting the error into along and across the path, and a controller for the half that matters.
Do this lesson in the simulatorThe robot is at one point and the path is somewhere near it. The error between them is a vector, and the single most useful thing you can do with it is to split it in two.
- Along-track error: how far ahead of or behind the reference the robot is, measured along the path.
- Cross-track error: how far off the path it is, measured at right angles to it.
They are different quantities with different consequences and different controllers. Being 10 cm behind is a timing problem: you arrive late. Being 10 cm to the side is a geometry problem: you are in the wrong place, and on a factory floor that is the difference between a lane and a shelf.
The path frame
For a straight leg from A to B, build two unit vectors:
ux, uy = (bx - ax) / length, (by - ay) / length # along the path
nx, ny = -uy, ux # ninety degrees to the left of it
Then, for a robot at (x, y):
along = (x - ax) * ux + (y - ay) * uy
cross = (x - ax) * nx + (y - ay) * ny
Two dot products. along says how far down the leg the robot has got, and it is exactly the arc length from U10.1. cross is signed, which is the point: it says which side the robot is on, and a controller needs the sign or it cannot know which way to push.
Getting the sign convention wrong gives a controller that drives the error to infinity instead of to zero. It is an unmistakable failure and it happens to everybody once.
Controlling it
On a car you cannot move sideways, so the only handle on the cross-track error is the steering, and the loop is second order: steering changes heading, heading changes lateral velocity, lateral velocity changes the error. Two integrators, and therefore the possibility of oscillation. The Stanley controller, which won the DARPA Grand Challenge, is the standard answer:
steering = heading error + atan(k * cross / speed)
Note the speed in the denominator. At high speed a given cross-track error needs a gentler correction, because the geometry gets to it sooner. A cross-track gain that is not scaled with speed is a robot that is calm at walking pace and unstable on the motorway.
The BugBot is holonomic, so it is easier and more interesting: you can simply add a sideways velocity.
correction = -k * cross # cm/s, across the path
world_velocity = along_unit * cruise + normal_unit * correction
One integrator, not two: velocity to position. That loop is first order and cannot oscillate on its own, whatever the gain. Everything that makes it oscillate in practice is a delay: the 0.25 s lag in the drive, the loop period, and the dead band that makes small corrections do nothing until they suddenly do something.
Choosing the gain
The first order loop has a time constant of 1 / k. At k = 0.9 the error decays with a time constant of about 1.1 s, which at 11 cm/s means it is corrected over roughly 12 cm of travel. That is a sensible shape: fast enough to look deliberate, slow enough that the lag does not matter.
Raise k and two things happen. The correction saturates (there is only 15 cm/s of sideways speed available) so the response stops getting faster. And the phase lost to the 0.25 s lag starts to matter, so the approach becomes a wobble.
Always clamp the correction, and always leave forward speed. A robot that spends all its authority on the cross-track error stops making progress along the path, and a robot that is exactly on the path but not moving has not followed anything.
from bugbot import *
import math
connect()
START, AX, AY, BX, BY = (85.0, 30.0), 60.0, 30.0, 60.0, 170.0
ux, uy = 0.0, 1.0
nx, ny = -uy, ux
for tick in range(120):
px, py = position()
x, y = START[0] + px, START[1] + py
cross = (x - AX) * nx + (y - AY) * ny
plot("cross track", cross)
correction = max(-12.0, min(12.0, -0.9 * cross)) # cm/s along the LEFT normal
# the robot faces +y, so its right is world +x: the normal's x part is the sideways command
drive(100 * 10.0 / 20.0, 100 * correction * nx / 15.0, 0)
wait(0.1)
stop()
The chart is the lesson: a curve from -25 cm to nothing, with the shape of an exponential decay, because that is exactly what it is. The sign is negative because the robot starts to the right of the path and the normal points left.
Note the nx in the drive() call. The correction is a speed along the path's left normal, and drive()'s sideways term is positive to the robot's right. Send the correction straight in and the robot moves away from the line at full sideways speed. Converting from the path frame to the body frame is not optional, and this is the bug it produces when it is skipped.
The dead band, again
Below about 15 percent command the vibration drive does nothing at all. So a cross-track error of 1 cm asks for 0.9 cm/s, which asks for 6 percent, which does nothing. The robot sits 1 cm off the path for ever and the controller is not broken, it is just talking to a drive that cannot hear it.
The standard fix is a dead band inverse: anything below the threshold is either rounded up to the threshold or set to zero.
def band(u):
if abs(u) < 5:
return 0.0
return max(17.0, min(100.0, u)) if u > 0 else min(-17.0, max(-100.0, u))
This buys you authority at small errors, at the price of a small limit cycle: the robot now hunts a centimetre either side instead of sitting quietly a centimetre off. Which of those you prefer is a real engineering choice, and it depends entirely on what the error is for.
Task: drive the cross-track error to zero
The taped line runs from (60, 30) to (60, 170). The robot starts at (85, 30), which is 25 cm to the right of it. Get onto the line, drive along it to the far end, plot cross track the whole way, and stop near (60, 163).
from bugbot import *
import math
connect()
DT = 0.1
START = (85.0, 30.0)
AX, AY, BX, BY = 60.0, 30.0, 60.0, 170.0
Challenges
- Try
kat 0.2 and at 4.0 and describe both. Which one is the lag showing? - Plot the along-track value as well and check it climbs steadily while the cross-track collapses.
- Remove the dead band inverse. What is the smallest cross-track error the robot can actually correct?