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

: 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.


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).
Castro DF, Santos DA In order to maximize the probability of success and save time in flight experiments, preliminary tests based on a realistic simulation environment are often desirable.In particular, the so-called software-in-the-loop (SIL) simulation is an interesting approach.It attempts to gain realism by combining in a simulation scheme 2 or more specilized softwares, each one for modeling a specific part of the system.It can also include the flight computer, which can be evaluated before flight experiments (Pizetta et al. 2012).Garcia and Barnes (2010) present a SIL simulation for formation flight of electric helicopters using one computer to implement the control laws in a centralized form and others to simulate the dynamics of the fleet members.The disadvantage of this work, as reported by the authors, is that the processing and communication capabilities of the (centralized) control computer are a bottleneck.
The present paper proposes a distributed SIL simulation scheme for testing position formation control systems of multicopters.The scheme consists of a network where each computer simulates a vehicle of the fleet using the MATLAB/ Simulink, for implementing the local control laws, and the flight simulator X-Plane, for simulating the flight dynamics and environment.
The paper is organized in the following manner.The next section presents the multicopter dynamic models.Thus, it is presented a position formation control method.In sequence, the proposed SIL simulation scheme is described.In "Simulation Tests" section, the proposed scheme is illustrated by testing a position formation flight of 3 octocopters.Finally, the last section concludes the paper.

MULTICOPTER MODELING
Consider a multicopter and 2 Cartesian coordinate systems (CCSs), as illustrated in Fig. 1.The body CCS S B = is fixed on the vehicle's structure with its origin at the vehicle's center of mass (CM).The reference CCS S R = ∆ {X R , Y R , Z R } is fixed on the ground at point O; its Z-axis is aligned with the local vertical axis.

AnAlyticAl Modeling
Here we present analytical models for a multicopter.These models will be later compared with the corresponding X-Plane model.The attitude kinematics equation in quaternions q is given by the following differential one (Wertz 1978): where: and [ω×] is the cross-product matrix of ω, i.e.: The attitude dynamics is described by the Euler equation: where of the vehicle's angular momentum; I ∈ ℝ 3×3 is the vehicle's inertia matrix w.r.t.S B .It is assumed that the vehicle has a symmetrical structure so that I can be written as: Neglecting perturbations, the translational dynamics model is obtained from the Newton's Second Law as: (1) A Software-in-the-Loop Simulation Scheme for Position Formation Flight of Multicopters Davi Ferreira de Castro and Davi Antônio dos Santos [ω×] (3) A Software-in-the-Loop Simulation Scheme for Position Formation Flight of Multicopters Davi Ferreira de Castro and Davi Antônio dos Santos A Software-in-the-Loop Simulation Scheme for Position Formation Flight of Multicopters Davi Ferreira de Castro and Davi Antônio dos Santos A Software-in-the-Loop Simulation Scheme for Position Formation Flight of Multicopters where: r = ∆ [r 1 r 2 r 3 ] T ∈ ℝ 3 is the position of the CM w.r.t.
is the unit vector perpendicular to the plane of rotors; f is the magnitude of the total thrust; g is the gravity acceleration; m is the vehicle's mass.Now denote the thrust and the reaction torque produced by the j-th rotor by, respectively, f j and τ j , j = 1, …, n, being n the number of rotors.The usual aerodynamic models for the rotors are: where: k f is the thrust coefficient; k τ is the reaction torque coefficient; ω j is the rotation speed of the j th rotor.Define the vector of individual thrusts by f We can relate the magnitude f of the total thrust and the resultant torque where, for the octocopter configuration taken into account in the present paper (see Fig. 1), and κ = ∆ k τ / k f and l the arm's length.

