Design and Real-Time Implementation of a Cascaded Model Predictive Control Architecture for Unmanned Aerial Vehicles

Abstract

Modeling and control are challenging in unmanned aerial vehicles, especially in quadrotors where there exists high coupling between the position and the orientation dynamics. In simulations, conventional control strategies such as the use of a proportional–integral–derivative (PID) controller under different configurations are typically employed due to their simplicity and ease of design. However, linear assumptions have to be made, which turns into poor performance for practical applications on unmanned aerial vehicles (UAVs). This paper designs and implements a hierarchical cascaded model predictive control (MPC) for three-dimensional trajectory tracking using a quadrotor platform. The overall system consists of two stages: the mission server and the commander stabilizer. Different from existing works, the heavy computational burden is managed by decomposing the overall MPC strategy into two different schemes. The first scheme controls the translational displacements while the second scheme regulates the rotational movements of the quadrotor. For validation, the performance of the proposed controller is compared against that of a proportional–integral–velocity (PIV) controller taken from the literature. Here, real-world experiments for tracking helicoidal and lemniscate trajectories are implemented, while for regulation, an extreme wind disturbance is applied. The experimental results show that the proposed controller outperforms the PIV controller, presenting less signal effort fluctuations, especially in terms of rejecting external wind disturbances.

1. Introduction

In recent years, the use of UAV technology has notably increased. Quadrotors are preferred over other UAVs because of their versatility and their stable dynamics [1]. For this reason, they are used in aerial surveillance, shipping, emergency situations, entertainment shows, and agriculture monitoring, among other areas. These applications require the quadrotors to be fast, agile, and lightweight in order to be capable of following precise trajectories in the presence of external disturbances.

To meet these requirements, some control strategies have been developed [2,3,4,5,6,7]. First, the well-known PID controller, which can handle both linear and nonlinear systems, can achieve zero steady-state errors with a simple-to-understand tuning principle [2]. A linear–quadratic regulator (LQR) control can be easily implemented and can handle MIMO systems [3]. Feedback linearization (FL) provides the capacity to linearize a model with a model inversion procedure instead of making linearization assumptions [4]. Even though these control strategies can achieve adequate reference response, they are not robust and some of them even lack the ability to achieve zero steady-state errors [3,4].

MPC is one control strategy that is useful in handling multivariable processes. This method consists of an optimal control technique that calculates the necessary inputs of a model to minimize its cost function, which represents its system dynamics using its constraints over a limited time horizon [8]. Therefore, MPC is a robust control technique capable of controlling complex nonlinear systems while meeting the system constraints and ensuring safe operation [9]. However, the application of large-scale MPC in real-time has its own challenges, mainly due to its reliability, large computational overhead, and the complexity of its software implementation [10]. A literature survey presented recent real-time applications in which the MPC strategy was used to address these concerns [10,11,12,13,14,15,16,17,18]. For these reasons, the control of quadrotors using MPC has become feasible in more sophisticated applications as research of their control algorithms advances.

A variety of linear and nonlinear MPCs have been developed to improve control performance in quadrotors [19,20,21,22,23,24,25,26]. In [19], a compound control technique that consists of two different control strategies was described. They used both an integral sliding mode control to reject matched disturbances and an MPC to treat the remaining disturbances for successful trajectory tracking of a quadrotor. However, one of the main problems in MPC is the large computational capacity required. Therefore, in [20], the input sequence of an MPC was approximated with the Laguerre function, decreasing the computational burden and improving the inner loop performance of the quadrotor. Moreover, in [21], a simplified optimal control problem was proposed to reduce the computational burden of traditional MPCs so their proposed ‘bang-bang’ MPC could be applied to smaller and lighter quadrotors with less computational capacity. In [22], the authors employed a nonlinear MPC (NMPC) for the inner control loop of a quadrotor while maintaining the computation cost that is similar to a traditional NMPC.

On the other hand, system failures are always to be expected. For that reason, in [23], the authors developed an NMPC capable of stabilizing a drone during a rotor failure. Quadrotors should be able to adapt themselves to whatever model uncertainties they are subjected too. Therefore, in [24], the authors used a hybrid adaptive NMPC to learn the model uncertainties in real time. This control algorithm improved the quadrotor’s control robustness and reduced tracking error by 90% compared to a non-adaptive NMPC while keeping the computational overhead burden at a minimum. In [25], an NMPC scheme was proposed for obstacle avoidance in quadrotors; the authors applied a potential field function-based penalty term and a dynamical adjustment for the prediction horizon to cut down the heavy computational load. Similarly, in [26], an NMPC for obstacle avoidance was employed; in this case, the NMPC control was formulated in the special Euclidean group SE(3) for the quadrotor to operate in messy environments with a large number of obstacles. Simulations were performed using algebraic ellipsoids as obstacles.

Although tremendous effort has been dedicated to implementing MPC in unmanned systems [27], there still exist significant challenges in high-speed real-time applications. An MPC is designed based on the entire nonlinear dynamics model of a quadrotor UAV, which introduces several computational challenges [18,28], and its real-time applicability is reduced. To avoid this, linear control strategies have been implemented while considering the advantages of being relatively easy to understand and design [29]. The linearization procedure is carried out by first trimming the quadrotor at zero wind and then at hover [30]. Thus, it is sufficient to use a single linear time invariant (LTI) model for MPC [27,31]. In this paper, to address the drawbacks of the advanced control strategies in quadrotors mentioned in [14,32], the real-time implementation is addressed. Here, the computationally efficient control architecture for a multivariable system is obtained, which reduces the complexity of the system dynamics and improves its real-time performance [13,33]. The overall MPC strategy is decomposed into two different schemes [16,34]. The first scheme controls the translational displacements while the second scheme regulates the rotational movements of the quadrotor. This turns into a less computationally expensive scheme, which is suitable for real-time implementation. To validate the tracking performance of the proposed MPC architecture, it is compared to a PIV controller with three real-time scenarios: tracking helicoidal, tracking lemniscate, and aggressive regulator under extreme external disturbances. All tests were carried out with a QDrone 2 quadrotor from Quanser^® [28,35].

This paper is organized as follows. Section 2 states the dynamic model of the QDrone 2 quadrotor UAV. Section 3 gives an overview of the MPC control technique. Section 4 details the overall system used for implementation, including the compared PIV control structure and the proposed MPC control scheme and its algorithm. Section 5 contains all the tuning and hardware parameters and presents the tracking and regulator scenarios. The implementation results illustrate the performance improvement of the proposed cascaded MPC control structure compared to a PIV controller. Finally, in Section 6, the paper summarizes the results and future work.

2. Kinematics

