Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 1 Chapter 12 Path Planning
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 1
Chapter 12
Path Planning
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 2
path planner
path manager
path following
autopilot
unmanned aircraft
waypoints
on-board sensors
position error
tracking error
status
destination,obstacles
servo commandsstate estimator
wind
path definition
airspeed,altitude,heading,
commands
map
x̂(t)
Control Architecture
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 3
Path Planning Approaches
• Deliberative
– Based on global world knowledge
– Requires a good map of terrain, obstacles, etc.
– Can be too computationally intense for dynamic
environments
– Usually executed before the mission
• Reactive
– Based on what sensors detect on immediate horizon
– Can respond to dynamic environments
– Not usually used for entire mission
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 4
Voronoi Graphs
−10 −8 −6 −4 −2 0 2 4 6 8 10−10
−8
−6
−4
−2
0
2
4
6
8
10
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 5
Voronoi Graph Example
−10 −5 0 5 10 15 20−10
−5
0
5
10
15
20
east position (km)
north
pos
ition
(km
)
• Graph generated using voronoi command in Matlab
• 20 point obstacles
• Start and end points of path not shown
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 6
Voronoi Graph Example
−10 −5 0 5 10 15 20−10
−5
0
5
10
15
20
east position (km)
north
pos
ition
(km
)
• Add start and end points to
graph
• Find 3 closest graph nodes to
start and end points
• Add graph edges to start and
end points
• Search graph to find “best” path
• Must define “best”
• Shortest?
• Furthest from obstacles?
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 7
Path Cost Calculation
Nodes of graph edge: v1 and v2
Point obstacle: p
Length of edge: kv1 � v2k
Any point on line segment: w(�) = (1� �)v1 + �v2 where � 2 [0, 1]
Minimum distance between p and graph edge
D(v1,v2,p)4= min
�2[0,1]kp�w(�)k
= min
�2[0,1]
q(p�w(�))>(p�w(�))
= min
�2[0,1]
qkp� v1k2 + 2�(p� v1)
>(v1 � v2) + �2 kv1 � v2k2.
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 8
Path Cost Calculation
Value for � that minimizes D is
�⇤=
(v1 � p)>(v1 � v2)
kv1 � v2k2
Location along edge for which D is minimum:
w(�⇤) =
s
kp� v1k2 �((v1 � p)>(v1 � v2))
2
kv1 � v2k2
Define distance to edge for �⇤ < 0, �⇤ > 1:
D0(v1,v2,p)
4=
8><
>:
w(�⇤) if �⇤ 2 [0, 1]
kp� v1k if �⇤ < 0
kp� v2k if �⇤ > 1
Distance between point set Q and line segment v1v2:
D(v1,v2,Q) = min
p2QD
0(v1,v2,p)
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 9
Path Cost Calculation
Cost for traveling along edge (v1
,v2
) is assigned as
J(v1
,v2
) = k1
kv1
� v2
k| {z }length of edge
+
k2
D(v1
,v2
,Q)| {z }reciprocal of distance to closest point in Q
k1
and k2
are positive weights
Choice of k1
and k2
allow tradeo↵ between path length and proximity to threats.
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 10
Voronoi Path Planning Algorithm
September 15, 2011 Time: 12:53pm chapter12.tex
Path Planning 211
East (km)
Nor
th (k
m)
Figure 12.4 Optimal path through the Voronoi graph.
Algorithm 9 Plan Voronoi Path:W = planVoronoi(Q, ps, pe)
Input: Obstacle pointsQ, start position ps, end position pe
Require: |Q| ≥ 10 Randomly add points if necessary.1: (V, E ) = constructVoronoiGraph(Q)2: V+ = V
!{ps}
!{pe}
3: Find {v1s, v2s, v3s}, the three closest points in V to ps, and{v1e, v2e, v3e}, the three closest points in V to pe
4: E+ = E!
i=1,2,3(vis, ps)!
i=1,2,3(vie, pe)5: for Each element (va, vb) ∈ E do6: Assign edge cost Jab = J (va, vb) according to equation (12.1).7: end for8: W = DijkstraSearch(V+, E+, J)9: return W
One of the disadvantages of the Voronoi method described in al-gorithm 9 is that it is limited to point obstacles. However, there arestraightforward modifications for non-point obstacles. For example,consider the obstacle field shown in figure 12.5(a). A Voronoi graph canbe constructed by first adding points around the perimeter of the obsta-cles that exceed a certain size, as shown in figure 12.5(b). The associatedVoronoi graph, including connections to start and end nodes, is shownin figure 12.5(c). However, it is obvious from figure 12.5(c) that theVoronoi graph includes many infeasible links that are contained inside
Dijkstra’s algorithm is used to search the graph
Voronoi graph and Dijkstra’s algorithm code are commonly available
Matlab:
Voronoi -> voronoiDijkstra -> shortestpath
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 11
Voronoi Path Planning Result
−10 −5 0 5 10 15 20−10
−5
0
5
10
15
20
east position (km)
north
pos
ition
(km
)
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 12
Voronoi Path Planning – Non-point Obstacles
0 2 4 6 8 10
0
2
4
6
8
10
east position (km)
north
pos
ition
(km
)
• How do we handle solid obstacles?
• Point obstacles at center of
spatial obstacles won’t provide
safe path options around
obstacles
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 13
Non-point Obstacles – Step 1
0 2 4 6 8 10
0
2
4
6
8
10
east position (km)
north
pos
ition
(km
)
Insert points around perimeter of obstacles
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 14
Non-point Obstacles – Step 2
Construct Voronoi graph
Note infeasible path edges inside obstacles
0 2 4 6 8 10
0
2
4
6
8
10
east position (km)
north
pos
ition
(km
)
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 15
Non-point Obstacles – Step 3
Remove infeasible path edges from graph
Search graph for best path
0 2 4 6 8 10
0
2
4
6
8
10
east position (km)
north
pos
ition
(km
)
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 16
Rapidly Exploring Random Trees (RRT)
• Exploration algorithm that randomly,
but uniformly explores search space
• Can accommodate vehicles with
complicated, nonlinear dynamics
• Obstacles represented in a terrain map
• Map can be queried to detect possible collisions
• RRTs can be used to generate a single feasible path or
a tree with many feasible paths that can be searched to
determine the best one
• If algorithm runs long enough, the optimal path through
the terrain will be found
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 17
RRT Tree Structure
• RRT algorithm is implemented
using tree data structure
• Tree is special case of
directed graph
• Edges are directed from
child node to parent
• Every node has one parent, except root
• Nodes represent physical states or configurations
• Edges represent feasible paths between states
• Each edge has cost associated traversing feasible path
between states
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 18
RRT Algorithm
• Algorithm initialized
– start node
– end node
– terrain/obstacle map
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 19
RRT Algorithm
• Randomly select configura-
tion p in workspace
• Select new configuration v1
fixed distance D from start
point along line connecting
ps and p
• Check new segment for col-
lisions
• If collision-free, insert v1 into
tree.
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 20
RRT Algorithm
• Generate random configura-
tion p in workspace
• Search tree to find node clos-
est to p
• From node closest to p, move
distance D along line con-
necting node and p to es-
tablish configuration v2
• Check new segment for col-
lisions
• If collision-free, insert v2 into
tree
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 21
RRT Algorithm
• Generate random configura-
tion p in workspace
• Search tree to find node clos-
est to p
• From node closest to p, move
distance D along line con-
necting node and p to es-
tablish new node
• Check new segment for col-
lisions
• If collision-free, insert new
node into tree
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 22
RRT Algorithm
• Continue adding nodes and checking for
collisions
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 23
RRT Algorithm
• Continue until node is generated that is within
distance D of end node
• At this point, terminate algorithm or search for
additional feasible paths
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 24
RRT Algorithm
September 15, 2011 Time: 12:53pm chapter12.tex
Path Planning 215
and v∗ may be large, line 5 plans a path of fixed length D from v∗ inthe direction of p. The resulting configuration is denoted as v+. If theresulting path is feasible, as checked in line 6, then v+ is added to G inline 7 and the costmatrix is updated in line 8. The if statement in line 10checks to see if the new node v+ can be connected directly to the endnode pe. If so, pe is added to G in line 11–12, and the algorithm ends inline 15 by returning the shortest waypoint path in G.
Algorithm 10 Plan RRT Path:W = planRRT(T , ps, pe)
Input: Terrain map T , start configuration ps, end configuration pe
1: Initialize RRT graph G = (V, E ) as V = {ps}, E = ∅2: while The end node pe is not connected to G, i.e., pe ̸∈ V do3: p ← generateRandomConfiguration(T )4: v∗ ← findClosestConfiguration(p, V)5: v+ ← planPath(v∗, p, D)6: if existFeasiblePath(T , v∗, v+) then7: Update graph G = (V, E ) as V ← V
!{v+}, E ← E
!{(v∗, v+)}
8: Update edge costs as C[(v∗, v+)] ← pathLength(v∗, v+)9: end if
10: if existFeasiblePath(T , v+, pe) then11: Update graph G = (V, E ) as V ← V
!{pe} E ← E
!{(v∗, pe)}
12: Update edge costs as C[(v∗, pe)] ← pathLength(v∗, pe)13: end if14: endwhile15: W = findShortestPath(G, C).16: return W
The result of implementing algorithm 10 for four different randomlygenerated obstacle fields and randomly generated start and end nodesis displayed with a dashed line in figure 12.8. Note that the paths gener-ated by Algorithm 10 sometimes wander needlessly and that eliminat-ing some nodes may result in a more efficient path. Algorithm 11 givesa simple scheme for smoothing the paths generated by algorithm 10.The basic idea is to remove intermediate nodes if a feasible path stillexists. The result of applying algorithm 11 is shown with a solid line infigure 12.8.
There are numerous extensions to the basic RRT algorithm. A com-mon extension, which is discussed in [74], is to extend the tree fromboth the start and the end nodes and, at the end of each extension,to attempt to connect the two trees. In the next two subsections, we
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 25
Path Smoothing
• Start with initial point (1)
• Make connections to
subsequent points in path
(2), (3), (4) …
• When connection collides
with obstacle, add previous
waypoint to smoothed path
• Continue smoothing from
this point to end of path
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 26
Path Smoothing Algorithm
September 15, 2011 Time: 12:53pm chapter12.tex
Path Planning 217
Algorithm 11 Smooth RRT Path: (Ws, Cs) = smoothRRT(T ,W, C)
Input: Terrain map T , waypoint pathW = {w1, . . . , wN}, costmatrix C
1: Initialized smoothed pathWs ← {w1}2: Initialize pointer to current node inWs: i ← 13: Initialize pointer to next node inW: j ← 24: while j < N do5: ws ← getNode(Ws, i)6: w+ ← getNode(W, j + 1)7: if existFeasiblePath(T , ws, w+) = FALSE then8: Get last node:w ← getNode(W, j)9: Add deconflicted node to smoothed path:Ws ←Ws
!{w}
10: Update smoothed cost: Cs[(ws, w)] ← pathLength(ws, w)11: i ← i + 112: end if13: j ← j + 114: endwhile15: Add last node fromW:Ws ←Ws
!{wN}
16: Update smoothed cost: Cs[(wi , wN)] ← pathLength(wi , wN)17: return Ws
algorithm 10 will be a path that follows the terrain at a fixed altitudehAGL . However, the smoothing step represented by algorithm 11 willresult in paths with much less altitude variation. For 3-D terrain, theclimb rate and descent rates of the MAV are usually constrained to bewithin certain limits. The function existFeasiblePath in algorithms 10and 11 can be modified to ensure that the climb and descent rates aresatisfied and that terrain collisions are avoided.
The results of the 3-D RRT algorithm are shown in figures 12.9and 12.10, where the thin lines represent the RRT tree, the thick dashedline is the RRT path generated by algorithm 10, and the thick solid lineis the smoothed path generated by algorithm 11.
RRTDubins Path Planning in a 2-D Obstacle FieldIn this section, we will consider the extension of the basic RRT al-gorithm to planning paths subject to turning constraints. Assumingthat the vehicle moves at constant velocity, optimal paths betweenconfigurations are given by Dubins paths, as discussed in section 11.2.Dubins paths are planned between two different configurations, wherea configuration is given by three numbers representing the north andeast positions and the course angle at that position. To apply the RRTalgorithm to this scenario, we need to have a technique for generatingrandom configurations.
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 27
RRT Results
0 10 20 30 40 50 60 70 80 90 1000
10
20
30
40
50
60
70
80
90
100
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 28
RRT Results
0 10 20 30 40 50 60 70 80 90 1000
10
20
30
40
50
60
70
80
90
100
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 29
RRT Results
0 10 20 30 40 50 60 70 80 90 1000
10
20
30
40
50
60
70
80
90
100
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 30
RRT Results
0 10 20 30 40 50 60 70 80 90 1000
10
20
30
40
50
60
70
80
90
100
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 31
RRT* Algorithm – Extend Step
vnewv0
N
• Generate new potential node vnew
identical to RRT.
• Instead of finding the closest node in
the tree, find all nodes within neigh-
borhood N .
• Let C(v) be the path cost from ps
to v, and let c(v1,v2) be the path
cost from v1 to v2.
• Let v0= argminv2N (C(v) + c(vnew,v))
(i.e., closest node in N to vnew).
• Add node V VS{vnew}.
• Add edge E ES{(v0,vnew)}.
• Set C(vnew) = C(v0) + c(vnew,v).
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 32
RRT* Algorithm – Re-wire Step
vnew
N
• Check all nodes in v 2 N to see if
C(vnew) + c(vnew,v) < C(v)
i.e., if re-routing through vnew re-
duces the path length.
• If so, remove edge between v and its
parent, and add edge between v and
vnew.
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 33
RRT* Algorithm – Re-wire Step
vnew
N
• Check all nodes in v 2 N to see if
C(vnew) + c(vnew,v) < C(v)
i.e., if re-routing through vnew re-
duces the path length.
• If so, remove edge between v and its
parent, and add edge between v and
vnew.
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 34
RRT vs. RRT*
From S. Karaman and E. Frazzoli, “Incremental Sampling-based Algorithms for
Optimal Motion Planning,” International Journal of Robotic Research, 2010.
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 35
RRT vs. RRT*
From S. Karaman and E. Frazzoli, “Incremental Sampling-based Algorithms for
Optimal Motion Planning,” International Journal of Robotic Research, 2010.
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 36
RRT Path Planning Over 3D Terrain
• Assume terrain map can be queried
to determine altitude of terrain at
any north-east location
• Must be able to determine altitude
for random configuration p in RRT
algorithm
• Must be able to detect collisions with
terrain – reject random candidate
paths leading to collision
• Options:
– random altitude within predetermined range
– random selection of discrete altitudes in desired range
– set altitude above ground level
• Test candidate paths to ensure flight path angles are feasible – reject
if infeasible
0
50
100
150
200
050
100150
2000
20
40
60
80
100
E
N
h
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 37
RRT Algorithm – 3D Terrain
September 15, 2011 Time: 12:53pm chapter12.tex
Path Planning 215
and v∗ may be large, line 5 plans a path of fixed length D from v∗ inthe direction of p. The resulting configuration is denoted as v+. If theresulting path is feasible, as checked in line 6, then v+ is added to G inline 7 and the costmatrix is updated in line 8. The if statement in line 10checks to see if the new node v+ can be connected directly to the endnode pe. If so, pe is added to G in line 11–12, and the algorithm ends inline 15 by returning the shortest waypoint path in G.
Algorithm 10 Plan RRT Path:W = planRRT(T , ps, pe)
Input: Terrain map T , start configuration ps, end configuration pe
1: Initialize RRT graph G = (V, E ) as V = {ps}, E = ∅2: while The end node pe is not connected to G, i.e., pe ̸∈ V do3: p ← generateRandomConfiguration(T )4: v∗ ← findClosestConfiguration(p, V)5: v+ ← planPath(v∗, p, D)6: if existFeasiblePath(T , v∗, v+) then7: Update graph G = (V, E ) as V ← V
!{v+}, E ← E
!{(v∗, v+)}
8: Update edge costs as C[(v∗, v+)] ← pathLength(v∗, v+)9: end if
10: if existFeasiblePath(T , v+, pe) then11: Update graph G = (V, E ) as V ← V
!{pe} E ← E
!{(v∗, pe)}
12: Update edge costs as C[(v∗, pe)] ← pathLength(v∗, pe)13: end if14: endwhile15: W = findShortestPath(G, C).16: return W
The result of implementing algorithm 10 for four different randomlygenerated obstacle fields and randomly generated start and end nodesis displayed with a dashed line in figure 12.8. Note that the paths gener-ated by Algorithm 10 sometimes wander needlessly and that eliminat-ing some nodes may result in a more efficient path. Algorithm 11 givesa simple scheme for smoothing the paths generated by algorithm 10.The basic idea is to remove intermediate nodes if a feasible path stillexists. The result of applying algorithm 11 is shown with a solid line infigure 12.8.
There are numerous extensions to the basic RRT algorithm. A com-mon extension, which is discussed in [74], is to extend the tree fromboth the start and the end nodes and, at the end of each extension,to attempt to connect the two trees. In the next two subsections, we
modify to test for collisions
with terrain and flight path
angle feasibility
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 38
RRT 3D Terrain Results
0
50
100
150
200
0
50
100
150
2000
20
40
60
80
100
EN
h
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 39
RRT 3D Terrain Results
0
50
100
150
200
0
50
100
150
2000
20
40
60
80
100
EN
h
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 40
RRT 3D Terrain Results
0
50
100
150
200
0
50
100
150
2000
20
40
60
80
100
EN
h
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 41
RRT 3D Terrain Results
0
50
100
150
200
050
100150
2000
20
40
60
80
100
E
N
h
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 42
RRT Dubins Approach
• To generate a new random configuration for
tree:
– Generate random N-E position in environment
– Find closest node in RRT graph to new random point
– Select a position of distance L from the closest RRT
node in direction of new point – use this position as
N-E coordinates of new configuration
– Define course angle for new configuration as the
angle of the line connecting the RRT graph to the new
configuration
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 43
RRT Dubins Results
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 44
RRT Dubins Results
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 45
RRT Dubins Results
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 46
Coverage Algorithms
• Goal: Survey an area
– Pass sensor footprint over entire area
• Algorithms often cell based
– Goal: visit every cell
0 20 40 60 80 1000
10
20
30
40
50
60
70
80
90
100
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 47
Coverage Algorithm
• Two maps in memory
– terrain map
• used to detect collisions with environment
– coverage or return map
• used to track coverage of terrain
• Return map stores value of returning to
particular location
– Return map initialized so that all locations have same
return value
– As locations are visited, return value of that location is
decremented by fixed amount:
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 48
Coverage Algorithm
• Finite look ahead tree search used to determine where
to go
• Tree generated from current MAV configuration
• Tree searched to determine path that maximizes return
value
• Two methods for look ahead tree
– Uniform branching
• Predetermined depth
• Uniform branch length
• Uniform branch separation
– RRT
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 49
Coverage Algorithm
September 15, 2011 Time: 12:53pm chapter12.tex
Path Planning 221
Figure 12.13 A look-ahead tree of depth three is generated from the position(0, 0), where L = 10, and ϑ = π/6.
look-ahead tree is generated by taking finite steps of length L and, atthe end of each step, by allowing the MAV to move straight or changeheading by ±ϑ at each configuration. A three-step look-ahead treewhere ϑ = π/6 is shown in figure 12.13.
Algorithm 12 Plan Cover Path: planCover(T , ϒ, p)
Input: Terrain map T , returnmap ϒ, initial configuration ps
1: Initialize look-ahead tree G = (V, E ) as V = {ps}, E = ∅2: Initialize returnmap ϒ = {ϒi : i indexes the terrain}3: p = ps
4: for Each planning cycle do5: G = generateTree(p, T , ϒ)6: W = highestReturnPath(G)7: Update p bymoving along the first segment ofW8: Reset G = (V, E ) as V = {p}, E = ∅9: ϒ = updateReturnMap(ϒ, p)
10: end for
The results of using a waypoint look-ahead tree in algorithm 12 areshown in figure 12.14. figure 12.14(a) shows paths through an obstaclefield where the look-ahead length is L = 5, the allowed heading changeat each step is ϑ = π/6, and the depth of the look-ahead tree is three.
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 50
Coverage Planning Results – Uniform Tree
0 20 40 60 80 1000
10
20
30
40
50
60
70
80
90
100
0
5
10
0
5
10−300
−200
−100
0
100
Look ahead length = 5
Heading change = 30 deg
Tree depth = 3
Iterations = 200
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 51
Coverage Planning Results – Uniform Tree
Look ahead length = 5
Heading change = 60 deg
Tree depth = 3
Iterations = 200
0
5
10
02
46
810
−300
−200
−100
0
100
0 20 40 60 80 1000
10
20
30
40
50
60
70
80
90
100
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 52
Coverage Planning Results – RRT Dubins
0
5
10
0
5
10−200
−100
0
100
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 53
Coverage Planning Results – RRT Dubins
0
5
10
0
5
10−150
−100
−50
0
50
100