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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Computed trajectories… 10/31/06 CS225B Brian Gerkey

Computed trajectories… 10/31/06 CS225B Brian Gerkey

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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