Quadrotor Formation Strategies Based on Distributed Consensus and Model Predictive Controls

Abstract

In this study, the distributed consensus control and model predictive control (MPC)-based formation strategies for quadrotors are proposed. First, the formation-control problem is decoupled into horizontal and vertical motions. The distributed consensus control and MPC-based formation strategy are implemented in the follower’s horizontal formation control. In the horizontal motion, the leader tracks the given waypoints by simply using the MPC, and generates the desired formation trajectory for each follower based on its flight information, predicted trajectory, and the given formation pattern. On the other hand, the followers carry out the formation flight based on the proposed horizontal formation strategy and the desired formation trajectories generated by the leader. In the vertical motion, formation control is carried out using only the MPC for both the leader and the follower. Likewise, the leader tracks the desired altitude/climb rate and generates the desired formation trajectories for the followers, and the followers track the desired formation trajectories generated by the leader using the MPC. The optimization problem considered in the MPC differs for the horizontal and vertical motions. The problem is formulated as a quadratic programming (QP) problem for the horizontal motion, and as a linear quadratic tracker (LQT) for the vertical motion. Simulation of a comprehensive maneuver was carried out under a Matlab/Simulink environment to examine the performance of the proposed formation strategies.

1. Introduction

As the quadrotor becomes increasingly popular and easy to acquire, it has gained much attention from researchers and manufacturers. Along with the various well-developed control algorithms of a single quadrotor, researchers have shifted their attention toward formation-control algorithms in recent years. A survey [1] classifies the formation flying control algorithms of spacecraft into five architectures: multiple-input and multiple-output, leader–follower, virtual structure, cyclic, and behavioral. The above classifications can be extended to the formation-control algorithms of the quadrotor. Among the above formation-control architectures, the leader–follower architecture [2,3,4,5,6] has received most attention from researchers.

In this study, the distributed consensus control and model predictive control (MPC)-based formation strategies considering the leader–follower architecture are proposed. First of all, the formation-control problem is decoupled into horizontal and vertical motions. In the horizontal motion, the leader tracks the given waypoints by using the MPC, and generates the desired formation trajectory for each follower based on its flight information, predicted trajectory, and the given formation pattern. The followers carry out the formation flight based on the proposed horizontal formation strategy and the desired formation trajectories generated by the leader. The follower’s horizontal formation strategy is set up based on the distributed consensus control and the MPC. Moreover, the optimized problem, which is considered in the MPC as adopted by the leader’s tracking control and the follower’s formation strategy in the horizontal motion, is formulated as a quadratic programming (QP) problem. In the vertical motion, the leader tracks the desired altitude and climb rate and generates the followers’ desired formation trajectories and the followers track the desired formation trajectories generated by the leader using the MPC. The optimized problem, which is considered in the MPC as adopted by the leader and the follower in the vertical motion, is formulated as a linear quadratic tracker (LQT).

Section 2 provides the background of this study. Both the linear and nonlinear equations of motion (EOMs) of the quadrotor are derived in Section 3. Section 4 briefly introduces the control laws that are adopted in the formation strategies. The full map of the formation strategies for the leader and the follower are carried out in Section 5. In Section 6, the preparation for the simulation, including the gains for the control laws as well as the Matlab/Simulink environment setup (Matlab R2016b, The MathWorks, Inc., Natick, MA 01760, USA), is provided. A simulation of the comprehensive maneuver was carried out to examine the performance of the formation strategies, and simulation results are illustrated in Section 7. Section 8 discusses the simulation results and the performance of the proposed formation strategies. The conclusions of the study are summarized in Section 9.

2. Background

Various methodologies of formation control have been proposed in recent years. For example, the backstepping method considering the nonlinear quadrotor model [7,8], linear quadratic control considering the linear quadrotor model [2], consensus control, and MPC [9]. In the field of consensus formation control, a comparative review of the latest trends has been carried out in Reference [10]. The author of Reference [11] provides a theoretical framework for analyzing the consensus algorithms for multiagent networked systems. Furthermore, a consensus algorithm for a second-order system is proposed in Reference [12].

This study is motivated by the results in Reference [13], in which the authors propose a formation-control method using decentralized MPC and consensus-based control. First, the leader computes all of the followers’ reference trajectories using a consensus-based algorithm, and transmits the trajectories to the followers according to the proposed network structure (Figure 1a). Then, each follower tracks its reference trajectory using the MPC.

In this study, the consensus-based algorithm is moved into the follower’s formation strategy in order to minimize the data that are transmitted around the network. The distributed network structure in Figure 1b is considered. In the horizontal motion, the leader tracks the given waypoints by using the MPC, and generates the desired formation trajectory of each follower based on its flight information, predicted trajectory, and the given formation pattern. The leader transmits the desired formation trajectories to the followers but does not receive any data from the followers. Moreover, the desired formation trajectory of each follower contains only the information of where the follower should be, that is, the desired position over the predicted time horizon. In the vertical motion, the leader tracks the desired altitude and climb rate by using the MPC, and generates the desired formation trajectory for each follower. Likewise, the desired formation trajectory for each follower in the vertical motion only contains the information of the desired altitude.

The follower’s formation control in the horizontal motion is carried out based on the distributed consensus control and the MPC. Consensus control requires the information of the desired velocity, attitude, and angular velocity. However, the desired formation trajectory of each follower generated by the leader does not contain this information. Therefore, an algorithm using a finite difference method is added to the formation strategy in the horizontal motion to generate the desired information from the desired formation trajectory required by the consensus control. Once the complete desired formation trajectory is generated, the formation control engages. The consensus control generates the flight paths of the follower under consideration and the other followers that are connected to it in the proposed network structure. However, the flight paths of other followers are not used by the proposed formation strategy of the follower under consideration. Despite that these paths might be valuable in cases such as interruption of communication or a collision-avoidance situation, they are beyond the scope of this study. The flight path of the follower under consideration is passed to the MPC. The MPC tracks the flight path and computes control inputs based on the unconstrained QP problem derived from the LQT. In the vertical motion, the LQT requires the information of the desired vertical velocity that is not included in the desired formation trajectory generated by the leader. Likewise, an algorithm using a finite-difference method is added to the formation strategy in the vertical motion to generate the desired information from the desired formation trajectory required by the LQT. Then, the LQT can compute the formation control based on the complete desired formation trajectory.

3. Dynamic Model of a Quadrotor

3.1. EOM of the Quadrotor

A quadrotor’s dynamics considering a flat Earth with an atmosphere at rest can be described by Newton–Euler formalism. The reference frames of the quadrotor are defined before EOMs are derived. The configuration, forces, and reference frames of the quadrotor that is considered in this study are illustrated in Figure 2.

In Figure 2, $\mathit{I}=\left(\widehat{\mathit{i}},\widehat{\mathit{j}},\widehat{\mathit{k}}\right)$ denotes the inertial frame and $\mathit{B}=\left({\widehat{\mathit{b}}}_1,{\widehat{\mathit{b}}}_2,{\widehat{\mathit{b}}}_3\right)$ denotes the body-fixed frame of the quadrotor. The axes $\left(\widehat{\mathit{i}},\widehat{\mathit{j}},\widehat{\mathit{k}}\right)$ of the frame $\mathit{I}$ point to North, East, and downward under the consideration of flat Earth, respectively. The origin of the body-fixed frame $\mathit{B}$ coincides with the center of gravity (CG) of the quadrotor at $\mathit{O}$. In addition, L represents the arm length of the rotor measured from $\mathit{O}$ to the rotation axis. The weight of the quadrotor is not shown in Figure 2, but it should be considered while deriving the EOMs. ${\mathit{T}}_i$ and ${\mathit{\tau }}_i$ are the thrust and torque of i-th rotor $\left(i=1,2,3,4\right)$ with respect to the frame $\mathit{B}$, respectively. ${\mathit{T}}_i$ and ${\mathit{\tau }}_i$ can be written as follows:

$${\mathit{T}}_i=-T_i{\widehat{\mathit{b}}}_3,$$

(1)

$${\mathit{\tau }}_i={(-1)}^i{\tau }_i{\widehat{\mathit{b}}}_3.$$

(2)

The Newton–Euler formalism with respect to frame $\mathit{B}$ is shown as follows:

$$\begin{array}{c}\hfill \left[\begin{array}{cc}m{\mathit{I}}_3& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathit{I}}_J\end{array}\right]\left[\begin{array}{c}{\dot{\mathit{V}}}_{\mathit{B}}\\ {\dot{\omega }}_{\mathit{B}}\end{array}\right]+\left[\begin{array}{c}{\omega }_{\mathit{B}}\times \left(m{\mathit{V}}_{\mathit{B}}\right)\\ {\omega }_{\mathit{B}}\times \left({\mathit{I}}_J{\omega }_{\mathit{B}}\right)\end{array}\right]=\left[\begin{array}{c}{\mathit{F}}_{\mathit{B}}\\ {\mathit{\tau }}_{\mathit{B}}\end{array}\right],\end{array}$$

(3)

$$\begin{array}{c}\hfill {\mathit{I}}_J=\left[\begin{array}{ccc}{I}_{xx}& I_{xy}& I_{xz}\\ I_{yx}& I_{yy}& I_{yz}\\ I_{zx}& I_{zy}& I_{zz}\end{array}\right].\end{array}$$

(4)

