Chapter 12

Path Planning

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 1
 Control Architecture
destination, map
obstacles

path planner
waypoints
status

path manager
path definition
tracking error

airspeed, path following

altitude,
heading, position error
commands
autopilot
servo commands
state estimator
wind
unmanned aircraft x̂(t)
on-board sensors

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 2
 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 3
 Voronoi Graphs

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 4
 Voronoi Graph Example
20

15

10
north position (km)

−5

−10
−10 −5 0 5 10 15 20
east position (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 5
 Voronoi Graph Example
20
• Add start and end points to
graph
15

• Find 3 closest graph nodes to

10
start and end points
north position (km)

• Add graph edges to start and

5 end points
• Search graph to find “best” path
0
• Must define “best”
−5 • Shortest?
• Furthest from obstacles?
−10
−10 −5 0 5 10 15 20
east position (km)

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 6
 Path Cost Calculation

Nodes of graph edge: v1 and v2

Point obstacle: p

Length of edge: kv1 v2 k

Any point on line segment: w( ) = (1 )v1 + v2 where 2 [0, 1]

Minimum distance between p and graph edge

4
D(v1 , v2 , p) = min kp w( )k
2[0,1]
q
= min (p w( ))> (p w( ))
2[0,1]
q
2 2
= min kp v1 k + 2 (p v1 )> (v1 v2 ) + 2 kv1 v2 k .
2[0,1]

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 7
 Path Cost Calculation

Value for that minimizes D is

⇤ (v1 p)> (v1 v2 )

= 2
kv1 v2 k

Location along edge for which D is minimum:

s
> (v 2
2 ((v 1 p) 1 v 2 ))
w( ⇤ ) = kp v1 k 2
kv1 v2 k
⇤
Define distance to edge for < 0, ⇤ > 1:
8
>
< w( ⇤ ) if ⇤
2 [0, 1]
0 4 ⇤
D (v1 , v2 , p) = kp v1 k if <0
>
: ⇤
kp v2 k if >1

Distance between point set Q and line segment v1 v2 :

0
D(v1 , v2 , Q) = min D (v1 , v2 , p)
p2Q

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 8
 Path Cost Calculation

Cost for traveling along edge (v1 , v2 ) is assigned as

k2
J(v1 , v2 ) = k1 kv1 v2 k +
| {z } D(v1 , v2 , Q)
length of edge | {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 9
 East (km)

Voronoi Path Planning Algorithm

Figure 12.4 Optimal path through the Voronoi graph.

Algorithm 9 Plan Voronoi Path: W = planVoronoi(Q, ps , pe )

Input: Obstacle points Q, 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 (v
i=1,2,3 is , p s ) i=1,2,3 (vie , pe )
5: for Each element (va , vb ) ∈ E do
6: Assign edge cost Jab = J (va , vb ) according to equation (12.1).
7: end for
8: W = DijkstraSearch(V + , E + , J)
9: return W

Dijkstra’s algorithm is used to search the graph

One of the disadvantages of the Voronoi method described in al-
gorithm
Voronoi graph9andis that it is algorithm
Dijkstra’s limited tocode
pointareobstacles.
commonlyHowever,
available there are
straightforward
Matlab: modifications for non-point obstacles. For example,
consider the obstacle
Voronoi field shown in figure 12.5(a). A Voronoi graph can
-> voronoi
be constructed by first adding
Dijkstra points around the perimeter of the obsta-
-> shortestpath
Beard cles that exceed
& McLain, a certain Aircraft,”
“Small Unmanned size, as shown in University
Princeton figure 12.5(b).
Press, The
2012,associated
Chapter 12: Slide 10
Voronoi graph, including connections to start and end nodes, is shown
 Voronoi Path Planning Result
20

15

10
north position (km)

−5

−10
−10 −5 0 5 10 15 20
east position (km)
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 11
 Voronoi Path Planning – Non-point Obstacles
• How do we handle solid obstacles?
• Point obstacles at center of
spatial obstacles won’t provide
10 safe path options around
obstacles
8
north position (km)

0 2 4 6 8 10
east position (km)
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 12
 Non-point Obstacles – Step 1
Insert points around perimeter of obstacles

10

8
north position (km)

0 2 4 6 8 10
east position (km)

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 13
 Non-point Obstacles – Step 2
Construct Voronoi graph
Note infeasible path edges inside obstacles

10

8
north position (km)

0 2 4 6 8 10
east position (km)

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 14
 Non-point Obstacles – Step 3
Remove infeasible path edges from graph
Search graph for best path

10

8
north position (km)

0 2 4 6 8 10
east position (km)

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 15
 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 16
 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 17
 RRT Algorithm
• Algorithm initialized
– start node
– end node
– terrain/obstacle map

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 18
 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 19
 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 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 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 21
 RRT Algorithm
• Continue adding nodes and checking for
collisions

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 22
 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 23
 resulting path is feasible, as checked in line 6, then v+ is added to G in
line 7 and the cost matrix is updated in line 8. The if statement in line 10
checks to see if the new node v+ can be connected directly to the end
RRT Algorithm
node pe . If so, pe is added to G in line 11–12, and the algorithm ends in
line 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 do
3: p ← generateRandomConfiguration(T )
4: v∗ ← findClosestConfiguration(p, V)
5: v+ ← planPath(v∗ , p, D)
6: if existFeasiblePath(T , v∗ , v+ ) then
! !
7: 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 ) then
! !
11: 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 if
14: end while
15: W = findShortestPath(G, C).
16: return W

The result of implementing algorithm 10 for four different randomly

Beard & McLain,
generated“Small Unmanned
obstacle fields Aircraft,” Princeton
and randomly University
generated Press,
start and 2012, Chapter 12: Slide 24
end nodes
is displayed with a dashed line in figure 12.8. Note that the paths gener-
 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 25
Path Planning Path Smoothing Algorithm 217

Algorithm 11 Smooth RRT Path: (Ws , Cs ) = smoothRRT(T , W, C)

Input: Terrain map T , waypoint path W = {w1 , . . . , w N }, cost
matrix C
1: Initialized smoothed path Ws ← {w1 }
2: Initialize pointer to current node in Ws : i ← 1
3: Initialize pointer to next node in W: j ← 2
4: while j < N do
5: ws ← getNode(Ws , i)
6: w+ ← getNode(W, j + 1)
7: if existFeasiblePath(T , ws , w+ ) = FALSE then
8: 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 +1
12: end if
13: j ← j +1
14: end while
!
15: Add last node from W: Ws ← Ws {w N }
16: Update smoothed cost: Cs [(wi , w N )] ← pathLength(wi , w N )
17: return Ws

algorithm 10 will be a path that follows the terrain at a fixed altitude

Beard &h McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 26
AG L . However, the smoothing step represented by algorithm 11 will
result in paths with much less altitude variation. For 3-D terrain, the
 RRT Results
100

90

80

70

60

50

40

30

20

10

0
0 10 20 30 40 50 60 70 80 90 100

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 27
 RRT Results
100

90

80

70

60

50

40

30

20

10

0
0 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
100

90

80

70

60

50

40

30

20

10

0
0 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
100

90

80

70

60

50

40

30

20

10

0
0 10 20 30 40 50 60 70 80 90 100

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 30
 RRT* Algorithm – Extend Step
• Generate new potential node vnew
identical to RRT.
• Instead of finding the closest node in
the tree, find all nodes within neigh- N
borhood N .
vnew
• Let C(v) be the path cost from ps v0
to v, and let c(v1 , v2 ) be the path
cost from v1 to v2 .
• Let v0 = arg minv2N (C(v) + c(vnew , v))
(i.e., closest node in N to vnew ).
S
• Add node V V {vnew }.
S
• Add edge E E {(v0 , vnew )}.
• Set C(vnew ) = C(v0 ) + c(vnew , v).

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 31
 RRT* Algorithm – Re-wire Step
• Check all nodes in v 2 N to see if

C(vnew ) + c(vnew , v) < C(v)

N
i.e., if re-routing through vnew re- vnew
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 32
 RRT* Algorithm – Re-wire Step
• Check all nodes in v 2 N to see if

C(vnew ) + c(vnew , v) < C(v)

N
i.e., if re-routing through vnew re- vnew
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 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 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 Path Planning Over 3D Terrain
• Assume terrain map can be queried
to determine altitude of terrain at 100

any north-east location 80

60 200

• Must be able to determine altitude

h
40
150

for random configuration p in RRT 20

0
100

algorithm 200
150 50
100

• Must be able to detect collisions with N

50
0
0
E

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

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 36
resulting path is feasible, as checked in line 6, then v+ is added to G in
line 7 and the cost matrix is updated in line 8. The if statement in line 10
checks to see if the new node v+ can be connected directly to the end

RRT Algorithm – 3D Terrain

node pe . If so, pe is added to G in line 11–12, and the algorithm ends in
line 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 do
3: p ← generateRandomConfiguration(T ) modify to test for collisions
4: v ← findClosestConfiguration(p, V)
∗
with terrain and flight path
5: v ← planPath(v , p, D)
+ ∗
angle feasibility
6: if existFeasiblePath(T , v∗ , v+ ) then
! !
7: 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 ) then
! !
11: 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 if
14: end while
15: W = findShortestPath(G, C).
16: return W

