Chapter 12. Path Planning. Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 2012,

Size: px

Start display at page:

Download "Chapter 12. Path Planning. Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 2012,"

Clifton Bradford
6 years ago
Views:

1 Chapter 12 Path Planning Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 1

2 Control Architecture destination, obstacles map path planner waypoints status path manager path definition tracking error airspeed, altitude, heading, commands path following autopilot position error servo commands wind unmanned aircraft state estimator on-board sensors ˆx(t) Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 2

3 Deliberative Path Planning Approaches 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, 212, Chapter 12: Slide 3

4 Voronoi Graphs Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 4

5 Voronoi Graph Example 2 15 north position (km) east position (km) Graph generated using voronoi command in Matlab 2 point obstacles Start and end points of path not shown Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 5

6 Voronoi Graph Example north position (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? east position (km) Furthest from obstacles? Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 6

7 Path Cost Calculation Nodes of graph edge: v 1 and v 2 Point obstacle: p Length of edge: kv 1 v 2 k Any point on line segment: w( )=(1 )v 1 + v 2 where 2 [, 1] Minimum distance between p and graph edge D(v 1, v 2, p) = 4 min kp w( )k 2[,1] = min 2[,1] = min 2[,1] q (p w( )) > (p w( )) q kp v 1 k 2 +2 (p v 1 ) > (v 1 v 2 )+ 2 kv 1 v 2 k 2. Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 7

8 Path Cost Calculation Value for that minimizes D is = (v 1 p) > (v 1 v 2 ) kv 1 v 2 k 2 Location along edge for which D is minimum: s w( )= kp v 1 k 2 ((v 1 p) > (v 1 v 2 )) 2 kv 1 v 2 k 2 Define distance to edge for <, > 1: 8 >< w( ) if 2 [, 1] D (v 1, v 2, p) = 4 kp v 1 k if >: < kp v 2 k if > 1 Distance between point set Q and line segment v 1 v 2 : D(v 1, v 2, Q) =min p2q D (v 1, v 2, p) Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 8

9 Path Cost Calculation Cost for traveling along edge (v 1, v 2 ) is assigned as k 2 J(v 1, v 2 )=k 1 kv 1 v 2 k + {z } D(v 1, v 2, Q) length of edge {z } reciprocal of distance to closest point in Q k 1 and k 2 are positive weights Choice of k 1 and k 2 allow tradeo between path length and proximity to threats. Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 9

10 Voronoi Path Planning Algorithm Algorithm 9 Plan Voronoi Path: W = planvoronoi(q, p s, p e ) Input: Obstacle points Q, start position p s,endpositionp e Require: Q 1 Randomly add points if necessary. 1: (V, E ) = constructvoronoigraph(q) 2: V + = V {p s } {p e } 3: Find {v 1s, v 2s, v 3s }, the three closest points in V to p s,and {v 1e, v 2e, v 3e }, the three closest points in V to p e 4: E + = E i=1,2,3 (v is, p s ) i=1,2,3 (v ie, p e ) 5: for Each element (v a, v b ) E do 6: Assign edge cost J ab = J (v a, v b ) 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 Voronoi graph and Dijkstra s algorithm code are commonly available Matlab: Voronoi -> voronoi Dijkstra -> shortestpath Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 1

11 Voronoi Path Planning Result 2 15 north position (km) east position (km) Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 11

12 Voronoi Path Planning Non-point Obstacles How do we handle solid obstacles? 1 Point obstacles at center of spatial obstacles won t provide safe path options around obstacles north position (km) east position (km) Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 12

13 Non-point Obstacles Step 1 Insert points around perimeter of obstacles 1 north position (km) east position (km) Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 13

14 Non-point Obstacles Step 2 Construct Voronoi graph Note infeasible path edges inside obstacles 1 north position (km) east position (km) Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 14

15 Non-point Obstacles Step 3 Remove infeasible path edges from graph Search graph for best path 1 north position (km) east position (km) Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 15

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, 212, Chapter 12: Slide 16

RRT Tree Structure RRT algorithm is implemented using tree data structure

parent Every node has one parent, except root Nodes represent physical

edge has cost associated traversing feasible path between states Beard &

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, 212, Chapter 12: Slide 17

18 Algorithm initialized start node end node terrain/obstacle map RRT Algorithm Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 18

19 RRT Algorithm Randomly select configuration p in workspace Select new configuration v 1 fixed distance D from start point along line connecting p s and p Check new segment for collisions If collision-free, insert v 1 into tree. Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 19

20 Generate random configuration p in workspace Search tree to find node closest to p From node closest to p,move distance D along line connecting node and p to establish configuration v 2 Check new segment for collisions RRT Algorithm If collision-free, insert v 2 into tree Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 2

21 Generate random configuration p in workspace Search tree to find node closest to p From node closest to p,move distance D along line connecting node and p to establish new node Check new segment for collisions If collision-free, insert new node into tree RRT Algorithm Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 21

22 RRT Algorithm Continue adding nodes and checking for collisions Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 22

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, 212, Chapter 12: Slide 23

24 RRT Algorithm Algorithm 1 Plan RRT Path: W = planrrt(t, p s, p e ) Input: Terrain map T,startconfigurationp s,endconfigurationp e 1: Initialize RRT graph G = (V, E )asv ={p s }, E = 2: while The end node p e is not connected to G,i.e.,p e 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 )asv V {v + }, E E {(v, v + )} 8: Update edge costs as C[(v, v + )] pathlength(v, v + ) 9: end if 1: if existfeasiblepath(t, v +, p e ) then 11: Update graph G = (V, E )asv V {p e } E E {(v, p e )} 12: Update edge costs as C[(v, p e )] pathlength(v, p e ) 13: end if 14: end while 15: W = findshortestpath(g, C). 16: return W Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 24

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, 212, Chapter 12: Slide 25

