Genetic Optimization-Based Consensus Control of Multi-Agent 6-DoF UAV System

A consensus control law is proposed for a multi-agent system of quadrotors with leader–follower communication topology for three quadrotor agents. The genetic algorithm (GA) is the proposed optimization technique to tune the consensus control parameters. The complete nonlinear model is used without any further simplifications in the simulations, while simplification in the model is used to theoretically design the controller. Different case studies and tests are done (i.e., trajectory tracking formation and switching topology) to show the effectiveness of the proposed controller. The results show good performance in all tests while achieving the consensus of the desired formations.


Introduction
In the last decade, unmanned aircrafts (UAs) have attracted researchers in different technical and scientific communities. UAs have a wide range of applications in various fields, such as agriculture, healthcare, and entertainment. Due to the great variety of UA types, UAs then can be classified by their application, range, altitude, endurance, vehicle type, and size. Many UA controlling missions are difficult, and some are impossible to accomplish using only a single agent UA, but instead a cooperative control must be designed for multiple agents to accomplish these missions. Interest in cooperative control of multi-agent systems (MASs) has recently increased, because of its broad applications, such as fire detection [1]. A literature survey [2] summarized some of the potential applications of the multi-agent system, such as large object transportation, which was done in the GRASP laboratory at the University of Pennsylvania, surveillance and searching of objects, quadrotor localization in outdoor environment, which was done at the Czech Technical University, and object transportation missions [3]. There are many cooperation control problems, such as consensus control, formation control, flocking control, and so on. Another important research field is optimization, which has attracted many researchers due to the advantages provided by optimization, especially with complex and nonlinear systems. There has been much research in this field with different optimization techniques, such as genetic algorithm (GA) optimization [4,5], particle swarm optimization (PSO) [6], ant colony optimization (ACO) [7], as well as hybridization of two or more techniques [8,9].
Consensus problem is closely related to formation control (forming a specific geometrical shape, which is discussed next), flocking (a behavior presented when a group of birds are foraging or inflight), and distributed estimation [11]; thus, it is an important and essential problem in the field of cooperative control. Consensus means reaching an agreement between multiple agents to achieve a common final state by communicating with each other through a communication network [12]. Figure 1 shows the consensus problem for four agents in their elevations only, where the four agents on the left side have different elevations above the ground (i.e., h , h , h , and h ) before applying the consensus control algorithm, while the agents on the right hand side present the agents after applying the consensus controller and reaching consensus in the elevation (i.e., h).

Formation Control
The formation control problem requires that all the agents reach a specific geometrical formation; thus, formation control differs from consensus by adding a distance to achieve the geometrical shape by all the agents; therefore, the formation control can be varied into consensus control by making the additional distance equal to zero. There are two cases for the formation control problem according to information known to agents. The first case is where the desired formation and its location are known for the team agents (i.e., centralized case). The second case is where the desired formation is known for the agents, while its location must be achieved by the team agents after negotiation [13]. Figure 2 shows five agents before and after applying formation control. In the initial positions, the agents are randomly distributed in two dimensions, and the heights of the agents are not taken into consideration. A formation controller is applied to achieve a triangle geometry shape, which is presented in the final formation.
Consensus and formation control algorithms can be categorized into leaderless and leaderfollower depending on how the agents are connected in the communication network represented using graph theory [14].

Formation Control
The formation control problem requires that all the agents reach a specific geometrical formation; thus, formation control differs from consensus by adding a distance to achieve the geometrical shape by all the agents; therefore, the formation control can be varied into consensus control by making the additional distance equal to zero. There are two cases for the formation control problem according to information known to agents. The first case is where the desired formation and its location are known for the team agents (i.e., centralized case). The second case is where the desired formation is known for the agents, while its location must be achieved by the team agents after negotiation [13]. Figure 2 shows five agents before and after applying formation control. In the initial positions, the agents are randomly distributed in two dimensions, and the heights of the agents are not taken into consideration. A formation controller is applied to achieve a triangle geometry shape, which is presented in the final formation.
Consensus and formation control algorithms can be categorized into leaderless and leader-follower depending on how the agents are connected in the communication network represented using graph theory [14].

