Planning for a car
Paths a car can drive: Dubins curves, A* over the car's own moves in position and heading, and replanning after every move so the errors never add up.
Do this lesson in the simulatorPrint it: worksheet (PDF) · answers (PDF) · as a web page
Every planner in this module so far returned a path made of straight lines between points, and U9.7 drove it with the robot sliding sideways round each corner. The BugBot can. The car from U2.8 cannot: it cannot slide, it cannot turn on the spot, and it cannot turn tighter than R_MIN. Hand it a grid path and the first corner is impossible. This lesson plans paths a car can drive, following Lynch and Park's Modern Robotics (chapters 10 and 13).
Why a grid path will not do
A grid path turns by 45 or 90 degrees in a single cell. A car doing 90 degrees needs a quarter circle of radius at least 25 cm: it swings out a long way from the corner the grid chose. And the grid never asked which way the car was facing, which for a car matters as much as where it is. A path for a car is a path in (x, y, heading), with every piece one the car can actually drive.
The shortest car path: Dubins
In 1957 Lester Dubins answered the clean version of the question. A car that only drives forwards, at one speed, with a minimum turning radius R, wants the shortest path from one pose to another. The answer is always made of at most three pieces, each a full-lock arc to the left (L), a full-lock arc to the right (R), or a straight (S), and it is one of six words:
LSL RSR LSR RSL RLR LRL
Try all six, keep the shortest. The four with a straight in the middle are two circles and a tangent line between them. The two curvy ones, RLR and LRL, can only be the shortest when the two positions are less than 4R apart; further apart than that, only the four CSC words need trying. If the car may also reverse, the same idea gives the Reeds and Shepp paths, up to five pieces with changes of direction.
Dubins paths are exact and fast, and they know nothing about obstacles. On an empty mat they are the answer. With a wall in the way you need search.
Searching over the car's moves
Lynch and Park's way in (section 10.4.2) is to discretise the controls rather than the space. The car has three moves, each held for a fixed time: straight, full lock left and full lock right. From any pose, those three moves lead to three new poses. From each of those, three more. That is a tree, and its branches are, by construction, paths the car can drive.
Each move's end pose comes from the exact step of U3.8: a steady twist for a known time is an arc you can compute.
from bugbot import *
import math
connect()
V_MAX, R_MIN = 20.0, 25.0
SPEED, STEP_S = 60, 1.6 # every move: 60 percent for 1.6 s, about 19 cm
START = (50.0, 40.0)
def arc(x, y, h, steer, seconds, n=8):
"""Where car(SPEED, steer, seconds) goes, by the exact step of U3.8, as n points along the way."""
v = SPEED / 100 * V_MAX
w = math.degrees(v / R_MIN) * steer
pts = []
for k in range(n):
dt = seconds / n
a, wr = math.radians(h), math.radians(w)
if abs(wr) < 1e-9:
C, S = math.cos(a) * dt, math.sin(a) * dt
else:
C = (math.sin(a + wr * dt) - math.sin(a)) / wr
S = (math.cos(a) - math.cos(a + wr * dt)) / wr
x, y, h = x + v * S, y + v * C, (h + w * dt) % 360
pts.append((x, y, h))
return pts
# every pose the car can reach in three moves: 3, then 9, then 27 branches
level, tree = [(START[0], START[1], 0.0)], []
for depth in range(3):
nxt = []
for x, y, h in level:
for steer in (-1, 0, 1):
pts = arc(x, y, h, steer, STEP_S)
tree += [(px, py) for px, py, _ in pts]
nxt.append(pts[-1])
level = nxt
print("after", depth + 1, "moves:", len(level), "poses")
draw("tree", tree, "blue", "dots", 1.5)
The blue fan on the mat is everything the car can do in about five seconds. Every point in it comes with a heading, and the fan's edges are the full-lock circles.
A planner searches this tree with A. The state is (x, y, heading). Two states that land in the same small cell of position and heading count as the same, so the tree does not grow forever (without that, three moves a level is 3 to the power 13 after thirteen moves, 1.6 million branches). The cost is the distance driven, with turning charged a little more. The heuristic is the straight-line distance to the goal less 8 cm, because the search stops anywhere within 8 cm of the goal: the plain straight-line distance could overestimate by up to 8 cm, and a heuristic that overestimates can cost A the shortest path. Less the 8 cm, it ignores heading and the turning circle and never overestimates.
Merging states into cells has a price, and it is more than a little optimality. Only one state is kept per cell, so a path that needed a state the merge threw away is lost, and the search can report no path when there is one. It misses fewer as the cells and the moves shrink, but no size of cell promises to miss none. In exchange the search finishes in a few milliseconds, which is the usual trade. Searching over the controls and keeping one state per cell is the grid method with motion constraints that Lynch and Park describe in 10.4.2, after Barraquand and Latombe. Keeping that state's exact continuous pose rather than the centre of its cell, as this planner does, is the same idea, known as Hybrid A* (Dolgov and others, 2008). A better heuristic would be the length of the Dubins path, which is much closer to the truth; Challenge 2 is about keeping it a lower bound.
The cell below plans, then drives the whole plan with car(). This car() differs from U2.8's in two ways. First, it steers by curvature. U2.8's works out the turn rate from the speed the car is told to make, and this robot makes about 10 percent less, so at full lock it turns on a circle tighter than R_MIN. This one starts each move from the speed and the turn it learned on the last, then measures the speed and the turn rate every tenth of a second and trims the turn until the turn rate is the measured speed over R_MIN. Once a turn has built up, that holds the radius within about 3 cm of 25. For the first half second of a turn it is wider, because the robot takes about a quarter of a second to answer a new command, and just after a swap from left to right it runs a little tight, down to about 20 cm, for half a second. It keeps the speed it measured in V_SEEN and the turn it needed in TURN, and arc() uses V_SEEN, so a plan made after some driving uses the robot's real speed. Second, it does not stop() and wait(0.4) after each move. In U2.8 the pause kept each manoeuvre separate so you could watch it; here the moves run back to back, as a car's do, and every stop would add a standing start that arc() knows nothing about.
from bugbot import *
import heapq, math
connect()
V_MAX, W_MAX, R_MIN = 20.0, 120.0, 25.0
SPEED, STEP_S = 60, 1.6 # every move: 60 percent for 1.6 s, about 19 cm on paper
INFLATE = 9.0
START = (50.0, 40.0)
GOAL, GOAL_H = (155.0, 40.0), 180.0 # the far side of the wall, facing back down the mat
WALLS = [(95.0, 0.0, 10.0, 120.0)]
V_SEEN = SPEED / 100 * V_MAX # the speed the model uses; car() replaces it with the one it measures
TURN = 100 / W_MAX # rot per degree a second of turn; car() learns the real figure as it drives
def free(x, y, margin=INFLATE):
if x < margin or y < margin or x > 200 - margin or y > 200 - margin:
return False
return not any(ox - margin <= x <= ox + ow + margin and oy - margin <= y <= oy + oh + margin
for ox, oy, ow, oh in WALLS)
def arc(x, y, h, steer, seconds, n=8):
"""Where car(SPEED, steer, seconds) goes at V_SEEN on a circle of R_MIN, as n points along the way."""
v = V_SEEN
w = math.degrees(v / R_MIN) * steer
pts = []
for k in range(n):
dt = seconds / n
a, wr = math.radians(h), math.radians(w)
if abs(wr) < 1e-9:
C, S = math.cos(a) * dt, math.sin(a) * dt
else:
C = (math.sin(a + wr * dt) - math.sin(a)) / wr
S = (math.cos(a) - math.cos(a + wr * dt)) / wr
x, y, h = x + v * S, y + v * C, (h + w * dt) % 360
pts.append((x, y, h))
return pts
def here():
px, py = position()
return START[0] + px, START[1] + py, heading()
def car(speed, steer, seconds, dt=0.1):
"""Drive at speed, steer -1..1, and trim the turn every dt so the radius stays R_MIN at the speed measured."""
global V_SEEN, TURN
speed, steer = max(-100, min(100, speed)), max(-1, min(1, steer)) # top speed and full lock, as in U2.8
v = V_SEEN * speed / SPEED # the speed the last move measured
rot = TURN * math.degrees(v / R_MIN) * steer # the turn that gives R_MIN at it
for k in range(round(seconds / dt)):
(x0, y0), h0 = position(), heading()
drive(speed, 0, rot)
wait(dt)
(x1, y1), h1 = position(), heading()
v = math.copysign(math.hypot(x1 - x0, y1 - y0) / dt, speed)
w = ((h1 - h0 + 180) % 360 - 180) / dt
if k >= 3: # the robot takes about 0.3 s to answer a new command
rot += 0.2 * (math.degrees(v / R_MIN) * steer - w) * TURN
V_SEEN += 0.2 * (abs(v) - V_SEEN)
if steer and seconds >= 4 * dt: # keep what the trim learned for the next move's first guess
TURN += 0.5 * (rot / (math.degrees(V_SEEN / R_MIN) * steer) - TURN)
def plan(sx, sy, sh):
"""A* over the car's three moves: the steers from (sx, sy, sh) to the goal, [] if already there, None if no way."""
def cell(x, y, h):
return (round(x / 6), round(y / 6), round(h / 45) % 8)
def cost_to_go(x, y):
return max(0.0, math.hypot(GOAL[0] - x, GOAL[1] - y) - 8) # the goal counts from 8 cm away
q = [(cost_to_go(sx, sy), 0.0, (sx, sy, sh), [])]
seen = set()
while q:
f, g, (x, y, h), moves = heapq.heappop(q)
if math.hypot(GOAL[0] - x, GOAL[1] - y) < 8 and abs((h - GOAL_H + 180) % 360 - 180) < 25:
return moves
c = cell(x, y, h)
if c in seen:
continue
seen.add(c)
for steer in (0, -1, 1):
pts = arc(x, y, h, steer, STEP_S)
if all(free(px, py) for px, py, _ in pts):
nx, ny, nh = pts[-1]
step = V_SEEN * STEP_S * (1.0 if steer == 0 else 1.15) # turning costs a little more
heapq.heappush(q, (g + step + cost_to_go(nx, ny), g + step, (nx, ny, nh), moves + [steer]))
return None
moves = plan(*here())
print("plan:", len(moves), "moves:", moves)
x, y, h, path = START[0], START[1], 0.0, []
for m in moves:
pts = arc(x, y, h, m, STEP_S)
path += [(px, py) for px, py, _ in pts]
x, y, h = pts[-1]
print("on paper it ends at", round(x), round(y), "facing", round(h))
draw("plan", [START] + path, "green", "line")
for m in moves: # now drive all of it, one move after another
car(SPEED, m, STEP_S)
stop()
wait(0.5) # let it come to rest
x, y, h = here()
print("the robot ends at", round(x), round(y), "facing", round(h), "which is", round(math.hypot(GOAL[0] - x, GOAL[1] - y)), "cm from the goal")
print("it really drove at", round(V_SEEN, 1), "cm/s, not", SPEED / 100 * V_MAX)
Thirteen moves: three straights up the left of the wall, then a U-turn over the top of it made of four right-hand arcs of about 44 degrees each, with a full straight move (about 19 cm) between each pair, then three straights down the other side. On paper it ends at (151, 44) facing 176, inside the goal's 8 cm and 25 degrees. Every piece is a car move, so the car can drive it exactly as planned. In principle.
Driving the plan: feedback by planning again
Driven one move after another, the thirteen moves take this robot to (154, 55) facing 163: 15 cm from the goal, just above the bay rather than in it. The plan assumed 12 cm/s, 19.2 cm a move, and this robot made 10.8 cm/s, 17.3 cm a move. The steering held the radius near 25 cm, so each arc, being shorter, turned about 40 degrees instead of 44. The four arcs came to about 160 degrees, not 176, so the car came down the far side off line, and every one of the thirteen moves fell short. A car cannot correct sideways, so none of that comes back.
Most of that miss is the wrong speed. Drive one straight move first, so car() measures V_SEEN, and plan with that: the same planner then brings this robot to 7 cm from the goal, in the bay, facing 159. It is not a cure. It still has nobody watching the errors add up, and across twenty simulated robots it lands only six of them.
The fix used here is to plan again. Drive only the first move of the plan, read where the robot really is, and plan from there, with the speed car() has now measured. The next plan quietly absorbs the error from the last move. This is receding horizon control, the idea behind model predictive control, and it is how many real planners are run: the plan is never followed to the end, only its first step, over and over. It is not the only way to close the loop. Lynch and Park's main answer for driving a planned path is feedback trajectory tracking (chapter 13): keep the plan, and let a controller steer the car back onto it as it goes. Replanning throws the old plan away each time; tracking keeps it and corrects towards it.
Replanning fixes where the car goes, but not which way it can finish facing. Each full-lock move turns about 40 degrees, so a plan made of the three moves can face 160 or 200 in the bay, never 180. The lesson's plan() gets away with that because its goal allows 25 degrees, which leaves the car almost nothing for the few degrees it drifts on its last move. Give the search the same three moves again, each held for half as long (STEP_S / 2, about 20 degrees of turn), and 180 is in reach. That is six moves, each a (steer, seconds) pair, and the planner can then ask for 15 degrees and still leave the car 10 to spare. Now and then the last move lands a few degrees outside the planner's 15, right beside the goal, where no short path can fix the heading: plan() reports no path and the robot stops, in the bay and inside the task's 25. That is why the task allows more than the planner asks for.
Drift can also carry the robot a centimetre or two into the 9 cm margin round the wall, usually as it goes over the top. From there every move starts at a point free() rejects, so plan() finds no path and the run stops beside the wall. The way out is to check the first move from where the robot really is against half the margin (free(x, y, INFLATE / 2)), which lets it steer back out, and every move after that against the whole margin.
Task: a U-turn round the wall, driven like a car
The green bay is on the other side of the wall, a 24 cm square round the goal, and the car must finish in it facing back down the mat (180 degrees, within 25). Driving one plan to the end misses it, as above.
Write plan(sx, sy, sh), an A* search over the car's six moves: straight, full lock left (steer -1) and full lock right (steer 1), each held for STEP_S and for STEP_S / 2 (write the half length as STEP_S / 2 in your code, not as a number, so the checker can find it). It takes the robot's pose on the mat, x and y in cm and the heading in degrees, and returns the moves to the goal as a list of (steer, seconds) pairs, [] if the robot is already there, or None if there is no way. A move costs the distance it drives, V_SEEN * seconds, with turning a little more. A state has arrived when it is within 8 cm of the goal and 15 degrees of 180. The first move from the robot's own pose is checked against half the margin, every later one against the whole of it.
Then write the loop. Each time round, plan from here(), print plan <n> has <k> moves (n counts the plans, k is how many moves this one has), drive only the first move with car(SPEED, steer, seconds), and plan again. Stop when the plan comes back empty, and say so if it comes back with no path at all: those are different endings. No sliding, no turning on the spot, no touching the wall, and no list of moves typed in by hand. free(), arc(), here() and car() are written for you.
from bugbot import *
import heapq, math
connect()
V_MAX, W_MAX, R_MIN = 20.0, 120.0, 25.0
SPEED, STEP_S = 60, 1.6 # every move: 60 percent for 1.6 s, about 19 cm on paper
INFLATE = 9.0
START = (50.0, 40.0)
GOAL, GOAL_H = (155.0, 40.0), 180.0 # the far side of the wall, facing back down the mat
WALLS = [(95.0, 0.0, 10.0, 120.0)]
V_SEEN = SPEED / 100 * V_MAX # the speed the model uses; car() replaces it with the one it measures
TURN = 100 / W_MAX # rot per degree a second of turn; car() learns the real figure as it drives
def free(x, y, margin=INFLATE):
if x < margin or y < margin or x > 200 - margin or y > 200 - margin:
return False
return not any(ox - margin <= x <= ox + ow + margin and oy - margin <= y <= oy + oh + margin
for ox, oy, ow, oh in WALLS)
def arc(x, y, h, steer, seconds, n=8):
"""Where car(SPEED, steer, seconds) goes at V_SEEN on a circle of R_MIN, as n points along the way."""
v = V_SEEN
w = math.degrees(v / R_MIN) * steer
pts = []
for k in range(n):
dt = seconds / n
a, wr = math.radians(h), math.radians(w)
if abs(wr) < 1e-9:
C, S = math.cos(a) * dt, math.sin(a) * dt
else:
C = (math.sin(a + wr * dt) - math.sin(a)) / wr
S = (math.cos(a) - math.cos(a + wr * dt)) / wr
x, y, h = x + v * S, y + v * C, (h + w * dt) % 360
pts.append((x, y, h))
return pts
def here():
px, py = position()
return START[0] + px, START[1] + py, heading()
def car(speed, steer, seconds, dt=0.1):
"""Drive at speed, steer -1..1, and trim the turn every dt so the radius stays R_MIN at the speed measured."""
global V_SEEN, TURN
speed, steer = max(-100, min(100, speed)), max(-1, min(1, steer)) # top speed and full lock, as in U2.8
v = V_SEEN * speed / SPEED # the speed the last move measured
rot = TURN * math.degrees(v / R_MIN) * steer # the turn that gives R_MIN at it
for k in range(round(seconds / dt)):
(x0, y0), h0 = position(), heading()
drive(speed, 0, rot)
wait(dt)
(x1, y1), h1 = position(), heading()
v = math.copysign(math.hypot(x1 - x0, y1 - y0) / dt, speed)
w = ((h1 - h0 + 180) % 360 - 180) / dt
if k >= 3: # the robot takes about 0.3 s to answer a new command
rot += 0.2 * (math.degrees(v / R_MIN) * steer - w) * TURN
V_SEEN += 0.2 * (abs(v) - V_SEEN)
if steer and seconds >= 4 * dt: # keep what the trim learned for the next move's first guess
TURN += 0.5 * (rot / (math.degrees(V_SEEN / R_MIN) * steer) - TURN)
# write plan(): A* over the car's six moves
# then the loop: plan from here(), drive only the first move, print a line, and plan again
Challenges
- Allow the car to reverse: the same moves again with
speednegative (arc()will need to know the sign). Does the planner find a shorter way in, and does it use a three-point turn? Once the car can reverse, the Dubins length of Challenge 2 is no longer a lower bound, because reversing can be shorter; the Reeds and Shepp length is. - Replace the straight-line heuristic with the length of the shortest Dubins path to the goal pose, less the 8 cm of slack. The CSC words are enough only when the two positions are more than
4Rapart; closer than that, add RLR and LRL, or the heuristic can overestimate. Convert the course's heading (clockwise from +y) into the maths frame first, or every L comes out as an R. The goal also accepts 25 degrees either side of 180, which a Dubins path to exactly 180 ignores, so it can still overestimate a little. Does A* now return a longer path? How many fewer states does it expand? - Halve
STEP_S. The plan gets smoother and the search gets slower. Why both?