First, to model the quadrotor dynamics with the Newton–Euler formalism taken from the literature [36], it is necessary to consider two frames, the inertial frame ($o_w{x}_w{y}_w{z}_w$) and the quadrotor body frame ($o_b{x}_b{y}_b{z}_b$). These two reference frames are needed to define both the position and orientation of the quadrotor UAV (Figure 1). Considering that each propeller contributes individually to the movement of the whole system, the force generated by each propeller and the thrust produced by the quadrotor is defined by

$$f_i=k{\omega }_i^2$$

(1)

$$T=\left[\begin{array}{c}0\\ 0\\ {\displaystyle \sum _{i=1}^4{f}_i}\end{array}\right]$$

(2)

where k is the lift constant and $\omega$ is the angular velocity of the rotor. Therefore, as the four rotors are fixed pointing upwards, the total thrust generated will be on the $z_b$ axis. The total thrust is equivalent to the sum of the forces generated by the rotors.

In the inertial frame, the absolute position is defined by a linear position vector $\xi$ with the coordinates $x_w,y_w,z_w$, and an angular position vector $\eta$ with the roll, pitch, and yaw angles, expressed as $\varphi ,\theta ,\psi$, respectively.

$$\xi ={\left[\begin{array}{ccc}{x}_w& y_w& z_w\end{array}\right]}^T$$

(3)

$$\eta ={\left[\begin{array}{ccc}\varphi & \theta & \psi \end{array}\right]}^T$$

(4)

On the other hand, the body frame is defined by a linear position vector $\sigma$ with $x_b,y_b,z_b$ coordinates, and an angular velocity vector $\gamma$ with $p,q,r$.

$$\sigma ={\left[\begin{array}{ccc}{x}_b& y_b& z_b\end{array}\right]}^T$$

(5)

$$\gamma ={\left[\begin{array}{ccc}p& q& r\end{array}\right]}^T$$

(6)

Both the inertial fixed frame and the body fixed frame linear-movement vectors are related by the transformation matrix $R_L$ taken from the $ZYX$ Tait–Bryan angles [37], where C and S denote cosine and sine functions, respectively.

$$R_L=\left[\begin{array}{ccc}C\psi ·C\theta & C\psi ·S\theta ·S\varphi -S\psi ·C\varphi & C\psi ·S\theta ·C\varphi +S\psi ·S\varphi \\ S\psi ·C\theta & S\psi ·S\theta ·S\varphi +C\psi ·C\varphi & S\psi ·S\theta ·C\varphi -C\psi ·S\varphi \\ -S\theta & C\theta ·S\varphi & C\theta ·C\varphi \end{array}\right]$$

(7)

$$\xi =R_L·\sigma$$

(8)

Similar to $R_L$, there is also a rotational movement transformation matrix $R_R$ that relates the angular velocity vectors from the inertial and body fixed frames [37].

$$R_R=\left[\begin{array}{ccc}1& 0& -S\theta \\ 0& C\varphi & C\theta ·S\varphi \\ 0& -S\varphi & C\theta ·C\varphi \end{array}\right]$$

(9)

$$\gamma =R_R·\dot{\eta }$$

(10)

The translational dynamical model of the quadrotor in the inertial fixed frame based on Newton’s second law of translational motion is

$$m·\ddot{\xi }=\left[\begin{array}{c}0\\ 0\\ -mg\end{array}\right]-k_d^L\dot{\xi }+R_{L}T$$

(11)

where T is the thrust vector, m is the mass of the quadrotor, $k_d^L$ is the thrust drag coefficient, and g is the Earth’s gravity constant. On the other hand, the rotational dynamical model in the body fixed frame using Euler’s equation of rotational motion is the following:

$$I\dot{\gamma }=\tau -\gamma \times \left(I\gamma \right)-\mu -k_d^R\gamma$$

(12)

$$\tau =\left[\begin{array}{c}{l}_r(f_2-f_4)\\ l_p(f_1-f_3)\\ d({\omega }_1^2-{\omega }_2^2+{\omega }_3^2-{\omega }_4^2)\end{array}\right]$$

(13)

$${\omega }_{\mathrm{\Gamma }}={\omega }_1-{\omega }_2+{\omega }_3-{\omega }_4$$

(14)

$$\mu =I_R\gamma \times \left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]{\omega }_{\mathrm{\Gamma }}$$

(15)

$$I=⌈\begin{array}{ccc}{I}_{xx}& 0& 0\\ 0& I_{yy}& 0\\ 0& 0& I_{zz}\end{array}⌉$$

(16)

where $\mu$ is the gyroscopic moment vector [38]; $k_d^R$ is the moment drag coefficient; $I_R$ is the gyroscopic inertia vector induced by the propellers; d is the drag coefficient; I is the diagonal matrix of inertia; ${\omega }_{\mathrm{\Gamma }}$ is the residual angular speed of the rotors; and $l_r$ and $l_p$ are the lengths between the quadrotor center of mass and the rotor that corresponds to the roll and pitch angle, respectively. Then, the complete nonlinear dynamical model of the quadrotor is based on the following equations:

$${\ddot{x}}_w=\frac{U_1}{m}\left(C\psi ·S\theta ·C\varphi +S\psi ·S\varphi \right)-\frac{k_d^L{\dot{x}}_w}{m}$$

(17)

$${\ddot{y}}_w=\frac{U_1}{m}\left(S\psi ·S\theta ·C\varphi -C\psi ·S\varphi \right)-\frac{k_d^L{\dot{y}}_w}{m}$$

(18)

$${\ddot{z}}_w=\frac{U_1}{m}\left(C\theta ·C\varphi \right)-g-\frac{k_d^L{\dot{z}}_w}{m}$$

(19)

$$\dot{p}=\frac{U_2}{I_{xx}}+\frac{I_{yy}-I_{zz}}{I_{xx}}qr-\frac{I_R{\omega }_{\mathrm{\Gamma }}}{I_{xx}}q-\frac{k_d^R}{I_{xx}}p$$

(20)

$$\dot{q}=\frac{U_3}{I_{yy}}+\frac{I_{zz}-I_{xx}}{I_{yy}}pr+\frac{I_R{\omega }_{\mathrm{\Gamma }}}{I_{yy}}p-\frac{k_d^R}{I_{yy}}q$$

(21)

$$\dot{r}=\frac{U_4}{I_{zz}}+\frac{I_{xx}-I_{yy}}{I_{zz}}pq-\frac{k_d^R}{I_{zz}}r$$

(22)

Here, the quadrotor system consists of four input variables described in vector U

