Chapter 10. Path Following. Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 2012, Chapter 10, Slide 1

Transcription

1 Chapter 10 Path Following Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 2012, Chapter 10, 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, 2012, Chapter 10, Slide 2

3 Path Following For small UAVs, a major issue is wind Always present to some degree Usually significant with respect to commanded airspeed Wind makes traditional trajectory tracking approaches difficult, if not infeasible Have to know the wind precisely at every instant to determine desired airspeed Better approach: path following Rather than follow this trajectory, we control UAV to stay on this path Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 2012, Chapter 10, Slide 3

4 Path Types We will focus on two types of paths to follow: Straight lines between two points in 3-D Inclination of path within climb capabilities of UAV Circular orbits or arcs in the horizontal plane Paths for common applications can be built up from these path primitives Methods for following other types of paths found in literature Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 2012, Chapter 10, Slide 4

5 Straight Line Path Description e px q e py e p = p r r p north east Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 2012, Chapter 10, Slide 5

6 Lateral Tracking Problem e px q e py r p e p = p r Path error: e p = 0 e px e py A = 4 R P i p i r i e pz north east where the transformation from inertial frame to path frame is 0 R P 4 i cos 1 q sin q 0 sin q cos q 0A Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 2012, Chapter 10, Slide 6

7 Lateral Tracking Problem Relative error dynamics in path frame: ėpx cos q sin = q Vg cos ė py sin q cos q V g sin cos( = V q) g sin( q) Regulate the cross-track error e py to zero by commanding the course angle: e px ė py = V g sin( q) = b ( c )+b ( c ) q e py Select c so that e py! 0 e p = p r r p north Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 2012, Chapter 10, Slide 7 east

8 Longitudinal Tracking Problem e p = p r q d h d ( r d ) h d p q 2 n + q 2 e r d Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 2012, Chapter 10, Slide 8

9 Longitudinal Tracking Problem By similar triangles (h d + r d ) p s 2 n + s 2 e = q d p q 2 n + q 2 e q d h d ( r d ) h d Desired altitude based on current location h d (r, p, q) = r d p s 2 n + s 2 e q d p q 2 n + q 2 e! p q 2 n + q 2 e r d Select h c so that h! h d (r, p, q) Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 2012, Chapter 10, Slide 9

10 Longitudinal Guidance Strategy Use altitude state machine from Ch. 6. Closed-loop altitude dynamics: h h c = b ḣ s + b h s 2 + bḣs + b h. Altitude error: Error dynamics: e h 4 = h hd (r, p, q) =h h c e h h c =1 = h h c s 2 s 2 + bḣs + b h Applying FVT: e h,ss =lim s!0 s s 2 s 2 + bḣs + b h h c =0, for h c = H 0 s, H 0 s 2 Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 2012, Chapter 10, Slide 10

11 Lateral Tracking - Vector Field Concept Desired course based on cross-track error: d(e py )= 1 2 tan 1 (k path e py ) 1 e py Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 2012, Chapter 10, Slide 11

12 Vector Field Tuning k path =0.02 k path =0.05 k path =0.1 k path is a positive constant that affects the rate of transition of the desired course k path large à short, abrupt transition k path small à long, gradual transition k path =0.2 k path =0.5 k path = Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 2012, Chapter 10, Slide 12

13 Lyapunov s 2 nd Method Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 2012, Chapter 10, Slide 13

14 Lateral Tracking Stability Analysis Define the Lyapunov function W (e py )= 1 2 e2 py Assume that course controller works and = q + d (e py ) Since Ẇ = e py ė py = V a e py sin 1 2 tan 1 (k path e py ) < 0 for e py 6= 0, then e py! 0 asymptotically Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 2012, Chapter 10, Slide 14

15 Smallest Angle Turn Logic Algorithm 3 Straight-line Following: [h c, χ c ] = followstraightline (r, q, p, χ) Input: Path definition r = (r n, r e, r d ) and q = (q n, q e, q d ), MAV position p= (p n, p e, p d ), course χ,gainsχ, k path,sampleratet s. 1: Compute commanded altitude using equation (10.5). 2: χ q atan2(q e, q n ) 3: while χ q χ < π do 4: χ q χ q + 2π 5: end while 6: while χ q χ > π do 7: χ q χ q 2π 8: end while 9: e py sin χ q (p n r n ) + cos χ q (p e r e ) 10: Compute commanded course angle using equation (10.8). 11: return h c, χ c Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 2012, Chapter 10, Slide 15

16 Orbit definition: P orbit (c,, )= Orbit Following n o r 2 R 3 : r = c + cos ', sin ' 0 >,'2 [0, 2 ) (p n,p e ) ' o d (c n,c e ) P orbit (c,, ) Center: c 2 R 3 Radius: 2 R Direction: = 1 (CW) or = 1(CCW) In polar coordinates, the position of MAV given by: d: radial distance from orbit center ': phase angle of relative position Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 2012, Chapter 10, Slide 16

17 Orbit Following Easiest to analyze in polar coordinates. Using ṗn ṗ e = Vg cos V g sin and converting to polar coordinates gives d cos ' sin ' ṗn = d ' sin ' cos ' ṗ e cos ' sin ' Vg cos = sin ' cos ' V g sin Vg cos( ') = V g sin( ') The result is (c n,c e ) ' o (p n,p e ) d d = V g cos( ') P orbit (c,, ) ' = V g d sin( ') = b + b ( c ) Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 2012, Chapter 10, Slide 17

18 Orbit Following (p n,p e ) ' o d (c n,c e ) Define P orbit (c,, ) o = ' + 2 When d! d o + 2. When d =! d = o Therefore, let the desired course angle be d d(d, )= o + tan 1 k orbit Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 2012, Chapter 10, Slide 18

19 Orbit Tracking Stability Analysis Define the Lyapunov function W = 1 2 (d )2 Assume that course controller works and = d (d, ) Since Ẇ =(d ) d =(d )(V g cos( ')) d = V g (d )sin tan 1 k orbit < 0 for d 6= 0, then d! 0 asymptotically Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 2012, Chapter 10, Slide 19

20 The commanded course is Orbit Following c (t) =' + apple d 2 + tan 1 k orbit The orbit angle must be wrapped: ' = atan2(p e c e,p n c n )+2 m ' (c n,c e ) P orbit (c,, ) o (p n,p e ) d Algorithm 4 Circular Orbit Following: [h c, χ c ] = followorbit (c, ρ, λ, p, χ) Input: Orbit center c = (c n, c e, c d ),radiusρ, and direction λ, MAV position p = (p n, p e, p d ), course χ,gainsk orbit,sampleratet s. 1: h c c d 2: d (p n c n ) 2 + (p e c e ) 2 3: ϕ atan2(p e c e, p n c n ) 4: while ϕ χ < π do 5: ϕ ϕ + 2π 6: end while 7: while ϕ χ > π do 8: ϕ ϕ 2π 9: end while 10: Compute commanded course angle using equation (10.13). 11: return h c, χ c Beard & McLain, Small Unmanned Aircraft, Princeton University Press, 2012, Chapter 10, Slide 20