A Software-in-the-Loop Simulation Scheme for Position ... - Scielo.br

doi: 10.5028/jatm.v8i4.612

A Software-in-the-Loop Simulation Scheme for Position Formation Flight of Multicopters Davi Ferreira de Castro1, Davi Antônio dos Santos1

Abstract: The cooperative control of small unmanned aerial vehicles such as the multicopters has been extensively investigated worldwide for functionality augmentation and cost reduction with respect to a single larger vehicle. The present paper proposes a software-in-the-loop simulation scheme for performance evaluation and demonstration of formation flight control systems of multicopters. The simulation scheme consists of a computer network where each computer simulates one of the vehicles using the MATLAB/Simulink for implementing the local control system and the X-Plane for simulating the flight dynamics and environment. For cooperation, the local control systems exchange position data by means of the network. In order to illustrate the proposed scheme, a group of 3 octocopters is taken into consideration and a leader-follower strategy is chosen for triangular position formation, with the leader moving in a straight line with constant speed. Keywords: Software-in-the-loop simulation, Formation control, Multicopter, Micro aerial vehicle.

INTRODUCTION Technological advances of the last 2 decades have boosted the use of micro aerial vehicles (MAV) for tasks that can be dangerous, inefficient or unsuitable for human beings. Possible applications of these vehicles include search and rescue (Tomic et al. 2012, environmental monitoring (Koo et al. 2012), buildings inspection (Eschmann et al. 2012), border surveillance (Girard et al. 2004), fire monitoring (Casbeer et al. 2005), powerline inspection (Li et al. 2010), traffic monitoring (Puri et al. 2007) and surveillance in urban areas (Semsch et al. 2009). Recently, we have seen an increasing interest from industry and academia in MAV formation flight (Hao and Bin 2011; Han et al. 2013; Wang and Xin 2013; Zhu et al. 2013.) This is motivated by the belief that a cooperative group of small robots can perform better and with a lower cost than a single large vehicle (Wen 2012). The position formation control of a group of vehicles with communication capability can be understood as the position control of each vehicle in such a way that the relative positions of the vehicles converge to a desired disposal. For evaluating cooperative systems involving MAV, some research groups have built outdoor experimental setups for conducting flight tests (Shim et al. 2005; How et al. 2004; King et al. 2004; Hoffmann et al. 2009). In these cases, the success of the tests are dependent on good weather conditions, availability of a safe flight area (Valenti et al. 2006) as well as of a team of trained people (How et al. 2008). Some other research groups alternatively adopt indoor flight tests relying on pose feedback provided by a stationary camera system (How et al. 2008; Lupashin et al. 2010; Michael et al. 2010; Stirling et al. 2012).

1.Departamento de Ciência e Tecnologia Aeroespacial – Instituto Tecnológico de Aeronáutica – Divisão de Engenharia Mecânica – São José dos Campos/SP – Brazil. Author for correspondence: Davi Ferreira de Castro | Departamento de Ciência e Tecnologia Aeroespacial – Instituto Tecnológico de Aeronáutica – Divisão de Engenharia Mecânica | Praça Marechal Eduardo Gomes, 50 – Vila das Acácias | CEP: 12.228-900 – São José dos Campos/SP – Brazil | Email: [email protected] Received: 02/03/2016 | Accepted: 07/04/2016

J. Aerosp. Technol. Manag., São José dos Campos, Vol.8, No 4, pp.431-440, Oct.-Dec., 2016

432

Castro DF, Santos DA

In order to maximize the probability of success and save is fixed on the vehicle’s structure with its origin at the vehicle’s ∆ time in flight experiments, preliminary tests based on a realistic center of mass (CM). The reference CCS SR = {XR, YR, ZR} is fixed on the ground at point O; its Z-axis is aligned with the simulation environment are often desirable. In particular, the local vertical axis. so-called software-in-the-loop (SIL) simulation is an interesting approach. It attempts to gain realism by combining in a simulation Analytical Modeling scheme 2 or more specilized softwares, each one for modeling a Here we present analytical models for a multicopter. specific part of the system. It can also include the flight computer, Davi Ferreira de Castro and Davi Antˆonio dos Santos These models will be later compared with the corresponding which can be evaluated before flight experiments (Pizetta et al. X-Plane model. The attitude kinematics equation in quaternions 2012). Garcia and Barnes (2010) present a SIL simulation for Davi Ferreira de Antˆdifferential onio dos Santos q isCastro given byand the Davi following one (Wertz 1978): formation flight of electric helicopters using one computer to implement the control laws in a centralized form and others to q˙ = Ωq, (1) simulate the dynamics of the fleet members. The disadvantage of this work, as reported by the authors, is that the processing   T 1 0 q−ω ˙ = Ωq, and communication capabilities of the (centralized) control Ω = where: , Davi Ferreira ω Castro −[ω×]and Davi Antˆonio dos Santos 2 de computer are a bottleneck.     T −ω 0 = 1−ω03 ω The present paper proposes a distributed SIL simulation 2 Ω , (2) −[ω×] [ω×]   ω3 2 0 ω −ω 1 . scheme for testing position formation control systems of Davi Ferreira de Castro and Davi0 Antˆonio dos Santos −ω2  ω1 multicopters. The scheme consists of a network where each 0 −ω3 ω2 q˙ = Ωq, T  3 ∆ ωω=  ˙ 3 and [ω ] ∈1 ℝ. is the SB representation of the computer simulates a vehicle of the fleet using the MATLAB/ [ω×] τ = h + ω × h,1 ω02 ω3−ω −ω ω 0 S and [ω×] is the cross-product 2 1  angular velocity of SB w.r.t. Simulink, for implementing the local control laws, and the 1  0 −ω T R  Ω0 ω, =˙ i.e.: , matrix flight simulator X-Plane, for simulating the flight dynamics 02 ωω× h, I1 τq˙of −[ω×] = + = hΩq,   0 0 I . I = 2 and environment.    3 ω2 0 0 I3 0 −ω The paper is organized in the following manner. The next T 0Davi0 Antˆonio dos 1  Davi Ferreira de Castro Santos 0Iand 1 −ω ω3 −ω1  . Ω =[ω×]  ,0 (3) section presents the multicopter dynamic models. Thus, it is I2 0  . I 2= ω 0 0−[ω×] 1 −ω2 ω1 0  ¨ 0 f v + , r = 0 0 I 3  presented a position formation control method. In sequence, the m 0 −ω ω2  −g 3 ˙ τ = h + ω h, proposed SIL simulation scheme is described. In “Simulation 0 ×  ω31 0 Davi −ω [ω×]de Castro 1  .onio dos Santos Davi Ferreira and Antˆ dynamics 0 0  , is described by the Euler equation: f v2 ,+ =−ω attitude Tests” section, the proposed scheme is illustrated by testing fj¨rThe = km  ωΩq, f2q 1  j ˙ = −g 0 0 I 1 a position formation flight of 3 octocopters. Finally, the last 2 τjτ ==kh˙τI ,× 0 h, I2  0  . (4) =jω + section concludes the paper. 1fj = 0 kf0−ω j2 ,0T I3 , Ω =  ω −[ω×]   2  f I21τ q˙ = ∆ 0 0 T , k  = Ωq, where: =1[ττ1 τ2j τ3] ∈0 ℝ3 is the SB representation of the total  =j τΓf, τ   . ω02 ∆ , I = ¨0 = 0 I2 f−ω v0+ 3  MULTICOPTER MODELING  control r(propeller) h = I ω ∈ ℝ3 is the SB representation   3 torque;   [ω×]  01ωf3 00m I0−ω T−ω −g 1 . 3×3 1 1 1 1 1 1 1 of the vehicle’s =ω√ Γf,momentum; √1 I ∈ ℝ is the vehicle’s inertia √−ω2angular √ Ω= , 0 1   2 τ ω −[ω×] 2 2 2 2 0 2  0 systems vehicle has a symmetrical Consider a multicopter and 2 Cartesian coordinate matrix 1 w.r.t. =iskassumed l ,l that the l Sf0 . It l −l −   ¨r = −√f v +˙ Bj 0√2 f , j  √   √ 2 2 2 ∆ Γ  1,  τ1 =so h that +1 ω−ω ×can h, m 1 1 1 1 1 {X , Y , Z } structure I be written as: (CCSs), as illustrated in Fig. 1. The body CCS SB = 0 ω 3 2 2√   B B B2 √ 2 2 2 √ √ −g , − j0  −l − l  2  0 2   ω3 τl j 2=0kτ l−ω 2 0 21 l . 2l 2 −   02 −[ω×] l l −l f Z  −ω 02 −κ κ  −κ κ2 κ 2 20j2 ,ω−κ 1 0 κ √ √ √ √ f f−κ kf2 fΓ τ  , j =I1 τ τ f f   2 2 2 2 Γf,  I =0  0 I2 0= Y . (5) f   τ l l 0 − l −l − ¯ 2 f f ˙ τ τ = h + ω × h, τ = k  , 2 2 2 τ j =0Γτ¯ f,20j I3 τ f τ κ −κ τ¯ κ −κ κ  −κ κ −κ f   τ X 1 1 1  √ 10  1 √1 1 √1 dynamics model √ Neglecting perturbations, the translational ¯ ¯ 1 0 0 I f f f 1 CM  ¯=  2  2 2  , ¨r = Γ2† f v=+,Γf, = Γ0¯f,  0 fis− Z I− τ τ¯−l l l Law as:  l .0 I =lm obtained Second τ¯0from 2 the0Newton’s   −g Y 2 2 2 √ √2 √ Γ , 0 0 √2I3    2 2  2 ¯ ¯ f1j =¯kf T f2 1 l 1 l 1 0 − j ,† 1 1 √ 1 1  l 0 −l − ff= Γ kf  2, √ , √ √= 2 2 2 j O j 0 ¯ τ   2 2 2 2 1τ Tκj . l 0−κ lκ l −κ κ −κ −κ  0 − κ l τ¯−l = k− (6) X ¨rj = f2v + 2 0 2 ,   2 2 τfj√ √ ¯j= √ kkτ fTjj,, √ m Γ , = ¯ −g 2  2 2 f 2 Figure 1. A multicopter and the Cartesian coordinate systems. l  l = Γ¯f,l 0 − 0  −l − 2 2 = x(k)Θ y(k) =(k), kτ¯τ Tj2. 2 τ¯j2+ f f−κ κ −κ κ κΓf, −κ j = k= f j, −κ κ J. Aerosp. Technol. Manag., São José dos Campos, Vol.8, No 4, pp.431-440, Oct.-Dec., 2016  τ¯ f¯ † f =k Γ2 , , = f¯τ=j+x(k)Θ τ¯ j (k), Y= XΘ E,Γτ ¯f, + y(k)   = ¯ τ 1 1 1 1 1 1 1 √1 √ √ √ ¯   f2j =   kf T j , 2 2  f fT ¯l 0   0 − 2 l −l l l − = Γf, ¯ T †−1

1

A Software-in-the-Loop Simulation Scheme for Position Formation FlightSimulation of Multicopters A Software-in-the-Loop Scheme for Position Formation Flight of Multicopters

A Software-in-the-Loop Simulation Scheme for (1) Position Formation Flight of Multicopters A Software-in-the-Loop Simulation Scheme for(2) Position Formation Flight of Multicopters 1

(3)

(4) A Software-in-the-Loop Simulation Scheme for Position Formation Flight of Multicopters (5) (1) A Software-in-the-Loop Simulation Scheme for(2) Position Formation Flight of Multicopters (6)

7

B

6

5

5

4

4

3

3

R

8

6

1

B

1

2

2

7

8

(7)

(3) (

(8) (4)

( (9)

( (5) ( (

(10) (6)(

(

(7)(1 ( (11) (8) (

B

( (12) (9)(1 (

R

R

(13)

(1 ( (14) ( ( (10)(1 (15)

(1 ((

(16)(11)(1(

(

q˙ = Ωq, (1) q˙ = Ωq, (1)   1  0 −ω T  T Ω = , onio dos Santos (2) 1 0and −ω Ferreira deΩCastro Davi Antˆ = 2 ω −[ω×] , (2) 1 ω −[ω×] A Software-in-the-Loop Simulation Scheme for Position Formation Flight of Multicopters 2  433 −ω3 ω2   0 0 −ω ω   21 . 0 3 −ω [ω×]  ω3 (3)  ω3 2 ω 01 −ω [ω×]   −ω (3) 01 . ∆ −ω q˙ 2= Ωq, (1) where: rω=1 [r1 r20 r3]T ∈ ℝ3 is the position of the CM w.r.t. (a) (4) (b) τ =∆h˙ + ω × h, T 3 v =h˙[v+1 ω v2 ×v3h, ] ∈ℝ is the unit vector perpendicular to the de Castro SτR; = (4)and Davi Antˆonio dos Santos Davi Ferreira T 1 Ferreira 0 −ωde Davi and DaviofAntˆ Santos fisCastro the magnitude theonio totaldos thrust; g is the Ωplane = Iof rotors; (2) 0 0  , 1 ω −[ω×] 2 gravity acceleration; m is the vehicle’s mass. 0 0 I   1 (5) I = 0 I2 0 .  00 denote I02−ω3I03 .ωthrust (5) I =Now the and the reaction torque produced 2 03j-th0  respectively, ω 0 I3 by, −ω …, n, Ferreira being n de Castro bythe rotor f and τj, j = 1, Davi [ω×]  (3) and Davi Antˆonio dos Santos 1 . q˙ = Ωq, (1) 0  0 q˙ = Ωq, j  −ω ω 1number 2 10rotors. The usual aerodynamic models for the the of Figure ¨r = 1 f v +  0  , (6)2. (a) Gyrofly Gyro-200ED-X8 octocopter; (b) X-Plane mf vare:     ¨r =rotors 0 , +  −g (6) model. T ˙ 1 (4)0 −ω T τm= h + ω × h, Ω = 1 0 −ω , (2) , −g Ω = 2 ω −[ω×] 2 ω −[ω×] fj = kf 2j2 ,   (7) (7)  Based Theory, the X-Plane reads   Ωq,Blade Elements fj I= (7)q˙ = on 1 kf0j , 0 0 −ω3 ω2 0 −ω3shape ω2of the aircraft modeled in Plane-Maker 2   the geometric   τ , (8) = k  0 0 I (5) I= j ω3 0 −ω1 . (3)  τ 2 2j[ω×] . ω3  0 −ω1 . [ω×]   (8) τj = (8) −ω2 ω1 0 0 kτ0j ,I3 T and −ω divides its surfaces 1 0 −ω ω 0 , into several small elements. Then, it 2 1    Ω=  simulates the forces ω −[ω×]  f  2 0 τ = h˙ + ω × h, (4) acting on each element. These forces are (9)˙ 1 fτ =Γf,  τ = h + ω × h,   that, when integrated over time, is 0the thrust coefficient; kτ is the reaction torque converted (9) ¨r = where: f v +=kfΓf, , (6) into accelerations   0 −ω3 ω2 mτ – −g th  0 0 of the rotation coefficient; ω3velocities 0  and −ωtranslation. 1 1 1 1 ωj is1the√ 1 I11 speed 1   j rotor. [ω×]result   in 1 . √ √ √ ∆ 0 0 I . (5) I = T 0 0 I 2 1 1 1 1 1 1 1 1 1 Define the2vector of individual thrusts The X-Plane dynamic model is generated numerically as  by f =[f1 f2 ...fn] . We −ω ω 0 2 2 2 √ √ √ √2 2 1 0 I3   0 I2 0  . I = (7) 0 − 2 l −l fj −= k2f lj 0, 2  2 l 0f lof the  can relate the magnitude total thrust and the resultant described above. In other words, it does not give the equations 2 2 2 2 0 − √ l −l − √ l 20 √ l l  √  , (10) 0 h˙ + 0 ω I×3 h, ∆ 02  2 2 2 2 2 torque  τ = 2 T2 3 √ √ √ √ τj = k  , (8) , (10) τ 1 [τ1 jτl2 τ3] 2∈l ℝ0 with f by:  τ= of motion neither ofsome physical parameters such as the inertia 0 −l − 2  ¨r =2 f v + − 022 l , (6) 22 l 0 − −g l  0rotor l 0 −l − 2 m matrix and the  coefficients. Therefore, for designing of  1 κ −κ κ  −κ κ −κ κ −κ 2 2 2 2 ¨r = f vI1+ 0 0 0 , κ −κ κ  f −κ κ −κ κ −κ control first to identify some parameters of the  = Γf, m  laws, we need (9) 0 I2−g 0(7) . I = (9) fj = kf j2 ,  fτ¯  ¯f, = Γ (11) ¯ X-Plane model. fτ¯ 3 = Γ¯f, (11) 0 02 I(8)  τj = kτ j2 , τ¯ 1 fj = kf j ,  1 1 1 1 1 1 1  √ √ √ f¯ √ 0 the octocopter †  2for  2 2 2 ¯f = 1   where, configuration taken into account Parameter Estimation 2 Γ , (12) ¯  τj =f vkτ+j , 0  , ¨r =(12) lfl l −l 0 − ¯ = Γ−† 2 fτl¯ 0,  2 2 m inputs of (9) √ √ τ¯ paper√2(seeτFig. = √ inf the present 1),Γf, The the analytical modeling (Eqs. 1 to 6) are the (10) , −g 2  2 2 2 ¯   f of the total thrust and the total torque τ produced (13) l  l 0 − 0 f¯ −l − j = kf T j , l magnitude  f 21 2 (13) fj =12kf Tj ,1 1 12 √1 1 fpropellers. kfΓf, j2 ,On the other hand, the inputs of the X-Plane √ √1 √κ −κ j == κ −κ κ τ¯ −κ κ −κ by the τ = k T . (14)  j τ j 2 2 2 2  l −l − l 0 l l τ¯j (14)are rotor throttles =0kτ T−j .√ model T ∈ [0, 1], j = 1,…, 8. Therefore, before 2 2  τj = kτ j2(10) , √2 √2 √ Γ  f¯  , (10)1 ¯ 1 1 1 1 √1 j 1 1  2 2 2  = Γf,2 (11) √ √ estimating the rotor coefficients√kf and kτ, and the moments l l 0 − l  −l −+ (k), 0 y(k) =τ¯x(k)Θ (15) 2  2 2 2 2 2 2 2    commands of f and  0 − l need l l of −linertia −f I ,l I , 0and I , we (k), κ y(k) = x(k)Θ (15) to convert −κ −κ κ −κ κ −κ κ  ¯+   1 2 3 2 2 2 2 √ √ √ √ – = Γf, – Γ  ,  ¯f = Γ† f ,   (12)  and f by f , τ–, and f ,  2 τ into Tτj. 2Denote the 2commands of2 f, τ, ¯ f ¯ τ   l l 0 − l 0 −l − ¯ Y = XΘ∆+ E, (16) = Γf, (11) 2 2 2 respectively. τ¯ arm’s length.   / k and l the κ =+k E, 9, one can write: Y and = XΘ (16) 2 1 From1 Eq. 1 1 κ 1 −κ 1 κ 1 −κ√ κ 1(13)−κ√ κ −κ f¯j = kf Tτj , f   √ √ ¯  2    2 (12) 2  = (XT X)−1 XT Y. ¯f = Γ† f ,   0 − 2 l −l (17) Θ l l l¯ 0 f¯ − τ¯ = k T . (14) τ ¯ T T −1   X-Plane Modeling j τ j  2 2 =2 Γf, (17) √ Θ = (X X) X Y. (11) √ √2 √ Γ , ¯ τ   2 2 2 2 ¯ In this study, the multicopter its flight environment (13) fj = kf Tdynamics, j,   −l − l l 0 − l 0 2  2 2 2 f¯ κ −κ (gravity, wind, as a graphic visualization κ of the −κ y(k) = x(k)Θ + etc.), (k), asτ¯well (15) ¯f κ= κ −κ j = kτ T j . Γ† −κ (14) , By Eq. 11, we can obtain the individual thrust mission, are modeled in Plane-Maker, the design tool associated  inverting  τ¯ – ¯ f – ¯ ¯ commands from f and τ ,by: with the flight simulator X-Plane. For illustration, we chose the = Γ f, = k T , f j ¯ f j (15) Y = XΘ + E, y(k) = x(k)Θ + (k), (16) τ Gyrofly Gyro-200ED-X8 octocopter (Fig. 2a). The corresponding τ¯j = kτTj ¯.  f X-Plane model is depicted in Fig. 2b. ¯ T T −1 (16) f(17) = Γ† ,  = (X X) X Y. Y = XΘ + E, (12) Θ τ¯ The X-Plane was chosen due to the following characteristics: y(k) = fx(k)Θ ¯j = kf+  = (XTAviation Tj(k), , it is certified by the Θ Federal (FAA); it is X)−1 XTAdministration Y. (17)

ormation Flight of Multicopters

1

A Software-in-the-Loop Simulation Scheme for oftware-in-the-Loop Simulation Scheme for Position Formation Flight of Multicopters osition Formation Flight of Multicopters A Software-in-the-Loop Simulation Scheme for Position Formation Flight of Multicopters

able to simulate wind and turbulence; it permits to design customized models by means of the Plane-Marker module; it allows simultaneous visualization of multiple vehicles on the same monitor; and, finally, it can import and export data along the simulation.

†∆

T –1 T where: of Γ. Tj(Γ . Γ) Γ is the pseudo-inverse τ¯j = Γkτ = – – YThe = XΘ + E, next step is to convert from f j and τ j to Tj. For this end, a preliminary test was made, which showed a linear relation y(k) =Tx(k)Θ T (k), −1 + between Y. Θ = (X X) these X quantities, i.e.:

1

(1) (2)

(1) (3)

(2) (4)

(3) (5)

(4)

(6) (5) (7)

(8)(6)

(9)(7)

(8)

(10)(9)

(11) (10) (12)

(11) (13)

(14) (12)

(15) (13)

(14) (16)

(15) (17)

Y = XΘ + E,

(16)

 = (XT X)−1 XT Y. Θ

(17)

J. Aerosp. Technol. Manag., São José dos Campos, Vol.8, No 4, pp.431-440, Oct.-Dec., 2016

 −g 0 −ω 2  ω√ 22  22 1 . 2[ω×] 32 √ √ √ , − 2 0 −ω 2 ω l  02 l 0 − 22 l  ˙ 1 q = Ωq, 2 2 2 l  l 2 l 0 − 0 − f = k  , −κ2 κ −κ κ −κ κ −κ 2 2j f j2 −κ κτ = −κ κ h, −κ  κ −κ h˙ + ω × T 2 ¯ 0 kτ  −ω, f 1τj =  Ω f= , ¯ = Γ¯f, j τ¯ 2 =ωΓ¯f,−[ω×]   CastroDF, Santos DA  434τ¯I1  0  0 3. ω2 I¯f==Γ0† 0fIf¯2 −ω ,0 Γf, ¯ = τ f †  ¯ τ ¯f =Γ0 ω30 I,30 −ω1  . [ω×] ¯ τ   −ω  ω1  0 (13) 1 1 f¯j = 1 kf T2 j√ 1, 0 1 √1 1 1 √ √ f¯1j f=v k+f T   j , 20  ,  2 2 2 ¨ r = ˙T+. ωl× h,   0 − τ==k h− l l 0 l τ¯m−l j   2 2−g 2 τ¯jj = kττ T√ √ √2 √ Γ , (14) j.   2 2 2 2    −l − l l 0 − l 0 I 20 2 2 2 =fx(k)Θ , 0 j = k1f  y(k) +2j(k),  . −κ κ  0 −κ κ −κ κ −κ I I = 2 y(k) = (k),0κ the In x(k)Θ order to+estimate parameters kf and kτ, we adopt the 2  k0τ j0, I3 τj = f¯ scheme SIL simulation  will be presented in “Software-in= Γ¯f, that Y = XΘ + E, ¯+ E, 0 section. The rotors are assumed to be τ   the-Loop Simulation” Y =fXΘ 1  0  , ¨r = f v=+Γf, identical and †theTidentifi cation is made only for the first rotor. f¯ m τ T  = (X ¯f X) = −1 Γ X Y.−g , Θ ¯ T Tτ −1  Th e vehicle is commanded to hover andan additive random Θ = (X X) X Y.  1 1 1 1 1 2 √1 1 1 √ √ kkfexcite  , the X-Plane√model through the throttle signal is fused =to fj¯j= f–Tjj , 2  2  2 2   0 − l l 0f 12and τ–l 1 denote l input −l T− (k). Let the measurements of f1 and  1 τ , = k  2 2 2 2 √  √ √ τj¯j = kτ τ Tjj .√ ,  measurement model:  2  2 the following 2 We adopt 2 τ1, respectively.  −l − l  l 0 − 0 2  l 2 2 2 f κ −κ κy(k) −κ −κ =κΓf, = x(k)Θ + −κ (k), κ (15) τ   ¯ f ¯   – 1 – 1 T 1 2 1 1 1 1τ¯Y ==√ 1Γ∆f,+ XΘ E, τ √(k)] ∈ ℝ is√ √ the output measurement where: y(k) = [ f 1(k) 1  2  2 2  ∆2  measurement;  0 − vector; ¯ l ¯ −l x(k) † − f= l[T 10(k)] ∈l ℝ lis the input   2 2 2 2 f = Γ , √ √ √ √ ∆  ,∆ T T  =  = [є1(k) є2(k)]T  2 є (k) 2 2 −1 X) XTparameter Y. 2 [k kτ](X ∈τ¯ℝ is the vector; Ѳ2 = Θ f   −l − l l 0 − l 0 2 2 2 vector. 2j , 2∈ ℝ f¯is thekfmodeling error = T j κ −κ κ −κ κ −κ κ −κ Applying the data set took at instants k = 1, 2, …, N in τ¯j = k¯τ Tj . Eq. 15, onefobtains = Γ¯af,set of linear equations with unknown Ѳ. τ¯ These equations can be compactly represented by the following  y(k) = x(k)Θ+ f(k), ¯ matrix¯equation: f = Γ† , τ¯ (16) Y = fXΘ ¯ =+kE,T ,

(3) (10)

−l −l κ κ

j

f

∆

T

(4) (8) (11) (2) (11) (5) (9) (12) (12) (3) Table 1. Estimation of the parameters kf , kτ, I1, I2, and I3. (13)

(13) kf(6) (4) (14) (14) (10)

28.0269 N(7)

T

T T

and E = [є(1) є(2) ... є(N) ] y(k) = x(k)Θ + (k), Using the measurement model (Eq. 16) and the data set represented in X and Y, one can immediately obtain a least= XΘ + E,Ѳ by (Ljung 1987): ˆ of squaresYestimate Ѳ  = (XT X)−1 XT Y. Θ

(17)

A similar procedure is adopted for estimating the moments of inertia I1, I2, and I3 from tests with the SIL simulation scheme. In this case, each rotation DOF is consider separately, resulting in the estimation of the corresponding moment of inertia. The vehicle is initially commanded to hover at a point and a random signal is added to the attitude command about the considered axis. The attitude command is sampled to obtain the input measurements X, while the true attitude computed by the X-Plane gives the output measurements Y. Finally, on the basis of a measurement model similar to Eq. 16, a least-squares estimate of the moment of inertia is obtained using Eq. 17. Table 1 presents the obtained estimates.

kτ

I1

I2

I3

0.3869 Nm

0.0628 kg m2

0.0762 kg m2

0.1014 kg m2

(15) (15) (5) (8) (16) (11) A POSITION FORMATION (16) (6) (9) CONTROL METHOD (17) (12) PreliMinAry deFinitions (17) (7) This section presents a position formation flight control (13) method based on the leader-follower strategy (Santos 2013). To (8) (10)(14) begin with, consider a team of N multicopters and the CCSs of Fig. 3. The body’s CCSs are denoted by SiB = {XiB, YiB, ZiB}, i ∈ ∆ (9)N} The ith body CCS Si is assumed to be attached .... I = {1, 2, (15) B to the(11) structure of the i-th multicopter and centered at its center i (16) . The reference CCS is denoted by SR = {XR, YR, ZR}. of mass CM It is assumed to be fixed on the ground at point O. (12)(10) (17) f1 ZB 1

f2 ZB 2

(13) (14) (11)

XB

YB2

YB1

2

1

(15) (12) r (16)(13) Z Y R

XB

f3 ZB 3

2

j

τ¯j =∆−1kτ TTjT.  = X Y.y(2)T ... y(N)T]T , X ∆= [x(1)T x(2)T ... x(N)]T, Θ (XT X) where: Y = [y(1)

(1) (7)

YB3

r1

R

XB3

r3

(14) (17) X

O

R

Figure 3. A team of N multirotor vehicles and the Cartesian (15) coordinate systems.

(16) that S is an inertial frame and neglect any Assume R disturbance force. The direct application of the Second Newton’s Law gives(17) the following translational dynamic models represented in SR.

  0 1 ¨ri = i f i vi +  0  , ∀i ∈ I, m −g

(18)

fi  f i vi , ∀i ∈ I. ∆ where: ri = [r1i r2i r3i ]T ∈ ℝ3 denotes the three-dimensional i i −1 3i ; v ∈ ℝv is the unit vector normal to the rotor position ofφiCM  cos 3 , ∀i ∈ I. th plane of the i multicopter; mi is the mass of the ith multicopter; of the total thrust produced by the rotors fi is the magnitude ˜ri  ri − rid , ∀i ∈ I. th of the i multicopter. Define thetotal 1thrust vectorf i ∈ ℝ3 of the ith vehicle by: F  rr , ρ , ρ2 , ..., ρN ,

J. Aerosp. Technol. Manag., São José dos Campos, Vol.8, No 4, pp.431-440, Oct.-Dec., 2016

  , Fd  rrd , ρ1d , ρ2d , ..., ρN d  i  fmin ,

i α3i < fmin

¨ri =



 0 1 ¨ri = i f i vi +  0  , ∀i ∈ I,   m −g 0 1 ¨ri = i i f i vi +  0  , ∀i ∈ I, m f  f i vi , ∀i ∈ −gI.

, ∀i ∈ I. 1f i fi v  0  , ∀i ∈ I, f v + i m 2 −g φi  cos−1 v3i , ∀i ∈ I. f2i  f i vi , ∀i ∈ I.

i ˜ri  ri −forrPosition I. Flight of Multicopters A Software-in-the-Loop Simulation Scheme Formation d , ∀i ∈ (18)φi  cos−1 v3i , ∀i ∈ I.

435

  F  rr , ρ1 , ρ2 , ..., ρN , (18) i  ri − rid , ∀i i∈ I. (19) (19)f˜ri. Assume that m , g, and v 3i ≠ 0are exactly known. magnitude 2   N The altitude lawρis rrd , ρ1dcontrol , ρ2d , ..., Fd feedback   , by (Santos et al. 2013): d given i i i 1 2 N f vi , ∀i ∈ I. f −1 F(19)  rr , ρ , ρ , ..., ρ , costhe inclination v3 , ∀i ∈ I.angle ϕi of vi as: (20) φi and  i 2 i i   α3 < fmin  fmin , i −1 i 0 i i1 i i 1i ii cos (20) α , α ∈ ] , f = v , ∀i ∈ I. (20) i iφ i i 3 (24)  3 rrd , ρ3d , ρ2d[f, min ..., ,ρfN F  , ∀i ∈ I, d(18) dmax, ¨r =˜r i fr v− + rd , ∀i0∈ I. (21)  i i i   α3 > fmax fmax , m −g 0  i i i i1 ii i ii i f I. i of, ∀i   ¨ri =˜ m 0 f v + ∈ I, (18)  r − r , ∀i ∈ (21) rNote min ,  i α3 i< fmin that ϕ consists the angle between v and Z . 1 2 N i d ir i, ρ ,iρi , ..., ρ R F m r , (22) i g − k r − ki 2 r˙3i] .,  − r α i i i 1 α3 ∈ [f I. (19) f i f∆ v i, ∀ii ∈ 3 d,3 i α , , f = f i T 3 2 3 min max r d,3 ] ∈ ℝ3 denote the position command withv Let r d = [r d,1 r d,2−g 3 i i  , α3i> fmax fmax    1 2 N  0 ir , 1ρvehicle for the fri-th and define the corresponding tracking i2ρi , ..., i i , ρ F  , (22) φimax , ∀iρ∈N I. (19) ii α 12 ,1 , ρ−1 ,fρdv,i ,..., (23) Fd φi r– i i α rd  12 0≤sin i d I.,  ∀i ∈ (20)   ¨ r = v + , ∀i i cosd3 v if  m 3 v = i i i ii∈ I, , α error r ∈ ℝ by:  ik 12 α  i gm i 12 . − r − r − k r ˙ (25)  sin φ , α  > sin φ 1 2 3 3 d,3 3 i i max max 12 −g α12   v3  i  0 1 2 N i i  i,1rrdi cos ,i3ρ< ρ∈d I., (23) dfmin α−1 f∀i F i, ρid vd3i, ,...,  i i i   i(20)  m (21) ¨ri = 0min v r+ (18) ˜riφ  rf − , ∀i I.i , ∀i ∈ I, (21) i i ∈ I. i i i ∈ i0, ∀i i − f rvα , fmax ] , f i = α3i ,mi α3i ∈d[fmin (24) ˙ 12φmax . α12  − i 1αk123,f ri12 12 +≤ksin 4r d,12 i −g i  i i , magnitude v12 =The¨riabove i fi i  respects i thrust control the ,i , i α3i α> fmax 0 i12 ,> =fφimax f i vαii12+law ∀i ∈total I, fmin fmin  max sin , α sin φ i3 < i max   α12  m ˜r i i r1 − (21)   i N I.i ii 2rid , ∀i i ∈ ii −g i α r,fr ,ρ α,f3ρv∈, ,..., [f fmax (24) f =iF  , ]be ,described by the set: −1 ≤ if i ≤cos fvmax andi is such that, if f i ≤ α3i ≤ f max , constraint (22) f i (19) position formation ∀i min ρ∈i,I. m  Leti 3the 12 v3 , ∀i ∈ I.  min i min i φ i i   . = v m i > f k1, r3i − αrd,3 − k . (25) r ˙ α3i  i g f−max ialtitude command r i i i2 i rv i the vehicle altitude 2 3 max 3 responds to the then 1 − r ∈ I.+ k4 r˙ 12 . α12  − i fik   d,3 3 r i 3i− 12 v3 N   , f(22) f12v , ∀id,12 (22) ρ1−1 , 2ρ2i, ...,ρ N iF i rr ,1  as if it were governed by a second-order linear time-invariant i r , ρ , ρ , ..., ρ , (23)  i Fm i i d  rd  cos v , ∀i ∈ I. (20) φ i i i di 3 d r − rd , ∀i ∈ I. i g − k1 dr − − kφ2 imax (25) ˜rr1  r˙3 . α3  α α ≤ sin 3 12r d,3 = rkvrdiare , design parameters and have effect model. The gains k1dand v3i 12 ,  i i 212i i −1 , (26) vi12 =   α  i i i . = v 12  cos v φ 1 2 N 3i12 , ..., sin φ α >i sin i i rrd qderivative 3 ,i ∀i 2∈ I.   ρd φasmax Fdwhere: , the (23)  1 − v of proportional and respectively. , r,r ,∈ fmin α max dα,3ρ d defined rqd = r(1) + ρ 3,actions, ..., N, iρℝ iis< 122 iri12 i fmin i reference position for the 1 2, d, q = ˜  r − r , ∀i ∈ I. (21) , α  ≤ sin φ α i F  rr , ρ , ρ , ..., ρN , d i12 i max 3[f α3i12, αρi α∈3i ∈ , f ] , (24)   i ℝ , ∀ ∈ I, are the actual relative positions of max min , (26) vi12i =f = formation; m  φi i i i i12i , i iαiii i >i sin i , φ.imax ˙ i12 ,r1212 >312f< fmax ˜ri rr1di = − (27) Position kmax +max α12  −sin − rrd , ∀iT ∈ I. ,w.r.t. fkthe α  rαr 3fmin 4r 3.α d,12 min  i Horizontal Using same notation, one can represent the vehicles r (t) = [λtControl 0d 1h] 2  f rd r 1 2 N i i i i i ρ , (22) , f ] f =i Fi α3 ,rr, ρα3, ρ∈ ,[f..., , (24) ∆ ∆ ρN q q (1)   r , ρ F i i i T 2, ρdi, ..., 1 max min d rd dT d i v,i ]T ∈ ℝ2, and r i    r = r + ρ , q = 2, 3, ..., N, m = [r r ] ∈ ℝ , v = [v Let r the corresponding desired formation by: m ρ (t) = [0 0 0]    i d d12 i i 12 , 1 2 d,12 ir i − kk24rr˙3ii12 . . (25) −rαrid,12 αi12 > f−  i−i ig f−max ki 3k1,rvi12 (27) 1 2 d 1 2 +max α N T 3  d,3 3 2 ρ2 (t) i i i 123 , [−η ρ , ρ−, ..., ρ F rrhorizontal , = γ 0] ∈ ℝ denote the projections of r , v , and r ,  . (28) v v= 3 f   d d i1 22 i iT i N 1 − v (23) , α < f f T 3     r , ρ , ρ , ..., ρ , (23) Fdi  12 i m 3 i min min rd  di id i  d ρ (t) = [−η γ 0] i r (t) = [λt 0 h] > 0 respectively. Consider the maximum allowable value ϕ rd d i i i i i r3α −12rd,3≤ − ˙3 . α3  i iα12g, − k1 v12 max sinkφ2 rmax ]i, f i1 =(25) α3i , 1 αi23i T∈ [fmin , fmax i i N . (28) = v v 3 = ρF (t) [0 0, ρ 0] , (26) vi12 = for the inclination ϕ . Assume m and f ≠ 0 are 3 r , ρ , ..., , iangle i ρd i that i α 2 d d rd i r1 i i 12 d d  = r , (29) fmax , i α3 > Tifmax − , i  > isin φ d i i 1, rd sin φ , v α 12   maxf < fmin maxi 2 α  3 α12 3the min ρ (t) = [−η − γ 0] I (t)  r (t) + ρ (t) − r (t). i 12 i desired a rd ∈ ℝ is relative reference position exactly known. The horizontal-position feedback control law where: r d d α12i  ≤ isin φ q q i ,,rd αi ∈ (1) αα i  fi = , (24)  qi = 2, ..., N,, fmax] max (30) 3 m i ρ12 3,1 α 3 i 3, [f min i+ i = [−η  iiγ 0]T i  i=1 d = r m d , (26) vi12ir= i i i ρ (t)  , α < f f i i  12 d (29) ∈i ℝ, ∈ iI, are of the formation; is givenαby (Santos al.k2013): ird = i> rd minget− min − k2 r˙3 . 1 r33 − rd,3 , ρ,iαα f∀max φ the desired positions of 3 (27) ,i12r− 12+ kf3max ki 4 r˙sin ri12 α12  −sini φmax 3 > α  rdd,12 i i vi i 12 .max , fmax ] , α3i ∈ [fmin fi =  3 3α3 , q thefvehicles q . w.r.t. r (1) T2, 3, ..., N,  rd r = r + ρ , q = (30) i i i i    i [λt i i   d (t) m d 0 h] ri rd =  , α > f f m i r (t). i i ii i  all Ia (t)  (25) rrdi(t) + ρ3d (t) max α−max i∈ g k− k iI,i rTlet rd,3 ∈ ˙3ithe. maximum allowable αi −For 12 , 12  ≤ sin φmax 2 r˙ ri12 −− rϕmax (27) α +−ℝ kk4be α12 i= i i  12 3 d,12 i vf i ρ1d3(t) [03 0rv112 0] , = v i=1 i α 3 i i i . (28) = v (26) 12 i i i 12     msin φmax i i , iα12  > sin T2angle of the 1inclination T , ϕ and f min ∈ ℝ and f max ∈ ℝ be, i φmax i (31) −− v rvalue 0γi12 h]0] αr12 − r − k . g − k  r ˙ α rd (t)= = ρ2d (t) 1 2  i[−η[λt d,3 3 3 3 i i i Tα v3 , [0 minimum  ≤ sin φimax α12 the 1 respectively, allowable values  120]T 12and the maximum 0v0] ρ3d ρ (t) α  d (t) i γ mi   i .,i (28) =i =1[−η vi = , (26) vi12 = (31) i i i  i i i 2 12 T r = r , (29) 2the ˙ 1 − v  r − r  − k r + k α rd . The position formation control of total thrust magnitudes f sin φ , α  > sin φ i i i d 3 4 i 12α d,12 12 12 . ρd3(t) max = α [−η − γ12 12 0] max αf12i , 12  12  ≤ sin φmax i  i T i 3 qρ q , v = (1) α problem is(t) find feedback control laws i N, 12 with(32) i γ  for f , ∀i ∈ I, which ρtod+ r rmrd +i= ,[−η qρid=N 2, 3,0]r..., (30) d (t) Ia (t) r (t) − (t). sin φimax αi12  , α  > sin  φimax 1  = d = i 12 i r (29) i i rrd , i i i i 12 d v12 r˙ 12 . −r  −3 F → (27) +the k4constraints r respecting α12make f min ≤ f ≤ f max Fk3while i=1 .  vii =  f i d q 12 i d,12 i i 2 q   i (1) i 1 − m T r = r + ρ , q = 2, 3, ..., N, (30) ≤ ϕ , ∀ ∈ I. ϕ i iv12  i Ia (t)rand  r (t) + ρ (t) − r (t). (32) d d rd (t) = [λt 0 h] (27) d max i rd α12  − i k3 r12 − rd,12 + k4 r˙ i12 .   iT f v 1 i=1 12 i ρd (t)v == [0  0 0] r1d = rrd ,  i T2T , . (31)(28)  1[λt −−0v 2r (t) Position i 120] Control ρLocal = =[−η γh] rd d (t) qi + ρvq12 (1) T T v 3ρ1 (t) r = r , qi =the 3,. ..., N, angle constraint 0γ0] the 22, inclination In to[0solve problem, we first The above respects ρd (t) = =[−η 0] afore-defined d control law d v d order 1− 12 (31) r1d = rrd , (29) T , 2 i i ρd3(t) the = local [−ηposition − γ 0]control law to be adopted by the and is such that, if ||α12i || ≤ sinϕmax , then r12i responds establish ϕi ≤ ϕmax T T q q (1) 3 1 ∆ i =γ2, 0] i..., N, i i i T ρ (t) = [−η r = r + ρ , q 3, (30) r (t) = [λt 0 h] r = r , rd Ia (t) i-th d dmulticopter. rrd (t) + ρd (t) − r law (t).is partitioned into an altitude (32)rd position d dThis r d,2 ] as if it to the horizontal command r d,12 = [r d,1 control T 1 ρ (t) = [0 0 0] q q i=1 (1) 3 d were governed linear time-invariant controller position controller.  and a horizontal rd = r2by a+pair ρd ,ofqsecond-order = 2, 3, ..., N, T , ρ = k[−η − γ 0] parameters and have Ia (t)  +ρ (t)T − ri (t). rrd (t) r= 0 idh] rd (t)[λt d (t) k and are design models. The (32) gains T 4 T ρ3d (t) 3 = [−η ρ1di=1 (t) Control = [0 0 0] T γ 0] effect of proportional and derivative actions, respectively. Altitude r (t) = [λt 0 h] , (31) rd T 2 ρLet [−η −ℝ,γ and 0] r i ∈ ℝ denote the vertical T 3 i = i 1 d (t)  ∈ ℝ, v ∈ The control law (Eq. 26) provides the horizontal projection r ρ (t) = [0 0 0] T d 3 d,3 i , i Ia (t) rrd (t) ρ3d (t) 3 = i [−η γi 0] 2  i i i + ρd (t) i T− r (t). = [−η − γ v0] is a unit vector, one can vector v . Since projections of r , v , and rd, respectively. Consider the minimum v 12 of the ρnormal d (t) i=1 T 3 i i i ρ3d (t) = [−η γ 0] ≥ 0 and maximum fimax > f min allowable values for the thrust f min thus write: Ia (t)  rrd (t) + ρd (t) − ri (t). (32) 3  i=1 (t) São José rdos + ρidVol.8, (t) − ripp.431-440, (t). J. Aerosp. Technol.IaManag., Campos, No 4, Oct.-Dec., 2016 rd (t) i=1

f mf v , ∀i ∈ I. i α3i  i g −fik1 rf3ii v−i ,r∀i I. k2 r˙3i . d,3∈ − v3 φi cos−1αvi 3ii ,, ∀i ∈ I. i ≤ sin φimax v3i 12 , ∀i ∈ I. φ12 i cos−1α i , v12 = α sin φimax αi12  , αi12  > sin φimax 12 i ˜r  ri − ridCastro ,i∀i ∈DF,iI. Santos DA 436  ri − ridi, ∀i∈ I. i  mi ˜r  i α12  − i k3 r12 − rd,12 + k4 r˙ 12 .  f   F  rr , ρ1 , ρ2 , ..., ρN1 , 2  F  rr , ρi , ρ , ...,ρN , v 12 i  (28)  v =1 2 1 − N i 2 . 12  , Fd  rrd , ρd , ρd, ..., ρdv , Fd  rrd , ρ1d , ρ2d , ..., ρN d  i ri 1d = rird , α3i fmax i  r (t) = [λt 0 h]    rd m leAder-Follower i i strAtegy ii T  k(t) rd,3 α3i  i g − 1 1 mr3 − 2 r˙3 i .  = [0 0−r0]ik− dAft v3 α3iρ − k2 r˙3i position . g − k er establishing stable local controllers, it 1 , 3 therd,3 i T ρ2d (t)v3 = [−η − γ 0]  to defi ne the position formation strategy. Here we iremains i i T α  ≤ sin φ α12 ρ, 3d (t) = [−η γ α 0]max i 12 i i i ,  ≤ sin φ α , = α 12i adopt strategy with a max single leader. 2 i12 12 a leader-follower i > sin φ sinvi12 φimax , α , = αi12 max 12 α  i i 3 12 (1)  sin φmax α sin φimax to attribute the ≡ r, i , α which corresponds Consider rr i12 12 i > Iai(t) rrd (t) + ρd (t) − r (t).   m i Assume thatthe local control laws i ˙1.  + −ito rivehicle k3 ri=1 . ki = αi12  − i leadership 4r 12m 12 i i i f αf12i =f i v−i, ∀i∈d,12 − r . –r i → 0,∀ ∈ I, for an k k4 r˙ i12 i.e. r 3are asymptotically d,12 + stable, 12 I, i fi   i arbitraryvposition command r di , ∀i ∈ I. Therefore, if the position  vi =  i 12 i 2 .vi12 commands the are chosen to be:  individual multicopters 1 v− = vof . 12  1 − vi12 2 r1d = rrd ,

r1d = rrd , q q (1) rd = r + ρq d , q (1) = 2, 3, ..., N, rd = r + ρqd , q = 2, 3, ..., N,

(29) (30)

T

rrd (t) = [λt 0 h] T rrd (t) T= [λt 0 h]i the ρ1d (t) =then [0 0 0]control laws fT, ∀i ∈ I, make F → Fd (Santos 2013). 1 ρd (t) = [0T 0, 0] ρ2d (t) =In [−η 0] the translation order− to γspecify T , of the fleet, the designer ρ2d (t) = T [−η − γ 0] 3 ρd (t) needs = to [−η γ 0] choose the desired leader position rrd as an appropriate T ρ3d (t) = [−η γ 0] 3 time-function. On the other hand, for specifying the form  3 i i  Ia (t)  rthe + ρd (t) − (t). rd (t) of formation, ther designer needs to establish the desired I (t)  r (t) + ρid (t) − ri (t). a rd i=1 relative positions of the followers ρdq w.r.t. the leader. i=1

(19) (20) (21)

(25)(19) (20) (26) (21) (27)

(22)

(22) The use(28) of a separate computer for simulating each multicopter is justified by the X-Plane’s inability to simulate (23) (23) more than 1 vehicle in a single computer. Nevertheless, despite (29) the use of a distributed simulation architecture, the X-Plane (30) still(24) allows simultaneous visualization of up to 20 vehicles on (24) the same monitor by means of its multiplayer function. The proposed scheme uses this function to show the formation (25) flight on a larger screen (Fig. 4b). (31)(25) For communicating with external processes and machines, (26) the X-Plane adopts the User Datagram Protocol (UDP). Unlike (26) the Transmission Control Protocol (TCP), the UDP does not (32) assure (27)that data packages will arrive completetly and orderly (27)2010). However, the communication via (Garcia and Barnes UDP is extremely fast and makes efficient use of the bandwidth (28) (Liu et al. 2002), (28) which are advantageous characteristics for the simulation under consideration. (29) In the proposed (29) scheme, the UDP interface between MATLAB/Simulink and X-Plane is used in 2 directions. On one (30) (30) hand, the X-Plane sends to Simulink the vehicle’s position, linear velocity, Euler angles, and angular velocity. On the other hand, the Simulink sends rotor throttle commands to the X-Plane. (31) (31) The present study considers a sampling frequency of 90 Hz. To enable the X-Plane exchanging data with Simulink, both softwares running in the same computer, a loopback network (32) i.e. the computer sends data via the UDP network is adopted, (32) Computer 1 X plane + MATLAB/Simulink

SOFTWARE-IN-THE-LOOP SIMULATION The present section proposes a software-in-the-loop simulation scheme for evaluation and demonstration of position formation control of multicopters. Particularly, we take into consideration a leader-follower formation with one leader and 2 followers. Each one of the vehicles is simulated by a separate desktop computer, where the control laws are implemented in MATLAB/Simulink and the flight dynamics and environment are simulated in the flight simulator X-Plane. The 3 computers are interconnected by an Ethernet network (Fig. 4). Computer 1 simulates the leader multicopter, while computers 2 and 3 simulate the followers 1 and 2, respectively. The data exchaged in the network throughtout the simulation is the leader’s position r1, which is sent from the leader to the 2 followers.

r (1)

Computer 2 X plane + MATLAB/Simulink

r (1)

r (1)

Computer 3 X plane + MATLAB/Simulink

Figure 4. (a) SIL simulation scheme; (b) Real simulation environment.

J. Aerosp. Technol. Manag., São José dos Campos, Vol.8, No 4, pp.431-440, Oct.-Dec., 2016

¨ri =

0 1 i i  0  , ∀i ∈ I, f v + mi −g fi  f i vi , ∀i ∈ I.

A Software-in-the-Loop Simulation Scheme for Position Formation Flight of Multicopters 2 −1 v i , ∀i ∈ I. φi  cos 3

437

2

to its own address.  Ribeiro  and Oliveira (2012) and Garcia and 0 Barnes1(2010) present about UDP communication  0more  , details  ¨ri = i f i vi +  ∀i ∈ I, 0 between MATLAB/Simulink and X-Plane. m 1 i i  −g  ¨ri =To perform 00 f v +the communication , ∀i ∈ I, between computers it was i m 1i i i   i −g   i i ¨ r = I, set an m Ethernet network star∈topology, by assigning a fixed v , ∀i00∈with I. , ∀i f fv f+ 1i i i  −g  i i i i ¨ 0 r = f v + , ∀i ∈ I, IP address for each computer. This network was assembled mif  f v , ∀i ∈ I. i a istandard i−g that has an internal switch. The switch i ivrouter through cosf−1 , ∀i∈ ∈I.I. φ  v , 3∀i f  i i to −1 i i isent allowsφdata be directly cosf v v, 3∀i , ∀i ∈ ∈ I.I. to the desired port, avoiding f  ii data excessive traffic i −1 i i on the network. The transmission is , ∀i∈∈I.I. φ˜r  rcos− rdv,3∀i performed through pair cables. i −1 i twisted i i r − rdv,3∀i , ∀i∈∈I.I. φ˜r  cos   1 i2 , rρd ,,∀i ...,∈ρN F ˜ri rrr, iρ− I. ,   1 i2 N ˜ri rrr,iρ− , rρd ,,∀i ...,∈ρI. F , SIMULATION TESTS   N 11 , ρ22 , ..., ρN  r , ρ FF , ρ , ρ , ..., ρ  r d rd d d d ,, r   11 22 NN ρd,,..., ...,the F ,,ρρvalidate ρρd SIL F  rrrd ,, scheme, a pure MATLAB d  r to d,,ρ In order i i i  , < f f 1 α2 N  3 min min , ..., ρd i ,models given in “Analytical Fd  i rusing rd , ρ , ρdanalytical simulation i  ] , αi3 , , αd13ithe ∈ fmax fi =  i[fmin ,i N 23 α < f f  min  r , , ρdi,isα ρalso ρimin F i section id , ..., d  for comparison. In the di , iModeling” > f f i rd i implemented max max 3 ,ifmax ] , f =  fαi3 , , α3 ∈αi[f< min f 3  min min pure MATLAB simulation, integrations are performed i i i i  >i fnumerical fmax  ,i f i ]i , αi 3i ,,, α3ii α∈ f i =m imin max α33i[f < fmax fg4th-order − k r − k   − r r ˙ α3i using min min the Runge-Kutta method  1 2 3 i d,3 3 . with step of 0.002 s. The ii i i α3[f>i fmax v3ii fα max , , α3ii ∈ , fmax ]i , fi i =m i 3 min k2chosen g − kinitial   −i rd,3 i−are r˙3 . to be: r1 = [0 0 2]T m, α3multicopter’s 1 r3 positions ii fi v  i T,  α3 > i f i max max m 3 2 3 ≤ sin i r = [−2α i α 0g122] and r i=[−4 0 2]φiTmax m. The vehicles’ mass is −, kim, 1 r3 − r12 d,3 − k2 r˙3 . ii i , vi12 =α3 m α    v i i 12 i 3 φ , α  > sin φ i m sin i i i , ≤ α kg is g = 9.796 m/s2. i  the gravity max αkand g12− −acceleration k2 r˙3max . α3= 2.132 i 1 r3 − r12 12 d,3 i i , v12 = α v i i i 12 3φ αi , i , k ,α k 212 ,i12k3 ,> kφ4max of the local position The sinparameters sin ≤and sinφ imax i 12α  1 α mmax 12 i ii i ,with overshoot of vi12α=i12 α ˙ r − r  − k r + k i i i i 12 3,are 12 4 response control tuned for a time d,12 12 ≤ sin φmax sinfφilaws , α > . iα12α 12  i12 mmax i i i i , vi12α=i M α i of+ 12 r˙ 12 −0.02 rtime r peak ≈ m tPk≈sin s.φiMoreover, the constraint 42 3and 12 Psin d,12 φiimax , − α > i 12 . kα max 12  i fm   12  vi i i i parameters i i 12 i of rthe local controllers . set at φmax = 30°, +. k4 r˙ 12  are α12  − 3 v f=ii k 12 − r i d,12 2   i m 1 − v  i i i if v12=12 N,kand 2 N. + k4 r˙ 12 . − rid,12 − αi12f max  3 r min v=i 40 . f=i  1 12 i i12 2 − v v Fori illustrating the proposed SIL simulation scheme, the 1 12 ,i 2 . v =  r d = rrd iv(see 1 −  the notation defined in Eq. 23) is desiredi formation F v 12 1q 12 d q v (1) rrd , i2, 3, . N, = +r d d= r = r ρ , q = ..., 2 d 1 − v  chosen with: 1 12 rqd = r(1) +rρdqd= , qrrd =, 2, 3, ..., N, T 1q =, rqrd =rρ 0=,h]2, 3, ..., N, d [λt rqdrrd =(t) r(1) + d TT 1 = rrd (t) = ρq[0 [λt000]h]2, 3, ..., N, d (t) = r(1) + rqdρ d, q = T T , 21 ρ = [−η − γT 0] ρrddrd(t) (t) = [0 0 00] (t) = [λt h] T , 32 TT0] T ρ = [−η 1(t) ρρ (t) −γ (31) [0 000] rddrd (t) = = [−η [λt h]γ 0] d (t) TT , T ρρ3d21(t) = [−η γ 0] 3 [−η0 0] − γ 0]  = [0 d d (t) , Ia (t) ρ r=rd (t)[−η + ρidγ(t)0] −TrTi (t). 32 (t) 3 − γ 0] d d i i i=1 Ia (t) ρ3 (t)3 r=rd (t) + ρdγ(t)0]−Tr (t). [−η d  i=1 Ia (t)  3 rrd (t) + ρid (t) − ri (t).  Ia (t) where: + ρid (t) ri (t). time; λ = 0.5 m/s is  i=1 tr≥rd0(t) denotes the−continuous i=1 the longitudinal velocity; h = 2 m is the formation altitude; η = 2.6 m is the longitudinal distance from the CM of the leader vehicle to the CM of the follower vehicles; γ = 1.5 m is the transversal distance from the CM of the leader vehicle to the CM of the follower vehicles.

i that Fd ˜ri the  r2afore-defi − rid , ∀ined ∈ I.

Note is a triangular formation (Fig. 5). Moreover,2 it is worth mentioning that, by Eq. 31, the (18)   1 2 leader vehicle toNfollow F  isrcommanded , a straight line on the r , ρ , ρ , ..., ρ (18) horizontal plane, with constant speed λ and with an altitude h. (18)   (19) , Fd  rrd , ρ1d , ρ2d , ..., ρN d (18) Leader (19) X R  i i (19) α3i < fmin  f(20) min , i i α3i , YαR 3i ∈ [fmin , fmax ] , fi = (20)  (19) i i i , α > f f(21) 3m 3m max max 3 (20) i  2.6 m    m (21) i − k1 r3i1.5 − k2 r˙3i . − mrd,3 α3i  i g(20) 1.5 m v3 (22) Follower 1(21) Follower 2  i i i (22) (21) , α  ≤ sin φ α max 12 12 , vi12 = αi12 (23) (22)α sin φimax αi12  > sin φimax i , 12

(23) (22)   m   i i k3 r12 − rid,12 + k4 r˙ i12 . α12  − i (23) f (24) (23)   (24) vi12 i  . v = (24) 1 − vi12 2 (25) (24) (25)r1d = rrd , Figure 5. A triangular desired formation flight. (a) Geometry (25) q q of the desired formation; X-Plane visualization of the (1) r = r (26) + ρd , q (b) = 2, 3, ..., N, desireddformation. (25) (26) T rrd (t)(26) = [λt 0 h] (27) In order error and compare the SIL T 1 to quantify the formation ρd (t)(26) = [0 0 0] (27) simulation2with the corresponding pure T , MATLAB simulation, ρd (t) = [−η − γ 0] T the following accuracy figureγof 0] merit is adopted: (27) ρ3d (t)(28) = [−η (27) 3(28)  Ia (t)  (29) rrd (t) + ρid (t) − ri (t). (32) (28) i

i=1

(29) (28) (30) (29) (t) ≥ 0, ∀i, is such that, if F → Fd, then Ia (t) → 0. Note that I(30) a (29) (30) First of all, we conducted a test with the SIL simulation scheme (30) the performance of the network’s data in order to evaluate (31) exchange. In this (31)test, data are exchanged between computers of the network(31) and compared to check if they are equal in the origin and(31) destination. As a result, it was verified that the (32) communication stays synchronized, as it shows no delays and (32) no data loss. The observed characteristics are essential for a (32) of the proposed SIL simulation scheme. consistent operation (32) the time response of the vehicles’ translation Figure 6 shows considering the desired formation given in Eq. 31. This figure compares the SIL simulation with the pure MATLAB simulation. In both cases, the true formation converges to the desired one with a transient of less than 5 s and an steady-state error of less than 0.02 m. These results also show that the performance observed

J. Aerosp. Technol. Manag., São José dos Campos, Vol.8, No 4, pp.431-440, Oct.-Dec., 2016

Castro DF, Santos DA

438

× 10–3

r1 [m]

10 5 0

0

5

10

15

20

r2 [m]

–10

1.98

–20

0

5

10

15

1.96

20

5

10

15

20

–2

10

15

20

0

5

10

15

20

0

5

10

15

20

2 1.95 5

0

10

15

20 2.2

1

0

5

2.1

1.5

5

0

2.05

–1

0

r3 [m]

2

0

6 4 2 0 –2

–5

0

2

0.5 5

0

10

15

20

0

5

0

Times [s]

10

15

1.8

20

Times [s]

Times [s]

Figure 6. Time response to the formation command (Eq. 31). The solid red lines represent the components of rd(1), rd(2), and rd(3). The dashed black lines represent the components of r(1), r(2), and r(3) obtained in the SIL simulation. The dashed blue lines represent the components of r(1), r(2), and r(3) obtained in the pure MATLAB simulation.

in the SIL simulation is quite similar to that obtained using the pure MATLAB simulation, which, in some sense, validates the model of “Analytical Modeling” section with parameters identified, as described in “Parameter Estimation” section. For a better visualization of the position formation, Fig. 7 illustrates the trajectory of the multicopters on the X-Y plane. 2

1

rd

Follower 1

1.5

r1

Each curve shows the trajectory of 1 vehicle, starting on the left and finishing at t = 20 s on the right extremity. In this figure, we can see the convergence of F towards Fd. Finally, we illustrate the position formation in the SIL simulation scheme under constant wind disturbances. Figure 8 compares the performance of the position formation using the figure of merit Ia (t) for 3 different values of the wind speed υ. Here, it is possible to see the impact of the wind on the perfomance of the formation flight.

2

rd

1

r t=0

YR [m]

0.5 0

X

X

t = 20 s Leader

X

2 3

5

r3

4

rd

Ia [m]

-0.5 -1 -1.5 -2

-5 -4 -3 -2 -1 0 1

Follower 2

2 3 4 5

XR [m]

v=0 v = 5 m/s v = 10 m/s

3 2 1

6 7

0

8 9 10

Figure 7. The position formation on the x-y plane, obtained in the SIL simulation.

0

5

10 Time [s]

15

20

Figure 8. The position formation error for different wind velocities.

J. Aerosp. Technol. Manag., São José dos Campos, Vol.8, No 4, pp.431-440, Oct.-Dec., 2016

A Software-in-the-Loop Simulation Scheme for Position Formation Flight of Multicopters

CONCLUSIONS The paper proposed a SIL simulation scheme for demonstration and evaluation of position formation control of multicopters. The scheme is implemented in a star-topology computer network, where each computer simulates 1 multicopter of the fleet. The MATLAB/Simulink is used for implementing the control laws, while the flight simulator X-Plane models the environment, the vehicles’ dynamics and provides a graphical visualization as well. For illustrating the SIL simulation, a leader-follower strategy together with distributed linear saturated position control laws were considered. The simulation tests showed that the SIL simulation scheme proved itself to be effective and reliable to simulate a fleet of multicopters in formation flight, allowing a good visualization of the scene and providing realistic data. The proposed SIL simulation scheme can be used effectively for preliminary evaluation of position formation control systems of multicopters, before conducting real flight tests. We argue

439

that, by adopting such a simulation, it is possible to reduce costs, avoid risks, and save time in the development of such a fleet of micro aerial vehicles.

ACKNOWLEDGEMENTS The first author would like to thank the Fundação de Amparo à Pesquisa do Estado do Amazonas (FAPEAM), for providing his master’s scholarship during the project. Furthermore, we are grateful to Conselho Nacional de Desenvolvimento Científico e Tecnológico (CNPq), for supporting the project under the Grant 475251/2013.

AUTHOR’S CONTRIBUTION Castro DF and Santos DA conceived the idea and co-wrote the text. Castro DF worked on the simulations results.

REFERENCES Casbeer DW, Li SM, Beard RW, Mehra RK, McLain TW (2005) Forest fire monitoring with multiple small UAVs. Proceedings of the American Control Conference; Portland, USA.

King E, Kuwata Y, Alighanbari M, Bertuccelli L, How J (2004) Coordination and control experiments on a multi-vehicle testbed. Proceedings of the IEEE American Control Conference; Boston, USA.

Eschmann C, Kuo CM, Kuo CH, Boller C (2012) Unmanned aircraft systems for remote building inspection and monitoring. Proceedings of the European Workshop on Structural Health Monitoring; Dresden, Germany.

Koo VC, Chan YK, Gobi V, Chua MY, Lim CH, Lim CS, Thum CC, Lim TS, Ahmad Z, Mahmood KA, Shahid MH, Ang CY, Tan WQ, Tan PN, Yee1 KS, Cheaw WG, Boey HS, Choo AL, Sew BC (2012). A new unmanned aerial vehicle synthetic aperture radar for environmental monitoring. PIER 122:245-268. doi: 10.2528/PIER11092604

Garcia R, Barnes LE (2010) Multi-UAV simulator utilizing X-Plane. J Intell Robot Syst 57:393-406. doi: 10.1007/s10846-009-9372-4 Girard AR, Howell AS, Hedrick JK (2004) Border patrol and surveillance missions using multiple unmanned air vehicles. Proceedings of the IEEE Conference on Decision and Control; Paradise Island, Bahamas. Han J, Hong Wang C, Xing Yi G (2013) Cooperative control of UAV based on multi-agent system. Proceedings of the IEEE 8th Conference on Industrial Electronics and Applications; Melbourne, Australia. Hao Y, Bin J (2011) Fault tolerant cooperative control with application to UAV team longitudinal flight. Proceedings of the IEEE 30th Chinese Control Conference; Yantai, China.

Li Z, Li Y, Walker R, Hayward R, Zhang J (2010) Towards automatic power line detection for a UAV surveillance system using pulse coupled neural filter and an improved Hough transform. Mach Vis Appl 21:677-686. doi: 10.1007/s00138-009-0206-y Liu PX, Meng M, Ye X, Cui J (2002) An UDP-based protocol for internet robots. Proceedings of the 4th World Congress on Intelligent Control and Automation; Shanghai, China. Ljung L (1987) System identification: theory for the user. 2nd edition. Englewood Cliffs: Prentice Hall.

Hoffmann GM, Waslander SL, Vitus MP, Huang H, Gillula JH, Pradeep V, Tomlin CJ (2009) Stanford testbed of autonomous rotorcraft for multi-agent control. Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems; St Louis, USA.

Lupashin S, Schollig A, Sherback M, D’Andrea R (2010) A simple learning strategy for high-speed quadrocopter multi-flips. Proceedings of the IEEE International Conference on Robotics and Automation; Anchorage, USA.

How J, King E, Kuwata Y (2004) Flight demonstrations of cooperative control for UAV teams. Proceedings of the AIAA 3rd “Unmanned Unlimited”Technical Conference, Workshop and Exhibit; Chicago, USA.

Michael N, Mellinger D, Lindsey Q, Kumar V (2010) The GRASP multiple micro-UAV testbed. IEEE Robot Autom Mag 17(3):56-65. doi: 10.1109/MRA.2010.937855

How JP, Behihke B, Frank A, Dale D, Vian J (2008) Real-time indoor autonomous vehicle test environment. IEEE Contr Syst Mag 28(2):5164. doi: 10.1109/MCS.2007.914691

Pizetta IHB, Brandão AS, Sarcinelli Filho M, Carelli R (2012) Uma plataforma hardware-in-loop para vants de asas rotativas. Proceedings of the XIX Congresso Brasileiro de Automática; Campina Grande, Brazil.

J. Aerosp. Technol. Manag., São José dos Campos, Vol.8, No 4, pp.431-440, Oct.-Dec., 2016

440

Castro DF, Santos DA

Puri A, Valavanis KP, Kontitsis M (2007) Statistical profile generation for traffic monitoring using real-time UAV based video data. Proceedings of the IEEE Mediterranean Conference on Control and Automation; Athens, Greece. Ribeiro LR, Oliveira NMF (2012) UAV autopilot controllers test platform using Matlab/Simulink and X-Plane. Proceedings of the ASEE/IEEE Frontiers in Education Conference; Seattle, USA. Santos DA (2013) Automatic position control of multirotor helicopters. Paris: ESIEE. Technical report. Santos DA, Saotome O, Cela A (2013) Trajectory control of multirotor helicopters with thrust vector constraints. Proceedings of the IEEE 21st Mediterranean Conference on Control and Automation; Chania, Greece. Semsch E, Jakob M, Pavlicek D, Pechoucek M (2009) Autonomous UAV surveillance in complex urban environments. Proceedings of the International Joint Conference on Web Intelligence and Intelligent Agent Technology; Stevenage, United Kingdom. Shim DH, Chung H, Kim HJ, Sastry S (2005) Autonomous exploration in unknown urban environments for unmanned aerial vehicles. Proceedings of the AIAA Guidance, Navigation, and Control Conference and Exhibit; San Francisco, USA.

Stirling T, Roberts J, Zufferey JC, Floreano D (2012) Indoor navigation with a swarm of flying robots. Proceedings of the IEEE International Conference on Robotics and Automation; St Paul, USA. Tomic T, Schmid K, Lutz P, Domel A, Kassecker M, Mair E, Grixa IL, Ruess F, Suppa M, Burschka D (2012) Toward a fully autonomous UAV: Research platform for indoor and outdoor urban search and rescue. IEEE Robot Autom Mag 19(3):46-56. doi: 10.1109/MRA.2012.2206473 Valenti M, Bethke B, Fiore G, How JP, Feron E (2006) Indoor multivehicle flight testbed for fault detection, isolation and recovery. Proceedings of the AIAA Guidance, Navigation and Control Conference and Exhibit; Keystone, USA. Wang J, Xin M (2013) Integrated optimal formation control of multiple unmanned aerial vehicles. IEEE Trans Contr Syst Tech 21(5):17311744. doi: 10.1109/TCST.2012.2218815 Wen G (2012) Distributed cooperative control for multi-agent systems (PhD thesis). Villeneuve-d’Ascq: École Centrale de Lille. Wertz JR (1978) Spacecraft attitude determination and control. Dordrecht, Boston: Reidel. Zhu S, Wang D, Low CB (2013) Cooperative control of multiple UAVs for moving source seeking. Proceedings of the IEEE International Conference on Unmanned Aircraft Systems; Grand Hyatt, USA.

J. Aerosp. Technol. Manag., São José dos Campos, Vol.8, No 4, pp.431-440, Oct.-Dec., 2016