1. Path Planning vs. Obstacle Avoidance
No clear distinction, but usually:
Global vs. local
Path planning
low-frequency, time-intensive search method for global finding of a (optimal) path to a goal
Examples: road maps, cell decomposition
Obstacle avoidance (aka “local navigation”)
fast, reactive method with local time and space horizon
Examples: vector field histogram, dynamic window algorithm
Gray area
Fast methods for finding path to goal that can fail if environment contains local minima
Example: potential field method
10/31/06
CS225B Brian Gerkey

2. Local minima Local navigation can easily get stuck
Current sensor data is insufficient for general navigation
Shallow minima may only cause a delay
10/31/06
CS225B Brian Gerkey

3. Deep minima Maintaining history can avoid oscillations
Look-ahead and randomness can avoid some minima
But eventually, local navigation can get irretrievably stuck
10/31/06
CS225B Brian Gerkey

4. Our goal Given: Compute: Also, execute the path map robot pose
goal pose
Compute:
a feasible (preferably efficient) path to the goal
Also, execute the path
may require replanning
10/31/06
CS225B Brian Gerkey

5. Configuration space
workspace (W): The ambient environment in which the robot operates
Usually R2 or R3
configuration (q): Complete specification of the position of all points in the robot’s body
Set of points in robot’s body is given by R(q)
configuration space, or C-space (Q): Space of all possible configurations
C-space dimensionality = # degrees of freedom
10/31/06
CS225B Brian Gerkey

6. Example 1: two-link arm Two angles fully specify the configuration
2-dimensional configuration space
Q = (1, 2)
Is the position of the end effector a valid configuration specification?
Q ?= (X, Y)
10/31/06
CS225B Brian Gerkey

7. Example 2: Sony AIBO 4 legs, 3 dof each 1 head, 3 dof
2 in the shoulder
1 in the knee
1 head, 3 dof
15 dof => 15-d C-space
Q = (l1,l2,l3,l4,h)
10/31/06
CS225B Brian Gerkey

8. Example 3: planar mobile robot
Workspace is R2
Q = (X,Y,)
How about a circular robot?
Body position doesn’t depend on 
 1,2:
R(x, y,1) = R(x,y,2)
Thus the C-space is 2-d:
Q = (X,Y)
10/31/06
CS225B Brian Gerkey

9. C-space obstacles
Given workspace obstacle WOi , C-space obstacle QOi is the set of configurations in which the robot intersects WOi :
QOi = {q  Q | R(q)  WOi  }
Free C-space, Qfree , is the set of configurations in which the robot intersects no obstacle:
Qfree = Q \ ( QOi)
10/31/06
CS225B Brian Gerkey

10. Computing C-space obstacles for a circular robot
Slide the robot around the obstacle
Effectively “grows” the obstacle by the radius of the robot
10/31/06
CS225B Brian Gerkey

11. C-space maps for circular robots
Workspace
C-space, radius=1.0m
C-space, radius=0.25m
10/31/06
CS225B Brian Gerkey

12. C-space map  plan path for a single point
Workspace W
Configuration space C
path y x
10/31/06
CS225B Brian Gerkey

13. Representation: geometric vs. sampled
Geometric representation
exact model of environment
possibility of optimal solution
difficult to obtain such a map
continuous solution space
combinatorial techniques
Sampled representation
approximate model of environment
usually, though not necessarily, uniform grid
optimality limited by resolution
map readily obtained from data
discrete solution space
navigation function techniques
10/31/06
CS225B Brian Gerkey

14. Geometric: Cell decomposition
How can we search a continuous space of configurations?
Decompose into non-overlapping cells
cell: a connected region of C-space
can be exact or approximate
Construct connectivity graph from cells and transitions between them
Search the graph for a path to the goal
Convert graph solution to real-world path
Example:
triangulation
Example:
trapezoidal decomposition
Images from LaValle
10/31/06
CS225B Brian Gerkey

15. Geometric: Roadmap / retraction
How can we search a continuous space of configurations?
Retract the free space into a set of curves
The curves are your roadmap
Plan like you would for a road trip
Get on the freeway
Traverse the network of freeways
Get off the freeway
May need special procedures for getting on/off the roadmap
Example:
generalized Voronoi graph
Example:
visibility graph
10/31/06
CS225B Brian Gerkey

16. Geometric example: pursuit-evasion
The Problem: Coordinate the motions of one or more searchers through a given environment so as to locate one or more evaders
Application areas: building security, search and rescue, environmental monitoring
10/31/06
CS225B Brian Gerkey

17. Implementation
Geometric computations consist mostly of planar and radial sweep lines, which can be done efficiently
When φ = π, the roadmap comprises only linear objects, on which exact computation can be done with rational, instead of real, numbers
Rather than building all of GI ahead of time, construct it online during the search, as states are encountered
10/31/06
CS225B Brian Gerkey

18. Computed trajectories…
10/31/06
CS225B Brian Gerkey

19. Computed trajectories…
10/31/06
CS225B Brian Gerkey