In Equation (3), m is the mass of the quadrotor and ${\mathit{I}}_3$, ${\mathbf{0}}_{3\times 3}\in {\mathrm{I}\mathrm{R}}^{3\times 3}$ denote the identity matrix and square zero matrix, respectively. ${\mathit{I}}_J$ is the inertial tensor of the quadrotor. The quadrotor is symmetric to the xz and yz planes. Therefore, $I_{xy}=I_{yx}=I_{yz}=I_{zy}=0$. Furthermore, $I_{zx}$ and $I_{xz}$ are relatively small compared to $I_{xx}$, $I_{yy}$, and $I_{zz}$. Hence, the inertial tensor of the quadrotor becomes ${\mathit{I}}_J=diag(I_{xx},I_{yy},I_{zz})$. ${\mathit{V}}_{\mathit{B}}=\left(u,v,w\right)$ and ${\omega }_{\mathit{B}}=\left(p,q,r\right)$ are the velocity and angular velocity of the quadrotor with respect to frame $\mathit{B}$. ${\mathit{F}}_{\mathit{B}}$ are the forces acting on the quadrotor with respect to frame $\mathit{B}$, including the thrust of each rotor and the weight of the quadrotor. ${\mathit{\tau }}_{\mathit{B}}$ includes the torque induced by each rotor and the moment induced by the thrust of each rotor.

To obtain the overall EOMs, the transformation between the velocity in frame $\mathit{I}$ and frame $\mathit{B}$, and the transformation between the angular velocity and the rate change of the Euler angles must be included. The above transformations are commonly seen in the derivation of the EOMs of unmanned aerial vehicles (UAVs) and drones. More details can be found in [14,15]. Here, rotation matrix $R_{I2B}$, which transforms the coordinate of a vector from frame $\mathit{I}$ to frame $\mathit{B}$, is introduced. Rotation matrix $R_{I2B}$ is defined as follows:

$$R_{I2B}=\left[\begin{array}{ccc}{C}_{\theta }{C}_{\psi }& C_{\theta }{S}_{\psi }& -S_{\theta }\\ S_{\varphi }{S}_{\theta }{C}_{\psi }-C_{\varphi }{S}_{\psi }& S_{\varphi }{S}_{\theta }{S}_{\psi }+C_{\varphi }{C}_{\psi }& S_{\varphi }{C}_{\theta }\\ C_{\varphi }{S}_{\theta }{C}_{\psi }+S_{\varphi }{S}_{\psi }& C_{\varphi }{S}_{\theta }{S}_{\psi }-S_{\varphi }{C}_{\psi }& C_{\varphi }{C}_{\theta }\end{array}\right],$$

(5)

where $S_{(·)}=sin(·)$ and $C_{(·)}=cos(·)$. In addition, $R_{I2B}$ is an orthogonal matrix. Therefore, the following property holds:

$$R_{I2B}^{-1}=R_{I2B}^T=R_{B2I}.$$

(6)

The transformation between the velocity with respect to frame $\mathit{I}$, ${\mathit{V}}_{\mathit{I}}=\left(\dot{x},\dot{y},\dot{z}\right)$, and the velocity with respect to frame $\mathit{B}$, ${\mathit{V}}_{\mathit{B}}=\left(u,v,w\right)$, can be written as follows:

$${\mathit{V}}_{\mathit{I}}=R_{B2I}{\mathit{V}}_{\mathit{B}},$$

(7)

$$\Rightarrow \left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{z}\end{array}\right]=\left[\begin{array}{ccc}{C}_{\theta }{C}_{\psi }& S_{\varphi }{S}_{\theta }{C}_{\psi }-C_{\varphi }{S}_{\psi }& C_{\varphi }{S}_{\theta }{C}_{\psi }+S_{\varphi }{S}_{\psi }\\ C_{\theta }{S}_{\psi }& S_{\varphi }{S}_{\theta }{S}_{\psi }+C_{\varphi }{C}_{\psi }& C_{\varphi }{S}_{\theta }{S}_{\psi }-S_{\varphi }{C}_{\psi }\\ -S_{\theta }& S_{\varphi }{C}_{\theta }& C_{\varphi }{C}_{\theta }\end{array}\right]\left[\begin{array}{c}u\\ v\\ w\end{array}\right],$$

(8)

where $(x,y,z)$ are the position of the quadrotor with respect to frame $\mathit{I}$. Additionally, the transformation between the angular velocity and the rate change of the Euler angles is shown as follows:

$$\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{ccc}1& S_{\varphi }tan\theta & C_{\varphi }tan\theta \\ 0& C_{\varphi }& -S_{\varphi }\\ 0& S_{\varphi }sec\theta & C_{\varphi }sec\theta \end{array}\right]\left[\begin{array}{c}p\\ q\\ r\end{array}\right].$$

(9)

Finally, the EOMs of the quadrotor can be obtained with the above definitions and transformations. In this study, the formation control is decoupled into horizontal and vertical motions. Therefore, the EOMs of the quadrotor are arranged in a similar form.

In the horizontal motion, states $(x,y,u,v,\theta ,\varphi ,q,p)$ are considered, and the EOMs are arranged as follows:

$$\begin{array}{c}\hfill \begin{array}{ccc}\hfill PitchAxis:\left[\begin{array}{c}\dot{x}\\ \dot{u}\\ \dot{\theta }\\ \dot{q}\end{array}\right]& =\hfill & \left[\begin{array}{c}\left(C_{\theta }{C}_{\psi }\right)u+(S_{\varphi }{S}_{\theta }{C}_{\psi }-C_{\varphi }{S}_{\psi })v+(C_{\varphi }{S}_{\theta }{C}_{\psi }+S_{\varphi }{S}_{\psi })w\\ rv-qw-gsin\theta \\ q{C}_{\varphi }-r{S}_{\varphi }\\ \frac{rp(I_{zz}-I_{xx})}{I_{yy}}+\frac{1}{I_{yy}}{M}_{\theta }\end{array}\right],\\ \hfill RollAxis:\left[\begin{array}{c}\dot{y}\\ \dot{v}\\ \dot{\varphi }\\ \dot{p}\end{array}\right]& =\hfill & \left[\begin{array}{c}\left(C_{\theta }{S}_{\psi }\right)u+(S_{\varphi }{S}_{\theta }{S}_{\psi }+C_{\varphi }{C}_{\psi })v+(C_{\varphi }{S}_{\theta }{S}_{\psi }-S_{\varphi }{C}_{\psi })w\\ pw-ru+gsin\varphi cos\theta \\ p+q{S}_{\varphi }tan\theta +r{C}_{\varphi }tan\theta \\ \frac{qr(I_{yy}-I_{zz})}{I_{xx}}+\frac{1}{I_{xx}}{M}_{\varphi }\end{array}\right].\end{array}\end{array}$$

(10)

On the other hand, the EOMs considering the states $(z,w,\psi ,r)$ in the vertical motion are arranged as follows:

$$\left[\begin{array}{c}\dot{z}\\ \dot{w}\\ \dot{\psi }\\ \dot{r}\end{array}\right]=\left[\begin{array}{c}(-S_{\theta })u+\left(S_{\varphi }{C}_{\theta }\right)v+\left(C_{\varphi }{C}_{\theta }\right)w\\ qu-pv+gcos\varphi cos\theta -\frac{1}{m}{T}_{total}\\ q{S}_{\varphi }sec\theta +r{C}_{\varphi }sec\theta \\ \frac{pq(I_{xx}-I_{yy})}{I_{zz}}+\frac{1}{I_{zz}}{M}_{\psi }\end{array}\right].$$

(11)

In Equations (10) and (11), $M_{\theta },M_{\varphi },M_{\psi }$, and $T_{total}$ are the controls and defined as follows:

$$\begin{array}{cc}{M}_{\varphi }=& \frac{\sqrt{2}}{2}L(T_1+T_2-T_3-T_4),\\ M_{\theta }=& \frac{\sqrt{2}}{2}L(T_1-T_2-T_3+T_4),\\ M_{\psi }=& -{\tau }_1+{\tau }_2-{\tau }_3+{\tau }_4,\\ T_{total}=& T_1+T_2+T_3+T_4.\end{array}$$

(12)

3.2. Linearization of the EOMs

The quadrotor model that is used in the consensus control and MPC is obtained by linearizing Equations (10) and (11). The small disturbance method is considered to linearize the EOMs, and the hovering condition of the quadrotor is chosen as the equilibrium reference. The equilibrium point of each state in horizontal and vertical motions are listed in

Table 1. Equilibrium pt. and small disturbance expression of each state and controls in horizontal motion.

State	Equilibrium pt.	Small Disturbance Expression	Unit
x	$x_0=x_{Init}$	$\Delta x$	m
y	$y_0=y_{Init}$	$\Delta y$	m
u	$u_0=0$	$\Delta u$	m/s
v	$v_0=0$	$\Delta v$	m/s
$\theta$	${\theta }_0=0$	$\Delta \theta$	rad
$\varphi$	${\varphi }_0=0$	$\Delta \varphi$	rad
q	$q_0=0$	$\Delta q$	rad/s
p	$p_0=0$	$\Delta p$	rad/s
$M_{\theta }$	$M_{\theta ,0}=0$	$\Delta M_{\theta }$	m · N
$M_{\varphi }$	$M_{\varphi ,0}=0$	$\Delta M_{\varphi }$	m · N

and

Table 2. Equilibrium pt. and small disturbance expression of each state and controls in vertical motion.

State	Equilibrium pt.	Small Disturbance Expression	Unit
z	$z_0=z_{Init}$	$\Delta z$	m
w	$w_0=0$	$\Delta w$	m/s
$\psi$	${\psi }_0=0$	$\Delta \psi$	rad
r	$r_0=0$	$\Delta r$	rad/s
$M_{\psi }$	$M_{\psi ,0}=0$	$\Delta M_{\psi }$	m · N
$T_{total}$	$T_{total,0}=mg$	$\Delta T_{total}$	N

where $x_{Init}$, $y_{Init}$, and $z_{Init}$ are the initial position of the quadrotor. Subscript 0 denotes the equilibrium point of the state, and $\Delta (·)$ denotes the small disturbance of the state.

, respectively.

Replacing the states in Equations (10) and (11) by the sum of the equilibrium point and the small disturbance of the states, and ignoring the cross-product terms of the small disturbance of the states, the linearized EOMs of the quadrotor in the horizontal and vertical motions can be obtained as follows:

$$\begin{array}{c}\hfill \begin{array}{ccc}\hfill PitchAxis:\left[\begin{array}{c}\Delta \dot{x}\\ \Delta \dot{u}\\ \Delta \dot{\theta }\\ \Delta \dot{q}\end{array}\right]& =\hfill & \left[\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& -g& 0\\ 0& 0& 0& 1\\ 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}\Delta x\\ \Delta u\\ \Delta \theta \\ \Delta q\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ 0\\ \frac{1}{I_{yy}}\end{array}\right]\Delta M_{\theta },\\ \hfill RollAxis:\left[\begin{array}{c}\Delta \dot{y}\\ \Delta \dot{v}\\ \Delta \dot{\varphi }\\ \Delta \dot{p}\end{array}\right]& =\hfill & \left[\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& g& 0\\ 0& 0& 0& 1\\ 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}\Delta y\\ \Delta v\\ \Delta \varphi \\ \Delta p\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ 0\\ \frac{1}{I_{xx}}\end{array}\right]\Delta M_{\varphi },\end{array}\end{array}$$

(13)

$$\left[\begin{array}{c}\Delta \dot{z}\\ \Delta \dot{w}\\ \Delta \dot{\psi }\\ \Delta \dot{r}\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 1\\ 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}\Delta z\\ \Delta w\\ \Delta \psi \\ \Delta r\end{array}\right]+\left[\begin{array}{cc}0& 0\\ \frac{-1}{m}& 0\\ 0& 0\\ 0& \frac{1}{I_{zz}}\end{array}\right]\left[\begin{array}{c}\Delta T_{total}\\ \Delta M_{\psi }\end{array}\right].$$

(14)

4. Control Law Preliminaries

4.1. Consensus Control

The main objective of the consensus control in this study is to generate the flight path for the MPC in the follower’s horizontal formation strategy. The consensus control of an agent is generated by taking information such as the flight states of other connected agents in the network structure into consideration, and eventually all the agents reach a consensus state through the consensus control.

Consider the leader–follower architecture with one leader and $N_F$ followers; consensus control input ${\mathit{u}}_i\left(k\right)$ of the i-th follower at time step k is given as follows:

$$\begin{array}{ccc}\hfill {\mathit{u}}_i\left(k\right)& =& -\sum _{j=1}^{N_F+1}{a}_{ij}\left[\sum _{ks=1}^{Ns}{\beta }_{ks}\left({\widehat{\mathit{s}}}_i^{\left(ks\right)}\left(k\right)-{\widehat{\mathit{s}}}_j^{\left(ks\right)}\left(k\right)\right)\right],\hfill \\ \hfill {\widehat{\mathit{s}}}_i^{\left(ks\right)}\left(k\right)& =& {\mathit{s}}_i^{\left(ks\right)}\left(k\right)-{\mathit{d}}_i^{\left(ks\right)}\left(k\right),\hfill \\ \hfill {\widehat{\mathit{s}}}_j^{\left(ks\right)}\left(k\right)& =& {\mathit{s}}_j^{\left(ks\right)}\left(k\right)-{\mathit{d}}_j^{\left(ks\right)}\left(k\right),\hfill \end{array}$$

(15)

where subscript j denotes the j-th follower in the network structure other than i-th follower. As in the case that subscript $j=N_F+1$, it refers to the leader. $a_{ij}$ represents the communication status between the i-th and j-th followers. If the i-th follower is receiving data from the j-th follower or leader ($j=N_F+1$), then $a_{ij}=1$. Otherwise, $a_{ij}=0$. ${\beta }_{ks}$$(ks=1,2,\dots ,Ns)$ are the control gains, where $Ns$ is the total number of the states. ${\widehat{\mathit{s}}}_{(·)}^{\left(ks\right)}\left(k\right)$ denotes the state error between the state ${\mathit{s}}_{(·)}^{\left(ks\right)}\left(k\right)$ and reference state ${\mathit{d}}_{(·)}^{\left(ks\right)}\left(k\right)$ at time step k.

4.2. Model Predictive Control

Model predictive control is a control algorithm based on the optimal theory and the plant model. In MPC, it first obtains the optimal trajectory and control sequence by solving the optimal problem according to the given cost function. The optimal trajectory can be seen as the prediction of the plant behavior over the predicted time horizon with the optimal control sequence. However, the plant might not behave identically to the prediction. Thus, MPC applies only to the first control in the optimal control sequence to the plant. When the next measurements of the states are acquired, MPC repeats the above sequence. By doing so, the MPC algorithm works like an optimal feedback control.

The following sections show the problem statement, the solution to the LQT problem, and how to rewrite an LQT problem into a QP problem.

4.2.1. Linear Quadratic Tracker

This section shows the problem statement and the solution to the LQT. The derivation of the solution to the LQT is not covered in this section. If the readers are interested in the derivation, it can be found in Reference [16].

Consider the following discrete state-space model with n states, m controls, and q outputs:

$$\begin{array}{ccc}{\mathit{x}}_{k+1}& =& A{\mathit{x}}_k+B{\mathit{u}}_k,\\ {\mathit{y}}_k& =& C{\mathit{x}}_k,\end{array}$$

(16)

where the system matrix $A\in {\mathrm{I}\mathrm{R}}^{n\times n}$, the control matrix $B\in {\mathrm{I}\mathrm{R}}^{n\times m}$, and the output matrix $C\in {\mathrm{I}\mathrm{R}}^{q\times n}$. $x_k\in {\mathrm{I}\mathrm{R}}^n$ is a column vector, which denotes the state at time step k in the considered time horizon. $u_k\in {\mathrm{I}\mathrm{R}}^m$ is a column vector, which denotes the control at time step k in the considered time horizon. $y_k\in {\mathrm{I}\mathrm{R}}^q$ is a column vector, which denotes the output state at time step k in the considered time horizon. Next, define the linear quadratic cost function based on the control and output state in the discrete state-space model and the reference trajectory for the output state as follows:

$$J=\frac{1}{2}{({\mathit{y}}_{N_p}-{\mathit{r}}_{N_p})}^{T}P({\mathit{y}}_{N_p}-{\mathit{r}}_{N_p})+\frac{1}{2}\sum _{k=0}^{N_p-1}\left[{({\mathit{y}}_k-{\mathit{r}}_k)}^{T}Q({\mathit{y}}_k-{\mathit{r}}_k)+{\mathit{u}}_K^{T}R{\mathit{u}}_k\right],$$

(17)

where k$\left(k=0,1,\dots ,N_p\right)$ denotes the time step over the time horizon. ${\mathit{r}}_k$$\left(k=0,1,\dots ,N_p\right)$ is a column vector, which denotes the reference trajectory for the output state at time step k. $P,Q,R$ are the weighting matrices. All the weighting matrices are symmetric. P, $Q\in {\mathrm{I}\mathrm{R}}^{q\times q}$ are positive semidefinite. $R\in {\mathrm{I}\mathrm{R}}^{m\times m}$ is positive definite:

$$P\ge 0,Q\ge 0,R>0.$$

(18)

The optimized control sequence ${\mathit{u}}_k$ ($k=0,1,\dots ,N_p-1$) that minimizes the cost function J in Equation (17) can be obtained by solving the following equations:

$$\begin{array}{ccc}\hfill K_k& =& {(B^T{S}_{k+1}B+R)}^{-1}{B}^T{S}_{k+1}A,S_{N_p}=C^{T}PC,\hfill \\ \hfill S_k& =& A^T{S}_{k+1}(A-B{K}_k)+C^{T}QC,\hfill \\ \hfill {\mathit{v}}_k& =& {(A-B{K}_k)}^T{\mathit{v}}_{k+1}+C^{T}Q{\mathit{r}}_k,v_{N_p}=C^{T}P{\mathit{r}}_{N_p},\hfill \\ \hfill K_k^v& =& {(B^T{S}_{k+1}B+R)}^{-1}{B}^T,\hfill \\ \hfill {\mathit{u}}_k& =& -K_k{\mathit{x}}_k+K_k^v{\mathit{v}}_{k+1}.\hfill \end{array}$$

(19)

In Equation (19), $K_k$, $S_k$, and $K_k^v$ can be solved offline since they only consider the matrices in the discrete state-space model (16) and the weighting matrices P, Q, and R given by the users. Furthermore, by solving ${\mathit{v}}_k$ backward in time, the optimal tracking control sequence can be obtained. Substituting the optimal control sequence into the discrete state-space model (16), the optimal tracking trajectory over the time horizon can be obtained.

4.2.2. Unconstrained Quadratic Programming Problem

The quadratic programming problem deals with the quadratic cost function that is subject to linear equality and/or inequality constraints on the variables in the cost function. A quadratic programming problem can be stated as follows:

$$\begin{array}{ccc}\hfill minimize{J}_{QP}& =& \frac{1}{2}{\mathit{\xi }}^T{Q}_c\mathit{\xi }+{\mathit{c}}^T\mathit{\xi },\hfill \\ \hfill subjectto{A}_c\mathit{\xi }& \le & \mathit{b},\hfill \\ \hfill E\mathit{\xi }& =& \mathit{d},\hfill \end{array}$$

(20)

where $J_{QP}$ denotes the cost function of the quadratic programming problem. $\mathit{\xi }\in {\mathrm{I}\mathrm{R}}^n$ is a column vector that contains n variables to be optimized. $Q_c\in {\mathrm{I}\mathrm{R}}^{n\times n}$ is a real symmetric matrix. $\mathit{c}\in {\mathrm{I}\mathrm{R}}^n$ is a real-valued column vector. $A_c\in {\mathrm{I}\mathrm{R}}^{m\times n}$ and $\mathit{b}\in {\mathrm{I}\mathrm{R}}^m$ describe the inequality constraints of the variables. Similarly, $E\in {\mathrm{I}\mathrm{R}}^{m\times n}$ and $\mathit{d}\in {\mathrm{I}\mathrm{R}}^m$ describe the equality constraints of the variables.

To find an analytical solution to a constrained quadratic programming problem is difficult, if not impossible. However, it can still be solved by methods such as interior-point, active-set, or gradient-projection. On the contrary, the optimized solution to the unconstrained quadratic programming problem can be found simply by solving the linear equations of the partial derivative of the modified cost function with respect to each variable and set to zero.

In formation control, our objective is to find the control input that tracks the reference trajectory. By rewriting the LQT into a quadratic programming problem, the variables to be optimized become the control input sequence. That is, we can obtain the optimal control sequence by solving linear equations directly.

To rewrite the cost function, we start from rearranging the discrete state-space model (16). Expanding the discrete state-space model with respect to time step k$\left(k=0,1,\dots ,N_p-1\right)$:

$$\begin{array}{ccc}\hfill {\mathit{x}}_1& =& A{\mathit{x}}_0+B{\mathit{u}}_0,\hfill \\ \hfill {\mathit{x}}_2& =& A{\mathit{x}}_1+B{\mathit{u}}_1=A\left(A{\mathit{x}}_0+B_0\right)+B{\mathit{u}}_1=A^2{\mathit{x}}_0+AB{\mathit{u}}_0+B{\mathit{u}}_1,\hfill \\ \hfill {\mathit{x}}_3& =& A{\mathit{x}}_2+B{\mathit{u}}_2=A\left(A^2{\mathit{x}}_0+AB{\mathit{u}}_0+B{\mathit{u}}_1\right)+B{\mathit{u}}_2=A^3{\mathit{x}}_0+A^{2}B{\mathit{u}}_0+AB{\mathit{u}}_1+B{\mathit{u}}_2,\hfill \\ \hfill ⋮\\ \hfill {\mathit{x}}_{N_p}& =& A{\mathit{x}}_{N_p-1}+B{\mathit{u}}_{N_p-1}=A^{N_p}{\mathit{x}}_0+A^{N_p-1}B{\mathit{u}}_0+\cdots +B{\mathit{u}}_{N_p-1},\hfill \end{array}$$

(21)

and defining the following vectors:

$$\begin{array}{ccc}\tilde{\mathit{x}}& =& {\left[\begin{array}{cccc}{\mathit{x}}_1^T& {\mathit{x}}_2^T& \cdots & {\mathit{x}}_{N_P}^T\end{array}\right]}^T,\\ \tilde{\mathit{u}}& =& {\left[\begin{array}{cccc}{\mathit{u}}_0^T& {\mathit{u}}_1^T& \cdots & {\mathit{u}}_{N_P-1}^T\end{array}\right]}^T,\end{array}$$

(22)

where $\tilde{\mathit{x}}\in {\mathrm{I}\mathrm{R}}^{n{N}_p\times 1}$ and $\tilde{\mathit{u}}\in {\mathrm{I}\mathrm{R}}^{m{N}_p\times 1}$. Equation (21) can be rewritten into the matrix form expressed by the initial condition and the vectors in Equation (22) as follows:

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill \tilde{\mathit{x}}& =\left[\begin{array}{c}A\\ A^2\\ A^3\\ ⋮\\ A^{Np}\end{array}\right]{\mathit{x}}_0+\left[\begin{array}{cccc}B& 0_{n\times m}& \cdots & 0_{n\times m}\\ AB& B& \cdots & ⋮\\ ⋮& ⋮& \ddots & 0_{n\times m}\\ A^{N_p-1}B& A^{N_p-2}B& \cdots & B\end{array}\right]\tilde{\mathit{u}}\hfill \\ & =\tilde{A}{\mathit{x}}_0+\tilde{B}\tilde{\mathit{u}},\hfill \end{array}\end{array}$$

(23)

where $\tilde{A}\in {\mathrm{I}\mathrm{R}}^{n{N}_p\times n}$ and $\tilde{B}\in {\mathrm{I}\mathrm{R}}^{n{N}_p\times m{N}_p}$. Before substituting Equation (23) into the cost function (17), substitute the output function in Equation (16) into cost function (17) and expand the cost function:

$$\begin{array}{ccc}J& =& \frac{1}{2}{\left({\mathit{y}}_{N_p}-{\mathit{r}}_{N_p}\right)}^{T}P\left({\mathit{y}}_{N_p}-{\mathit{r}}_{N_p}\right)+\frac{1}{2}{\sum }_{k=0}^{N_p-1}\left[{\left({\mathit{y}}_k-{\mathit{r}}_k\right)}^{T}Q\left({\mathit{y}}_k-{\mathit{r}}_k\right)+{\mathit{u}}_K^{T}R{\mathit{u}}_k\right]\\ & =& \frac{1}{2}{\left(C{\mathit{x}}_{N_p}-{\mathit{r}}_{N_p}\right)}^{T}P\left(C{\mathit{x}}_{N_p}-{\mathit{r}}_{N_p}\right)+\frac{1}{2}{\sum }_{k=0}^{N_p-1}\left[{\left(C{\mathit{x}}_k-{\mathit{r}}_k\right)}^{T}Q\left(C{\mathit{x}}_k-{\mathit{r}}_k\right)+{\mathit{u}}_K^{T}R{\mathit{u}}_k\right]\\ & =& \frac{1}{2}\left({\mathit{x}}_{N_p}^T{C}^{T}PC{\mathit{x}}_{N_p}-2{\mathit{x}}_{N_p}^T{C}^{T}P{\mathit{r}}_{N_p}+{\mathit{r}}_{N_p}^{T}P{\mathit{r}}_{N_p}\right)+\cdots \\ & & \frac{1}{2}{\sum }_{k=0}^{N_p-1}\left[{\mathit{x}}_k^T{C}^{T}QC{\mathit{x}}_k-2{\mathit{x}}_k^T{C}^{T}Q{\mathit{r}}_k+{\mathit{r}}_k^{T}Q{\mathit{r}}_k+{\mathit{u}}_k^{T}R{\mathit{u}}_k\right]\\ & =& \frac{1}{2}{\tilde{\mathit{x}}}^T\left[\begin{array}{cccc}{C}^{T}QC& 0_{n\times n}& \cdots & 0_{n\times n}\\ 0_{n\times n}& \ddots & ⋮\\ ⋮& C^{T}QC& 0_{n\times n}\\ 0_{n\times n}& \cdots & 0_{n\times n}& C^{T}PC\end{array}\right]\tilde{\mathit{x}}-{\tilde{\mathit{x}}}^T\left[\begin{array}{cccc}{C}^{T}Q& 0_{n\times q}& \cdots & 0_{n\times q}\\ 0_{n\times q}& \ddots & ⋮\\ ⋮& C^{T}Q& 0_{n\times q}\\ 0_{n\times q}& \cdots & 0_{n\times q}& C^{T}P\end{array}\right]\tilde{\mathit{r}}+\cdots \\ & & {\tilde{\mathit{u}}}^T\left[\begin{array}{cccc}R& 0_{m\times m}& \cdots & 0_{m\times m}\\ 0_{m\times m}& \ddots & ⋮\\ ⋮& \ddots & 0_{m\times m}\\ 0_{m\times m}& \cdots & 0_{m\times m}& R\end{array}\right]\tilde{\mathit{u}}+{\tilde{\mathit{r}}}^T\left[\begin{array}{cccc}Q& 0_{q\times q}& \cdots & 0_{q\times q}\\ 0_{q\times q}& \ddots & ⋮\\ ⋮& Q& 0_{q\times q}\\ 0_{q\times q}& \cdots & 0_{q\times q}& P\end{array}\right]\tilde{\mathit{r}}+\cdots \\ & & {\mathit{x}}_0{C}^{T}QC{\mathit{x}}_0-2{\mathit{x}}_0{C}^{T}Q{\mathit{r}}_0+{\mathit{r}}_0^{T}Q{\mathit{r}}_0\\ & =& \frac{1}{2}{\tilde{\mathit{x}}}^T{\tilde{Q}}_1\tilde{\mathit{x}}-{\tilde{\mathit{x}}}^T{\tilde{Q}}_2\tilde{\mathit{r}}+\frac{1}{2}{\tilde{\mathit{u}}}^T\tilde{R}\tilde{\mathit{u}}+\underset{Constantcontributiontocostfunction}{\underbrace{\frac{1}{2}{\tilde{\mathit{r}}}^T{\tilde{Q}}_3\tilde{\mathit{r}}+{\mathit{x}}_0{C}^{T}QC{\mathit{x}}_0-2{\mathit{x}}_0{C}^{T}Q{\mathit{r}}_0+{\mathit{r}}_0^{T}Q{\mathit{r}}_0}},\end{array}$$

(24)

where ${\tilde{Q}}_1\in {\mathrm{I}\mathrm{R}}^{n{N}_p\times n{N}_p}$, ${\tilde{Q}}_2\in {\mathrm{I}\mathrm{R}}^{n{N}_p\times q{N}_p}$, ${\tilde{Q}}_3\in {\mathrm{I}\mathrm{R}}^{q{N}_p\times q{N}_p}$, $\tilde{\mathit{r}}\in {\mathrm{I}\mathrm{R}}^{q{N}_p\times 1}$ and $\tilde{R}\in {\mathrm{I}\mathrm{R}}^{m{N}_p\times m{N}_p}$. In Equation (24), ${\mathit{x}}_0$ is the given initial condition and ${\mathit{r}}_0$ is the reference of the output state at $k=0$. $\tilde{\mathit{r}}$ is a column vector consists of the given reference trajectory of the output state at each time step. Thus, the terms depend only on ${\mathit{x}}_0$, ${\mathit{r}}_0$, or $\tilde{\mathit{r}}$ are constant contribution to cost function J and can be ignored in the subsequent derivation. The modified cost function now becomes:

$$\tilde{J}=\frac{1}{2}{\tilde{\mathit{x}}}^T{\tilde{Q}}_1\tilde{\mathit{x}}-{\tilde{\mathit{x}}}^T{\tilde{Q}}_2\tilde{\mathit{r}}+\frac{1}{2}{\tilde{\mathit{u}}}^T\tilde{R}\tilde{\mathit{u}}.$$

(25)

Replacing $\tilde{\mathit{x}}$ in Equation (25) by Equation (23)

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill \tilde{J}& =\frac{1}{2}{\left(\tilde{A}{\mathit{x}}_0+\tilde{B}\tilde{\mathit{u}}\right)}^T{\tilde{Q}}_1\left(\tilde{A}{\mathit{x}}_0+\tilde{B}\tilde{\mathit{u}}\right)-{\left(\tilde{A}{\mathit{x}}_0+\tilde{B}\tilde{\mathit{u}}\right)}^T{\tilde{Q}}_2\tilde{\mathit{r}}+\frac{1}{2}{\tilde{\mathit{u}}}^T\tilde{R}\tilde{\mathit{u}}\hfill \\ & =\frac{1}{2}{\tilde{\mathit{u}}}^T\left({\tilde{B}}^T{\tilde{Q}}_1\tilde{B}+\tilde{R}\right)\tilde{\mathit{u}}+\left[\begin{array}{cc}{\mathit{x}}_0^T& {\tilde{\mathit{r}}}^T\end{array}\right]\left[\begin{array}{c}{\tilde{A}}^T{\tilde{Q}}_1\tilde{B}\\ -{\tilde{Q}}_2^T\tilde{B}\end{array}\right]\tilde{\mathit{u}}+\underset{Constantcontributiontocostfunction}{\underbrace{\frac{1}{2}{\mathit{x}}_0^T{\tilde{A}}^T{\tilde{Q}}_1\tilde{A}{\mathit{x}}_0-{\mathit{x}}_0^T{\tilde{A}}^T{\tilde{Q}}_2\tilde{\mathit{r}}}}\hfill \end{array}\end{array}$$

(26)

and ignoring the constant contribution part in cost function $\tilde{J}$, the cost function becomes

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill \overline{J}& =\frac{1}{2}{\tilde{\mathit{u}}}^T\left({\tilde{B}}^T{\tilde{Q}}_1\tilde{B}+\tilde{R}\right)\tilde{\mathit{u}}+\left[\begin{array}{cc}{\mathit{x}}_0^T& {\tilde{\mathit{r}}}^T\end{array}\right]\left[\begin{array}{c}{\tilde{A}}^T{\tilde{Q}}_1\tilde{B}\\ -{\tilde{Q}}_2^T\tilde{B}\end{array}\right]\tilde{\mathit{u}}\hfill \\ & =\frac{1}{2}{\tilde{\mathit{u}}}^{T}H\tilde{\mathit{u}}+\overline{\mathit{x}}F\tilde{\mathit{u}},\hfill \end{array}\end{array}$$

(27)

where $H\in {\mathrm{I}\mathrm{R}}^{m{N}_p\times m{N}_p}$, $\overline{\mathit{x}}\in {\mathrm{I}\mathrm{R}}^{(n+q{N}_p)}$ is a row vector consists of initial condition $x_0$ and the reference trajectory at each time step and $F\in {\mathrm{I}\mathrm{R}}^{(n+q{N}_p)\times m{N}_p}$. The optimal control sequence can be obtained by taking the partial derivative of $\overline{J}$ with respect to $\tilde{u}$ and setting it to zero:

$$\begin{array}{ccc}\frac{\partial \overline{J}}{\partial \tilde{u}}& =& \frac{1}{2}\left(H\tilde{u}+H^T\tilde{u}\right)+{\left(\overline{\mathit{x}}F\right)}^T=0\\ & =& H\tilde{u}+{\left(\overline{\mathit{x}}F\right)}^T=0.\end{array}$$

(28)

The optimal control sequence is then given by

$$\tilde{u}=-H^{-1}{F}^T{\overline{\mathit{x}}}^T.$$

(29)

Equation (29) will be used in the MPC in leader’s and follower’s horizontal formation strategies to compute the optimal tracking control sequence.

5. Formation Strategies

The formation control strategies of the leader and the followers are discussed in detail in the following sections. How the leader generates followers’ desired formation trajectories in both horizontal and vertical motions will be carried out in Section 5.1. Section 5.2 shows how a follower computes the formation control input using consensus control and MPC after receiving the desired formation trajectory information.

5.1. Formation Strategy: Leader

Figure 3 shows the block diagram of a leader’s formation strategy. First, the leader tracks the predefined waypoints and desired altitude/climb rate using the MPC. The MPC is formulated as a QP problem for the horizontal motion and LQT for the vertical motion. After the predicted trajectory is generated along with the tracking control input, a built-in algorithm generates followers’ desired formation trajectories and the trajectories are transmitted to each follower.

5.1.1. Generation of the Desired Formation Trajectory in the Horizontal Motion

In the horizontal motion, the leader generates the followers’ desired formation trajectories based on its position, direction of the horizontal velocity in frame $\mathit{I}$, predicted trajectory, and the given formation pattern. The transformation from the leader’s predicted trajectory to the desired formation trajectories of the followers can be derived according to Figure 4.

First of all, from Figure 4a, the follower’s desired formation position in horizontal plane in frame $\mathit{I}$ can be described as follows:

$${\mathit{r}}_{d,F}={\mathit{r}}_L+{\mathit{r}}_{L2F,\mathit{I}},$$

(30)

where ${\mathit{r}}_{d,F}$ denotes the position vector of the follower’s desired formation position and ${\mathit{r}}_L$ denotes the leader’s position vector. ${\mathit{r}}_{L2F,\mathit{I}}$ is a vector pointing from leader’s position to follower’s desired formation position with respect to frame $\mathit{I}$.

Considering Figure 4b, the desired formation position with respect to the leader’s velocity tangential coordinate can be written as follows:

$$\begin{array}{ccc}\hfill {\mathit{r}}_{L2F,\mathit{V}}& =& \left[\begin{array}{cc}{\rho }_{\parallel }& {\rho }_{\perp }\end{array}\right]\left[\begin{array}{c}{\widehat{V}}_{L_{\parallel }}\\ {\widehat{V}}_{L_{\perp }}\end{array}\right],\hfill \end{array}$$

(31)

where ${\widehat{V}}_{L_{\parallel }}$ and ${\widehat{V}}_{L_{\perp }}$ are the unit vectors of the leader’s velocity tangential coordinate in which ${\widehat{V}}_{L_{\parallel }}$ is parallel to ${\mathit{V}}_L$ and ${\widehat{V}}_{L_{\perp }}$ is perpendicular to ${\mathit{V}}_L$. $\left({\rho }_{\parallel },{\rho }_{\perp }\right)$ is the coordinate of the desired formation position with respect to the leader’s tangential coordinate.

The rotation matrix, denoted as $R_{V2I}$, that transforms the position vector from $r_{L2F,V}$ to $r_{L2F,I}$ is defined as follows:

$$\begin{array}{ccc}\hfill R_{V2I}& =& \left[\begin{array}{cc}cos{\theta }_{VL}& sin{\theta }_{VL}\\ -sin{\theta }_{VL}& cos{\theta }_{VL}\end{array}\right],\hfill \end{array}$$

(32)

where ${\theta }_{VL}$ is the angle between ${\mathit{V}}_L$ and $\widehat{\mathit{i}}$, which can be obtained by

$$\begin{array}{ccc}\hfill {\theta }_{VL}& =& {cos}^{-1}\left(\frac{{\mathit{V}}_L·\widehat{\mathit{i}}}{\parallel {\mathit{V}}_L\parallel }\right).\hfill \end{array}$$

(33)

${\mathit{V}}_L$ and its magnitude can be computed as follows:

$$\begin{array}{ccc}\hfill {\mathit{V}}_L& =& \dot{x}\widehat{\mathit{i}}+\dot{y}\widehat{\mathit{j}},\hfill \\ \hfill \parallel {\mathit{V}}_L\parallel & =& \sqrt{{\dot{x}}^2+{\dot{y}}^2},\hfill \end{array}$$

(34)

where $\dot{x}$ and $\dot{y}$ can be obtained from Equation (8). Thus, position vector $r_{L2F,I}$ can be obtained as follows:

$$\begin{array}{ccc}\hfill r_{L2F,I}& =& \left[\begin{array}{cc}{\rho }_{\parallel }& {\rho }_{\perp }\end{array}\right]\left[\begin{array}{cc}cos{\theta }_{VL}& sin{\theta }_{VL}\\ -sin{\theta }_{VL}& cos{\theta }_{VL}\end{array}\right]\left[\begin{array}{c}\widehat{\mathit{i}}\\ \widehat{\mathit{j}}\end{array}\right].\hfill \end{array}$$

(35)

Finally, the coordinate of the follower’s desired formation position ${\mathit{r}}_{d,F}$ can be obtained by

$$\begin{array}{ccc}\hfill {\mathit{r}}_{d,F}& =& {\mathit{r}}_L+{\mathit{r}}_{L2F,\mathit{I}}\hfill \\ \hfill \Rightarrow {\left\{{\mathit{r}}_{d,F}\right\}}_{\mathit{I}}& =& {\left\{{\mathit{r}}_L\right\}}_{\mathit{I}}+\left[\begin{array}{cc}{\rho }_{\parallel }& {\rho }_{\perp }\end{array}\right]\left[\begin{array}{cc}cos{\theta }_{VL}& sin{\theta }_{VL}\\ -sin{\theta }_{VL}& cos{\theta }_{VL}\end{array}\right].\hfill \end{array}$$

(36)

Applying Equation (36) to every points of the leader’s position in the predicted trajectory, the desired formation trajectory of each follower in the horizontal motion over the predicted time horizon can be obtained.

5.1.2. Generation of the Desired Formation Trajectory in the Vertical Motion

In this study, followers are designated to maintain the same altitude as the leader throughout the flight. Thus, the leader’s predicted trajectory of the altitude computed by the MPC algorithm directly becomes the followers’ desired formation trajectories. The leader’s vertical motion is determined by the desired altitude and/or climb rate, which can be classified into four cases as shown in

Table 3. Cases of leader’s vertical motion with desired altitude and climb rate.

Case	Desired Altitude	Climb Rate	Vertical Motion
I	Not Given	Not Given	Maintain current altitude.
$II$	Given	Not Given	Climb to desired altitude.
$III$	Not Given	Given	Continuous climbing with given climb rate.
$IV$	Given	Given	Climb to desired altitude with given climb rate.

.

In the vertical motion, the first two states, $\Delta z$ and $\Delta w$ in the state-space model (13) are used to construct a new second-order state-space model. The new second-order state-space model describes the leader’s vertical motion and can be rewritten as follows:

$$\left[\begin{array}{c}\Delta \dot{z}\\ \Delta \dot{w}\end{array}\right]=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]\left[\begin{array}{c}\Delta z\\ \Delta w\end{array}\right]+\left[\begin{array}{c}0\\ \frac{-1}{m}\end{array}\right]\Delta T_{total}.$$

(37)

$\Delta z$ is positive as the quadrotor moving downward. However, it is more intuitive for us to consider moving upward as positive. Hence, variable $\Delta h$ is defined to replace $\Delta z$ in the state-space model:

$$\Delta h=-\Delta z\Rightarrow \Delta \dot{h}=-\Delta w,$$

and therefore

$$\left[\begin{array}{c}\Delta \dot{h}\\ \Delta \dot{w}\end{array}\right]=\left[\begin{array}{cc}0& -1\\ 0& 0\end{array}\right]\left[\begin{array}{c}\Delta h\\ \Delta w\end{array}\right]+\left[\begin{array}{c}0\\ \frac{-1}{m}\end{array}\right]\Delta T_{total}.$$

(38)

With the given desired altitude and/or climb rate, the vertical reference trajectory of the leader for the MPC can be obtained. The MPC computes the control input and the predicted trajectory according to the reference trajectory and the state-space model (38). The following illustrate how the reference trajectory for the MPC is generated in each case in Table 3:

• Case I: maintain the current altitude.
  In this case, the leader is designated to maintain the altitude. Thus, the reference trajectories of $\Delta h$ and $\Delta w$ over the predicted time horizon are equal to zero.
• Case II: climb to the desired altitude.
  If the desired altitude is given but not the climb rate, the leader is designated to climb to the desired altitude with the default climb rate. The reference for $\Delta w$ is the negative default climb rate and the reference trajectory of $\Delta h$ is computed by

  $$\begin{array}{c}\hfill {\mathit{d}}_{Alt}\left(k\right)={\overline{L}}_p+R{C}_{default}\times k\times \Delta h_s,\end{array}$$

  (39)

  where ${\mathit{d}}_{Alt}\left(k\right)$ denotes the reference altitude at time step k, which is equal to the leader’s current altitude ${\overline{L}}_p$ plus the default climb rate $R{C}_{default}$ times the time step k and the interval of the time step $\Delta h_s$. Time step k is defined over the predicted time horizon considered by the MPC with the interval of $\Delta h_s$.
  A deceleration action is taken as the leader is approaching the desired altitude. If the distance between the leader’s current position and the desired altitude is smaller than the altitude that can increase/decrease according to the climb rate within one second, then the reference of the altitude is set to the desired altitude and the reference of the climb rate will be set to zero.
• Case III: continuous climbing with the given climb rate.
  In this case, the reference trajectory of the altitude is generated by Equation (39) as well, but $R{C}_{default}$ is replaced by the given climb rate. No deceleration action is taken.
• Case IV: climb to desired altitude with the given rate.
  In this case, the reference trajectory of the altitude is generated by Equation (39) as well, but $R{C}_{default}$ is replaced by the given climb rate. This is the same if the statement in Case II is taken into consideration as the leader is approaching the desired altitude.

5.2. Formation Strategy: Follower

The follower’s formation strategies in horizontal and vertical motions are illustrated in Figure 5. In follower’s formation strategy, a follower is expected to receive two sets of data. The first data set includes the desired formation trajectories of itself and the followers that have a direct connection to it in the network structure. The second data set includes the states of the connected followers. How the blocks manipulate the received data sets and generate the control input is discussed in the following sections.

5.2.1. Formation Strategy in the Horizontal Motion

In the horizontal motion, the formation strategy is carried out by the consensus control and MPC. As mentioned in Section 2, the desired formation trajectories possess only the information of the desired position for the followers over the time horizon. Therefore, an algorithm of the finite difference method, the block FDM Algorithm in Figure 5, is added to the formation strategy in the horizontal motion to generate the required information for the consensus control.

Consider point ${\zeta }_i$ on given function $f\left(\zeta \right)$, the first to third derivative of ${\zeta }_i$ given by the finite difference method are shown as follows:

$$\begin{array}{ccc}\hfill f^{\prime }\left({\zeta }_i\right)& =& \frac{-f\left({\zeta }_{i+2}\right)+8f\left({\zeta }_{i+1}\right)-8f\left({\zeta }_{i-1}\right)+f\left({\zeta }_{i-2}\right)}{12h},\hfill \\ \hfill f^{″}\left({\zeta }_i\right)& =& \frac{-f\left({\zeta }_{i+2}\right)+16f\left({\zeta }_{i+1}\right)-30f\left({\zeta }_i\right)+16f\left({\zeta }_{i-1}\right)-f\left({\zeta }_{i-2}\right)}{12h^2},\hfill \\ \hfill f^{\prime \prime \prime }\left({\zeta }_i\right)& =& \frac{-f\left({\zeta }_{i+3}\right)+8f\left({\zeta }_{i+2}\right)-13f\left({\zeta }_{i+1}\right)+13f\left({\zeta }_{i-1}\right)-8f\left({\zeta }_{i-2}\right)+f\left({\zeta }_{i-3}\right)}{8h^3},\hfill \end{array}$$

(40)

where h is the time interval between each point. As shown in Equation (40), computing the third derivative of $f\left(\zeta \right)$ at ${\zeta }_i$ requires the points three steps prior to, and three steps after, ${\zeta }_i$. This would be a problem when dealing with the first three points and the last three points of the desired formation trajectory, since we don’t have knowledge of the points prior to the first point and after the last point of the desired formation trajectory. However, these points can be acquired by applying linear extrapolation backward in time to the first point and forward in time to the last point of the desired formation trajectory. Once the complete desired trajectory is generated, consensus control engages. The consensus control algorithm computes the flight paths of the followers that track the corresponding desired formation trajectories using the control input given in Equation (15). Here, we rearrange the states at time step k of the i-th follower as follows:

$${\mathit{s}}_i^{\left(1\right)}\left(k\right)=\left[\begin{array}{c}\Delta x\left(k\right)\\ \Delta y\left(k\right)\end{array}\right],{\mathit{s}}_i^{\left(2\right)}\left(k\right)=\left[\begin{array}{c}\Delta u\left(k\right)\\ \Delta v\left(k\right)\end{array}\right],{\mathit{s}}_i^{\left(3\right)}\left(k\right)=\left[\begin{array}{c}\Delta \theta \left(k\right)\\ \Delta \varphi \left(k\right)\end{array}\right],{\mathit{s}}_i^{\left(4\right)}\left(k\right)=\left[\begin{array}{c}\Delta q\left(k\right)\\ \Delta p\left(k\right)\end{array}\right].$$

(41)

The corresponding desired formation trajectory of each modified state $s_i^{\left(ks\right)}\left(k\right)$$\left(ks=1,2,3,4\right)$ at time step k is defined as ${\mathit{d}}_i^{\left(1\right)}\left(k\right)$, ${\mathit{d}}_i^{\left(2\right)}\left(k\right)$, ${\mathit{d}}_i^{\left(3\right)}\left(k\right)$ and ${\mathit{d}}_i^{\left(4\right)}\left(k\right)$. The error between the modified state and the corresponding desired formation trajectory at time step k can be written as:

$${\widehat{\mathit{s}}}_i^{\left(ks\right)}\left(k\right)={\mathit{s}}_i^{\left(ks\right)}\left(k\right)-{\mathit{d}}_i^{\left(ks\right)}\left(k\right),ks=1,2,3,4.$$

(42)

The consensus control input at time step k can be computed by using the modified states and the control gains ${\beta }_{ks}$ ($ks=1,2,3,4$). The control input will result in a two-dimensional column matrix. The first and second elements of the control input are the pitch and roll control commands, respectively. The next step is to apply the consensus control input to the discrete state-space model of (13). Repeating the steps above until the flight path over the predicted time horizon is obtained.

After the flight path of the follower under consideration is generated, it then serves as the reference trajectory for the MPC. The MPC obtains the optimal control sequence using Equation (29). First, rearrange the initial condition and the reference trajectory in the form of $\overline{x}$ described in Equation (27). With the discrete state-space model and the selected weighting matrices, the predicted trajectory and the control input sequence can be obtained by Equations (23) and (29). Then, the first control input of the optimal control sequence becomes the formation control input for the follower.

5.2.2. Formation Strategy in the Vertical Motion

The formation strategy in the vertical motion is simpler than the one in the horizontal motion. The state-space model that is adopted in the vertical motion is describe by Equation (38). As mentioned in Section 2, the desired formation trajectory generated by the leader possesses only the information of the altitude, namely $\Delta h$. Therefore, an algorithm of the finite-difference method, block FDM Algorithm in Figure 5 is added to the follower’s vertical formation strategy to generate the required information for the LQT.

With the complete desired formation trajectory and the selected weighting matrices, the LQT computes the optimal control sequence using Equation (19). The formation control input in the vertical motion for the follower is the first input of the optimal control sequence.