The result of implementing algorithm 10 for four different randomly

Beard & McLain,
generated obstacle“Small
fieldsUnmanned Aircraft,”
and randomly Princeton
generated University
start and endPress,
nodes2012, Chapter 12: Slide 37
is displayed with a dashed line in figure 12.8. Note that the paths gener-
 RRT 3D Terrain Results

100

80

60
h

40
200

20
150
0
200 100
150
100 50
50
E
0 0
N

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 38
 RRT 3D Terrain Results
100

80

60
h

40

20

0
200
200
150
150
100 100

50 50

N 0 0 E

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 39
 RRT 3D Terrain Results
100

80

60
h

40

20

0
200
200
150
150
100 100

50 50

N 0 0 E

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 40
 RRT 3D Terrain Results

100

80

60
h

200
40

150
20

100
0
200
150 50
100
50 E
0
0
N

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 41
 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 42
 RRT Dubins Results

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
 Coverage Algorithms
• Goal: Survey an area
– Pass sensor footprint over entire area
• Algorithms often cell based
– Goal: visit every cell 100

90

80

70

60

50

40

30

20

10

0
0 20 40 60 80 100

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 46
 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 47
 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 48
 (0, 0), where L = 10, and ϑ = π/6.

look-ahead tree is generated by taking finite steps of length L and, at

