A qLPV-MPC Control Strategy for Trajectory Tracking of Quadrotors

Abstract

This article proposes a model predictive control (MPC) strategy for a quadrotor drone trajectory tracking based on a compact state-space model based on a quasi-linear parameter varying (qLPV) representation of the nonlinear quadrotor. The use of a qLPV representation allows for faster execution times, which can be suitable for real-time applications and for solving the optimization problem using quadratic programming (QP). The estimation of future values of the scheduling parameters along the prediction horizon is made by using the planned trajectory based on the previous optimal control actions. The performance of the proposed approach is tested by following different trajectories in simulation to show the effectiveness of the proposed control scheme.

1. Introduction

Research on unmanned aerial vehicles (UAV) has been increasing in the last two decades. UAVs have many applications such as communications, surveillance, assistance in natural disasters, mapping, delivery of goods, etc. The main reason for the popularity of UAVs in these applications is the size and speed of these vehicles, their range of mobility in harsh conditions, and the advantage of being an unmanned vehicle.

The autonomy of UAVs is crucial for a wide variety of applications without human involvement such as trajectory tracking, disturbance rejection, and collaboration with other UAVs. Therefore, the design of a controller is crucial for the use of UAVs in most applications.

One of the most popular types of UAVs is the quadrotor, a vehicle driven by four propellers. The quadrotor is a 12-state highly nonlinear system controlled only by four control inputs and the speed of each propeller. The translation of the quadrotors is possible by controlling the lift and the pitch and roll angles of the UAV to manipulate the tilt of the drone to manage the direction of translation. They are also characterized for their fast dynamics, being able to change directions quickly and to follow complex paths in the presence of external disturbances caused by the environment.

Trajectory planning and tracking is one of the most important tasks for the autonomy of quadrotors. Several studies have focused on this problem and the design of a controller that can track a desired trajectory efficiently. Some controllers that have been explored for this type of control problem are PID [1,2,3,4], LQG [5,6,7], H2 and H∞ [8,9,10], and fuzzy logic controllers [11,12,13,14]. All of these controllers achieve acceptable performance on trajectory tracking. However, most of these controllers rely on advanced tuning techniques rather than an optimal controller design considering the quadrotor’s nonlinear dynamics.

Another control strategy widely used and researched for trajectory tracking of quadrotors is model predictive control (MPC). In this strategy, the trajectory of the quadrotor is predicted along a prediction horizon based on a model as a function of the control actions. Afterward, an optimization problem is conducted online at every sampling time to obtain the optimal set of control actions based on a desired performance expressed as a cost function. One of the main advantages of MPC over other control strategies is the possibility to include constraints in the optimization problem.

In [15], a linear MPC controller for trajectory tracking control of a quadrotor is presented. In this approach, a linear model is used to represent a nonlinear quadrotor by assuming small-angle movements. This is ensured by constraints in the states of the systems and the control inputs. The results showed proper tracking of the altitude of the drone, while the trajectory control in the X and Y coordinates is not explored due to the limitations of the linear representation. Other linear MPC strategies for trajectory tracking are presented in [16,17,18,19].

To overcome the limitations of using a linear representation to design a controller for a highly nonlinear system such as a quadrotor, the nonlinear MPC (NMPC) control strategy has been used widely. In this approach, nonlinear dynamics are considered to predict the behavior of the drone along the prediction horizon. In [20], a nonlinear MPC is presented for trajectory tracking of a quadrotor. In this framework, the computational load is a limitation due to the complexity of the cost function involving nonlinear dynamics. To overcome this limitation, a numerical method solution such as the generalized minimal residual method is used. The use of numerical methods to achieve fast execution times results also in the need to add constraints for the stability of the system, which are included as contraction constraints. The results showed appropriate performance, but the stability conditions affect the convergence time. Other studies regarding NMPC strategies in UAVs are presented in [21,22,23,24,25].

Linear parameter varying (LPV) models have been used to deal with nonlinear models as linear systems or quasi-linear systems. In this type of model, the nonlinearities of the system are embedded in scheduling parameters, which are updated at each sampling time depending on the conditions of the system or external factors such as time and disturbances. LPV representations are often used to design controllers without including the complex nonlinear dynamics of most systems.

