Chapter 11. Path Manager. Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 2012,

Transcription

1 Chapter 11 Path Manager Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, 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 11, Slide 2

3 Path Definition Waypoint path defined as ordered sequence of waypoints where w i =(w n,i,w e,i,w d,i ) > 2 R 3. W = {w 1, w 2,...,w N } Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 3

4 Two methods Waypoint Switching b-ball around waypoint half plane through waypoint Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 4

5 Waypoint Switching Given point r 2 R 3 and normal vector n 2 R 3, define half plane H(r, n) 4 = p 2 R 3 :(p r) > n Define unit vector pointing in direction of line w i w i+1 as 4 w i+1 w i q i = kw i+1 w i k Unit normal to the 3-D half plane that separates the line w i 1 w i from the line w i w i+1 is given by 4 q i 1 + q i n i = kq i 1 + q i k MAV tracks straight-line path from w i 1 to w i until it enters H(w i, n i ), at which point it will track straight-line path from w i to w i+1 Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 5

6 Waypoint Following Algorithm 5 Follow Waypoints: (r, q) = followwpp(w, p) Input: Waypoint path W ={w 1,...,w N }, MAV position p = (p n, p e, p d ). Require: N 3 1: if New waypoint path W is received then 2: Initialize waypoint index: i 2 3: end if 4: r w i 1 5: q i 1 w i w i 1 w i w i 1 6: q i w i+1 w i w i+1 w i 7: n i q i 1+q i q i 1 +q i 8: if p H(w i, n i ) then 9: Increment i (i + 1) until i = N 1 1: end if 11: return r, q = q i 1 at each time step Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 6

7 Waypoint Following Results Path Manager Straight Line north position east position Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 7

8 Fillet Transition Transition between path segments can be smoothed by adding a fillet Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 8

9 Fillet Geometry R tan % 2 R Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 9

10 Fillet Smoothing Half Planes Fillet center defined as c = w i R sin % 2 qi 1 q i kq i 1 q i k Half plane H 1 is defined by location R r 1 = w i tan % 2 q i 1 H 1 R H 2 and normal vector q i 1 Half plane H 2 is defined by location R r 2 = w i + tan % q i 2 and normal vector q i Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 1

11 Waypoint Following with Fillets Algorithm 6 Follow Waypoints with Fillets: (flag, r, q, c, ρ, λ) = followwppfillet(w, p, R) Input: Waypoint path W ={w 1,...,w N }, MAV position p = (p n, p e, p d ),filletradiusr. Require: N 3 1: if New waypoint path W is received then 2: Initialize waypoint index: i 2, and state machine: state 1. 3: end if 4: q i 1 w i w i 1 w i w i 1 5: q i w i+1 w i w i+1 w i 6: ϱ cos 1 ( q i 1 q i) 7: if state = 1 then 8: flag 1 9: r w i 1 1: q q i 1 ( 11: z w i R tan(ϱ/2) ) q i 1 12: if p H(z, q i 1 ) then 13: state 2 14: end if 15: else if state = 2 then 16: flag 2 ( 17: c w i + 18: ρ R R sin(ϱ/2) ) qi 1 q i q i 1 q i 19: λ sign(q i 1,n q i,e q i 1,e q i,n ) ( ) 2: z w i + R tan(ϱ/2) q i 21: if p H(z, q i ) then 22: i (i + 1) until i = N 1. 23: state 1 24: end if 25: end if 26: return flag, r, q, c, ρ, λ. H 1 R H 2 Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 11

12 Waypoint Following with Fillets Path Manager Fillets north position east position Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 12

13 Fillet Path Length R tan % 2 Straight-line path length (no fillets): NX W = 4 kw i w i 1 k. i=2 R Path length with fillets: W F = W + NX i=2 R% i 2R tan % i 2. Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 13

14 Dubins Paths p s turn-straight-turn Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 14 p e

15 Four Cases c ls s c ls s p s p start p s c rs c rs Case I: R-S-R p c e re e c le Case II: R-S-L c re p e e c le c ls p start p s c rs s e p e c le c re p start p s s c rs e c le cre p e Case III: L-S-R Dubins path is defined as shortest of four cases Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 15 c ls

