1 of 56

Chapter 12

Path Planning

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

2 of 56

Control Architecture

path planner

path manager

path following

autopilot

unmanned aircraft

waypoints

on-board sensors

position error

tracking error

status

destination,�obstacles

servo commands

state estimator

wind

path definition

airspeed,�altitude,

heading,

commands

map

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

3 of 56

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

4 of 56

Voronoi Graphs

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

5 of 56

Voronoi Graph Example

  • 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

6 of 56

Voronoi Graph Example

  • 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 6

7 of 56

Path Cost Calculation

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

8 of 56

Path Cost Calculation

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

9 of 56

Path Cost Calculation

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

10 of 56

Voronoi Path Planning Algorithm

Dijkstra’s algorithm is used to search the graph

​

Voronoi graph and Dijkstra’s algorithm code are commonly available

Matlab:

Voronoi -> voronoi

Dijkstra -> shortestpath

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

11 of 56

Voronoi Path Planning Result

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

12 of 56

Voronoi Path Planning – Non-point Obstacles

  • 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 12

13 of 56

Non-point Obstacles – Step 1

Insert points around perimeter of obstacles

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

14 of 56

Non-point Obstacles – Step 2

Construct Voronoi graph

​

Note infeasible path edges inside obstacles

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

15 of 56

Non-point Obstacles – Step 3

Remove infeasible path edges from graph

​

Search graph for best path

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

16 of 56

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

17 of 56

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

18 of 56

RRT Algorithm

  • Algorithm initialized
    • start node
    • end node
    • terrain/obstacle map

​

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

19 of 56

RRT Algorithm

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

20 of 56

RRT Algorithm

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

21 of 56

RRT Algorithm

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

22 of 56

RRT Algorithm

  • Continue adding nodes and checking for collisions

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

23 of 56

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

24 of 56

RRT Algorithm

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

25 of 56

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

26 of 56

Path Smoothing Algorithm

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

27 of 56

RRT Results

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

28 of 56

RRT Results

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

29 of 56

RRT Results

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

30 of 56

RRT Results

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

31 of 56

RRT* Algorithm – Extend Step

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

32 of 56

RRT* Algorithm – Re-wire Step

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

33 of 56

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 33

34 of 56

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

35 of 56

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

​

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

36 of 56

RRT Algorithm – 3D Terrain

modify to test for collisions

with terrain and flight path

angle feasibility

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

37 of 56

RRT 3D Terrain Results

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

38 of 56

RRT 3D Terrain Results

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

39 of 56

RRT 3D Terrain Results

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

40 of 56

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 40

41 of 56

RRT Algorithm - Dubins

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

42 of 56

RRT Algorithm - Dubins

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

43 of 56

RRT Algorithm - Dubins

Collision

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

44 of 56

RRT Algorithm - Dubins

  • Continue adding nodes and checking for collisions

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

45 of 56

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 45

46 of 56

RRT Dubins Results

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

47 of 56

RRT Dubins Results

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

48 of 56

RRT Dubins Results

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

49 of 56

Coverage Algorithms

  • Goal: Survey an area
    • Pass sensor footprint over entire area

​

  • Algorithms often cell based
    • Goal: visit every cell

​

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

50 of 56

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 50

51 of 56

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 51

52 of 56

Coverage Algorithm

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

53 of 56

Coverage Planning Results – Uniform Tree

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 53

54 of 56

Coverage Planning Results – Uniform Tree

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 54

55 of 56

Coverage Planning Results – RRT Dubins

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

56 of 56

Coverage Planning Results – RRT Dubins

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