Upload
lethien
View
227
Download
0
Embed Size (px)
Citation preview
3/30/2015 CS 336, G.D. Hager (loosely based on notes by Nancy Amato
and Howie Choset)
Algorithms for Sensor-Based Robotics:
RoadMap Methods
Computer Science 336
http://www.cs.jhu.edu/~hager/Teaching/cs336
Professor Hager
http://www.cs.jhu.edu/~hager
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
The Basic Idea
• Capture the connectivity of Q_{free} by a graph or network of
paths.
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
RoadMap Definition
• A roadmap, RM, is a union of curves such that for all start and
goal points in Qfree that can be connected by a path:
– Accessibility: There is a path from qstart Qfree to some q’ RM
– Departability: There is a path from some q’’ RM to qgoal Qfree
– Connectivity: there exists a path in RM between q’ and q’’
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
RoadMap Path Planning
1. Build the roadmap
a) nodes are points in Q_{free}
(or its boundary)
b) two nodes are connected by
an edge if there is a free
path between them (i.e.
interpolated motion)
2. Connect start end goal points
to the road map
at point q’ and q’’,
respectively
3. Connect find a path on the
roadmap between q’ and q’’
The result is a path in
Q_{free} from start to goal
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
Overview
• Deterministic methods
– Some need to represent , and some don’t.
– are complete
– are complexity-limited to simple (e.g. low-dimensional)
problems
• example: Canny’s Silhouette method (5.5)
– applies to general problems
– is singly exponential in dimension of the problem
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
Visibility Graph methods • Defined for polygonal
obstacles
• Nodes correspond to
vertices of obstacles
• Nodes are connected if
– they are already connected
by an edge on an obstacle
– the line segment joining
them is in free space
• Not only is there a path
on this roadmap, but it
is the shortest path
• If we include the start
and goal nodes, they
are automatically connected
• Algorithms for constructing
them can be efficient
– O(n3) brute force
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
The Visibility Graph in Action (Part 1)
• First, draw lines of sight from the start and goal to
all “visible” vertices and corners of the world.
start
goal
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
The Visibility Graph in Action (Part 2)
• Second, draw lines of sight from every vertex of
every obstacle like before. Remember lines along
edges are also lines of sight.
start
goal
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
The Visibility Graph in Action (Part 3)
start
goal
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
The Visibility Graph in Action (Part 4)
start
goal
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
The Visibility Graph (Done)
• Repeat until you’re done.
start
goal
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
Visibility Graphs
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
The Sweepline Algorithm
Analysis: For a vertex, n log n to create initial list, log n for each ai
Overall: n log (n) (or n2 log (n) for all n vertices
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
Example Algorithm:
Initially:
calculate the angle ai of segment v-vi and sort
vertices by this creating list E
create a list of edges that intersect the horizontal
from v sorted by intersection distance
For each ai
if vi is visible to v then add v-vi to graph
if vi is the “beginning” of an edge E, insert E in S
if vi is the “end” of and edge E, remove E from S
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
Reduced Visibility Graphs
• The current graph as too many lines
– lines to concave vertices
– lines that “head into” the object
• A reduced visibility graph consists of
– nodes that are convex
– edges that are “tangent” (i.e. do not head into the object at either
endpoint)
CS336/436 Greg
Hager based on
notes by Howie
Robotic Motion Planning:
Cell Decompositions (with some discussion on coverage)
CS 336/436
http://www.cs.jhu.edu/~hager/Teaching/cs33
6
Gregory D. Hager
CS336/436 Greg
Hager based on
notes by Howie
Definition
Qfree
Qfree
CS336/436 Greg
Hager based on
notes by Howie
Exact Cell vs. Approximate Cell
• Cell: simple region
CS336/436 Greg
Hager based on
notes by Howie
Adjacency Graph
– Node correspond to a cell
– Edge connects nodes of adjacent cells
• Two cells are adjacent if they share a common boundary
c11
c1
c2
c4
c3
c6
c5 c8
c7
c
10
c9
c12
c13
c14
c15
c1 c10
c2
c3
c4 c5
c6
c7
c8
c9
c11
c12
c13
c14
c15
CS336/436 Greg
Hager based on
notes by Howie
Path Planning
• Path Planning in three steps:
– Planner determines cells that contain the start and goal
– Planner searches for a path within adjacency graph
– Find the exact path in the configuration space.
CS336/436 Greg
Hager based on
notes by Howie
Types of Decompositions
• Trapezoidal Decomposition
• Morse Cell Decomposition
– Boustrophedon decomposition
– Morse decomposition definition
– Sensor-based coverage
– Examples of Morse decomposition
• Visibility-based Decomposition
CS336/436 Greg
Hager based on
notes by Howie
Trapezoidal Decomposition
CS336/436 Greg
Hager based on
notes by Howie
Trapezoidal Decomposition
CS336/436 Greg
Hager based on
notes by Howie
Trapezoidal Decomposition
CS336/436 Greg
Hager based on
notes by Howie
Trapezoidal Decomposition
CS336/436 Greg
Hager based on
notes by Howie
Trapezoidal Decomposition
CS336/436 Greg
Hager based on
notes by Howie
Trapezoidal Decomposition
CS336/436 Greg
Hager based on
notes by Howie
Trapezoidal Decomposition
Path
CS336/436 Greg
Hager based on
notes by Howie
Implementation
• Input is vertices and edges
• Sort n vertices O(n log(n))
• Determine vertical extensions
– For each vertex, intersect vertical line with each edge – O(n) time
– Total O(n2) time
CS336/436 Greg
Hager based on
notes by Howie
Sweep line approach
Sweep a line through the space stopping at vertices which are often
called events
Maintain a list L of the current edges the slice intersects
Determining the intersection of slice with L requires O(n) time but with
an efficient data structure like a balanced tree, perhaps O(log n)
Really, determine between which two edges the vertex or event lies
These edges are
So, really maintaining L takes O(n log n) – log n for insertions, n for vertices
CS336/436 Greg
Hager based on
notes by Howie
Events
In
Out Middle
%% Assume an adjacency list data structure of vertices V
Vsorted = sort(V, x_coord)
E = {} %% E is sorted on y coordinate
S = {}
for i=1:n
v = Vsorted(i)
vadj = sort (adjacent(v), ycoord)
elower = edge(v,vadj(1))
eupper = edge(v,vadj(2))
cases
vadj(1) right of v and vadj(2) right of v:
E = E + {elower,eupper}; opendir(elower=d); opendir(eupper=u);
S = S + <nextopen(elower), nextopen(eupper)>
vadj(1) left of v and vadj(2) left of v:
S = S + <nextopen(elower), nextopen(eupper)>
E = E - {elower,eupper}
vadj(1) left of v and vadj(2) right of v:
E = E – elower + eupper; opendir(eupper) = opendir(elower)
S = S + <v, nextopen(eupper)>
vadj(1) right of v and vadj(2) left of v:
E = E – eupper + elower; opendir(elower) = opendir(eupper)
S = S + <v, nextopen(eupper)>
return S
CS336/436 Greg
Hager based on
notes by Howie
Example
CS336/436 Greg
Hager based on
notes by Howie
Example
CS336/436 Greg
Hager based on
notes by Howie
Example
CS336/436 Greg
Hager based on
notes by Howie
Example
CS336/436 Greg
Hager based on
notes by Howie
Trapezoidal Decomposition
CS336/436 Greg
Hager based on
notes by Howie
Coverage
Planner determines an exhaustive walk
through the adjacency graph
Planner computes explicit robot motions
within each cell
Problems
1. Polygonal representation
2. Quantization
3. Position uncertainty
4. Full information
5. What else?
CS336/436 Greg
Hager based on
notes by Howie
Boustrophedon Decomposition
CS336/436 Greg
Hager based on
notes by Howie
Complete Coverage
CS336/436 Greg Hager based on notes by Howie Choset
• Slice function: h(x,y)= x
• At a critical point x of where M = {x|m(x)=0} )()(,| xmxhh M
h
m
Morse Decomposition in Terms of Critical Points
CS336/436 Greg Hager based on notes by Howie Choset
1-connected
CS336/436 Greg Hager based on notes by Howie Choset
2-connected
CS336/436 Greg Hager based on notes by Howie Choset
1-connected
CS336/436 Greg Hager based on notes by Howie Choset
2-connected
CS336/436 Greg Hager based on notes by Howie Choset
• Connectivity of the slice in the free space
changes at the critical points
CS336/436 Greg Hager based on notes by Howie Choset
• Each cell can be covered by back and
forth motions
CS336/436 Greg Hager based on notes by Howie Choset
• Reeb graph represents the topology of
the cellular decomposition
CS336/436 Greg
Hager based on
notes by Howie
Detect Critical Points
Voronoi Diagrams
• Given a set of points (called
sites), divide the space in
Voronoi regions
– The region of a site is defined
by all the points closer the
site
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
Generalized Voronoi Diagram
• Extends the Voronoi
diagram to non-point sites
– A region is defined by all
the points that are closer to
an obstacle
• The boundaries
(composed of points
between two equidistant
obstacles) are used to
plan the path
• Topological skeleton of a
simple polygon (no holes)
Choset CGC96
CS336/436 Greg
Hager based on
notes by Howie
Brushfire Decomposition
CS336/436 Greg
Hager based on
notes by Howie
Brushfire Decomposition
h(x,y) = D(x,y)
CS336/436 Greg
Hager based on
notes by Howie
Brushfire Decomposition
h(x,y) = D(x,y)
CS336/436 Greg
Hager based on
notes by Howie
Brushfire Decomposition
h(x,y) = D(x,y)
CS336/436 Greg
Hager based on
notes by Howie
Brushfire Decomposition
h(x,y) = D(x,y)
CS336/436 Greg
Hager based on
notes by Howie
Brushfire Decomposition
Coverage Path
Is This A Roadmap?
• How do we know the roadmap is connected?
– Accessibility: follow gradient uphill to diagram – guaranteed to be
free as you are going away from obstacles
– Departability: go downhill from closest point to goal
– Connectivity: The book talks about deformation retracts as a way of
showing this; we will not cover this
• It is also a cell decomposition …. as a homework exercise …
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
Voronoi Diagram
Fortune’s Algorithm • Given a line l and a point p (not on the line), the set of points that
are equidistant to l and p form a parabola P(p,l)
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
www.ams.org
l
p
P(p,l)
Points on this side
are closer to p
Points on this side
are closer to l
Voronoi Diagram
Fortune’s Algorithm
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
www.ams.org
l
p
P(p,l)
q
P(q,l)
r P(r,l)
Beach line
Points above the beach
line are closer to p, q or
r than l
Because this beach line point
is on P(r,l) and P(q,l) it is
equidistant from r and q
Voronoi Diagram
Fortune’s Algorithm
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
www.ams.org
l
p
P(p,l)
q
P(q,l)
r P(r,l)
Voronoi Diagram
Fortune’s Algorithm
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
www.ams.org
l
P(s,l) s
p
P(p,l)
q
P(q,l)
r P(r,l)
Voronoi Diagram
Fortune’s Algorithm
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
www.ams.org
l
P(s,l) s P(t,l) t
p
P(p,l)
q
P(q,l)
r P(r,l)
Voronoi Diagram
Fortune’s Algorithm
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
www.ams.org
l
p
q
r
P(s,l) s P(t,l)
t
P(u,l)
u
Voronoi Diagram
Fortune’s Algorithm
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
www.ams.org
p
q
r
s t
u
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
Silhouette Method
Canny’s Roadmap Algorithm
The Opportunistic Path Planner
The Basic Ideas
• Pick a sweeping surface (n-1 dimensional)
• As sweeping happens, detect extremal points (roadmap)
and critical points (= places where connectivity changes)
– Note extremal points are 1-dimensional curve
• For each slice where a critical point occurs, repeat this
process recursively
– This connects extremal points
• Use this as the roadmap
– A variation: use this to define a more interesting roadmap
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
Accessibility and Departability (1)
In order to access and depart the roadmap
we treat the slices which contain qs and qg
as critical slices and run the algorithm the
same way.
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
Accessibility and Departability (2)
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
Accessibility and Departability (3)
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
Accessibility and Departability (4)
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
Accessibility and Departability (5)
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
Building the Roadmap
Given that the algorithm is now clear
conceptually, let’s establish the
mathematical machinery to actually
construct the roadmap. We must define
• The sets
• The slices
• How to find extrema
• How to find critical points
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
The Sets
The S which this algorithm deals with are
semi-algebraic sets that are closed and compact.
Def: A semi-algebraic set Sr defined by the
polynomials F1,..,FnQr is a set derived from the
sets
Si={xr | Fi(x)>0}
by finite union, intersection and complement.
Ex: (x2+y2 ≤ 1) (z ≤ 1) (z ≥ -1)
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
The Slices
Given that the algorithm is now clear conceptually, let’s
establish the mathematical machinery to actually construct
the roadmap.
SS
cSxPSS
c
c
cc
}:{ 1
knk xxxx ),....,,( 21
The slices are the intersection of a hyperplane and S
where 1 is the projection on to the first coordinate
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
How To Find Extrema (1)
When constructing the silhouette curves, we
look for extrema of 2|Sc, the extrema of the
projection of Sc in a second direction.
In order to find the extrema on a manifold
we will refer to the Lagrange Multiplier
Theorem.
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
How To Find Extrema (2)
Lagrange Multiplier Theorem:
Let S be an n-surface in n+1, S=f-1(c)
where f:U is such that f(q)0 qS.
Suppose h:U is a smooth function and
pS is a extremum point of h on S.
Then s.t. h(p)= f(p) (they are
parallel)
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
How To Find Extrema
Example:
Consider S=f -1(0) where f=x2+y2+z2-1 (a solid
unit sphere). Extrema of h=1(x,y,z)=(x).
001
222
),(
zyx
hfd
y = z = 0 (y-z plane) and only points on sphere is x = 1, x = -1, left most and right most
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
How To Find Extrema (3)
Canny’s Generalization of
the Lagrange Multiplier
Theorem:
Suppose that U is an open
subset of the kernel of some
map f:rn, and let f be
transversal to {0}. Let
g:rm be a map, then
xU is an extremum of g|U
iff the following matrix is not
full rank.
)()(
)()(
)()(
)()(
),(
1
1
1
1
1
1
1
1
xx
gx
x
g
xx
gx
x
g
xx
fx
x
f
xxr
fx
x
f
gfd
r
mm
r
r
nn
x
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
How To Find Extrema (4)
Canny’s Slice Lemma:
The set of critical points of 12|S, (12|S),
is the union of the critical points of 2|Sc where we sweep in the 1
direction.
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
How To Find Extrema (5)
Example:
Consider S=f -1(0) where f=x2+y2+z2-1 (a solid
unit sphere). If we sweep in the x direction and
extremize in the y direction h=12(x,y,z)=(x,y).
010
001
222
),(
zyx
hfd
So the silhouette curve is the unit circle on the x-y plane
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
Finding Critical Points
The critical points which denote changes in
connectivity of the silhouette curves also follow
from Canny’s Generalization. They are the
extrema of the projection on to the sweeping
direction of the silhouette curves. Simply
)( )(|1 12
Can be viewed as
the distance to the y
axis from a point
Critical point is where
roadmap tangent is parallel
to slice
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
Finding Critical Points in Higher Dims
Define roadmap as the pre-image of f, but cannot do so. Df,
however is a m-1 x m matrix.
This matrix forms the top m-1 rows of
Null of Df is tangent to roadmap, so m-1 row vectors of Df
form a plane orthogonal to roadmap tangent
Slice function has gradient which
forms the bottom row of
When roadmap tangent lies in slice plane, this means that
and slice plane are orthogonal to each other
looses rank
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
Connectivity change at Critical Points
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
Between Critical Points
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
Building the Roadmap (Conclusion)
• We can now find the extrema necessary to build the silhouette
curves
• We can find the critical points where linking is necessary
• We can run the algorithm recursively to construct the whole
roadmap
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
Illustrative Example (1)
Let S be the ellipsoid
with a through hole.
Pc is a hyperplane
of codimension 1
( x = c ) which will be
swept through S in
the X direction.
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
Illustrative Example (2)
At each point
the slice travels
along X we’ll
find the extrema
in SPc in the Y
direction. If we
trace these out we
get silhouette
curves.
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
Illustrative Example (3)
Observations:
• The silhouette curves are one-dimensional.
• This is not a roadmap, it’s not connected.
• There are points where extrema disappear and reappear, these will be
called critical points and the slices that go through these points are
critical slices.
• A point on a silhouette curve is a critical point if the tangent to the curve
at the point lies in Pc.
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
Illustrative Example (4)
We’ll connect a
critical point to the rest
of the silhouette curve
with a path that lies
within SPc. This can
be done by running the
algorithm recursively.
Each time, we increase
the codimension of the
hyperplane by 1.
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
Illustrative Example (5)
Final points
• The recursion is repeated until there are no more critical points or
the critical slice has dimension 1(it is its own roadmap)
• The roadmap is the union of all silhouette curves
One path to Showing Complexity
• Using this method, one can show that path planning is
pn (log p) w(O(n^4)
p is # of polynomials (# of obstacles)
w is degree (obstacle complexity)
n is # of degrees of freedom of robot
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato
Summary
• Basic roadmap ideas and definitions
• Visibility graphs
• Voronoi graphs
• Canny’s sweep method
16-735, Howie Choset, with significant copying from G.D. Hager who
loosely based his notes on notes by Nancy Amato