16 Path Length Preliminaries Given position p, course, and radius R, centers of right and left turning circles are given by c r = p + R cos( + 2 ), sin( + 2 ), > c l = p + R cos( 2 ), sin( 2 ), > For clockwise circles, angular distance between # 1 and # 2 given by # 2 # 1 CW 4 = h2 + #2 # 1 i, where h'i = 4 ' mod 2 For counter clockwise circles, CW # 2 # 1 CCW 4 = h2 #2 + # 1 i Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 16 CCW

17 Alternative Viewpoint x = # 2 +2 # 1 2 # 1 CW Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 17

18 Alternative Viewpoint x =2 # 2 + # 1 2 # 2 CCW Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 18

19 Dubins Case I: R-S-R s p s c rs Distance traveled along c rs Distance traveled along c re Total path length: L 1 = kc rs c re k + Rh2 + h# Rh2 + h# Rh2 + h e 2 i h s 2 i h s 2 ii 2 i h# 2 ii s 2 2 ii + Rh2 + h e e c re p e e 2 i h# 2 ii 2 Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 19

20 Dubins Case II: R-S-L s p s c rs 2 4 R 2 s 2 e pe Distance traveled along c rs 2 Rh2 + h# # 2 i h s 2 ii c le e + 2 Distance traveled along c le Rh2 + h# 2 + i h e + 2 ii Total path length: L 2 = p`2 4R 2 + Rh2 + h# # 2 i h s 2 ii + Rh2 + h# 2 + i h e + 2 ii Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 2

21 Dubins Case III: L-S-R p s s + 2 s c ls 2 Distance traveled along c ls 2 4 R 2 c re Rh2 + h s + 2 i h# + # 2ii e p e e 2 Distance traveled along c re Total path length: Rh2 + h e 2 i h# + # 2 ii L 3 = p`2 4R 2 +Rh2 +h s + 2 i h#+# 2ii+Rh2 +h e 2 i h#+# 2 ii Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 21

22 Dubins Case IV: L-S-L p s s + 2 s c ls e p e Distance traveled along c ls c le e + 2 Rh2 + h s + 2 i h# + 2 ii Distance traveled along c le Rh2 + h# + 2 i h e + 2 ii Total path length: L 4 = kc ls c le k + Rh2 + h s + 2 i h# + 2 ii + Rh2 + h# + 2 i h e + 2 ii Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 22

23 Dubins Path Half-plane Switching Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 23

24 Dubins Path Parameters Algorithm Algorithm 7 Find Dubins Parameters: (L, c s, λ s, c e, λ e, z 1, q 1, z 2, z 3, q 3 ) = finddubinsparameters(p s, χ s, p e, χ e, R) Input: Start configuration (p s, χ s ), End configuration (p e, χ e ), Radius R. Require: p s p e 3R Require: R is larger ( than minimum turn radius of MAV 1: c rs p s + RR π ) z 2 (cos χs, sin χ s, ) ( 2: c ls p s + RR π ) z ( 2 (cos χs, sin χ s, ) 3: c re p e + RR π ) z ( 2 ) (cos χe, sin χ e, ) 4: c le p e + RR z π (cos χe, sin χ e, ) 2 5: Compute L 1, L 2, L 3,andL 3 using equations (11.9) through (11.12). 6: L min{l 1, L 2, L 3, L 4 } 7: if arg min{l 1, L 2, L 3, L 4 }=1then 8: c s c rs, λ s +1, c e c re, λ e +1 9: q 1 ce cs c e c s ( ) 1: z 1 c s + RR z π ( q1 2 ) 11: z 2 c e + RR z π q1 2 12: else if arg min{l 1, L 2, L 3, L 4 }=2then 13: c s c rs, λ s +1, c e c le, λ e 1 14: l c e c s 15: ϑ angle(c e c s ) 16: ϑ 2 ϑ π 2 + sin 1 2R l 17: q 1 R z ( ϑ2 + π 2 ) e1 18: z 1 c s + RR z (ϑ 2 ) e 1 19: z 2 c e + RR z (ϑ 2 + π) e 1 2: else if arg min{l 1, L 2, L 3, L 4 }=3then 21: c s c ls, λ s 1, c e c re, λ e +1 22: l c e c s, 23: ϑ angle(c e c s ), 24: 25: ϑ 2 cos 1 ( 2R l ) q 1 R z ϑ + ϑ2 π e1, 2 26: z 1 c s + RR z (ϑ + ϑ 2 ) e 1, 27: z 2 c e + RR z (ϑ + ϑ 2 π) e 1 28: else if arg min{l 1, L 2, L 3, L 4 }=4then 29: c s c ls, λ s 1, c e c le, λ e 1 3: q 1 ce cs c, e c s ( 31: z 1 c s + RR π ) z q1, 2 32: z 2 c e + RR z ( π 2 ) q2 33: end if 34: z 3 p e 35: q 3 R z (χ e )e 1 Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 24