$$U=\left[\begin{array}{c}{U}_1\\ U_2\\ U_3\\ U_4\end{array}\right]=⌈\begin{array}{cccc}k& k& k& k\\ 0& l_{r}k& 0& -l_{r}k\\ l_{p}k& 0& -l_{p}k& 0\\ d& -d& d& -d\end{array}⌉\left[\begin{array}{c}{\omega }_1^2\\ {\omega }_2^2\\ {\omega }_3^2\\ {\omega }_4^2\end{array}\right]$$

(23)

where $U_1$ denotes the total thrust, $U_2$ and $U_3$ stand for the torques generated about $\varphi$ and $\theta$ axes, respectively, and $U_4$ is the moment produced about the $\psi$ axis. Thus, the choice of state vector is given as follows

$$X={\left[\begin{array}{cccccccccccc}{x}_w& y_w& z_w& {\dot{x}}_w& {\dot{y}}_w& {\dot{z}}_w& \varphi & \theta & \psi & p& q& r\end{array}\right]}^T$$

(24)

Moreover, considering linear control techniques are widely used for UAV flight control [28,30,31,34,39], as they can ensure optimal performance when operating close to the equilibrium point [40,41], a linearization of the model under the following assumptions is carried out:

• Standard quadrotor structure.
• The structure is perfectly rigid.
• The center of mass of the quadrotor coincides with the origin of the body frame.
• Aerodynamics and other complex phenomena that are difficult to model are ignored.
• The quadrotor is hovering.

Then, considering that the quadrotor is assumed to be in hover position [30], the following is true:

$$\varphi =\theta =\psi \approx 0,\dot{\varphi }=\dot{\theta }=\dot{\psi }\approx 0$$

(25)

$$I_R\approx 0,k_d^L\approx 0,k_d^R\approx 0$$

(26)

$$(\dot{\varphi },\dot{\theta },\dot{\psi })\approx (p,q,r)$$

(27)

$$U=\left[\begin{array}{c}{U}_1\\ U_2\\ U_3\\ U_4\end{array}\right]=\left[\begin{array}{c}mg\\ 0\\ 0\\ 0\end{array}\right]$$

(28)

Given that ${\ddot{x}}_w$ depends directly on $\theta$ and ${\ddot{y}}_w$ depends directly on $\varphi$, the input $U_1$, Equation (28), is substituted in (17) and (18) [42]. Then, by performing small-angle approximations and eliminating the products of two terms near zero, the linearized dynamical model used for the proposed MPC is obtained:

$${\ddot{x}}_w=g\theta$$

(29)

$${\ddot{y}}_w=-g\varphi$$

(30)

$${\ddot{z}}_w=\frac{U_1}{m}-g$$

(31)

$$\ddot{\varphi }=\frac{U_2}{I_{xx}}$$

(32)

$$\ddot{\theta }=\frac{U_3}{I_{yy}}$$

(33)

$$\ddot{\psi }=\frac{U_4}{I_{zz}}$$

(34)

The linearized dynamics in (29)–(34) are decoupled for state space representation. Each of them follows the state space structure Ax + Bu.The altitude controller (31) is rewritten as

$$\left[\begin{array}{c}{\dot{z}}_w\\ {\ddot{z}}_w\end{array}\right]=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]\left[\begin{array}{c}{z}_w\\ {\dot{z}}_w\end{array}\right]+\left[\begin{array}{c}0\\ \frac{1}{m}\end{array}\right]T+\mathbf{g}$$

(35)

where m corresponds to the mass of the drone and

$$\mathbf{g}=\left[\begin{array}{c}0\\ -g\end{array}\right]$$

(36)

For the $x_w{y}_w$ position controller, (29) and (30) can be represented as

$$\left[\begin{array}{c}{\dot{x}}_w\\ {\ddot{x}}_w\\ {\dot{y}}_w\\ {\ddot{y}}_w\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}{x}_w\\ {\dot{x}}_w\\ y_w\\ {\dot{y}}_w\end{array}\right]+\left[\begin{array}{cc}0& 0\\ g& 0\\ 0& 0\\ 0& -g\end{array}\right]\left[\begin{array}{c}{\theta }^d\\ {\varphi }^d\end{array}\right]$$

(37)

And the state space representation of the rotational dynamics, (32)–(34) can be rewritten as

$$\left[\begin{array}{c}\dot{\varphi }\\ \ddot{\varphi }\\ \dot{\theta }\\ \ddot{\theta }\\ \dot{\psi }\\ \ddot{\psi }\end{array}\right]=\left[\begin{array}{cccccc}0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}\varphi \\ \dot{\varphi }\\ \theta \\ \dot{\theta }\\ \psi \\ \dot{\psi }\end{array}\right]+\left[\begin{array}{ccc}0& 0& 0\\ \frac{1}{I_{xx}}& 0& 0\\ 0& 0& 0\\ 0& \frac{1}{I_{yy}}& 0\\ 0& 0& 0\\ 0& 0& \frac{1}{I_{zz}}\end{array}\right]\tau$$

(38)

The controllability of the linearized system is tested before the design of the controller. Here, the controllability matrix has full rank, which implies that the linear system is controllable. On the other hand, even though the system is unstable, linear controllers ensure system stability and optimal performance when operating close to the equilibrium point [40,41].

3. Control Strategy

In this section, MPCs are designed for translational and rotational dynamics. The translational subsystem considers two MPCs, one for altitude and the other for $x_w{y}_w$ position; meanwhile, the rotational subsystem has an MPC for attitude. Hence, the control actions are obtained in each subsystem separately.

The MPC strategies for $x_w{y}_w$ position and attitude are based on [43]. However, considering that $z_w$ axis depends on gravity constant g, the following LTI state space model is used to represent the altitude system dynamics:

$$\mathbf{x}\left(k+1\right)=\mathbf{A}\mathbf{x}\left(k\right)+\mathbf{B}\mathbf{u}\left(k\right)+\mathbf{g}$$

(39)

$$\mathbf{y}\left(k\right)=\mathbf{C}\mathbf{x}\left(k\right)$$

(40)

where the state vector is denoted as $\mathbf{x}\in {\mathcal{R}}^n$, the input vector is $\mathbf{u}\in {\mathcal{R}}^{n_u}$, the output vector is $\mathbf{y}\in {\mathcal{R}}^{n_y}$, the state matrix is $\mathbf{A}\in {\mathcal{R}}^{n\times n}$, the input matrix is $\mathbf{B}\in {\mathcal{R}}^{n\times n_u}$, the output matrix is $\mathbf{C}\in {\mathcal{R}}^{n_y\times n}$, and the sampling time is expressed as $k\in N$. Furthermore, the projection matrix ${\mathbf{\Pi }}_i^{(n,N)}$ is defined as follows:

$${\mathbf{\Pi }}_i^{(n,N)}\equiv \left[\begin{array}{ccc}\underset{\left(i-1\right)terms}{\underbrace{{\mathbf{0}}_{n\times n}\dots {\mathbf{0}}_{n\times n}}}& {\mathbf{I}}_{n\times n}& \underset{\left(N-i\right)terms}{\underbrace{{\mathbf{0}}_{n\times n}\dots {\mathbf{0}}_{n\times n}}}\end{array}\right]$$

(41)

where n stands for the vector length, N refers to the prediction horizon, and i is the term desired.

Then, the state predictions for the sampling time are

$$\begin{array}{c}\mathbf{x}\left(k+1\right)=\mathbf{A}\mathbf{x}\left(k\right)+\mathbf{B}\mathbf{u}\left(k\right)+\mathbf{g},\\ \mathbf{x}\left(k+2\right)={\mathbf{A}}^2\mathbf{x}\left(k\right)+\mathbf{AB}\mathbf{u}\left(k\right)+\mathbf{B}\mathbf{u}\left(k+1\right)+\mathbf{Ag}+\mathbf{g},\\ ⋮\\ \mathbf{x}\left(k+i\right)=\left[{\mathbf{A}}^i\right]\mathbf{x}\left(k\right)+\left[\begin{array}{cccc}{\mathbf{A}}^{i-1}\mathbf{B}& \dots & \mathbf{AB}& \mathbf{B}\end{array}\right]\left[\begin{array}{c}\mathbf{u}\left(k\right)\\ ⋮\\ \mathbf{u}\left(k+i-2\right)\\ \mathbf{u}\left(k+i-1\right)\end{array}\right]+\left[\begin{array}{cccc}{\mathbf{A}}^{i-1}\mathbf{B}& \dots & \mathbf{A}& \mathbf{I}\end{array}\right]\left[\begin{array}{c}\mathbf{g}\\ ⋮\\ \mathbf{g}\\ \mathbf{g}\end{array}\right],\\ \mathbf{x}\left(k+i\right)=\left[{\mathbf{A}}^i\right]\mathbf{x}\left(k\right)+\left[\begin{array}{cccc}{\mathbf{A}}^{i-1}\mathbf{B}& \dots & \mathbf{AB}& \mathbf{B}\end{array}\right]\left[\begin{array}{c}{\mathbf{\Pi }}_1^{(n_u,N)}\\ ⋮\\ {\mathbf{\Pi }}_{i-1}^{(n_u,N)}\\ {\mathbf{\Pi }}_i^{(n_u,N)}\end{array}\right]\tilde{\mathbf{u}}\left(k\right)+\left[\begin{array}{cccc}{\mathbf{A}}^{\mathbf{i}-\mathbf{1}}& \dots & \mathbf{A}& \mathbf{I}\end{array}\right]\tilde{\mathbf{g}},\end{array}$$

$$\mathbf{x}\left(k+i\right)={\mathbf{\Phi }}_i\mathbf{x}\left(k\right)+{\mathbf{\Psi }}_i\tilde{\mathbf{u}}\left(k\right)+{\mathbf{\Theta }}_i\tilde{\mathbf{g}},\forall i\in \{1,\dots ,N\}$$

(42)

where ${\mathbf{\Phi }}_i\in {\mathcal{R}}^{n\times n}$, ${\mathbf{\Psi }}_i\in {\mathcal{R}}^{n\times \left(Nnu\right)}$, and ${\mathbf{\Theta }}_i\in {\mathcal{R}}^{n\times \left(Nn\right)}$ stand for the N-step-ahead in LTI systems in a more compact manner [44]. Hence, the above equation can be reformulated in the following form:

$$\tilde{\mathbf{x}}\left(k\right)=\mathbf{\Phi }\mathbf{x}\left(k\right)+\mathbf{\Psi }\tilde{\mathbf{u}}\left(k\right)+\mathbf{\Theta }\tilde{\mathbf{g}}$$

(43)

where $\mathbf{\Phi }={\left[\begin{array}{cccc}{\mathbf{\Phi }}_1& {\mathbf{\Phi }}_2& \dots & {\mathbf{\Phi }}_N\end{array}\right]}^T,\mathbf{\Psi }={\left[\begin{array}{cccc}{\mathbf{\Psi }}_1& {\mathbf{\Psi }}_2& \dots & {\mathbf{\Psi }}_N\end{array}\right]}^T,\mathbf{\Theta }={\left[\begin{array}{cccc}{\mathbf{\Theta }}_1& {\mathbf{\Theta }}_2& \dots & {\mathbf{\Theta }}_N\end{array}\right]}^T,$ $\tilde{\mathbf{x}}\left(k\right)\in {\mathcal{R}}^{N·nu}$ stands for the whole state trajectory of $\mathbf{x}={\left[{\begin{array}{cccc}{\mathbf{x}}_1& {\mathbf{x}}_2& \dots & \mathbf{x}\end{array}}_n\right]}^T;\tilde{\mathbf{u}}\left(k\right)\in {\mathcal{R}}^{N·n_u}$ contains the computed sequence of $\mathbf{u}={\left[{\begin{array}{cccc}{\mathbf{u}}_1& {\mathbf{u}}_2& \dots & \mathbf{u}\end{array}}_{n_u}\right]}^T$; $\tilde{\mathbf{g}}\in R^{(N·n)}$ contains the gravity vectors $\mathbf{g}={\left[\begin{array}{cc}0& -g\end{array}\right]}^T$; and $\tilde{\mathbf{y}}\left(k\right)\in {\mathcal{R}}^{N·n_y}$ is the output trajectory of $\mathbf{y}={\left[{\begin{array}{cccc}{\mathbf{y}}_1& {\mathbf{y}}_2& \dots & \mathbf{y}\end{array}}_{n_y}\right]}^T$. Then, depending on the prediction horizon $N,\tilde{\mathbf{x}}\left(k\right),\tilde{\mathbf{u}}\left(k\right)$, and $\tilde{\mathbf{y}}\left(k\right)$ are expressed as

$$\tilde{\mathbf{x}}\left(k\right)\equiv \left[\begin{array}{c}\mathbf{x}(k+1)\\ \mathbf{x}(k+2)\\ ⋮\\ \mathbf{x}(k+N)\end{array}\right],\tilde{\mathbf{u}}\left(k\right)\equiv \left[\begin{array}{c}\mathbf{u}\left(k\right)\\ \mathbf{u}(k+1)\\ ⋮\\ \mathbf{u}(k+N-1)\end{array}\right],\tilde{\mathbf{y}}\left(k\right)\equiv \left[\begin{array}{c}\mathbf{y}(k+1)\\ \mathbf{y}(k+2)\\ ⋮\\ \mathbf{y}(k+N)\end{array}\right].$$