6. Simulation Preparation

This section introduces the configuration of the systems that are used in the simulation under the Matlab/Simulink environment, as well as the necessary setup for the simulation. A transformation of variables is carried out in Section 6.1 for coding purpose. In Section 6.2, the configuration of the simulation systems under the Matlab/Simulink environment are introduced block by block and leader to follower. To mention, one leader, three followers, and the network structure in Figure 1b are considered in the simulation.

6.1. Transformation of the Variable of the Discrete State-Space Model

The state-space model coded in the formation strategies is the discretized state-space model with a sampling time of 0.1 s. That is, the controls for the leader and the followers are expected to be updated every 0.1 s. For the convenience of coding the control algorithm in the horizontal motion, a transformation of variables is applied to the horizontal state-space model (13). The states and the controls in the horizontal state-space model (13) are arranged as ${\mathit{x}}_h$ and ${\mathit{U}}_h$ in Equation (43), respectively. The transformation of states $T_s$ and the transformation of controls $T_u$ are considered and shown as follows:

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill {\mathit{x}}_h& =\left[\begin{array}{c}\Delta x\\ \Delta y\\ \Delta u\\ \Delta v\\ \Delta \theta \\ \Delta \varphi \\ \Delta q\\ \Delta p\end{array}\right]=\left[\begin{array}{cccccccc}1& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& \frac{-1}{g}& 0& 0& 0\\ 0& 0& 0& 0& 0& \frac{1}{g}& 0& 0\\ 0& 0& 0& 0& 0& 0& \frac{-1}{g}& 0\\ 0& 0& 0& 0& 0& 0& 0& \frac{1}{g}\end{array}\right]\left[\begin{array}{c}\Delta x\\ \Delta y\\ \Delta u\\ \Delta v\\ \Delta \tilde{\theta }\\ \Delta \tilde{\varphi }\\ \Delta \tilde{q}\\ \Delta \tilde{p}\end{array}\right]=T_s{\tilde{\mathit{x}}}_h,\hfill \\ \hfill {\mathit{U}}_h& =\left[\begin{array}{c}\Delta M_{\theta }\\ \Delta M_{\varphi }\end{array}\right]=\left[\begin{array}{cc}\frac{-I_{yy}}{g}& 0\\ 0& \frac{I_{xx}}{g}\end{array}\right]\left[\begin{array}{c}\Delta {\tilde{M}}_{\theta }\\ \Delta {\tilde{M}}_{\varphi }\end{array}\right]=T_u{\tilde{\mathit{U}}}_h.\hfill \end{array}\end{array}$$

(43)

Applying the above transformation to the states and the controls in Equation (13), the normalized horizontal state-space model considering the modified state ${\tilde{\mathit{x}}}_h$ and the modified control ${\tilde{\mathit{U}}}_h$ can be obtained. The normalized system matrix ${\tilde{A}}_h$ and the control matrix ${\tilde{B}}_h$ are shown as follows:

$$\begin{array}{c}\hfill {\tilde{A}}_h=\left[\begin{array}{cccccccc}0& 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right],{\tilde{B}}_h=\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ 0& 0\\ 0& 0\\ 0& 0\\ 1& 0\\ 0& 1\end{array}\right].\end{array}$$

(44)

The discrete state-space model that is adopted by the formation strategies in the horizontal motion is obtained by applying the Matlab function c2d to the normalized continuous state-space model (44).

6.2. Simulink Environment Setup for the Formation Simulation

The basic configuration of the simulated quadrotor system under the Matlab/Simulink environment is shown in Figure 6. The system includes a Control System module, a Gravity Influence module, and a six degrees of freedom (6DOF) model module (Quad 6DOF Model). Both leader and followers adopt the same configuration with the difference in the Control System module and the input/output ports.

The Gravity Influence module computes the distribution of the quadrotor’s weight on the axes of frame $\mathit{B}$. The 6DOF rigid body EOMs solver inside the Quad 6DOF Model module is shown in Figure 7. Block 6DOF (Euler Angles) solves the nonlinear 6DOF EOMs of the rigid body with the given inputs with respect to frame $\mathit{B}$. The block is provided by the Simulink/Aerospace Blockset toolbox [17].

The Control System module basically includes a yaw control submodule, pitch/roll control submodule, and an altitude control submodule. Figure 8 shows the configuration of the leader’s Control System module. The follower’s Control System module differs from the leader’s Control System module in the pitch/roll control submodule. The leader’s and follower’s pitch/roll control submodule will be introduced later in the section. The yaw control submodule controls the yaw motion of the quadrotor based on the dual-loop proportional plus integral (PI) control. The pitch/roll and altitude controls carry out the formation control strategies depending on the role that it is a leader or a follower. Likewise, the input/output ports differ depending on the role of the quadrotor.

Figure 9 shows the configuration of the Altitude Control submodule of the leader and the follower. The Altitude Control submodule carries out the vertical formation strategies described in Section 5.1.2 for the leader and in Section 5.2.2 for the follower. In the leader’s Altitude Control submodule, the function Track Generator generates the reference trajectory Lv_ref for the LQT using the method provided in Section 5.1.2 depending on the vertical state of the leader, desired altitude, and climb rate. The vertical state of the leader, labeled as vN, is defined by Equation (38). The desired altitude and the climb rate, labeled as required_alt and required_RC, are given by the user. Function LQT tracks reference trajectory Lv_ref by solving Equation (19) with the given weighting matrices, vertical state vN, and vertical state-space model (38). Function LQT outputs thrust command dT, the follower’s reference trajectory F_vertical_ref, and leader’s velocity L_velocity_w on ${\widehat{\mathit{b}}}_3$ of frame $\mathit{B}$. In the follower’s Altitude Control submodule, function FDM Algorithm computes the complete reference trajectory Fv_ref consisting of the references for states $\Delta h$ and $\Delta w$ in Equation (38) using the finite-difference method (40). Then, function LQT solves the equations in (19) depending on the given weighting matrices, the vertical state of follower vN, and vertical state-space model (38). After the computation, function LQT outputs the thrust command dT. It is worth mentioning that the thrust command dT in both the leader’s and follower’s control submodules is the thrust deviation from the thrust at the equilibrium point $T_{total,0}$. Therefore, actual thrust command T_t in Figure 6 that is input into the 6DOF model equals the thrust deviation dT plus the equilibrium thrust $T_{total,0}$. Leader’s velocity L_velocity_w is used to compute the leader’s velocity in frame $\mathit{I}$ in the control strategy in horizontal motion.

Figure 10 and Figure 11 show the pitch/roll control submodule for the leader and the follower, respectively. The leader’s pitch/roll control submodule, MPC Control(Pitch, Roll) in Figure 8, includes the waypoints datasets (orange dashed square), the waypoints update rules (orange square), the block Preset Constant which includes the formation pattern and the parameters for the waypoint update rules, the MPC algorithm Leader MPC Algorithm, and the function Follower ref. Trajectory Generator that generates the followers’ reference trajectories as shown in Figure 10. The waypoints datasets in the orange dashed square includes the coordinate of the waypoints in frame $\mathit{I}$, named as L_wp, and the number of the waypoints, named as wp_length. The waypoint update rules update the waypoint if the distance between the leader and the waypoint, denoted by R2wp, is smaller than $d_2$ and hold at the last waypoint if R2wp is smaller than $d_2$. Parameter $d_1$ is used under the circumstances that if the user designates the followers to hold a specific position in which the last waypoint is reached. Function Leader MPC Algorithm solves the optimal tracking control input M by using Equation (29) with the given weighting matrices, the horizontal state of the leader, and the horizontal state-space model. The predicted trajectory for the leader is computed by Equation (23). Note that the horizontal state and the state-space model that are adopted in the Leader MPC Algorithm are the modified state ${\tilde{\mathit{x}}}_h$ and the normalized state-space model in Equation (44). In addition, the control output by the Leader MPC Algorithm is the modified control in Equation (43). Function Follower ref. Trajectory Generator computes the reference trajectories for the followers based on the method in Section 5.1.1 using the leader’s predicted trajectory and the formation pattern. The leader’s vertical velocity L_velocity_w is input into the function to compute the leader’s velocity in frame $\mathit{I}$ using Equation (8).

The control submodule shown in Figure 11 is the control submodule of Follower #1. The follower’s control submodule first computes the complete desired formation trajectories, including the information of the velocity, attitude, and angular velocity, using the finite difference method coded in function FDM Algorithm. Then, the function Consensus Algorithm computes all the followers’ flight paths over the predicted time horizon based on the consensus control (15), modified state ${\tilde{\mathit{x}}}_h$ (43), and the normalized state-space model (44). The signal, labeled as rN, is the modified state of the follower under consideration and the signal, labeled as rNF_all, contains all the modified states of other followers connected to it in the network structure. In the case shown in Figure 11, rN is the modified state of Follower #1 and rNF_all contains the modified states of Follower #2 and Follower #3. After the flight paths are computed, the function MPC algorithm engages. The MPC algorithm computes the optimal tracking control by using Equadtion (29) based on the modified state (43), the normalized state-space model (44) of the quadrotor, and the corresponding flight path.

7. Simulation Results

The simulation of a comprehensive maneuver is carried out to examine the performance of the proposed formation strategies. The parameters of the quadrotor, the gains of the consensus control, and the weighting matrices that are used in the simulation are given in Section 7.1. The simulation results of the comprehensive maneuver are shown in Section 7.2.

7.1. Quadrotor Parameter, Control Gain, and Weighting Matrices

This section summarizes the parameters of the quadrotor (

Table 4. Parameters of the quadrotor.

Parameter	Symbol	Value	Unit
Mass of the Quadrotor	m	1.5	$kg$
Acceleration of Gravity	g	9.807	$m/s^2$
Quadrotor Arm Length	L	0.225	m
	$I_{xx}$	0.022	$kg·m^2$
Moment of Inertia	$I_{yy}$	0.022	$kg·m^2$
	$I_{zz}$	0.0018	$kg·m^2$

Note: the above parameters were acquired from the quadrotor in the lab.

), weighting matrices, and consensus control gains that are used in the simulation.

