Time scaling a path

Cubic and quintic time laws and what smoothness costs, the speed limit that depends on direction (this robot is fastest on the diagonal), and the time-optimal speed along a path.

U10.8Following a trajectoryUniversity40 min

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

U10.1 split a trajectory into two jobs: a path, the geometry, and a time law that says how fast to go along it. U10.2 built the trapezoid for a straight line. This lesson takes the time law further, following Lynch and Park's Modern Robotics (chapter 9): smoother laws and what they cost, and a speed limit that depends on which way the robot is going. On this robot that second one turns out to matter a lot, though not always in the way you would first guess.

A time law as a function

Lynch and Park write every point-to-point move the same way. The path is fixed, and a single number s runs from 0 at the start to 1 at the end. The time scaling s(t) says where along the path the robot should be at time t. The speed along the path is L · ds/dt for a path of length L, so the whole question of timing is the shape of one function of time.

Three shapes for a move that takes T seconds, written with u = t / T:

trapezoid (ramps of T/3)   speed rises in a straight line, holds, falls           peak speed 1.5 L/T
cubic      s = 3u² - 2u³                speed zero at both ends                  peak speed 1.5 L/T
quintic    s = 10u³ - 15u⁴ + 6u⁵        speed and acceleration zero at both ends peak speed 1.875 L/T

The cubic starts from rest but its acceleration jumps from nothing to its full value at t = 0: an infinite jerk, the rate of change of acceleration. The quintic starts with zero acceleration as well, so nothing jumps. A robot arm carrying a cup of water wants the quintic. It pays for it in peak speed: to cover the same distance in the same time with a gentler start, it must go 25 percent faster in the middle.

What the robot makes of them

Here the robot drives 60 cm three times, once with each law, over six seconds, following the plan with the feedforward and feedback of U10.5: the speed the law asks for, plus a correction for how far behind it is. Two pieces of the robot's model go into the feedforward, as U10.5 said they should. The speed per percent uses this robot's own 18.3 cm/s at full forward, which the next section measures, not the data sheet's 20. And commands below 15 percent do nothing at all, so command() rounds a small one up to 15 percent or down to zero rather than send a push the drive will ignore.

from bugbot import *
import math
connect()

L, T, DT, K = 60.0, 6.0, 0.05, 2.0
V_FWD = 18.3                            # this robot's forward speed at 100 percent, measured in the next section

def trapezoid(u, r=1 / 3):              # s and ds/du, for u = t/T from 0 to 1
    vp = 1 / (1 - r)
    if u < r:
        return 0.5 * vp * u * u / r, vp * u / r
    if u < 1 - r:
        return 0.5 * vp * r + vp * (u - r), vp
    w = 1 - u
    return 1 - 0.5 * vp * w * w / r, vp * w / r

def cubic(u):
    return 3 * u**2 - 2 * u**3, 6 * u - 6 * u**2

def quintic(u):
    return 10 * u**3 - 15 * u**4 + 6 * u**5, 30 * u**2 - 60 * u**3 + 30 * u**4

def command(v):                         # a speed in cm/s as a percent, past the 15 percent dead band
    c = 100 * v / V_FWD
    if abs(c) < 7.5:
        return 0                        # too slow to be worth a push: stay still
    return max(-100, min(100, math.copysign(max(15, abs(c)), c)))

for name, law in (("trapezoid", trapezoid), ("cubic", cubic), ("quintic", quintic)):
    y0 = position()[1]
    worst, peak, t = 0.0, 0.0, 0.0
    while t < T + 1.0:
        s, ds = law(min(t / T, 1.0))
        v = L * ds / T if t < T else 0.0          # feedforward: the speed the law asks for
        gap = L * s - (position()[1] - y0)          # feedback: how far behind the plan the robot is
        forward(command(v + K * gap))
        peak, worst = max(peak, v), max(worst, abs(gap))
        wait(DT)
        t += DT
    stop()
    wait(1.0)
    print(name.ljust(9), "asks for up to", round(peak, 1), "cm/s; worst gap from the plan", round(worst, 1), "cm")
    backward(100, distance=L)
    wait(0.5)

Run this in the simulator

All three follow within two centimetres, and the quintic is the worst of them, not the best. Its peak of 18.7 cm/s is just past this robot's top speed forward, 18.3 cm/s, so for a moment there is no speed left for the feedback to use. Squeeze the move into four seconds and the peaks become 22.5, 22.5 and 28.1 cm/s. None of the three is possible any more, and the gaps grow to between 8 and 15 cm.

