algorithms for sensor-based robotics: roadmap methodssleonard/cs436/roadmaps.pdf · 3/30/2015 cs...

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

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.



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’’



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



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



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



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




• Second, draw lines of sight from every vertex of

every obstacle like before. Remember lines along

edges are also lines of sight.

start

goal




start

goal



The Visibility Graph (Done)

• Repeat until you’re done.

start

goal



Visibility Graphs



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



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



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


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


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


1-connected


2-connected


1-connected


2-connected


• Connectivity of the slice in the free space

changes at the critical points


• Each cell can be covered by back and

forth motions


• 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



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


h(x,y) = D(x,y)

CS336/436 Greg

Hager based on

notes by Howie


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 …



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)



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



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




www.ams.org

l

p

P(p,l)

q

P(q,l)

r P(r,l)

Voronoi Diagram




www.ams.org

l

P(s,l) s

p

P(p,l)

q

P(q,l)

r P(r,l)

Voronoi Diagram




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




www.ams.org

l

p

q

r

P(s,l) s P(t,l)

t

P(u,l)

u

Voronoi Diagram




www.ams.org

p

q

r

s t

u



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





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.



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



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)



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



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.




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)



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




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




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.




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



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



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



Connectivity change at Critical Points



Between Critical Points



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



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.




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.




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.




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.




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



Summary

• Basic roadmap ideas and definitions

• Visibility graphs

• Voronoi graphs

• Canny’s sweep method



algorithms for sensor-based robotics: roadmap methodssleonard/cs436/roadmaps.pdf · 3/30/2015 cs...

Documents