Design of a Multi-Constraint Formation Controller Based on Improved MPC and Consensus for Quadrotors

Abstract

The formation flight of quadrotor unmanned aerial vehicles (UAVs) is a complex multi-constraint process. When designing a formation controller, the dynamic model of the UAV itself has modeling errors and uncertainties. Model predictive control (MPC) is one of the best control methods for solving the constrained problem. First, a mathematical model of the quadrotor considering disturbance and uncertainty is established using the Lagrange–Euler formulation and is divided into a rotational subsystem (RS) and a translational subsystem (TS). Here, an improved MPC (IMPC) strategy based on an error model is introduced for the control of UAVs. The tracking errors caused by synthesis disturbance can be eliminated because of the integrator embedded in the augmented model. In addition, by modifying the parameters of the cost function, not only can the degree of stability of the closed-loop subsystem be specified, but also numerical problems in the MPC calculation can be improved. The simulation results demonstrate the stability of the designed controller in formation maintenance and its robustness to external disturbances and uncertainties.

1. Introduction

The unmanned aerial vehicle (UAV) system is the fastest developing and most practical application in the field of unmanned systems, with its low cost, convenient operation, and flexible characteristics. The quadrotor has many advantages, including hovering, vertical take-off, landing ability, and indoor flight. With the diversification of application scenarios and missions, the capability of a single UAV is increasingly limited, and the coordination of multi-UAVs formation flight has become an important trend of unmanned aerial applications [1]. A team of low-cost UAVs could replace expensive multifunctional drones for large-scale, high-precision missions and increase mission success rates [2].

However, the quadrotor has six degrees of freedom, but only four inputs. Therefore, its dynamics are not only nonlinear but also coupled and under-actuated, which is difficult to control. In addition, the quadrotor will be affected by aerodynamic or random disturbance during the formation flight. Therefore, an advanced control strategy is needed to ensure the stability of autonomous formation flight.

Several strategies have recently been proposed to cope with the control problems in formation flight. Fu et al. studied the algorithm for the maintenance and reconstruction of formation combined with the artificial potential field [3]. Another paper by Liu et al. focuses on the visual servoing switching-topology control approach for formation tracking problems [4]. Zhen et al. investigated multivariate adaptive control based on consensus to address the disturbances and uncertainties present in the formation [5]. Yu et al. developed a strategy for multi-UAVs with actuator faults and wind effects [6]. Wolfe et al. verified the application of distributed multi-model Model Predictive Control (MPC) in tracking ground targets by quadrotor. The control algorithm was implemented by the on-board computer of the UAV [7]. Huang et al. proposed a new method for cooperative collision avoidance based on the Kalman filter and MPC without considering the constrains and uncertainties of the UAV [8]. Kuriki et al. designed a formation controller with obstacle avoidance based on distributed MPC and linearized model [9]. A controller for inward collision avoidance based on nonlinear model predictive control (NMPC) was designed by Zhou et al., where the model was first order in two-dimensional space [10]. Liao et al. developed a distributed cascade robust feedback controller for formation that takes into account the dynamic constraints of the UAV [11]. The paper by Hegde et al. covered the applicability of multi-agent systems to agriculture and the design of distance-based control algorithms to maintain a specific formation of UAVs [12]. Zhang et al. investigated the control problem of high-order linear systems in the presence of time delays [13]. Shadeed et al. proposed a trajectory tracking method using a mapped desired output into input sequence based on differential flatness modeling [14]. However, most of the UAV systems in the literature are based on either two-dimensional space or ideal models [15,16,17,18], without considering the disturbances and uncertainties. In addition, the performance of model-based controllers relies heavily on the accuracy of the modeling. On the one hand, it is difficult to obtain an accurate model of the quadrotor due to its complex aerodynamic characteristics. On the other hand, small UAVs are more sensitive to disturbances, such as gusts or load changes. Therefore, overlooking these two factors will cause the performance of the controller and the stability of the formation to deteriorate.

Yan et al. proposed a consensus-based sliding mode controller (SMC) for formation with disturbances in a three-dimensional environment [19]. Huang et al. investigated the distributed finite-time formation controller based on a disturbance observer (DOB) and SMC [20]. Liang et al. combined a high-order sliding mode DOB and back-stepping control to solve the suspension failures in payload formation transport [21]. Messai et al. designed a robust controller for linear cluster systems subject to state constraints [22]. Guo et al. investigated a learning-based formation algorithm with obstacle avoidance and anti-disturbance [23]. Thien et al. designed a PID controller and an SMC to cope with constant and time-varying disturbances or commands in the formation, respectively [24]. Liu et al. proposed a distributed adaptive fault-tolerant control to cope with multiple uncertainties and actuator failures in the formation [25]. Wang et al. designed a distributed formation controller based on sliding mode theory using nearby information [26]. Wu et al. solved the obstacle avoidance problems in UAV formations by combining a particle swarm optimization algorithm and MPC [27]. Dubay et al. investigated the problem of the collision avoidance of UAVs in the process of reaching consensus [28]. However, most of these studies were based on ideal models and the physical constraints of UAVs were not paid enough attention [29], which is not in line with reality.

Essentially, the core of MPC is to optimize the future control trajectory subject to plant constraints at each step of the control period. It is then guaranteed that the calculated controls will be optimal for the real-time situation. Therefore, MPC is one of the most effective control methods to solve constrained problems [30]. In fact, there are already some examples of the application of MPC to single UAV or formation control, such as in [11,31,32,33,34,35,36]. However, the discrete-time model of the UAV is critically stable. In the process of rolling optimization, this critical stability may be further deteriorated, which is ignored by many studies. In this paper, a modified multi-constrained MPC is designed for multiple UAVs to achieve the stability of the formation and trajectory tracking. Because of the addition of integrators to the augmented model, it is possible to eliminate the effect of disturbances and model uncertainties on the performance of the controller without adding disturbance observers.

A three-dimensional dynamic model of the UAV considering disturbance and uncertainty is described in Section 2. Multi-constrained MPCs for translational subsystem (TS) and rotational subsystem (RS) are presented in Section 3. The stability of the previously designed algorithm is provided in Section 4. Comparison and simulation results are given in Section 5. Section 6 provides a summary and directions for future work.

2. Problem Description

2.1. Dynamic Model of the Quadrotor

The UAV structure considered in this article is identical to that shown in Figure 1. For the ith quadrotor, the dynamic model considering disturbance and uncertainty can be obtained by the Lagrange method [37]:

$$\left\{\begin{array}{l}{\ddot{\varphi }}_i=a_1{u}_{i,1}+b_1{\dot{\theta }}_i{\dot{\psi }}_i+b_2{\mathsf{\Omega }}_{\mathrm{r}}{\dot{\theta }}_i+\Delta f_1+d_1\\ {\ddot{\theta }}_i=a_2{u}_{i,2}+b_3{\dot{\psi }}_i{\dot{\varphi }}_i-b_4{\mathsf{\Omega }}_{\mathrm{r}}{\dot{\varphi }}_i+\Delta f_2+d_2\\ {\ddot{\psi }}_i=a_3{u}_{i,3}+b_5{\dot{\varphi }}_i{\dot{\theta }}_i+\Delta f_3+d_3\\ {\ddot{x}}_i=u_{i,x}/m+\Delta f_4+d_4\\ {\ddot{y}}_i=u_{i,y}/m+\Delta f_5+d_5\\ {\ddot{z}}_i=u_{i,z}/m+\Delta f_6+d_6\end{array}\right.$$

(1)

where $(x_i,y_i,z_i)$ are the states of position; $({\theta }_i,{\varphi }_i,{\psi }_i)$ are pitch, roll, and yaw angle, respectively; $m$ is the mass of the UAV; $u_{i,x}$, $u_{i,y}$, $u_{i,z}$, and $u_{i,j}\left(j=1,2,3\right)$ are variables with control inputs; ${\mathsf{\Omega }}_{\mathrm{r}}$ is the residual angular speed of the propellers; $f_i,d_i$$\left(j=1,\dots ,6\right)$ are uncertainty and unknown disturbance, respectively; and $\left|d_i\right|\le \overline{d}$, in which $\overline{d}$ is a bounded positive number. The expressions of $a_j(j=1,2,3)$ and $b_k(k=1,2,3,4,5)$ can be found in [37]. $u_{i,x}$, $u_{i,y}$, and $u_{i,z}$ are as follows:

$$\left\{\begin{array}{l}{u}_{i,x}=u_{i,4}\left(\cos {\varphi }_i\sin {\theta }_i\cos {\psi }_i+\sin {\varphi }_i\sin {\psi }_i\right)\\ u_{i,y}=u_{i,4}\left(\cos {\varphi }_i\sin {\theta }_i\sin {\psi }_i-\sin {\varphi }_i\cos {\psi }_i\right)\\ u_{i,z}=mg-u_{i,4}\cos {\varphi }_i\cos {\theta }_i\end{array}\right.$$

(2)

where $u_{i,4}$ is the input of the translational subsystem and $g$ is the acceleration of gravity.

2.2. Linear Discrete-Time Model of UAV

To facilitate the design of the MPC, the dynamic model of the quadrotor is divided into a rotational subsystem (RS) and translational subsystem (TS), and then linearized and discretized. For the RS, considering the disturbance, the linear time-varying discrete state-space model is as follows:

$$\left\{\begin{array}{l}{\mathit{X}}_{i,R}(k+1)={\mathit{A}}_{i,R}{\mathit{X}}_{i,R}(k)+{\mathit{B}}_{i,R}{\mathit{U}}_{i,R}(k)+{\mathit{G}}_{D,R}{\mathit{D}}_{i,R}(k)\hfill \\ {\mathit{Y}}_{i,R}(k+1)={\mathit{C}}_{i,R}\mathit{X}(k+1)\hfill \end{array}\right.$$

(3)

where ${\mathit{X}}_{i,R}(k)=\left[{\varphi }_i(k);{\dot{\varphi }}_i(k);{\theta }_i(k);{\dot{\theta }}_i(k);{\psi }_i(k);{\dot{\psi }}_i(k)\right]$ is the state vector of the RS, ${\mathit{U}}_{i,R}(k)=\left[u_{i,2};u_{i,3};u_{i,4};{\mathsf{\Omega }}_{i,r}\right]$ is the input vector, and ${\mathit{D}}_{i,R}(k)=\left[0;{\widehat{d}}_{i,R,1}(k);\right.0;{\widehat{d}}_{i,R,2}(k);0;\left.{\widehat{d}}_{i,R,3}(k)\right]$ is the synthesis of disturbance and uncertainty, where ${\widehat{d}}_{i,R,j}(k)=\Delta f_j+d_j,\left(j=1,2,3\right)$. When the sampling period of the RS is $T_R$, ${\mathit{A}}_{i,R}, {\mathit{B}}_{i,R}, {\mathit{G}}_{D,R}$, and ${\mathit{C}}_{i,R}$ in (4) are:

$$\begin{array}{cc}\hfill & {\mathit{A}}_{i,R}=\left[\begin{array}{cccccc}1& T_R& 0& 0& 0& 0\\ 0& 1& 0& b_1{T}_R{\dot{\psi }}_i& 0& b_1{T}_R{\dot{\theta }}_i\\ 0& 0& 1& T_R& 0& 0\\ 0& b_3{T}_R{\dot{\psi }}_i& 0& 1& 0& b_3{T}_R{\dot{\varphi }}_i\\ 0& 0& 0& 0& 1& T_R\\ 0& b_5{T}_R{\dot{\theta }}_i& 0& b_5{T}_R{\dot{\varphi }}_i& 0& 1\end{array}\right],{\mathit{B}}_{i,R}=\left[\begin{array}{cccc}0& 0& 0& 0\\ T_R{a}_1& 0& 0& T_R{b}_2{\dot{\theta }}_i\\ 0& 0& 0& 0\\ 0& T_R{a}_2& 0& -T_R{b}_4{\dot{\theta }}_i\\ 0& 0& 0& 0\\ 0& 0& T_R{a}_3& 0\end{array}\right]\hfill \\ \hfill & {\mathit{G}}_{D,R}=\left[\begin{array}{cccccc}0\hfill & 0& 0& 0& 0& 0\\ 0\hfill & T_R& 0& 0& 0& 0\\ 0\hfill & 0& 0& 0& 0& 0\\ 0\hfill & 0& 0& T_R& 0& 0\\ 0\hfill & 0& 0& 0& 0& 0\\ 0\hfill & 0& 0& 0& 0& T_R\end{array}\right],{\mathit{C}}_{i,R}=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 1& 0\end{array}\right]\hfill \end{array}$$

(4)

Similarly, the linear discrete-time model of TS is as follows:

$$\left\{\begin{array}{l}{\mathit{X}}_{i,T}(k+1)={\mathit{A}}_{i,T}{\mathit{X}}_{i,T}(k)+{\mathit{B}}_{i,T}{\mathit{U}}_{i,T}(k)+{\mathit{G}}_{D,T}{\mathit{D}}_{i,T}(k)\hfill \\ {\mathit{Y}}_{i,T}(k+1)={\mathit{C}}_{i,T}{\mathit{X}}_{i,T}(k+1)\hfill \end{array}\right.$$

(5)

where ${\mathit{X}}_{i,T}(k)=\left[x_i(k);{\dot{x}}_i(k);y_i(k);{\dot{y}}_i(k);z_i(k);{\dot{z}}_i(k)\right]$ is the state vector of the TS, ${\mathit{U}}_{i,T}(k)=\left[u_{i,x};u_{i,y};u_{i,z}\right]$ is the input vector, and ${\mathit{D}}_{i,T}=\left[0;{\widehat{d}}_{i,T,4}(k);0;\right.{\widehat{d}}_{i,T,5}(k);0;\left.{\widehat{d}}_{i,T,6}(k)\right]$ is the synthesis of disturbance and uncertainty, where ${\widehat{d}}_{i,T,j}(k)=\Delta f_j+d_j,\left(j=4,5,6\right)$. When the sampling period of the TS is $T_T$, ${\mathit{A}}_{i,T}, {\mathit{B}}_{i,T}, {\mathit{G}}_{D,T}$, and ${\mathit{C}}_{i,T}$ in (6) are:

$$\begin{array}{cc}\hfill & {\mathit{A}}_{i,T}=\left[\begin{array}{cccccc}1\hfill & T_T& 0& 0& 0& 0\\ 0\hfill & 1& 0& 0& 0& 0\\ 0\hfill & 0& 1& T_T& 0& 0\\ 0\hfill & 0& 0& 1& 0& 0\\ 0\hfill & 0& 0& 0& 1& T_T\\ 0\hfill & 0& 0& 0& 0& 1\end{array}\right],{\mathit{B}}_{i,T}=\left[\begin{array}{ccc}0& 0& 0\\ T_T/m& 0& 0\\ 0& 0& 0\\ 0& T_T/m& 0\\ 0& 0& 0\\ 0& 0& T_T/m\end{array}\right]\hfill \\ \hfill & {\mathit{G}}_{D,T}=\left[\begin{array}{cccccc}0\hfill & 0& 0& 0& 0& 0\\ 0\hfill & T_T& 0& 0& 0& 0\\ 0\hfill & 0& 0& 0& 0& 0\\ 0\hfill & 0& 0& T_T& 0& 0\\ 0\hfill & 0& 0& 0& 0& 0\\ 0\hfill & 0& 0& 0& 0& T_T\end{array}\right],{\mathit{C}}_{i,T}=\left[\begin{array}{cccccc}1\hfill & 0& 0& 0& 0& 0\\ 0\hfill & 0& 1& 0& 0& 0\\ 0\hfill & 0& 0& 0& 1& 0\end{array}\right]\hfill \end{array}$$

(6)

2.3. Formation Algorithm Based on Consensus

Since this paper focuses on the improvement of control performance as well as trajectory tracking capability, communication loss is not considered and it is assumed that all states of the UAVs can be shared among each other. Only the consensus of the position is considered here.

The undirected graph composed of $n$ UAVs is given as $\mathsf{\Lambda }=\{\Delta ,\epsilon \}$, where $\Delta =\{{\Delta }_1,{\Delta }_2,\dots ,{\Delta }_n\}$ is the set of quadrotors; $\epsilon \subseteq \Delta \times \Delta$ is the edge of the graph, which means the states can be obtained between UAVs; and $e_{ij}$ indicates the weight of the edge between ${\Delta }_i$ and ${\Delta }_j$. $e_{ij}=1$ if ${\Delta }_i$ and ${\Delta }_j$ can receive information from each other; otherwise, $e_{ij}=0$. Take the x-axis of TS as an example:

$$\left\{\begin{array}{l}\dot{x}=\dot{x}\\ \ddot{x}=u_{i,x}/m\end{array}\right.$$

(7)

The consensus law for the fixed topology of the formation on the x-axis is [7]:

$$u_{i,x,c}(t)=-{\displaystyle {\sum }_{j=1}^n{e}_{ij}}[k_x(x_i(t)-x_j(t)-p_{ij})],i=1,\dots ,n$$

(8)

where $u_{i,x,c}(t)$ is the component of consensus control low on the x-axis, $p_{ij}$ is the distance between ${\mathrm{UAV}}_i$ and ${\mathrm{UAV}}_j$, and $k_x$ is the control coefficient that meets $k_x>0$. The consensus laws $u_{i,y,c}(t)$, $u_{i,z,c}(t)$ of the y- and z-axis, respectively, can be obtained in the same way.

The structure of the formation controller is shown in Figure 2. The stability and accuracy of the formation trajectory tracking are determined by both the consensus control law and IMPC.

3. Multi-Constrained MPC

The multi-UAV system is a complex system with multiple constraints. MPC is one of the most effective methods to solve constrained control problems. Figure 3 illustrates the MPC structure of the ith quadrotor, where the planned path and the location of the neighbors determine the reference trajectory of the TS. The reference angle is generated by the TS and passed to the RS to stabilize the quadrotor.

3.1. MPC of TS

As shown in Figure 3, the controller of the TS is independent of the RS, while the results of the TS can provide reference values for the RS. Therefore, the MPC of the TS is implemented first. The difference equation of (5) is:

$$\begin{array}{l}{\mathit{X}}_{i,T}\left(k+1\right)-{\mathit{X}}_{i,T}\left(k\right)={\mathit{A}}_{i,T}\left({\mathit{X}}_{i,T}\left(k\right)-{\mathit{X}}_{i,T}\left(k-1\right)\right)+{\mathit{B}}_{i,T}\left({\mathit{U}}_{i,T}(k)-{\mathit{U}}_{i,T}(k-1)\right)\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em} +{\mathit{G}}_{i,D,T}\left({\mathit{D}}_{i,T}(k)-{\mathit{D}}_{i,T}(k-1)\right)\end{array}$$

(9)

The augmented system is derived as follows:

$$\left\{\begin{array}{l}{\widehat{\mathit{X}}}_{i,T}\left(k+1\right)={\widehat{\mathit{A}}}_{i,T}{\widehat{\mathit{X}}}_{i,T}\left(k\right)+{\widehat{\mathit{B}}}_{i,T}\left({\mathit{U}}_{i,T}(k)-{\mathit{U}}_{i,T}(k-1)\right)+{\widehat{\mathit{G}}}_{i,D,T}{\mathit{e}}_D(k)\\ {\mathit{Y}}_{i,T}\left(k+1\right)={\widehat{\mathit{C}}}_{i,T}{\widehat{\mathit{X}}}_{i,T}\left(k+1\right)\end{array}\right.$$

(10)

where ${\widehat{\mathit{X}}}_{i,T}\left(k+1\right)=\left[\begin{array}{c}{\mathit{X}}_{i,T}\left(k+1\right)-{\mathit{X}}_{i,T}\left(k\right)\\ {\mathit{Y}}_{i,T}\left(k+1\right)\end{array}\right]$, ${\widehat{\mathit{A}}}_{i,T}=\left[\begin{array}{cc}{\mathit{A}}_{i,T}& {\mathbf{0}}_{6\times 3}\\ {\mathit{C}}_{i,T}{\mathit{A}}_{i,T}& {\mathit{I}}_{3\times 3}\end{array}\right]$, ${\widehat{\mathit{B}}}_{i,T}=\left[\begin{array}{c}{\mathit{B}}_{i,T}\\ {\mathit{C}}_{i,T}{\mathit{B}}_{i,T}\end{array}\right]$, ${\widehat{\mathit{G}}}_{i,\mathrm{d},T}=\left[\begin{array}{c}{\mathit{G}}_{i,\mathrm{d},T}\\ {\mathit{C}}_{i,T}{\mathit{G}}_{i,\mathrm{d},T}\end{array}\right]$, and ${\widehat{\mathit{C}}}_{i,T}=\left[\begin{array}{cc}{\mathbf{0}}_{3\times 6}& {\mathit{I}}_{3\times 3}\end{array}\right]$. ${\mathit{e}}_D(k)={\mathit{D}}_{i,T}(k)-{\mathit{D}}_{i,T}(k-1)$ is a set of integrated white noise. Since the expected values of states are used in the calculation of the MPC, ${\mathit{e}}_D(k)=0$. Assuming that $N_{c,T}$ is the number of future controls and $N_{p,T}$ is the length of the prediction, the predicted output vector ${\widehat{\mathit{Y}}}_{i,T}$ and future control vector $\Delta {\widehat{\mathit{U}}}_{i,T}$ at time k are as follows:

$$\begin{array}{l}{\widehat{\mathit{Y}}}_{i,T}\left(k\right)=\left[{\mathit{Y}}_{i,T}\left(k+1|k\right);{\mathit{Y}}_{i,T}\left(k+2|k\right);\dots ;{\mathit{Y}}_{i,T}\left(k+N_{p,T}|k\right)\right]\\ \Delta {\widehat{\mathit{U}}}_{i,T}(k)=\left[\Delta {\mathit{U}}_{i,T}(k);\Delta {\mathit{U}}_{i,T}(k+1);\dots ;\Delta {\mathit{U}}_{i,T}(k+N_{c,T}-1)\right]\end{array}$$

(11)

The predictive equation for future outputs within one optimization window is, by substitution:

$${\widehat{\mathit{Y}}}_{i,T}\left(k\right)={\mathit{M}}_{i,T}{\widehat{\mathit{X}}}_{i,T}\left(k|k\right)+{\mathit{N}}_{i,T}\Delta {\widehat{\mathit{U}}}_{i,T}(k)$$

(12)

where ${\mathit{M}}_{i,T}$ and ${\mathit{N}}_{i,T}$ are represented as follows:

$$\begin{array}{cc}\hfill & {\mathit{M}}_{i,T}=\left[\begin{array}{c}{\widehat{\mathit{C}}}_{{}_{i,T}}{\widehat{\mathit{A}}}_{i,T}\\ {\widehat{\mathit{C}}}_{i,T}{\widehat{\mathit{A}}}_{i,T}^2\\ {\widehat{\mathit{C}}}_{i,T}{\widehat{\mathit{A}}}_{i,T}^3\\ ⋮\\ {\widehat{\mathit{C}}}_{i,T}{\widehat{\mathit{A}}}_{i,T}^{N_{p,T}}\end{array}\right]\hfill \\ \hfill & {\mathit{N}}_{i,T}=\left[\begin{array}{ccccc}{\widehat{\mathit{C}}}_{i,T}{\widehat{\mathit{B}}}_{i,T}& 0& 0& \dots & 0\\ {\widehat{\mathit{C}}}_{i,T}{\widehat{\mathit{A}}}_{i,T}{\widehat{\mathit{B}}}_{i,T}& {\widehat{\mathit{C}}}_{i,T}{\widehat{\mathit{B}}}_{i,T}& 0& \dots & 0\\ {\widehat{\mathit{C}}}_{i,T}{\widehat{\mathit{A}}}_{i,T}^2{\widehat{\mathit{B}}}_{i,T}& {\widehat{\mathit{C}}}_{i,T}{\widehat{\mathit{A}}}_{i,T}{\widehat{\mathit{B}}}_{i,T}& {\widehat{\mathit{C}}}_{i,T}{\widehat{\mathit{B}}}_{i,T}& \dots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ {\widehat{\mathit{C}}}_{i,T}{\widehat{\mathit{A}}}_{i,T}^{N_{p,T}-1}{\widehat{\mathit{B}}}_{i,T}& {\widehat{\mathit{C}}}_{i,T}{\widehat{\mathit{A}}}_{i,T}^{N_{p,T}-2}{\widehat{\mathit{B}}}_{i,T}& {\widehat{\mathit{C}}}_{i,T}{\widehat{\mathit{A}}}_{i,T}^{N_{p,T}-3}{\widehat{\mathit{B}}}_{i,T}& \dots & {\widehat{\mathit{C}}}_{i,T}{\widehat{\mathit{A}}}_{i,T}^{N_{p,T}-N_{c,T}}{\widehat{\mathit{B}}}_{i,T}\end{array}\right]\hfill \end{array}$$

(13)

The essence of the constrained MPC is to minimize $J_{i,T}(k)$ at each sample k by solving the optimal control vector $\Delta {\widehat{\mathit{U}}}_{i,T}$, and this minimization is based on the real-time optimization of $J_{i,T}(k)$ constrained by a series of linear inequalities. For ${\mathrm{UAV}}_i$, if the expected states at time k are ${\mathit{r}}_s(k\left|k\right.)=\left[x_r(k\left|k\right.);{\dot{x}}_r(k\left|k\right.);y_r(k\left|k\right.);{\dot{y}}_r(k\left|k\right.);\right.z_r(k\left|k\right.);\left.{\dot{z}}_r(k\left|k\right.)\right]$, the state vector-matrix for the new augmented system is rewritten as ${\overline{\mathit{X}}}_{i,T}(k+i|k)=\left[\Delta {\mathit{X}}_{i,T}(k+i|k);\mathit{Y}(k+i|k)-{\mathit{C}}_{i,T}{\mathit{r}}_s(k\left|k\right.)\right]$. $J_{i,T}(k)$ and the linear inequality constraints are as follows:

$$\begin{array}{l}\underset{\Delta {\mathit{U}}_{i,T}}{\min }{J}_{i,T}\left(k\right)={\displaystyle \sum _{i=1}^{N_{p,T}}{\overline{\mathit{X}}}_{i,T}{\left(k+i|k\right)}^T}{\overline{\mathit{Q}}}_{i,T}{\overline{\mathit{X}}}_{i,T}\left(k+i|k\right)\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}+{\displaystyle \sum _{j=0}^{N_{c,T}-1}\Delta {\mathit{U}}_{i,T}{(k+j)}^T{\overline{\mathit{R}}}_{i,T}\Delta {\mathit{U}}_{i,T}(k+j)}\\ \mathrm{subject} \mathrm{to}: {\mathsf{\Gamma }}_{i,T}\Delta {\mathit{U}}_{i,T}(k)\le {\mathsf{\Theta }}_{i,T}\end{array}$$

(14)

where ${\overline{\mathit{Q}}}_{i,T}={\widehat{\mathit{C}}}_{i,T}^{\mathrm{T}}{\widehat{\mathit{Q}}}_{i,T}{\widehat{\mathit{C}}}_{i,T}$, ${\widehat{\mathit{Q}}}_{i,T}>0$, and ${\overline{\mathit{R}}}_{i,T}\ge 0$. ${\mathsf{\Gamma }}_{i,T}$ and ${\mathsf{\Theta }}_{i,T}$ represent the matrices when the constraints are converted to $\Delta {\mathit{U}}_{i,T}$ [31].

The optimality and feasibility of this optimization problem is evaluated by the Kuhn–Tucker conditions of (14), which are satisfied. In order to minimize the cost function subject to constraints, the Lagrange form is obtained as:

$$\begin{array}{l}\underset{\Delta {\widehat{\mathit{U}}}_{i,T},{\lambda }_{i,T}}{\min }{L}_{i,T}\left(\Delta {\widehat{\mathit{U}}}_{i,T},{\mathbf{\lambda }}_{i,T}\right)={\left({\mathit{R}}_{s,T}-{\overline{\mathit{Y}}}_{i,T}\right)}^T{\mathit{Q}}_{i,T}\left({\mathit{R}}_{s,T}-{\overline{\mathit{Y}}}_{i,T}\right)\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}+\Delta {\widehat{\mathit{U}}}_{i,T}^T{\mathit{R}}_{i,T}\Delta {\widehat{\mathit{U}}}_{i,T}+{\mathbf{\lambda }}_{i,T}^T\left({\mathsf{\Gamma }}_{i,T}\Delta {\widehat{\mathit{U}}}_{i,T}-{\mathsf{\Theta }}_{i,T}\right)\end{array}$$

(15)

where ${\mathit{R}}_{s,T}\left(k\left|k\right.\right)=\left[{\widehat{\mathit{C}}}_{i,T}{\widehat{\mathit{r}}}_s\left(k+1\left|k\right.\right);{\widehat{\mathit{C}}}_{i,T}{\widehat{\mathit{r}}}_s\left(k+2\left|k\right.\right);\dots ;{\widehat{\mathit{C}}}_{i,T}{\widehat{\mathit{r}}}_s\left(k+N_p\left|k\right.\right)\right]$ is the expected outputs matrix and ${\widehat{\mathit{r}}}_s\left(k+1\left|k\right.\right)=\left[0_{6\times 1};{\mathit{C}}_{i,T}{\mathit{r}}_s\left(k+1\left|k\right.\right)\right]$. ${\overline{\mathit{Y}}}_{i,T}\left(k\right)={\mathit{M}}_{i,T}{\overline{\mathit{X}}}_{i,T}\left(k\left|k\right.\right)+{\mathit{N}}_{i,T}\Delta {\widehat{\mathit{U}}}_{i,T}(k)$. ${\mathit{Q}}_{i,T}$ and ${\mathit{R}}_{i,T}$ are block diagonal matrices composed by ${\overline{\mathit{Q}}}_{i,T}$ and ${\overline{\mathit{R}}}_{i,T}$, respectively. To decrease the computational burden of the constrained optimization problem, the active set methods (ASM) are applied in this paper. The specific process involves finding the inactive constraints and removing them at each moment of the rolling optimization process; only the active constraints are involved in the computational process of optimization [25]. Without losing generality, we assume that the desired output in one prediction window is slowly changing. By substituting ${\overline{\mathit{Y}}}_{i,T}\left(k\right)$ into (15), the problem is equivalent to:

$$\begin{array}{l}\underset{\Delta {\widehat{\mathit{U}}}_{i,T},{\lambda }_{i,T}}{\min }{L}_{i,T}\left(\Delta {\widehat{\mathit{U}}}_{i,T},{\mathbf{\lambda }}_{i,T}\right)=\Delta {\widehat{\mathit{U}}}_{i,T}^T\left({\mathit{N}}_{i,T}^T{\mathit{Q}}_{i,T}{\mathit{N}}_{i,T}+{\mathit{R}}_{i,T}\right)\Delta {\widehat{\mathit{U}}}_{i,T}\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}+2\Delta {\widehat{\mathit{U}}}_{i,T}{\mathit{N}}_{i,T}^T{\mathit{Q}}_{i,T}\left({\mathit{M}}_{i,T}{\overline{\mathit{X}}}_{i,T}\left(k\left|k\right.\right)-{\mathit{R}}_{s,T}\right)\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}+{\mathbf{\lambda }}_{i,T}^T\left({\mathsf{\Gamma }}_{i,T}\Delta {\widehat{\mathit{U}}}_{i,T}-{\mathsf{\Theta }}_{i,T}\right)\end{array}$$

