Roadmap-Based
d
d
Path Planning
Chapter 7
Objectives
ƒ Understand the definition of a Road Map
ƒ Investigate
g
techniques
q
for roadmap-based
p
goalg
directed path planning in 2D environments
– geometry
geometry-based
based algorithms that decompose the environment
into regions between which robot can travel
– sampling-based algorithms that choose fixed or random
l
locations
in the environment and
d then interconnect them to
form potential paths.
ƒ To
T understand
d t d some issues
i
in
i applying
l i
th
these
algorithms to real robots
Chapter 7 – Roadmap-Based Path Planning
7-2
Winter 2012
What’s in Here ?
ƒ Road Maps
ƒ Geometry-Based Road Maps
– Visibility
y Graph
p Paths
-
Shortest Paths in 2D Among Obstacles
-
Shortest Paths in a Grid
-
Real Robot Shortest Paths
– Triangulation Dual Graph Paths
– Generalized Voronoi Diagram Paths
– Cell Decomposition Paths
-
Trapezoidal
p
Decomposition
p
-
Canny’s Silhouette Algorithm
-
Boustrophedon Decomposition
ƒ Sampling-Based
p g
Road Maps
p
– Grid Based Sampling
– Probabilistic Road Maps
– Rapidly Exploring Random Tree Maps
Chapter 7 – Roadmap-Based Path Planning
7-3
Winter 2012
Road Maps
ƒ A Road Map is:
– a kind of topological map
– represents a set off paths
h (or
( roads)
d ) between
b
two points in the environment that the robot
can travel on without collision
ƒ Road Maps assume that global knowledge of the
environment is available.
available
ƒ They are commonly used to compute pre-planned
paths.
paths
– i.e., the first step towards goal-directed path planning
Chapter 7 – Roadmap-Based Path Planning
7-4
Winter 2012
Road Maps
ƒ Usually, the set of paths are stored as:
– a graph of nodes and edges
– a raster grid
by cell
id (graph
(
h is
i implied
i
li d b
ll arrangement))
ƒ Usually, the graph is pre-computed ahead of time
without
i h
k
knowledge
l d off start/goal
/
l llocations.
i
– start/goal locations are given later as a query.
– a few ed
edges
es a
are
e ssometimes
metimes added tto graph
aph tto a
answer
swe q
query
e
ƒ In all cases, the graph is searched to find an efficient
(e
(e.g.,
shortest)
sh test) path to
t the goal.
al
– usually Dijkstra’s algorithm, A* or something similar
Chapter 7 – Roadmap-Based Path Planning
7-5
Winter 2012
Road Map Algorithms
ƒ They are categorized into two main categories:
– Geometry-based algorithms
– Sampling-based
li
b d algorithms
l
i h
ƒ Geometry-based algorithms use computational
geometry methods
h d to compute nodes
d and
d graph
h
edges based on various constraints.
ƒ Sampling-based algorithms select random robot
configurations (e.g., points) as nodes and then
interconnect them based on some constraints.
Chapter 7 – Roadmap-Based Path Planning
7-6
Winter 2012
Geometry-Based
d
Road Maps
Geometry-Based Road Maps
ƒ There are a few that we will look at based on:
– Visibility graphs
– Triangulations
i
l i
d l graphs
dual
h
– Generalized Voronoi diagrams
– Cell decompositions
• Trapezoidal decompositions
• Boustrophedon decompositions
• Canny’s Silhouette algorithm
Chapter 7 – Roadmap-Based Path Planning
7-8
Winter 2012
Visibility Graph Paths
Shortest Path Problem
ƒ How do we get a robot to move efficiently without
collisions from one location to another ?
start
Chapter 7 – Roadmap-Based Path Planning
end
7-10
Winter 2012
Shortest Path Problem
ƒ Moving without collisions is simple with adequate
sensors, but how do we direct it towards a goal ?
ƒ What if the environment is complex ?
end
start
Chapter 7 – Roadmap-Based Path Planning
7-11
Winter 2012
Shortest Path Problem
ƒ Need to examine the map and plan a path
– Consider simpler problem where robot is a point & obstacles
are convex.
convex
end
start
Chapter 7 – Roadmap-Based Path Planning
7-12
Winter 2012
Shortest Path Properties
start
ƒ Shortest path will travel around
obstacles, touching boundaries.
ƒ Consider the robot standing at point s.
end
ƒ Determine
D t
i
supportt lines
li
off polygon
l
p:
– A support line is a line intersecting p such that p lies
completely
l t l on one side
id off that
th t line.
li
pL
– exactly two calledleft support and
right support lines.
s
p
– defined by 2 vertices of p called left
& right support vertices (pL & pR)
Chapter 7 – Roadmap-Based Path Planning
pR
7-13
Winter 2012
Shortest Path Properties
ƒ Can find pL and pR by checking each vertex using
left/right turn test:
pi-1
– For co
convex
vex polygo
polygons,
s pi = pL (resp.
(resp pR)
if both spipi-1 and spipi+1 are right
(resp. left) turns.
(resp
turns
pi+1
s
spipi-1 is left turn, proving
that pi is not left support
support.
– Just compute:
pi
t1 = (pix-sx)(pi+1y-sy)-(piy-sy)(pi+1x-sx)
t2 = (pix-sx)(pi-1y-sy)-(piy-sy)(pi-1x-sx)
IF ((t1 < 0) AND (t2 < 0)) THEN pL = pi
IF ((t1 > 0) AND (t2 > 0)) THEN pR = pi
Chapter 7 – Roadmap-Based Path Planning
pi
s
pi-1
pi+1
Now both
spipi-1 both
spipi+1 is right
turn, proving
that pi is the
left support.
support
7-14
Winter 2012
Shortest Path Properties
ƒ This support-finding algorithm can take O(n) time
but it is practical for small polygons.
ƒ A more efficient algorithm can use a binary search
for the left/right support vertices in O(log n) time.
Can YOU do this ?
ƒ There are some numerical issues with collinearity:
Slight bend may result in near collinearity. Even 3 collinear points may give a
cross product result of 0
0.000000023
000000023 which can register as a right turn!!!
ƒ May have to allow for computational margins.
Chapter 7 – Roadmap-Based Path Planning
7-15
Winter 2012
Shortest Path Algorithm
ƒ We can now apply this by finding all support vertices
of our obstacles:
We will include our destination
as a special case in our set of
support lines.
end
start
Chapter 7 – Roadmap-Based Path Planning
7-16
Winter 2012
Shortest Path Algorithm
ƒ Now determine which support lines represent valid
paths for the robot to travel (i.e., the visible support vertices):
end
start
Chapter 7 – Roadmap-Based Path Planning
7-17
Winter 2012
Shortest Path Algorithm
ƒ Do this by eliminating any support line segments
that intersect another polygon.
Can compare every polygon
edge with a support line
segment for intersection and
remove all that intersect other
than at the support endpoints.
end
start
Chapter 7 – Roadmap-Based Path Planning
7-18
Winter 2012
Shortest Path Algorithm
ƒ Since obstacles are convex, it is enough to compare
support lines against line segments joining polygon
support vertices:
2n+1 = O(n)
support lines
O(n) polygons
end
start
Simple iterative
intersection test for
all pairs takes O(n2)
time.
Chapter 7 – Roadmap-Based Path Planning
7-19
Winter 2012
Line Intersection test
ƒ How do we check for line-segment intersection ?
(x3,y3)
(x2,y2)
(x,y)
(x1,y1)
(x4,y4)
ƒ Can
C
use well-known
ll k
equation
ti
off a line:
li
y = max + ba
y = mbx + bb
where
ma = (y2 – y1) / (x
( 2 – x 1)
Must handle special case
where lines are vertical.
(i.e., x1 = x2 or x3 = x4)
mb = (y4 – y3) / (x4 – x3)
ba = y1 – x1ma
bb = y3 – x3mb
Chapter 7 – Roadmap-Based Path Planning
7-20
Winter 2012
Line Intersection test
ƒ Intersection occurs when these are equal:
max + ba = mbx + bb
→ x = (bb – ba) / (m
( a – mb)
ƒ If (ma = mb) the lines are parallel and there is no
i
intersection
i
ƒ Otherwise solve for x, plug back in to get y.
ƒ Final test is to ensure that intersection (x, y) lies on
line segment … just make sure that each of these is
true:
(x3,yy3)
(x y)
(x,y)
– max(x1, x2) ≥ x ≥ min(x1, x2)
– max(x3, x4) ≥ x ≥ min(x3, x4)
Chapter 7 – Roadmap-Based Path Planning
(x2,y2)
(x1,y1)
(x4,y4)
7-21
Winter 2012
More Efficient Approach – Radial Sweep
ƒ More efficient approach can compute and remove all
intersections in O(n log n) time by using a radial
sweep.
1 : Sort support
st
lines radially with
respect to s.
2nd: Choose a right
support line that you
know is visible. In a
rare case, none are
visible, then we must
split polygons.
end
d
s
Chapter 7 – Roadmap-Based Path Planning
7-22
Winter 2012
More Efficient Approach – Radial Sweep
ƒ 3rd: Do a radial sweep, keeping track of the closest
(visible) polygon.
When reaching a
right support
vertex, check if it
is in front or
behind the
current polygon.
end
d
s
Not visible since it is
behind (i.e., intersects)
current visible polygon.
Current visible polygon
Chapter 7 – Roadmap-Based Path Planning
7-23
Winter 2012
More Efficient Approach – Radial Sweep
ƒ When left support vertex encountered:
If it belongs to
current visible
polygon, mark it
as visible and
then set current
polygon to null.
null
d
end
s
When left support
vertex
t reached,
h d sett
current visible
polygon to null.
Chapter 7 – Roadmap-Based Path Planning
7-24
Winter 2012
More Efficient Approach – Radial Sweep
ƒ When left support vertex encountered:
If it does not belong
to current visible
polygon, the vertex
is not visible, so
discard it.
Discard these since they are left
vertices that do not belong to current
visible polygon.
end
s
Chapter 7 – Roadmap-Based Path Planning
7-25
Winter 2012
Shortest Path Algorithm
(Continued)
ƒ Repeat this process iteratively by letting each visible
support vertex become point s
(i e assume
(i.e.,
robot
e g , co
e.g.,
compute
pute tthe
e visibility
s b ty as if
the robot was now here.
moved
th
there):
)
end
More visible nodes. We will
compute visibility from these
as well.
Chapter 7 – Roadmap-Based Path Planning
7-26
Winter 2012
Shortest Path Algorithm
(Continued)
ƒ By appending all these visible segments together, a
visibility graph is obtained:
end
start
Chapter 7 – Roadmap-Based Path Planning
7-27
Winter 2012
Shortest Path Algorithm
(Continued)
ƒ We can then search this visibility graph for the
shortest path from the start to the goal:
end
start
Chapter 7 – Roadmap-Based Path Planning
7-28
Winter 2012
Shortest Path in Graph
ƒ We can use Dijkstra’s shortest path algorithm to
compute the shortest path in this graph.
– Takes O(V log V + E) time for a V-vertex
V vertex / E-edge
E edge graph
1 function Dijkstra(G, s, t)
2
3
4
for (each vertex v in V[G]) do
d[v] = infinity
previous[v] = undefined
5
d[s] = 0
7
while (Q is not empty) do
6
8
9
10
11
12
13
14
queue of all vertices
Q=q
Initialize all distances to vertices (i.e., d[v]) to ∞, except
start which has distance 0
Remembers how we got
g to this node.
Priority queue sorted by distances from s.
u = Extract-Min(Q)
if (u == t) then return;
Get the next closet unprocessed vertex. If it
is the destination, we are done.
for each edge (u,v) outgoing from u do
if (d[v] > d[u] + dist(u,v)) then
d[v] = d[u] + dist(u,v)
previous[v] = u
Q = Update(Q)
Chapter 7 – Roadmap-Based Path Planning
Relax all edges from u to v, by updating
(reducing) the cost d[v] at each v if can be
reached quicker from u.
Re sort the queue since priorities may have changed
Re-sort
changed.
7-29
Winter 2012
Shortest Path in Graph
ƒ Alternatively, we can use the A* algorithm.
– employs "heuristic estimate" that ranks each node by an
estimate of the best route that goes through that node
node.
ƒ Dijkstra employs breadth-first-search, while A* does
a best-first-search.
best-first-search
A* has more
Dijkstra has no
particular focus,
all nodes treated
equal.
s
t
focused
propagation
pattern.
s
t
ƒ Can be just as bad as Dijkstra in worst case but often
much quicker in practice.
Chapter 7 – Roadmap-Based Path Planning
7-30
Winter 2012
Shortest Path in Graph
ƒ A* algorithm is the same as Dijkstra’s except that:
– In Dijkstra’s
j
alg.,
g nodes vi in q
queue are sorted by
y d[v
[ i]
– In A*, they are sorted by d[vi] + E[vi,t] where E[vi,t] is an
underestimate of the distance from vi to t.
– Only the Extract-Min(Q) function of line 8 is affected.
ƒ E is often simply the straight line distance from vi’ss
coordinate to t’s coordinate.
– This is always
l
s an underestimate
d
sti
t since
si
the
th reall cost
st from
f
vi to
t
t can never be greater than the straight line cost.
Chapter 7 – Roadmap-Based Path Planning
7-31
Winter 2012
Shortest Path Traversal
ƒ From this produced path, we have a set of points
and angles.
ƒ We can then apply inverse kinematics for our robot
to move it along this path.
ƒ But wait a minute! Our robot is not a point, it’s a
rectangle. We cannot simply hug along the obstacle
b
boundaries!
d ies!
Chapter 7 – Roadmap-Based Path Planning
7-32
Winter 2012
Real Robot Shortest Path
ƒ Our real-robot will collide with obstacles if it
travels along a path computed as we described:
end
start
Chapter 7 – Roadmap-Based Path Planning
7-33
Winter 2012
Real Robot Shortest Path
ƒ We need a kind of “safety buffer” around each
obstacle according to the robot’s size & shape:
As long as robot
robot’s
s center stays
outside safety buffer, robot’s body
won’t hit the actual obstacle.
In some cases,
there may be no
safe way between
the obstacles
(usually when two
buffers intersect).
Chapter 7 – Roadmap-Based Path Planning
Buffer keeps robot safely
away from obstacle edges.
We need
W
d to
t
compute these
safety buffers by
“growing” the
obstacles by an
amount that
reflects the
robot’s shape
and size.
7-34
Winter 2012
Real Robot Shortest Path
ƒ If robot is symmetrical in all directions, we can still
work with our same algorithm.
– Only circles are symmetrical in all directions.
p
assume a square.
– For simplicity,
Our robot
Robot’s area
as it spins.
Chapter 7 – Roadmap-Based Path Planning
A circle that
models our
robot in any
orientation
orientation.
We will use this point (arbitrarily
chosen) as our reference point.
We will assume a square
model such that robot is
contained in this square in
any
y orientation.
7-35
Winter 2012
Real Robot Shortest Path
ƒ Can apply a “growing” procedure to each obstacle:
– Determine edge vectors along
m4
m3
model
d l iin CCW
CC order:
d
m1
m2
p1
– Determine edge vectors along
p2
polygon in CW order:
p4
p3
– Sort combined edge vectors
p4
m4
m2
by angle
p1
p2
Chapter 7 – Roadmap-Based Path Planning
p3
m3
m1
7-36
Winter 2012
Real Robot Shortest Path
ƒ Traverse the vectors radialy clockwise
starting at m1.
mi and polygon vector pi, translate the
model such that mi = pi
m4 m3
p4
p2
p4
m3
m4
– When sweeping between model vector
p1
p3
m1 m2
m4 m3
m2
p1
p2
m1
Connect reference
points of all model
translations to form
the grown obstacle
which will have at
most n+4 vertices.
m1 m2
p3
m4 m3 m4 m3
m1 m2 m1 m2
Chapter 7 – Roadmap-Based Path Planning
7-37
Winter 2012
Real Robot Shortest Path
ƒ It is easy to see that as long the reference point of
our model lies on our outside the grown obstacle,
then the robot will not collide with the real obstacle.
Reference point lies
on or outside
t id grown
obstacle.
Chapter 7 – Roadmap-Based Path Planning
7-38
Winter 2012
Real Robot Shortest Path
Grown obstacles may overlap …
indicating that robot cannot travel
safely in between.
ƒ Apply this
to all
obstacles
to obtain
the
grown
t
s
obstacle
space.
Chapter 7 – Roadmap-Based Path Planning
7-39
Winter 2012
Real Robot Shortest Path
ƒ Now apply previous point-robot algorithm.
t
s
New starting and
destination points are
found by centering robot
model shape about
original start and
destination, then using
the reference points.
points
Chapter 7 – Roadmap-Based Path Planning
7-40
Winter 2012
Real Robot Shortest Path
ƒ Robot now moves safely along path:
Robot avoids
b
boundaries
d i off
real obstacles.
t
s
Chapter 7 – Roadmap-Based Path Planning
7-41
Winter 2012
Real Robot Shortest Path
ƒ But our robot should be able to fit in between those
obstacle. Why doesn’t our solution allow this ?
Robot will fit at
certain angles.
Chapter 7 – Roadmap-Based Path Planning
7-42
Winter 2012
Real Robot Shortest Path
ƒ Use a more
Robot will fit through
here now.
accurate
model
to produce
a more
accurate
path.
Robot
R
b still
ill cannot
fit through some
places.
Chapter 7 – Roadmap-Based Path Planning
7-43
Winter 2012
Real Robot Shortest Path
ƒ We could use a more complicated approach that
allows the robot to pass through certain
areas only
l iin specific
ifi directions.
di
ti
– can shrink the model .
– must allow model to rotate.
course but can be done
done.
– Too complicated for us in this course,
– Realistically, robot sensors are not reliable enough
nor accurate enough to ensure safe travel within areas that
require a small margin of error.
Chapter 7 – Roadmap-Based Path Planning
7-44
Winter 2012
Non–Convex Obstacles
ƒ What about non-convex obstacles ?
– Can divide them into convex polygons and then apply the
same algorithms (although better solutions exist).
exist)
end
start
Chapter 7 – Roadmap-Based Path Planning
7-45
Winter 2012
Non-Convex Obstacles
ƒ What about non-convex obstacles ?
– Can divide them into convex polygons and then apply the
same algorithms (although better solutions exist).
exist)
end
start
Chapter 7 – Roadmap-Based Path Planning
7-46
Winter 2012
Shortest Paths in Grids
ƒ How do we find the shortest path in a binary grid ?
– Can apply
pp y Dijkstra’s
j
algorithm
g
by
y creating
g an “implicit”
p
graph from the grid.
– Assign weights to nodes
according to realistic distance
√2
1
√2
1 √2
∞
1
∞
Chapter 7 – Roadmap-Based Path Planning
7-47
Winter 2012
Shortest Paths in Grids
ƒ As cells are processed in order of distance from
source, a wavefront propagates through the grid:
Colors simply indicate a change in
distance. Each “ring” of color indicates
the same distance from the source.
Eventually,
destination is
reached Just
reached.
remember which
grid cell neighbor
reached here first
and trace
backwards.
In this example, only 4 directions
or travel were allowed: →↓↑←
Chapter 7 – Roadmap-Based Path Planning
7-48
Winter 2012
Shortest Paths in Grids
ƒ For an M x N grid, the graph has O(MN) vertices and
O(MN) edges.
ƒ Algorithm thus takes O(MV log MN) runtime.
ƒ More accurate paths can be produced if we increase
the number of edges:
√5
Additional edges
affect
ff t runtime
ti
by
b a
factor of around 2.
√5
√2
1
√5 √2
√5
Chapter 7 – Roadmap-Based Path Planning
√5
1 √2
∞ ∞
1∞ ∞
May set to 2 assuming
rectilinear travel
around obstacle:
2
∞
7-49
Winter 2012
Shortest Paths in Grids
ƒ It takes no effort to handle complicated obstacles
since algorithm merely concentrates on moving from
one grid unit to another.
ƒ What about non-point robots ?
ƒ Since robot shape is known
ahead of time, we can adjust
j
the weights of adjacent nodes
in the g
grid accordingly.
gly
Chapter 7 – Roadmap-Based Path Planning
7-50
Winter 2012
Shortest Paths in Grids
ƒ For each grid location, center robot model (i.e., a
collection of grid cells) around that point. Cell disabled since
ƒ If any obstacle locations
obstacle lies within
this square.
intersect it, disable this
grid location either by:
– removing the node from the
graph entirely
– setting the weights of edges
going in and out of it to ∞.
Chapter 7 – Roadmap-Based Path Planning
7-51
Winter 2012
Shortest Paths in Grids
ƒ Another solution to the grid shortest path problem
is to convert the grid into vector obstacles, then
apply the vector
vector-based
based algorithm:
s
t
Chapter 7 – Roadmap-Based Path Planning
7-52
Winter 2012
Shortest Paths in Grids
ƒ Of course we can even do the reverse if we prefer to
work with grids (i.e., convert vector to grid):
s
t
Chapter 7 – Roadmap-Based Path Planning
7-53
Winter 2012
Shortest Paths in Grids
ƒ A problem does arise however in the more realistic
maps (i.e., certainty grids) since sensor data is noisy
and we no longer have binary values.
Not clear whether an
obstacle lies here or
not.
We can always choose
some threshold to
produce binary grid
(e.g., >40% certainty
indicates obstacle)
Chapter 7 – Roadmap-Based Path Planning
Realistically, robots
cannot operate in
such a cluttered
environment since
its sensors would
produce too much
noise and false
readings So
readings.
choosing threshold
is reasonable.
7-54
Winter 2012
Triangulation
l
Dual Graph Paths
Triangulation
ƒ A geometric strategy is based on computing a
triangulation of the environment:
– Decompose into
i to tria
triangular
gular free
free-space
space regions
regio s
Chapter 7 – Roadmap-Based Path Planning
7-56
Winter 2012
Triangulation
ƒ There are MANY such triangulations and also many
algorithms for obtaining them.
ƒ One approach is to start by decomposing the freespace region into y-monotone polygons.
– A simple polygon is called y-monotone if any horizontal line is
connected.
Connected
Chapter 7 – Roadmap-Based Path Planning
Broken, thus not
y-monotone
7-57
Winter 2012
Triangulation
ƒ We
W need
d to
t understand
d t d different
diff
t types
t
off vertices:
ti
– A regular vertex is a vertex that is adjacent (connected to) at
lleast one v
vertex
x with a larger
l g y-coordinate
y
and one with a
smaller y-coordinate.
– A irregular up vertex is a vertex that is not connected to any
vertices with a larger y
y-coordinate
coordinate.
– A irregular down vertex is a vertex that is not connected to
any vertices with a smaller y-coordinate.
Irregular Up
Regular
Irregular Down
Chapter 7 – Roadmap-Based Path Planning
7-58
Winter 2012
Triangulation
ƒ We need to regularize the polygon with holes:
– Break it into a subgraph such that all vertices
are regular
l except the
h most extreme vertices
i
iin
the y direction.
ƒ Result is a decomposition into monotone pieces.
ƒB
Basic idea:
– Vertical sweep from top to bottom regularizing vertices that
are irregular down
– Vertical
i l sweep from
f
bottom
b
to top, regularizing
l i i
vertices
i
that
h
are irregular up.
Chapter 7 – Roadmap-Based Path Planning
7-59
Winter 2012
Triangulation
ƒ Can do this by first sorting vertices in vertical order:
Chapter 7 – Roadmap-Based Path Planning
7-60
Winter 2012
Triangulation
ƒ Perform vertical sweep downwards connecting
irregular down vertices to the next nearby vertex:
Some edges will
be invalid, hence
ignored.
Chapter 7 – Roadmap-Based Path Planning
7-61
Winter 2012
Triangulation
ƒ Perform vertical sweep upwards connecting irregular
up vertices to the next nearby vertex:
Chapter 7 – Roadmap-Based Path Planning
7-62
Winter 2012
Triangulation
ƒ Some details have been left out, but result is a set of
monotone polygons:
Chapter 7 – Roadmap-Based Path Planning
7-63
Winter 2012
Triangulation
ƒ Monotone pieces each triangulated separately:
– Do vertical sweep downward, connect vertices from left
monotone chain to right
Right chain
Left chain
Connections added.
There are some
special
i l cases th
thatt
must be considered.
Chapter 7 – Roadmap-Based Path Planning
7-64
Winter 2012
Triangulation
ƒ Monotone pieces each triangulated separately, then
results are joined together:
Can all be done in
O(n log n) time.
Chapter 7 – Roadmap-Based Path Planning
Unfortunately, this can produce
long/thin triangles
triangles.
7-65
Winter 2012
Triangulation
ƒ Better algorithm is a Constrained Delaunay
Triangulation
– Produces “fatter” triangles
– Nicer looking decomposition
– More
M
complicated,
li t d b
butt still
till practical
ti l
– Beyond the scope of this course
vs.
vs
Chapter 7 – Roadmap-Based Path Planning
7-66
Winter 2012
The Dual Graph
ƒ Compute the dual graph which gives a rough idea as
to the paths that the robot may travel.
To get dual
graph, place
vertex at center
of each triangle.
Connect vertices
only if they share
a triangle edge.
Will contain
loops around
obstacles.
May contain dead ends.
Chapter 7 – Roadmap-Based Path Planning
7-67
Winter 2012
Computing a Path
ƒ Robot can compute path in dual graph from start
triangle to goal triangle:
– Can
C
use Dijkstra’s
ijk
’ algorithm
l
i h
Chapter 7 – Roadmap-Based Path Planning
Path not necessarily
y efficient.
7-68
Winter 2012
Computing a Path
ƒ The efficiency of the path solutions are highly
dependant on the triangulation:
Triangulations with long thin triangles tend to
cause zig-zag effects on the path.
Chapter 7 – Roadmap-Based Path Planning
7-69
Winter 2012
Refining the Path
ƒ Can also simplify (i.e., refine) any path in the dual
graph by computing the shortest path in the sleeve
formed by connected triangles:
Chapter 7 – Roadmap-Based Path Planning
7-70
Winter 2012
Refining the Path
ƒ As a result, the computed path will be more
efficient in terms of length
– But path
h will
ill generally
ll travell close
l
to b
boundaries.
d i
Possible
collision due
to inaccurate
sensors.
Chapter 7 – Roadmap-Based Path Planning
7-71
Winter 2012
Refining the Path
ƒ The zig-zag effect essentially disappears when path is
refined.
Caution: long thin triangles can also lead to
numerical problems during computations.
Chapter 7 – Roadmap-Based Path Planning
7-72
Winter 2012
Problems
ƒ If traveling between centers of
triangles, this could be
dangerous, for thin triangles:
ƒ Computing refined path will
always correct this:
Chapter 7 – Roadmap-Based Path Planning
7-73
Winter 2012
Problems
ƒ Alternatively, we can connect
vertices at midpoints of
triangulation edges:
Chapter 7 – Roadmap-Based Path Planning
7-74
Winter 2012
Generalized
l d Voronoi
Diagram Paths
Voronoi Road Maps
ƒ A Voronoi road map is a set of paths
in an environment that represent
maximum clearance between obstacles.
ƒ They are sometimes preferred in robotics since they
reduce the chance of collisions because sensors are
often inaccurate and prone to error.
ƒ Other names for this roadmap are generalized
Voronoi diagram and retraction method.
ƒ It is considered as a generalization of the Voronoi
g
for p
points.
diagram
Chapter 7 – Roadmap-Based Path Planning
7-76
Winter 2012
Voronoi Diagram
ƒ Let S be a set of n points. For each point p of S, the
Voronoi cell of p is the set of points that are closer to
p than to any other points of S.
ƒ The Voronoi diagram is the
space partition
i i
iinduced
d
d b
by
Voronoi cells.
ƒ If the points were obstacles, a robot would
travel
v l along
l g the edges
g off a Voronoi
diagram if it wanted to keep maximum
distance away from the obstacles.
Chapter 7 – Roadmap-Based Path Planning
7-77
Winter 2012
Voronoi Diagram
ƒ Multiple ways of computing a Voronoi Diagram.
– We consider the simplest, and leave the more advanced
algorithms for a computational geometry course.
course
ƒ Basically, we compute each Voronoi cell as the
intersection of a set of half-planes: O(n2 log n) time.
time
Completed cell in
O(n log n) time.
Some cells will
remain “open”.
Chapter 7 – Roadmap-Based Path Planning
7-78
Winter 2012
Generalized Voronoi Diagram
ƒ What if the obstacles are polygons ?
ƒ Now we compute
p
the G
Generalized Voronoi Diagram
g
in
which the edges forming it maintain maximal
distance between edges of the environment, as
opposed to just points.
points
Chapter 7 – Roadmap-Based Path Planning
7-79
Winter 2012
Generalized Voronoi Diagram
ƒ Edges formed based on three types of interaction:
Edge-Edge
g
g
Edge-Vertex
g
Vertex-Vertex
Certain portions
will be curved.
ƒ There are different ways
y of computing
p
g this diagram:
g
– Exact computation
– Approximation – Discretize Obstacles
– Approximation – Discretize Space
Chapter 7 – Roadmap-Based Path Planning
7-80
Winter 2012
Computing the GVD
ƒ Exact computation
– Based on computing
p
g analytic
y
boundaries
– Boundaries may be composed of high-degree curves and
surfaces and their intersections.
– Complex and difficult to implement
– Robustness and accuracy problems
Chapter 7 – Roadmap-Based Path Planning
7-81
Winter 2012
GVD Approximation – Method 1
ƒ Approximation – Discretize Obstacles
– Convert each obstacle into a set of p
points by
y selecting
g samples
p
along boundaries.
– Compute regular Voronoi diagram on resulting point sets.
– Will produce some diagram edges that are not traversable.
Must prevent travel along these portions.
– Can be slow to compute, depending on samples.
Yellow portions are
not traversable since
they are inside the
obstacle.
Chapter 7 – Roadmap-Based Path Planning
This is no longer
a curve.
7-82
Winter 2012
GVD Approximation – Method 1
ƒ Approximation – Discretize Obstacles
(continued)
– Consider computing
p
g the GVD for the following
g example:
p
Chapter 7 – Roadmap-Based Path Planning
7-83
Winter 2012
GVD Approximation – Method 1
ƒ Approximation – Discretize Obstacles
(continued)
– Compute sample points along obstacle border.
The points were computed
manually from this website:
http://www.cs.cornell.edu/I
nfo/People/chew/Delaunay
.html
Chapter 7 – Roadmap-Based Path Planning
7-84
Winter 2012
GVD Approximation – Method 1
ƒ Approximation – Discretize Obstacles
(continued)
– Here is the Voronoi Diagram for the point set:
Chapter 7 – Roadmap-Based Path Planning
7-85
Winter 2012
GVD Approximation – Method 1
ƒ Approximation – Discretize Obstacles
(continued)
– Can discard (ignore) all edges of GVD that are defined by two
consecutive points from the same obstacle:
Can also
C
l di
discard
d allll
edges that lie
completely interior
to any obstacle
(i.e., green ones
here).
here)
Chapter 7 – Roadmap-Based Path Planning
7-86
Winter 2012
GVD Approximation – Method 1
ƒ Approximation – Discretize Obstacles
(continued)
– Resulting GVD edges can be searched for a path from start to
goal (e
(e.g.,
g store GVD as graph,
graph run Dijkstra
Dijkstra’ss shortest path
algorithm)
First/Last
edges
connect
start/goal to
closest
vertex of
GVD.
Chapter 7 – Roadmap-Based Path Planning
7-87
Winter 2012
GVD Approximation – Method 2
ƒ Approximation – Discretize Space
– Convert the environment into a grid.
g
– Compute the Voronoi diagram on resulting grid by
propagating shortest paths from each obstacle point.
– Remember which obstacle point the shortest path came from
for each non-obstacle grid cell.
– Can be slow to compute, depending on samples.
A finer grid
produces a more
accurate answer
but takes longer
Chapter 7 – Roadmap-Based Path Planning
7-88
Winter 2012
GVD Approximation – Method 2
ƒ Approximation – Discretize Space
(continued)
– Create a grid from the environment
Chapter 7 – Roadmap-Based Path Planning
7-89
Winter 2012
GVD Approximation – Method 2
ƒ Approximation – Discretize Space
(continued)
– Compute the Voronoi diagram by running a grid shortest
path setting each obstacle cell as a source
path,
All cells in
each obstacle
i given unique
IDi.
Roadmap paths
found when point
is equally
reachable by two
different sources:
That is,
IDi == IDj
where i ≠ j.
Environment
border
included as
obstacle.
Chapter 7 – Roadmap-Based Path Planning
7-90
Winter 2012
GVD Approximation – Method 2
ƒ Approximation – Discretize Space
(continued)
– Use secondary-ID’s to get path portions in between areas of
non-convex obstacles.
obstacles
Each cell k in
obstacle i
given a
unique IDik
such that
consecutive
cells along
border g
given
consecutive
IDs as IDi,
IDi+1, ID1+2,
etc…
Chapter 7 – Roadmap-Based Path Planning
Now also keep
path-portions in
which cell has
been reached
by two same
primary sources
with nonconsecutive
secondary IDs
IDs.
That is,
IDik = IDim
where
ABS(k-m) >
1
1.
7-91
Winter 2012
GVD Approximation – Method 2
ƒ Approximation – Discretize Space
(continued)
– Compute a path in the Voronoi diagram
Chapter 7 – Roadmap-Based Path Planning
7-92
Winter 2012
GVD Approximation – Method 2
ƒ Approximation – Discretize Space
(continued)
– Resulting path is pretty good too:
Green path is discretized
space path (i.e., grid).
Red path is discretized
obstacles path (i.e., previous).
Here iis a website
H
b it th
thatt you
can try:
http://www.cs.columbia.ed
u/~pblaer/projects/path_pl
anner/applet.shtml
Chapter 7 – Roadmap-Based Path Planning
7-93
Winter 2012
Cell Decomposition Paths
Cell Decomposition
ƒ There are various ways to decompose
(i.e., split up) the environment into cells.
– We have already looked at grid-based
grid based methods
methods, which are
based on the same idea
ƒ Now we will look at how to geometrically break up
the environment into small-sized polygonal regions
called cells.
ƒ We will then see how to determine a path through
these cells.
Chapter 7 – Roadmap-Based Path Planning
7-95
Winter 2012
Trapezoidal Decomposition
ƒ Perhaps a simpler way to compute paths is to
decompose the environment into simpler vertical cells
in the form of trapezoids or triangles:
Chapter 7 – Roadmap-Based Path Planning
7-96
Winter 2012
Trapezoidal Decomposition
ƒ How do we make the trapezoids ?
x
x
– extend rays vertically directed up and
d
down
ffrom each
h vertex off each
h obstacle
b
l
x
x
and (including outer boundary).
– when rays intersect obstacles (or
boundary), the ray stops, becoming
Two separate
trapezoids are
created here.
a trapezoid edge
– need to compute intersections of each
ray with
ith all
ll other
th obstacles.
b t l
- can be done efficiently using a plane sweep technique, assuming
vertices of all obstacles are sorted in x direction.
Chapter 7 – Roadmap-Based Path Planning
7-97
Winter 2012
Trapezoidal Decomposition
ƒ While doing this, maintain which trapezoids are
adjacent (i.e., beside) one another.
– Adjacent trapezoids will share an
edge with the exact same endpoints.
– Determine midpoint of each trapezoid
edge (except polygon/boundary edges).
– Form a graph where
• the nodes are the midpoints of the trapezoidal
edges and two nodes are connected if they
represent midpoints of edges belonging to
the same trapezoid
p
Chapter 7 – Roadmap-Based Path Planning
7-98
Winter 2012
Computing a Path
ƒ Easy to compute path now in the resulting graph:
– Just determine which trapezoid contains start/goal and
connect the start/goal to each node of that trapezoid.
trapezoid
Computed path can be inefficient.
Goal
Start
Chapter 7 – Roadmap-Based Path Planning
7-99
Winter 2012
Improving the Path
ƒ Can we make the computed path more efficient ?
– Add more p
points (not jjust midpoint):
p
• fixed number per edge, or
• fixed distance between points
ƒ As a result, the path:
– may take different path around obstacles
– will be more efficient
– may travel closer to boundaries
Chapter 7 – Roadmap-Based Path Planning
7-100
Winter 2012
Boustrophedon Cell Decomposition
ƒ Boustrophedon cell decomposition considers only
critical points.
– critical
i i l points
i
are obstacle
b
l vertices
i
ffrom which
hi h a ray can be
b
extended both upwards and downwards through free space.
– Connect midpoints of formed line
segments as with the trapezoidal
decomposition technique.
x
x
x
x
x
x
x x
ƒ Cells, in general, are no longer trapezoids or
triangles
Chapter 7 – Roadmap-Based Path Planning
7-101
Winter 2012
Boustrophedon Cell Decomposition
ƒ Now less cells than trapezoidal, but cells are more
complex
Critical points
Chapter 7 – Roadmap-Based Path Planning
7-102
Winter 2012
Boustrophedon Cell Decomposition
ƒ Can interconnect cells, but connections are
topological, not actual valid paths:
Chapter 7 – Roadmap-Based Path Planning
7-103
Winter 2012
Boustrophedon Cell Decomposition
ƒ To find a path now, we can use various strategies:
– Bug algorithm, cell boundary following etc…
Bug2 algorithm
Cell boundary
following
Chapter 7 – Roadmap-Based Path Planning
7-104
Winter 2012
Canny’s Silhouette Algorithm
ƒ Another approach is to decompose the environment
into silhouette curves which represent borders of the
obstacles:
Regions are no
longer trapezoids
or triangles
(in general).
Chapter 7 – Roadmap-Based Path Planning
7-105
Winter 2012
Canny’s Silhouette Algorithm
ƒ To do this, consider a vertical line sweeping
horizontally from the leftmost environment vertex to
the rightmost Sweep
S
liline iis split
lit iinto
t ttwo vertical
ti l split
lit
Two split lines
lines when an obstacle is encountered.
As the line
sweeps, the
topmost and
d
bottommost
extreme points
form the
silhouette
boundary
boundary.
will merge
when their
endpoints
meet again
upon leaving
the obstacle.
The points at
which splits &
merges occur
are called
critical points
Chapter 7 – Roadmap-Based Path Planning
7-106
Winter 2012
Canny’s Silhouette Algorithm
ƒ Compute a path by determining extreme points of
vertical line passing through start/goal and then
following silhouette path:
Here are two
solutions … one
going upwards,
the other
downwards.
Chapter 7 – Roadmap-Based Path Planning
7-107
Winter 2012
Sampling-Based
l
d
Road Maps
Sampling-Based Road Maps
ƒ There are a few that we will look at based on:
– Fixed Grid sampling
– Probabilistic sampling
– Random Tree expansion
ƒ Such algorithms work by choosing fixed or random
g them
valid robot p
positions and then interconnecting
based on close proximity to form a graph of valid
paths.
Chapter 7 – Roadmap-Based Path Planning
7-109
Winter 2012
Grid-Based
Grid
Based Sampling
Grid-Based Sampling
ƒ Grid-based sampling is perhaps the simplest
technique based on overlaying a grid of vertices and
connecting adjacent ones.
– Accuracy and feasibility of resulting path
p
on g
granularity
y of g
grid.
depends
ƒ We already looked at this strategy
in terms of grid maps.
maps
ƒ Only interconnect vertices that do not intersect
obstacle
b t l boundaries
b
d i
ƒ Multiple ways of interconnecting…
Chapter 7 – Roadmap-Based Path Planning
7-111
Winter 2012
Grid-Based Sampling
ƒ Here is a straight forward 4-connectivity grid.
– Compute
p
p
path from start to g
goal using
g g
graph
p search:
Chapter 7 – Roadmap-Based Path Planning
7-112
Winter 2012
Grid-Based Sampling
ƒ With additional “neighbor” connections, the graph
allows more efficient paths…at a cost of increased
space and slower computation time.
Can use a variety
of neighbor
interconnection
strategies per
node:
Chapter 7 – Roadmap-Based Path Planning
7-113
Winter 2012
Grid-Based Sampling
ƒ Here is the result with a reduced-size sample set (i.e.,
more coarse grid)
Uh Oh! Graph will
become disconnected
eventually, as grid
becomes more
coarse.
As a result, there may
be no valid path from
start to goal nodes.
Chapter 7 – Roadmap-Based Path Planning
7-114
Winter 2012
Setting the Grid Size
ƒ We can ensure that a path exists:
– choose grid size (i.e., width between connected nodes) to be
smaller than minimum distance between any two obstacle
edges
d
that
h d
do not share
h
a vertex:
Minimal edge distance here.
Choose grid size
accordingly:
gy
Chapter 7 – Roadmap-Based Path Planning
7-115
Winter 2012
Setting the Grid Size
ƒ How do we determine the shortest distance between
two line segments L1 and L2 ?
ƒ Consider first the distance from a
point p to a line L:
L
b
q
ε
a
p
– p will intersect line L at a right angle, say at point q
g L from a to b
– let u be the distance of q along
u=
(xp – xa)(xb – xa) + (yp – ya)(yb – ya)
(xb – xa)2 + (yb – ya)2
– the coordinates of q are:
xq = xa + u(xb – xa)
Chapter 7 – Roadmap-Based Path Planning
and
yq = ya + u(yb – ya)
ε is then just the
distance between p
and q.
7-116
Winter 2012
Setting the Grid Size
ƒ We then need to determine whether or not q=(xq,yq)
lies on the segment L = ab.
ƒ If 0 ≤ u ≤ 1 then q lies on the segment and
therefore ε = ||pq|| else ε = min(|pa|,
| | ||pb|)
|
L
a
b
q
ε
L
p
L2
q
b
ε
a
p
q
a
L
b
ε
p
0≤u≤1
Chapter 7 – Roadmap-Based Path Planning
u>1
u<0
7-117
Winter 2012
Setting the Grid Size
ƒ Let ε = δ(p, L) be the shortest distance function from
a point p to a segment L.
ƒ We can use this to find the minimum distance
between two segments L1 and L2 as:
ε = Min(δ(a, L2), δ(b, L2), δ(c, L1), δ(d, L1))
b
L1
a
Chapter 7 – Roadmap-Based Path Planning
ε
d
c
L2
7-118
Winter 2012
Grid-Based Sampling
ƒ The main problem here is it causes too many grid
points in open areas.
Wasteful to have
many
y grid
g p
points
here.
Chapter 7 – Roadmap-Based Path Planning
7-119
Winter 2012
Grid-Based Sampling
ƒ Can always do a quad-tree-like decomposition,
determining the smallest gaps within certain areas,
recursively.
Details have
been left out
as to how to
connect at
borders.
Chapter 7 – Roadmap-Based Path Planning
7-120
Winter 2012
Probabilistic Road Maps
Probabilistic Road Maps
ƒ Probabilistic Road Maps (PRM)
are sampling-based mapping
strategies.
y are created by
y selecting
g random points
p
((i.e.,, samples)
p )
– They
from the environment and interconnecting points that
represent valid short path lengths.
– They perform
f
fairly
f l well,
ll but
b are best
b
for
f situations in which
robot configurations are more complex than a single point
robot.
– Solution depends on how many nodes are used and how much
interconnectivity there is between nodes.
Chapter 7 – Roadmap-Based Path Planning
7-122
Winter 2012
Probabilistic Road Maps
ƒ Algorithm produces a graph G=(V,E) as follows:
LET V and E be empty.
REPEAT
Let v be a random robot configuration (i.e., random point)
IF (v is a valid configuration) THEN
add v to V
// i.e., does not intersect obstacles
UNTIL V has n vertices
FOR (each vertex v of V) DO
Let C be
b the
h k closest
l
neighbors
i hb
off v
FOR (each neighbor ci in C) DO
// ii.e., the
h k closest
l
vertices
i
to v
IF (E does not have edge from v to ci) AND (path from v to ci is valid)
THEN
Add an edge from v to ci in E
ENDFOR
ENDFOR
Chapter 7 – Roadmap-Based Path Planning
7-123
Winter 2012
Probabilistic Road Maps
ƒ Here is an example of randomly added nodes and
their interconnections (roughly, n = 52 and k = 4):
Graph may be
disconnected
if n and/or k
are too small
small.
Chapter 7 – Roadmap-Based Path Planning
7-124
Winter 2012
Probabilistic Road Maps
ƒ How do we find the k-nearest neighbors ?
ƒ Multiple
p strategies:
g
– “Brute Force” (check everything O(n2 log n))
– KD-Trees
– R-Trees
Most p
popular
p
– VP-Trees
ƒ The KD tree is the most popular since it is relatively
straight forward to implement.
ƒ Basically, divides recursively the sets of points in
half…alternating with vertical/horizontal cuts.
Chapter 7 – Roadmap-Based Path Planning
7-125
Winter 2012
K-D Trees
ƒ Here is how to a KD-Tree is constructed:
2
4
5
1
3
6
8
10
9
11
Each node in the
tree defines a
rectangular region.
7
Each leaf
in the tree
represents
one of the
points.
12
13
14
Chapter 7 – Roadmap-Based Path Planning
7-126
Winter 2012
K-D Trees
ƒ Once constructed, we find the k-nearest
neighbors of a leaf.
– Start by recursively searching down the tree to
find the rectangle that contains the vertex v (for
which we are trying to find its neighbors)
Chapter 7 – Roadmap-Based Path Planning
e.g., Look for this
guy’s neighbors.
7-127
Winter 2012
K-D Trees
ƒ Compute closest neighbor on way back from
recursion:
We can find the k neareast
neighbors as follows:
vc
v′c
v
vc
1. Let closest neighbor vc be the
point in the first window on way
back from recursion.
v
2 Compute a circle with radius vvc.
2.
3. Check vertices in all rectangles
that intersect the circle for a
better neighbor.
4. If a better neighbor v′c is found,
shrink the circle to a smaller
radius defined by vv′c.
vc
vc
v
Chapter 7 – Roadmap-Based Path Planning
5. Continue in this way until the root
is reached.
reached
v
Repeat the above procedure k
times…making sure to flag the
closest neighbor each time so that it
is not found again.
7-128
Winter 2012
Probabilistic Road Maps
ƒ Here are some maps for various n and k values:
n=100, k=5
n=100, k=10
n=100, k=20
n=500,
n
500, k
k=5
5
n=500,
n
500, k
k=10
10
n=1000,
n
1000, k
k=5
5
Chapter 7 – Roadmap-Based Path Planning
7-129
Winter 2012
Probabilistic Road Maps
ƒ PRMs perform well in practice, but are susceptible to
missing vertices in narrow passages
– Could lead to disco
disconnected
ected graphs a
and
d no
o solutio
solution:
Chapter 7 – Roadmap-Based Path Planning
7-130
Winter 2012
Probabilistic Road Maps
ƒ PRMs perform well when the robot configurations
are more complex
– when
whe robots are not
ot just points,
poi ts but differe
differentt shapes iin
different positions.
– performs very well for robot arm kinematics
Chapter 7 – Roadmap-Based Path Planning
7-131
Winter 2012
Rapidly-Exploring
dl
l
Random
d
Tree Maps
Rapidly Exploring Random Trees
ƒ Rapidly Exploring Random Trees (RRTs):
– each node represents
p
random robot configuration
g
(i.e., p
point
representing valid robot location in environment).
– single query planner which covers the space
between the start/goal locations quickly
– root starts at the current robot position.
– grows outwards from the start either completely randomly or
somehow biased towards the goal location.
– input parameters are the number of nodes to be used in the
tree and the length (i.e., step size) of edges to add.
Chapter 7 – Roadmap-Based Path Planning
7-133
Winter 2012
RRT Algorithm
ƒ The algorithm produces a tree G=(V,E)
G=(V E) as follows:
LET V contain the start vertex and E be empty.
REPEAT
LET q be a random valid robot configuration (i.e., random point)
LET v be the node of V that is closest to q.
LET p be
b th
the point
i t along
l
the
th ray from
f
v to
t q that
th t iis att distance
di t
s
from v.
IF (vp is a valid edge) THEN
obstacles
add new node p to V with parent v
to p in E
UN IL V has
UNTIL
h n vertices
i
Chapter 7 – Roadmap-Based Path Planning
// i.e., does not intersect
// i.e., add edge from v
s
v
q
p
7-134
Winter 2012
RRT Maps
ƒ Here are some maps for various n and s values:
n=100, s=10
n=100, s=25
n=100, s=50
n=1000,
n
1000, s
s=10
10
n=1000,
n
1000, s
s=25
25
n=1000,, s=50
Chapter 7 – Roadmap-Based Path Planning
7-135
Winter 2012
RRT Problems
ƒ RRTs have problems expanding through narrow
passages and getting around obstacles:
Difficult to expand into this
area since any random points
generated in this area tend to
result in an intersection with
the “L”L shaped obstacle
obstacle.
Chapter 7 – Roadmap-Based Path Planning
7-136
Winter 2012
RRT Guiding
ƒ Can we bias the results to head towards the goal ?
– Use goal point for expand direction instead of random
Stuck in
“local
minima”.
Use goal 95%
Use goal 75%
Use goal 50%
Use goal 25%
Use goal 5%
Use goal 0%
Chapter 7 – Roadmap-Based Path Planning
7-137
Winter 2012
Greedy RRTs
ƒ A greedy approach to the RRT growth is to allow the
tree to expand beyond the step size s:
Without greedy
n=100 s=10
n=100,
n=200
n
200, s
s=25
25
s
q
p
4
v p1 p2 p3
LET V contain the start vertex and E be empty.
REPEAT
LET q be a random valid robot configuration
REPEAT
LET v be the node of V that is closest to q.
LET p be point along vq at distance s from v.
IF (vp is a valid edge) THEN
add new node p to V with parent v
UNTIL (p is invalid) OR (p is within distance s from q)
UNTIL V has n vertices
Just need to add this
REPEAT loop.
With greedy
Chapter 7 – Roadmap-Based Path Planning
7-138
Winter 2012
Reaching the Goal
ƒ We have yet to see how to stop the growth when the
goal is reached.
ƒ Make
M k th
the following
f ll i
changes
h
to
t the
th algorithm:
l
ith
LET V contain the start vertex and E be empty.
REPEAT
LET q be a random valid robot configuration (i.e., random point)
LET v be the node of V that is closest to q.
IF (distance from v to goal < s) THEN
p = goall
ELSE
LET p be the point along the ray from v to q that is at distance s from v.
IF (vp is a valid edge) THEN
add new node p to V with parent v
UNTIL V has n vertices
Chapter 7 – Roadmap-Based Path Planning
// i.e.,
i e does not intersect obstacles
// i.e., add edge from v to p in E
7-139
Winter 2012
Dual Trees
ƒ It is more beneficial (faster) to maintain two trees
G1=(V1,E1) and G2=(V2,E2)
LET V1 contain the start vertex, V2 contain the goal vertex, LET E1 and E2 be empty.
REPEAT
LET q be a random valid robot configuration (i.e., random point)
LET v be the node of V1 that is closest to q
q.
LET p be the point along the ray from v to q that is at distance s from v.
IF (p is a valid configuration) THEN
add new node p to V1 with parent v
LET q
q′ be p
s
LET v′ be the node of V2 that is closest to q′.
LET p′ be the point along the ray from v′ to q′
p′
that is at distance s from v′.
IF (p′
( ′ is
i a valid
lid configuration)
fi
ti ) THEN
add new node p′ to V2 with parent v′
ENDIF
Swap G1 and G2
G2
v′
s
G1
q
v p =q′
UNTIL V1 and V2 have n vertices in total
Chapter 7 – Roadmap-Based Path Planning
7-140
Winter 2012
Merging Trees
ƒ As a result, the trees grow towards each other and
eventually (hopefully) merge:
n=100, s=10
n=200, s=10
n=300, s=10
ƒ Trees remain separate graphs, but merge when a
node
de ffrom one
e ttree
ee is within
withi distance
dista ce s from
f
the
other tree.
Chapter 7 – Roadmap-Based Path Planning
7-141
Winter 2012
Merging Trees
ƒ A variety of environments work using this strategy:
Each of these
results have
n = 100
s = 20
Chapter 7 – Roadmap-Based Path Planning
7-142
Winter 2012
Merging Trees
ƒ Sometimes, it takes a while to get them to merge:
n=100, s=20
n=500, s=20
There are nott
Th
10,000 points
here…the path
was found
before that.
n=1000, s=20
Chapter 7 – Roadmap-Based Path Planning
n=10000, s=20
7-143
Winter 2012
Summary
ƒ You should now understand:
– How to efficiently
y plan
p
the motion of a robot from one
location to another in a 2D environment.
– Various techniques for computing planned paths.
– How to “grow” obstacles to accommodate real robot solutions.
– How to combine what we
we’ve
ve learned here with what we
learned in robot position estimation and navigation to fully
control a robot’s position at all times.
Chapter 7 – Roadmap-Based Path Planning
7-144
Winter 2012