The quintic's other advantage does not show up here either. The drive's 0.25 s lag already smooths every command: a jump in the speed asked for becomes a rise over a quarter of a second, so the cubic's infinite jerk never reaches the robot. On an arm whose motors follow their commands closely, that jerk is real and the quintic earns its keep.

That is the first rule of time scaling, and it matters more than the shape: the law must fit inside the machine's limits. A speed limit V and an acceleration limit A each set a shortest time. The cubic's peak speed is 1.5 L / T and its peak acceleration, at the very start, is 6 L / T², so

cubic     T >= max( 1.5 L / V ,   sqrt(6 L / A) )
quintic   T >= max( 1.875 L / V , sqrt(5.77 L / A) )

The quintic's peak acceleration, 10 / sqrt(3) ≈ 5.77 L / T², comes a fifth of the way in, and is slightly lower than the cubic's. For the 60 cm move at 18.3 cm/s and 15 cm/s², the cubic needs 4.9 s by speed and 4.9 s by acceleration: both limits bite at once. Smoothness is bought with time.

The speed limit depends on direction

So far "the machine's limit" has been one number. For this robot it is not. The forward axis tops out at V_MAX and the sideways axis at V_LAT, separately, so the set of body velocities the robot can have is a rectangle:

|vy| <= V_MAX (forward)        |vx| <= V_LAT (sideways)

The fastest direction is a corner of the rectangle, not an axis. Full forward and full sideways together gives sqrt(20² + 15²) = 25 cm/s, at atan(15 / 20) = 37 degrees off forward. Going straight forward only gives 20. There are four corners, at 37 degrees either side of forward and either side of backward, all equally fast. In general, the top speed in a direction φ from forward is whichever axis runs out first:

v_max(φ) = min( V_MAX / |cos φ| ,  V_LAT / |sin φ| )

from bugbot import *
import math
connect()

for name, cmd in (("forward", (100, 0, 0)), ("sideways", (0, 100, 0)), ("both", (100, 100, 0))):
    drive(*cmd)
    wait(1.0)                                   # past the lag
    xa, ya, ha = *position(), heading()
    wait(2.0)
    xb, yb, hb = *position(), heading()
    stop()
    wait(0.6)
    h = ha + ((hb - ha + 180) % 360 - 180) / 2  # the robot turns a little as it goes: measure from its average heading
    print(name.ljust(8), "speed", round(math.hypot(xb - xa, yb - ya) / 2.0, 1), "cm/s,",
          round((math.degrees(math.atan2(xb - xa, yb - ya)) - h + 180) % 360 - 180), "degrees from forward")
    drive(*[-c for c in cmd])                   # and back again, to make room for the next one
    wait(3.0)
    stop()
    wait(0.6)

Run this in the simulator

On this robot the measured figures are 18.3 forward, 16.0 sideways and 24.6 cm/s on the diagonal, at 42 degrees. The diagonal is a third faster than straight ahead. It is at 42 degrees, not the data sheet's 37, because this robot's sideways axis is relatively stronger: atan(16.0 / 18.3) is 41 degrees, and the measurement is within a degree of it. Lynch and Park make the same point for robot arms, where the limits are on each joint's speed and torque: the fastest way along a path depends on where along it you are, because the limits in the path's direction change as the machine's configuration changes. Here the configuration that changes is the heading, and the robot is free to choose it.

Time-optimal along a path

Put both ideas together for a path with corners, such as the one U9.7 planned. At every point along the path there is a top speed, v_max(φ) for the direction of travel there, and there is an acceleration limit you choose, as in U10.2. The fastest time law is the one that is as fast as both allow everywhere, and there is a simple way to find it:

1. Forward pass. From the start, go along the path in small steps. At each step the speed may rise by no more than the acceleration allows (v² ≤ v_prev² + 2 A Δs), and may not pass the limit there.
2. Backward pass. From the end, do the same in reverse, so the robot can always brake in time for a slow section or the finish.
3. The time-optimal speed is the smaller of the two at every point.

Lynch and Park draw this in the phase plane of (s, ṡ): position along the path across, speed along it up. In their general case the acceleration a robot arm can manage depends on where it is and how fast it is going, so each point of the plane has a lowest and a highest possible s̈. Where the lowest would have to exceed the highest, no motion is possible, and that boundary is the velocity limit curve. The fastest law accelerates as hard as it can, then brakes as hard as it can, and it generally touches the limit curve only at the points where it switches from one to the other. The two passes above are the special case this robot needs: a constant acceleration limit A, and a speed cap that depends only on where you are. Then the fastest law can ride the cap between the switches, and its shape is bang-coast-bang: flat out, cruise, flat out braking. That is the trapezoid of U10.2, bent to fit a cap that changes along the way.

