NPS: : I. Kaminer, V. Dobrokhodov IST: : A. Pascoal, A. Aguiar, R. Ghabcheloo VPI: : N. Hovakimyan, E. Xargay, C. Cao

Transcription

1 NPS: : I. Kaminer, V. Dobrokhodov IST: : A. Pascoal, A. Aguiar, R. Ghabcheloo VPI: : N. Hovakimyan, E. Xargay, C. Cao The 17th IFAC World Congress July 6-11, 2008, Seoul, Korea

2 Time Critical Applications for Multiple UAVs with spatial constraints Time Coordinated Control of Multiple UAVs for small unit OPS Sequential Autoland Coordinated Reconnaissance synchronized high resolution pictures Coordinated Road Search Introduction Coordinate on the arrival of the leader subject to deconfliction, network and spatial constraints UAV 1 UAV UAV 3 UAV 2 y(m) UAV x(m) 2

3 Introduction An integrated Time solution Coordinated to Control time-critical of Multiple UAVs coordination for small unit OPS problems that includes real-time (RT) path generation accouting for vehicle dynamics plus spatial and temporal coordination constraints nonlinear path following that relies on UAV attitude to follow the given path leaving speed along the path as a degree of freedom 3

4 Introduction Time Coordinated Control of Multiple UAVs for small unit OPS L 1 adaptation to augment off-the-shelf autopilot to enable it to follow the paths it was not designed to follow (a RT generated path is significantly more aggressive than a typical waypoint path these autopilots are designed to follow). Time-critical coordination controlling the speed of each vehicle over time varying faulty networks to provide robustness account for the uncertainties that cannot be addressed in the path generation step 4

5 Introduction System Architecture L 1 adaptation Path Generation desired path Path following (Outer loop) Pitch rate Yaw rate commands Onboard A/P + UAV (Inner loop) Network coordination variables Coordination Velocity command L 1 adaptation 5

6 Time Critical Coordination: RT Path Planning Assume polynomial paths Decouple space and time in problem formulation drastic reduction in the number of optimization parameters (suitable for RT implementation), i.e let c ( τ) : ( τ), ( τ), ( τ) T τ 0; τ f p = x x x where is e.g. virtual arc length a ik x ( τ ) i N = a k = 0 ik k τ 6

7 Time Critical Coordination: RT Path Planning Impact of changing total path length Impact of changing total path length and initial jerk 7

8 Time Critical Coordination: RT Path Planning Deconfliction in space sequentially assigned final conditions y(m) UAV 3 UAV Z(m) UAV 1 UAV UAV UAV 3, x(m) 2000 Y(m) X(m) essentially a 2D solution t f1 dt AM v min v 1 v 2 v 3 v max d min 235s.005s 48s 15 m/s 16m/s 21m/s 30.2m/ s 35m/s 203m 8

9 Time Critical Coordination: RT Path Planning Multiple UAVs simultaneous arrival (deconfliction in time) , D paths t f = 324 sec, AM = 271 sec Speed profiles distances In this case the paths intersect, but if each vehicle follows the nominal speed profile they will maintain a minimum separation distance at all times disturbance rejection is addressed at the coordination level 9

10 UAV Path Following Concept Objective: follow predefined spatial 3D paths paths are time-independent: decoupling between space and time = separation of 3D path and speed speed can be used as an additional DOF for time coordination Limitation: Traditional UAV AP is not designed to follow an aggressive 3D path. Solution: Adaptive augmentation without any modifications to commercial autopilot Conventional solution backstepping requires modification of source code of A/P Global results but cancels inner loop Intuitive Analogy Analogous to Tunnel in the Sky concept familiar to pilots of 3D path and speed profile following Eliminate path following (both distance and attitude) errors using angular rates Follow speed profile to keep the a/c within its dynamic limitations. 10

11 UAV Path Following System Architecture Inner/Outer Loop Solution L 1 adaptive controller Trajectory Generation polynomial path Path following (Outer loop) Pitch rate Yaw rate commands Onboard A/P (Inner loop) Onboard PC104 Boundary conditions User Laptop 11