20. Sampled: potential / navigation functions
For each sampled state, estimate the cost to goal
similar to a policy in machine learning
Grid maps: for each cell, compute the cost (number of cells, distance, etc.) to the goal
potential functions are local
navigation functions are global
10/31/06
CS225B Brian Gerkey
Images from LaValle

21. wavefront planner Given grid map M, robot position p, goal position g
From M, create C-space map MQ
Run brushfire algorithm on MQ:
Label grid cell g with a “0”
Label free cells adjacent to g with a “1”
Label free cells adjacent to those cells with a “2”
Repeat until you reach p
Labels encode length of shortest (Manhattan distance) obstacle-free path from each cell to g
Starting from p, follow the gradient of labels
Resolution-complete
10/31/06
CS225B Brian Gerkey

22. wavefront in action Gets very close to obstacles… 10/31/06
CS225B Brian Gerkey

23. gradient planner Given grid map M, robot position p, goal position g
Define intrinsic cost I(p) of being at p
Define adjacency cost A(pi,pj) from pi to pj
For a path P={p1,p2,…}, define path cost F(P):
F(P) = i I(pi) + i A(pi,pi+1)
Compute costs (next slide), then find lowest-cost path to goal
Resolution-complete
10/31/06
CS225B Brian Gerkey

24. gradient cost update algorithm (LPN)
q, Cost(q) = ∞
Cost(g) = 0
ActiveList = {g}
while p  ActiveList: (or ActiveList  )
q = ActiveList.pop()
for each n  4neighbors(q):
newcost = F(qn)
if newcost < Cost(n):
Cost(n) = newcost
ActiveList.push(n)
10/31/06
CS225B Brian Gerkey

25. Obstacle costs To set intrinsic costs, first run LPN with:
goal = obstacle points
I(p) = 0
A(pi,pj) = ||pj - pi||
We have for each cell d(p), the minimum distance from p to the closest obstacle
Intrinsic cost is:
I(p) = Q(d(p))
where Q() is a decreasing function of distance.
10/31/06
CS225B Brian Gerkey

26. gradient in action Running time: approx O(n) for n grid cells.
Only practical for modestly-sized 2d grids
Images from Konolige
10/31/06
CS225B Brian Gerkey

27. Dealing with local obstacles
The map isn’t perfect; neither is localization
How do you deal locally-sensed obstacles?
In general, how do you compute velocities from the gradient?
Option 1: Hand off waypoints to local navigator
Pro: easy to implement
Con: planner / navigator interaction may be suboptimal
Option 2: Compute velocities directly in planner
Pro: potentially optimal behavior
Con: planner has to run FAST
10/31/06
CS225B Brian Gerkey

28. Handing waypoints to navigator
Waypoint selection strategy:
Look ahead some distance, checking for line-of-sight
Add intermediate waypoints on long stretches
Things to look out for:
Don’t need to exactly reach waypoints; give a large envelope around them
Make sure navigator signals planner when it’s done (to get a new waypoint)
When do you replan?
10/31/06
CS225B Brian Gerkey

29. Computing velocities directly
Given cost-to-goal values for each cell, how do you compute velocities for the robot?
Pick a point p some distance (e.g., 0.5m) ahead of the robot on the best path
error = (bearing to p) - (current heading)
derror = distance to p
if abs(error) < min_angle_threshold:
d = 0
else:
d = k * error
if abs(error) > max_angle_threshold:
dX = 0
dX = kx * derror or dX = kx * derror - kx * error
10/31/06
CS225B Brian Gerkey

30. Computing velocities directly (cont’d)
Obstacles that are not in the map?
Add local sensor readings into the map
But age them out over time
Must be able to replan every control cycle (~10Hz)
10/31/06
CS225B Brian Gerkey

31. Another way: trajectory rollout
10/31/06
CS225B Brian Gerkey

32. Gradient + trajectory rollout: LAGR
10/31/06
CS225B Brian Gerkey
Average speed > 1m/s

33. Computing true distance
Naïve single-neighbor cost update gives Manhattan distance (maybe with diagonal transitions)
The gradient unnecessarily tracks the grid
In reality, the grid potentials are discrete samples of wave that propagates from the goal
A better distance can be had by approximating this wave:
Sethian’s Fast Marching Method
Konolige’s (unpublished) 2-neighbor plane wave approximation
10/31/06
CS225B Brian Gerkey

34. Practical concerns
Goals and robot positions will rarely fall exactly at the center of a grid cell
Pick the closest cell center OR interpolate cost information between cells
When computing obstacle costs, stop LPN when costs drop below a threshold (i.e., far enough from any obstacle)
Q(d(p)) should be very large (essentially infinite) when
d(p) < (1/2 * robot_radius) + safety_distance
Costs should decrease smoothly and quickly after that.
For non-circular robots (e.g,. Erratic), use the longer half-width as the radius
No longer resolution-complete, but usually works
Integer math is fastest; consider working with squared distances
10/31/06
CS225B Brian Gerkey