Chap5-Road Map-Methods howie robot motion planning PDF

Preview

Summary

RoadMap Definition
•
A roadmap, RM, is a union of curves such that for all start and
goal points in Q free that can be connected by a path:
– Accessibility: There is a path from q start ∈ Q free to some q’ ∈ RM
– Departability: There is a path from some q’’∈ RM to q goa...

Description

Robotic Motion Planning: Roadmap Methods Robotics Institute http://voronoi.sbp.ri.cmu.edu/~motion Howie Choset http://voronoi.sbp.ri.cmu.edu/~choset

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’’ – One dimensional

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

2. Connect start end goal points to the road map at point q’ and q’’, respectively 3. Connect find a path on the roadmap betwen q’ and q’’ The result is a path in Q_{free} from start to goal Question: what is the hard part here?

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(n^3) 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.

goal start

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.

goal start

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) •

Second, draw lines of sight from every vertex of every obstacle like before. Remember lines along edges are also lines of sight.

goal start

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) •

Second, draw lines of sight from every vertex of every obstacle like before. Remember lines along edges are also lines of sight.

goal start

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.

goal start

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

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(n^3) brute force 16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

The Sweepline Algorithm •

Goal: calculate the set of vertices vi that are visible from v

•

visibility: a segment v-vi is visible if – it is not within the object – the closest line intersecting v-vi is beyond vi

•

Algorithm: Initially: – calculate the angle αi 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 αi

– – – Analysis:

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 For a vertex, n log n to create initial list, log n for each αi 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

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)

interestingly, this all only works in ℜ2 16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

Voronoi Diagrams

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

Beyond Points: Basic Definitions Single-object distance function X

Ci

d ix

x

∇d ix X

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

X for “X-ray” Points within line of sight

x

QOi ~

Ci ( x) = {c ∈ QOi : ∀t ∈[0,1], x(1 − t ) + ct ∈ Qfree} 16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

Visible Distance Functions •

Single-object

d i ( x) = •

~

distance to ∞

if c i ∈ C i ( x ) otherwise

Multi-object

x

D( x) = min d i ( x) i

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

Two-Equidistant •

Two-equidistant surface

S ij = {x ∈ Q free : d i ( x ) − d j (x ) = 0} QOi

QOj 16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

More Rigorous Definition Going through obstacles

d k( x) ≤ d i ( x) = d j ( x)

QOi

QOk

Two-equidistant face

Sij

QO j

Fij = {x ∈ SSij : d i ( x) = d j ( x) ≤ d h ( x), ∀h ≠ i, j} 16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

General Voronoi Diagram n −1

GVD = U

n

UF

ij

i =1 j =i +1

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

What about concave obstacles?

vs

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

What about concave obstacles?

∇d j

∇d j vs

∇di

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

∇d i

What about concave obstacles?

∇d j

∇d j vs

∇di

∇d j ∇di

∇d j ∇di

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

∇d i

Two-Equidistant •

Two-equidistant surface

S ij = {x ∈ Q free : d i ( x ) − d j (x ) = 0} Two-equidistant surjective surface

SSij = {x ∈ S ij : ∇d i ( x) ≠ ∇d j ( x)} Two-equidistant Face

GVD = U

n

UF

ij

i =1 j =i +1

∇ di

Ci

Fij = {x ∈ SSij : d i ( x) ≤ d h ( x), ∀h ≠ i} n −1

∇d j

Sij

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

Cj

Curve Optimization Approach: Homotopy Classes

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

Pre-Image Theorem

f : Rm → Rn −1

f ( c) = { x ∈ R m : f ( x) = c} e.g.

f (x , y ) = x2 + y 2

f : R2 → R

f −1 (9) is a circle with radius3

if ∀x ∈ f −1 (c ), Df ( x) is full rank, then f −1 (c) is a manifold of dimension m − n

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

Proof for GVD Dimension

f (x ) = di (x ) − d j (x ), f : R m → R , c = 0 (di − d j )−1 (0) s.t. D (d i − d j ) is full rank ⇒ dim((d i − d j ) −1 (0)) = m − n = m − 1 Show D (di − d j ) is full rank ⇔ D (d i − d j ) is not a 0 row vector ∇di ≠ ∇d j ⇔ ∇di − ∇d j ≠ 0 ⇔ ∇( di − d j ) ≠ 0 ⇔ D( d i − d j )

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

More on GVD (cont.) Is the GVD a 1-Dimension manifold?

No, but it’s the union of 1D manifolds

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

Accessibility (in the Plane)

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

Departability

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

GVD Connected? Proof:

Im : Qfree → GVD •

Im is continuous (Prof. Yap, NYU)

•

Im of a connected set, under a continuous map, is a connected set

Q for each connected component of Q free, GVD is connected.

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

Traceability in the Plane

Tangent Pass a line through two closest points on two closest obstacles

Orthogonal is tangent

Correction

y k +1 = y k − (∇ y G) −1 G( y k , λk ) 16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

Control Laws Edge : G(x) = 0 = d i (x) − d j (x)

& = α Null(∇G(x)) + β(∇G(x)) † G(x) xwhere – – –

α and β are scalar gains Null(∇G(x)) is the null space of ∇G(x) (∇G(x))† is the Penrose pseudo inverse of ∇G(x), i.e.,

(∇G(x))† = (∇G(x)) T (∇G(x)( ∇G(x)) T ) −1