12 Problem Geometry F: Serret-Frenet frame W: wind frame I: inertial frame p () c l desired trajectory v(t) UAV speed γ flight path angle ψ heading angle τ path length q I = [ xi yi zi ] position of UAV in inertial frame Inertial Frame {I} z I Desired path to follow q I y I y F (N) p c UAV x I Q q F y F P τ v z F (B) z 1 x F (T) Serret Frenet Frame {F} 3D Kinematics Equations dx / dt = v cosγ cosψ I dy / dt = vcosγ sinψ I dz / dt = v sinψ I dγ / dt 1 0 q 1 dψ / dt = 0cos γ r dτ / dt = τ v Input: q r τ v 12

13 UAV Path Following Key idea: use virtual target to determine desired location on the path Minimize the distance from the UAV to the virtual target on the path Reduce the angle between the vehicle velocity vector and local tangent to the path Virtual target s motion extra degree of freedom Path q Q s 1 y 1 {I} : Inertial Frame Virtual Target P {F} : Serret-Frenet Frame 13

14 UAV Path Following (cont.) Control the evolution of the virtual target : added degree of freedom Path Q {I} : Inertial Frame P 14

15 Kinematics Coordinate systems Kinematics equations in I I F γ, ψ φe, θe, ψe W q [ ] F = xf yf zf - difference between q and p c (τ ) in F φe, θe, ψe - Euler angles from F to W Error Equations in F I where dxf / dt = τv(1 κ( τ) yf) + v cosθecosψe dyf / dt = τv( κτ ( ) xf ζ ( τ) zf) + v cosθesinψe Ge : = dzf / dt = ζττ ( ) vyf vsinθe dθe / dt q = Dt (, θe, ψe) + Tt (, θe) dψ e / dt r dτ / dt = τv 15

16 Kinematic Control Law Desired shaping functions Path following control laws & τ = Kx + vcosθ cosψ 1 F e e ( ) 2 e θ uθ = K c 2 θe δθ + zfv + δθ c1 θe δθ ( ) c sinθ sinδ c sinψ sinδ u = K ψ δ y vcosθ + & δ 2 e ψ ψc 3 e ψ F e ψ c1 ψe δψ & 16

17 Kinematic Control Law y = [ q r] T Path following kinematics Exponentially stable Path following controller Semiglobal result: true for all kinematic error states initialized in arbitrary region Ω [ ] T y = q r c c c 17

18 Degradation of Performance Autopilot UAV y Path following kinematics Poor performance y y c y c Path following controller Without adaptation: Errors 50m Poor perf. in windy cond. 18

19 Cascaded System u Gp UAV with Autopilot y( s) = Gp ( s)( u( s) + z( s)) y Ge Path Following Kinematics x & = f ( x) + g( x) y x u Autopilot G p UAV y Conventional solution backstepping requires modification of source code of A/P Global results but cancels inner loop 19

20 Problem Reformulation UAV with autopilot Design objective ( ) qs () = G() s q() s + z() s q c q ( ) rs () = G() s r() s+ z() s r c r qs () Msq () () s rs () Msr () () s c c Autopilot UAV y = [ q r] T Path following kinematics T [ ]? y = q r ad ad ad = y M ( s y ( ) y) c y( s) s c L 1 Adaptive controller [ ] T y = q r c c c Path following controller 20

21 Path Following: Summary Autopilot UAV y = [ q r] T Path following kinematics [ ] T y = q r ad ad ad = y M ( s y ( ) y) c y( s) s c L 1 Adaptive controller [ ] T y = q r c c c Path following controller The cascaded system is UUB The UUB can be reduced via selection of the filter bandwidth and the reference system bandwidth Reducing the UUB leads to reduced robustness L1 guarantees that the region of attraction for kinematic errors does not change 21

22 Hardware-in-the-Loop Simulation CAN Bus 6DOF nonlinear model of the UAV 22

23 Hardware-in-the-Loop Simulation comparison: with and without adaptation Path following w/o L 1 adaptation Path following with L 1 adaptation 23

24 Hardware ( Second Generation) Networked Aircraft Airframe; Sig Rascal meter span, 8 kg 26 cc gas engine 2 3 hour endurance m/s velocity Payload: Cannon G9 12Mp gimbaled Camera PC104 with Wave Relay Mesh card PC104 for gimbal control and AP interface ADL MSMT3SEG PELCO NET 350 video server 24

25 Flight Test Results: Path Following no adaptation effect of gain changes 25

26 Flight Test Results: Path Following L1 ON Wind 5-7 m/s UAV1 Fin.C X North [m] UAV1 I.C Cmd UAV Y East [m] 26