(44)

Thus, given this formulation, any chain of inputs $\tilde{\mathbf{u}}\left(k\right)$ has its corresponding set of future states $\tilde{\mathbf{x}}\left(k\right)$ and outputs $\tilde{\mathbf{y}}\left(k\right)$. Now, the state, input, and output vectors at an instant $k+i$ can be obtained with

$$\mathbf{x}\left(k+i\right)={\mathbf{\Pi }}_i^{(n,N)}·\tilde{\mathbf{x}}\left(k\right),$$

$$\mathbf{u}\left(k+i-1\right)={\mathbf{\Pi }}_i^{(n,N)}·\tilde{\mathbf{u}}\left(k\right),$$

(45)

$$\mathbf{y}\left(k+i\right)={\mathbf{\Pi }}_i^{(n_y,N)}·\tilde{\mathbf{y}}\left(k\right).$$

Now, considering that all linear stable systems are quadratically stable [28], the cost function $J\left(\tilde{\mathbf{u}}|\mathbf{x}\left(k\right),{\tilde{\mathbf{w}}}^d\left(k\right),{\mathbf{u}}^d\right)$ to find the best possible set of actions in the prediction horizon $[k,k+N]$ is presented [43]

$$J\equiv {\displaystyle \sum _{i=1}^N{∥{\mathbf{\Pi }}_i^{(n_y,N)}·\tilde{\mathbf{y}}-{\mathbf{\Pi }}_i^{(n_y,N)}·{\tilde{\mathbf{w}}}^d∥}_{\mathbf{Q}}^2+\sum _{i=1}^N{∥{\mathbf{\Pi }}_i^{(n_u,N)}·\tilde{\mathbf{u}}-{\mathbf{u}}^d∥}_{\mathbf{R}}^2}$$

(46)

In the first term, the error between the output trajectory $\tilde{\mathbf{y}}\left(k\right)\in {\mathcal{R}}^{{N·n}_y}$ and the reference ${\tilde{\mathbf{w}}}^d\left(k\right)\in {\mathcal{R}}^{{N·n}_y}$ is penalized by the positive definite matrix $\mathbf{Q}\in {\mathcal{R}}^{n_y\times n_y}$. Here, ${\tilde{\mathbf{w}}}^d\left(k\right)$ contains the sequence of references ${\mathbf{w}}^d={\left[\begin{array}{cccc}{\mathbf{w}}_1^d& {\mathbf{w}}_2^d& \dots & {\mathbf{w}}_{n_y}^d\end{array}\right]}^T$, in which, depending on the prediction horizon $N,{\tilde{\mathbf{w}}}^d\left(k\right)$ can be expressed as

$${\tilde{\mathbf{w}}}^d\left(k\right)\equiv \left[\begin{array}{c}{\mathbf{w}}^d(k+1)\\ {\mathbf{w}}^d(k+2)\\ ⋮\\ {\mathbf{w}}^d(k+N)\end{array}\right]$$

(47)

Additionally, in the second term, the difference between the input vector $\tilde{\mathbf{u}}\left(k\right)$ and the desired input ${\mathbf{u}}^d\in {\mathcal{R}}^{n_u}$ is penalized by the positive definite weighting matrix $\mathbf{R}\in {\mathcal{R}}^{n_u\times n_u}$. Therefore, the minimization problem is solved to find the input vector $\tilde{\mathbf{u}}\left(k\right)$; however, only the first control input is implemented on the system. Then, the system moves to $k+1$ to solve the minimization problem again for the new control action.

The above cost function is rewritten using notations (45) and (46):

$$J\equiv {\displaystyle \sum _{i=1}^N{∥\mathbf{y}(k+i)-{\mathbf{w}}^d(k+i)∥}_{\mathbf{Q}}^2+\sum _{i=1}^N{∥{\mathbf{\Pi }}_i^{(n_u,N)}·\tilde{\mathbf{u}}-{\mathbf{u}}^d∥}_{\mathbf{R}}^2}$$

(48)

Hence, the general cost function implemented for the altitude controller in space state representation is as follows:

$$J\equiv {\displaystyle \sum _{i=1}^N{∥\mathbf{C}{\mathbf{\Phi }}_i\mathbf{x}\left(k\right)+\mathbf{C}{\mathbf{\Psi }}_i\tilde{\mathbf{u}}\left(k\right)+\mathbf{C}{\mathbf{\Theta }}_i\tilde{\mathbf{g}}-{\mathbf{\Pi }}_i^{(n_y,N)}{\tilde{\mathbf{w}}}^d\left(k\right)∥}_{\mathbf{Q}}^2+\sum _{i=1}^N{∥{\mathbf{\Pi }}_i^{(n_u,N)}·\tilde{\mathbf{u}}-{\mathbf{u}}^d∥}_{\mathbf{R}}^2}$$

(49)

Thus, the control law can be obtained by rewriting the performance index in a more compact form, as follows:

$$J\equiv \frac{1}{2}{\tilde{\mathbf{u}}}^T\mathbf{H}\tilde{\mathbf{u}}+{\left[{\mathbf{F}}_1\mathbf{x}\left(k\right)+{\mathbf{F}}_2{\tilde{\mathbf{w}}}^d\left(k\right)+{\mathbf{F}}_3{\mathbf{u}}^d+{\mathbf{F}}_4\right]}^T\tilde{\mathbf{u}}$$

(50)

where

$$\mathbf{H}\equiv 2\sum _{i=1}^N\left[{\mathbf{\Psi }}_i^T{\mathbf{C}}^T\mathbf{Q}\mathbf{C}{\mathbf{\Psi }}_i+{\left({\mathbf{\Pi }}_i^{\left(n_u,N\right)}\right)}^T\mathbf{Q}\left({\mathbf{\Pi }}_i^{(n_u,N)}\right)\right]$$

$${\mathbf{F}}_1\equiv 2\sum _{i=1}^N\left[{\mathbf{\Psi }}_i^T{\mathbf{C}}^T\mathbf{Q}\mathbf{C}{\mathbf{\Phi }}_i\right]$$

$${\mathbf{F}}_2\equiv -2\sum _{i=1}^N\left[{\mathbf{\Psi }}_i^T{\mathbf{C}}^T\mathbf{Q}{\mathbf{\Pi }}_i^{(n_y,N)}\right]$$

(51)

$${\mathbf{F}}_3\equiv -2\sum _{i=1}^N\left[{{\mathbf{\Pi }}_i^{(n_u,N)}}^T\mathbf{R}\right]$$

$${\mathbf{F}}_4\equiv 2\sum _{i=1}^N\left[{\mathbf{\Psi }}_i^T{\mathbf{C}}^T\mathbf{Q}\mathbf{C}{\mathbf{\Theta }}_i\tilde{\mathbf{g}}\right]$$

Then, from (50), the control law is obtained:

$${\tilde{\mathbf{u}}}^{opt}\left(\mathbf{x}\left(k\right)\right)=-{\mathbf{H}}^{-1}\left[{\mathbf{F}}_1\mathbf{x}\left(k\right)+{\mathbf{F}}_2{\tilde{\mathbf{w}}}^d\left(k\right)+{\mathbf{F}}_3{\mathbf{u}}^d+{\mathbf{F}}_4\right]$$

(52)

When applying the first action of the control input chain ${\tilde{\mathbf{u}}}^{opt}\left(\mathbf{x}\left(k\right)\right)$, the MPC state feedback can be expressed as

$$K_{MPC}\left(\mathbf{x}\left(k\right)\right)={\mathbf{\Pi }}_i^{\left(n_u,N\right)}·{\tilde{\mathbf{u}}}^{opt}\left(\mathbf{x}\left(k\right),{\tilde{\mathbf{w}}}^d\left(k\right),{\mathbf{u}}^d\right)$$

(53)

Moreover, the optimization problem below is addressed at every sample time instant; therefore, the measurements of the states and outputs of the system are updated constantly.

$$\mathbf{P}\left(\mathbf{x}\left(\mathbf{k}\right)\right):\underset{\tilde{\mathbf{u}}\in {\mathcal{R}}^{{Nn}_u}}{min}J\left(\tilde{\mathbf{u}}|\mathbf{x}\left(k\right),{\tilde{\mathbf{w}}}^d\left(k\right),{\mathbf{u}}^d\right)$$

(54)

4. Quadrotor Control Structure

The overall system structure shown in Figure 2 for this implementation consists of two stages: the mission server and the commander stabilizer. The mission server stage is equipped with an OptiTrack^® Flex13 motion tracking system, providing the UAV location in real-time at 120 Hz to the main PC workstation. Here, as the communication between the server and the UAV is continuous, complex reference trajectories are sent by the user in real time. Therefore, the actual and desired coordinates serve as inputs to the commander stabilizer stage. The commander stabilizer corresponds to the main elements of the Quanser^® QDrone 2 platform, such as the onboard PC, battery, and rotors. The onboard PC, NVIDIA Jetson Xavier NX SOM, contains the proposed control structure MPC, a control input PWM translator, and a battery drop compensator. Additionally, the quadrotor includes a 4S 14.8 V LiPo (3700 mAh) battery to actuate four motors with 6-inch propellers, each providing a maximum of 8.5 N lift force. Thus, the mission server and the commander stabilizer use QUARC™ 2023 SP2 software to generate real-time code directly from SIMULINK^®.

Thus, based on Figure 2, the overall control process is described in detail:

• The camera system provides the linear and yaw positions of the quadrotor, while the IMU of the quadrotor sends the angular status.
• The trajectory generator commands the desired position in the inertial frame $(x_w^d,y_w^d,z_w^d)$. These will be the references for translational control.
• The implemented cascaded control structures, PIV, and MPC receive the desired and actual position and rotational coordinates to compute $U_1,U_2,U_3,U_4$.
• Once all the control actions are calculated, the transformation matrix provided by Quanser® is used to change the control inputs to pulse-width modulation (PWM).

  $$T_m=\left[\begin{array}{cccc}0.0489& -0.4581& 0.5566& 5.1335\\ 0.0489& -0.4581& -0.5566& -5.1335\\ 0.0489& 0.4581& 0.5566& -5.1335\\ 0.0489& 0.4581& -0.5566& 5.1335\end{array}\right]$$

  (55)
• Most of the quadrotors in real-world implementations are powered by batteries [45]. Here, the reduction in power supply, named battery drainage, occurs gradually [46,47]. Therefore, the control performance of the drone is affected [48]. For this reason, we designed a compensation subsystem by measuring the altitude and battery voltage rate of change. Hence, a relationship between the PWM and the necessary battery level to reach hover position was obtained.
• The rotors receive their compensated PWM control signal.

4.1. PIV Controller Design

The main difference between the PIV controller and the well-known PID controller comes from the definition of the error [49]. In the PID, only the position error $e_p$ is considered, whereas in the PIV, the position and velocity errors, $e_p$ and $e_v$, are considered. Here, the velocity error is obtained not from the time derivative of the position error, but from calculating the difference between the desired velocity and the actual velocity of the quadrotor. Then, the compared PIV controller can be defined with the following equation:

$$PIV=K_p{e}_p+K_i{e}_i+K_v{e}_v$$

(56)

where $e_i$ stands for the position error integral with respect to time of $e_p$, and $K_p$, $K_i$, $K_v$ represent the proportional, integrative, and velocity constants, respectively. Thus, Figure 3 shows the PIV control structure.

Hence, based on Figure 2 and Figure 3, the overall PIV control process is described in detail:

• The position PIV controller obtains the desired $x_w^d,y_w^d,z_w^d$ positions from the trajectory generator while the motion tracking system provides the actual $x_w,y_w,z_w$ position of the drone. Here, the thrust input $U_1$ and the angular reference ${\varphi }^d,{\theta }^d$ of the angular PIV are computed as follows:

  $$U_1=k_{p{z}_w}{e}_{z_w}+k_{i{z}_w}{\int }_0^{\infty }{e}_{z_w}-k_{v{z}_w}{\dot{z}}_w$$

  (57)

  $${\varphi }^d=k_{p{y}_w}{e}_{y_w}+k_{i{y}_w}{\int }_0^{\infty }{e}_{y_w}-k_{v{y}_w}{\dot{y}}_w$$

  (58)

  $${\theta }^d=k_{p{x}_w}{e}_{x_w}+k_{i{x}_w}{\int }_0^{\infty }{e}_{x_w}-k_{v{x}_w}{\dot{x}}_w$$

  (59)
• Then, the angular PIV receives the desired angular positions $({\varphi }^d,{\theta }^d,{\psi }^d)$, the actual angular positions $(\varphi ,\theta ,\psi )$, and the actual angular velocities $(\dot{\varphi },\dot{\theta },\dot{\psi })$. These are processed to compute the reference angular velocities $({\dot{\varphi }}^d,{\dot{\theta }}^d,{\dot{\psi }}^d)$ of the PIV velocities, which are saturated with a minimum and a maximum rate of −45° and 45°, respectively.

  $${\dot{\varphi }}^d=k_{p\varphi }{e}_{\varphi }-k_{v\varphi }\dot{\varphi }$$

  (60)

  $${\dot{\theta }}^d=k_{p\theta }{e}_{\theta }-k_{v\theta }\dot{\theta }$$

  (61)

  $${\dot{\psi }}^d=k_{p\psi }{e}_{\psi }-k_{v\psi }\dot{\psi }$$

  (62)