26 Path Smoothing Algorithm Algorithm 11 Smooth RRT Path: (W s, C s ) = smoothrrt(t, W, C) Input: Terrain map T,waypointpathW ={w 1,...,w N },cost matrix C 1: Initialized smoothed path W s {w 1 } 2: Initialize pointer to current node in W s : i 1 3: Initialize pointer to next node in W: j 2 4: while j < N do 5: w s getnode(w s, i) 6: w + getnode(w, j + 1) 7: if existfeasiblepath(t, w s, w + ) = FALSE then 8: Get last node: w getnode(w, j) 9: Add deconflicted node to smoothed path: W s W s {w} 1: Update smoothed cost: C s [(w s, w)] pathlength(w s, w) 11: i i : end if 13: j j : end while 15: Add last node from W: W s W s {wn } 16: Update smoothed cost: C s [(w i, w N )] pathlength(w i, w N ) 17: return W s Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 26

27 RRT Results Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 27

28 RRT Results Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 28

29 RRT Results Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 29

30 RRT Results Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 3

31 RRT* Algorithm Extend Step Generate new potential node v new identical to RRT. Instead of finding the closest node in the tree, find all nodes within neighborhood N. N v newv Let C(v) be the path cost from p s to v, and let c(v 1, v 2 ) be the path cost from v 1 to v 2. Let v = arg min v2n (C(v)+c(v new, v)) (i.e., closest node in N to v new ). Add node V V S {v new }. Add edge E E S {(v, v new )}. Set C(v new )=C(v )+c(v new, v). Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 31

32 RRT* Algorithm Re-wire Step Check all nodes in v 2 N to see if C(v new )+c(v new, v) <C(v) re- i.e., if re-routing through v new duces the path length. N v new If so, remove edge between v and its parent, and add edge between v and v new. Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 32

33 RRT* Algorithm Re-wire Step Check all nodes in v 2 N to see if C(v new )+c(v new, v) <C(v) re- i.e., if re-routing through v new duces the path length. N v new If so, remove edge between v and its parent, and add edge between v and v new. Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 33

34 RRT vs. RRT* From S. Karaman and E. Frazzoli, Incremental Sampling-based Algorithms for Optimal Motion Planning, International Journal of Robotic Research, 21. Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 34

35 RRT vs. RRT* From S. Karaman and E. Frazzoli, Incremental Sampling-based Algorithms for Optimal Motion Planning, International Journal of Robotic Research, 21. Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 35

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 h N E 15 2 Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 36

37 RRT Algorithm 3D Terrain Algorithm 1 Plan RRT Path: W = planrrt(t, p s, p e ) Input: Terrain map T,startconfigurationp s,endconfigurationp e 1: Initialize RRT graph G = (V, E )asv ={p s }, E = 2: while The end node p e is not connected to G,i.e.,p e 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 )asv V {v + }, E E {(v, v + )} 8: Update edge costs as C[(v, v + )] pathlength(v, v + ) 9: end if 1: 11: if existfeasiblepath(t, v +, p e ) then Update graph G = (V, E )asv V {p e } E E {(v, p e )} 12: Update edge costs as C[(v, p e )] pathlength(v, p e ) 13: end if 14: end while 15: W = findshortestpath(g, C). 16: return W Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 37

38 RRT 3D Terrain Results 1 8 h N 5 E Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 38

39 RRT 3D Terrain Results 8 6 h N E Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 39

40 RRT 3D Terrain Results 8 6 h N E Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 4

41 RRT 3D Terrain Results h N 5 5 E Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 41

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, 212, Chapter 12: Slide 42

43 RRT Dubins Results Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 43

44 RRT Dubins Results Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 44

45 RRT Dubins Results Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 45

46 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, 212, Chapter 12: Slide 46

47 Two maps in memory terrain map Coverage Algorithm 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, 212, Chapter 12: Slide 47

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 RRT Predetermined depth Uniform branch length Uniform branch separation Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 48

49 Coverage Algorithm Algorithm 12 Plan Cover Path: plancover(t, ϒ, p) Input: Terrain map T,returnmapϒ,initialconfigurationp s 1: Initialize look-ahead tree G = (V, E )asv ={p s }, E = 2: Initialize return map ϒ ={ϒ i : i indexes the terrain} 3: p = p s 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 )asv ={p}, E = 9: ϒ = updatereturnmap(ϒ, p) 1: end for Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 49

50 Coverage Planning Results Uniform Tree Look ahead length = 5 Heading change = 3 deg Tree depth = 3 Iterations = 2 Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 5

51 Coverage Planning Results Uniform Tree Look ahead length = 5 Heading change = 6 deg Tree depth = 3 Iterations = 2 Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 51

Coverage Planning Results RRT Dubins 1 1 2 1 5 5 1 Beard & McLain, Small

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

53 Coverage Planning Results RRT Dubins Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 12: Slide 53

Probabilistic Methods for Kinodynamic Path Planning

16.412/6.834J Cognitive Robotics February 7 th, 2005 Probabilistic Methods for Kinodynamic Path Planning Based on Past Student Lectures by: Paul Elliott, Aisha Walcott, Nathan Ickes and Stanislav Funiak