27 Feature Following using Path Following Y - Lat Desired Trajectory Generation X - Long Feature Selection Resulting Mosaic using Overlaid G9 images Google Earth Overlay 27

28 Time-Critical Coordinated Motion Control Target positions, UAVs : initial positions UAVs must arrive at the same time; absolute time is not a priority! 28

29 Time-Critical Coordinated Motion Control l f1 l f2 l f3, Paths parameterized by length (τ=l) l t f f i - length of path for vehicle i - time of arrival (unknown) l () t = l ()/ t l ; l ( t ) = 1, i f i f f f i i i l = 1 l = 1 f f 2 1 l = 1 f 3, 29

30 Coordinated Path Following PATHS (HIGHWAYS TO BE FOLLOWED) Initial configuration Reach (in-line) FORMATION at a desired speed v! d

31 IN-LINE FORMATION Divide to Conquer Approach Each vehicle runs its own PATH FOLLOWING controller to steer itself to the path Vehicles TALK and adjust their SPEEDS in order to COORDINATE themselves (reach formation) Coordination error

32 Coordination state / error Coordination error (in-line formation): l 1 l 2 l = l l l 12 Normalized Path lengths l and l 2 1

33 vehicle link Communication Constraints What is the communications topology? (GRAPH) Bidirectional Links undirected graphs Non-bidirectional links directed graphs R. Murray [2002], B. Francis [2003], A. Jadbabaie [2003]

34 Communication Constraints Communication Delays Temporary Loss of Comms Switching Comms Topology Asynchronous Comms J i - set of vehicles that vehicle i communicates with Links with Networked Control and Estimation Theory

35 V2 Communication Constraints V1 Node Adjacency Matrix A = edge V V1 receives info from neighbours V2 and V3 V2 receives info from neighbour V1 V3 receives info from neighbour V1 Degree Matrix D =

36 V2 V1 Node L= D A= Laplacian V3 edge l 1 l 1 l 2 + l 1 l 3 L l 2 = l 2 l 1 Neighbour set 1={ V2, V3} l 3 l 3 l 1 Neighbour set 2={ V1} Properties: Neighbour set 3={ V1} - L 1= 0 -Graph is connected Communication Constraints ( ) ( ) rankl = n 1 = 2 Ll = 0 l = l = l 1 2 3

37 Switching Communications: brief connectivity losses V2 V2 V1 p 1 V1 V1 p 1 p 2 V3 p 2 V3 p 3 V3 connected p = 1, p = 1, p = disconnected time connected p = 1, p = 0, p = L is a function of p, denoted p = 0, p = 1, p = Lp

38 Switching Communications: uniformly connected in mean V2 V2 V1 p1 V1 V1 V3 V3 p 3 V3 time p = 1, p = 0, p = p = 0, p = 0, p = p = 1, p = 0, p = L p1 L p2 L p3 the union graph over time interval T is connected L + = L + L + L [, tt T] p p p rank L[, tt+ T] = n 1 = 2 L[, tt+ T] 1= 0

39 Time-Critical Coordinated Motion Control Coordination Control Law Remember from Path Following dl / dt = κ x + v cosθ cosψ i 1 Fi i ei. ei. Adjust at the Coordination Level!

40 Time-Critical Coordinated Motion Control Coordination Control Law Speed Asssignment Integral term Communications Topology

41 Time-Critical Coordinated Motion Control Key results (example) Suppose the Graph satisfies 1 T t+ T t xql() τ Qxdτ μ, t 0, x R T T n 1 for some T, μ > 0 (graph does not even have to be connected at any time!) sensible performance of the complete Path Following + Coordination Controllers

42 Time-Critical Coordinated Motion Control A function of normalized path lengths An ISS-like result

43 Time-Critical Coordinated Motion Control (HIL)

44 Conclusion Coordinated Control of Multiple UAVs: Theoretical Framework / Practice L 1 adaptation Path Generation desired path Path following (Outer loop) Pitch rate Yaw rate commands Onboard A/P + UAV (Inner loop) Network coordination variables Coordination Velocity command L 1 adaptation 44

45 NPS: : I. Kaminer, V. Dobrokhodov IST: : A. Pascoal, A. Aguiar, R. Ghabcheloo VPI: : N. Hovakimyan, E. Xargay, C. Cao The 17th IFAC World Congress July 6-11, 2008, Seoul, Korea