Chapter 12uavbook.byu.edu/lib/exe/fetch.php?media=lecture:chap12.pdf · Beard & McLain, “Small...

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

Chapter 12

Path Planning


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


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


Voronoi Graphs

−10 −8 −6 −4 −2 0 2 4 6 8 10−10

−8

−6

−4

−2

0

2

4

6

8

10


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


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?


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.



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)



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.


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


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

)


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


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



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

)



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

)


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


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


RRT Algorithm

• Algorithm initialized

– start node

– end node

– terrain/obstacle map


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.


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


lisions

• If collision-free, insert v2 into

tree


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


lisions

• If collision-free, insert new

node into tree


RRT Algorithm

• Continue adding nodes and checking for

collisions


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


RRT Algorithm


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


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


Path Smoothing Algorithm


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.


RRT Results

0 10 20 30 40 50 60 70 80 90 1000

10

20

30

40

50

60

70

80

90

100


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


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.


RRT vs. RRT*

From S. Karaman and E. Frazzoli, “Incremental Sampling-based Algorithms for

Optimal Motion Planning,” International Journal of Robotic Research, 2010.


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


RRT Algorithm – 3D Terrain


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


RRT 3D Terrain Results

0

50

100

150

200

0

50

100

150

2000

20

40

60

80

100

EN

h



0

50

100

150

200

0

50

100

150

2000

20

40

60

80

100

EN

h



0

50

100

150

200

050

100150

2000

20

40

60

80

100

E

N

h


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


RRT Dubins Results


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


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:


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


Coverage Algorithm


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.


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


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


Coverage Planning Results – RRT Dubins

0

5

10

0

5

10−200

−100

0

100


Coverage Planning Results – RRT Dubins

0

5

10

0

5

10−150

−100

−50

0

50

100

Chapter 12uavbook.byu.edu/lib/exe/fetch.php?media=lecture:chap12.pdf · Beard & McLain, “Small...

Documents

Transcript of Chapter 12uavbook.byu.edu/lib/exe/fetch.php?media=lecture:chap12.pdf · Beard & McLain, “Small...