Optimization Techniques
Finding the optimum performance index by tuning the controller parameters without any constraint is the purpose of the optimization techniques in this paper; therefore, the optimization technique used must be unconstrained. There are several methods to solve an unconstrained minimization problem, which can be classified as nongradient methods (e.g., random search method, grid search method, simplex method, etc.) and gradient methods (e.g., Fletcher-Reeves method, Newton's method, Marquardt method, etc.).
For simple problems that involve a small number of variables, the nongradient methods are efficient, while the gradient methods require the first and sometimes the second derivative of the objective function (performance index). Both methods are not efficient in the case of this work due to the number of variables available in the tuning process and the complexity gained by getting the first or second derivative of the performance index used. Therefore, the modern optimization techniques are the best choice for the tuning and optimization problem of the proposed controller.
There is a wide range of modern methods available, such as genetic algorithms, simulated annealing, PSO, ACO, etc., which can be used in the tuning process. Due to the ability of the GA to avoid local optimal solutions, it was chosen as the optimization method to tune the parameters of the proposed controller.

Genetic Algorithm
The genetic algorithm (GA) is categorized as an evolutionary algorithm that is suitable in searching for a global solution. Inspiration in GA development comes from natural genetics working principles. Three main operators are used at each step to produce the next generation from the current one. These three operators are selection, crossover, and mutation, which are briefly explained in the following [4]: (1) Selection: The selection operation forms a mating pool for the next generation by selecting genes (parents) from the existing generation.
(2) Crossover: The crossover process combines two of the parents to form the next generation (children).
(3) Mutation: The mutation forms new children by applying random changes to the parents.
A concise diagram for the GA steps is shown in Figure 3. In this paper, the MATLAB function ga was used in the optimization and tuning processes.

Optimization Techniques
Finding the optimum performance index by tuning the controller parameters without any constraint is the purpose of the optimization techniques in this paper; therefore, the optimization technique used must be unconstrained. There are several methods to solve an unconstrained minimization problem, which can be classified as nongradient methods (e.g., random search method, grid search method, simplex method, etc.) and gradient methods (e.g., Fletcher-Reeves method, Newton's method, Marquardt method, etc.).
For simple problems that involve a small number of variables, the nongradient methods are efficient, while the gradient methods require the first and sometimes the second derivative of the objective function (performance index). Both methods are not efficient in the case of this work due to the number of variables available in the tuning process and the complexity gained by getting the first or second derivative of the performance index used. Therefore, the modern optimization techniques are the best choice for the tuning and optimization problem of the proposed controller.
There is a wide range of modern methods available, such as genetic algorithms, simulated annealing, PSO, ACO, etc., which can be used in the tuning process. Due to the ability of the GA to avoid local optimal solutions, it was chosen as the optimization method to tune the parameters of the proposed controller.

Genetic Algorithm
The genetic algorithm (GA) is categorized as an evolutionary algorithm that is suitable in searching for a global solution. Inspiration in GA development comes from natural genetics working principles. Three main operators are used at each step to produce the next generation from the current one. These three operators are selection, crossover, and mutation, which are briefly explained in the following [4]: (1) Selection: The selection operation forms a mating pool for the next generation by selecting genes (parents) from the existing generation. (2) Crossover: The crossover process combines two of the parents to form the next generation (children). (3) Mutation: The mutation forms new children by applying random changes to the parents.
A concise diagram for the GA steps is shown in Figure 3. In this paper, the MATLAB function ga was used in the optimization and tuning processes.

Literature Survey
There is a large variation of research on cooperative control techniques for MASs. Getting an agreement between the agents (i.e., consensus) was the aim of some of the researchers, while others intended to obtain a specific formation for the MAS under some constraints (i.e., communication topology, leader/leaderless, . . . , etc.). This depends on the application where the control technique is Sensors 2020, 20, 3576 4 of 31 used. Some of the recent research, such as authors in [15], proposed a distributed adaptive controller for a class of an under-actuated Lagrangian system to track a dynamic leader and control the actuated variables, while being subjected to parameter uncertainties and external disturbance with or without actuator faults. In [16], the authors studied the distributed consensus problem for a team of quadrotors with fixed and switching topologies after transforming the quadrotor model to a simplified one using feedback linearization technique. Furthermore, others in [17] presented a two-layer hierarchical model predictive control (MPC) to stabilize a quadrotors team subjected to some constraints. Authors in [18] designed an adaptive consensus controller based on multiple surface control (MSC) to achieve a specific leader-follower formation for a team of quadrotors, taking into consideration the uncertainties generated by the ground effects. The results presented showed good performance with good adaptation of the slack variables and the disturbance of landing or taking off. Five different controllers were developed in [19]. Two of them (i.e., the nonlinear suboptimal H ∞ control technique and the integral backstepping (IBS) controller) were based on Lyapunov theory and were proposed for trajectory tracking and leader-follower formation control. The others were proportional derivative square (PD 2 ), which was compared with H ∞ control by applying them on the attitude system and the linear quadratic regulator (LQR) with the optimal iterative LQR, which were applied and compared for attitude, trajectory tracking, and formation control. Researchers in [20] used a simplified quadrotor model to develop a novel distributed consensus algorithm, while one of the uniform constant delays and asynchronous time-varying delays existed as a communication delay. A backstepping based on proportional integral derivative (PID) control was designed in [21] to solve the formation problem for a team of quadrotors in a decentralized manner. The system stability and retention of the desired geometric formation with or without the existence of the obstacles were presented in the simulations. Authors in [22] proposed a novel consensus leader-follower formation control based on a non-smooth backstepping design. The different numerical simulations verified the effectiveness of the proposed controller, and theoretical analysis showed that all the quadrotors converged to the desired formation.

Literature Survey
There is a large variation of research on cooperative control techniques for MASs. Getting an agreement between the agents (i.e., consensus) was the aim of some of the researchers, while others intended to obtain a specific formation for the MAS under some constraints (i.e., communication topology, leader/leaderless, …, etc.). This depends on the application where the control technique is used. Some of the recent research, such as authors in [15], proposed a distributed adaptive controller for a class of an under-actuated Lagrangian system to track a dynamic leader and control the actuated variables, while being subjected to parameter uncertainties and external disturbance with or without actuator faults. In [16], the authors studied the distributed consensus problem for a team of quadrotors with fixed and switching topologies after transforming the quadrotor model to a simplified one using feedback linearization technique. Furthermore, others in [17] presented a twolayer hierarchical model predictive control (MPC) to stabilize a quadrotors team subjected to some constraints. Authors in [18] designed an adaptive consensus controller based on multiple surface control (MSC) to achieve a specific leader-follower formation for a team of quadrotors, taking into consideration the uncertainties generated by the ground effects. The results presented showed good performance with good adaptation of the slack variables and the disturbance of landing or taking off. Five different controllers were developed in [19]. Two of them (i.e., the nonlinear suboptimal

Motivation and Contributions
In the literature survey mentioned previously, a simplified model for the UA system was used in most of the research. This issue and others motivated us to simulate the complete UA quadrotor system model by using a proposed consensus controller based on optimization techniques. Moreover, the leader-follower topology was used in some case studies, where the leader agent was designed as a complete quadrotor system controlled by the Nonlinear PID (NLPID) and Improved Active Disturbance Rejection Control (IADRC) scheme proposed in our previous work, with the followers controlled by the controller scheme proposed in this work.
The main contributions of this paper can be summarized as follows: • Proposing a new consensus controlling scheme based on optimization techniques for a MAS of 6-degree of freedom (DOF) quadrotors.
• Simulating a leader-follower topology in some cases by adding additional terms to control the geometry of the agents, and if these terms are equalized to zero, the agents goes to the consensus case.

Notations
In this paper, vectors and matrices are denoted by bold symbols (e.g., A or a). Dot notation is used for time derivative. In this notation a dot is placed above the function name to denote the time derivative of a function (e.g., the first and second derivatives of x with respect to time t are denoted by . x, .. x). The integration in this paper is denoted by the symbol (e.g., the integration of the function x with respect to time t between a lower limit a and upper limit b is denoted by b a xdt). The sign ⊗ is used in this paper as a Kronecker product operation.

Paper Organization
The rest of the paper is organized as follows: Some preliminaries on the quadrotor mathematical model, graph theory, mathematical representation of consensus control, and finally the problem statement of this paper are given in Section 2; thereafter, the proposed consensus control scheme is demonstrated in Section 3. Next, some simulations and tests are performed and discussed in Section 4. Finally, Section 5 includes the paper conclusions and some of the future aspects.

Quadrotor Mathematical Model
As mentioned in our previous works, the mathematical model of the quadrotor system is derived in Appendix A based on Newton-Euler equation and can be represented in Equation (1), while Newton's law is used to obtain Equation (2), which is helpful to design a controller for the under-actuated quadrotor system using the acceleration in 3 dimensions [23,24]; all the parameters used are described in Table 1. Total mass kg c( ) ≡ cos( ), s( ) ≡ sin( ), and t( ) ≡ tan( )

Graph Theory
The mathematical preliminaries of the consensus control are important to be studied and understood to pave the way for dealing with the consensus control. The consensus control design procedure consists of a number of consecutive steps, which are listed below: • Determining the number of agents.

•
Deriving a graph based on agent communication.

•
Deriving an adjacency matrix from the graph.

•
Converting the adjacency matrix into a Laplacian matrix.

•
Using the Laplacian matrix to design the consensus controller.
Thus, graph theory plays a key role in these steps, since it can reflect the communication topology of MASs [25].
Graph theory is commonly used in consensus control to describe the topology of the connection between the agents. Connection topology can be categorized as directed and undirected based on the information transmitted between the agents.
A graph G is built upon a finite set, which has a limited number of elements. This finite set is called vertex set V, which includes the vertices of the graph. Assuming a graph G has n vertices, then the vertex set V can be represented as . . , n and i j, is called edge, while a set of edges is denoted by E. Sometimes another notation is used for the vertices and edges of the graph G as V(G) and E(G), respectively, and for simplicity edge The directed graph (digraph) is defined as a graph D that consists of a vertex set V(D) and directed edge set E(D). In a directed graph, the direction of the edge is shown by an arrow. The graph is undefined if the vertex set is undefined.

Adjacency and Degree Matrices
For an undirected graph G, each vertex V i has a degree d(V i ). This degree is equal to the number of vertices adjacent to V i in G. All the vertices' degrees of the graph are represented in matrix form as a diagonal matrix, which is called a degree matrix and is given as where ∆( ) is the degree matrix of graph G, and n is the number of vertices or agents. The adjacency matrix A(G) is a symmetrical matrix that reflects adjacency links between the vertices in graph G, and can be derived from the following relationship: In all previously mentioned cases, the edge weights are considered to be one; but sometimes other weights are used instead of one. Generally, for weighted digraphs, the adjacency matrix is given as where ij is the weight for the edge between vertices V i and V j . The diagonal degree matrix is given as which represents the sum of the weights of the edge that is coming from vertex V j into vertex V i and is given in the following relation:

Laplacian Matrix
There are many ways to represent the Laplacian of a graph. The most straight forward method is given as follows: where ∆(G) is the degree matrix of graph G, and A(G) is the adjacency matrix for graph G.

Mathematical Representation of Consensus Control
Consensus control stability and convergence analysis is a complex method due to the fact that consensus control is dealing with MAS, not a single agent. This is the reason why researchers in their publications simplify the system modeling to simplify the analysis approach, unlike the system modeling in a single agent. Some of the commonly used approaches for consensus control are the single-integrator system, double-integrator, and higher-order MASs, which are described next [25].

Single-Integrator MAS
MASs of single-integrator dynamics can be mathematically represented as follows: where x i ∈ R n are the states vector for agent i, u i ∈ R m is the input vector for agent i, and N refers to the number of agents in the MAS. The consensus control is represented by Sensors 2020, 20, 3576 where ij is the (i, j) element of the Laplacian matrix, and K ∈ R m×n is the control matrix, which needs to be determined. To write the consensus in a general matrix form, substitute Equation (10) in Equation (9).

Double-Integrator MAS
MASs of double-integrator dynamics can be mathematically represented as where x i , v i ∈ R n are the position and velocity vectors, respectively, for agent i, and u i ∈ R m is the input vector for agent i. The consensus control is represented by The general matrix representation for the double-integrator MAS can be achieved by using Equation (12) with the controller in Equation (13) as follows:

Higher-Order MAS
An extension for the single and double-integrator MASs is given as follows: .
where A ∈ R n×n , B ∈ R n×m , C ∈ R p×n , x i ∈ R n is the state vector for agent i, u i ∈ R m is the input vector for agent i, and y i ∈ R p is the output vector for agent i. The consensus control can be achieved using the control defined in Equation (10) applied on the MAS of Equation (15). Then, the matrix form representation is as follows: .

Problem Statement
A network of homogenous connected MAS is considered as a leader-follower connection, where each agent mathematical model can be represented in the compact form. The control signal of the leader is treated as a single agent control problem, while the followers' controllers have to be designed and tuned based on the GA optimization technique such that they stabilize the unstable followers, reach consensus between all the MAS agents, and achieve desired formations while tracking different trajectories.

Proposed Controlling Scheme Design
The consensus control is proposed in this paper to achieve tracking formation for the MAS of the quadrotors while using the leader-follower connection topology between the agents (directed graph). The aim of the tracking formation is to keep the agents in a formation on the 3D space (x, y, z) and the angle of rotation around z-axis (ψ) while tracking the leader. Therefore, the leader control is different from the followers, which can be represented by normal quadrotor control using a combination of NLPID for the outer loop control (OLC) and IADRC for the inner loop control (ILC) of the quadrotor system, which was designed in our previous works [26][27][28] and is shown in Figure 4. The followers' control is a mix of two controllers; the first is the local one and the second is the consensus controller, as shown in Figure 5.
designed and tuned based on the GA optimization technique such that they stabilize the unstable followers, reach consensus between all the MAS agents, and achieve desired formations while tracking different trajectories.

Proposed Controlling Scheme Design
The consensus control is proposed in this paper to achieve tracking formation for the MAS of the quadrotors while using the leader-follower connection topology between the agents (directed graph). The aim of the tracking formation is to keep the agents in a formation on the 3D space ( , , ) and the angle of rotation around -axis ( ) while tracking the leader. Therefore, the leader control is different from the followers, which can be represented by normal quadrotor control using a combination of NLPID for the outer loop control (OLC) and IADRC for the inner loop control (ILC) of the quadrotor system, which was designed in our previous works [26][27][28] and is shown in Figure  4. The followers' control is a mix of two controllers; the first is the local one and the second is the consensus controller, as shown in Figure 5.  designed and tuned based on the GA optimization technique such that they stabilize the unstable followers, reach consensus between all the MAS agents, and achieve desired formations while tracking different trajectories.

Proposed Controlling Scheme Design
The consensus control is proposed in this paper to achieve tracking formation for the MAS of the quadrotors while using the leader-follower connection topology between the agents (directed graph). The aim of the tracking formation is to keep the agents in a formation on the 3D space ( , , ) and the angle of rotation around -axis ( ) while tracking the leader. Therefore, the leader control is different from the followers, which can be represented by normal quadrotor control using a combination of NLPID for the outer loop control (OLC) and IADRC for the inner loop control (ILC) of the quadrotor system, which was designed in our previous works [26][27][28] and is shown in Figure  4. The followers' control is a mix of two controllers; the first is the local one and the second is the consensus controller, as shown in Figure 5.  The follower agent control system for (φ, θ) states are the IADRC designed for the leader agent, before proceeding in the proposed controller for the states (x, y, z, ψ) a brief explanation and equations for the NLPID and the IADRC are given.

Nonlinear PID (NLPID) Controller
The NLPID controller is developed based on the PID controller to be better while dealing with highly complicated nonlinear systems such as the 6-DOF quadrotor system. The NLPID controller is given in Equation (17).
where ε could be e, . e, or e dt, α i ∈ R + and the function k i (ε) is a positive function with coefficients k i1 , k i2 , µ i ∈ R + . The term k i (ε) is introduced here to make the NLPID controller more sensitive for small errors. Small errors which is approximate to zero, the value of the term k i (ε) approaches to the upper bound k i1 + k i2 2 , while for large errors, the term k i (ε) approaches the lower bound k i1 , this means that the limits of the term k i (ε) is bounded in the range k i1 , k i1 + k i2 2 .

Improved Active Disturbance Rejection Control (IADRC)
ADRC, is in general, a grouping of three dynamic systems: Extended State Observer (ESO), Tracking Differentiator (TD), and State Error Feedback (SEF) controller. In our previous paper, IADRC is proposed, which is a grouping of the abovementioned three dynamic systems taking into consideration that linear ESO (LESO) is used, TD is replaced by improved TD (ITD) and that SEF is changed by NLPID control.

Design of Linear Extended State Observer (LESO)
The key idea of this element is to convert the ESO with the error equation of the system to a disturbed asymptotical stable system, in which the high-gain eliminates the effect of the total disturbance error. The mathematical representation for the LESO used is given as, where u 0 is control input, e 1 = (y − z 1 ), and β i are gain constants of the observer needed to be tuned, i ∈ 1, 2, . . . , ρ, ρ + 1 . β i = c i ω 0 , c i , i ∈ 1, 2, . . . , ρ, ρ + 1 are relevant constants, and ω 0 is the bandwidth of the observer and need to be chosen to get better estimation for the state and the disturbance with minimum OPI, and z 1 , z 2 , . . . , z ρ+1 represent the states of the plant estimated by the ESO.

Design of Improved Tracking Differentiator (ITD)
The aim of this element is to generate a smooth approximation of the reference signal and its derivatives for the control system, which can be mathematically represented as: where r 1 , r 2 , . . . , r ρ are the tracking signals (i.e., the reference signal and the derivatives of r) tracked by the differentiator signals and having initial values r 10 , r 20 , . . . , r ρ0 . The parameters R, β, α and γ are the ITD parameters which have to be tuned to get the best tracking with small errors.

Design of Nonlinear PD Controller (NLPD)
The NLPID will be used as the SEF in the IADRC, which is the same in Equation (17), but for general chain of integrators system of order ρ. The general chain of integrators system can be mathematically represented as: 1+exp(µ i ε 2 ) , i ∈ 1, 2, . . . , ρ + 1 (21) where ε could be e, . e, . . . , e ρ or e dt, and (k i1 , k i2 , µ i , α i ) are the controller parameters and are tuned to get minimum OPI. Due to the derivation in [24] of the IADRC, which changes the system into a chain of integrators, the integrator term in the NLPID will be neglected. The NLPD controller for general chain of integrators system of order ρ is represented as:

Consensus Control for x − y Position Subsystems
The x − y position dynamics of the follower agents are given by ..
where N is the number of agents, while the leader agent is denoted by 0. The proposed controller for x − y position subsystems can be represented as where ul and uc are the local and the consensus control signals for each agent, respectively. The consensus controller is designed as a single integrator system consensus controller, which is given by Equation (10) adding the formation position terms, while the local controller is designed to ensure that lim t→∞ .
x = lim t→∞ . y = 0 and is given by where k x and k y are the controllers' parameters, which need to be tuned and optimized to give better tracking performance. Then Equation (24) can be written as where ρ xi and ρ yi are the reference state in the x and y, respectively, and K x and K y ∈ R are the consensus control parameters, which need to be tuned and optimized to give better performance.

Consensus Control for z and ψ Subsystems
The dynamics of z subsystem is given as It is very clear that it is coupled and nonlinear. Therefore, to overcome this problem and design the consensus controller, the quadrotor system is assumed to be near the hovering point where φ = θ ≈ 0. Then the dynamics of z subsystem is represented as follows: which is a double integrator system and can be controlled using the consensus controller expressed in Equation (13), assuming that γ = 1 and adding the formation position terms. The controller of the z subsystem is given as while for the dynamics of ψ subsystem is given as The assumption of the hovering point is still true and the inertias of the quadrotor are nearly equal (i.e., I xx I yy I zz ). Then, the dynamics of the ψ subsystem is represented as a double integrator in the following form: which can be controlled using the consensus controller given in Equation (13); for simplicity, γ is assumed to equal one (γ = 1), because its effect will be reflected on the value of the consensus controller gain (K ψ2 ) and adding the formation position terms. The controller of the ψ subsystem is given as where ρ zi and ρ ψi are the reference state in the z and ψ, respectively, and K z1 , K z2 , K ψ1 and K ψ2 ∈ R are the consensus control parameters that need to be tuned and optimized to give better performance. The overall quadrotor system with the consensus controller is shown in Figure 6.
Sensors 2020, 20, x FOR PEER REVIEW 2 of 32 Figure 6. The overall ith quadrotor system with the consensus controller.
As shown in Figure 6, the local control units (i.e., IADRC) of the and subsystems were removed because the IADRC units keep tracking reference signals, which in this case must be zero signal. Therefore, the agents' states will be forced to reach zero. This causes canceling of the consensus control effect.  As shown in Figure 6, the local control units (i.e., IADRC) of the z and ψ subsystems were removed because the IADRC units keep tracking reference signals, which in this case must be zero signal. Therefore, the agents' states will be forced to reach zero. This causes canceling of the consensus control effect.
In contrast, the φ and θ subsystems do not have consensus controllers, while the local control units are still in place. This is due to the fact that the φ and θ subsystems have their own desired signals generated internally from the x and y position subsystems, as explained earlier, and can be treated as an internal part of the system. Therefore, adding consensus controllers will affect the x and y position subsystems and will never achieve the desired formation. (23), Equation (28) and Equation (31) of the 6-DOF quadrotor are used only for theoretical design of the controller to choose the appropriate one, while the complete nonlinear model represented by Equation (1) is used in all the simulations.

Simulation Results and Discussion
In this section, a MAS of three quadrotors using the leader-follower topology for communication network was studied, as shown in Figure 7. All quadrotor agents' parameters values are defined in Table 2. The leader agent control system (NLPID-IADRC combination) parameters are given in Tables 3-6. As shown in Figure 6, the local control units (i.e., IADRC) of the and subsystems were removed because the IADRC units keep tracking reference signals, which in this case must be zero signal. Therefore, the agents' states will be forced to reach zero. This causes canceling of the consensus control effect.
In contrast, the and subsystems do not have consensus controllers, while the local control units are still in place. This is due to the fact that the and subsystems have their own desired signals generated internally from the and position subsystems, as explained earlier, and can be treated as an internal part of the system. Therefore, adding consensus controllers will affect the and position subsystems and will never achieve the desired formation. (23), Equation (28) and Equation (31) of the 6-DOF quadrotor are used only for theoretical design of the controller to choose the appropriate one, while the complete nonlinear model represented by Equation (1) is used in all the simulations.

Simulation Results and Discussion
In this section, a MAS of three quadrotors using the leader-follower topology for communication network was studied, as shown in Figure 7. All quadrotor agents' parameters values are defined in Table 2. The leader agent control system (NLPID-IADRC combination) parameters are given in Tables 3-6.     The Laplacian matrix for the MAS based on the connection topology shown in Figure 7 is given as where the first row represents the leader agent, which does not receive any information from the followers; it just transmits its information to them. The weights used for the connection were ones. This represents that there was information transfer between the agents. The single agent system has only one error for each state, while for the MAS the number of errors depends on the number of agents in the system. In the next simulations, for each state of the quadrotor, there were three errors (i.e., for state κ the errors were κ 1 − κ 2 , κ 1 − κ 3 , and κ 2 − κ 3 where κ ∈ x, y, z, ψ ). For the reason mentioned previously, the multi-objective performance index (OPI) used here depended on the integral time absolute error (ITAE) performance index, which is given as where ITAE ij represents the ITAE = t f 0 e(t) dt for each of the three consensus errors. ω ij was chosen to be 1 3 for all i and j, whileω i was chosen to be 0.25 for all i.
All the values of the consensus control parameters are shown in Table 7. The performance indices and the OPI values after GA tuning are given in Table 8 based on the initial values and the reference signals mentioned. Some tests were done on the MAS with the proposed consensus control scheme. These are represented as follows.

Achieving Consensus
The purpose of this test was to check the errors between the agents' states while following the leader agent and the time it took to reach consensus between the agents. The initial states for each agent are given in Table 9, while the reference signals applied for the altitude (z), position (x, y), and the Yaw angle (ψ) were zero signal with simulation time t f = 10 s.

Fixed Triangle Formation
A fixed formation for the agents was reached, while the leader was supposed to keep increasing in its altitude. The initial states for this test and the tests coming after were the same, as represented in Table 9, taking into consideration that the followers' initial altitude was the same altitude as that of the leader (i.e., initial z = 0.001 for both followers). The reference signals of the leader for the x, y, and ψ were zero signal, while for z a ramp input was used with a slope = 0.2 m with simulation time t f = 30. The values of the reference states to form the triangle shape (ρ xi , ρ yi , ρ zi , and ρ ψi ) mentioned in Equation (26), Equation (29) and Equation (32), respectively, are given as The time response for the x − y position and the altitude z of the leader and the two followers are shown in Figure 12, while the motion of the agents in 3D is represented in Figure 13. From the results represented in Figure 12, the followers tracked the leader trajectory perfectly with fast response, keeping the distances given previously to form the triangle shape, while the altitude of the followers matched the leader path without a variation in the response. The motion in Figure 13 of the agents shows the agents, while changing their positions to form the triangle shape, which was formed by the three agents after a few seconds from starting the test, and the shape was kept fixed while the leader altitude was increasing.

Fixed Formation Tracking Ramp Trajectory
This test differed from the previous one by the reference signals used with the leader to track them, where the (x and y) position was a changeable not zero signal. Forming a fixed triangle shape while the agents were moving was required in this test. The reference states values for the triangle shape are given as follows:   The reference trajectory signals for the leader agent were a ramp input signal for the x and y position states with starting time = 10 s and slope = 0.01 m, a step input for the altitude z, and a zero signal for the yaw angle ψ, with a full simulation time t f = 500 s. Figure 14 shows the motion of the leader while tracking the reference trajectory and the followers keeping the required shape while tracking the leader trajectory. The shape was well performed by the three agents using the proposed controllers. The time response for this test is represented in Figure 15, which shows the fast-tracking of the followers to make the required shape with the leader agent without peaks and with a very smooth response. This test differed from the previous one by the reference signals used with the leader to track them, where the ( and ) position was a changeable not zero signal. Forming a fixed triangle shape while the agents were moving was required in this test. The reference states values for the triangle shape are given as follows: ϱ = ϱ = 0, ϱ = −1; ϱ = ϱ = 0, ϱ = −1;

Fixed Formation Tracking Ramp Trajectory
The reference trajectory signals for the leader agent were a ramp input signal for the and position states with starting time = 10 and slope = 0.01 , a step input for the altitude , and a zero signal for the yaw angle , with a full simulation time = 500 . Figure 14 shows the motion of the leader while tracking the reference trajectory and the followers keeping the required shape while tracking the leader trajectory. The shape was well performed by the three agents using the proposed controllers. The time response for this test is represented in Figure 15, which shows the fast-tracking of the followers to make the required shape with the leader agent without peaks and with a very smooth response.

Leader Orientation-Based Formation Tracking a Helical Trajectory
The previous test showed good trajectory tracking performance while keeping the formation shape fixed. This test differed from the previous one by following the rotation of the leader and keeping the formation shape, but rotation occurred when the leader changed its position. This test is very important for the search and rescue applications of the MAS. In this test, the helical path was used as a reference trajectory for the leader agent. These reference signals are given in Table 10. Another difference in this test as compared to all the cases mentioned previously is that the reference states (ϱ , ϱ , ϱ , and ϱ ) were time-varying here to guarantee that the same triangle shape was kept depending on the leader orientation. The signals of the reference states of the and position are given in Table 11, while the altitude and the yaw angle reference states (ϱ , and ϱ ) were zero for all agents.

Leader Orientation-Based Formation Tracking a Helical Trajectory
The previous test showed good trajectory tracking performance while keeping the formation shape fixed. This test differed from the previous one by following the rotation of the leader and keeping the formation shape, but rotation occurred when the leader changed its position. This test is very important for the search and rescue applications of the MAS. In this test, the helical path was used as a reference trajectory for the leader agent. These reference signals are given in Table 10. Table 10. Leader orientation-based formation tracking a helical trajectory input signals.

State
Reference Trajectory Time (s) x Another difference in this test as compared to all the cases mentioned previously is that the reference states (ρ xi , ρ yi , ρ zi , and ρ ψi ) were time-varying here to guarantee that the same triangle shape was kept depending on the leader orientation. The signals of the reference states of the x and y position are given in Table 11, while the altitude and the yaw angle reference states (ρ zi , and ρ ψi ) were zero for all agents. Table 11. The x and y reference states.

Leader
Follower 1 Follower 2 The 3D motion of the agents while forming the triangle shape is shown in Figure 16. The triangle shape was well performed by the agents depending on the orientation of the leader and the path it was following. In Figure 16, each subgraph represents the triangle shape formed by the agents at a different time to show that the orientation of the triangle was changing depending on the leader direction. The 3D motion of the agents while forming the triangle shape is shown in Figure 16. The triangle shape was well performed by the agents depending on the orientation of the leader and the path it was following. In Figure 16, each subgraph represents the triangle shape formed by the agents at a different time to show that the orientation of the triangle was changing depending on the leader direction.
The time responses are presented in Figure 17 for the , , and states. The results show a good tracking for the desired signals, while the leader direction was changing the value of the reference states (ϱ and ϱ ) for the two follower agents (i.e., in second 75 of the simulation ϱ = 1, ϱ = −1, and ϱ = ϱ = 1, which represented a triangle shape directed into the -( ) direction, while in second 125 of the simulation ϱ = ϱ = 1, and ϱ = −1, ϱ = 1, which represented a triangle shape with the same dimensions but directed into the -( ) direction). The mentioned two examples from the results showed that the shape was kept, while its direction was changed depending on the leader agent.  The time responses are presented in Figure 17 for the x, y, and z states. The results show a good tracking for the desired signals, while the leader direction was changing the value of the reference states (ρ x and ρ y ) for the two follower agents (i.e., in second 75 of the simulation ρ x f 1 = 1, ρ x f 2 = −1, and ρ y f 1 = ρ y f 2 = 1, which represented a triangle shape directed into the −ve (y) direction, while in second 125 of the simulation ρ x f 1 = ρ x f 2 = 1, and ρ y f 1 = −1, ρ y f 2 = 1, which represented a triangle shape with the same dimensions but directed into the -ve (x) direction). The mentioned two examples from the results showed that the shape was kept, while its direction was changed depending on the leader agent. Sensors 2020, 20, x FOR PEER REVIEW 2 of 32

Switching Formation Topology
This test required changing the formation shape during hovering mode for the MAS. The reference trajectory signals used for the leader agent are given in Table 12, while the state references for and used are given in Table 13.   The results of this test are represented in the plots of Figure 18, which show that the agents formed a line shape on the -direction in a fast response time through the interval (0 − 25) of the time. In the second time interval (25 − 50) , a triangle shape headed by the leader agent and directed to the --direction was performed by the agents in a smooth and fast response. In the third time interval (50 − 75) , the leader started to move to the --direction, while the agents

Switching Formation Topology
This test required changing the formation shape during hovering mode for the MAS. The reference trajectory signals used for the leader agent are given in Table 12, while the state references for x and y used are given in Table 13. Table 12. Switching formation topology input signals.

State
Reference Trajectory Time (s) The results of this test are represented in the plots of Figure 18, which show that the agents formed a line shape on the y-direction in a fast response time through the interval (0 − 25) s of the time. In the second time interval (25 − 50) s, a triangle shape headed by the leader agent and directed to the -ve x-direction was performed by the agents in a smooth and fast response. In the third time interval (50 − 75) s, the leader started to move to the -ve y-direction, while the agents formed a line shape on the x-direction, and the shape was well performed. In the last time interval (75 − 100) s, another triangle shape was performed but this time was headed by the second follower and was directed to the +ve y-direction.
Sensors 2020, 20, x FOR PEER REVIEW 2 of 32 formed a line shape on the -direction, and the shape was well performed. In the last time interval (75 − 100) , another triangle shape was performed but this time was headed by the second follower and was directed to the + -direction. In this test, the 3D motion in Figure 19 was represented in multiple subgraphs to show the exact formation achieved by the MAS. These subgraphs present the motion sequence of the three agents, and it is clear that all the desired formations were performed perfectly without any error.  In this test, the 3D motion in Figure 19 was represented in multiple subgraphs to show the exact formation achieved by the MAS. These subgraphs present the motion sequence of the three agents, and it is clear that all the desired formations were performed perfectly without any error.

Fixed Formation Tracking with Exogenous Disturbances
In this test, the same trajectory and parameters of the fixed formation tracking ramp trajectory test (third test) were used in addition to the existence of different disturbances applied on the leader agent to show the ability of the proposed controller to cope with such practical case. The disturbances applied on the different states of the leader agent were a step of f wz = 100N at t = 50 s, pulses for the roll, and pitch and yaw angles (τ wx = 1N·m at t = 150 s, τ wy = 1N·m at t = 250 s, τ wz = 1N·m at t = 300 s) with pulse widths of 50 s. Figures 20 and 21 show the 3D motion of the agents and the time response of each agent while tracking the trajectory and the disturbances applied. The agents performed the trajectory tracking while different disturbances existed in a perfect way, where small peaks and fast tracking are shown in Figure 21. This proves the high immunity of the proposed controlling scheme against external disturbances.

Fixed Formation Tracking with Exogenous Disturbances
In this test, the same trajectory and parameters of the fixed formation tracking ramp trajectory test (third test) were used in addition to the existence of different disturbances applied on the leader agent to show the ability of the proposed controller to cope with such practical case. The disturbances applied on the different states of the leader agent were a step of = 100 at = 50 , pulses for the roll, and pitch and yaw angles (

Conclusions and Future Aspects
The consensus control proposed for three agents of quadrotors in this work showed good performance in time. From the simulation cases and the results presented, it can be concluded that the complex nonlinear model for the quadrotor system is treated as a multiple double integrator subsystem. As compared to the other simplified quadrotor models used by some researchers mentioned in the literature survey, the proposed controller outperformed the others by dealing with the complete nonlinear model of the quadrotor system with good performance. This work can be further developed by taking into consideration the following points: proving the closed-loop stability of the proposed control scheme with the nonlinear quadrotor model, developing the proposed controller to achieve containment and flocking control, and implementing the consensus control on a hardware platform using the proposed controlling scheme. Another direction is to extend other control design methods, as in [29,30], to control the single agent UA based on its nonlinear model while using the proposed consensus control scheme. Funding: This study was supported and funded by Prince Sultan University, Riyadh, Saudi Arabia. We would like to show our gratitude to Prince Sultan University, Riyadh, Saudi Arabia for funding this paper. Special thanks to the Robotics and Internet-of-Things Lab (RIOTU) at the Prince Sultan University, Riyadh, Saudi Arabia.

Conflicts of Interest:
The authors declare no conflict of interest. Figure A1. Quadrotor coordinate systems.

A2. Euler Angles
Leonhard Euler describes the angular position for a rigid body by using three angles (roll " ", pitch " ", and yaw " "), which are called (Euler angles). These three angles can be given in different ways to describe the angular position in 3D Euclidean space. Euler angles can be used to specify the orientation of a frame relative to another frame, which can be obtained by three successive rotations [31]. In this model, ZYX rotation ( − − ) was used, as shown in Figure A2, and described by the following matrices: Figure A1. Quadrotor coordinate systems.

Appendix A.2. Euler Angles
Leonhard Euler describes the angular position for a rigid body by using three angles (roll "φ", pitch "θ", and yaw "ψ"), which are called (Euler angles). These three angles can be given in different ways to describe the angular position in 3D Euclidean space. Euler angles can be used to specify the orientation of a frame relative to another frame, which can be obtained by three successive rotations [31]. In this model, ZYX rotation (ψ − θ − φ) was used, as shown in Figure A2, and described by the following matrices: where c( ) = cos( ), and s( ) = sin( ). where ( ) = cos( ), and ( ) = sin( ). Figure A2. Rotation around Z, Y, and X representation.

A3. Model Assumptions
To control a nonlinear, complex, and severely under-actuated system such as a quadrotor system, some assumptions need to be made during the quadrotor modeling process [31]: • The quadrotor is rigid.
• The quadrotor structure is symmetric.
• The ground effect is neglected.
• The center of gravity origin and the principal axes coincide with the body frame origin and axes.
• Input forces and torques proportional to the squared angular velocity of the rotors.

A4. Mathematical Model
The quadrotor nonlinear mathematical model is based on Newton and Euler equations. The two frames (i.e., inertial frame and body frame) are related to each other by the following relations: To control a nonlinear, complex, and severely under-actuated system such as a quadrotor system, some assumptions need to be made during the quadrotor modeling process [31]:

•
The quadrotor is rigid.

•
The quadrotor structure is symmetric.

•
The ground effect is neglected.

•
The center of gravity origin and the principal axes coincide with the body frame origin and axes.

•
Input forces and torques proportional to the squared angular velocity of the rotors.

Appendix A.4. Mathematical Model
The quadrotor nonlinear mathematical model is based on Newton and Euler equations. The two frames (i.e., inertial frame and body frame) are related to each other by the following relations: where V = [x y z] T ∈ R 3 is the linear position vector in the earth frame, W = [φ θ ψ] T ∈ R 3 is the angular position vector in the earth frame, V B = [u v w] T ∈ R 3 is the linear velocity vector in the body frame, W B = [p q r] T ∈ R 3 is the angular velocity vector in the body frame, R = R zyx , and T is the angular transformation matrix, which is given by [31] where t( ) = tan( ). The quadrotor kinematic model is given as The total force applied on the quadrotor system is stated by Newton's law, while the total torque acting on the quadrotor is given by Euler's equation. Both total force and total torque are given by the following relations: where m is the quadrotor mass, × is the cross product sign, f B = f x f y f z T ∈ R 3 is the total force, m B = m x m y m z T ∈ R 3 is the total torque, and I is the inertia matrix, which is given as [31] Then, the dynamical model for the quadrotor is given as [31] pI xx − qrI yy + qrI zz m y = . qI yy + prI xx − prI zz m z = . rI zz − pqI xx + pqI yy (A9) Appendix A. 5

. Quadrotor Forces and Moments
Several external forces are applied to the quadrotor and are given here: wherek I is the inertial z-axis unit vector,k B is the body z-axis unit vector, g is the gravitational acceleration, f t is the rotors total thrust, and f w = f wx f wy f wz T ∈ R 3 are the wind forces applied on the quadrotor system. The external moments applied on the quadrotor system are given as where τ B = τ x τ y τ z T ∈ R 3 are the rotor speeds differences torques, τ w = τ wx τ wy τ wz T ∈ R 3 are the wind torques applied on the quadrotor system, and m g is the gyroscopic moments generated by a combination between the four rotors rotation and the quadrotor body, which can be described by the following relation: where J p is the rotor inertia, which is found to be small enough to be neglected; thus, the gyroscopic moments are excluded from the total equations. Ω i is the i th rotor angular speed, while the (−1) i+1 term represent the direction of rotation for each rotor. Then, the forces and moments are substituted in Equation (A9) to obtain the final dynamical model as follows [31]: pI xx − qrI yy + qrI zz τ y + τ wy = . qI yy + prI xx − prI zz τ z + τ wz = . rI zz − pqI xx + pqI yy The inputs for the 6-DOF quadrotor system are combinations of the rotor speed (Ω), which in this case is a force f t to control the vertical thrust (z) and the torques (τ x , τ y , and τ z ) to control the angles (φ, θ, and ψ), respectively. Their mathematical relations are given as [31] where b is the thrust coefficient, l is the distance from the quadrotor center to the rotor, and d is the drag coefficient.
The overall nonlinear mathematical model for the quadrotor can be represented as There is another dynamic model for the quadrotor system, which is helpful in the controller design. Using Newton's law, the following is obtained: