Driving like a car

The unicycle and the car, the velocity constraint that makes them non-holonomic, the bracket that gets a car sideways anyway, and parallel parking.

U2.8Kinematics and framesUniversity35 min

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

U2.3 said the BugBot is holonomic and a car is not, and moved on. This lesson stays with the car, because most wheeled robots are built like one, and the ideas it needs are some of the best in Lynch and Park's Modern Robotics (chapter 13). The BugBot is the ideal test rig: it can drive like a car when you tell it to, and like itself when you want to compare.

Three robots on one mat

All three live on the mat with the same three coordinates, (x, y, heading). What differs is which velocities they can have.

• The BugBot (omnidirectional). Any forward speed, any sideways speed and any turn rate, together. Three controls for three coordinates.
• The unicycle (a differential drive, like most robot vacuums). Forward speed and turn rate, but no sideways speed: the wheels will not slide sideways. It can still turn on the spot.
• The car. No sideways speed either, and it cannot turn on the spot: it only turns while it is moving, and never tighter than its minimum turning radius. Turn rate is tied to speed: |w| <= |v| / R_MIN.

The BugBot can be each of the others. drive(fwd, 0, rot) is a unicycle: never use the middle term. For a car, tie the turn to the speed as well. This function is the car used in the rest of the lesson:

V_MAX, W_MAX, R_MIN = 20.0, 120.0, 25.0        # full scales from U1, and a car that turns no tighter than 25 cm

def car(speed, steer, seconds):
    """speed -100..100 (negative reverses), steer -1..1 (full lock left..right). Turn rate is tied to speed."""
    speed = max(-100, min(100, speed))                # a car has a top speed...
    steer = max(-1, min(1, steer))                    # ...and a full lock: steer 3 is still full lock
    v = speed / 100 * V_MAX
    w = math.degrees(v / R_MIN) * steer
    drive(speed, 0, 100 * w / W_MAX)
    wait(seconds)
    stop()
    wait(0.4)

The two clamps matter. Without them car(60, 3, 2) would turn three times as tight as R_MIN allows, and the car would stop being a car.

The dead band from U1 still applies underneath. Full lock asks for a turn of 0.38 times the speed, so at 60 percent speed it asks for 23 percent, and any steer smaller than about 0.65 asks for less than 15 percent and the car goes straight. Below about 39 percent speed even full lock is inside the dead band: the car drives straight however you steer. Steer at full lock, or not at all, and keep the speed up.

The constraint is on velocity, not on position

"No sideways speed" is a rule about velocity. In the body frame it is just vx = 0. Written in the world frame, with this course's clockwise heading, it reads

wx cos(h) - wy sin(h) = 0

Lynch and Park call a rule of this form, a linear equation in the velocities whose coefficients depend on where the robot is, a Pfaffian constraint. The important question about it is whether it also limits positions. A train on a track has a velocity constraint that does: it can never leave the track, so it has fewer places it can be. That kind is holonomic, and it takes a dimension away.

A car's constraint does not. It removes a direction the car can move in right now, but not a place it can get to: any (x, y, heading) on an open mat can be reached, with enough shuffling. The constraint is non-integrable: there is no function of (x, y, heading) whose rate of change it says is zero, so it cannot be integrated into a rule about positions. A velocity constraint that is non-integrable is non-holonomic. That is the precise meaning of the word from U2.3.

Getting sideways without going sideways

Here is a demonstration that the unicycle can reach a place directly to its side. Drive forward a little, turn, drive back the same distance, turn back. Repeat.

from bugbot import *
connect()

x0, y0 = position()
for i in range(4):
    forward(60, distance=10)
    turn_right(40, angle=30)
    backward(60, distance=10)
    turn_left(40, angle=30)
    x, y = position()
    print("cycle", i + 1, ": sideways", round(x - x0, 1), "forward", round(y - y0, 1), "heading", round(heading()))

Run this in the simulator

No command in that loop has a sideways part, and the robot ends 21 cm to the left, facing the way it started (heading 359), having moved only 5 cm forward.

Why it works: the bracket

Take one cycle with a step d and a turn θ (in radians). The back step happens with the robot turned, so it does not undo the forward step: it goes back along a slightly different line. What is left over is

sideways  d sin θ  ≈  d θ          forward  d (1 - cos θ)  ≈  d θ² / 2

For d = 10 cm and θ = 30 degrees that is 5.0 cm sideways (5.2 by the small-angle version) and 1.3 cm forward a cycle; the run shows 5.3 and 1.3. Now shrink the whole cycle by a factor ε, the step and the turn together. The sideways part d θ shrinks as ε²: it is second order in the size of the cycle. The forward part d θ² / 2 shrinks as ε³, third order. So small cycles move the robot almost purely sideways, but slowly: halve the cycle and each one gains only a quarter as much.

This is the Lie bracket of the two motions, and it can be worked out exactly. Write each thing the unicycle can do as a vector field: the rate of change of (x, y, heading) it gives for a unit input. With this course's heading, measured clockwise from +y, forward is (sin h, cos h) on the mat, so