25 Dubins Path Following Algorithm Algorithm 8 Follow Waypoints with Dubins: (flag, r, q, c, ρ, λ) = followwppdubins(p, p, R) Input: Configuration path P ={(w 1, χ 1 ),...,(w N, χ N )}, MAV position p = (p n, p e, p d ),filletradiusr. Require: N 3 1: if New configuration path P is received then 2: Initialize waypoint pointer: i 2, and state machine: state 1. 3: end if 4: (L, c s, λ s, c e, λ e, z 1, q 1, z 2, z 3, q 3 ) finddubinsparameters(w i 1, χ i 1, w i, χ i, R) 5: if state = 1 then 6: flag 2, c c s, ρ R, λ λ s 7: if p H(z 1, q 1 ) then 8: state 2 9: end if 1: else if state = 2 then 11: if p H(z 1, q 1 ) then 12: state 3 13: end if 14: else if state = 3 then 15: flag 1, r z 1, q q 1 16: if p H(z 2, q 1 ) then 17: state 4 18: end if 19: else if state = 4 then 2: flag 2, c c e, ρ R, λ λ e 21: if p H(z 3, q 3 ) then 22: state 5 23: end if 24: else if state = 5 then 25: if p H(z 3, q 3 ) then 26: state 1 27: i (i + 1) until i = N. 28: (L, c s, λ s, c e, λ e, z 1, q 1, z 2, z 3, q 3 ) finddubinsparameters(w i 1, χ i 1, w i, χ i, R) 29: end if 3: end if 31: return flag, r, q, c, ρ, λ. Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 25

26 Dubins Path Following Results Path Manager Dubins north position east position Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 26

27 Dubins Airplane Model Adapted from: Mark Owen, Randal W. Beard, Timothy W. McLain, Implementing Dubins Airplane Paths on Fixed-wing UAVs, Handbook of Unmanned Aerial Vehicles, ed. Kimon P. Valavanis, George J. Vachtsevanos, Springer Verlag, Section XII, Chapter 68, p , 214. Dubins Airplane model: ṙ n = V cos cos c ṙ e = V sin cos c ṙ d = V sin c g = V tan c Where the commanded flight path angle c and the commanded roll angle c are constrained by i i (north) j i (east) k i (down) ṙ n = V cos cos v horizontal component of airspeed vector i i ṙ d = V sin ṙ e = V sin cos c apple c apple. V cos Flight path projected onto ground Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 27

28 3D Vector Field Path Following Adapted from: V. M. Goncalves, L. C. A. Pimenta, C. A. Maia, B. C. O. Durtra, G. A. S. Pereira, B. C. O. Dutra, and G. A. S. Pereira, Vector Fields for Robot Navigation Along Time-Varying Curves in n-dimensions, IEEE Transactions on Robotics, vol. 26, pp , Aug 21. The path is specified as the intersection of two 2D manifolds given by 1 (r) = 2 (r) = r 2 R 3. Define the composite function Note that the gradient points away from the path. W (r) = (r) = (r)+ (r). Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 28

