Content uploaded by Anam Tahir
Author content
All content in this area was uploaded by Anam Tahir on Jun 03, 2020
Content may be subject to copyright.
Available via license: CC BY 4.0
Content may be subject to copyright.
Received March 19, 2020, accepted March 30, 2020, date of publication April 20, 2020, date of current version June 3, 2020.
Digital Object Identifier 10.1109/ACCESS.2020.2988773
Comparison of Linear and Nonlinear Methods
for Distributed Control of a Hierarchical
Formation of UAVs
ANAM TAHIR 1, JARI M. BÖLING2, MOHAMMAD-HASHEM HAGHBAYAN 1, (Member, IEEE),
AND JUHA PLOSILA 1, (Member, IEEE)
1Autonomous Systems Laboratory, Department of Future Technologies, Faculty of Science and Engineering, University of Turku, 20014 Turku, Finland
2Laboratory of Process and Systems Engineering, Åbo Akademi University, 20500 Turku, Finland
Corresponding author: Anam Tahir (anam.tahir@utu.fi)
This work was supported in part by the Academy of Finland, project no. 314048.
ABSTRACT A key problem in cooperative robotics is the maintenance of a geometric configuration during
movement. As a solution for this, a multi-layered and distributed control system is proposed for the swarm
of drones in the formation of hierarchical levels based on the leader–follower approach. The complexity
of developing a large system can be reduced in this way. To ensure the tracking performance and response
time of the ensemble system, nonlinear and linear control designs are presented; (a) Sliding Mode Control
connected with Proportional-Derivative controller and (b) Linear Quadratic Regular with integral action
respectively. The safe travel distance strategy for collision avoidance is introduced and integrated into
the control designs for maintaining the hierarchical states in the formation. Both designs provide a rapid
adoption with respect to their settling time without introducing oscillations for the dynamic flight movement
of vehicles in the cases of (a) nominal, (b) plant-model mismatch, and (c) external disturbance inputs. Also,
the nominal settling time of the swarm is improved by 44% on average when using the nonlinear method as
compared to the linear method. Furthermore, the proposed methods are fully distributed so that each UAV
autonomously performs the feedback laws in order to achieve better modularity and scalability.
INDEX TERMS Unmanned aerial vehicles (UAVs), distributed control, hierarchical systems.
I. INTRODUCTION
Recently, the use of swarms/fleets of Unmanned Aerial Vehi-
cles (UAVs), or drones, have become increasingly popular;
attracting attention from researchers and industries represent-
ing miscellaneous disciplines [1]. Controlling the formation
of a swarm using either centralized or decentralized (i.e.
distributed) architecture is a demanding control problem.
Due to the lack of observability, each drone might have to
dynamically adjust its location w.r.t. the other drones.
Based on recent studies, the strategies to control the for-
mation of a drone swarm are generally divided into three
categories; (a) leader–follower [2]–[4], (b) virtual struc-
ture [5]–[7], and (c) behavioural [8]–[10] approaches. In a
leader–follower approach, one or more drones are given a
leading role, i.e. the responsibility to decide on the general
path of the swarm. The desired positions of the other drones,
The associate editor coordinating the review of this manuscript and
approving it for publication was Yilun Shang .
the followers, are defined relative to the actual states of
the associated leaders. A swarm can be further divided into
subgroups of leader–followers, building a hierarchy, which
improves the scalability of the approach. The main advantage
of this approach is that the path planning algorithm needs
to be executed only in the leaders’ workspace which signifi-
cantly reduces the overall computation time of path planning,
indicating fast decision making within the swarm. In a virtual
structure approach, a virtual moving structure, i.e. a forma-
tion, decides the pose reference of each element of the fleet.
Based on these pose references, the controller manipulates
the actuators to reduce the error as much as possible. In this
approach, each virtual vacant pose can be filled by any drone,
and therefore, an actual leader is not needed. In contrast to
the first two, in a behaviour-based approach, the actuation is
defined as several desired behaviours that are assigned to each
drone. The overall control is derived by assigning different
weights to behaviours in each drone to form the desired shape
of the swarm.
VOLUME 8, 2020 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ 95667
A. Tahir et al.: Comparison of Linear and Nonlinear Methods for Distributed Control of a Hierarchical Formation of UAVs
One of the main problems in distributed control of the for-
mation is the lack of perception of the dynamics of the whole
swarm in each partially distributed controller that is being
executed in each drone of the swarm, i.e. top–down approach.
In most of the state-of-the-art focusing on distributed con-
trollers, the local controller of each drone, unaware of the
behaviour and constraints of the swarm’s higher levels of hier-
archy, only manipulates its actuators based on its observations
and local dynamic models of a subset of all nodes constituting
the swarm, i.e. bottom–up approach. These techniques try
to optimize and refine the partial behaviour of the system
that subsequently contributes to general emergent behaviour.
However, manipulating only the actuators in each node based
on partial dynamic models cannot guarantee optimal or even
near-optimal desired top-level behaviour of the system, which
is the final goal of such control design. Therefore, this work
aims to propose a strategy to reach this goal. In other words,
it addresses the problem based on the overall dynamics of the
whole swarm and the local dynamics of a node in order to
implement the partial controller of each drone.
The main contributions of this paper can be summarized as
follows:
•Reducing the complexity of the large system of swarm-
ing drones by implementing the decentralized control
systems using leader–follower architecture in which the
goal is to form the hierarchical states for the swarm
of drones that keep the desired formation using set-
points/offsets to avoid collisions with each other.
•Testing of distributed control designs based on Sliding
Mode Control (SMC) with Proportional-Derivative (PD)
and Linear Quadratic Regulator (LQR) with integral
action to guarantee the tracking performance in such
a way that the system’s settling time is reduced as
much as possible without introducing oscillations in the
response.
•Presenting the side by side comparative analysis based
on setpoint tracking, response and settling time, collision
avoidance, and computational costs for the proposed dis-
tributed control systems. Also, the testing of plant-model
mismatch and external disturbance inputs are examined.
To do this, two well-known control methods, i.e. SMC
PD and LQR integral action, are adapted and integrated that
are applied to each drone of a hierarchical swarm forma-
tion. Both control methods are responsible to fulfil the high-
level expectation from the hierarchical swarm formation, e.g.
translational positioning, and to fulfil the low-level expecta-
tion, e.g. the stability of angular positioning. More precisely,
in LQR integral action, both are done using a single constant
state feedback law. While in SMC PD, the overall control
system of each drone in a swarm is composed of two main
parts, i.e. inner and outer feedback loops. The inner loop
is responsible for angular positions of the drone while the
outer loop is responsible for its translational positions. The
outer loop observes the current translational pose of the drone
and tries to eliminate the lateral, longitudinal, and vertical
distance errors and generates a reference for pitch, roll, and
yaw angles respectively to be tracked by the inner loop that
specifies engine propulsion. The parameters of the controllers
change based on the location of each drone in the swarm. This
provides a flexible reconfigurable grid of sub-functionalities
demonstrated by individual drones contributing to the gen-
eral behaviour of the swarm. Each of the sub-functionalities
can be adjusted according to the expected behaviour from
the located drone and via manipulating the controller’s
parameters.
This paper is divided into seven sections. The motivation
and problem formulation is stated in section I. Related work
is discussed in section II. The dynamics of the model for
the considered system is presented in section III. The control
designing for the swarm of drones in a hierarchical manner
is described in section IV. The simulation setup and the per-
formance analysis are elaborated in section Vand VI respec-
tively. Lastly, concluding remarks are given in section VII.
II. RELATED WORK
In [11], Proportional Integral Derivative (PID) control as the
inner loop and parametric weighted control as the outer loop
of the controller for a swarm of four identical drones are
investigated. By changing a few parameters in the paramet-
ric weighed controller using four control input drivers, it is
shown that the behaviour of UAVs can be rapidly changed
in-field testing. However, each node has its own histogram
of visited areas, and synchronized information does not get
shared to help the swarm as a whole. In [12], PID control for
the inner loop and extended nonlinear dynamic inversion con-
trol for the outer loop in the context of autonomous formation
flight of three drones are proposed. Regardless of ambiguity
in response time efficiency and error measurements of the
system, the scalability and top–down/bottom–up influence
of dynamics on the controller are not comprehensively con-
sidered. In [13], for hovering synchronization in horizontal
and vertical planes using three drones, PD control as the
inner loop and multi-agent-based consensus control as the
outer loop are presented. A Type 2 system is considered in
order to perform the simulation tests. In [14], experiments
have been conducted to study the flight test for a quadcopter
using a PD control architecture. Also, the swarm behaviour
through a coordinated flight path for a leader drone with four
follower drones is highlighted. In [15], an event-triggered
distributed Model Predictive Control (MPC) technique for
formation control of three drones is studied, where each drone
shares information with its neighbours. In [16], a fuzzy logic
approach for task assignment and controlling a swarm of
agents, representing UAVs, in a decentralized environment
is examined, and the results of collision attempts and com-
pletion times between the distributed and centralized strate-
gies through simulations are compared. The central control
design is based on finite state machines, and the proposed
solution is simulated using the Unity3D engine. Furthermore,
in [17], SMC and integral SMC for nonlinear second-order
multi-agent systems with unknown nonlinear interactions are
95668 VOLUME 8, 2020
A. Tahir et al.: Comparison of Linear and Nonlinear Methods for Distributed Control of a Hierarchical Formation of UAVs
presented. However, the method lacks scalability in case of
increasing number of drones. In [18] and [19], a performance
analysis is considered, in a centralized environment with three
drones, using SMC for the inner control loop combined with
LQR control for the outer control loop. However, in [18],
the results for the proposed architecture lack the fast response
time, and in [19], the controllability matrix related to the
linear dynamics of the position controller does not have the
full row rank. Hence, the given set of equations is not feasible
for designing position control of xand yusing the proposed
method due to the lack of information provided.
III. DYNAMIC MODEL OF A DRONE
In this section, the dynamic model of a leader–follower
swarm of drones is examined. This model serves as a case
study to demonstrate the idea of the proposed control designs.
In the system under consideration, the swarm consists of a
hierarchical organization of leader–follower drones that are
responsible for tracking the desired trajectories as well as
for hovering at desired positions for given time periods. The
trajectory of each follower is defined based on the orientation
and actions of its respective leader.
The model of each node in the swarm is based on the
model of a quadcopter, i.e. a drone that has four propellers
with fixed pitch mechanically movable blades, as shown
in Fig. 1.
FIGURE 1. Kinematics of the quadcopter.
The major forces acting on the quadcopter are the gravity g
and the thrust Ti,i∈ {1,2,3,4}, of the propellers. In this
model, the inertial reference is the earth shown as (x,y,z)
that is the origin of the reference frame. The drone is assumed
to be a rigid body that has the constant mass symmetrically
distributed with respect to the planes (x,y), (y,z), and (x,z).
The two possible configurations, i.e. cross and plus, are used
for most of the quadcopter designs. The cross configuration
is considered as more suitable than the plus configuration
in case of aerial photography because it better keeps the
propellers out of the camera view.
The orientation of a quadcopter reference frame (x,y,z)
with respect to an inertial frame (x,y,z)0can be expressed
mathematically in a state variable form [20]–[22], where
translational and angular accelerations are given by
˙vx= −vzwy+vywz−gsin θ
˙vy= −vxwz+vzwx+gcos θsin φ
˙vz= −vywx+vxwy+gcos θcos φ−T
m
(1)
and
˙wx=1
Jx
(−wywz(Jz−Jy)+Mx−kwT
kMT
JmpMzwy)
˙wy=1
Jy
(−wxwz(Jx−Jz)+My−kwT
kMT
JmpMzwx)
˙wz=Mz
Jz
(2)
respectively. The thrust produced by each propeller Tiis
translated into a total thrust Tand the reactive torques Mi,
i∈ {x,y,z}, is affecting the rotations along the corresponding
axis. The Ji,i∈ {x,y,z}, is known as the moment of inertia
along the corresponding axis, and Jmp is the moment of inertia
of a motor with propeller. The angular velocities of pro-
pellers are assumed to be proportional to thrusts of propellers,
i.e. wj=kwT Ti,j∈ {x,y,z}. Similarly, the reactive moments
of propellers are assumed to be proportional to the thrust
of propellers, i.e. Mi=kMT Ti. Depending on the chosen
configuration, the propeller thrusts Tiwill generate different
thrust Tand torques Minamely
T
Mx
My
Mz
=
1 1 1 1
0−`0`
`0−`0
kMT −kMT kMT −kMT
T1
T2
T3
T4
(3)
for plus configuration and
T
Mx
My
Mz
=
1 1 1 1
√2
2`−√2
2`−√2
2`√2
2`
√2
2`√2
2`−√2
2`−√2
2`
kMT −kMT kMT −kMT
T1
T2
T3
T4
(4)
for cross configuration where `is a length of the fixed pitch
to mechanically movable blades [23]. The velocities corre-
sponding to Equations (1) and (2) are
˙x=vxcos ψcos θ+vy(−sin ψcos φ+cos ψsin θ
sin φ)+vz(sin ψsin φ+cos ψsin θcos φ)
˙y=vxsin ψcos θ+vy(cos ψcos φ+sin ψsin θsin φ)
+vz(−cos ψsin φ+sin ψsin θcos φ)
˙z=vxsin θ−vycos θsin φ−vzcos θcos φ
(5)
and
˙
θ=wycos φ−wzsin φ
˙
φ=wx+wysin φtan θ+wzcos φtan θ
˙
ψ=wy
sin φ
cos θ+wz
cos φ
cos θ
(6)
VOLUME 8, 2020 95669
A. Tahir et al.: Comparison of Linear and Nonlinear Methods for Distributed Control of a Hierarchical Formation of UAVs
respectively. The Equations (1)–(6) represent the complete
nonlinear model of a quadcopter, composed of 12 states,
4 inputs, and 12 outputs. More precisely,
x=vxvyvzwxwywzθ φ ψ x y z T(7)
is the state or system vector,
u=T MxMyMzT(8)
is the input or control vector,
y=x(9)
is the output (measured) vector. Furthermore, the reduced
state vector
xs=vxvyvzwxwywzT(10)
and the performance outputs
yp1∈ {x,y,z},(11)
yp2∈ {θ, φ, ψ},(12)
and
yp=x y z T(13)
are defined for future use.
Using standard linearization, nonlinear dynamic equations
can be converted into a set of standard linear equations to
examine the stability and controllability of the system as well
as to design an LQR integral control to compare with SMC
PD method. This yields,
˙
x=−gθgφ−T
m
Mx
Jx
My
Jy
Mz
Jz
wywxwzvxvy−vzT
(14)
y=x
that can further written into the standard state space form
˙
x=Ax+Bu
y=Cx+Du(15)
where A,B,C, and Dare known as the state or system
matrix, input or control matrix, output (measured) matrix, and
feedthrough matrix respectively. Correspondingly, x,u, and
yare known as the state or system vector, input or control
vector, and output (measured) vector as in Equations (7)–(9).
The system parameters are taken from [20] and illustrated
in Table 1.
TABLE 1. System parameters.
Refer to linear model Equation (15) and system param-
eters in Table 1, the system has 12 eigenvalues at the ori-
gin. Prior to any control design, the controllability matrix
B AB A2B. . . An−1Bis calculated and found to be of
rank 12. Thus, all the 12 states are controllable.
IV. CONTROL DESIGNS FOR THE SWARM OF DRONES
A hierarchical swarm of drones consisting of ten quad-
copters is considered in a leader–follower operational con-
figuration, as presented in Fig. 2. The states/nodes can be
seen to form clusters at each level of the hierarchy. The
distance between any two neighbouring nodes pand qis
defined as dp,qwhere p,q∈ {L,1–9}, and each node
is operated on its internal control system. The straight
arrows show the direction in which coordinate variables are
shared.
FIGURE 2. Organization of the considered swarm of drones.
The swarm in Fig. 2is composed of the main leader L
and multiple layers of sub-leaders forming interconnected
sub-clusters/swarms of different sizes. At any level of the
hierarchy, each drone is directly communicating with its
respective neighbours, i.e. with its immediate leader and fol-
lower, by providing its orientation information that results in
maintaining the distance between any two adjacent nodes in
the swarm [1]. The control architecture, in a swarm of drones,
is responsible for tracking the generated reference commands
in a stabilized manner while keeping a safe distance between
neighbouring drones to maintain the desired flight formation.
To form the swarm based on the partially distributed con-
troller on each node in such a system, the leader receives
known bounded reference commands,
r1∈ {xr,yr,zr}(16)
and
r=xryrzrT,(17)
that may or may not vary with time and the controller of
the follower node is dependent on its respective leader’s
orientation and movement. The translational positions xi,yi
and zi,i∈ {L,1–9}, are fed to the respective follower(s) as a
reference. On top of that, all followers have their local offsets
in order to avoid collisions with each other.
Due to the hierarchical nature of the architecture as a
whole, the main problem in the adjustment of the controller’s
design parameters is the dependence of each drone’s location
95670 VOLUME 8, 2020
A. Tahir et al.: Comparison of Linear and Nonlinear Methods for Distributed Control of a Hierarchical Formation of UAVs
on the other drones’ locations in the swarm. In other words,
deviations in the control design parameters of a drone might
cause significant unwanted changes in the other drones’ loca-
tions, especially in the case of the follower nodes, which can
be observed as oscillations. The parameters of the controller
are determined through testing of the overall dynamics of
the swarm. The goal of the control system is to improve the
tracking performance in such a way that the system’s settling
time is reduced as much as possible without introducing
oscillations in the response. To accomplish this, a nonlinear
SMC PD and a linear LQR integral action control design
methods are proposed.
A. SMC PD CONTROL SYSTEM
An SMC PD control scheme from [24] and [25] has been
adapted for this study and tailored for the leader–follower
tightly coupled formation flight. This technique has been
selected because of its high flexibility and reconfigurability
for different formations. Another advantage of using SMC is
that the system is insensitive to disturbances when it enters the
sliding surface. However, it leads to a chattering phenomenon
that is reduced by using proper switching gains in the satura-
tion function. Fig. 3shows the decision-making process of
each drone that is split into two feedback control blocks, i.e
outer and inner feedback loops.
FIGURE 3. Block diagram of SMC PD control design.
In the control design depicted in Fig. 3, SMC is tied
to the PD structure and the parameters can be adjusted
based on the dynamics of the swarm. The outer feedback
loop, section IV-A1, is responsible for the drone’s trajectory,
i.e. translational positions x,y, and z, whereas the inner
feedback loop, section IV-A2, is manipulating the control
variables of actuator thrust Tand the torques Mi.
1) TRAJECTORY CONTROL DESIGN
The internal structure of each drone’s trajectory tracking
control block is illustrated in Fig. 4. The angular errors,
e2∈ {θe, φe, ψe}, are generated using three PD controllers
that are fed as inputs to the angular position stabilization
block.
FIGURE 4. The trajectory control structure.
From Fig. 4, the feedback law
e2=yp2−(KP·(r1−yp1)+KD
d
dt (r1−yp1)) (18)
is obtained. The signals yp1and yp2are given in Equa-
tions (11) and (12) respectively. The parameters KPand KD
are the proportional and derivative gains respectively of the
PD controller.
To reduce the overshoots of translational trajectories x,y,
and z, a lead compensator is connected in series with the
PD control block. This compensation is included in level 2
(Fig. 2, nodes 1 and 2) for yposition, and in levels 3–4 (Fig. 2,
nodes 3–9) for xand ypositions. Mathematically, the transfer
function
G(s)=s−zz
s−zp
(19)
of the lead compensator, where zzis the zero, and zpis the pole
satisfying 0 <zz<zp. The parameters used in this study are
given in Appendix A.
2) ANGULAR POSITION CONTROL DESIGN
SMC control design is used for generating the actuator thrust
T, to drive each drone in a swarm formation at the desired
altitude, and the torques Mi, for the stabilization of the orien-
tation angles, as pictured in Fig. 5. This practice reduces the
order of the state equations and provides a quick response.
FIGURE 5. The angular position control structure.
Consider a time varying surface sa(t) in the state space Rn
by the scalar equation sa(x,t)=0. The sliding surface is
chosen as
sα=cαeα+ ˙eα(20)
where α∈ {T,Mx,My,Mz},eαis the tracking error, and cα
is a strictly positive constant [26]. The four control laws have
been proposed here for thrust Tand torques Mi;Mx(along
x-axis, i.e. for roll φ), My(along y-axis, i.e. for pitch θ), and
Mz(along z-axis, i.e. for yaw ψ). The chattering free control
law is given by
uα= ˆuα−Kαsat( sα
βα
) (21)
where ˆuαand Kαare the control design parameters. The ˆuα
indicates the motion of the state trajectory along the sliding
surface sα, and Kαrepresents the maximum controller output.
The sat(·) is the saturation function defined as
sat( sα
βα
)=
sα
βα
,|sα
βα| ≤ 1
sgn( sα
βα
),otherwise (22)
where βαis a constant that defines the thickness of the bound-
ary layer. The saturation function is used to smooth out the
VOLUME 8, 2020 95671
A. Tahir et al.: Comparison of Linear and Nonlinear Methods for Distributed Control of a Hierarchical Formation of UAVs
control discontinuity in a thin boundary layer neighbouring
the switching surfaces. To accomplish a trade-off between
tracking precision and robustness to unmodeled dynamics,
this function allocates a low pass filter to the local dynamics
of the variable sα, thus eliminating chattering phenomena.
The control laws are much more efficient when the mini-
mum amount of boundary layer is introduced. The equivalent
control ˆuα,
ˆuT= − mcT
cos θcos φ(vxsin θ−vycos θsin φ−vz
cos θcos φ)+m(−vywx+vxwy
+gcos θcos φ)
ˆuMx= −cxJx(wx+wysin φtan θ+wzcos φtan θ)
+wywz(Jz−Jy)
ˆuMy= − cyJy
cos φ(wycos φ−wzsin φ)+wxwz
(Jx−Jz)
ˆuMz= −czJz(wytan φ+wz),
(23)
is obtained using nonlinear model Equations (1)–(6). The
control law presented in Equation (21) ensure both the
reachability condition, sα˙sα<0, and the sliding condi-
tion, ˙
Vα=1
2d
dt s2
α, using Lyapunov’s stability analysis that is
described below.
Consider a positive definite scalar function for the thrust T
and torques Mias
Vα=1
2s2
α(24)
and its derivative leads to
˙
Vα=sα˙sα(25)
where ˙sα,
˙sT=cT(vxsin θ−vycos θsin φ−vzcos θcos φ)
−(−vywx+vxwy+gcos θcos φ−uT
m)
cos θcos φ
˙sMx=cx(wx+wysin φtan θ+wzcos φtan θ)
+1
Jx
(−wywz(Jz−Jy)+uMx)
˙sMy=cy(wycos φ−wzsin φ)+cos φ
Jy
(−wxwz
(Jx−Jz)+uMy)
˙sMz=cz(wy
sin φ
cos θ+wz
cos φ
cos θ)+uMz
Jz
cos φ
cos θ,
(26)
is obtained using nonlinear model Equations (1)–(6). Keeping
the control law from Equation (21) in Equation (25),
˙
V= −η|sα|<0 (27)
is obtained where η=1, a strictly positive constant. It is
proved from Equation (27) that all the system trajectories
point towards the sliding surface sαin a finite time. To be
specific, once on the surface, the system trajectories stay
on the surface. In a nutshell, sliding condition makes the
surface an invariant set. Some dynamic disturbances can
be tolerated while still keeping the surface an invariant set.
The conditions are verified by sαand therefore the inner
closed-loop system is guaranteed to be stable. The outer PD
loop is not taken into account in the analysis, and it can affect
the stability of the whole system. However, a PD loop has
good stability properties, and SMC cancels to some extent
the nonlinearity of the system, so it is unlikely that the PD
will cause instability. The parameters used in this study are
given in Appendix A.
B. LQR INTEGRAL ACTION CONTROL SYSTEM
LQR is a simple control design method that can handle a
multivariable system and is known to provide good robust-
ness properties in the full state feedback case [27]. Therefore,
a standard LQR augmented with integral action is tested, sim-
ilar to what was done in [28] and [29], for the leader–follower
tightly coupled formation flight. Each drone has its control
system, with the setpoints coming from the above level. Based
on linear model Equation (15), LQR provides an optimal
full state feedback controller that is applied on the nonlinear
model Equations (1)–(6) for each quadcopter in the swarm
under consideration.
Considering the linear model Equation (15), the control
input uminimizes the quadratic cost function
J(u)=Z∞
0(˙
xT
aQ˙
xa+˙
uTR˙
u)dt (28)
where
xa=Zt
0
e(τ)TdτxTT
,(29)
the states xand the control vector uare defined in Equa-
tions (7) and (8) respectively. Qis a positive semi-definite
matrix that defines the weights on states, whereas Ris
a positive definite matrix that weights the control inputs.
Furthermore,
e=r−yp(30)
is the error term, and rfrom Equation (17) is the reference
for ypthat is defined in Equation (13). To get the desired
response, the controller can be tuned by changing the (diag-
onal) entries in the Qand Rmatrices. The state feedback law
u=Ki
se−Kpx(31)
gives the four control inputs, i.e. thrust Tand torques Mi;Mx
(along x-axis, i.e. for roll φ), My(along y-axis, i.e. for pitch
θ), and Mz(along z-axis, i.e. for yaw ψ). The controller uses
two separate gain matrices Kpand Kiwhere Kpis the state
feedback gain, and Kiis the integral gain. The feedback law
is shown in Figure 6.
FIGURE 6. Block diagram of LQR integral control design.
The tuning parameters Q and R used in this study are given
in Appendix B.
95672 VOLUME 8, 2020
A. Tahir et al.: Comparison of Linear and Nonlinear Methods for Distributed Control of a Hierarchical Formation of UAVs
V. SIMULATION SETUP
The system architecture of the swarm based on a
leader–follower hierarchical formation that consists of four
levels of hierarchy in a rigid environment, as shown in Fig. 2,
is simulated in SimulinkrMATLAB. Level one consists of
the leader L, level two of the followers 1–2, level three of
the followers 3–7, and level four of the followers 8–9. The
control systems of each drone follow the reference commands
that are defined by the pre-specified formation and its respec-
tive leader. In the design of the MISO nonlinear SMC PD
control technique, different gain adjustments are performed
to reconfigure the controller based on the dynamics of the
fleet whereas in the design of the MIMO optimal control
using LQR integral action, the same weights are used in the
controllers of each drone. The initial launching position xof
each drone is set to 1m away from its respective neighbouring
drones, and the further data for simulation is shown in Table 2.
In all simulations, the sampling time of 0.01s is used for all
the figures.
TABLE 2. Initial positions (m) and offsets (m) of drones used in
simulation.
Three different scenarios are used to evaluate the pro-
posed distributed controller in case of similar movement of
the swarm of drones. The reference commands, specified
in Table 3, are given to the leader drone, and all the followers
track its position with safe distance strategy using offsets,
stated in Table 2, in order to avoid collisions with each other.
In the first scenario, the tracking performances of the swarm-
ing drones is examined using a step change reference, see
Fig. 7(a). In the second scenario, a circular reference based
on lemniscate of gerono equations, also known as 8 curve,
is used, and the results are displayed in Fig. 7(b). In the third
scenario, another circular reference based on Archimedean
spiral equations, also known as an arithmetic spiral, is tracked
in Fig. 7(c).
VI. RESULTS
The 3D response of the formation for all three reference
models, as presented in Fig. 7, depicts that the leader drone
tracks the reference signal and the followers track the output
of the associated leader in steady hierarchical manner while
keeping the desired distance with its neighbouring peer(s),
i.e. collision avoidance using offsets mentioned in Table 2.
The corresponding trajectory errors w.r.t. related reference
trajectories using both control designs, depicted in Fig. 8,
show the average of the error data points for the trajectories
of the swarm which is referred to as arithmetic mean. It is
understandable from the graphs that the performance of the
SMC PD control design is seen better in comparison to the
LQR integral action control strategy in most of the drones
due to its low average error.
Only the results for the step change reference model are
illustrated in Fig. 9for the sake of brevity. Fig. 9(a)–(e)
describe the control actions, thrust Tand torques Mi,
of drones in order to lift, fly, and track the given trajectory.
The performance outputs x,y, and z, in Fig. 9(f)–(h), have
rapid response and settling time w.r.t. their setpoints using
the SMC PD control design in contrast to the LQR integral
action strategy. Also in Fig. 9(i)–(l), the sliding surfaces of
the SMC PD control design converge to zero in a finite time
by driving the respective system states to reach their sliding
surfaces.
For all reference trajectories, the total kinetic energy KE
produced by the swarm due to its motion versus the total
stored potential energy PE is pictured in Fig. 10. Total energy
possessed and held is calculated as KE =0.5mv2and
PE =mgh respectively. These energies relate how much
work is conserved in the process of the swarm movement.
Each drone in the swarm depends fully on the variable speed
of the motors driving the propellers. This variation, along
with the rpm thrust and control in propeller/motor speed,
provides the quadcopter with all of the essential control
to fly. Flying with payload affects the control and the sta-
bility of a drone. The performance of the swarm in terms
of work done and energy stored w.r.t. time is calculated by
EKE =KEmax, LQR int −KEmax, SMC PD
KEmax, LQR int +KEmax, SMC PD ×100% (32)
and
EPE =tPE, LQR int −tPE, SMC PD
tPE, LQR int ×100% (33)
respectively, where EKE and EPE are defined as performance
indices of kinetic and potential energies respectively. The
KEmax is represented as the peak value of the kinetic energy
TABLE 3. Test sequences for simulations. The Tis defined as a ramp function with the slope of 1.
VOLUME 8, 2020 95673
A. Tahir et al.: Comparison of Linear and Nonlinear Methods for Distributed Control of a Hierarchical Formation of UAVs
FIGURE 7. Tracking performance of swarming drones, using SMC PD, for the reference models.
FIGURE 8. Mean value of Euclidean distance from setpoints for the two different control designs.
TABLE 4. Overall system performance indices, defined in Equations (32) and (33).
whereas tPE is the settling time of the potential energy. All
the obtained values, using MATLAB command stepinfo, are
given in Table 4. For the reference commands, the corre-
sponding settling time of the swarm is improved by 47.2%,
42.3%, and 42.8% respectively using the SMC PD control
design in comparison to the LQR integral action strategy. As a
consequence of this, the position and velocity errors converge
much quickly and smoothly. Hence, the work is performed
faster as the energy is transmitted or used in lesser time.
The distance between any two neighbouring nodes pand q
in Fig. 2is calculated by
dp,q=dq,p=q(qx−px)2+(qy−py)2+(qz−pz)2(34)
where its minimum distance dmin should be greater than
zero in order to hold the collision avoidance phenomenon.
The results of nominal and minimum distances, dnom and
dmin respectively, are presented in Tables 5–7. It is evident
from the results that both techniques fulfil the collision
avoidance constraint by maintaining the desired separations
from the respective nodes. There is a lower risk of drone’s
collisions using the SMC PD control design for all the
reference commands. On the other hand there is a high
risk of drone’s collisions using distance separation of 1m
between the respective neighbours with the LQR integral
action control strategy for spiral reference model. Therefore,
in this case, it is recommended to set the offset greater than
2m between any two neighbouring nodes in order to avoid
collisions with each other.
Using derivatives of Equation (28), the cost com-
putations that are minimized by control inputs uare
given in Table 8. From the cost computations of
proposed controllers, it is asserted that the overall
hierarchical swarm has a lower total cost with the
SMC PD control design, i.e. 2.73E+07, 1.52E+08, and
2.29E+08 for the corresponding references whereas with
the LQR integral action control strategy, the correspond-
ing total computations are 3.29E+07, 1.58E+08, and
2.58E+08.
A. PLANT-MODEL MISMATCH
In this section, the swarm of drones is implemented with a
plant-model mismatch to observe the impact on the proposed
control designs in the hierarchical levels of the swarm for
the parametric and input uncertainties. For better clarity,
the simulation is only conducted for the step change reference
commands.
95674 VOLUME 8, 2020
A. Tahir et al.: Comparison of Linear and Nonlinear Methods for Distributed Control of a Hierarchical Formation of UAVs
FIGURE 9. Results with the step change reference model.
In the case of parametric uncertainties, the mass m,
mL=0.9kg
mf1,f2 =1kg
mf3–f7 =1.1kg
mf8,f9 =1.2kg,
(35)
is set for the quadcopter(s) on each level of the swarm. This
is to be considered here that for the SMC PD control design,
the mass should be greater than 0.8kg for the stability of
its altitude. In the case of input uncertainties, the closed-
loop system of each quadcopter has different thrust input
Treal =T×K, where Tis the output of the controllers and
VOLUME 8, 2020 95675
A. Tahir et al.: Comparison of Linear and Nonlinear Methods for Distributed Control of a Hierarchical Formation of UAVs
TABLE 5. The nominal and minimum distances (m) between neighbouring drone(s) in the swarm, with step change.
TABLE 6. The nominal and minimum distances (m) between neighbouring drone(s) in the swarm, with 8 curve.
95676 VOLUME 8, 2020
A. Tahir et al.: Comparison of Linear and Nonlinear Methods for Distributed Control of a Hierarchical Formation of UAVs
TABLE 7. The nominal and minimum distances (m) between neighbouring drone(s) in the swarm, with arithmetic spiral.
TABLE 8. LQR cost, R˙
eTQe˙
edt +R˙
xTQx˙
xdt +R˙
uTR˙
udt, where Qeand Qxare diagonal matrices with corresponding weights from Q.
Treal is the actual thrust obtained, and the gain K,
KL=0.9
Kf1,f2 =0.8
Kf3–f7 =0.7
Kf8,f9 =0.6,
(36)
is set for the quadcopter(s) on each level of the swarm. It is
to be noted here that for the SMC PD control design, the gain
should be less than 1 for the stability of its altitude. Rest of
the data for both tests is given in Tables 1–3, and the acquired
results are shown in Fig. 11 and Fig. 12. It is presented,
in Fig. 11(c) and Fig. 12(c) that when using the SMC PD
control design, the zpositions have undershoots in all the
levels of hierarchical formation compared to its nominal case.
Also in the altitudes, the acceptable tracking errors w.r.t. their
settling points exist and may increase with the increase in the
FIGURE 10. Kinetic vs. potential energies of the swarm.
mass of the quadcopter(s) whereas, the LQR integral action
control strategy is very much insensitive to such disturbances.
VOLUME 8, 2020 95677
A. Tahir et al.: Comparison of Linear and Nonlinear Methods for Distributed Control of a Hierarchical Formation of UAVs
FIGURE 11. Results with the step change reference model, parametric uncertainties.
FIGURE 12. Results with the step change reference model, input uncertainties.
FIGURE 13. Results with the step change reference model, external disturbance inputs.
B. EXTERNAL DISTURBANCE INPUTS
In this section, a step change in thrust, Treal =T+S,
S=0−→ −1N, is added as an external disturbance
to the quadcopter(s). This is to be considered here that for
the SMC PD control design, the external disturbance should
be less than 0N for the stability of its altitude. On each
level of the swarm, the external disturbance is added at
time t= {15,20,25,30}s respectively, and the simula-
tion’s running time tis 40s. Rest of the data for this test is
given in Tables 1–3, and the obtained results are illustrated
in Fig. 13. The results exhibit that the proposed nonlinear
and linear, i.e. the SMC PD and the LQR integral action
respectively, control designs provide the efficient stability of
the closed-loop system and reject the disturbances quite well.
For a clear depiction, the zpositions, {zL,zf1,f2,zf3–f7,zf8,f9},
are plotted at the heights of {10,11,12,13}m respectively
in Fig. 13(c) although all setpoints are set to 10m. Similar
to section VI-A, the performance of the LQR integral action
control design is seen better due to the elimination of the
tracking errors w.r.t. their settling points in the altitudes. How-
ever, fast response time is achieved using the SMC PD control
strategy.
VII. CONCLUDING REMARKS
In this paper, the model of a swarm of drones that are arranged
in the leader–follower architecture having hierarchical lev-
els for a tightly coupled formation flight is proposed. This
architecture is modular in the sense that it divides the overall
system into four layers forming a hierarchical swarm: top-
level leader drone, mid-level drones, and end-level followers.
95678 VOLUME 8, 2020
A. Tahir et al.: Comparison of Linear and Nonlinear Methods for Distributed Control of a Hierarchical Formation of UAVs
For the evaluation of the control architecture, the distributed
nonlinear and linear control methodologies comprising of
SMC PD and LQR integral action designs respectively are
applied on each drone. The control systems are tested for
the tracking of a step change, an 8 curve, and an arith-
metic spiral. The side by side comparative analysis based
on setpoint tracking, response and settling time, collision
avoidance, and computational costs is presented. It is evident
from the results that both the proposed control designs are
effective w.r.t. their nominal case, plant-model mismatch, and
external disturbance inputs. In the nominal case, the SMC
PD control design avoids a large deviation of the individual
drone from its optimal navigation track due to its optimized
transient behaviour in comparison to the LQR integral action
technique. Also, the nominal settling time of the swarm is
improved by 44% on average. Since a slight change in the
system’s dynamics can affect the overall system’s state due to
hierarchical and tightly coupled nature of the system, both the
proposed approaches are tested for the cases of plant-model
mismatch and external disturbance inputs. It is confirmed
from the results of these cases that the performance using the
LQR integral action control design is better than that using the
SMC PD control strategy w.r.t. their settling time. One reason
for the LQR to perform better in this case is that it includes
integral action that is good for the elimination of unknown
disturbances and model errors. Contrarily, the SMC uses the
model in the calculation of the feedback law and is thus
dependent on its PD structure while the LQR is a constant
feedback of the real states. Satisfactory performances are
achieved using both the proposed distributed control method-
ologies for the drones that are arranged in the leader–follower
architecture having hierarchical levels for a tightly cou-
pled formation flight. Both the approaches are scalable and
effective as well as develop the versatility for an emerging
area of UAVs.
APPENDIX
A. SMC PD CONTROL SYSTEM
Kα≥ { m
cosθcosφ,Jx,Jy
cosφ,Jzcosθ
cosφ}
βα= {0.1,0.1,0.1,0.1}
cα= {0.25,4,8,0.5}
TABLE 9. Control gains for the PD blocks.
B. LQR INTEGRAL ACTION CONTROL SYSTEM
Gr= {Gxr,Gyr,Gzr}={1,1,1
0.57s+1}
Q=diag3,4.5,6000,1080,1080,1080,180,180,
180,0.5,0.75,1000,15,22.5,30000
R=I4
ACKNOWLEDGMENT
The authors would like to thank FinELib consortium, Finland
for the APC funding, and Prof. H. Toivonen, Laboratory of
Process and Systems Engineering, Åbo Akademi University,
Turku, Finland, for discussions related to this article.
REFERENCES
[1] A. Tahir, J. Böling, M.-H. Haghbayan, H. T. Toivonen, and J. Plosila,
‘‘Swarms of unmanned aerial vehicles—A survey,’’ J. Ind. Inf. Integr.,
vol. 16, Oct. 2019, Art. no. 100106.
[2] P. Wang and F. Hadaegh, ‘‘Coordination and control of multiple
microspacecraft moving in formation,’’ J. Astron. Sci., vol. 44, no. 3,
p. 315–355, 1996.
[3] D. Galzi and Y. Shtessel, ‘‘UAV formations control using high order sliding
modes,’’ in Proc. Amer. Control Conf., 2006, p. 4249.
[4] B. Yun, B. M. Chen, K. Y. Lum, and T. H. Lee, ‘‘Design and imple-
mentation of a leader-follower cooperative control system for unmanned
helicopters,’’ J. Control Theory Appl., vol. 8, no. 1, pp. 61–68, Feb. 2010.
[5] M. Lewis and K. Tan, ‘‘High precision formation control of mobile robots
using virtual structures,’’ Auto. Robots, vol. 4, no. 4, p. 387–403, Oct. 1997.
[6] T. Paul, T. R. Krogstad, and J. T. Gravdahl, ‘‘Modelling of UAV formation
flight using 3D potential field,’’ Simul. Model. Pract. Theory, vol. 16, no. 9,
pp. 1453–1462, Oct. 2008.
[7] Z. Chao, S.-L. Zhou, L. Ming, and W.-G. Zhang, ‘‘UAV formation flight
based on nonlinear model predictive control,’’ Math. Problems Eng.,
vol. 2012, pp. 1–15, Feb. 2012.
[8] T. Balch and R. C. Arkin, ‘‘Behavior-based formation control for mul-
tirobot teams,’’ IEEE Trans. Robot. Autom., vol. 14, no. 6, pp. 926–939,
Dec. 1998.
[9] J. R. T. Lawton, R. W. Beard, and B. J. Young, ‘‘A decentralized approach
to formation maneuvers,’’ IEEE Trans. Robot. Autom., vol. 19, no. 6,
pp. 933–941, Dec. 2003.
[10] D. Bennet and C. McInnes, ‘‘Verifiable control of a swarm of unmanned
aerial vehicles,’’ Proc. Inst. Mech. Eng., G, J. Aerosp. Eng., vol. 223, no. 7,
p. 939–953, 2009.
[11] S. Engebraten, K. Glette, and O. Yakimenko, ‘‘Field-testing of high-level
decentralized controllers for a multi-function drone swarm,’’ in Proc. IEEE
14th Int. Conf. Control Autom. (ICCA), Jun. 2018, p. 379.
[12] T. F. K. Cordeiro, H. C. Ferreira, and J. Y. Ishihara, ‘‘Non linear controller
and path planner algorithm for an autonomous variable shape formation
flight,’’ in Proc. Int. Conf. Unmanned Aircr. Syst. (ICUAS), Jun. 2017,
p. 1493.
[13] B. Niemoczynski, S. Biswas, J. Kollmer, and F. Ferrese, ‘‘Hovering syn-
chronization of a fleet of quadcopters,’’ in Proc. 7th Int. Symp. Resilient
Control Syst. (ISRCS), Aug. 2014, p. 1–5.
[14] S. Srigrarom, H. X. Lin, Z. Y. Saw, J. Zhang, and C. H. Lim, ‘‘Design
and build of swarm quadrotor UAVs at UGS,’’ in Proc. 15th AIAA Aviation
Technol., Integr., Oper. Conf., Jun. 2015, p. 238.
[15] Z. Cai, H. Zhou, J. Zhao, K. Wu, and Y. Wang, ‘‘Formation control of
multiple unmanned aerial vehicles by event-triggered distributed model
predictive control,’’ IEEE Access, vol. 6, pp. 55614–55627, 2018.
[16] P. Lucas, K. Loayza, and E. Pelaez, ‘‘A distributed control of movements
and fuzzy logic-based task allocation for a swarm of autonomous agents,’’
in Proc. IEEE Int. Conf. Fuzzy Syst. (FUZZ-IEEE), Jul. 2018, p. 1–8
[17] H. M. H. Abdoli, M. Najafi, I. Izadi, and F. Sheikholeslam, ‘‘Sliding
mode approach for formation control of multi-agent systems with unknown
nonlinear interactions,’’ ISA Trans., vol. 80, pp. 65–72, Sep. 2018.
[18] K. Choutri, M. Lagha, L. Dala, and M. Lipatov, ‘‘Quadrotors UAVs swarm-
ing control under leader-followers formation,’’ in Proc. 22nd Int. Conf.
Syst. Theory, Control Comput. (ICSTCC), Oct. 2018, p. 794.
[19] K. A. Ghamry and Y. Zhang, ‘‘Formation control of multiple quadrotors
based on leader-follower method,’’ in Proc. Int. Conf. Unmanned Aircr.
Syst. (ICUAS), Jun. 2015, p. 1037.
VOLUME 8, 2020 95679
A. Tahir et al.: Comparison of Linear and Nonlinear Methods for Distributed Control of a Hierarchical Formation of UAVs
[20] F. Šolc, ‘‘Modelling and control of a quadrocopter,’’ Adv. Mil. Technol.,
vol. 5, no. 2, p. 29–38, 2010.
[21] P. Gabrlik, V. Kriz, and L. Zalud, ‘‘Reconnaissance micro uav system,’’
Acta Polytechnica CTU Proc., vol. 2, no. 2, pp. 15–21, 2015.
[22] A. Basci, K. Can, K. Orman, and A. Derdiyok, ‘‘Trajectory tracking
control of a four rotor unmanned aerial vehicle based on continuous sliding
mode controller,’’ Elektronika ir Elektrotechnika, vol. 23, no. 3, pp. 12–19,
2017.
[23] Q. Quan, Introduction to Multicopter Design Control.Singapore: Springer,
2017.
[24] D. A. Mercado, R. Castro, and R. Lozano, ‘‘Quadrotors flight formation
control using a leader-follower approach,’’ in Proc. Eur. Control Conf.
(ECC), Jul. 2013, pp. 3858–3863.
[25] J. J. Castillo-Zamora, K. A. Camarillo-Gómez, G. I. Pérez-Soto, and
J. RodriGuez-ReséNdiz, ‘‘Comparison of PD, PID and sliding-mode posi-
tion controllers for V–Tail quadcopter stability,’’ IEEE Access, vol. 6,
pp. 38086–38096, 2018.
[26] J.-J. E. Slotine and W. Li, Appl. Nonlinear Control. Englewood Cliffs, NJ,
USA: Prentice-Hall, 1991.
[27] M. Safonov and M. Athans, ‘‘Gain and phase margin for multiloop LQG
regulators,’’ IEEE Trans. Autom. Control, vol. 22, no. 2, pp. 173–179,
Apr. 1977.
[28] Q. Ali and S. Montenegro, ‘‘Explicit model following distributed con-
trol scheme for formation flying of mini UAVs,’’ IEEE Access, vol. 4,
pp. 397–406, 2016.
[29] C. Masse, O. Gougeon, D.-T. Nguyen, and D. Saussie, ‘‘Modeling and
control of a quadcopter flying in a wind field: A comparison between LQR
and structured H∞control techniques,’’ in Proc. Int. Conf. Unmanned
Aircr. Syst. (ICUAS), Jun. 2018, pp. 1408–1417.
ANAM TAHIR received the B.S. degree in com-
puter engineering, the M.S. degree in electrical
engineering (major: control systems) from COM-
SATS University, Islamabad, Pakistan, and the
Master of Engineering degree in autonomous mar-
itime operations from the NOVIA University of
Applied Sciences, Turku, Finland. She is currently
pursuing the Ph.D. degree with the Department
of Future Technologies, Faculty of Science and
Engineering, University of Turku, Finland. Her
research interests include autonomous systems, adaptive and nonlinear con-
trol, modeling, and simulation of dynamical systems.
JARI M. BÖLING received the M.Sc. degree in
chemical engineering and the Ph.D. degree in
control engineering from Åbo Akademi Univer-
sity, Turku, Finland, in 1994 and 2001, respec-
tively. From 2003 to 2004, he was a Postdoctoral
Researcher with the University of California Santa
Barbara, USA. Since 2005, he has been a Senior
Lecturer of control engineering with the Faculty
of Science and Engineering, Åbo Akademi Uni-
versity. His research interests include system iden-
tification, adaptive control, machine learning, modeling and simulation of
dynamical systems.
MOHAMMAD-HASHEM HAGHBAYAN
(Member, IEEE) received the B.A. degree in com-
puter engineering from the Ferdowsi University
of Mashhad, the M.S. degree in computer archi-
tecture from the University of Tehran, Iran, and
the Ph.D. degree (Hons.) from the University of
Turku, Finland. Since 2018, he has been a Post-
doctoral Researcher with the Department of Future
Technologies, Facultyof Science and Engineering,
University of Turku. His research interests include
machine learning, autonomous systems, high-performance energy-efficient
architectures, and on-chip/fog resource management.
JUHA PLOSILA (Member, IEEE) is currently a
Full Professor in autonomous systems and robotics
with the Department of Future Technologies, Fac-
ulty of Science and Engineering, University of
Turku (UTU), Finland, and the Ph.D. degree in
electronics and communication technology from
UTU, in 1999. He is the Head of the EIT Digital
Master Program in embedded systems with the
EIT Digital Master School, European Institute of
Innovation and Technology, and represents UTU
in the Node Strategy Committee of the EIT Digital Helsinki/Finland node.
He has a strong research background in adaptive multiprocessing systems
and platforms, and their design, including, specification, development and
verification of self-aware multiagent monitoring and control architectures for
massively parallel systems, machine learning and evolutionary computing-
based approaches, as well as application of heterogeneous energy efficient
architectures to new computational challenges in the cyber-physical systems
and internet-of-things domains, with a recent focus on fog/edge computing
(edge intelligence), and autonomous multidrone systems.
95680 VOLUME 8, 2020