Download - Robotic Motion Planning: Roadmap Methods
![Page 1: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/1.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Robotic Motion Planning:Roadmap Methods
Robotics Institutehttp://voronoi.sbp.ri.cmu.edu/~motion
Howie Chosethttp://voronoi.sbp.ri.cmu.edu/~choset
![Page 2: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/2.jpg)
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.
![Page 3: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/3.jpg)
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
![Page 4: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/4.jpg)
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 roadmapa) 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 mapat 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?
![Page 5: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/5.jpg)
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
![Page 6: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/6.jpg)
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 pathon this roadmap, but itis the shortest path
• If we include the startand goal nodes, theyare automatically connected
• Algorithms for constructing them can be efficient
– O(n^3) brute force
![Page 7: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/7.jpg)
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
![Page 8: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/8.jpg)
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
![Page 9: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/9.jpg)
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.
start
goal
![Page 10: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/10.jpg)
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.
start
goal
![Page 11: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/11.jpg)
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
![Page 12: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/12.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Visibility Graphs
![Page 13: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/13.jpg)
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 pathon this roadmap, but itis the shortest path
• If we include the startand goal nodes, theyare automatically connected
• Algorithms for constructing them can be efficient
– O(n^3) brute force
![Page 14: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/14.jpg)
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
– 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
Analysis: For a vertex, n log n to create initial list, log n for each αiOverall: n log (n) (or n2 log (n) for all n vertices
![Page 15: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/15.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Example
![Page 16: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/16.jpg)
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
![Page 17: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/17.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Voronoi Diagrams
![Page 18: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/18.jpg)
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
xid x x
id∇
iC
X
X
![Page 19: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/19.jpg)
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
})1(],1,0[:{)( free
~QcttxtQOcxC ii ∈+−∈∀∈=
x
iQO
![Page 20: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/20.jpg)
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
• Multi-object
)(min)( xdxD ii=
=)( xd i otherwise)( if todistance
~
∞∈ xCcC iii
iC
jC
x
![Page 21: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/21.jpg)
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
}0)()(:{ free =−∈= xdxdQxS jiij
iQO
jQO
![Page 22: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/22.jpg)
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
Two-equidistant face
},),()()(:{ jihxdxdxdSSxF hjiijij ≠∀≤=∈=
iQO
jQO
)()()( xdxdxd jik =≤
ijSSkQO
![Page 23: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/23.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
General Voronoi Diagram
U U1
1 1
GVD−
= +=
=n
i
n
ijijF
![Page 24: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/24.jpg)
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
![Page 25: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/25.jpg)
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?
vsid∇
jd∇
id∇
jd∇
![Page 26: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/26.jpg)
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?
vsid∇
jd∇
id∇
jd∇
id∇
jd∇
jd∇
id∇
![Page 27: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/27.jpg)
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
Two-equidistant surjective surface
Two-equidistant Face
}0)()(:{ free =−∈= xdxdQxS jiij
jCiC
ijS
id∇
jd∇)}()(:{ xdxdSxSS jiijij ∇≠∇∈=
}),()(:{ ihxdxdSSxF hiijij ≠∀≤∈=
U U1
1 1
GVD−
= +=
=n
i
n
ijijF
![Page 28: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/28.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Curve Optimization Approach: Homotopy Classes
![Page 29: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/29.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Pre-Image Theorem
})(:{)(:1 cxfRxcf
RRfm
nm
=∈=
→−
3 radius with circle a is )9( : ),( e.g.
1
222
−
→+=
fRRfyxyxf
nmcfxDfcfx
−
∈∀−
−
dimension of manifold a is )(then rank, full is )(),( if
1
1
![Page 30: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/30.jpg)
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
)(0)(0
vectorrow 0 anot is )(
rank full is )( Show
1))0()(dim(
rank full is )( s.t. (0))(
0 ,: ),()()(
1
1
jijijiji
ji
ji
ji
jiji
mji
ddDdddddd
ddD
ddD
mnmdd
ddDdd
cRRfxdxdxf
−⇔≠−∇⇔≠∇−∇⇔∇≠∇
−
⇔−
−=−=−⇒
−−
=→−=
−
−
![Page 31: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/31.jpg)
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 unionof 1D manifolds
![Page 32: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/32.jpg)
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)
![Page 33: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/33.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Departability
![Page 34: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/34.jpg)
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 is continuous (Prof. Yap, NYU)
• Im of a connected set, under a continuous map, is a connected set
GVDQ →free:Im
connected. is , ofcomponent connectedeach for free
GVDQQ
![Page 35: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/35.jpg)
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
),()( 11 kky
kk yGGyy λ−+ ∇−=
Correction
Pass a line through two
closest points on two closest
obstacles
Orthogonal is tangent
![Page 36: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/36.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Control Laws
where– α 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.,
D. Siler C. KristoffSBP Class
(x)d(x)d0G(x) : Edge ji −==
G(x)G(x))β(G(x))Null( αx †∇+∇=&
1TT† )G(x))G(x)((G(x))(G(x))( −∇∇∇=∇
⎭⎬⎫
⎩⎨⎧
−−
==(x)d(x)d(x)d(x)d
0G(x) :Point Meet ki
ji
G(x)G(x))β(G(x))Null( αx †∇+∇=&
![Page 37: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/37.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
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.
![Page 38: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/38.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Demo
![Page 39: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/39.jpg)
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
• In m-Dimensionsjkikijijk FFFF ∩∩=
imikijmijk FFFF ∩∩= ......
immij FF ∩= −1...
![Page 40: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/40.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
GVD vs. GVG
m-11mGVG
1m-12GVD
CodimDimEquidistant (#obs)
Proofs by Pre-Image Theorem to come
![Page 41: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/41.jpg)
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
( )
kijiki
ji
ki
ji
dddddddd
D
ff
RRfdddd
f
∇≠∇∇≠∇≠⎟⎟⎠
⎞⎜⎜⎝
⎛−−
=⎟⎟⎠
⎞⎜⎜⎝
⎛
→⎟⎟⎠
⎞⎜⎜⎝
⎛−−
=
−−
, since ,0
000
: ,
11
23
![Page 42: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/42.jpg)
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
1)1()dim( Theorem, Image-PreBy
: where,...
:
1
1
1
21
=−−=
→
⎟⎟⎟⎟⎟⎟
⎠
⎞
⎜⎜⎜⎜⎜⎜
⎝
⎛
−
−
−
−
mmf
RRf
dd
dd
f mm
ii
ii
m
![Page 43: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/43.jpg)
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
– “sweep” coordinate
• Define
),...( 2 mzzy =
1z=λ
11
1
31
21
: where
),)((...
),)((),)((
),( −− →×
⎥⎥⎥⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢⎢⎢⎢
⎣
⎡
−
−−
= mm
m
RRRG
ydd
yddydd
yG
λ
λλ
λ
Tangent is orthogonal to this hyperplane
Pass a hyperplane through the m closest points on the m closest obstacles
![Page 44: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/44.jpg)
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 direction (tangent).
– Correct using iterative Newton’s Method
1zλΔ
),()( 11 kky
kk yGGyy λ−+ ∇−=
![Page 45: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/45.jpg)
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
– Maintain 2-way equidistant while
ijF
D↑
∏∏∏ ∇=∇=∇ijxijxijx FT jFT iFT
ddD
![Page 46: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/46.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
GVG Connected?
jkijk
ikijk
ijijk
FF
FF
FF
∂⊆
∂⊆
∂⊆
connected? Is
connected is Assuming
GVG
FF ijij ∀∂
![Page 47: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/47.jpg)
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
ijF∂
![Page 48: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/48.jpg)
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
} and
0)()()()(
,,,:{|
ji
jikh
ijFk
dd
xdxdxdxd
kjihFxFij
∇≠∇
>=>>
≠∀∈=
![Page 49: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/49.jpg)
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
– increases distance to
– decreases distance to
– projection
– tangent space of
– projection onto the tangent space
kd∇
∏ ∇−ijxFT kd
kd∇−kC
kC
∏
ijxFT
∏−ijxFT
![Page 50: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/50.jpg)
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
∏ ∇−=ijxFT k tcdxc )()(
.
0.never is Assuming ∏ ∇−ijxFT kd
kji ddd <=
⇓↓= kji ddd ,)( ↓↑= kji ddd ,)(
kji ddd ==
![Page 51: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/51.jpg)
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
![Page 52: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/52.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Finding Occluding Edges
InnerBox
Ceiling
Fij
Floor
![Page 53: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/53.jpg)
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
• Multi-object
)(min)( xdxD ii=
=)( xd i otherwise)( if todistance
~
∞∈ xCcC iii
iC
jC
x
![Page 54: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/54.jpg)
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?
![Page 55: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/55.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
More Linking
} and
0)()()()(
,,,:{|
ji
jikh
ijFk
dd
xdxdxdxd
kjihFxFij
∇≠∇
>=>>
≠∀∈=
jikh dddd =>=
jikh dddd =>>
0>==> jikh dddd
0==>> jikh dddd
ji dd ∇=∇
• GVG2
• Occluding edges
• GVG Edge
• Boundary Edge
• Floating boundary edge
![Page 56: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/56.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Basic Links
![Page 57: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/57.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Room with Box
![Page 58: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/58.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Floating Box
![Page 59: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/59.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Box with Opening
![Page 60: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/60.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
This is COMPLICATED!
![Page 61: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/61.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Topological Maps (Kuipers)
• Topological map represents spatial properties of actions and of places and paths in the environment. Topological map is defined as the minimal models of an axiomatic theory describing the relationship between the different sources of information explained by map (Remolina and Kuipers, Artificial Intelligence, 2003)
• Topological maps represent the world as a graph of places with the arcs of the graph representing movements between places (Kortenkamp & Weymouth, AAAI-94)
• Topological maps represent the robot environment as graphs, where nodes corresponds to distinct places, and arcs represent adjacency. A key advantage of topological representations is their compactness (Thrun, et at. 1999)
• Topological localization uses a graph representation that captures the connectivity of a set of features in the environment (Radhakrishnan & Nourkbash, IROS 1999)
![Page 62: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/62.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Topology (Really, Connectivity)
• A topology is a collection T of subsets of X– ∅, X ∈ T, a1 ∪ a2 ∪ … ∈ T, a1 ∩ a2 ∩ … ∩ an ∈ T– What is the relevance?
• path connectedY-axis ∪ graph of sin(1/x)
not connected Connected, path connected
connected, but not path connected
• simply connected (contractible)
simply connected not simply connected
a(t)
b(t)
a(t)
b(t)
![Page 63: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/63.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
• A is a deformation retract iff H:X×[0,1]→X, H(x,0)=x and H(x,1)∈A ∀x∈X, and H(a,t)=a ∀a∈A, t∈[0,1].
– H is called a deformation retraction
• Fundamental group π1 : set of path-homotopy classes– X is simply connected if π1 is the trivial (one-element)
Homotopy
• Two paths f, f' : [0,1] → X, are path-homotopicF(s,0)=f(s) and F(s,1)=f'(s); F(0,t)=x0 and F(1,t)=x1 ∀∈s [0,1], ∀∈t [0,1]
obstacle• Path-Homotopy class [f] set of the mappings that are path-homotopic to f.
a(t)
b(t)
![Page 64: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/64.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Deformation Retraction: GVG in Plane
![Page 65: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/65.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Topological Map: Good and Bad
• Topological Map: For each homotopy class in free space, there is a corresponding homotopy class in the map.
Good Topological map : the first fundamental groups have the same cardinality
Bad Topological map : redundant homotopy classes in the map
![Page 66: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/66.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Bad Topological Maps in Higher Dims
• In general, there cannot be a one-dimensional deformation retract in a space with dimension greater than two → There can not be “good” one-dimensional topological maps for
ℜ3
• GVG in ℜ3 : Not a good topological map
loop
HGVG in R3
![Page 67: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/67.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Application of Topological Maps:Sensor Based Planning for a Rod Robot:
• Challenges– Three and Five dimensional Space– Non-Euclidean– Sensor Based Approach– Workspace -> Configuration Space
• Piecewise Retract of R2 x S1 and R3 x S2
RodSnake
q = (x, y, θ)
rcminqDR(q)r ,Cci
i
−=∈∈
)(
Ci
![Page 68: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/68.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Rod-HGVG
Rod-GVG’s Di(q) = Dj(q) = Dk(q)(Diffeo to S1 if rod is small enough)
C1
C2
C3
C5 C6
J125
J235 J356 J346
J146J156
Piece-wise retractRetract in Cspace CellsPoint-GVG connects retracts
C4
1tan edges, Tan to Pt GVGDi(q) = Dj(q)
![Page 69: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/69.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Ci Cj
Ck
Junction Region Jijk
Piece-wise Retract:A Bad Topological Map Containing Good Topological
Submaps
• Accessibilty: path between any configuration and roadmap
• Deformation Retraction:– H(q, t) : SE(2) × [0,1] → CFijk
– H(q,0) = q – H(q,1) = a configuration on the
roadmapθ(H(q,1)) = θ(q)
• H(q, t) is continuous in Junction Region
• Sensor-Based Implementation
![Page 70: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/70.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
More Piece-wise Retracts
• Rod-HGVG in Three-Dimension– Rod-GVG edges – 1-tan edges– 2-tan edges
Maps Comprising Good Sub Maps
Sensor-Based Deployment
• Convex GVG in the plane– Convex GVG edges – Fat 1-tan edges
![Page 71: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/71.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Implication for complexity??
• Base and Fiber Variables (Ostrowski and Burdick)
• Internal Shape and Position Variables
• Purely configuration and Workspace Variables
![Page 72: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/72.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Topological Simultaneous Localization & Mapping
NodeDistance to nearby obstaclesNumber of emanating edges
Departure angles
EdgePath Length“Correspondence”
![Page 73: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/73.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Application: Hierarchical SLAM
Implement a feature-based technique in a topological framework
SubmapChong
AtlasBosse & Leonard
Work with Kantor
![Page 74: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/74.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Embedded H-SLAM Map
Free Space with Topological Map (GVG)
![Page 75: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/75.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Embedded H-SLAM Map
Free Space with Topological Map (GVG) + 1 Edge-map
![Page 76: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/76.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Embedded H-SLAM Map
Free Space with Topological Map (GVG) + 2 Edge-maps
![Page 77: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/77.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Experimental Platform
![Page 78: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/78.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Experimental Results
Odometry
Feature-maps tied to meetpoint locations
![Page 79: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/79.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Local Edge-Map Example
Region of interest on the sixth floor of Wean Hall
![Page 80: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/80.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Local Edge-Map Example
Edge environment
![Page 81: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/81.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Local Edge-Map Example
Bounding meet-points
![Page 82: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/82.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Local Edge-Map Example
Robot trajectory along the GVG
![Page 83: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/83.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Local Edge-Map Example
Embedded edge-map
![Page 84: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/84.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Benefits & Drawbacks of Topological Maps
• Scale– dimension– geometric size
• Reduce planning problem– Graph search– Localization along a “line”
• Induces a hierarchy of maps for SLAM– T: Topological (Kuipers, Choset) – F: Feature-based (Leonard, Durrant-Whyte)– L: Local/Pixel-based (Morevac, Elfes, Thrun)– D: Dead-reckoning (Borenstein)
• Provides sensor space decomposition useful for control
– Brooks and other: Behaviors – sense/act– Brockett; Manikonda, Krishnaprasad, and Hendler – Motion Description
Languages– Rizzi, Burridge, Koditschek – Hybrid Controls– Kuipers and Choset – Topological Maps
• Cannot position in arbitrary locations
• Fails when environments is not topologically “rich”
– Hyper-symmetric– Large open spaces
![Page 85: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/85.jpg)
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 AlgorithmThe Opportunistic Path Planner
![Page 86: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/86.jpg)
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 ellipsoidwith a through hole.Pc is a hyperplaneof codimension 1( x = c ) which will beswept through S inthe X direction.
![Page 87: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/87.jpg)
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 pointthe slice travelsalong X we’llfind the extremain S∩Pc in the Ydirection. If wetrace these out weget silhouettecurves.
![Page 88: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/88.jpg)
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.
![Page 89: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/89.jpg)
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 acritical point to the restof the silhouette curvewith a path that lieswithin S∩Pc. This canbe done by running thealgorithm recursively.Each time, we increasethe codimension of thehyperplane by 1.
![Page 90: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/90.jpg)
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
![Page 91: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/91.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Another Example (1)
![Page 92: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/92.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Another Example (2)
![Page 93: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/93.jpg)
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 roadmapwe treat the slices which contain qs and qg
as critical slices and run the algorithm thesame way.
![Page 94: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/94.jpg)
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)
![Page 95: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/95.jpg)
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)
![Page 96: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/96.jpg)
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)
![Page 97: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/97.jpg)
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)
![Page 98: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/98.jpg)
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 clearconceptually, let’s establish themathematical machinery to actuallyconstruct the roadmap. We must define• The sets• The slices• How to find extrema• How to find critical points
![Page 99: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/99.jpg)
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 aresemi-algebraic sets that are closed and compact.
Def: A semi-algebraic set S⊆ℜr defined by thepolynomials F1,..,Fn∈Qr is a set derived from thesets
Si={x∈ℜr | Fi(x)>0}by finite union, intersection and complement.
Ex: (x2+y2 ≤ 1) ∧ (z ≤ 1) ∧ (z ≥ -1)
![Page 100: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/100.jpg)
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’sestablish the mathematical machinery to actually constructthe roadmap.
SS
cSxPSS
cc
cc
=
=∈=∩=
U}:{ 1π
knk xxxx =),....,,( 21π
The slices are the intersection of a hyperplane and S
where π1 is the projection on to the first coordinate
![Page 101: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/101.jpg)
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, welook for extrema of π2|Sc, the extrema of theprojection of Sc in a second direction.
In order to find the extrema on a manifoldwe will refer to the Lagrange MultiplierTheorem.
![Page 102: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/102.jpg)
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 ∀q∈S.Suppose h:U→ℜ is a smooth function andp∈S is a extremum point of h on S.Then ∃λ∈ℜ s.t. ∇h(p)= λ∇ f(p) (they areparallel)
![Page 103: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/103.jpg)
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 solidunit sphere). Extrema of h=π1(x,y,z)=(x).
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡= 001
222),(
zyxhfd
y = z = 0 (y-z plane) and only points on sphere is x = 1, x = -1, left most and right most
![Page 104: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/104.jpg)
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 ofthe Lagrange MultiplierTheorem:Suppose that U is an opensubset of the kernel of somemap f:ℜr→ℜn, and let f betransversal to {0}. Letg:ℜr→ℜm be a map, thenx∈U is an extremum of g|Uiff the following matrix is notfull rank.
⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢
⎣
⎡
∂∂
∂∂
∂∂
∂∂
∂∂
∂∂
∂∂
∂∂
=
)()(
)()(
)()(
)()(
),(
1
1
1
1
1
1
1
1
xxgx
xg
xxgx
xg
xxfx
xf
xxrfx
xf
gfd
r
mm
r
r
nn
x
L
MM
L
MM
L
![Page 105: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/105.jpg)
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.
![Page 106: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/106.jpg)
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 solidunit sphere). If we sweep in the x direction andextremize in the y direction h=π12(x,y,z)=(x,y).
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
010001222
),(zyx
hfd
So the silhouette curve is the unit circle on the x-y plane
![Page 107: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/107.jpg)
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 inconnectivity of the silhouette curves also followfrom Canny’s Generalization. They are theextrema of the projection on to the sweepingdirection 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
![Page 108: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/108.jpg)
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 Dfform 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
![Page 109: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/109.jpg)
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
![Page 110: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/110.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Between Critical Points
![Page 111: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/111.jpg)
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
![Page 112: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/112.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Proof of Connectivity (1)
First, let S|≤c denote the set S∩(x≤c). We areclaiming that R(S) |≤c is connected within eachcomponent of S|≤c.
Base case: c is small enough such that S|≤c isempty and the claim is vacuously true
Induction hypothesis: The claim is true for some c=c0
![Page 113: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/113.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Proof of Connectivity (2)
Inductive step:It remains true as c is increased until we comeupon another critical value c1 associated with acritical point p.
If the algorithm works on slice S∩Pc1, then if anew component of silhouette appears or if severalcomponents of S come together at p, they will bejoined recursively by the algorithm. Therefore theclaim is true for c1 and all other critical values.
![Page 114: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/114.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
The Opportunistic Path Planner
The Opportunistic Path Planner is similar toCanny’s Roadmap but differs in the followingways• Silhouette curves are now called freeways and are constructed slightly
differently• Linking curves are now called bridges• It does not always construct the whole roadmap• The algorithm is not recursive
![Page 115: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/115.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Channels (1)
Def: A channel slice is a slice at which the connectivity of the intersection with the sweeping hyperplane and the freespacechanges.
Def: A channel is a subset of the freespacewhich is bounded by channel slices and configuration space obstacles
![Page 116: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/116.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Channels (2)
![Page 117: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/117.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Interesting Critical Points and Inflection Points
Def: An interesting critical point is a criticalpoint that corresponds to the joining orsplitting of the intersection of the sweepinghyperplane and the freespace
Def: An inflection point is a point where thetangent to the freeway curve becomesorthogonal to the sweep direction
![Page 118: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/118.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Freeways
Freeways are defined by the following artificialpotential field which induces an artificial repulsionfrom the surface of obstacles
)()( xDxUart =
A freeway is the locus of the maxima of Uart(x) as you sweep through the configuration space
![Page 119: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/119.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Bridges
Def: A bridge is a one-dimensional set which links freeways from channels that have just joined or are about to split (as you sweep across)
![Page 120: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/120.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Freeway Tracing
![Page 121: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/121.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Freeway Tracing
Freeway tracing is done by tracking thelocus of the maxima of the artificial potentialfield and terminates when(1) The freeway runs into an inflection point where you create a
bridge(2) The freeway runs into an obstacle where it ends
![Page 122: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/122.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Also create bridges at interesting critical points
![Page 123: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/123.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Accessibility and Departability
The roadmap is accessed and departed byconnecting qs and qg to a local maximum on theslice which they reside (which is part of a freeway).
This is referred to as hill-climbing and is the sameprocedure we use when creating bridges except inthe case of bridges we hill-climb in two directions.
![Page 124: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/124.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Building the Roadmap
(1) Hill-climb from both qs and qg. Then trace freeway curves from both start and goal
(2) If the curves leading from start and goal are not connected enumerate a split point or join point and add a bridge curve near the point. Else stop.
(3) Find all points on the bridge curve that lie on other freeways and trace from these freeways. Go to step 2.
![Page 125: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/125.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Example
![Page 126: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/126.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Nonsmooth Analysis
![Page 127: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/127.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Proof of Connectivity (1)
Let A be the set of all x1-coordinates (sweepingdirection) of critical points which are order inascending order A = {a1,a2,…..,am}
Base case: x1=a1, S|≤a1 should consist of a singlepoint which will be part of the roadmap
Inductive hypothesis: The roadmap condition issatisfied for x1≤ai-1
![Page 128: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/128.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Proof of Connectivity (2)
Inductive step:It is a fact that you can smoothly deform or retracta manifold or union of manifolds in the absence ofcritical points. So S|<ai can be smoothly retractedon to S|≤ai-1 because (ai-1,ai) is free of criticalpoints.
![Page 129: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/129.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Proof of Connectivity (3)
Inductive step (continued):Also, R(S)|<ai can be retracted on to R(S)|≤ai-1.This implies that there are no topological changesin R(S) or S on the interval (ai-1,ai) and if R(S)|≤ai-1
satisfies the roadmap condition so does R(S)|<ai
![Page 130: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/130.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Proof of Connectivity (4)
Inductive step (continued):Let pi be the critical point that corresponds to ai.As xi increases to ai the only way connectivity canbe lost is if pi is an inflection point or a join point.Both of these situations will be handled by theapplication of hill-climbing which will create linkingcurves. Therefore the roadmap condition holds forR(S)|≤ai and our inductive step is proven.
![Page 131: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/131.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Assumptions
• Robot is a point• Workspace contains only convex obstacles• Non-convex obstacles are modeled as the union of convex
obstacles• Bounded space
IntroductionDistance FunctionGVG and Pre-Image TheoremNumerical Curve TracingDefinition of HGVG
![Page 132: Robotic Motion Planning: Roadmap Methods](https://reader034.vdocuments.us/reader034/viewer/2022052515/58945b401a28ab73208b7992/html5/thumbnails/132.jpg)
16-735, Howie Choset, with significant copying from G.D. Hager who loosely based his notes on notes by Nancy Amato
Summary
• Roadmap methods create a graph of “roads” that will move you through the space; just get on and get off again
The visibility graph is one method of doing this for polygonal worlds
Voronoi diagrams are a second form of roadmap
We will see more graphs in the second half of the semester...