g1 = (sin h, cos h, 0)        drive:  the BugBot's drive(60, 0, 0)
g2 = (0, 0, 1)                turn:   the BugBot's drive(0, 0, 40)

The bracket of two fields is [g1, g2] = (∂g2/∂q) g1 - (∂g1/∂q) g2, where q = (x, y, h). g2 is the same everywhere, so the first term is zero. g1 depends only on h, so the second term is its derivative with respect to h, times 1:

[g1, g2] = -(cos h, -sin h, 0) = (-cos h, sin h, 0)

The robot's right, in the world frame, is (cos h, -sin h). So the bracket points straight out of the robot's left side, with no turn in it. Drive ε, turn ε, drive back ε, turn back ε, and to second order the robot ends up ε² along [g1, g2]: moved to its left and nowhere else. That is the cell's cycle (forward, turn right, back, turn left), and it went left, by d θ a cycle. On the BugBot you can see the bracket directly: it is the direction drive(0, -70, 0) slides in, the one direction the unicycle was not given.

g1, g2 and [g1, g2] point three independent ways at every pose (the determinant of the three is -1, whatever h is), so between them they cover every velocity of (x, y, heading). Chow's theorem, which Lynch and Park use in chapter 13, then says the unicycle is small-time locally controllable: from any pose, in however short a time, it can reach every pose in a small neighbourhood of it, including the ones directly to its side. The car gets the same result. Its two moves are forward on full left lock and forward on full right lock, g1 - g2 / R_MIN and g1 + g2 / R_MIN, and their bracket is 2 / R_MIN times [g1, g2]: sideways again.

That result needs both signs of every input. The wiggle drives backwards as much as forwards and turns both ways. A car that can only go forwards can still reach any pose on an open mat, but not locally: to end up 5 cm to its side it has to drive a loop at least 2 R_MIN across. Reverse is what makes a short shuffle possible, and it is why every parking manoeuvre has reverse in it.

Parallel parking

A car cannot turn on the spot, so the wiggle above is not open to it. It uses arcs instead. Drive past the bay, then reverse in two arcs at full lock, one each way and the same length: the first swings the back of the car in towards the kerb, the second straightens it up. Two arcs of radius R, each turning by φ, move the car

sideways  2 R (1 - cos φ)          backwards  2 R sin φ

and it ends facing the way it started. This is the bracket again, built from arcs: the sideways part is second order, growing with φ² for small arcs, while the road used grows only with φ. With R = 25 cm and arcs of 30 degrees that is 6.7 cm sideways for 25 cm of road. A space too short for the whole offset in one go takes several shuffles, each a small bracket.

To park, run the formulas the other way. The sideways offset you need gives φ; φ gives the road the two arcs use, and so how far past the bay they must start; the arc length R φ and the car's speed give how long each arc takes. The formulas use the radius you asked for. The robot's own motor gains make its real arcs a little different, so leave some room.

Why the BugBot does not bother

drive(0, 70, 0) slides the BugBot the 22 cm the bay needs in about two seconds (about 11 cm/s), with no road at all: the car took three manoeuvres. That is the whole case for a holonomic drive, and the price is the one from U2.3: more actuators and a sideways axis that is slower and harder to model. For planning, the difference is large. A car's planner has to produce paths the car can drive, with no corner tighter than R_MIN and no sideways moves. U9.9 builds one.

Task: parallel park

The bay is to the robot's right, between two parked cars. All the distances are from the robot's starting point, facing along +y (heading 0).

• The bay is 18 cm wide and 47 cm long. Across, it runs from 13 cm to 31 cm to your right; along, from 10 cm behind you to 37 cm ahead. Its middle is 22 cm to your right and 13.5 cm ahead.
• The parked cars are 14 cm wide and 22 cm long, from 16 cm to 30 cm to your right. The one behind runs from 35 cm to 13 cm behind you; the one ahead from 42 cm to 64 cm ahead.
• The robot is 7 cm across, and touching a car counts from its edge, not its centre.

To pass, the robot's centre must finish inside the bay, facing within 15 degrees of the way it started (a final heading between 345 and 15), without touching either car, inside 40 seconds. Every move must go through car(), kept exactly as it is given with V_MAX, W_MAX and R_MIN unchanged: no other drive(), no sliding and no turning on the spot.

from bugbot import *
import math
connect()

V_MAX, W_MAX, R_MIN = 20.0, 120.0, 25.0

def car(speed, steer, seconds):
    """speed -100..100 (negative reverses), steer -1..1 (full lock left..right). Turn rate is tied to speed."""
    speed = max(-100, min(100, speed))
    steer = max(-1, min(1, steer))
    v = speed / 100 * V_MAX
    w = math.degrees(v / R_MIN) * steer
    drive(speed, 0, 100 * w / W_MAX)
    wait(seconds)
    stop()
    wait(0.4)

# your moves, all with car()

Challenges

1. Park in the same bay in three shuffles of shorter arcs instead of two long ones. Does it need less road?
2. Run the wiggle with θ doubled and d halved. The sideways shift per cycle stays about the same. Why, from the formula?
3. A car-like robot is told to reach a point directly to its side, 30 cm away, as fast as possible. Sketch the path. How does the fastest path change if the robot may also reverse?