Coverage Algorithm
the end of each step, by allowing the MAV to move straight or change
heading by ±ϑ at each configuration. A three-step look-ahead tree
where ϑ = π/6 is shown in figure 12.13.

Algorithm 12 Plan Cover Path: planCover(T , ϒ, p)

Input: Terrain map T , return map ϒ, initial configuration ps
1: Initialize look-ahead tree G = (V, E ) as V = {ps }, E = ∅
2: Initialize return map ϒ = {ϒi : i indexes the terrain}
3: p = ps
4: for Each planning cycle do
5: G = generateTree(p, T , ϒ)
6: W = highestReturnPath(G)
7: Update p by moving along the first segment of W
8: 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 are

shown in figure 12.14. figure 12.14(a) shows paths through an obstacle
field where the look-ahead length is L = 5, the allowed heading change
Beard & at each “Small
McLain, step isUnmanned and thePrinceton
ϑ = π/6,Aircraft,” depth ofUniversity
the look-ahead tree isChapter
Press, 2012, three. 12: Slide 49
 Coverage Planning Results – Uniform Tree

100

90

80 100

70 0
60
−100
50

40 −200

30
−300
20 10
10
10 5
5
0
0 20 40 60 80 100 0 0

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 50
Coverage Planning Results – Uniform Tree

100

90

80 100
70
0
60
−100
50
−200
40
−300
30 10
8
20 10
6
10 4 5
2
0
0 20 40 60 80 100 0 0

Look ahead length = 5

Heading change = 60 deg
Tree depth = 3
Iterations = 200
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 51
 Coverage Planning Results – RRT Dubins

100

−100

−200
10
10
5
5

0 0

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 52
Coverage Planning Results – RRT Dubins

100

50

−50

−100

−150
10
10
5
5

0 0

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 12: Slide 53