X-PlAne Modeling
In this study, the multicopter dynamics, its flight environment (gravity, wind, etc.), as well as a graphic visualization of the mission, are modeled in Plane-Maker, the design tool associated with the flight simulator X-Plane.For illustration, we chose the Gyrofly Gyro-200ED-X8 octocopter (Fig. 2a).The corresponding X-Plane model is depicted in Fig. 2b.
The X-Plane was chosen due to the following characteristics: it is certified by the Federal Aviation Administration (FAA); it is 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.Based on Blade Elements Theory, the X-Plane reads the geometric shape of the aircraft modeled in Plane-Maker and divides its surfaces into several small elements.Then, it simulates the forces acting on each element.These forces are converted into accelerations that, when integrated over time, result in velocities and translation.
The X-Plane dynamic model is generated numerically as described above.In other words, it does not give the equations of motion neither of some physical parameters such as the inertia matrix and the rotor coefficients.Therefore, for designing of control laws, we need first to identify some parameters of the X-Plane model.

PArAMeter estiMAtion
The inputs of the analytical modeling (Eqs. 1 to 6) are the magnitude f of the total thrust and the total torque τ produced by the propellers.On the other hand, the inputs of the X-Plane model are rotor throttles T j ∈ [0, 1], j = 1, …, 8. Therefore, before estimating the rotor coefficients k f and k τ , and the moments of inertia I 1 , I 2 , and I 3 , we need to convert commands of f and τ into T j .Denote the commands of f, τ, and f by f -, τ -, and f -, respectively.From Eq. 9, one can write: By inverting Eq. 11, we can obtain the individual thrust commands from f and τ -,by: where: The next step is to convert from f j and τ j to T j .For this end, a preliminary test was made, which showed a linear relation between these quantities, i.e.: (10) (2) rmation Flight of Multicopters Ferreira de Castro and Davi Antônio dos Santos (2) (3) 1 A Software-in-the-Loop Simulation Scheme for Position Formation Flight of Multicopters Davi Ferreira de Castro and Davi Antônio dos Santos (2) (3) (2) 1 ftware-in-the-Loop Simulation Scheme for sition Formation Flight of Multicopters Davi Ferreira de Castro and Davi Antônio dos Santos [ω×] (3) In order to estimate the parameters k f and k τ , we adopt the SIL simulation scheme that will be presented in "Soft ware-inthe-Loop Simulation" section.Th e rotors are assumed to be identical and the identifi cation is made only for the fi rst rotor.Th e vehicle is commanded to hover and an additive random signal is used to excite the X-Plane model through the throttle input T 1 (k).Let f -1 and τ -1 denote the measurements of f 1 and τ 1 , respectively.We adopt the following measurement model:

PreliMinAry deFinitions
Th is section presents a position formation fl ight control method based on the leader-follower strategy (Santos 2013).To begin with, consider a team of N multicopters and the CCSs of Fig. 3. Th e body's CCSs are denoted by , 2, .... N} Th e i th body CCS S i B is assumed to be attached to the structure of the i-th multicopter and centered at its center of mass CM i .Th e reference CCS is denoted by It is assumed to be fi xed on the ground at point O. −g [ω×] [ω×] A similar procedure is adopted for estimating the moments of inertia I 1 , I 2 , and I 3 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.Th e vehicle is initially commanded to hover at a point and a random signal is added to the attitude command about the considered axis.Th e 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.
Assume that S R is an inertial frame and neglect any disturbance force.Th e direct application of the Second Newton's Law gives the following translational dynamic models represented in S R . where: T ∈ ℝ 3 denotes the three-dimensional position of CM i ; v i ∈ ℝ 3 is the unit vector normal to the rotor plane of the i th multicopter; m i is the mass of the i th multicopter; f i is the magnitude of the total thrust produced by the rotors of the i th multicopter.
Defi ne the total thrust vector f i ∈ ℝ 3 of the i th vehicle by: A Software-in-the-Loop Simulation Scheme for Position Formation Flight of Multicopters and the inclination angle ϕ i of v i as: magnitude f i .Assume that m i , g, and v i 3 ≠ 0are exactly known.The altitude feedback control law is given by (Santos et al. 2013): Note that ϕ i consists of the angle between v i and Z R .Let ] T ∈ ℝ 3 denote the position command for the i-th vehicle and define the corresponding tracking error r -i ∈ ℝ 3 by: Let the position formation be described by the set: where: r r ∈ ℝ 3 is defined as the reference position for the formation; ρ i ∈ ℝ 3 , ∀ i ∈ I, are the actual relative positions of the vehicles w.r.t.r r .Using the same notation, one can represent the corresponding desired formation by: where: r rd ∈ ℝ 3 is the desired relative reference position of the formation; ρ i d ∈ ℝ, ∀ i ∈ I, are the desired positions of the vehicles w.r.t.r rd .
For all i ∈ I, let ϕ i max ∈ ℝ be the maximum allowable value of the inclination angle ϕ i and f i min ∈ ℝ and f i max ∈ ℝ be, respectively, the minimum and the maximum allowable values of the total thrust magnitudes f i .The position formation control problem is to find N feedback control laws for f i , ∀ i ∈ I, which make F → F d while respecting the constraints

