Planning and navigation

1
Planning and navigation

• Are we there yet?

Image courtesy flicker user Caveman92223
2
Planning and Navigation

• Mission Planning
• Where am I going?
• Path Planning
• Whats the best way there?
• Map Making
• Where have I been?
• Localization
• Where am I?

3
Path Planning

• Path Planning
• Given a map and a goal location determine an
  obstacle-free trajectory to reach the goal
• Path planning is relevant to industrial robots as
  well as mobile robots.
• Path planning involves considerations of how to
  represent the world, the map and the path itself
• Two ways of representing a map/path
• Topological
• Metric
• A metric path includes topological path (the
  topological can be derived from the metric).

4
Configuration Space

• Given a robot with k degrees of freedom
• Every possible state can be modeled as a
  k-element vector.
• This vector corresponds to a point in some
  k-dimensional space
• The k-dimensional space is the configuration
  space (C-space) of the robot
• Can plan paths in C-space rather than in
  coordinate space
• C-space can be applied to mobile robots where the
  configuration space is given by (x, y, angle) and
  obstacles correspond to point columns in
  C-space

5
What is the C-space of this robot?
6
Configuration Space
7
Representing maps for planning

• Generally have two ways of representing maps.
• Topological
• Relational
• Data graphs where nodes are locations and edges
  are trajectories
• Algorithms graph algorithms (single-source
  shortest path)
• Associative
• Data perceptual signatures that map to an
  associated location
• Algorithm neural networks
• Metric
• Data Grids (regular, hierarchical, quad trees)
  or CAD-like representations
• Algorithms Graph algorithms

8
Metric Maps(cell decomposition)

• Divide space into non-overlapping smaller
  regions.
• Determine a path from one point to goal by
  determining which regions should be traversed.
• Which regions shouldnt be traversed?
• Generally interested in best or optimal
  route. What does this mean?
• Path planning assumes an a priori map of relevant
  aspects
• Only as good as last time map was updated.
• Can a metric map be dynamically updated?

9
Topological Maps Use Landmarks

• A landmark is one or more perceptually
  distinctive features of interest on an object or
  locale of interest
• Natural landmark configuration of existing
  features that wasnt put in the environment to
  aid with the robots navigation (ex. gas station
  on the corner)
• Artificial landmark set of features added to the
  environment to support navigation (ex. highway
  sign).

10
A gateway is an opportunity to change path heading
Nodes indicate landmarks, gateways, or goal
locations Edges indicate a navigable path
11
Example

• Create a relational graph for this floorplan
• Label each edge with the appropriate behavior
• FH follow hall
• ND navigate door
• Label each node with its type
• DE dead end
• R room
• T T intersection

12
Path Planning Algorithm

• Representation is a graph.
• Nodes are places and edges are trajectories
• Each edge corresponds to some behavior or
  sequence of behaviors follow corridor or go
  through door.
• How to plan a path?
• Must consider costs / risks
• Must consider behaviors
• Might use a weighted graph shortest path
  algorithm.

13
(No Transcript)
14
Transition Table
15
How to handle un-expected changes to the map?
16
Meadow map

• Assume a CAD-like map of the shapes (polygons,
  lines, arcs) of the environment.
• How to process it to determine good roads or
  good trajectories?
• Take the borders of all obstacles and expand them
  by the width of the robot to form new
  boundaries.

17
Meadow map

• Construct convex polygons by connecting edges of
  the obstacles.
• Why convex polygons?
• Interior has no obstacles so can safely transit
  (freeway, free space)
• Is the resulting map unique?
• Not necessarily a unique set of polygons
• How can this be used to generate a trajectory?

18
Meadow map

• A path is given by following lines that connect
  the center of edges to other edge centers until
  the goal is reached. This reduces to a
  graph-based search.

19
Path Relaxation

• This technique yields very jerky paths
• Can be smoothed by path relaxation
• Can the map be dynamically updated?

20
Example

• Create a meadow map and relational graph using
  mid-point of line segments

21
Generalized Voronoi Graphs

• Imagine a fire starting at the boundaries,
  creating a line where they intersect,
  intersections of lines are nodes
• Each point on every line is equally distant to
  all nearest obstacles.
• Points where lines meet are known as Voronoi
  vertices.

http//www.cs.unc.edu/geom/voronoi/vplan/
http//www.cs.columbia.edu/pblaer/projects/path_p
lanner/applet.shtml
22
Example

• Create a GVG of this space

23
Regular Grids

• Represent a space as a regular 2D grid
• Often on the order of 4inches square
• How to represent grid elements?
• Make a relational graph by each element as a
  node, connecting neighbors (4-connected,
  8-connected)

24
Adaptive grids

• Instead of regular grids adapt the grid
  structure to the environment.
• Represent large empty regions by large grid
  elements
• Represent small empty regions by small grid
  elements
• Do the same with occupied regions
• Quad tree representation
• Each region of the quad tree is either occupied
  or not occupied.