(16)

By taking derivatives of (16) with respect to $\Delta {\widehat{\mathit{U}}}_{i,T}$ and ${\mathbf{\lambda }}_{i,T}$, the optimal control vector and Lagrange multiplier of ${\mathrm{UAV}}_i$ are obtained as follows:

$$\begin{array}{l}\Delta {\widehat{\mathit{U}}}_{i,T}\left(k\right)=-{({\mathit{N}}_{i,T}^T{\mathit{Q}}_{i,T}{\mathit{N}}_{i,T}+{\mathit{R}}_{i,T})}^{-1}\left({\mathit{N}}_{i,T}^T{\mathit{Q}}_{i,T}\right.\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\left.\left.\left({\mathit{M}}_{i,T}{\overline{\mathit{X}}}_{i,T}\left(k\left|k\right.\right)\right.-{\mathit{R}}_{s,T}\left(k\left|k\right.\right)\right)+\frac{{\mathsf{\Gamma }}_{i,T}^T{\mathbf{\lambda }}_{i,T}\left(k\right)}{2}\right)\\ {\mathbf{\lambda }}_{i,T}\left(k\right)=-2{\left({\mathsf{\Gamma }}_{i,T}{\left({\mathit{N}}_{i,T}^T{\mathit{Q}}_{i,T}{\mathit{N}}_{i,T}+{\mathit{R}}_{i,T}\right)}^{-1}{\mathsf{\Gamma }}_{i,T}^T\right)}^{-1}\left({\mathsf{\Theta }}_{i,T}\right.\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\left.+{\mathsf{\Gamma }}_{i,T}{\left({\mathit{N}}_{i,T}^T{\mathit{Q}}_{i,T}{\mathit{N}}_{i,T}+{\mathit{R}}_{i,T}\right)}^{-1}{\mathit{N}}_{i,T}^T{\mathit{Q}}_{i,T}\left({\mathit{M}}_{i,T}{\overline{\mathit{X}}}_{i,T}\left(k\left|k\right.\right)-{\mathit{R}}_{s,T}\left(k\left|k\right.\right)\right)\right)\end{array}$$