• The angular PIV velocity gathers the desired angular velocities $({\dot{\varphi }}^d,{\dot{\theta }}^d,{\dot{\psi }}^d)$, the actual angular velocities $(\dot{\varphi },\dot{\theta },\dot{\psi })$, and the actual angular accelerations $(\ddot{\varphi },\ddot{\theta },\ddot{\psi })$. Then, it computes the optimal angular torques $(U_2,U_3,U_4)$; this is performed by considering the thrust input $\left(U_1\right)$ computed in step (a).

  $$U_2=k_{p\dot{\varphi }}{e}_{\dot{\varphi }}-k_{v\dot{\varphi }}\ddot{\varphi }$$

  (63)

  $$U_3=k_{p\dot{\theta }}{e}_{\dot{\theta }}-k_{v\dot{\theta }}\ddot{\theta }$$

  (64)

  $$U_4=k_{p\dot{\psi }}{e}_{\dot{\psi }}$$

  (65)

The PIV controllers always stabilize with respect to the attitude commands’ set point. Here, the attitude torques are added to the output of the remaining PIV controllers to generate the generalized force.

4.2. MPC Controller Design

The proposed cascaded MPC translational and rotational structure is shown in Figure 4, which considers the dependency between the linear displacement and the rotation of the drone.

The translational control (outer loop) tracks the altitude while computing the thrust input $\left(U_1\right)$, and the $x_w{y}_w$ position, feeding the reference orientations to the rotational control. Then, the rotational control (inner loop) tracks the attitude, applying feasible torques $(U_2,U_3,U_4)$ to the quadrotor.

The proposed cascaded MPC used to fulfill the linear drone dynamics works as follows:

• The altitude controller obtains the reference $z_w^d$ from the trajectory generator while the motion tracking system provides the actual $z_w$ position of the drone. Here, the thrust input $U_1$ is computed.
• On the other hand, $x_w$ and $y_w$ accelerations depend directly on the rotation of the drone. Thus, the dynamics are dependent, and the controllers must be related. This relation is described in the dynamical models (29), (30), where it is shown that the product between gravity and the respective angle gives a linear displacement. Then, the output of the $x_w{y}_w$ MPC algorithm is saturated with a minimum and maximum of −45° and 45°, respectively, and the desired angles are sent to the attitude controller.
• The attitude MPC receives the following: from the motion tracking system, the angular positions $(\varphi ,\theta ,\psi )$ and the angular velocities $(\dot{\varphi },\dot{\theta },\dot{\psi })$; and from the $x_w{y}_w$ MPC algorithm, the saturated orientation references $({\varphi }^d,{\theta }^d,{\psi }^d,{\dot{\varphi }}^d,{\dot{\theta }}^d,{\dot{\psi }}^d)$. Then, the attitude MPC uses this information to obtain the optimal angular torques $(U_2,U_3,U_4)$; factoring into the calculations the thrust input $\left(U_1\right)$ computed in step (a).

Additionally, for the translational and rotational control described in detail in this section, each MPC controller works according to the following algorithm in

Table 1. Methodology that describes each MPC controller implemented.

MPC Methodology
Input:	Actual states of the quadrotor and desired values
Output:	Optimal manipulation
1	Define the LTI mathematical model of the physical system in state space representation
2	Define the sampling time and discretize the state space representation
3	Set the sampling instants N for the prediction horizon
4	Compute the matrices ${\mathbf{\Phi }}_i$,${\mathbf{\Psi }}_i$ and ${\mathbf{\Theta }}_i$$,\forall i\in 1,\dots ,N$ used to represent the N-step-ahead prediction map for LTI systems
5	Define weight matrices $\mathbf{Q}$ and $\mathbf{R}$
6	Determine the saturations
7	Compute the online matrices $\mathbf{H}$ and $\mathbf{F}$
8	Obtain the control law ${\tilde{\mathbf{u}}}^{opt}$ and apply to the system the first control input of the best control input sequence

.

5. Real-Time Implementation

5.1. Experimental Setup

In this section, flight experiments are carried out to validate the effectiveness of the proposed cascaded MPC strategy compared to the traditional PIV control scheme.

Table 2. Parameters of the drone used for testing.

QDrone 2 Parameters
Dimensions	50 × 50 × 15 cm	-
Mass	1500 g	-
Max. Payload	300 g	-
$K_v$	2100 $\raisebox{1ex}{$\mathrm{RPM}$}\!\left/ \!\raisebox{-1ex}{$\mathrm{V}$}\right.$	-
Max. Continuous Current	25 A	-
$K_{v,eff}$	$1295.4 \raisebox{1ex}{$\mathrm{RPM}$}\!\left/ \!\raisebox{-1ex}{$\mathrm{V}$}\right.$	Effective motor speed constant
$W_c$	$2132.6$ RPM	Voltage-to-angular velocity offset
$W_f$	$1004.5$ RPM	Angular velocity-to-force offset
$C_t$	$2.0784\times {10}^{-8} \raisebox{1ex}{$\mathrm{N}$}\!\left/ \!\raisebox{-1ex}{${\mathrm{RPM}}^2$}\right.$	Motor force constant
$F_b$	$-0.2046$ N	Motor force offset
$k_T$	$81.0363 \raisebox{1ex}{$\mathrm{N}$}\!\left/ \!\raisebox{-1ex}{$\mathrm{Nm}$}\right.$	Motor thrust–torque constant

shows the quadrotor parameters of the QDrone 2 for the real-time implementation programmed using MATLAB^® SIMULINK^® [50].

The constant values for the PIV implementation are shown in

Table 3. PIV tuning parameters.

PIV Parameters	Position PIV $\left[\begin{array}{ccc}{\mathit{x}}_{\mathit{w}}& {\mathit{y}}_{\mathit{w}}& {\mathit{z}}_{\mathit{w}}\end{array}\right]$	Angular PIV $\left[\begin{array}{ccc}\mathit{\varphi }& \mathit{\theta }& \mathit{\psi }\end{array}\right]$	Angular Vel. PIV $\left[\begin{array}{ccc}\dot{\mathit{\varphi }}& \dot{\mathit{\theta }}& \dot{\mathit{\psi }}\end{array}\right]$
$K_p$	$\left[\frac{\pi }{2}\frac{\pi }{2}35\right]$	$\left[121215\right]$	$\left[0.1880.1540.040\right]$
$K_i$	$\left[006\right]$	-	-
$K_v$	$\left[\frac{\pi }{2}\frac{\pi }{3}28\right]$	$\left[0.10.14\right]$	$\left[0.0030.0030\right]$