The cell below plans the route three ways. The robot comes to rest at every corner, so a change of heading means a turn on the spot there, and a turn takes time too. The cell measures that time on the robot rather than guessing it. For the fast diagonal, each leg can use any of the four corners of the box, and the cell takes whichever needs the least turn from the heading the robot already has.

from bugbot import *
import math
connect()

V_MAX, V_LAT, A = 20.0, 15.0, 15.0              # full scales, and an acceleration limit you choose (cm/s²)
ROUTE = [(30, 30), (30, 125), (100, 125), (100, 75), (170, 75), (170, 170)]   # a route with square corners
BOX = math.degrees(math.atan2(V_LAT, V_MAX))    # 36.9: the corners of the box are this far either side of forward and back

def v_limit(phi):
    """Top speed moving at phi degrees from the body's forward."""
    c, s = abs(math.cos(math.radians(phi))), abs(math.sin(math.radians(phi)))
    return min(V_MAX / c if c > 1e-9 else 1e9, V_LAT / s if s > 1e-9 else 1e9)

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

def time_optimal(headings, ds=1.0):
    """headings(course) lists the headings allowed on a leg; each leg takes the one that needs the least turn."""
    h, turns, lim = 0.0, [], [0.0]              # the top speed at every point, 1 cm apart; zero at each corner
    for (ax, ay), (bx, by) in zip(ROUTE, ROUTE[1:]):
        course = math.degrees(math.atan2(bx - ax, by - ay))
        new = min(headings(course), key=lambda k: abs(wrap(k - h)))
        turns.append(round(abs(wrap(new - h)), 1))
        h = new
        lim += [v_limit(course - h)] * (round(math.hypot(bx - ax, by - ay) / ds) - 1) + [0.0]
    fwd = lim[:]                                # forward pass: how fast it could be, accelerating from the start
    for i in range(1, len(fwd)):
        fwd[i] = min(fwd[i], math.sqrt(fwd[i - 1] ** 2 + 2 * A * ds))
    back = fwd[:]                               # backward pass: slow enough to brake for everything ahead
    for i in range(len(back) - 2, -1, -1):
        back[i] = min(back[i], math.sqrt(back[i + 1] ** 2 + 2 * A * ds))
    return sum(2 * ds / (back[i] + back[i + 1]) for i in range(len(back) - 1)), turns

measured = {}
def turn_time(deg):
    """How long this robot takes to turn by deg, measured once and then remembered."""
    if deg < 0.5:
        return 0.0
    if deg not in measured:
        t0 = clock()
        turn_right(60, angle=deg)
        measured[deg] = clock() - t0
        turn_left(60, angle=deg)                # and back, ready for the next one
    return measured[deg]

for name, headings in (("facing up the mat all the way", lambda c: [0.0]),
                       ("facing along each leg", lambda c: [c, c + 180]),
                       ("a fast diagonal on each leg", lambda c: [c - BOX, c + BOX, c + 180 - BOX, c + 180 + BOX])):
    drive_s, turns = time_optimal(headings)
    turn_s = sum(turn_time(d) for d in turns)
    print(name.ljust(30), round(drive_s, 1), "s driving +", round(turn_s, 1), "s turning =", round(drive_s + turn_s, 1), "s   turns:", turns)

Run this in the simulator

The driving times are 27.3, 25.7 and 23.5 seconds. Holding the heading at zero, as U9.7 did, makes the two sideways legs slow. Turning to face along each leg makes every leg a 20 cm/s leg. Turning so that each leg runs along a fast diagonal makes every leg a 25 cm/s leg. The gain is smaller than the jump from 20 to 25 cm/s suggests, because on these short legs the acceleration limit, not the speed limit, is what holds the robot back for much of the time.

Then the turns are added, and the order changes. Facing along each leg needs a 90 degree turn at every corner, 2 s each, and becomes the slowest of the three. The diagonal is cleverer: by picking the mirrored corner of the box, it gets away with 16 degrees at each corner after a first turn of 37. But even a small turn costs this robot most of a second, because the lag has to spin it up and let it settle whatever the angle, and five of them add 4.9 s. That is more than the 3.8 s the diagonal saves, so on this route, with a stop at every corner, holding the heading wins: 27.3 s against 28.4. The fast direction pays on long legs, where there is time to use it, and not on a route that stops and turns every metre.

Driving the plan

The plan is only worth something if the robot can follow it. This cell runs the forward and backward passes along one straight metre of the fast diagonal, with this robot's measured speeds, and follows the result the way the first cell followed its laws: the speed the plan asks for, plus a correction for the gap. The same percent goes to both axes, so the robot keeps moving along the corner of its box at any speed.