29 3D Vector Field Path Following The desired velocity vector can be chosen as 1 K 1 + K 2 } 2 } velocity directed toward the path velocity directed along the path where K 1 > and K 2 are symmetric tuning matrices, and the definiteness of K 2 determines the direction of travel along the path. Since u may not equal V a, normalize to get u = V a u ku k. Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 29

30 3D Vector Field Path Following Setting the NED components of the velocity of the Dubins airplane model to u =(u 1,u 2,u 3 ) > gives V cos d cos c = u 1 V sin d cos c = u 2 V sin c = u 3. Solving for c, and d results in h c = sat sin 1 u i 3 V d = atan2(u 2,u 1 ). Assuming the inner-loop lateral-directional dynamics are accurately modeled by the coordinated-turn equation, the commanded roll angle is c = sat k ( d ), where k is a positive constant. Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 3

31 3D Vector Field Straight Line path The straight line path is given by P line (c`, `, `) = r 2 R 3 : r = c` + q`, 2 R, where Define to get q` = n q n q e q d 1 A = 1 sin ` cos ` A n lat = n lon q` = 1 cos ` cos sin ` cos ` A. sin 1 cos ` sin ` sin ` sin ` A, cos ` lon (r) =n > lon(r c`) = lat (r) =n > lat(r c`) =. lon (r) = n lat n lon c` q` lat (r) = P line (r, q`) Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 31

32 3D Vector Field Helical Path A helical path is then defined as P helix (c h, h, h,r h, h) ={r 2 R 3 : cyl (r) = and pl (r) =}. where cyl (r) = pl (r) = rn c n R h rd c d R h 2 2 re c e + 1 R h + tan h tan 1 re h r n c e c n h where the initial position along the helix is 1 R h cos h r() = c h R h sin h A. c h is the center of the helix, R h is the radius, is the climb angle. h Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 32

33 Dubins Airplane Paths Given the start configuration z s =(z ns,z es,z ds, s ) > and the end configuration z e =(z ne,z ee,z de, e ) > and the turn radius R, letl car (R, z s, z e )bethe length of the Dubins car path. Recall that is the limit of the fight path angle. There are three possible cases for the commanded altitude gain: Low Altitude: z de z ds apple L car (R min ) tan, i.e., the altitude gain can be achieved by following the Dubins car path with a flight path angle c apple. High Altitude: z de z ds > [L car (R min )+2 R min ] tan. i.e., the altitude gain is larger than following the Dubins car path plus one orbit, at flight path angle. Medium Altitude: L car (R min ) tan < z de z ds apple [L car (R min )+2 R min ] tan. Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 33

34 Low Altitude Dubins Airplane Paths North North z e 5 RSR z e z s 5 East LSR North 5 RSL 5 LSL z e z s z e East z s L air (R min, = tan 1 zde z ds L car (R min ) R = R min )= L car(r min ) cos. z s East 15 5 North East Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 34

35 High Altitude Dubins Airplane Paths RSR z e RSL z e North East z s 5 15 North z e 15 East z s 5 North LSR North z e 5 zs 15 East z s 5 LSL East 5 Find smallest integer k such that (L car (R min )+2 kr min ) tan apple z de z ds < (L car (R min )+2 (k + 1)R min ) tan. Increase the radius R so that (L car (R )+2 kr ) tan = z de z ds. The resulting path is L air (R, ) = L car(r ). cos Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 35

36 Medium Altitude Dubins Airplane Paths c e z e H` w` q` c e e e =1 z e = w e q e H e c i z i i =1 w s c i q i i H i w i ' c s s = 1 c s s q s H s z s z s Key idea: Add an intermediate helix along the path. Could add intermediate helix at start, end, or in the middle of path. Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 36

37 Medium Altitude Dubins Airplane Paths 25 2 RLSR z e 25 2 RLSL ze z s 5 North East North z s 5 East 15 z s z s z e LSLR 5 North East North z e LSRL East Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 212, Chapter 11, Slide 37