In quadrotors, LPV models have been used to design linear MPC controllers, which are dependent on the scheduling parameters. In [26], an MPC controller based on a quasi-LPV (qLPV) representation of a quadrotor is presented. In this work, a cascade control strategy is chosen and the quadrotor model is divided into a translational model and an attitude model. The translational model is defined as a linear model, while the attitude model is an LPV model. Afterward, the translational model was controlled using an LQR controller, while the LPV attitude was governed using an LPV-MPC controller. The results showed proper trajectory tracking for a spiral trajectory.

Several studies with MPC strategies for the control of UAVs are presented in [27,28,29,30]. In all of previous studies, the model of the quadrotor was divided into two models, due to the highly nonlinear dynamics of the quadrotor system. The main limitation of this kind of approach is that the scheduling parameters are unknown along the prediction horizon and an estimation of the parameters needs to be performed. If the parameters are not bounded, the estimation may result in improper estimation and non-optimal performance.

In this study, a novel qLPV-MPC control strategy for trajectory tracking of quadrotors is presented. The highly nonlinear model dynamics of the quadrotor are presented by a 12-state LPV system with six scheduling parameters depending on the states of the system and the angular velocity of the propellers. Afterward, an LPV-MPC controller is defined as a regular linear MPC depending on the scheduling parameters. The parameter estimation along the prediction horizon is considered via estimation of the states and control actions derived from the previous optimal set of control actions.

This work is structured as follows: Section 2 presents the quadrotor dynamics, including translation and attitude dynamics defined from the Earth frame and the body frame. Section 3 describes the state-space LPV model for the design of the MPC controller. In Section 4, the Model Predictive Control for trajectory tracking of the controller is presented. Section 5 shows the results of the proposed control strategy for two different trajectories. Finally, Section 6 goes over future work and conclusions.

2. Quadrotor Dynamics

The quadrotor is composed of two main features: the body frame and the propellers. The function of the propellers is to give thrust to the UAV and to steer the quadrotor. The dynamics of the quadrotor are defined by the translation of the body in the XYZ space and the attitude of the body, which is composed of the rotational angles: roll, pitch, and yaw. Figure 1 presents a schematic of the quadrotor.

As seen in Figure 1, the propellers rotate in cross directions; if all the propellers rotate at the same speed and there is no external disturbance, the quadrotor will stay in a hovering state. The roll $\varphi$, pitch $\theta$, and yaw $\psi$ represent the rotational angles along the $x_B$, $y_B$ and, $z_B$-axes of the quadrotor respectively. $[{\Omega }_1,{\Omega }_2,{\Omega }_3,{\Omega }_4]$ represent the velocity of each propeller. Note that the $x_B$, $y_B$ and, $z_B$-axes of the quadrotor are different from the XYZ axes defined by the Earth frame shown in black in Figure 1. To define the dynamics of the quadrotor, the equations will be divided into the Earth frame (E-frame) equations and the body frame (B-frame) equations, as shown in [30]. Afterward, both frames will be combined in a compound frame (C-frame).

2.1. E-Frame Dynamics

The E-frame dynamics are related to the movement of the center of gravity (COG) of the quadrotor with respect to an arbitrary origin set on a point of the Earth. These movements are the translation of the drone and are defined by the position, velocity, and acceleration of the quadrotor and the roll, pitch, and yaw movements. The set of equations used to describe the translation of the quadrotor is shown in Equation (1):