In the simulation, we assume that all the states in both horizontal and vertical motions are obtainable. That is, output matrix C in the state-space model for both motions for leader and followers is an identity matrix. The weighting matrices for the LQT and QP problem for the leader are given as follows:

• Weighting matrices P, Q, and R of the unconstrained QP problem coded in simulink function Leader MPC Algorithm in Figure 10 for the leader’s horizontal motion control are given as follows:

  $$\begin{array}{ccc}\hfill Q& =& I_8,\hfill \\ \hfill R& =& 0.5I_2,\hfill \\ \hfill P& =& \left[\begin{array}{cccccccc}29.8176& 0& 37.9636& 0& 25.3123& 0& 7.1204& 0\\ 0& 29.8176& 0& 37.9636& 0& 25.3123& 0& 7.1204\\ 37.9636& 0& 86.4879& 0& 67.0493& 0& 20.8541& 0\\ 0& 37.9636& 0& 86.4879& 0& 67.0493& 0& 20.8541\\ 25.3123& 0& 67.0493& 0& 75.6689& 0& 26.9672& 0\\ 0& 25.3123& 0& 67.0493& 0& 75.6689& 0& 26.9672\\ 7.1204& 0& 20.8541& 0& 26.9672& 0& 18.4432& 0\\ 0& 7.1204& 0& 20.8541& 0& 26.9672& 0& 18.4432\end{array}\right].\hfill \end{array}$$

  (45)
• Weighting matrices P, Q, and R for the LQT coded in the simulink function LQT in Figure 9 for the leader’s altitude control are given as follows:

  $$P=Q=\left[\begin{array}{cc}3& 0\\ 0& 10\end{array}\right],R=0.5.$$

  (46)

As for the follower, the control gains for the consensus control in the horizontal motion and the weighting matrices for the LQT in vertical motion and the unconstrained QP problem in horizontal motion are given as follows:

• Consensus control gain ${\beta }_{ks}$ ($ks=1,2,3,4$) in the horizontal motion considering the states in (41) and the consensus control (15) coded in the Consensus Algorithm in Figure 11 are given as follows:

  $$\left({\beta }_1,{\beta }_2,{\beta }_3,{\beta }_4\right)=\left(13,30,60,5\right).$$

  (47)
• Weighting matrices P, Q, and R of the unconstrained QP problem coded in simulink function MPC Algorithm in Figure 11 for the follower’s horizontal motion control are given as follows:

  $$\begin{array}{ccc}\hfill Q& =& I_8,\hfill \\ \hfill R& =& 0.3I_2,\hfill \\ \hfill P& =& \left[\begin{array}{cccccccc}28.8987& 0& 35.3119& 0& 22.1493& 0& 5.5294& 0\\ 0& 28.8987& 0& 35.3119& 0& 22.1493& 0& 5.5294\\ 35.3119& 0& 78.6321& 0& 57.3341& 0& 15.6844& 0\\ 0& 35.3119& 0& 78.6321& 0& 57.3341& 0& 15.6844\\ 22.1493& 0& 57.3114& 0& 62.9640& 0& 19.4694& 0\\ 0& 22.1493& 0& 57.3114& 0& 62.9640& 0& 19.4694\\ 5.5294& 0& 15.6844& 0& 19.4694& 0& 12.6822& 0\\ 0& 5.5294& 0& 15.6844& 0& 19.4694& 0& 12.6822\end{array}\right].\hfill \end{array}$$

  (48)
• Weighting matrices P, Q, and R for the LQT coded in simulink function LQT in Figure 9 for the follower’s altitude control are given as follows:

  $$Q=P=\left[\begin{array}{cc}3& 0\\ 0& 10\end{array}\right],R=0.5.$$

  (49)

The selection of weighting matrix P for the unconstrained QP problem utilizes the methodology provided by Reference [18]. The method ensures the MPC objective function has the same quadratic cost as the infinite horizon quadratic cost used by LQR.

7.2. Simulation of the Comprehensive Maneuver

A simulation of the comprehensive maneuver that was carried out is outlined in this section along with the simulation results. The objective of the simulation was to examine whether the followers can carry out the formation flight, including maintaining the given formation pattern and tracking the desired altitude, with the proposed formation strategies while the leader performs a series of maneuvers.

Assume that two quests, mapping the landscape and taking photos of a target building’s exterior, are assigned to the leader. The predefined waypoints are illustrated in Figure 12. The first quest, mapping the landscape, is carried out through Waypoint #1 to Waypoint #9. An imaginary terrain is placed at waypoint #6 so that the leader has to change its altitude in order to cross the terrain. The leader is designated to climb to 20 meters with respect to the zeros reference altitude at the climb rate of 4 m/s at Waypoint #5 and descend to the zeros reference altitude at the climb rate of –4 m/s at Waypoint #7. The second quest, taking photos of a target building’s exterior, is carried through Waypoint #15 to Waypoint #32. The leader follows a circle trajectory in the $xy$ plane and climbs at the climb rate of 0.45 m/s at the same time. The climbing continues until the leader reaches 60 meters with respect to the zeros reference altitude. After all the quests are completed, the leader heads to its holding point at Waypoint #47 by following the Waypoint #33 to Waypoint #47. The leader will start to descend to zero reference altitude at Waypoint #38 at the climb rate of –2 m/s.

The triangle formation pattern is chosen and the simulation results are shown in the following figures. The trajectories of the quadrotors throughout the maneuver are shown in Figure 13. Figure 14 shows the top view of the quadrotors with the formation pattern throughout the flight. In Figure 13 and Figure 14, the x-axis points to the north, the y-axis points to the east, and the z-axis points upward, opposite to the $\widehat{\mathit{k}}$ axis in frame $\mathit{I}$. Figure 15 shows the altitude and climb rate of the quadrotors. The attitude of the leader and followers are shown in Figure 16, Figure 17 and Figure 18.

8. Discussion

In the simulation of the comprehensive maneuver, the leader performs a series of maneuvers while carrying out the assigned quests. The quadrotor trajectories in Figure 13 show that the followers carry out the formation flight well based on the proposed formation strategies. Furthermore, from the top view of the quadrotor trajectories in Figure 14, the designated formation pattern is maintained throughout the flight. Although the formation pattern slightly deforms during the turn, it recovers soon after the turn.

In the vertical motion, the leader climbs to the desired altitude at the designated climb rate in three stages, R1, R2, and R3, as shown in Figure 15. Moreover, the followers can keep up with the leader’s vertical motion with the proposed formation strategy in Section 5.2.2. In Figure 15, oscillation in the altitude and climb rate can be observed throughout the flight. The oscillation can be seen more clearly in Figure 19, which is a zoom-in view of the vertical motion responses in Figure 15 during the time interval of 190 to 210 seconds. The oscillation is caused by the coupling effects between the horizontal and vertical motions in the 6DOF nonlinear EOMs. The quadrotor tilts while tracking the waypoints as shown in Figure 16 and Figure 17, which disturbs the equilibrium condition and causes the altitude to change. Since the equilibrium condition is disturbed, the altitude control algorithm will engage to regain the equilibrium condition in the vertical motion and therefore result in the oscillating phenomenon.

In the horizontal motion, the responses of the pitch and roll angles in Figure 16 and Figure 17 show that the angles remain in an acceptable range ($\pm {25}^{\circ }$) throughout the flight. Furthermore, the yaw angle is well-controlled, close to zero, by the dual-loop PI control as shown in Figure 18. Additionally, the responses of the pitch and roll angles before and after the upward circling motion are shown in Figure 20 and Figure 21, respectively. Before the quadrotors initiate and after the quadrotors finish the upward circling motion, they are simply performing a straight flight with a ${90}^{\circ }$ turn. According to the results illustrated in Figure 20 and Figure 21, the formation strategies in the horizontal motion are fully adequate to perform the formation in straight flight. Even during the turn, the pitch and roll angles stay in an acceptable range ($\pm {25}^{\circ }$).

A zoom-in figure of the responses of the attitude during the upward circling motion is shown in Figure 22. The figure zooms in the responses in the time interval of 80 to 100 seconds during the upward circling motion. In this 20-second interval, the quadrotors travel about 1/4 of a circle in an $xy$ plane. During the upward circling motion, the leader has to change its direction frequently to stay on the circle track as well as the followers to stay in formation. The results in Figure 22 show that the pitch and roll angles are still within an acceptable range for leader and followers. The followers can keep up with leader’s horizontal motion, or, in other words, to stay in formation during the upward circling motion with the proposed formation strategy in Section 5.2.1. The yaw angles of the leader and followers are controlled closed to zero with the dual-loop PI control in the upward circling motion as shown in Figure 18 and Figure 22.

9. Conclusions

In this study, the formation control strategies for quadrotors considering the leader–follower architecture are proposed in Section 5. The leader is designated to track the predefined waypoints in horizontal motion and the desired altitude/climb rate in vertical motion. Furthermore, the leader generates the followers’ desired formation trajectories using the methods in Section 5.1.1 and Section 5.1.2 as a guidance for the followers to perform the formation flight. The follower’s formation strategy is proposed in Section 5.2. The consensus control and MPC with the QP problem are adopted in the horizontal formation control, and the MPC with the LQT is adopted in the vertical formation control. The formation control strategies are examined based on the 6DOF nonlinear simulation under the Matlab/Simulink environment and the simulation results are illustrated in Section 7. A discussion of the simulation results as well as the performance of the formation strategies are carried out in Section 8. The results of the simulation of comprehensive maneuver in Section 7.2 show that the proposed formation strategies are feasible in straight flight, turning, and upward circling motion.