Formation Flying using GENEX and Differential geometric guidance law

Proceedings of the 2015 IFAC Workshop on Advanced Control Proceedings of the 2015 IFAC Workshop on Advanced Control Proceedings of the 2015 on Advanced and Navigation Aerospace Proceedings of for theAutonomous 2015 IFAC IFAC Workshop Workshop on Vehicles Advanced Control Control and Navigation for Autonomous Aerospace Vehicles Available online at www.sciencedirect.com and Navigation for Autonomous Aerospace Vehicles June 10-12, 2015. Seville, Spain and Navigation for Autonomous Aerospace Vehicles June 10-12, 2015. Seville, Spain June June 10-12, 10-12, 2015. 2015. Seville, Seville, Spain Spain

ScienceDirect

IFAC-PapersOnLine 48-9 (2015) 019–024

Formation Flying using GENEX and Formation Flying using GENEX and Formation Flying using GENEX and Differential geometric guidance law Differential geometric guidance law Differential geometric guidance law S. Mondal ∗∗∗ R. Padhi ∗∗ ∗∗ S. Mondal ∗ R. Padhi ∗∗ S. S. Mondal Mondal R. R. Padhi Padhi ∗∗ ∗ Ph.D student, Indian Institute of Science, Bangalore 560012, India ∗ ∗ Ph.D student, Indian Institute ofScience, Science,Bangalore Bangalore560012, 560012,India India ∗∗ ∗ Ph.D student, Indian Institute Professor, Ph.D student,Indian IndianInstitute Instituteofof of Science, Science, Bangalore Bangalore 560012, 560012, India India ∗∗ ∗∗ Professor, Indian Institute of Science, Bangalore 560012, India ∗∗ Professor, Indian Institute of Science, Bangalore 560012, India Professor, Indian Institute of Science, Bangalore 560012, India Abstract: A new formation flying scheme using Generalized Vector Explicit Guidance Abstract: A formation flying using Vector Guidance Abstract: A new new formation flying scheme using Generalized Generalized Vector Explicit Explicit Guidance (GENEX) and Dynamic inversion (DI)scheme based guidance law for autonomous formation flying Abstract: A new formation flying scheme using Generalized Vector Explicit Guidance (GENEX) and Dynamic inversion (DI) based guidance law for autonomous formation flying (GENEX) and Dynamic inversion (DI) based guidance law for autonomous formation flying of Unmanned Vehicles (UAVs) been proposed thisautonomous paper. In this new scheme, (GENEX) andAerial Dynamic inversion (DI)have based guidance lawinfor formation flying of Aerial Vehicles have been proposed in this paper. In thisare newguided scheme, of Unmanned Aerial Vehicles (UAVs) have been in paper. In theUnmanned entire formation divided(UAVs) into two portions. In the first portion, followers to of Unmanned Aerial is Vehicles (UAVs) have been proposed proposed in this this paper. In this this new new scheme, scheme, the entire formation is divided into two portions. In the first portion, followers are guided to the formation divided into portions. the first followers are to meetentire the leader at a is desired position in space withIn vector of the followers aligned the entire formation is divided into two two portions. Inthe thevelocity first portion, portion, followers are guided guided to meet the leader a desired position in space the velocity vector of the intact followers aligned meet the at a position in with the vector of aligned with that of theat In second portion, thewith followers keep the formation after they meet the leader leader atleader. a desired desired position in space space with the velocity velocity vector of the the followers followers aligned with that of the leader. In second portion, the followers keep the formation intact after they with that of the leader. In second portion, the followers keep the formation intact after met the solution, computational efficiency, implementation with thatleader. of the Closed leader. form In second portion, the followers keep thesimplicity formationinintact after they they met the leader. form computational efficiency, simplicity in implementation met the Closed form solution, computational efficiency, simplicity in and accuracy areClosed some of the solution, key features that are needed for such a mission. In this regard, met the leader. leader. Closed form solution, computational efficiency, simplicity in implementation implementation and accuracy are some of the key features that are needed for such a mission. regard, and accuracy are of features that needed for mission. In this regard, GENEX and dynamic nonlinear guidance areaa found to In be this relevant as and accuracy are some some inversion of the the key keybased features that are are neededscheme for such such mission. In this regard, GENEX and dynamic inversion based nonlinear guidance scheme are found to be relevant as GENEX and dynamic inversion based nonlinear guidance scheme are found to be relevant as reported in thedynamic literature. The guidance commandsguidance (namelyscheme the required thrust, of attack GENEX and inversion based nonlinear are found to angle be relevant as reported in the literature. The guidance commands (namely the required thrust, angle of attack reported in the literature. The guidance commands (namely the required thrust, angle of attack and bankinangle) are generated using the dynamic(namely model of vehicle, thrust, makingangle it practically reported the literature. The guidance commands thearequired of attack and angle) are generated using the dynamic vehicle, making it practically and bank angle) are using the model of vehicle, it morebank relevant. Extensive simulation indicatemodel that of theaaa proposed technique is capable and bank angle) are generated generated usingstudies the dynamic dynamic model of vehicle, making making it practically practically more relevant. Extensive simulation studies indicate that the proposed technique is capable more relevant. studies the technique is of bringing the Extensive UAVs intosimulation desired formation and thenthat maintaining the formation. more relevant. Extensive simulation studies indicate indicate that the proposed proposed technique Simulation is capable capable of bringing the UAVs into desired formation and then maintaining the formation. Simulation of bringing the UAVs into desired formation and then maintaining the formation. Simulation results showthe different leader maneuvers that and are then followed by the leader to maintain a close of bringing UAVs into desired formation maintaining the formation. Simulation results show different leader maneuvers that are followed by the leader to maintain a close results show different leader maneuvers formation throughout trajectory. results show different the leader maneuvers that that are are followed followed by by the the leader leader to to maintain maintain aa close close formation throughout the trajectory. formation throughout the trajectory. formation throughout the trajectory. © 2015, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: Unmanned aerial vehicle, Formation flying, Dynamic inversion, Generalized Vector Keywords: Unmanned Dynamic inversion, inversion, Generalized Vector Vector Keywords: Unmanned aerial aerial vehicle, vehicle, Formation Formation flying, flying, Dynamic Explicit Guidance Keywords: Unmanned aerial vehicle, Formation flying, Dynamic inversion, Generalized Generalized Vector Explicit Guidance Explicit Guidance Explicit Guidance 1. INTRODUCTION be followed by the followers need to be either carefully 1. be followed the need to to be either carefully 1. INTRODUCTION INTRODUCTION be followedorby bydynamically the followers followers need carefully predefined redefined theeither inertial frame 1. INTRODUCTION be followed by the followers need toinbe be either carefully or dynamically redefined in the inertial frame predefined or dynamically redefined in the inertial frame Unmanned aerial vehicles (UAVs) have been a very im- predefined and then accounted for in the design process (see Koo and predefined or dynamically redefined in the inertial frame Unmanned aerial vehicles have been very imthen accounted for design process (see Unmanned aerial vehiclesin(UAVs) (UAVs) have been a very im- and and then(2001) accounted for in in the the Such design process (see Koo Koo and and portant topic of research aerospace engineering, serving Shahruz for example). logics are obviously not Unmanned aerial vehicles (UAVs) have been aa very imand then accounted for in the design process (see Koo and portant topic research in engineering, (2001) for Such are not portant topic of of research in aerospace aerospace engineering, serving Shahruz (2001) for example). example). Such logics logics are obviously obviously not both defense and commercial requirements. Key serving advan- Shahruz generic and maintaining formation for arbitrary dynamic portant topic of research in aerospace engineering, serving Shahruz (2001) for example). Such logics are obviously not both defense and commercial requirements. Key advangeneric and maintaining formation for arbitrary dynamic both defense and commercial requirements. Key advangeneric and maintaining formation for arbitrary dynamic tages defense of usingand UAVs include flying into high-risk areas generic trajectory of the leader as well as online reconfigboth commercial requirements. Key advanandchange maintaining formation for arbitrary dynamic tages of any using UAVs include flying high-risk areas of leader asbewell well as online online reconfigreconfigtages of UAVs include flying into into high high-risk areas trajectory trajectory change of the the as without pilot, longer reconnaissance, maneuveruration of change formation turnleader out toas a challenging (rather tages of using using UAVs include flying into high-risk areas trajectory change of the leader as well as online reconfigwithout any longer reconnaissance, high of formation without any pilot, pilot, longer reconnaissance, high maneuvermaneuveruration of formation turn out out to to be be aaa challenging challenging (rather (rather ability, indoor flying capability etc. Close formation flying uration near-impossible) task.turn without any pilot, longer reconnaissance, high maneuveruration of formation turn out to be challenging (rather ability, indoor flying capability etc. Close Close formation formation flying ability, indoor flying capability reduction etc. near-impossible) task. task. of aircrafts brings a significant induced flying drag, near-impossible) ability, indoor flying capability etc. Close in formation flying near-impossible) task. of aircrafts brings aa significant reduction in In this paper the formation flight is considered to be of aircrafts brings the significant reduction in induced induced drag, thereby extending range of entire formation (Blakedrag, and In of aircrafts brings a significant reduction in induced drag, this the flight is to be be In this ofpaper paper the formation formation flight is considered considered to thereby extending the range of entire formation (Blake and consist two parts. First part is designed for the followers In this paper the formation flight is considered to be thereby extending the range of entire entire formation (Blake and consist Multhopp (1999)).the In range addition, requirement of(Blake formation thereby extending of formation and of two parts. First part is designed for the followers consist of two parts. First part is designed for the followers Multhopp (1999)). In requirement of formation formation to meetofthe a specific coordinate At twoleader parts. at First part is designed for in thespace. followers Multhopp (1999)). In addition, addition, of flying include defense missions requirement such as quick wide-area consist Multhopp (1999)). In addition, requirement of formation to meet the at coordinate in At to meet theofleader leader at aaa specific specific coordinate in space. space. At flying include defense missions such as quick wide-area the instant formation creation the followers will have to meet the leader at specific coordinate in space. At flying include defense missions such as quick wide-area surveillance, damage assessment, flying includebattle defense missions such as radar quick deception, wide-area the instant formation creation the will have the instant of of formation creation the followers followers will while have surveillance, battle damage deception, the attitude and velocity same as that of the leader instant of formation creation the followers will have surveillance, battlesuppression damage assessment, assessment, radar deception, missile jamming, of enemy radar air defence etc. the surveillance, battle damage assessment, radar deception, attitude and velocity same as that of the leader while the attitude and velocity same as that of the leader while missile jamming, suppression of enemy air defence etc. maintaining a desired distance as instructed by the leader. the attitude and velocity same as that of the leader while missile jamming, suppression of enemy enemy air defence defence etc. Civilianjamming, applications of formation flying include forest etc. fire maintaining missile suppression of air desired distance as instructed instructed by the the leader. maintaining distance as by Civilian of formation forest fire Second part aaaisdesired designed for keeping the formation intact. desired distance as instructed by the leader. leader. Civilian applications of search formation flying include forest law fire maintaining fighting, applications crop dusting, andflying rescueinclude operations, Civilian applications of formation flying include forest fire Second part is designed for keeping the formation intact. Second part is designed for keeping the formation intact. fighting, crop dusting, search and rescue operations, law Realization of the first part is achieved by guiding the Second part is designed for keeping the formation intact. fighting, crop dusting, search and rescue operations, law enforcement, sensing fighting, cropremote dusting, searchetc. and rescue operations, law Realization of the part is is achieved achieved by guiding guiding the Realization of different the first first positions part by the enforcement, remote sensing etc. followers from to the desired location Realization of the first part is achieved by guiding the enforcement, remote sensing etc. enforcement, remote sensing etc. from different positions to theform desired location followers from which different positions to the desired location Various example of formation flying scheme can be found followers using GENEX provides a closed solution and followers from different positions to the desired location Various example of formation flying scheme can be found using GENEX which provides a closed form solution and Various example of formation flying scheme can be found using GENEX which provides a closed form solution and in the literature. such as PID feedback the requirement of the first part can be metsolution in optimal Various example ofDifferent formationideas flying scheme can be found using GENEX which provides a closed form and in the Differentlinear ideasdecentralized such as as PIDformation feedback the of the the of firstthe part can be be met in optimal optimal in the literature. literature. Different the requirement of can in (Pachter et al. (1994)), way.requirement The cohesiveness formation is achieved by in the literature. Different ideas ideas such such as PID PID feedback feedback the requirement of the first first part part can be met met in optimal (Pachter et al. (1994)), linear decentralized formation way. The cohesiveness of the formation is achieved by (Pachter et al. (1994)), (1994)), linear etc. decentralized formation way. The cohesiveness of the thea formation formation isguidance achievedlogic by controller et (Wolfe et al. (1996)) have been formation proposed. way. guiding thecohesiveness followers through non-linear is (Pachter al. linear decentralized The of achieved by controller al. (1996)) been proposed. followers through guidance logic controller (Wolfe etare al. used (1996)) etc. have been literature. proposed. guiding guiding the followers through non-linear logic Linearized (Wolfe modelset in etc. mosthave of these (derived the in this paper) based aaaonnon-linear DI (Enns guidance et al. (1994)) controller (Wolfe et al. (1996)) etc. have been proposed. guiding the followers through non-linear guidance logic Linearized models are used in most of these literature. (derived in this paper) based on DI (Enns et al. (1994)) Linearized models are used in most of these literature. (derived in this paper) based on DI (Enns et al. (1994)) These design process are of notthese validliterature. for large (derived that is scalable the intention of the The Linearized models are and usedlogics in most in this based paper)onbased on DI (Enns et leader. al. (1994)) These design and notintermediate valid is based the of leader. The These design process process and logics logics are not valid for for large large that is scalable scalable based on on the intention intention of the the leader. The initial condition perturbations or are large de- that commanded formation is based on a local frame attached These design process and logics are not valid for large that is scalable based on the intention of the leader. The initial condition perturbations or large intermediate decommanded formation is based on a local frame attached initial condition perturbations or large intermediate decommanded formation isleader’s based on on a local localframe) frame as attached parturecondition from formation. Moreover, many formation flyto the leaderformation (i.e. the is velocity found initial perturbations or large intermediate de- commanded based a frame attached parture from many the the frame) asfollower found parture from formation. formation. Moreover, many formation formation flyto the leader leader (i.e. the leader’s leader’s velocity frame) ing algorithms available Moreover, in the literature are based flyon to in nature (e.g.(i.e. formation flyingvelocity of birds). The as parture from formation. Moreover, many formation flyto the leader (i.e. the leader’s velocity frame) as found found ing algorithms available in the literature are based on in nature (e.g. formation flying of birds). The follower ing algorithms available in the literature are based on in nature (e.g. formation flying of birds). The follower simple ‘kinematic’ (i.e. geometric) considerations and do performance is not affected by the dynamical change in the ing algorithms available in the literature are based on in nature (e.g. formation flying of birds). The follower simple ‘kinematic’ (i.e. geometric) considerations do not affected by change simple ‘kinematic’ (i.e. geometric) considerations and do performance performance isand notthe affected by the theisdynamical dynamical change in the not account for the physics (i.e. dynamics) of aand flying leader’s pathis formation maintained even in if the the simple ‘kinematic’ (i.e. geometric) considerations and do performance is not affected by the dynamical change in not account for the physics (i.e. dynamics) of a flying leader’s path and the formation is maintained even if the not account for the physics (i.e. dynamics) of a flying leader’s pathchanging and the the its formation is maintained maintained eventhe if forthe vehicle. This typically results in(i.e. unacceptable leader keeps own trajectory. Note that not account for the physics dynamics)performance of a flying leader’s path and formation is even if the vehicle. This typically results in logics unacceptable performance keeps changing trajectory. Note the forvehicle. ThisInterestingly, typically results in performance leader keeps changing its own trajectory. Note that that the forin practice. some are confined only to leader mulation accounts forits theown intricate ‘dynamics of flight’ of vehicle. This typically results in unacceptable unacceptable performance leader keeps changing its own trajectory. Note that the forin practice. Interestingly, some logics are confined only to mulation accounts for the intricate ‘dynamics of flight’ of in practice. Interestingly, some logics are confined only to mulation accounts for the intricate ‘dynamics of flight’ of a limited class of formation, whichare theconfined trajectories in practice. Interestingly, someinlogics only to mulation accounts for the intricate ‘dynamics of flight’ of aa limited in which which the trajectories trajectories to limited class class of of formation, formation, in a limited class of formation, in which the the trajectories to to Copyright © 2015, 2015 IFAC 19 Hosting by Elsevier Ltd. All rights reserved. 2405-8963 © IFAC (International Federation of Automatic Control) Copyright 2015 IFAC 19 Copyright © 2015 IFAC 19 Peer review© of International Federation of Automatic Copyright ©under 2015 responsibility IFAC 19 Control. 10.1016/j.ifacol.2015.08.053

IFAC ACNAAV 2015 20 June 10-12, 2015. Seville, Spain

S. Mondal et al. / IFAC-PapersOnLine 48-9 (2015) 019–024

the followers, instead of merely relying on the kinematics, making the formulation is practically more relevant. The guidance commands assumed are the desired thrust, angle of attack and bank angle, which are physical quantities. In addition to the fact that the followers need to travel to a position as dictated by the leader in its ‘velocity frame’, it is also assured that by the time a follower reaches its intended position, its velocity vector gets aligned to that of the leader. This unique feature (which is conspicuous in nature, but absent in many formation flying algorithms) helps in maintaining close formation after achieving the desired configuration. Moreover, since dynamic inversion offers closed form solution in general (or very close to it), the solution proposed is computationally quite efficient as well and hence can be implemented in on-board processors. Extensive simulation studies are done to demonstrate the effectiveness of the proposed algorithm, which clearly indicate that the proposed technique is capable of bringing the UAVs into desired formation from arbitrary initial geometry and then maintaining the formation for arbitrary maneuvers of the leader (which need not be decided apriori).

coefficients respectively. Note that the drag and lift forces act on the velocity frame where as the aerodynamic forces are available in the body frame, with the x-axis being pointed backwards Chawla and Padhi (2011). Hence the available forces are transformed into velocity axes by using an appropriate transformation matrix, which is as given below [ ] [ ] [ ] Fax CX −D −S = TV /B Fay = −¯ q STV /B CY (9) −L Faz CZ where Fax , Fay and Faz are the aerodynamic forces obtained in body axis frame of the vehicle, which is obtained from extensive wind-tunnel testing, followed by curvefitting. CX , CY , CZ are the corresponding force coefficients respectively. TV /B is the transformation matrix from body axes to velocity axes with β ≈ 0 (which is true as fixedwing UAV maneuvers are usually done with ‘turn coordination’ (Singh and Padhi (2009)) and is given by [

] cos α 0 sin α 0 1 0 TV /B = (10) − sin α 0 cos α The force coefficients are further expanded as polynomial functions of angle of attack α and combining those with the explicit expression of TV /B from (10), the drag and lift coefficients can finally be written as

2. MATHEMATICAL MODEL OF UAV Even though a detail Six Degree-of-Freedom (DOF) model of UAV is available the formulation presented here is concerned with the outermost guidance loop of flight control system. Hence a compatible point-mass model is adequate Hull (2007), which can be described by the following set of differential equations. x˙ = V cos γ cos ψ

(1)

y˙ = V cos γ sin ψ

(2)

) ( D = q¯S CX0 + CXa1 α + CXa2 α2 cos α

+ q¯S (CZ0 + CZa1 α) sin α ) ( L = −¯ q S CX0 + CXa1 α + CXa2 α2 sin α

+ q¯S (CZ0 + CZa1 α) cos α Numerical values of the coefficients are given in Table 1.

z˙ = V sin γ (3) 1 V˙ t = (T cos α − D − mg sin γ) (4) m 1 ((T sin α + L) cos µ − mg cos γ) (5) γ˙ = mV 1 (T sin α + L) sin µ (6) ψ˙ = mV cos γ where, (x, y, z) are the position co-ordinates (in the inertial frame). V , γ and ψ are the velocity vector components representing the velocity magnitude, flight path angle and the heading angle respectively. α and µ are the angle of attack and bank angle respectively. Forces acting on the vehicle, i.e. T , D and L are the thrust, drag and lift vectors respectively. For more details, one can refer to Hull (2007). It is assumed that the maximum value of thrust that can be produced by the electric motor and propeller assembly is 40 N. It is also assumed that thrust produced has linear relation with the throttle input. The aerodynamic forces, i.e. D and L are computed as D = q¯ S CD

(7)

L = q¯ S CL

(8)

Table 1. Force Coefficients of UAV CX0 0.0386

CXa1 −0.0040376

CXa2 −0.0010525

CZ0 0.1653

CZa1 0.087138

3. GUIDANCE COMMAND FOR FORMATION FLYING The relevant geometric and mathematical formulation for generating guidance commands GENEX and Dynamic Inversion based guidance law is described in this section. 3.1 Generalized Explicit Guidance (GENEX) Generalized Explicit guidance law has been used successfully as a terminal guidance of an interceptor missiles (Ohlmeyer (2006)). The first part of the formation flight can be fulfilled by using GENEX due to its relevant features described in (Ohlmeyer (2006)). A brief overview of the guidance command generation by GENEX is presented. In GENEX linear dynamics consisting of two states are considered. Those states are given as follows X1 = z = yf − ym − y˙ m tgo

2

Here q¯ = (1/2)ρV is the dynamic pressure, where ρ is the atmospheric density (which is available as a function of altitude) and S = 0.6m2 is the aerodynamic surface area of the vehicle. CD and CL are the drag and lift

(11)

(12) X2 = v = y˙ f − y˙ m yf denotes the predicted position of interception in ‘y’ direction. ym denotes the missile current position. The 20

IFAC ACNAAV 2015 June 10-12, 2015. Seville, Spain

S. Mondal et al. / IFAC-PapersOnLine 48-9 (2015) 019–024

The transformation matrix TR is obtained through a series of three euler angle rotations Stevens and Lewis (2003), which is defined as follows )( ( ) ( −1 ) Tγ−1 TR−1 = Tψ−1 Tµl (17) l l

boundary (terminal) conditions are considered as X1 (tgo = 0) = 0 and X2 (tgo = 0) = 0 where, tgo = tf − t is time-togo and tf is final time. The cost function is given by J=

∫0

tgo0

u2 dtgo 2tngo

(13)

The expressions for Tψl , Tγl and Tµl can be found in Padhi et al. (2014). Since the data is now available in a reference frame that is parallel to the inertial frame, it can be easily used to find follower co-ordinates in the groundfixed inertial frame as follows (see Fig.1)

The expression for optimal control for scalar case (considering only y coordinate) is obtained by minimizing (13) and is given as follows 1 u = 2 [K1 z + K2 vtgo ] (14) tgo

[

where, K1 = (n + 2)(n + 3) and K2 = −(n + 1)(n + 2). Similar expressions can be obtained for optimal control considering the x and z coordinates also. Considering the three axis ‘x’, ‘y’ and ‘z’ together the control expressions can be written in vector form as 1 U = 2 [K1 (Rf − Rm − Vm tgo ) + K2 (Vf − Vm )tgo ] tgo T

21

xd yd zd

]

=

[

xl yl zl

]

F + PLI

(18)

which is obtained by shifting the reference axis from the leader to the origin of earth fixed inertial frame. Here [xd yd zd ]T is the desired co-ordinates of the followers in earth fixed inertial frame and [xl yl zl ]T is the co-ordinates of the leader in earth fixed inertial frame. vl

T

where, Rf = [xf yf zf ] , Rm = [xm ym zm ] , Vm = T [x˙ m y˙ m z˙m ] . Detailed derivation about this guidance law can be found in Ohlmeyer (2006).

L

γ

l

ψ l

vf

R

γf F

3.2 Generation of Formation Command for Followers using Dynamic Inversion

ψf

XI

To maintain generic nature of the problem formulation, the commanded values to the followers assumed to be with respect to a reference frame attached to the leader. Since the followers are expected to fly along with the leader, a natural choice is the leader’s velocity frame, which is also compatible with the point-mass model described in (1)(6). The x-axis of this frame is aligned in the direction of velocity vector of the leader, whereas the y and z axes are orthogonal to it and are as shown in Fig.1.

YI

ZI

Fig. 1. Formation flight geometry in the inertial frame The objective here is to keep tracking the [xd yd zd ]T while simultaneously aligning the velocity vector of the follower with respect to that of the leader. In the formulation proposed in this paper time derivative of [xd yd zd ]T is be obtained as [ ] [ ] x˙ d x˙ l d ( F) y˙ d = y˙ l + PLI (19) dt z˙ z˙

The formation commands to a follower are assumed to be given from the leader in its velocity frame in terms of relative separation distance R, as well as two geometrical angles ξ and θ, as shown in Fig.1. Hence the desired position co-ordinates of the follower in the leader’s velocity frame is given by F

PL =

[

−R sin ξ R cos ξ cos θ −R cos ξ sin θ

]

d

(15)

The task here is to compute the desired thrust (Td ), desired angle of attack (αd ) and desired bank angle (µd ) of the follower, so that it not only reaches this desired point, but its velocity vector also gets aligned with that of the leader by the time it reaches this point. The desired position co-ordinates (available in leader’s velocity frame) are first transformed into a ‘local inertial frame’, which is attached to the leader and hence keep moving with the leader, but its orientation is maintained constant and parallel to a true inertial frame fixed on the ground. The follower coordinates in this local inertial frame is obtained as follows F

PLI = TR−1 PLF

l

where, the position derivatives of the leader in the inertial frame can be given as follows Stevens and Lewis (2003)

and

x˙ l = Vl cos γl cos ψl

(20)

y˙ l = Vl cos γl sin ψl

(21)

z˙l = −Vl sin γl

(22)

d ( F ) dTR−1 ( F ) P PL = dt LI dt

(23)

Without loss of generality, the commanded variables R,ξ and θ are assumed to be constant and hence PLF is assumed to be a constant vector. Otherwise, if the command vector

(16) 21

IFAC ACNAAV 2015 22 June 10-12, 2015. Seville, Spain

S. Mondal et al. / IFAC-PapersOnLine 48-9 (2015) 019–024

(1)–(3). Note that V˙ ,γ˙ and ψ˙ as obtained in (29) are the ‘desired values’, which can be considered as ‘pseudo control variables’ as well. Introducing quasi-steady approximation x ¨d = y¨d = z¨d = 0 (which is valid at each grid point of time of control update in discrete-time implementation), from (29), one can write

keeps changing dynamically (which is usually rare), its rate of change should also be accounted for in (23). The dT −1

expression for dtR , which is derived from (17), is omitted here for brevity. 3.3 Guidance Design Philosophy for Followers

 ([ ] [ ]) V˙ d x˙ x˙ d −1  γ˙ d  = −A (K1 + K2 ) y˙ − y˙ d z˙d z˙ ψ˙ d [ ] x − xd − A−1 K1 K2 y − yd z − zd 

As mentioned earlier, one contribution of this paper is the fact that the velocity vector must get aligned with that of the leader (in both magnitude and direction sense) while the position coordinate command is tracked for maintaining the formation i.e. [ ] [ ] [ ] [ ] x xd V Vd y → yd , γ → γd (24) z ψ zd ψd The vectors Z1 and Z2 and the error in those variables are defined as follows [ ] [ ] x V Z1 ≡ y , Z2 ≡ γ (25) z ψ

∆Z1 ≡

[

x − xd y − yd z − zd

]

,

∆Z2 ≡

[

V − Vd γ − γd ψ − ψd

]

(30)

where, V˙ d , γ˙ d , ψ˙ d are the ‘desired values’ of V˙ , γ˙ and ψ˙ respectively. Rearranging (5) and (6), mV γ˙ + mg cos γ = (T sin α + L) cos µ mV cos γ ψ˙ = (T sin α + L) sin µ

(31) (32)

Dividing (32) by (31) −1

µ = tan (26)

(

mV cos γ ψ˙ mV γ˙ + mg cos γ

)

(33)

Substituting γ˙ and ψ˙ by their desired values γ˙ d and ψ˙ d respectively in (33) one gets the desired bank angle µd , which serves as a guidance parameter. Squaring and adding (31) and (32) and then taking the square-root of both sides, √ ( )2 2 T sin α + L = (mV γ˙ + mg cos γ) + mV cos γ ψ˙ (34)

The objective is to drive both ∆Z1 → 0 and ∆Z2 → 0 simultaneously. The total tracking error is defined as ∆Z = ∆Z˙ 1 + K1 ∆Z1 (27) where, K1 needs to be a positive definite gain matrix (a design tuning parameter). Here K1 = diag(k111 , k122 , k133 ) is selected as diagonal matrix with diagonal elements being positive. It can be noted that if ∆Z → 0 asymptotically, both ∆Z˙ 1 → 0 and ∆Z1 → 0 simultaneously as long as γ ϵ (− π2 , π2 ). Proof of this claim can be found in Padhi et al. (2014).

Also from (4)

T cos α − D = mV˙ + mg sin γ

(35)

Substituting V˙ , γ˙ and ψ˙ by their desired values V˙d , γ˙ d and ψ˙ d respectively in (34) and (35), the values for T and α can be solved. These values serve as the desired value of thrust Td and desired value of angle of attack αd respectively, which are the other two guidance parameters. However, equations (34) and (35) are nonlinear and difficult to solve in closed form. Hence, Td and αd are generated by numerically solving (34) and (35) through Newton-Raphson method (Sastry (2005)), resulting in a ‘semi closed form’ solution. The initial guess values of the variables are taken as their corresponding ‘trim values’ Td = 5N and αd = 4.8◦ respectively. Note that the Newton-Raphson method has quadratic convergence (Sastry (2005)) and hence is easy to implement in practice. Note that bounds were also put on the commanded variables to account for the vehicle limitations. Numerical values of these bounds are 0 ≤ Td ≤ 40N for thrust, −5◦ ≤ αd ≤ 15◦ for angle of attack and −30◦ ≤ µd ≤ 30◦ for bank angle.

3.4 Command Generation for Followers With the observation in Section 3.3, the key aim of the guidance loop is to make sure that ∆Z → 0 asymptotically. The following error dynamics is enforced according to DI methodology Enns et al. (1994). ∆Z˙ + K2 ∆Z = 0 (28) where, K2 is a positive definite gain matrix (another design tuning parameter). Here K2 = diag(k211 , k222 , k233 ) is selected as diagonal matrix with diagonal elements being positive. Using the definition of ∆Z and ∆Z1 and carrying out the necessary algebra, (28) can be simplified to   [ ] [ ] V˙ x˙ − x˙ d x ¨d A  γ˙  − y¨d + K1 y˙ − y˙ d z¨d z˙ − z˙d ψ˙ [ ]) ([ ] x − xd x˙ − x˙ d y˙ − y˙ d + K1 y − yd =0 (29) + K2 z − zd z˙ − z˙d

3.5 Command Generation for the Leader It is assumed that the commanded velocity, flight path angle and heading angle, i.e. Vc , γc and ψc respectively are directly available from the type of maneuver that the

(Z2 ) . In (29), [x˙ d y˙ d z˙d ]T is computed from where, A ≡ ∂f∂Z 2 (19), where as [x˙ y˙ z] ˙ T is computed from system dynamics

22

IFAC ACNAAV 2015 June 10-12, 2015. Seville, Spain

S. Mondal et al. / IFAC-PapersOnLine 48-9 (2015) 019–024

23

leader intends to carry out (in other words, these are the commanded values that the leader receives to guide its own trajectory). Based on this information, the desired values V˙ d , γ˙ d and ψ˙ d are generated as per the following first-order autopilot loops V˙ d = − kv (Vd − Vc )

γ˙ d = − kγ (γd − γc ) ψ˙ d = − kψ (ψd − ψc )

(36) (37) (38)

where kv > 0, kγ > 0 and kψ > 0 are the gain values that need to be tuned carefully. Desired thrust Td , desired angle of attack αd and desired bank angle µd respectively, which is done exactly as per the procedure described for the followers in equations (31)-(35) and the discussion there after.

Fig. 3. XY view of formation of two followers and one leader

4. SIMULATION Simulation results have been generated considering one leader and two followers. The initial positions [x y z ]T of the two followers are considered as [20m 200m 100m]T for follower 1 and [20m 600m 150m]T for follower 2 and that of the leader when the followers started following the leader is [0m 400m 400m]T in the inertial reference frame. The desired positions (instructed by the leader in leader velocity frame) defined by the parameters R, ξ, θ that are to be followed is 150m, 450 , 00 for follower 1 and 150m, 1350 , 00 for follower 2. First order autopilot dynamics have been used to get the achieved guidance commands. Fig. 4. Thrust of the followers

Fig. 2. XZ view of formation of two followers and one leader Two different views (XZ and XY ) of the formation are shown in fig. (2) and fig. (3) respectively. It can be observed from these two figures that the followers reached the position dictated by the leader and after that they followed the leader’s manuever. Relative distance between the initial positions of the followers and the desired position of meeting with the leader is shown in fig. (7) and (8) for the followers 1 and 2 respectively. It can be seen from these two figures that after 22.3 sec (during GENEX) relative distances become almost zero i.e. two followers reach the desired position where they were supposed to meet the leader. Hence the objective of GENEX has been achieved. After the followers meet the leader they fly in the formation using the guidance commands generated by DI. The guidance commands i.e.

Fig. 5. Angle of attack of the followers thrust Tm , angle of attack α and bank angle µ are shown in fig. (4), (5) and (6) respectively. It can be observed that the commanded and achieved thrusts are within the maximum limit. It can also be observed that the thrust changes according to the curvature of the trajectory of the leader. The other two guidance parameters i.e. α and µ also change as the followers follows the trajectory defined by the leader. Hence it can be understood that a close and tight formation can be achieved by using the combination of these two guidance law and the results justify the relevance of using these two guidance laws. 23

IFAC ACNAAV 2015 24 June 10-12, 2015. Seville, Spain

S. Mondal et al. / IFAC-PapersOnLine 48-9 (2015) 019–024

quite relevant. The guidance commands are available in semi-closed form and hence the formation geometry can be implemented without much of computational difficulty. Simulation results indicate that the proposed technique is capable of bringing the UAVs into desired formation from arbitrary initial geometry and then maintaining the formation for arbitrary maneuvers of the leader, which need not be decided apriori. REFERENCES Blake, W. and Multhopp, D. (1999). Design, performance and modeling considerations for close formation flight. In Proceedings of the 1999 AIAA Guidance, Navigation and Control Conference, 476–486. Portland, OR. Chawla, C. and Padhi, R. (2011). Neuro-adaptive augmented dynamic inversion based PIGC design for reactive obstacle avoidance of UAVs. In Proceedings of AIAA Guidance, Navigation, and Control Conference. Portland, OR. Enns, D., Bugajski, D., Hendrick, R., and Stein, G. (1994). Dynamic inversion: an evolving methodology for flight control design. International Journal of Control, 59(1), 71–91. Hull, D.G. (2007). Fundamentals of Airplane Flight Mechanics. Springer. Koo, T.J. and Shahruz, S.M. (2001). Formation of a group of unmanned aerial vehicles. In Proceedings of the American Control Conference, 69–74. Arlington, VA. Ohlmeyer, E.J. (2006). Generalized vector explicit guidance. Journal Of Guidance, Control, and Dynamics, 29. Pachter, M., D’Azzo, J.J., and Dargan, J.L. (1994). Automatic formation flight control. Journal of Guidance, Control, and Dynamics,AIAA, 17(6), 1380–1383. Padhi, R., Rakesh, P.R., and Venkataraman, R. (2014). Formation flying with nonlinear partial integrated guidance and control. IEEE Transaction on Aerospace and Electronic Systems, 50, 2847–2859. Sastry, S.S. (2005). Introductory Methods of Numerical Analysis, 4th Edition. Prentice Hall of India. Singh, S.P. and Padhi, R. (2009). Automatic path planning and control design for autonomous landing of UAVs using dynamic inversion. In Proceedings of American Control Conference, 2409–2414. St. Louis, USA. Stevens, B. and Lewis, F. (2003). Aircraft Control and Simulation, 2nd Edition. J.Wiley & Sons. Wolfe, J., Chichka, D., and Speyer, J. (1996). Decentralized controllers for unmanned aerial vehicle formation flight. In Proceedings of AIAA Guidance, Navigation, and Control Conference. San Diego, CA.

Fig. 6. Bank angles of the followers

Fig. 7. Error in GENEX of follower 1

Fig. 8. Error in GENEX of follower 2 5. CONCLUSION A guidance scheme is presented in this paper for autonomous formation flying using GENEX and dynamic inversion based non-linear guidance law. Formation commands are assumed to be generated by the leader in its velocity frame. The followers are then guided to those desired points, while simultaneously making sure about the alignment of their velocity vectors with respect to that of the leader. This is done accurately by GENEX due to its features. Guidance commands obtained from the nonlinear guidance law makes it easier to maintain the formation after achieving it. Unlike many existing literature, the guidance commands are generated using the ‘dynamic model’ of a real vehicle, making the formulation practically 24