$$\left\{\begin{array}{c}\dot{x}=cos\theta cos\psi u+(sin\varphi sin\theta cos\psi -cos\varphi sin\psi )v+(cos\varphi cos\theta cos\psi +sin\varphi sin\psi )w\hfill \\ \dot{y}=cos\theta sin\psi u+(sin\varphi sin\theta sin\psi +cos\varphi cos\psi )v+(cos\varphi sin\theta sin\psi -sin\varphi cos\psi )w\hfill \\ \dot{z}=-sin\theta u+sin\varphi sin\theta v+cos\varphi cos\theta w\hfill \\ \dot{\varphi }=p+sin\varphi tan\theta q+cos\varphi tan\theta r\hfill \\ \dot{\theta }=cos\varphi q-sin\varphi r\hfill \\ \dot{\psi }=\frac{sin\varphi }{cos\theta }q+\frac{cos\varphi }{cos\theta }r\hfill \end{array}\right.$$

(1)

in which $[u,v,w]$ is the velocity vector, while $[p,q,r]$ is the angular velocity vector with respect to the B-frame. As seen in Equation (1), this representation is not enough to model the whole quadrotor because the previous equation is dependent on parameters defined in the body frame.

2.2. B-Frame Dynamics

In the B-frame, the origin is located at the COG of the quadrotor and moves with it. This type of representation is useful to see the effects that the rotation of each propeller generates at the quadrotor. Equation (2) shows the set of equations needed to represent these dynamics, including the gravitational term, the gyroscopic effect, and the input effect on these dynamics.

$$\left\{\begin{array}{c}\dot{u}=vr-wq+sin\theta g\hfill \\ \dot{v}=wp-ur-sin\varphi cos\theta g\hfill \\ \dot{w}=uq-vp-cos\varphi cos\theta g+\frac{U_1}{m}\hfill \\ \dot{p}=\frac{1}{J_{xx}}[-(J_{zz}-J_{yy})qr-J_{p}q{\Omega }_p+U_2]\hfill \\ \dot{q}=\frac{1}{J_{yy}}[(J_{zz}-J_{xx})pr-J_{p}p{\Omega }_p+U_3]\hfill \\ \dot{r}=\frac{1}{J_{zz}}[-(J_{yy}-J_{xx})pq+U_4]\hfill \end{array}\right.$$

(2)

in which g is the gravitational constant; m is the mass of the quadrotor; $[J_{xx},J_{yy},J_{zz}]$ are the inertia tensor; and $[U_1,U_2,U_3,U_4]$ are the auxiliary control inputs being functions of the speed of each rotor and defined by Equation (3)

$$\left\{\begin{array}{c}{U}_1=b({\Omega }_1^2+{\Omega }_2^2+{\Omega }_3^2+{\Omega }_4^2)\hfill \\ U_2=bl({\Omega }_4^2-{\Omega }_2^2)\hfill \\ U_3=bl({\Omega }_3^2-{\Omega }_1^2)\hfill \\ U_4=d(-{\Omega }_1^2+{\Omega }_2^2-{\Omega }_3^2+{\Omega }_4^2)\hfill \end{array}\right.$$

(3)

where b represents the thrust factor, l is the distance from the center of the quadrotor to the propeller, and d is the drag factor. The overall velocity of the propellers is defined as ${\Omega }_p=-{\Omega }_1+{\Omega }_2-{\Omega }_3+{\Omega }_4$.

Similar to the E-frame, the B-frame is not enough to understand all the dynamics of the quadrotor due to the dependence on parameters represented in the E-frame such as the roll, pitch, and yaw values. In order to build a model that includes all the dynamics of both the E-frame and the B-frame, a compound frame (C-frame) needs to be developed.

2.3. C-Frame Dynamics

In order to define a compound frame, a relation between the B-frame and the E-frame needs to be obtained. The relation of these two frames is represented by Equation (4).

$$\dot{\zeta }=J_{\Theta }v$$

(4)

in which $\zeta ={[x,y,z,\varphi ,\theta ,\psi ]}^T$ is the E-frame vector; $v={[u,v,w,p,q,r]}^T$ is the B-frame vector; and $J_{\Theta }$ is a transformation matrix defined as:

$$J_{\Theta }=\left[\begin{array}{cccccc}cos\theta cos\psi & sin\varphi sin\theta cos\psi -cos\varphi sin\psi & cos\varphi sin\theta cos\psi +sin\varphi sin\psi & 0& 0& 0\\ cos\theta sin\psi & sin\varphi sin\theta sin\psi +cos\varphi cos\psi & cos\varphi sin\theta sin\psi -sin\varphi cos\psi & 0& 0& 0\\ -sin\theta & sin\varphi sin\theta & cos\varphi cos\theta & 0& 0& 0\\ 0& 0& 0& 1& sin\varphi tan\theta & cos\varphi tan\theta \\ 0& 0& 0& 0& cos\varphi & -sin\varphi \\ 0& 0& 0& 0& \frac{sin\varphi }{cos\theta }& \frac{cos\varphi }{cos\theta }\end{array}\right]$$

With this transformation matrix and Equations (1)–(4), the equations defining the dynamics of the quadrotor can be derived. These equations are shown in Equation (5).

$$\left\{\begin{array}{c}\ddot{x}=\frac{1}{m}[cos\left(\varphi \right)sin\left(\theta \right)cos\left(\psi \right)+sin\left(\varphi \right)sin\left(\psi \right)]U_1\hfill \\ \ddot{y}=\frac{1}{m}[cos\left(\varphi \right)sin\left(\theta \right)sin\left(\psi \right)-sin\left(\varphi \right)cos\left(\psi \right)]U_1\hfill \\ \ddot{z}=\frac{1}{m}[cos\left(\varphi \right)cos\left(\theta \right)]U_1-g\hfill \\ \dot{p}=\frac{1}{J_{xx}}[-(J_{zz}-J_{yy})qr-J_{p}q{\Omega }_p+U_2]\hfill \\ \dot{q}=\frac{1}{J_{yy}}[(J_{zz}-J_{xx})pr-J_{p}p{\Omega }_p+U_3]\hfill \\ \dot{r}=\frac{1}{J_{zz}}[-(J_{yy}-J_{xx})pq+U_4]\hfill \end{array}\right.$$

(5)

3. LPV State Space Model of the Quadrotor

In order to build the MPC cost function, an LPV-state-space model with scheduling variable-dependent matrices A and B is defined. Using Equations (1)–(5), a nonlinear state-space model to track all the variables for the movement of the quadrotor can be derived. The nonlinear state-space model for the quadrotor is shown in Equation (6).

$$\left\{\begin{array}{c}\dot{x}=v_x\hfill \\ \dot{y}=v_y\hfill \\ \dot{z}=v_z\hfill \\ {\dot{v}}_x=\frac{1}{m}[cos\left(\varphi \right)sin\left(\theta \right)cos\left(\psi \right)+sin\left(\varphi \right)sin\left(\psi \right)]U_1\hfill \\ {\dot{v}}_y=\frac{1}{m}[cos\left(\varphi \right)sin\left(\theta \right)sin\left(\psi \right)-sin\left(\varphi \right)cos\left(\psi \right)]U_1\hfill \\ {\dot{v}}_z=\frac{1}{m}[cos\left(\varphi \right)cos\left(\theta \right)]U_1-g\hfill \\ \dot{\varphi }=p+sin\left(\varphi \right)tan\left(\theta \right)q+cos\left(\varphi \right)tan\left(\theta \right)r\hfill \\ \dot{\theta }=cos\left(\varphi \right)q-sin\left(\varphi \right)r\hfill \\ \dot{\psi }=\frac{sin\left(\varphi \right)}{cos\left(\theta \right)}q+\frac{cos\left(\varphi \right)}{cos\left(\theta \right)}r\hfill \\ \dot{p}=\frac{1}{J_{xx}}[-(J_{zz}-J_{yy})qr-J_{p}q{\Omega }_p+U_2]\hfill \\ \dot{q}=\frac{1}{J_{yy}}[(J_{zz}-J_{xx})pr-J_{p}p{\Omega }_p+U_3]\hfill \\ \dot{r}=\frac{1}{J_{zz}}[-(J_{yy}-J_{xx})pq+U_4]\hfill \end{array}\right.$$

(6)

As seen in the previous state-space system, the quadrotor is highly nonlinear; therefore, in order to treat the system as a linear state-space system, an LPV representation with six different scheduling variables depending directly on the states of the system is derived. The scheduling variables are presented in Equation (7).

$$\left\{\begin{array}{c}{\rho }_1=\varphi \hfill \\ {\rho }_2=\theta \hfill \\ {\rho }_3=\psi \hfill \\ {\rho }_4=p\hfill \\ {\rho }_5=q\hfill \\ {\rho }_6={\Omega }_p\hfill \end{array}\right.$$

(7)

The scheduling variables were chosen to represent the nonlinear state-space system of the quadrotor in an LPV state-space model using the least amount of scheduling variables needed to model the nonlinear dynamics of the quadrotor. With Equations (6) and (7), a quasi-LPV state-space system can be used to compute the MPC cost function in a linear form. The qLPV-SS with matrices $\mathbf{A}$ and $\mathbf{B}$ being scheduling parameter-dependent is shown in Equation (8).

$$\dot{\mathbf{x}}\left(t\right)=\mathbf{A}({\rho }_1,{\rho }_2,{\rho }_3,{\rho }_4,{\rho }_5,{\rho }_6)\mathbf{x}\left(t\right)+\mathbf{B}({\rho }_1,{\rho }_2,{\rho }_3)\left[\begin{array}{c}\mathbf{u}\left(t\right)\\ g\end{array}\right]$$

(8)

where $\mathbf{x}\left(t\right)={[x,y,z,v_x,v_y,v_z,\varphi ,\theta ,\psi ,p,q,r]}^T$, $\mathbf{u}\left(t\right)={[U1,U2,U3,U4]}^T$, and matrices A and B are defined as follows:

$$\mathbf{A}=\left[\begin{array}{cccccccccccc}0& 0& 0& 1& 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& 1& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& ϵ{\rho }_2& ϵ{\rho }_3& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& ϵ{\rho }_1& 0& ϵ{\rho }_3& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& ϵ{\rho }_1& ϵ{\rho }_2& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 1& sin\left({\rho }_1\right)tan\left({\rho }_2\right)& cos\left({\rho }_1\right)tan\left({\rho }_2\right)\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& cos\left({\rho }_1\right)& -sin\left({\rho }_1\right)\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& \frac{sin\left({\rho }_1\right)}{cos\left({\rho }_2\right)}& \frac{cos\left({\rho }_1\right)}{cos\left({\rho }_2\right)}\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& -\frac{J_p{\rho }_6}{J_{xx}}& \frac{-(J_{zz}-J_{yy}){\rho }_5}{J_{xx}}\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& \frac{J_p{\rho }_6}{J_{yy}}& 0& \frac{(J_{zz}-J_{xx}){\rho }_4}{J_{yy}}\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& \frac{-(J_{yy}-J_{xx}){\rho }_4}{J_{zz}}& 0\end{array}\right]$$

$$\mathbf{B}=\left[\begin{array}{ccccc}0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\\ \frac{1}{m}[cos\left({\rho }_1\right)sin\left({\rho }_2\right)cos\left({\rho }_3\right)+sin\left({\rho }_1\right)sin\left({\rho }_3\right)]& 0& 0& 0& 0\\ \frac{1}{m}[cos\left({\rho }_1\right)sin\left({\rho }_2\right)sin\left({\rho }_3\right)-sin\left({\rho }_1\right)cos\left({\rho }_3\right)]& 0& 0& 0& 0\\ \frac{1}{m}[cos\left({\rho }_1\right)cos\left({\rho }_2\right)]& 0& 0& 0& -1\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& \frac{1}{J_{xx}}& 0& 0& 0\\ 0& 0& \frac{1}{J_{yy}}& 0& 0\\ 0& 0& 0& \frac{1}{J_{zz}}& 0\end{array}\right]$$

The value of $ϵ$ is defined as a very low constant (e.g., $ϵ=1\times 10$${}^{-3}$) and is included for the system to be controllable and to avoid having three zero rows in the A matrix. The parameter $ϵ$ is therefore added for the MPC to identify that, by changing the attitude angles, the acceleration on the XYZ plane will be affected, but the predicted trajectory will not be significantly affected due to the small value of $ϵ$. The inclusion of the six different scheduling variables on the qLPV state-space allows for expressing the highly nonlinear quadrotor system into a compact quasi-linear state-space system, which is updated at each sampling time depending on the values of the states and the speed of the propellers. This compact qLPV-SS representation simplifies the computation of the future states of the system along the prediction horizon and improves the construction of the MPC cost function without splitting the system into several sub-systems. The gravitational constant is treated as an uncontrollable input or disturbance, which is constant at every sampling instant. In the next section, the proposed qLPV-SS presented in (8) will be used to compute the MPC cost function and to solve the optimization problem to obtain the optimal set of control actions.

4. Model Predictive Control Based on the LPV Representation of the Quadrotor

To build an MPC based on an LPV model, the prediction of the future states along the prediction horizon $N_p$ is formulated considering not only the state matrices A and B but also the variations in the scheduling variables along the prediction horizon. Therefore, matrices A and B are not considered constant along the prediction horizon, and the values of these matrices need to be considered for each step of the horizon in order to consider the nonlinear behavior of the quadrotor. Thus, Equation (9) presents the i-steps ahead calculation of the states of the quadrotor.

$$\mathbf{x}(k+i|k)=\prod _{j=0}^{i-1}{\mathbf{A}}_d\left({\rho }_i(k+j)\right)x\left(k\right)+\left(\sum _{s=1}^{i-1}\left(\prod _{l=s}^{i-1}{\mathbf{A}}_d\left({\rho }_i(k+l)\right)\right){\mathbf{B}}_d\left({\rho }_i(k+s-1)\right)\left[\begin{array}{c}\mathbf{u}(k+s-1)\\ g\end{array}\right]\right)+{\mathbf{B}}_d\left({\rho }_i(k+i-1)\right)\left[\begin{array}{c}\mathbf{u}(k+i-1)\\ g\end{array}\right]$$

(9)

where ${\mathbf{A}}_d\left({\rho }_i\right)$ and ${\mathbf{B}}_d\left({\rho }_i\right)$ are the discrete matrices obtained from the continuous matrices A and B presented in Section 3 using sampling time $T_s$ and zero-order-hold (ZOH) discretization. These matrices are updated at every sampling instant k as the set of scheduling parameters $\left({\rho }_i\right)$ are also updated at each iteration. The prediction of the future states is performed along the prediction horizon, from k to $k+N_p$; thus, Equation (9) can be expressed as a compact matrix form to calculate the values of the states in the prediction horizon, as shown in Equation (10).

$$\mathbf{X}=\mathsf{\Phi }·\mathbf{x}\left(k\right)+\mathsf{\Psi }·\mathbf{U}$$

(10)

where

$$\mathbf{X}=\left[\begin{array}{c}\mathbf{x}(k+1|k)\\ \mathbf{x}(k+2|k)\\ :\\ .\\ \mathbf{x}(k+N_p|k)\end{array}\right]$$

(11)

$$\mathsf{\Phi }\left(k\right)=\left[\begin{array}{c}{\mathbf{A}}_d\left({\rho }_i\left(k\right)\right)\\ \prod _{j=0}^1\left({\mathbf{A}}_d\left({\rho }_i(k+j)\right)\right)\\ ⋮\\ \prod _{j=0}^{N_p-1}\left({\mathbf{A}}_d\left({\rho }_i(k+j)\right)\right)\end{array}\right]$$

(12)

$$\mathsf{\Psi }\left(k\right)=\left[\begin{array}{cccc}{\mathbf{B}}_d\left({\rho }_i\left(k\right)\right)& 0_{nx·n_u}& \cdots & 0_{nx·n_u}\\ {\mathbf{A}}_d\left({\rho }_i(k+1)\right){\mathbf{B}}_d\left({\rho }_i\left(k\right)\right)& {\mathbf{B}}_d\left({\rho }_i(k+1)\right)& \cdots & 0_{nx·n_u}\\ {\mathbf{A}}_d\left({\rho }_i(k+2)\right){\mathbf{A}}_d\left({\rho }_i(k+1)\right){\mathbf{B}}_d\left({\rho }_i\left(k\right)\right)& {\mathbf{A}}_d\left({\rho }_i(k+2)\right){\mathbf{B}}_d\left({\rho }_i(k+1)\right)& \cdots & 0_{nx·n_u}\\ ⋮& ⋮& \ddots & ⋮& ⋮\\ \left(\prod _{i=1}^{N_p-1}{\mathbf{A}}_d\left({\rho }_i(k+1)\right)\right){\mathbf{B}}_d\left({\rho }_i\left(k\right)\right)& \left(\prod _{i=1}^{N_p-1}{\mathbf{A}}_d\left({\rho }_i(k+2)\right)\right){\mathbf{B}}_d\left({\rho }_i(k+1)\right)& \cdots & {\mathbf{B}}_d\left({\rho }_i(k+N_p-1)\right)\end{array}\right]$$

(13)

$$\mathbf{U}=\left[\begin{array}{c}\mathbf{u}\left(k\right)\\ g\\ \mathbf{u}(k+1)\\ g\\ :\\ .\\ \mathbf{u}(k+N_p-1)\\ g\end{array}\right]$$

(14)

where $\mathbf{X}\in {\mathbb{R}}^{N_p·n_x}$, $\mathsf{\Phi }\in {\mathbb{R}}^{N_p\times n_x·n_x}$, $\mathsf{\Psi }\in {\mathbb{R}}^{N_p·n_x\times N_p·n_u}$ and $\mathbf{U}\in {\mathbb{R}}^{N_p·(n_u+1)}$, where $n_x$ represents the number of states and $n_u$ represents the number of controllable inputs of the quadrotor. With Equation (10), the computation of the future values of the states of the quadrotor is performed, and a cost function for trajectory tracking can be designed to make the quadrotor follow a specified trajectory and to minimize the effort of the propellers. The proposed cost function is the following:

$$J={(\mathbf{X}\left(k\right)-{\mathbf{X}}_r\left(k\right))}^{T}Q(\mathbf{X}\left(k\right)-{\mathbf{X}}_r\left(k\right))+{(\mathbf{U}\left(k\right)-\mathbf{U}(k-1))}^{T}R(\mathbf{U}\left(k\right)-\mathbf{U}(k-1))$$

(15)

where $Q\in {\mathbb{R}}^{n_x·N_p\times n_x·N_p}$ and $R\in {\mathbb{R}}^{n_u·N_p\times n_u·N_p}$ are weighting matrices, $X_r$ is the trajectory reference vector along the prediction horizon, and $\mathbf{U}(k-1)$ is the previous set of optimal control actions. For this application, the trajectory reference vector only includes the desired trajectory in the XYZ coordinates and the yaw angle reference. The other states are considered free states and are only bonded by the constraint limits. The values of the scheduling variables along the prediction horizon are calculated by using Equation (10) from the previous iteration. Having the future values of the states along the prediction horizon allows us to represent the nonlinear behavior of the quadrotor correctly in the MPC algorithm. The objective of the MPC strategy is to minimize the cost function (15) at every sampling instant k subject to the constraints shown in (16) and (17).

$${\mathbf{U}}_{min}\le \mathbf{U}\le {\mathbf{U}}_{max}$$

(16)

$${\mathbf{X}}_{min}\le \mathbf{X}\le {\mathbf{X}}_{max}$$

(17)

where ${\mathbf{U}}_{min},{\mathbf{U}}_{max}\in {\mathbb{R}}^{(n_u+1)·N_p}$ and ${\mathbf{X}}_{min},{\mathbf{X}}_{max}\in {\mathbb{R}}^{n_x·N_p}$ are the defined limits for the controlled inputs and the states, respectively. The gravitational term g included in the control vector $\mathbf{U}$ is constrained to be equal to 9.81 at every iteration. Thus, the optimization problem to solve at each sampling instant k is presented in Equation (18).

$$\underset{\mathbf{U}}{min}J\mathbf{s}.\mathbf{t}.\left(16\right)\mathrm{and}\left(17\right)$$

(18)

Solving the optimization problem (18) results in the optimal set of control actions $\mathbf{U}$ along the prediction horizons; therefore, a conversion from the control actions to the actual speeds of the propellers that are input to the system needs to be performed. Using Equation (3), the values of the speed of each propeller can be calculated as shown in Equation (19).

$$\left[\begin{array}{c}{\Omega }_1^2\\ {\Omega }_2^2\\ {\Omega }_3^2\\ {\Omega }_4^2\end{array}\right]={\left[\begin{array}{cccc}b& b& b& b\\ 0& -bl& 0& bl\\ -bl& 0& bl& 0\\ -d& d& -d& d\end{array}\right]}^{-1}\left[\begin{array}{c}{U}_1\\ U_2\\ U_3\\ U_4\end{array}\right]$$

(19)

To obtain the optimal velocities of each propellers ${[{\Omega }_1,{\Omega }_2,{\Omega }_3,{\Omega }_4]}^T$, the absolute value of the square root of each square velocity is computed. Figure 2 presents the qLPV-MPC control strategy for the quadrotor using a compact 12-state qLPV system.

5. Results and Discussion

In order to test the performance of the proposed qLPV-MPC control strategy, a simulation of a quadrotor following two different trajectories will be performed. In the first test, a linear trajectory will be tested, while in the second test, the quadrotor will follow a spiral trajectory. In both tests, the quadrotor will start at a different starting point and will need to track and follow the trajectory.

Table 1. Constant Values of the quadrotor.

Variable	Value	Units
g	9.8	m/s${}^2$
m	0.698	kg
$J_{xx}$	0.0034	kg · m${}^2$
$J_{yy}$	0.0034	kg · m${}^2$
$J_{zz}$	0.006	kg · m${}^2$
$J_p$	1.302 × 10${}^{-6}$	kg · m${}^2$
b	7.6184 × 10${}^{-9}$	N · s${}^2$
d	2.6839 × 10${}^{-9}$	N · s${}^2$
l	0.171	m

shows the specifications of the quadrotor as presented in [26].

The linear trajectory and the spiral trajectory are shown in Figure 3 and Figure 4, respectively. The simulations were performed in the Matlab-Simulink (Matlab R2019b) software environment running on a Macbook Air with a dual-core Intel Core i5 1.8 GHz processor and 8 GB of RAM with 1600 MHz DDR3. The sampling time was set to $T_s=10$ ms to be able to measure and detect the fast dynamics of the quadrotor in any flying situation. The weighting matrix $\mathbf{Q}$ was designed in order to penalize the states more regarding the XYZ position and velocity at the first prediction steps. Weighting matrix $\mathbf{R}$ is designed as an identity matrix in order to penalize all the control actions equally. The optimization was performed using the Matlab function fmincon with the solver of sequential quadratic programming (SQP) to manage the cost function as a Quadratic Programming (QP) problem.

A prediction horizon of $N_p=5$ was defined after several simulations testing the performance for different prediction horizons. Increasing the prediction horizon will not improve the performance of the proposed control strategy significantly but will increase the optimization times, making real-time implementation impossible. Figure 5 presents the results of the trajectory tracking for the proposed qLPV-MPC for the linear reference trajectory. Figure 6, Figure 7 and Figure 8 show the values of the XYZ reference coordinates individually and the tracking carried out by the quadrotor with the qLPV-MPC strategy.

Figure 9 shows the behavior of the linear velocity in each coordinate and the angular position of the quadrotor during the tracking of the linear trajectory, while Figure 10 and Figure 11 show the control actions chosen and the required speed for each propeller.

Figure 12 presents the results of the trajectory tracking for the proposed qLPV-MPC for the linear reference trajectory. Figure 13, Figure 14 and Figure 15 show the values of the XYZ reference coordinates individually and the tracking performed by the quadrotor with the qLPV-MPC strategy.

Figure 16 shows the behavior of the linear velocity in each coordinate and the angular position of the quadrotor during the tracking of the linear trajectory, while Figure 17 and Figure 18 show the control actions chosen and the required speed for each propeller.

The control actions U present a small jitter, as shown in Figure 10 and Figure 17, as the nonlinearities of the system rapidly change the dynamics in the qLPV system; however, the speed of the rotors $\Omega$, which are the physical control actions, do not present this behavior and make small variations to guide the drone to the desired trajectory. The performance of the proposed qLPV-MPC control for trajectory tracking of the quadrotor proved to be appropriate for tracking both the linear and the spiral trajectory even when starting at a different initial point. The qLPV-MPC plans an optimal trajectory for the quadrotor to be incorporated into the setpoint trajectory in the XYZ coordinates without trying to reach a point in the trajectory that will be unfeasible due to the constraints in both the states and the inputs, resulting in a smooth transition to the desired trajectory. As a result of this smooth transition, the quadrotor attack angles remain at small values, in order to avoid the instability of the UAV. It is also important to note that when the starting point of the quadrotor is also the starting point of the desired trajectory, the path tracking is performed perfectly, and the reference curves and the actual trajectory overlap each other even with nonlinear changes in the reference. This is an advantage of the MPC control strategy over other classical controllers that react over the tracking error.

The proposed controller also exhibits fast execution times, with an average time of 6.14 ms per optimization each iteration, and a worst-case time of 8.76 ms using the Matlab function ’fmincon’. Therefore, the proposed qLPV-MPC allows real-time implementation for the quadrotor with a sampling time of $Ts=10$ ms.

6. Conclusions

In this research work, a novel qLPV-MPC control strategy for a quadrotor based on a compact 12-state-space system is presented. The MPC controller is based on a qLPV system with six different scheduling parameters being states of the nonlinear quadrotor system in order to allow real-time implementation by solving the optimization problem in a short time. The prediction of the states along the prediction horizon is used in the next iterations to update the prediction matrices $\mathsf{\Phi }$ and $\mathsf{\Psi }$ to model the nonlinear behavior of the quadrotor system by means of the scheduling variables. The proposed qLPV-MPC control strategy was tested by following two different trajectories, a linear trajectory, and a spiral trajectory. The results showed proper tracking of both trajectories while maintaining small roll, pitch, and yaw angles to maintain the quadrotor in a stable state. The execution times averaged 6.14 ms per optimization of each iteration which allows real-time implementation of the control strategy for the chosen sampling time of 10 ms. Future research work will focus on the robustness conditions of the proposed algorithm in the presence of wind disturbance and obstacle avoidance when following a desired path. An experimental test using a real quadrotor is also considered for future work.