locAl Position control
In order to solve the afore-defined problem, we first establish the local position control law to be adopted by the i-th multicopter.This control law is partitioned into an altitude controller and a horizontal position controller.

Altitude Control
Let r i 3 ∈ ℝ, v i 3 ∈ ℝ, and r i d,3 ∈ ℝ denote the vertical projections of r i , v i , and r i d , respectively.Consider the minimum f i min ≥ 0 and maximum f i max > f i min allowable values for the thrust with The above control law respects the total thrust magnitude constraint max , then the vehicle altitude r i 3 responds to the altitude command r i d,3 as if it were governed by a second-order linear time-invariant model.The gains k 1 and k 2 are design parameters and have effect of proportional and derivative actions, respectively.

Horizontal Position Control
Let ∈ ℝ 2 denote the horizontal projections of r i , v i , and r i d , respectively.Consider the maximum allowable value ϕ i max > 0 for the inclination angle ϕ i .Assume that m i and f i ≠ 0 are exactly known.The horizontal-position feedback control law is given by (Santos et al. 2013): r q d = r (1) + ρ q d , q = 2, 3, ..., N, (30) I a (t) r q d = r (1) + ρ q d , q = 2, 3, ..., N, (30) .
The above control law respects the inclination angle constraint ϕ i ≤ ϕ i max and is such that, if ||α i 12 || ≤ sinϕ i max , then r i 12 responds to the horizontal position command r i ] T as if it were governed by a pair of second-order linear time-invariant models.The gains k 3 and k 4 are design parameters and have effect of proportional and derivative actions, respectively.
The control law (Eq.26) provides the horizontal projection v i 12 of the normal vector v i .Since v i is a unit vector, one can thus write: . (28) r q d = r (1) + ρ q d , q = 2, 3, ..., N, (   (32) . (28) r q d = r (1) + ρ q d , q = 2, 3, ..., N, (   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 identifi ed, 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. .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.Each curve shows the trajectory of 1 vehicle, starting on the left and fi nishing at t = 20 s on the right extremity.In this fi gure, we can see the convergence of F towards F d .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 fi gure of merit I a (t) for 3 diff erent values of the wind speed υ.Here, it is possible to see the impact of the wind on the perfomance of the formation fl ight.

Figure 1 .
Figure 1.A multicopter and the Cartesian coordinate systems.

1A
Software-in-the-Loop Simulation Scheme for Position Formation Flight of Multicopters Davi Ferreira de Castro and Davi Antônio dos Santos the modeling error vector.Applying the data set took at instants k = 1, 2, …, N in Eq. 15, one obtains a set of linear equations with unknown Ѳ. Th ese equations can be compactly represented by the following matrix equation: where: Y = ∆ [y(1) T y(2) T ... y(N) T ] T , X = ∆ [x(1) T x(2) T ... x(N)] T , and E = ∆ [є(1) T є(2) T ... є(N) T ] TUsing the measurement model (Eq.16) and the data set represented in X and Y, one can immediately obtain a leastsquares estimate Ѳ ˆ of Ѳ by(Ljung 1987):

Figure 3 .
Figure 3.A team of N multirotor vehicles and the Cartesian coordinate systems. f

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

Figure 6 .
Figure 6.Time response to the formation command (Eq.31).The solid red lines represent the components of r d (1) , r d (2) , and r d (3)

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

Table 1 .
Estimation of the parameters k f , k τ , I 1 , I 2 , and I 3 .