[28,35]. Here, the PIV controller parameters $K_p$, $K_i$, and $K_v$ are defined for the positions $(x_w,y_w,z_w)$, the angular positions $(\varphi ,\theta ,\psi )$, and the angular velocities $(\dot{\varphi },\dot{\theta },\dot{\psi })$ of the drone.

On the other hand, the tunning parameters for a desired closed loop performance of each MPC controller are presented in

Table 4. MPC tuning parameters.

MPC Parameters	Altitude MPC	${\mathit{x}}_{\mathit{w}}{\mathit{y}}_{\mathit{w}}$ Position MPC	Attitude MPC
$\mathbf{Q}$ matrix	10	$\left[1000500\right]$	$\left[15007501500\right]$
$\mathbf{R}$ matrix	$0.00001$	$\left[7550\right]$	$\left[200500250\right]$
Prediction horizon N	10	10	15
Sample time k	$0.25$	$0.05$	$0.025$

.

5.2. Experimental Results

To validate the performance of the proposed cascaded MPC controllers over the conventional PIV control structure, the following three scenarios are real-time implemented:

• Scenario 1. Helicoidal trajectory;
• Scenario 2. Lemniscate trajectory;
• Scenario 3. Wind disturbance rejection.

Hence, to demonstrate the ability of a quadrotor in automatic flight mode, scenario 1 and scenario 2 are selected, considering that they are commonly used as a standard benchmark for tracking control [37]. Meanwhile, to test the performance of the quadrotor under external disturbances, scenario 3 is selected for regulatory control [51]. Therefore, the positions $(x_w,y_w,z_w)$ and the PWM signals in voltage of each rotor $(m_1,m_2,m_3,m_4)$ based on the control inputs $(U_1,U_2,U_3,U_4)$ must be plotted.

5.2.1. Scenario 1: Helicoidal Trajectory

Regarding the first scenario, the helicoidal trajectory is given by Equation (66). Figure 5 shows the comparison between the proposed MPC controller performance with respect to PIV control structure.

$$\begin{array}{c}{x}_w^d=sin\left(0.5t\right)\\ y_w^d=cos\left(0.5t\right)\\ z_w^d=0.5sin\left(0.05t\right)\end{array}$$

(66)

Here, even though the quadrotor under the MPC control strategy had a communication error between the main PC workstation and the UAV at 35 s, the comparison indicates that the MPC does not present steady-state errors compared to the PIV control method.

5.2.2. Scenario 2: Lemniscate Trajectory

For the second scenario, a lemniscate trajectory is programmed in the main PC workstation based on the Equation (67). Figure 6 shows the comparison between the proposed MPC controller performance with respect to PIV control strategy.

$$\begin{array}{c}{x}_w^d=2cos\left(\frac{1}{45}t\right)\\ y_w^d=sin\left(\frac{2}{45}t\right)\end{array}$$

(67)

Similarly to scenario 1, the tracking results for scenario 2 show that the PIV control presents steady-state errors.

5.2.3. Scenario 3: Disturbance Rejection

To assess the performance of the MPC and the PIV under external disturbances, the third scenario is considered in the presence of wind gusts. Initially, the quadrotor is in a hovering position according to the inertial frame (Equation (68)).

$$\begin{array}{c}{x}_w^d=-1.987\left[\mathrm{m}\right]\\ y_w^d=0.21\left[\mathrm{m}\right]\\ z_w^d=0.753\left[\mathrm{m}\right]\end{array}$$

(68)

A leaf blower with a blowing rate of 900 [m³/h] at a speed of 177 [km/h] is placed at a distance of 3 [m] away at a 45° angle. The duration of the random disturbance applied for the quadrotor under the PIV control structure is 15.96 [s], in which 3193 time steps are collected; this is considering a sample time of 5 [ms]. Meanwhile, the duration of the random disturbance under the MPC control structure is 13.355 [s], which implies the collection of 2672 time steps of 5 [ms].

Table 5. Time lapses of the wind disturbance.

Quadrotor	Start	End
Under PIV	7.005 s	22.965 s
Under MPC	9.645 s	23 s

shows the specific time in which the disturbance is applied to the quadrotor under both control structures. As is shown in Figure 7, the wind gusts produced greater disturbance against the PIV method controller than the proposed cascaded MPC. In addition, when using the proposed cascaded MPC, the signal effort presents less fluctuations.

Thus, to verify the efficiency of both controllers, a root-mean-squared error (RMSE) comparison of tracking performance for each scenario is made (Equation (69)).

$$RMSE=\sqrt{\sum _{i=1}^n\frac{{\left(y_i^d-y_i\right)}^2}{n}}$$

(69)

where n is the number of time steps, $y_i^d$ is the reference value, and $y_i$ is the measured value. Figure 8 shows the RMSE comparison, which includes 10,039 and 9026 collected time steps of 5 [ms] for the helicoidal and lemniscate trajectories, respectively. The lower the RMSE value, the better the tracking performance.

Based on the result of the practical scenarios, it can be concluded that the proposed MPC controller improves the overall tracking performance in comparison with the PIV control method. The RMSE comparison analysis from the experimental results indicates that the proposed cascaded MPC architecture outperforms the PIV method. During the helicoidal trajectory, an RMS value of 0.17 is achieved for the proposed MPC, while 0.495 is obtained using the PIV strategy. Moreover, at the end of the lemniscate trajectory, RMS values of 0.095 and 0.315 are obtained for the proposed MPC and PIV structures, respectively. Therefore, it can be seen that the PIV controller cannot adequately control the quadrotor [52].

6. Conclusions

This paper proposes a multivariable cascaded MPC structure for real-time implementation in quadrotors. The proposed architecture is used to test the translational and attitude control of the QDrone 2 by Quanser^®. For tracking control, helicoidal and lemniscate trajectories are chosen to demonstrate the ability of the quadrotor in automatic flight mode. Additionally, for regulatory control, wind gusts are applied to the quadrotor in a hovering position to validate disturbance rejection. For tracking control, using the proposed cascaded MPC architecture, the RMS values are reduced by more than a half. Similarly, lower RMS values for the three positions of the quadrotor are achieved in the presence of external disturbances, especially for $x_w$ position, which receives the wind gusts directly. Thus, higher tracking and regulatory performances are obtained. Our future work will be a maneuver control of multiple drones by using the proposed method and applying particle swarm optimization.