⎧di (x) − d j (x) ⎫ Meet Point : G(x) = 0 = ⎨ ⎬ d (x) − d (x) k ⎩ i ⎭

x& = α Null( ∇G(x)) + β( ∇G(x)) † G(x) 16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

D. Siler C. Kristoff SBP Class

Algorithm for exploration • • • •

Trace an edge until reach a meet point or a boundary point If a boundary point, return to the previous meet point, otherwise pick a new edge to trace If all edges from meet point are already traced, search the graph for a meet point with untraced edges When all meet points have no untraced edges, complete.

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

Demo

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

General Voronoi Graph •

In 3-Dimensions

Fijk = Fij ∩ Fik ∩ Fjk •

In m-Dimensions

Fijk ...m = Fij ∩ Fik ... ∩ Fim

= Fij ...m −1 ∩ Fim

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

GVD vs. GVG Equidistant (#obs)

Dim

Codim

GVD

2

m-1

1

GVG

m

1

m-1

Proofs by Pre-Image Theorem to come

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

Proof for GVG Dimension •

For 3-Dimensions

⎛ di − d j ⎞ ⎟⎟, f : R 3 → R 2 f = ⎜⎜ ⎝ di − d k ⎠ −1 ⎛ 0 ⎞ f ⎜⎜ ⎟⎟ = f −1(0) ⎝ 0⎠ ⎛ di − d j ⎞ ⎟⎟ ≠ 0, since ∇ di ≠ ∇ d j , ∇ di ≠ ∇ d k D⎜⎜ ⎝ di − d k ⎠ 16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

Proof for GVG (cont.) •

For m-Dimensions

⎛ di1 ⎜ ⎜ f :⎜ ⎜ ⎜ ⎜ ⎝ di 1

− di2 . . .

⎞ ⎟ ⎟ ⎟, where f : R m → R m−1 ⎟ ⎟ − dim ⎟⎠

By Pre - Image Theorem, dim( f −1 ) = m − ( m − 1) = 1 16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

Traceability in m dimensions •

x is a point on the GVG – normal slice plane

Pass a hyperplane through the m closest points on the m closest obstacles

y = ( z2 ,... zm )

– “sweep” coordinate

•

λ = z1

Define

⎡ ( d 1 − d 2 )( y , λ ) ⎤ ⎢ ( d − d )( y , λ ) ⎥ 3 ⎥ ⎢ 1 ⎥ ⎢ . G (y , λ ) = ⎢ ⎥ . ⎥ ⎢ ⎥ ⎢ . ⎥ ⎢ d d y ( )( , ) − λ ⎥⎦ ⎢⎣ 1 m

Tangent is orthogonal to this hyperplane

where G : R m −1 × R → R m −1

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

Traceability (cont.) Predictor-corrector scheme – Take small step,Δλ in

z1 direction (tangent).

– Correct using iterative Newton’s Method

y k +1 = y k − ( ∇ y G) −1 G( yk , λk )

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

Accessibility Gradient Ascent: Cascading Sequence of Gradient Ascent Operations – Move until

Fij

– Maintain 2-way equidistant while ↑ D

∏

T xFij

∇D =∏T F ∇d i =∏T F ∇d j x ij

x ij

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

GVG Connected?

Fijk ⊆ ∂Fij Fijk ⊆ ∂Fik Fijk ⊆ ∂F jk

Assuming ∂Fij is connected ∀Fij Is GVG connected? 16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

GVG Connected? is not connected

∂Fij

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

GVG2 Second-order two-equidistant surface

Fk |Fij = {x ∈ Fij : ∀h ≠ i , j , k , d h (x ) > d k (x ) > d i ( x ) = d j ( x ) > 0 and ∇ di ≠ ∇d j }

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

Linking to GVG Cycle • •

Detect GVG Cycle Gradient Descent

− ∏T F ∇dk x ij

–

∇d k increases distance to

Ck

–

− ∇d k decreases distance to

Ck

–

∏

–

Tx Fij tangent space of

– − ∏T F

x ij

projection

projection onto the tangent space

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

From GVG2 to GVG Cycle .

c ( x) = −∏T F ∇d k c(t ) x ij Assuming − ∏T F ∇d k is never 0. x ij

di = d j < d k

(d i = d j ) ↑, d k ↓

(d i = d j ) ↓, d k ⇓

di = d j = d k 16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

Two Problems •

Gradient goes to 0?

•

Going on top of the box – Define occluding edges

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

Finding Occluding Edges

Ceiling

Fij

Inner Box Floor

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

Visible Distance Revisited •

Single-object

d i ( x) = •

~

distance to ∞

if c i ∈ C i ( x ) otherwise

Multi-object

x

D( x) = min d i ( x) i

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

Occluding Edges (cont.) •

Change in second closest object – GVG two-equidistant edges (continuous)

– Occluding edges (not continuous)

•

Questions? – When to link? – Do we have all possible edges?

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

More Linking

Fk | Fij = { x ∈ Fij : ∀h ≠ i, j, k ,

d h ( x) > d k ( x) > d i ( x) = d j ( x) > 0 and ∇d i ≠ ∇d j }

• GVG2

dh = dk > di = d j

• Occluding edges

dh > dk > di = d j

• GVG Edge

d h > d k = di = d j > 0

• Boundary Edge

dh > dk > di = d j = 0

• Floating boundary edge

∇di = ∇d j

16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato

Basic Links

16-735, H...