• A region that has been divided is always divided
  into four quadrants
• How to generate a quad-tree grid from a geometric
  map?

25
Quad tree grid
26
Exact decomposition
27
Problems with GVG and Grids

• GVG
• Sensitive to sensor noise
• Path execution requires robot to be able to
  sense boundaries
• Grids
• World doesnt always line up on grids
• Digitization bias left over space marked as
  occupied

28
Map Making Basic Idea

• Sense and create a local map
• Move a little
• Record change in position, orientation
• Sense and create a local map
• Fuse/tile together

29
Observations

• The map is almost always a type of regular grid
• Move D and Integrate local map are difficult
• Integration requires accurate measurement of D
  (usually on the order of inches and lt 5 degrees)

Black is ground truth, Purple is measured using
shaft encoders for D
30
Occupancy Grids

• Type of regular grid
• Came out of sonar sensing techniques
• Each element L is marked with belief that L is
  empty or occupied
• Usually a number on a scale
• 0,1 for probability theories
• 0-15 for HIMM

31
Sonars and Occupancy Grids

• Every element L under the sonar beam gets
  marked with some value for empty, occupied
• Exact values depends on the sonar being used.
• Generic sonar model
• 3 regions
• R theoretical range, r measured range
• b half angle

32
Histogramic In-Motion Mapping

• A procedure for mobile robots to create maps
• Uses an occupancy grid approach
• Uses range-sensors to generate data
• Assumes noisy sensors (probably sonar)
• Large confidence values in a cell indicate high
  probability that the cell is occupied.

33
Summary

• Metric path planning requires
• Representation of world space, usually try to
  simplify to cspace
• Algorithms which can operate over representation
  to produce best/optimal path
• Representation
• Usually try to end up with relational graph
• Regular grids are currently most popular in
  practice, GVGs are interesting
• Tricks of the trade
• Grow obstacles to size of robot to be able to
  treat holonomic robots as point
• Relaxation (string tightening)
• Metric methods often ignore issue of
• how to execute a planned path
• Impact of sensor noise or uncertainty,
  localization

34
Algorithms

• Path planning
• A for relational graphs
• Wavefront for operating directly on regular grids
• Interleaving Path Planning and Execution

35
Example

• Use 4-connected neighbors to create a relational
  graph

36
Motivation for A

• Single Source Shortest Path algorithms are
  exhaustive, visiting all edges
• Cant we throw away paths when we see that they
  arent going to the goal, rather than follow all
  branches?
• This means having a mechanism to prune branches
  as we go, rather than after full exploration
• Algorithms which prune earlier (but correctly)
  are preferred over algorithms which do it later.

37
A

• Similar to breadth-first at each point in the
  time the planner can only see its node and 1
  set of nodes in front
• Rate the choices, choose the best one first,
  throw away any choices whenever you can
• f(n) is the estimated cost of the path from Start
  to n
• g(n) is the actual cost of going from the Start
  to node n
• H(n) is the estimated cost of going from n to the
  Goal
• H is for heuristic function a guessing
  function
• Guess the cost of n to Goal since cant see the
  path between n and the Goal

f(n)g(n)h(n)
38
A Heuristic Function
f(n) g(n) h(n)

• G(n) just sum up the path costs to n
• H(n) is a lower-bound on the cost of getting to
  the goal from node n. Can be defined as
  Euclidean distance.

39
A algorithm

• Maintain a priority queue of partial solutions
• Even partial solutions have an estimated total
  cost
• Estimated total cost (f) is the key
• Remove solution from queue
• If it has reached the goal this is it!
• For every possible path from N, extend the
  solution and add to queue

40
A Algorithm

• algorithm astart(Graph G, Node S, Node T)
• INPUT S is in G and is the start node
• INPUT T is in G and is the goal node
• OUTPUT Minimal cost path from S to T
• Let PQ be a priority queue
• PQ.insert(S0, h(s))
• while PQ.peek() is not a path ending in T do
• P PQ.remove()
• K last node on path P
• for every unvisited node N adjacent to K do
• P P.append(N)
• Pg Pg edge(N,K)
• PQ.insert(P, h(N))
• return PQ.remove()

41
Example of A
42
A Algorithm

• Properties of A
• Always finds a solution (if one exists)
• Always finds best (lowest-cost) solution
• Has optimal speed (searches fewest paths)
• Following conditions must hold
• From each node, finite options exist
• The cost of each path segment is positive
• H is lt actual cost (H is admissable)

Image by Vincent Van Gogh Starry Night
43
Proof

• Suppose the first item in queue is a path P
  reaching the goal with cost K.
• Also in the queue is a path P to node N from
  which the goal is accessible via sub-path S
• The actual cost of the path through N is the
  actual cost of P plus the actual cost of S.
• The actual cost of s gt h(N)
• The actual cost of PS gt g(N)h(N) f(N)
• Since P occurred before P in the queue, its
  actual cost was less than or equal to f(N)