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. The planner is only resolution complete: it finds a path if there is one at the size of its cells, and smaller cells find more. In exchange the search finishes in a few milliseconds, which is the usual trade. Keeping one state per cell but remembering its exact continuous pose, as this planner does, is Hybrid A*, the version Lynch and Park describe in 10.4.2. 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 9 percent less, so at full lock it turns on a circle of about 22 cm, tighter than R_MIN. This one 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, which holds the radius at 25 cm to within about a centimetre. It keeps the speed it measured in V_SEEN, and arc() uses that, 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
def free(x, y):
if x < INFLATE or y < INFLATE or x > 200 - INFLATE or y > 200 - INFLATE:
return False
return not any(ox - INFLATE <= x <= ox + ow + INFLATE and oy - INFLATE <= y <= oy + oh + INFLATE
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 every dt set the turn rate from the speed measured, so the radius stays R_MIN."""
global V_SEEN
speed, steer = max(-100, min(100, speed)), max(-1, min(1, steer)) # top speed and full lock, as in U2.8
rot = 100 * math.degrees(speed / 100 * V_MAX / R_MIN) * steer / W_MAX # first guess, from the nominal speed
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
rot += 0.5 * (math.degrees(v / R_MIN) * steer - w) * 100 / W_MAX
if k >= 4: # once it is up to speed
V_SEEN += 0.2 * (abs(v) - V_SEEN)
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()
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 end at (159, 58) facing 158: 18 cm from the goal, above the bay rather than in it. The plan assumed 12 cm/s, 19.2 cm a move, and the robot made 10.9 cm/s, 17.4 cm a move. The steering held the radius at 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 about 20 degrees off, and every one of the thirteen moves fell short. A car cannot correct sideways, so none of that comes back.
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.
Task: a U-turn round the wall, driven like a car
The green bay is on the other side of the wall, a 30 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(), an A* search over the car's three moves, then a loop that plans from where the robot really is, drives only the first move with car(), and plans 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
def free(x, y):
if x < INFLATE or y < INFLATE or x > 200 - INFLATE or y > 200 - INFLATE:
return False
return not any(ox - INFLATE <= x <= ox + ow + INFLATE and oy - INFLATE <= y <= oy + oh + INFLATE
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 every dt set the turn rate from the speed measured, so the radius stays R_MIN."""
global V_SEEN
speed, steer = max(-100, min(100, speed)), max(-1, min(1, steer)) # top speed and full lock, as in U2.8
rot = 100 * math.degrees(speed / 100 * V_MAX / R_MIN) * steer / W_MAX # first guess, from the nominal speed
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
rot += 0.5 * (math.degrees(v / R_MIN) * steer - w) * 100 / W_MAX
if k >= 4: # once it is up to speed
V_SEEN += 0.2 * (abs(v) - V_SEEN)
# write plan(): A* over the car's three moves
# then the loop: plan from here(), drive only the first move, and plan again
Challenges
- Allow the car to reverse: three more moves 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?