(17)

where ${\mathbf{\lambda }}_{i,T}\left(k\right)\ge 0$ can be calculated using Hildreth’s quadratic programming procedure for each sample k. For each component of ${\mathbf{\lambda }}_{i,T}\left(k\right)$, if the Lagrange multiplier is positive, the corresponding component of $\Delta {\widehat{\mathit{U}}}_{i,T}\left(k\right)$ is a locally optimal point. For a negative Lagrange multiplier, the corresponding constraint can be omitted; that is, the corresponding component of ${\mathbf{\lambda }}_{i,T}\left(k\right)$ is set to zero. Then $\Delta {\widehat{\mathit{U}}}_{i,T}\left(k+1\right)$ and ${\mathbf{\lambda }}_{i,T}\left(k+1\right)$ can be obtained by continuous computation in the next step.

Remark 1. 

After solving the optimal problem, only the first row of $\Delta {\widehat{\mathit{U}}}_{i,T}$ is used, and the control signal for the next optimization window is $\left[u_{i,x,m}(k+1);\right.u_{i,y,m}(k+1);$ $\left.u_{i,z,m}(k+1)\right]={\mathit{U}}_{i,T}\left(k\right)+\Delta {\widehat{\mathit{U}}}_{i,T}$, while the control law considering consensus and the MPC is $\left[u_{i,x}(k+1);u_{i,y}(k+1);u_{i,z}(k+1)\right]=\left[u_{i,x,m}(k+1);\right.\left.u_{i,y,m}(k+1);u_{i,z,m}(k+1)\right]+\left[u_{i,x,c}(k+1);\right.u_{i,y,c}(k+1);$$\left.u_{i,z,c}(k+1)\right]$. Supposing that the reference yaw angle is zero, the control signal of the ${\mathrm{UAV}}_i’s$ TS at time $k+1$ is:

$$u_{i,4}(k+1)=\sqrt{{(u_{i,z}(k+1)-mg)}^2+u_{i,x}^2(k+1)+u_{i,y}^2(k+1)}$$

(18)

Assuming that $-\pi /2<{\varphi }_{i,r},{\theta }_{i,r}<\pi /2$, the reference angles ${\varphi }_{i,r}$ and ${\theta }_{i,r}$ can be calculated as:

$$\begin{array}{l}{\varphi }_{i,r}(k+1)=-\arcsin \left(\frac{u_{i,y}(k+1)}{u_{i,4}(k+1)}\right)\\ {\theta }_{i,r}(k+1)=\arcsin \left(\frac{u_{i,x}(k+1)}{u_{i,4}(k+1)\cos ({\varphi }_{i,r}(k+1))}\right)\end{array}$$

(19)

Finally, the reference states for the RS at time k will be:

$$\left[{\varphi }_{i,r}(k+1);\frac{{\varphi }_{i,r}(k+1)-{\varphi }_{i,r}(k)}{T_T};{\theta }_{i,r};\frac{{\theta }_{i,r}(k+1)-{\theta }_{i,r}(k)}{T_T};0;0\right]$$

(20)

3.2. MPC of the Rotational Subsystem

Similar to the TS, the optimal control and Lagrange multiplier of the RS are obtained as follows:

$$\begin{array}{l}\Delta {\widehat{\mathit{U}}}_{i,R}\left(k+1\right)=-{({\mathit{N}}_{i,R}^T{\mathit{Q}}_{i,R}{\mathit{N}}_{i,R}+{\mathit{R}}_{i,R})}^{-1}\left({\mathit{N}}_{i,R}^T{\mathit{Q}}_{i,R}\right.\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em} \left.\left({\mathit{M}}_{i,R}{\overline{\mathit{X}}}_{i,R}\left(k\left|k\right.\right)-{\mathit{R}}_{s,R}\left(k\left|k\right.\right)\right)+\frac{{\mathsf{\Gamma }}_{i,R}^T{\mathbf{\lambda }}_{i,R}\left(k+1\right)}{2}\right)\\ {\mathbf{\lambda }}_{i,R}\left(k+1\right)=-2{\left({\mathsf{\Gamma }}_{i,R}{\left({\mathit{N}}_{i,R}^T{\mathit{Q}}_{i,R}{\mathit{N}}_{i,R}+{\mathit{R}}_{i,R}\right)}^{-1}{\mathsf{\Gamma }}_{i,R}^T\right)}^{-1}\left({\mathsf{\Theta }}_{i,R}\right.\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em} \left.+{\mathsf{\Gamma }}_{i,R}{\left({\mathit{N}}_{i,R}^T{\mathit{Q}}_{i,R}{\mathit{N}}_{i,R}+{\mathit{R}}_{i,R}\right)}^{-1}{\mathit{N}}_{i,R}^T{\mathit{Q}}_{i,R}\left({\mathit{M}}_{i,R}{\overline{\mathit{X}}}_{i,R}\left(k\left|k\right.\right)-{\mathit{R}}_{s,R}\left(k\left|k\right.\right)\right)\right)\end{array}$$

(21)

Therefore, after solving the above optimization problems, the first row of $\Delta {\widehat{\mathit{U}}}_{i,R}$ is selected as the incremental control signal of the RS and applied to ${\mathrm{UAV}}_i$.

4. Stability Analysis

Since the integrators were introduced in the augmentation system, the condition number in (17) and (21) deteriorates if $N_{P,T}$ is large [32]. As can be seen from (4) and (6), the eigenvalues are in the vicinity of the unit circle, which will make the numerical problem worse. Therefore, it is necessary to make sure that the controller can avoid the drawback while ensuring the stability of the closed-loop system. Similar to the introduction of exponential factors in the linear quadratic regulator (LQR), the same modification can be made in MPC, which will push the poles of the closed-loop system into the unit circle. The benefits can be obtained through the following analysis.

For the convenience of analysis, the expected outputs are ignored without affecting the results. Take the TS as an example (the analysis of the RS is the same), with a given ${\tau }_T>1$, the cost function $J_{i,T}\left(k\right)$ with exponential factors in MPC can be processed as follows:

$$\begin{array}{l}\underset{\Delta {\mathit{U}}_{i,T}}{\min }{J}_{i,T}\left(k\right)={\displaystyle \sum _{m=1}^{N_{p,T}}{\tau }_T^{-2m}{\widehat{\mathit{X}}}_{i,T}{\left(k+m|k\right)}^T}{\overline{\mathit{Q}}}_{i,T}{\widehat{\mathit{X}}}_{i,T}\left(k+m|k\right)\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}+{\displaystyle \sum _{j=0}^{N_{c,T}-1}{\tau }_T^{-2j}\Delta {\mathit{U}}_{i,T}{(k+j)}^T{\overline{\mathit{R}}}_{i,T}\Delta {\mathit{U}}_{i,T}(k+j)}\\ \mathrm{subject} \mathrm{to}:{\mathsf{\Gamma }}_{i,T}\Delta {\mathit{U}}_{i,T}(k)\le {\mathsf{\Theta }}_{i,T}\end{array}$$

(22)

where ${\tau }_T>1$ means that the cost function $J_{i,T}\left(k\right)$ places more emphasis on the states and control of the current time than the states of the future; the weights decrease as $m$ and $j$ increase. Thus, the sequence of new states and incremental control vectors in one prediction window are chosen as follows:

$$\begin{array}{l}{\widehat{\mathit{X}}}_{\tau }\left(k\right)={\left[\begin{array}{cccc}{\tau }_T^{-1}{\widehat{\mathit{X}}}_{i,T}{\left(k+1|k\right)}^T& {\tau }_T^{-2}{\widehat{\mathit{X}}}_{i,T}{\left(k+2|k\right)}^T& \dots & {\tau }_T^{-N_{p,T}}{\widehat{\mathit{X}}}_{i,T}{\left(k+N_{p,T}|k\right)}^T\end{array}\right]}^T\\ \Delta {\widehat{\mathit{U}}}_{\tau }(k)={\left[\begin{array}{cccc}{\tau }_T^0\Delta {\mathit{U}}_{i,T}{(k|k)}^T& {\tau }_T^{-1}\Delta {\mathit{U}}_{i,T}{(k+1|k)}^T& \dots & {\tau }_T^{-(N_{c,T}-1)}\Delta {\mathit{U}}_{i,T}{(k+N_{c,T}-1|k)}^T\end{array}\right]}^T\end{array}$$

(23)

where the elements of ${\widehat{\mathit{X}}}_{\tau }\left(k\right)$ and $\Delta {\widehat{\mathit{U}}}_{\tau }(k)$ are:

$$\begin{array}{l}{\widehat{\mathit{X}}}_{\tau }\left(k+m+1\left|k\right.\right)={\tau }_T^{-\left(m+1\right)}{\widehat{\mathit{X}}}_{i,T}\left(k+m+1\left|k\right.\right)\\ \Delta {\widehat{\mathit{U}}}_{\tau }\left(k+m\left|k\right.\right)={\tau }_T^{-m}\Delta {\mathit{U}}_{i,T}\left(k+m\left|k\right.\right)\end{array}$$

(24)

As described in (5), the elements of ${\widehat{\mathit{X}}}_{\tau }\left(k\right)$ and $\Delta {\widehat{\mathit{U}}}_{\tau }(k)$ satisfy the following difference equation:

$${\widehat{\mathit{X}}}_{\tau }\left(k+m+1|k\right)=\frac{{\widehat{\mathit{A}}}_{i,T}}{{\tau }_T}{\widehat{\mathit{X}}}_{\tau }\left(k+m|k\right)+\frac{{\widehat{\mathit{B}}}_{i,T}}{{\tau }_T}\Delta {\widehat{\mathit{U}}}_{\tau }(k+m)$$

(25)

The optimization problem with multiple constraints can be rephrased as:

$$\begin{array}{l}\underset{\Delta {\widehat{U}}_{\tau }}{\min }{J}_{\tau }\left(k\right)={\displaystyle \sum _{m=1}^{N_{p,T}}{\widehat{\mathit{X}}}_{\tau }{\left(k+m|k\right)}^T}{\overline{\mathit{Q}}}_{i,T}{\widehat{\mathit{X}}}_{\tau }\left(k+m|k\right)\\ \hspace{1em}\hspace{1em}\hspace{1em} +{\displaystyle \sum _{j=0}^{N_{c,T}-1}\Delta {\widehat{\mathit{U}}}_{\tau }{(k+j)}^T{\overline{\mathit{R}}}_{i,T}\Delta {\widehat{\mathit{U}}}_{\tau }(k+j)}\\ \mathrm{subject} \mathrm{to}: {\mathsf{\Gamma }}_{T,\tau }\Delta {\widehat{\mathit{U}}}_{\tau }(k)\le {\mathsf{\Theta }}_{T,\tau }\end{array}$$

(26)

where ${\mathsf{\Gamma }}_{T,\tau }$ is as follows:

$${\mathsf{\Gamma }}_{T,\tau }={\mathsf{\Gamma }}_T\left[\begin{array}{cccc}I& 0& \dots & 0\\ 0& {\tau }_{T}I& \dots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \dots & {\tau }_T^{N_{p,T}}I\end{array}\right]$$

(27)

As can be seen from (14) and (26), the two cost functions have the same form and are identical to each other. Moreover, the optimal solution of (26) is $\Delta {\widehat{\mathit{U}}}_{\tau }\left(k+0\left|k\right.\right)={\tau }_T^0\Delta {\mathit{U}}_{i,T}\left(k\left|k\right.\right)=\Delta {\mathit{U}}_{i,T}\left(k\left|k\right.\right)$, which is the same as the answer to (14). Therefore, if ${\tau }_T>1$ is chosen properly, the cost function (26) could be used in the process of MPC, while all the eigenvalues of ${\tau }_T^{-1}{\widehat{\mathit{A}}}_{i,T}$ are inside the unit circle. The corresponding state space (25) is stable, which has a positive effect on solving the numerical problems in the MPC calculation process even if $N_{P,T}$ is large. However, the optimization problem (25) is equivalent to the discrete-time linear quadratic regulator (DLQR) problem when $N_{P,T}$ and $N_{C,T}$ are large enough [34,35,36]. The DLQR problem of the corresponding state-space model $\left({\tau }_T^{-1}{\widehat{\mathit{A}}}_{i,T},{\tau }_T^{-1}{\widehat{\mathit{B}}}_{i,T}\right)$ is always solved by the algebraic Riccati equation, as follows with the same weight matrices ${\overline{\mathit{Q}}}_{i,T}$ and ${\overline{\mathit{R}}}_{i,T}$:

$$\frac{{\widehat{\mathit{A}}}_{i,T}^T}{{\tau }_T}{\mathit{S}}_{\infty }\frac{{\widehat{\mathit{A}}}_{i,T}}{{\tau }_T}-{\mathit{S}}_{\infty }-\frac{{\widehat{\mathit{A}}}_{i,T}^T}{{\tau }_T}{\mathit{S}}_{\infty }\frac{{\widehat{\mathit{B}}}_{i,T}}{{\tau }_T}{\left(\frac{{\widehat{\mathit{B}}}_{i,T}^T}{{\tau }_T}{\mathit{S}}_{\infty }\frac{{\widehat{\mathit{B}}}_{i,T}}{{\tau }_T}+{\overline{\mathit{R}}}_{i,T}\right)}^{-1}\frac{{\widehat{\mathit{B}}}_{i,T}^T}{{\tau }_T}{\mathit{S}}_{\infty }\frac{{\widehat{\mathit{A}}}_{i,T}}{{\tau }_T}+{\overline{\mathit{Q}}}_{i,T}=0$$

(28)

The state feedback control gain matrix ${\mathit{K}}_{T,\tau }$ and the corresponding closed-loop system can be obtained as follows:

$$\begin{array}{c}{\mathit{K}}_{T,\tau }={({\tau }_T^{-2}{\widehat{\mathit{B}}}_{i,T}^T{\mathit{S}}_{\infty }{\widehat{\mathit{B}}}_{i,T}+{\overline{\mathit{R}}}_{i,T})}^{-1}{\tau }_T^{-2}{\widehat{\mathit{B}}}_{i,T}^T{\mathit{S}}_{\infty }{\widehat{\mathit{A}}}_{i,T}\\ {\widehat{\mathit{X}}}_{\tau }(k+m+1|k)={\tau }_T^{-1}({\widehat{\mathit{A}}}_{i,T}-{\widehat{\mathit{B}}}_{i,T}{\mathit{K}}_{T,\tau }){\widehat{\mathit{X}}}_{\tau }(k+m|k)\end{array}$$

(29)

Since ${\tau }_T>1$ is chosen, the eigenvalue ${\lambda }_{\max }\left|{\tau }_T^{-1}({\widehat{\mathit{A}}}_{i,T}-{\widehat{\mathit{B}}}_{i,T}{\mathit{K}}_{T,\tau })\right|<1$ can be guaranteed, while the eigenvalue of the actual system will be ${\lambda }_{\max }\left|({\widehat{\mathit{A}}}_{i,T}-{\widehat{\mathit{B}}}_{i,T}{\mathit{K}}_{T,\tau })\right|<{\tau }_T$, which means there is no guarantee of the actual closed-loop system’s stability. However, it can be solved by choosing the values of ${\overline{\mathit{Q}}}_{i,T}$ and ${\overline{\mathit{R}}}_{i,T}$ properly.

Theorem 1. 

Let ${\sigma }_T={\tau }_T^{-1}{\beta }_T$, $\left(0<{\beta }_T<1\right)$, ${\mathit{Q}}_{\sigma }={\sigma }_T^2{\overline{\mathit{Q}}}_{i,T}+(1-{\sigma }_T^2){\mathit{S}}_{\infty }$, and${\mathit{R}}_{\sigma }={\sigma }_T^2{\overline{\mathit{R}}}_{i,T}$. The optimal solution of (30) can ensure the stability of the closed-loop system by choosing a suitable ${\beta }_T$.

$$\begin{array}{l}\underset{\Delta {\widehat{U}}_{\alpha }}{\min }{J}_{\sigma }\left(k\right)={\displaystyle \sum _{m=1}^{N_{p,T}}{\widehat{\mathit{X}}}_{\tau }{\left(k+m|k\right)}^T}{\mathit{Q}}_{\sigma }{\widehat{\mathit{X}}}_{\tau }\left(k+m|k\right)+{\displaystyle \sum _{j=0}^{N_{c,T}-1}\Delta {\widehat{\mathit{U}}}_{\tau }{(k+j)}^T{\mathit{R}}_{\sigma }\Delta {\widehat{\mathit{U}}}_{\tau }(k+j)}\\ \mathrm{subject} \mathrm{to}:{\mathsf{\Gamma }}_{T,\tau }\Delta {\widehat{\mathit{U}}}_{\tau }(k)\le {\mathsf{\Theta }}_{T,\tau }\end{array}$$

(30)

Proof of Theorem 1.

Similar to (26) and (28), the algebraic Riccati equation of (30) is:

$$\frac{{\widehat{\mathit{A}}}_{i,T}^T}{{\tau }_T}{\mathit{S}}_{\infty }\frac{{\widehat{\mathit{A}}}_{i,T}}{{\tau }_T}-{\mathit{S}}_{\infty }-\frac{{\widehat{\mathit{A}}}_{i,T}^T}{{\tau }_T}{\mathit{S}}_{\infty }\frac{{\widehat{\mathit{B}}}_{i,T}}{{\tau }_T}{\left(\frac{{\widehat{\mathit{B}}}_{i,T}^T}{{\tau }_T}{\mathit{S}}_{\infty }\frac{{\widehat{\mathit{B}}}_{i,T}}{{\tau }_T}+{\mathit{R}}_{\sigma }\right)}^{-1}\frac{{\widehat{\mathit{B}}}_{i,T}^T}{{\tau }_T}{\mathit{S}}_{\infty }\frac{{\widehat{\mathit{A}}}_{i,T}}{{\tau }_T}+{\mathit{Q}}_{\sigma }=0$$

(31)

Substituting ${\mathit{Q}}_{\sigma }$ and ${\mathit{R}}_{\sigma }$ into (31) gives:

$$\begin{array}{l}\frac{{\widehat{\mathit{A}}}_{i,T}^T}{{\tau }_T}{\mathit{S}}_{\infty }\frac{{\widehat{\mathit{A}}}_{i,T}}{{\tau }_T}-{\mathit{S}}_{\infty }-\frac{{\widehat{\mathit{A}}}_{i,T}^T}{{\tau }_T}{\mathit{S}}_{\infty }\frac{{\widehat{\mathit{B}}}_{i,T}}{{\tau }_T}{\left(\frac{{\widehat{\mathit{B}}}_{i,T}^T}{{\tau }_T}{\mathit{S}}_{\infty }\frac{{\widehat{\mathit{B}}}_{i,T}}{{\tau }_T}+\frac{{\beta }_T^2{\overline{\mathit{R}}}_{i,T}}{{\tau }_T^2}\right)}^{-1}\frac{{\widehat{\mathit{B}}}_{i,T}^T}{{\tau }_T}{\mathit{S}}_{\infty }\frac{{\widehat{\mathit{A}}}_{i,T}}{{\tau }_T}\\ +\frac{{\beta }_T^2{\overline{\mathit{Q}}}_{i,T}}{{\tau }_T^2}+{\mathit{S}}_{\infty }-\frac{{\beta }_T^2{\mathit{S}}_{\infty }}{{\tau }_T^2}=0\end{array}$$

(32)

Multiplying both sides of (32) by ${\tau }_T^2{\beta }_T^2$ results in the following equivalent equation:

$$\frac{{\widehat{\mathit{A}}}_{i,T}^T}{{\beta }_T}{\mathit{S}}_{\infty }\frac{{\widehat{\mathit{A}}}_{i,T}}{{\beta }_T}-\frac{{\widehat{\mathit{A}}}_{i,T}^T}{{\beta }_T}{\mathit{S}}_{\infty }\frac{{\widehat{\mathit{B}}}_{i,T}}{{\beta }_T}{\left(\frac{{\widehat{\mathit{B}}}_{i,T}^T}{{\beta }_T}{\mathit{S}}_{\infty }\frac{{\widehat{\mathit{B}}}_{i,T}}{{\beta }_T}+{\overline{\mathit{R}}}_{i,T}\right)}^{-1}\frac{{\widehat{\mathit{B}}}_{i,T}^T}{{\beta }_T}{\mathit{S}}_{\infty }\frac{{\widehat{\mathit{A}}}_{i,T}}{{\beta }_T}+{\overline{\mathit{Q}}}_{i,T}-{\mathit{S}}_{\infty }=0$$

(33)

The corresponding states feedback gains matrix ${\mathit{K}}_{T,\sigma }$ and closed-loop system are obtained as follows:

$$\begin{array}{c}{\mathit{K}}_{T,\sigma }={\left({\overline{\mathit{R}}}_{i,T}+{\beta }_T^{-2}{\widehat{\mathit{B}}}_{i,T}^T{\mathit{S}}_{\infty }{\widehat{\mathit{B}}}_{i,T}\right)}^{-1}{\beta }_T^{-2}{\widehat{\mathit{B}}}_{i,T}^T{\mathit{S}}_{\infty }{\widehat{\mathit{A}}}_{i,T}\\ {\widehat{\mathit{X}}}_{\tau }(k+m+1|k)={\beta }_T^{-1}({\widehat{\mathit{A}}}_{i,T}-{\widehat{\mathit{B}}}_{i,T}{\mathit{K}}_{T,\sigma }){\widehat{\mathit{X}}}_{\tau }(k+m|k)\end{array}$$

(34)

where the definition of ${\widehat{\mathit{X}}}_{\tau }(k+m+1|k)$ is the same as (29). From the closed-loop system equation in (34), it can be seen that an appropriate choice of ${\beta }_T<1$ can ensure the eigenvalue ${\lambda }_{\max }\left|({\widehat{\mathit{A}}}_{i,T}-{\widehat{\mathit{B}}}_{i,T}{\mathit{K}}_{T,\sigma })\right|<{\beta }_T$, which means that the closed-loop system is stable. □

In summary, the IMPC proposed here can improve the stability margin of the system by forcing the poles of the closed-loop system into the unit circle. Therefore, IMPC is able to cope with the influence of disturbances and the uncertainties of the system itself. Compared with RMPC, IMPC has better dynamic performance, which enables the system to return to steady state faster. The focus is on the continuous transformation of the original cost function (14) by (22) and (30), which is equivalent to obtaining the final closed-loop function as shown in (34). As mentioned earlier, the purpose of this is not only to ensure the stability of the model used in the predictive control process, but also the closed-loop system. At the same time, $N_{P,T}$ and $N_{C,T}$ can be large without numerical problems.

5. Simulation

The tasks of trajectory tracking and instructions procurement can be completed by ${\mathrm{UAV}}_1$. The main parameters of the UAV can be obtained from [19].

The following two cases are presented in the simulation: (1) A comparison with the dual closed-loop sliding mode formation controller under ideal dynamics [19]. (2) A comparison of IMPC and RMPC in achieving the same formation trajectory tracking in the presence of disturbance and model uncertainty.

5.1. Case 1

The distance of the three axes between UAVs remained the same in this simulation. Figure 4 shows the geometry of three UAVs’ formation flights in Case 1, where $r_1=2 \mathrm{m}$.

The expected trajectory in case 1 is as follows:

$$R_{x1}=\left\{\begin{array}{c}3, t\le 10 \mathrm{s} \\ 3+\left(t-10\right),t>10 \mathrm{s}\end{array}\right., R_{y1}=\left\{\begin{array}{c}1.732+t, t\le 10 \mathrm{s}\\ 11.732, t>10 \mathrm{s}\end{array}\right., R_{z1}=\left\{\begin{array}{c}0.1+t, t\le 10 \mathrm{s}\\ 10.1, t>10 \mathrm{s}\end{array}\right.$$

(35)

where $R_x$, $R_y$, and $R_z$ are the expected trajectories of axis X, Y, Z, respectively.

The initial positions of the three UAVs were: ${\mathrm{UAV}}_1$ (3, 1.732, 0.1) m, ${\mathrm{UAV}}_2$ (0, 3.464, 0.1) m, and ${\mathrm{UAV}}_3$ (0, 0, 0.1) m. The initial speeds were all 0 m/s. For the TS, the sampling period was $T_S=0.05 \mathrm{s}$, while $T_R=0.05 \mathrm{s}$ for the RS. The main parameters of IMPC in the rolling optimization process were: ${\widehat{\mathit{Q}}}_{i,T}={\widehat{\mathit{Q}}}_{i,R}=20$, ${\overline{\mathit{R}}}_{i,T}={\overline{\mathit{R}}}_{i,R}=0.01I_6$, where $I_6$ was the unit matrix; $N_{c,T}=N_{c,R}=1$; $N_{p,T}=10$; $N_{p,R}=4$, ${\tau }_T=1.6$; ${\beta }_T=0.92$; ${\tau }_R=1.2$; and ${\beta }_R=0.92$. The parameters of the consensus low were $k_x=k_y=k_z=0.01$. Due to the constraints of UAVs’ rotor, the states and inputs were limited as follows:

$$-\frac{\pi }{2}(\mathrm{rad})<\varphi ,\theta <\frac{\pi }{2}(\mathrm{rad}), 0<\psi <\pi (\mathrm{rad}), -0.76(\mathrm{rad}\cdot {\mathrm{s}}^{-1})<\dot{\varphi },\dot{\theta }<0.76(\mathrm{rad}\cdot {\mathrm{s}}^{-1})$$

$$-1(\mathrm{rad}\cdot {\mathrm{s}}^{-1})<\dot{\psi }<1(\mathrm{rad}\cdot {\mathrm{s}}^{-1}), -1.5(\mathrm{m}\cdot {\mathrm{s}}^{-1})\le \dot{x}, \dot{y}, \dot{z}\le 1.5(\mathrm{m}\cdot {\mathrm{s}}^{-1})$$

$$0\le U_1\le 11.23(\mathrm{N}), |U_2|\le 5.61(\mathrm{N}\cdot \mathrm{m}), |U_3|\le 5.61(\mathrm{N}\cdot \mathrm{m}), |U_4|\le 0.16(\mathrm{N}\cdot \mathrm{m})$$

The simulation results are shown in Figure 5, Figure 6, Figure 7, Figure 8 and Figure 9. The total time of simulation was 40 s. Figure 5 shows the trajectory tracking of the UAVs in 3D space with SMC (a) and IMPC (b). Figure 6 and Figure 7 illustrate the states of the TS and RS, respectively, demonstrating that the UAVs with IMPC (b) provide significantly better performance than SMC (a). Figure 8 and Figure 9 exhibit the inputs and distance of the three axes of the formation, respectively. They show that in order to maintain the expected formation, the inputs of SMC (a) must been far greater, while IMPC (b) can remain within the specified constraints. The reason for the oscillation process of SMC in Figure 7, Figure 8 and Figure 9 is the fact that the large uncertainty in the model requires a large gain of the switching term, which can be solved by replacing the symbolic function with a saturation function [38].

5.2. Case 2

For Case 2, the formation structure and main parameters of IMPC were the same as in Case 1. The expected trajectory in this part was:

$$\begin{gathered} R_{x2}=\left\{\begin{array}{c}\begin{array}{l}3, t\le 10 \mathrm{s}\\ 3+(t-10), 10 \mathrm{s}<t\le 20 \mathrm{s} \end{array}\\ \begin{array}{l}13, 20 \mathrm{s}<t\le 25 \mathrm{s}\\ 13+(t-25), 25 \mathrm{s}<t\le 35 \mathrm{s}\\ 23, 35 \mathrm{s}<t\le 40 \mathrm{s} \\ 23+(t-40), 40 \mathrm{s}<t\le 50 \mathrm{s}\\ 33, 50 \mathrm{s}<t\le 55 \mathrm{s}\\ 33+(t-55), 25 \mathrm{s}<t\le 35 \mathrm{s}\end{array}\end{array}\right., R_{y2}=\left\{\begin{array}{l}1.732+t, t\le 10 \mathrm{s}\\ 11.732, 10 \mathrm{s}<t\le 20 \mathrm{s}\\ 11.732-(t-20), 20 \mathrm{s}<t\le 25 \mathrm{s}\\ 6.732, 25 \mathrm{s}<t\le 35 \mathrm{s}\\ 6.732+(t-35), 35 \mathrm{s}<t\le 40 \mathrm{s}\\ 11.732, 40 \mathrm{s}<t\le 50 \mathrm{s}\\ 11.732-(t-50), 50 \mathrm{s}<t\le 55 \mathrm{s}\\ 6.732, t>55 \mathrm{s}\end{array}\right., \\ R_{z2}=0.2t \end{gathered}$$

(36)

The initial positions of the three UAVs were: ${\mathrm{UAV}}_1$ (3, 1.732, 0.1) m, ${\mathrm{UAV}}_2$ (0.1, 3.464, 0.1) m, and ${\mathrm{UAV}}_3$ (0.1, 0.1, 0.1) m. In the real formation flight, the quantified uncertainties and disturbances were unknown and had to be estimated online. To verify the robustness of IMPC to the uncertainties and disturbances, they were simply set to fixed values. The synthesis of uncertainties and disturbances of the ${\mathrm{UAV}}_i$ in the formation given in this paper are $d_{i,1}=0.4$, $d_{i,2}=-0.1$, $d_{i,3}=0.1$, $d_{i,4}=0.3$, $d_{i,5}=0.3$ and $d_{i,6}=0.1$, while the additional impulse for the TS given in this paper is $I{M}_{i,1}$, where:

$$I{M}_{i,1}=\left\{\begin{array}{l}[0;3;0;3;0;1], 13 \mathrm{s}<t\le 14 \mathrm{s}, 33 \mathrm{s}<t\le 34 \mathrm{s}\\ [0;0.7;0;0.7;0;0.1], t>45 \mathrm{s}\end{array}\right.$$

(37)

The condition number of regular MPC (RMPC) and IMPC is shown in

Table 1. Condition number of the RMPC and IMPC.

	Condition Number of TS	Condition Number of RS	Average Calculation Period of TS	Average Calculation Period of RS
RMPC	$3.4629\times {10}^3$	407.92	0.0104 s	0.0121 s
IMPC	51.56	243.91	0.0052 s	0.0086 s

, where the TS of IMPC was about two orders of magnitude less than RMPC, and the RS of RMPC was about twice that of IMPC. This means that the computational complexity of the IMPC designed in this paper can be greatly reduced compared to the RMPC. The average computation period also demonstrates the superiority of IMPC.

The poles of the closed-loop system for the TS and RS lie in the circle of ${\beta }_T={\beta }_R=0.92$ with IMPC, as shown in Figure 10, which makes the closed-loop system more stable than RMPC.

The simulation results are shown in Figure 11, Figure 12, Figure 13, Figure 14, Figure 15 and Figure 16. Figure 11, Figure 12, Figure 13 and Figure 14 show the formation trajectory tracking in the presence of disturbances and uncertainties. As can be seen from the figure, both controllers were able to suppress the effects of disturbances and system uncertainties. However, the formation with IMPC (b) had smaller errors and a shorter settling time than that of RMPC (a). Figure 15 describes the distance between the UAVs on three axes, where IMPC (b) had better formation stability than RMPC (a). As can be seen in Figure 16, the angle of the UAVs in formation changed when they turned or were affected by disturbances; with IMPC, the changes in the angles were much smoother and took less time to return to the stable states. This is further supported by the control inputs in Figure 17.

6. Conclusions and Future Work

The formation control based on consensus and IMPC was proposed in this paper to ensure the stability and trajectory tracking of the formation. A literature investigation showed that most studies are based on simplified or ideal models, and constraints on UAVs are not paid enough attention.

Firstly, the UAV dynamic model with disturbance and uncertainty is introduced, and it is divided into an RS and a TS. The corresponding multi-constrained IMPC is designed after obtaining the error-augmented models of the two subsystems. The influence of external disturbance and uncertainty can be eliminated because of the integral in the augmented system. On this basis, by appropriately modifying the cost function, not only can the numerical problems in the rolling optimization process be reduced, but also the stability of the two closed-loop subsystems is ensured.

Because of the addition of integrators to the augmented model, it is possible to eliminate the effect of disturbances and model uncertainties on the performance of the controller without adding disturbance observers.

The simulation results demonstrate that the proposed procedure guarantees a better formation effect than SMC under the condition of hard constraints. The IMPC outperformed the RMPC in terms of condition number, which means that IMPC has higher computational efficiency. It can be seen from the final simulation that IMPC can eliminate the influence of disturbance and uncertainty. The robustness to impulse can provide a meaningful reference for pesticide spraying and other formation problems under load.

Future work should consider the challenges of parameter changes, obstacle avoidance, and reconstruction problems.