from bugbot import *
import math
connect()

V_FWD, V_SIDE, A = 18.3, 16.0, 15.0             # this robot's measured speeds, and the acceleration limit
LEG, DT, K = 100.0, 0.05, 2.0                   # 1 m along the fast diagonal
V_TOP = math.hypot(V_FWD, V_SIDE)               # full forward and full sideways together
BOX = math.atan2(V_SIDE, V_FWD)                 # the direction that gives, right of forward

# the forward and backward passes along one straight leg, 1 cm apart
n = round(LEG)
v = [0.0] + [V_TOP] * (n - 1) + [0.0]
for i in range(1, n + 1):
    v[i] = min(v[i], math.sqrt(v[i - 1] ** 2 + 2 * A))
for i in range(n - 1, -1, -1):
    v[i] = min(v[i], math.sqrt(v[i + 1] ** 2 + 2 * A))
times = [0.0]                                   # when the plan reaches each centimetre
for i in range(n):
    times.append(times[-1] + 2 / (v[i] + v[i + 1]))

def plan(t):
    """Where the plan is at time t, and how fast it is going."""
    if t >= times[-1]:
        return LEG, 0.0
    i = max(k for k in range(n) if times[k] <= t)
    f = (t - times[i]) / (times[i + 1] - times[i])
    return i + f, v[i] + f * (v[i + 1] - v[i])

def command(speed):                             # a speed along the diagonal as a percent on both axes, past the dead band
    c = 100 * speed / V_TOP
    if abs(c) < 7.5:
        return 0
    return max(-100, min(100, math.copysign(max(15, abs(c)), c)))

x0, y0 = position()
ux, uy = math.sin(BOX), math.cos(BOX)           # the diagonal on the mat, for a robot facing up it
t, worst, passed = 0.0, 0.0, {}
while t < times[-1] + 1.5:
    s, speed = plan(t)
    x, y = position()
    along = (x - x0) * ux + (y - y0) * uy       # how far along the diagonal the robot really is
    gap = s - along
    c = command(speed + K * gap)
    drive(c, c, 0)
    plot("planned", s)
    plot("measured", along)
    worst = max(worst, abs(gap))
    for mark in (25, 50, 75, 95):
        if mark not in passed and along >= mark - 0.5:
            passed[mark] = t
    wait(DT)
    t += DT
stop()
for mark in (25, 50, 75, 95):
    print("at", mark, "cm: planned", round(times[mark], 2), "s, the robot", round(passed.get(mark, float("nan")), 2), "s")
print("the plan's top speed", round(max(v), 1), "cm/s; worst gap from the plan", round(worst, 1), "cm; stopped at", round(along, 1), "cm")

Run this in the simulator

The plan takes 5.73 s: 1.6 s up to 24.3 cm/s, a cruise, and 1.6 s back down. The robot runs about a tenth of a second behind it on the way up and through the cruise, which is the lag, and slightly ahead of it at 95 cm, because braking comes late through the same lag. It is never more than 2.4 cm from the plan, and it stops within a millimetre of the end.

Task: dash to the corner

The far corner of the mat is about two metres away, on the diagonal. Be in it by 8.5 seconds and stay there until the clock passes 11, then let the program end; the run is cut off at 12. Facing the corner and driving flat out gets there after about 11 seconds on this robot, which is too slow. Work out which way to face so that the robot's fastest direction points at the corner, turn to face it, and go.

Two things will pull it off the line. Aim with your own measured speeds, not the data sheet's: they put the corner of the box at 41 degrees rather than 37, and a few degrees of aim is 10 to 15 cm at two metres. And even a perfect aim will not stay perfect, because the vibration drive turns the robot slowly as it goes, and leaks a little of its forward push sideways. So steer as you drive: compare the direction the robot is actually moving with the direction to the target, and turn a little to close the gap.

from bugbot import *
import math
connect()

V_FWD, V_SIDE = 20.0, 15.0                   # replace with the speeds you measured
START, TARGET = (30.0, 30.0), (170.0, 170.0)

def here():
    px, py = position()
    return START[0] + px, START[1] + py

The figure shows the two runs against the clock.

Challenges

1. Add the last cell's acceleration limit to the dash, so the robot follows a trapezoid along the diagonal instead of going flat out at once. How much time does 15 cm/s² cost, and does it still make 8.5 s?
2. Drive the whole route from the third cell with the heading held at zero, following the time-optimal speed leg by leg as the last cell did for one. Does the time the robot takes match the prediction?
3. The diagonal lost on that route because its turns cost more than it saved. Make the legs longer, or the acceleration limit higher, in the third cell: at what point does the fast diagonal start to pay for its turns?