# Tube MPC via flatness for multicopter trajectory tracking

Huu-Thinh Do, Ionela Prodan

<figure><img src="image_1.png" /></figure>

To cite this version:

Huu-Thinh Do, Ionela Prodan. Tube MPC via flatness for multicopter trajectory tracking. 2024 European Control Conference, Jun 2024, Stockholm, Sweden. hal-04620816

HAL Id: hal-04620816

https://hal.science/hal-04620816

Submitted on 22 Jun 2024

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_2.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
# Tube MPC via flatness for multicopter trajectory tracking

Huu-Thinh Do<fnref n="1" /> and Ionela Prodan<fnref n="1" />

**Abstract**—Nonlinearity and dimensionality have always been computationally challenging problems when it comes to the online implementation of optimization-based control approaches, especially in the presence of disturbances. In this paper, we show how to alleviate the problem via a variable reformulation derived from differential flatness for a quadcopter vehicle. More specifically, we present a robust model predictive control design, to track a predefined trajectory of a quadcopter in the presence of disturbances. The synthesis procedure starts with a coordinate change mediated by the model's flatness property. In this new representation, the dynamics become linear in closed-loop at the price of more convoluted constraint expressions, which are usually disregarded in the literature or simple approximations are proposed. Subsequently, with a proper parameterization to portray the feasible domain, the trajectory tracking problem is transformed into the stabilization of a constrained linear time-invariant system under disturbances, which is then handled by a robust model predictive controller. Simulations and experimental results are presented to analyze and validate the proposed scheme.

**Index Terms**—Quadcopter trajectory tracking, feedback linearization, tube-based MPC, differential flatness.

## I. INTRODUCTION

The problem of maneuvering the quadcopter vehicles with not only stability, but also optimality and robustness, still remains open for discussion in the literature. The primary issue lies in the vehicle's trigonometric complexity, which typically demands high computational power to find a feasible stabilizing control for the non-linear problem [1], [2]. To address these challenges, while many robust techniques have been offered, Model Predictive Control (MPC) is of special interest to us thanks to its optimality and constraint-handling capability. Theoretically, over a finite prediction horizon in the future, stability, feasibility, and robustness can be ensured with a proper formulation of the cost, constraints, and a local controller within the MPC design [3]. However, due to the problem's complex expressions, its solution is hard to find, at least in the limited sampling time of aerial vehicles. As a partial solution, linearization of the system dynamics around its operating points is typically exploited to achieve a linear representation (also referred to as the approximation with “small” valued angles/states), then robust or adaptive approaches can be applied to ensure performance and constraints. Although the strategy appears to be generic and computationally attractive, the controller's validity is only secured “near” a reference [4], [5], or partially assured for decoupled dynamics [6], [7].

In contrast, the notion of exact linearization has promised to solve the issue by reshaping the nonlinear dynamics into a linear, controllable system. Indeed, the quadcopter's model is known to be differentially flat [8], [9] (i.e., all states and inputs can be expressed algebraically with a special output called flat output and a finite number of its derivatives). This input-output representation implies that, via an input change and a dynamic feedback, the system can be converted into chains of integrators in a new coordinate, called the *flat output space* [10]. Ideally, this property is beneficial for both the vehicle's trajectory planning problem and control synthesis, as they are now reduced to the level of trivial integrators. However, owing to that reformulation, although the dynamics' nonlinearity is discarded, the original constraint set becomes convoluted. To handle this disadvantage, conservative linear estimations of such a new feasible domain have been exploited to design a controller for the new linear system [8], [11]. Even so, due to such conservatism, the robustness of the quadcopter under this strategy remains inadequately covered in the literature, especially for the trajectory tracking problem. Thus, in this paper, we exploit the well-known linearizing transformation for the quadcopter's translation dynamics and present a synthesis procedure for robust trajectory tracking based on MPC. Specifically, we:

* address the problem of the nonlinear convoluted constraints arising from the linearization in closed-loop in the flat output space by a surface parameterization;

* formulate an efficient synthesis procedure for the quadcopter's position tracking with tube-based MPC with ellipsoidal terminal ingredients;

* validate the design via simulation and experimental tests over the Crazyflie 2.1 nano-drone platform. Experiment video can be found at https://youtu.be/aJHZyouXtwo.

**Notation:** Bold capital letters are matrices with appropriate dimensions. diag(·) returns a diagonally arranged matrix with the components given within. $Q > 0$ and $Q \succeq 0$ imply $Q$ is positive definite and semi-definite, respectively. For $Q \succeq 0$, $\lambda_{max}(Q)$, $\lambda_{min}(Q)$ denote its largest and smallest eigenvalues, respectively. Bold letters denote vectors, while the subscript $k$ denote the discrete step $k$. $0_{n \times m}$ represents a matrix of dimension $n \times m$ of which all components are 0. For $x \in \mathbb{R}^n$, $i^k x$ represents its $i$-th entry, while the superscript “ref” presents the reference for the signal to follow. $\|x\|_P \triangleq \sqrt{x^\top P x}$ and $\|x\|_2 \triangleq \sqrt{x^\top x}$. $N^p(a, b)$ returns a set of $p$ evenly spaced samples over the interval $[a, b]$. $\text{co}\{\cdot\}$ denote the convex hull. Finally, $\oplus$ and $\ominus$ denote the Minkowski sum and Pontryagin difference, respectively.

<footnote n="1">¹Univ. Grenoble Alpes, Grenoble INP†, LCIS, 26000 Valence, France.
Email: {huu-thinh.do, ionela.prodan}@lcis.grenoble-inp.fr
†Institute of Engineering and Management Univ. Grenoble Alpes. This work
is funded by La Région, Pack Ambition Recherche 2021 - PlanMAV.</footnote>
# Tube MPC via flatness for multicopter trajectory tracking

Huu-Thinh Do<fnref n="1" /> and Ionela Prodan<fnref n="1" />

**Abstract**—Nonlinearity and dimensionality have always been computationally challenging problems when it comes to the online implementation of optimization-based control approaches, especially in the presence of disturbances. In this paper, we show how to alleviate the problem via a variable reformulation derived from differential flatness for a quadcopter vehicle. More specifically, we present a robust model predictive control design, to track a predefined trajectory of a quadcopter in the presence of disturbances. The synthesis procedure starts with a coordinate change mediated by the model's flatness property. In this new representation, the dynamics become linear in closed-loop at the price of more convoluted constraint expressions, which are usually disregarded in the literature or simple approximations are proposed. Subsequently, with a proper parameterization to portray the feasible domain, the trajectory tracking problem is transformed into the stabilization of a constrained linear time-invariant system under disturbances, which is then handled by a robust model predictive controller. Simulations and experimental results are presented to analyze and validate the proposed scheme.

**Index Terms**—Quadcopter trajectory tracking, feedback linearization, tube-based MPC, differential flatness.

## I. INTRODUCTION

The problem of maneuvering the quadcopter vehicles with not only stability, but also optimality and robustness, still remains open for discussion in the literature. The primary issue lies in the vehicle's trigonometric complexity, which typically demands high computational power to find a feasible stabilizing control for the non-linear problem [1], [2]. To address these challenges, while many robust techniques have been offered, Model Predictive Control (MPC) is of special interest to us thanks to its optimality and constraint-handling capability. Theoretically, over a finite prediction horizon in the future, stability, feasibility, and robustness can be ensured with a proper formulation of the cost, constraints, and a local controller within the MPC design [3]. However, due to the problem's complex expressions, its solution is hard to find, at least in the limited sampling time of aerial vehicles. As a partial solution, linearization of the system dynamics around its operating points is typically exploited to achieve a linear representation (also referred to as the approximation with “small” valued angles/states), then robust or adaptive approaches can be applied to ensure performance and constraints. Although the strategy appears to be generic and computationally attractive, the controller's validity is only secured “near” a reference [4], [5], or partially assured for decoupled dynamics [6], [7].

In contrast, the notion of exact linearization has promised to solve the issue by reshaping the nonlinear dynamics into a linear, controllable system. Indeed, the quadcopter's model is known to be differentially flat [8], [9] (i.e., all states and inputs can be expressed algebraically with a special output called flat output and a finite number of its derivatives). This input-output representation implies that, via an input change and a dynamic feedback, the system can be converted into chains of integrators in a new coordinate, called the *flat output space* [10]. Ideally, this property is beneficial for both the vehicle's trajectory planning problem and control synthesis, as they are now reduced to the level of trivial integrators. However, owing to that reformulation, although the dynamics' nonlinearity is discarded, the original constraint set becomes convoluted. To handle this disadvantage, conservative linear estimations of such a new feasible domain have been exploited to design a controller for the new linear system [8], [11]. Even so, due to such conservatism, the robustness of the quadcopter under this strategy remains inadequately covered in the literature, especially for the trajectory tracking problem. Thus, in this paper, we exploit the well-known linearizing transformation for the quadcopter's translation dynamics and present a synthesis procedure for robust trajectory tracking based on MPC. Specifically, we:

* address the problem of the nonlinear convoluted constraints arising from the linearization in closed-loop in the flat output space by a surface parameterization;

* formulate an efficient synthesis procedure for the quadcopter's position tracking with tube-based MPC with ellipsoidal terminal ingredients;

* validate the design via simulation and experimental tests over the Crazyflie 2.1 nano-drone platform. Experiment video can be found at https://youtu.be/aJHZyouXtwo.

**Notation:** Bold capital letters are matrices with appropriate dimensions. diag(·) returns a diagonally arranged matrix with the components given within. $Q > 0$ and $Q \succeq 0$ imply $Q$ is positive definite and semi-definite, respectively. For $Q \succeq 0$, $\lambda_{max}(Q)$, $\lambda_{min}(Q)$ denote its largest and smallest eigenvalues, respectively. Bold letters denote vectors, while the subscript $k$ denote the discrete step $k$. $0_{n \times m}$ represents a matrix of dimension $n \times m$ of which all components are 0. For $x \in \mathbb{R}^n$, $i^th$ $x$ represents its $i$-th entry, while the superscript “ref” presents the reference for the signal to follow. $||x||_P \triangleq \sqrt{x^\top P x}$ and $||x||_2 \triangleq \sqrt{x^\top x}$. $\mathbb{N}^p(a, b)$ returns a set of $p$ evenly spaced samples over the interval $[a, b]$. $\text{co}\{\cdot\}$ denote the convex hull. Finally, $\oplus$ and $\ominus$ denote the Minkowski sum and Pontryagin difference, respectively.

<footnote n="1">¹Univ. Grenoble Alpes, Grenoble INP†, LCIS, 26000 Valence, France.
Email: {huu-thinh.do, ionela.prodan}@lcis.grenoble-inp.fr
†Institute of Engineering and Management Univ. Grenoble Alpes. This work
is funded by La Région, Pack Ambition Recherche 2021 - PlanMAV.</footnote>
## II. MODEL REPRESENTATION AND MPC SETUP

First, we recapitulate the quadcopter model employed for position control design and its representation in the flat output space, which possesses linear dynamics and a non-linear constraint. The ellipsoidal terminal ingredient synthesis for constrained linear system is also presented.

### A. Quadcopter position control

The quadcopter's discretized translation dynamics:

$$x_{k+1} = A_d x_k + B_d h_{\psi}(u_k), \quad (1)$$

where the vectors and matrices are defined as:

$$\begin{align}
x_k &= [x_k, \dot{x}_k, y_k, \dot{y}_k, z_k, \dot{z}_k]^\top, \quad u_k = [T_k, \phi_k, \theta_k]^\top, \nonumber \\
A_d &= \operatorname{diag}(A_1, A_1, A_1), \quad B_d = \operatorname{diag}(B_1, B_1, B_1) \nonumber \\
h_{\psi}(u_k) &= \begin{bmatrix} T_k(\cos \phi_k \sin \theta_k \cos \psi + \sin \phi_k \sin \psi) \\ T_k(\cos \phi_k \sin \theta_k \sin \psi - \sin \phi_k \cos \psi) \\ -g + T_k \cos \phi_k \cos \theta_k \end{bmatrix} \tag{2}
\end{align}$$

where, at time step $k$, $x_k \in \mathbb{R}^6$ denotes the system's with $x_k, y_k, z_k$ and $\dot{x}_k, \dot{y}_k, \dot{z}_k$ as the positions of the drone and their time derivative, respectively; $u_k \in \mathbb{R}^3$ gathers the three input including the normalized thrust $T_k$, the roll ($\phi_k$) and pitch ($\theta_k$) angles; the yaw angle $\psi \in [-\pi; \pi]$ is assumed to be known by sensor measurement. $t_s$ is the sampling time and $A_1 = \begin{bmatrix} 1 & t_s \\ 0 & 1 \end{bmatrix}$, $B_1 = \begin{bmatrix} 0.5t_s^2 & t_s \end{bmatrix}^\top$.

The inputs' constraints are defined as:

$$u_k \in \mathcal{U} = \{0 \le T_k \le T_{\max}; |\phi_k|, |\theta_k| \le \epsilon_{max}\} \quad (3)$$

where $g$ is the gravitational acceleration and $T_{\max} > 0$, $\epsilon_{max} \in (0; \pi/2)$ are constant bounds of the inputs.

Recall the variable reformulation (a.k.a. inverse kinematics) $u_k = \varphi_{\psi}(v_k)$ where:

$$\begin{align}
\varphi_{\psi}(v_k) &\triangleq [{}^1\varphi_{\psi}(v_k), {}^2\varphi_{\psi}(v_k), {}^3\varphi_{\psi}(v_k)]^\top \nonumber \\
&= \begin{bmatrix} \sin^{-1}\left(\frac{\sqrt{v_{1k}^2 + v_{2k}^2 + (v_{3k} + g)^2}}{\left(v_{1k} \sin \psi - v_{2k} \cos \psi\right)/\left({}^1\varphi_{\psi}(v_k)\right)}\right) \\ \tan^{-1}\left(\frac{\sqrt{v_{1k}^2 + v_{2k}^2 + (v_{3k} + g)^2}}{\left(v_{1k} \cos \psi + v_{2k} \sin \psi\right)/\left(v_{3k} + g\right)}\right) \end{bmatrix}, \tag{4}
\end{align}$$

the system (1) becomes:

$$x_{k+1} = A_d x_k + B_d v_k, \quad (5)$$

under the condition $v_{3k} \ge -g$, with $v_k \triangleq [v_{1k}, v_{2k}, v_{3k}]^\top$ gathering the new input. For a more detailed computation, we refer to the work [11] where both the flat representation and the linearizing law are derived.

**Remark 1:** It is important to note that the transformation (4) is deduced from the input-output relationship of the quadcopter's model, where the flat output is considered as $[x, y, z]^\top$ [9], [11]. Moreover, for this model, as a property of differentially flat systems, the system dynamics (5) describe three concatenated chains of integrators.

As discussed in our previous work [9], the feasible domain $\tilde{U} \triangleq \{v_k : \varphi_{\psi}(v_k) \in \mathcal{U}\}$ is non-convex and time varying (due to the dependence on $\psi$), which proves impractical for real-time control. To handle this problem, the convex time-invariant subset of the domain is adopted as:

$$\mathcal{V}_c = \left\{ \mathbf{v} = [v_1, v_2, v_3]^\top \in \mathbb{R}^3 : \begin{bmatrix} v_1^2 + v_2^2 + (v_3 + g)^2 - T_{\max}^2 \\ \sqrt{v_1^2 + v_2^2 - (v_3 + g) \tan \epsilon_{\max}} \end{bmatrix} \le 0 \right\}. \quad (6)$$

For brevity, the proof for the inclusion $\mathcal{V}_c \subset \tilde{U}, \forall \psi \in \mathbb{R}$ is omitted, the details can be found in [9].

**Remark 2:** It can be seen directly that, $\mathcal{V}_c$ contains the origin in its interior, thus, there exists $v_\epsilon > 0$ such that the set can be under-approximated by the saturation constraint: $|v| \le v_\epsilon$. Then, anti-saturation control can be designed via an anti-windup PID or nested saturation control [11], [12] with global asymptotic stability. However, as shown in [9], such norm-based approximation truncates significantly the original set, conservatively limiting the control admissible solution. Thus, subsequently, we exploit the set's geometric representation to construct a less conservative approximation.

From (6), $\mathcal{V}_c$ contains a sphere of radius $T_{\max}$ and a convex cone characterized by $\epsilon_{max}$. With this geometric interpretation and by parameterizing such bounding surfaces, an inner-approximation of $\mathcal{V}_c$ can be constructed as:

$$\mathcal{V}_c^{\text{in}} = \text{co} \left\{ \begin{array}{l} [0, 0, -g]^\top, [R^* \cos \alpha_i, R^* \sin \alpha_i, v_3^*]^\top, \\ [r_j \cos \alpha_i, r_j \sin \alpha_i, \sqrt{T_{\max}^2 - r_j^2 - g}]^\top \end{array} \right\} \quad (7)$$

with $\alpha_i \in \mathbb{N}^{s_1}(0, 2\pi)$, $r_j \in \mathbb{N}^{s_2}(0, R^*)$ for some integer $s_1, s_2 > 2$ (the larger $s_1$ and $s_2$, the more closely $\mathcal{V}_c^{\text{in}}$ approaches $\mathcal{V}_c$) and $R^* \triangleq T_{\max} \sin \epsilon_{max}$, $v_3^* \triangleq T_{\max} \cos \epsilon_{max} - g$. An illustration of the parameterization is shown in Fig. 1. Hereinafter, as opposed to $\mathcal{V}_c$, $\mathcal{V}_c^{\text{in}}$ will be employed to take advantage of its linear representation in the control problem, which is discussed later.

<figure><img src="image_1.png" /><figcaption>Fig. 1. $\mathcal{V}_c$ parameterization with $s_1 = 7, s_2 = 3, g = T_{\max}/1.45 = 9.81m/s^2, \epsilon_{max} = 0.1745 rad$.</figcaption></figure>

### B. Tube-MPC synthesis

Next, we summarize the MPC synthesis for a linear system with stability and feasibility guaranteed, which later serves as the ingredients for the tube-based MPC design.

**Proposition 1:** (Largest constrained ellipsoid [13]) Consider a symmetric matrix $P > 0$ and a polyhedron $\mathcal{H} = \{x \in \mathbb{R}^n : a_i^\top x \le b_i, i = 1, ..., p\}$, define an ellipsoid: $\mathcal{E} = \{x \in \mathbb{R}^n : \|x\|_P \le \epsilon\}$. Then, the maximum $\epsilon$ such that $\mathcal{E} \subset \mathcal{H}$ can be found by solving:

$$\epsilon^* = \arg\max_{\epsilon} \log \det \mathbf{B}(\epsilon), \quad (8)$$

$$\text{s.t } \mathbf{B}(\epsilon) = (\mathbf{V}\mathbf{S}^{1/2}\mathbf{U}^\top)^{-1}\epsilon \text{ and } \|\mathbf{B}(\epsilon)a_i\|_2 \le b_i.$$

where $\mathbf{V}, \mathbf{S}, \mathbf{U}$ come from the singular value decomposition of $\mathbf{P}$ with $\mathbf{P} = \mathbf{U}\mathbf{S}\mathbf{V}^\top$. $\square$

**Proposition 2:** (Stabilizing MPC with ellipsoidal terminal region [14]) Given the matrices $\mathbf{Q} > 0, \mathbf{R} \succeq 0$ and a constrained linear dynamics:

$$\tilde{x}_{k+1} = A_d \tilde{x}_k + B_d \tilde{v}_k \text{ subject to } \tilde{v}_k \in \mathcal{V}^*, \quad (9)$$

where $\tilde{x}_k \in \mathbb{R}^n, \tilde{v}_k \in \mathbb{R}^m$, and $\mathcal{V}^*$ is a polyhedral constraint set, consider the following procedure:
## II. MODEL REPRESENTATION AND MPC SETUP

First, we recapitulate the quadcopter model employed for position control design and its representation in the flat output space, which possesses linear dynamics and a non-linear constraint. The ellipsoidal terminal ingredient synthesis for constrained linear system is also presented.

### A. Quadcopter position control

The quadcopter's discretized translation dynamics:

$$x_{k+1} = A_d x_k + B_d h_{\psi}(u_k), \quad (1)$$

where the vectors and matrices are defined as:

$$\begin{align}
x_k &= [x_k, \dot{x}_k, y_k, \dot{y}_k, z_k, \dot{z}_k]^\top, \quad u_k = [T_k, \phi_k, \theta_k]^\top, \nonumber \\
A_d &= \operatorname{diag}(A_1, A_1, A_1), \quad B_d = \operatorname{diag}(B_1, B_1, B_1) \nonumber \\
h_{\psi}(u_k) &= \begin{bmatrix}
T_k(\cos \phi_k \sin \theta_k \cos \psi + \sin \phi_k \sin \psi) \\
T_k(\cos \phi_k \sin \theta_k \sin \psi - \sin \phi_k \cos \psi) \\
-g + T_k \cos \phi_k \cos \theta_k
\end{bmatrix} \tag{2}
\end{align}$$

where, at time step $k$, $x_k \in \mathbb{R}^6$ denotes the system's with $x_k, y_k, z_k$ and $\dot{x}_k, \dot{y}_k, \dot{z}_k$ as the positions of the drone and their time derivative, respectively; $u_k \in \mathbb{R}^3$ gathers the three input including the normalized thrust $T_k$, the roll ($\phi_k$) and pitch ($\theta_k$) angles; the yaw angle $\psi \in [-\pi; \pi]$ is assumed to be known by sensor measurement. $t_s$ is the sampling time and $A_1 = \begin{bmatrix} 1 & t_s \\ 0 & 1 \end{bmatrix}$, $B_1 = \begin{bmatrix} 0.5t_s^2 & t_s \end{bmatrix}^\top$.

The inputs' constraints are defined as:

$$u_k \in \mathcal{U} = \{0 \le T_k \le T_{\max}; |\phi_k|, |\theta_k| \le \epsilon_{max}\} \quad (3)$$

where $g$ is the gravitational acceleration and $T_{\max} > 0$, $\epsilon_{max} \in (0; \pi/2)$ are constant bounds of the inputs.

Recall the variable reformulation (a.k.a. inverse kinematics) $u_k = \varphi_\psi(v_k)$ where:

$$\begin{align}
\varphi_\psi(v_k) &\triangleq [{}^1\varphi_\psi(v_k), {}^2\varphi_\psi(v_k), {}^3\varphi_\psi(v_k)]^\top \nonumber \\
&= \begin{bmatrix}
\sin^{-1}\left(\frac{\sqrt{v_{1k}^2 + v_{2k}^2 + (v_{3k} + g)^2}}{\left(v_{1k} \sin \psi - v_{2k} \cos \psi\right)/\left({}^1\varphi_\psi(v_k)\right)}\right) \\
\tan^{-1}\left(\frac{(v_{1k} \cos \psi + v_{2k} \sin \psi)}{(v_{3k} + g)}\right)
\end{bmatrix}, \tag{4}
\end{align}$$

the system (1) becomes:

$$x_{k+1} = A_d x_k + B_d v_k, \quad (5)$$

under the condition $v_{3k} \ge -g$, with $v_k \triangleq [v_{1k}, v_{2k}, v_{3k}]^\top$ gathering the new input. For a more detailed computation, we refer to the work [11] where both the flat representation and the linearizing law are derived.

**Remark 1:** It is important to note that the transformation (4) is deduced from the input-output relationship of the quadcopter's model, where the flat output is considered as $[x, y, z]^\top$ [9], [11]. Moreover, for this model, as a property of differentially flat systems, the system dynamics (5) describe three concatenated chains of integrators.

As discussed in our previous work [9], the feasible domain $\tilde{U} \triangleq \{v_k : \varphi_\psi(v_k) \in \mathcal{U}\}$ is non-convex and time varying (due to the dependence on $\psi$), which proves impractical for real-time control. To handle this problem, the convex time-invariant subset of the domain is adopted as:

$$\mathcal{V}_c = \left\{ \mathbf{v} = [v_1, v_2, v_3]^\top \in \mathbb{R}^3 : \begin{bmatrix} v_1^2 + v_2^2 + (v_3 + g)^2 - T_{\max}^2 \\ \sqrt{v_1^2 + v_2^2 - (v_3 + g) \tan \epsilon_{\max}} \end{bmatrix} \le 0 \right\}. \quad (6)$$

For brevity, the proof for the inclusion $\mathcal{V}_c \subset \tilde{U}, \forall \psi \in \mathbb{R}$ is omitted, the details can be found in [9].

**Remark 2:** It can be seen directly that, $\mathcal{V}_c$ contains the origin in its interior, thus, there exists $v_\epsilon > 0$ such that the set can be under-approximated by the saturation constraint: $|v| \le v_\epsilon$. Then, anti-saturation control can be designed via an anti-windup PID or nested saturation control [11], [12] with global asymptotic stability. However, as shown in [9], such norm-based approximation truncates significantly the original set, conservatively limiting the control admissible solution. Thus, subsequently, we exploit the set's geometric representation to construct a less conservative approximation.

From (6), $\mathcal{V}_c$ contains a sphere of radius $T_{\max}$ and a convex cone characterized by $\epsilon_{max}$. With this geometric interpretation and by parameterizing such bounding surfaces, an inner-approximation of $\mathcal{V}_c$ can be constructed as:

$$\mathcal{V}_c^{\text{in}} = \text{co} \left\{ \begin{array}{l} [0, 0, -g]^\top, [R^* \cos \alpha_i, R^* \sin \alpha_i, v_3^*]^\top, \\ [r_j \cos \alpha_i, r_j \sin \alpha_i, \sqrt{T_{\max}^2 - r_j^2 - g}]^\top \end{array} \right\} \quad (7)$$

with $\alpha_i \in \mathbb{N}^{s_1}(0, 2\pi)$, $r_j \in \mathbb{N}^{s_2}(0, R^*)$ for some integer $s_1, s_2 > 2$ (the larger $s_1$ and $s_2$, the more closely $\mathcal{V}_c^{\text{in}}$ approaches $\mathcal{V}_c$) and $R^* \triangleq T_{\max} \sin \epsilon_{max}$, $v_3^* \triangleq T_{\max} \cos \epsilon_{max} - g$. An illustration of the parameterization is shown in Fig. 1. Hereinafter, as opposed to $\mathcal{V}_c$, $\mathcal{V}_c^{\text{in}}$ will be employed to take advantage of its linear representation in the control problem, which is discussed later.

<figure><img src="image_3.png" /><figcaption>Fig. 1. $\mathcal{V}_c$ parameterization with $s_1 = 7, s_2 = 3, g = T_{\max}/1.45 = 9.81m/s^2, \epsilon_{max} = 0.1745 rad$.</figcaption></figure>

### B. Tube-MPC synthesis

Next, we summarize the MPC synthesis for a linear system with stability and feasibility guaranteed, which later serves as the ingredients for the tube-based MPC design.

**Proposition 1:** (Largest constrained ellipsoid [13]) Consider a symmetric matrix $P > 0$ and a polyhedron $\mathcal{H} = \{x \in \mathbb{R}^n : a_i^\top x \le b_i, i = 1, ..., p\}$, define an ellipsoid: $\mathcal{E} = \{x \in \mathbb{R}^n : \|x\|_P \le \epsilon\}$. Then, the maximum $\epsilon$ such that $\mathcal{E} \subset \mathcal{H}$ can be found by solving:

$$\epsilon^* = \arg\max_{\epsilon} \log \det \mathbf{B}(\epsilon), \quad (8)$$

$$\text{s.t } \mathbf{B}(\epsilon) = (\mathbf{V}\mathbf{S}^{1/2}\mathbf{U}^\top)^{-1}\epsilon \text{ and } \|\mathbf{B}(\epsilon)\|_2 \le b_i.$$

where $\mathbf{V}, \mathbf{S}, \mathbf{U}$ come from the singular value decomposition of $\mathbf{P}$ with $\mathbf{P} = \mathbf{U}\mathbf{S}\mathbf{V}^\top$. $\square$

**Proposition 2:** (Stabilizing MPC with ellipsoidal terminal region [14]) Given the matrices $\mathbf{Q} > 0, \mathbf{R} \succeq 0$ and a constrained linear dynamics:

$$\tilde{x}_{k+1} = A_d \tilde{x}_k + B_d \tilde{v}_k \text{ subject to } \tilde{v}_k \in \mathcal{V}^*, \quad (9)$$

where $\tilde{x}_k \in \mathbb{R}^n, \tilde{v}_k \in \mathbb{R}^m$, and $\mathcal{V}^*$ is a polyhedral constraint set, consider the following procedure:
• Choose a feedback gain $K$ such that $A_{cl} \triangleq A_d+B_dK$ is a Schur matrix;

• Choose $M$ such that

$$M \geq Q + \lambda_{max}(R)K^{\top}K; \quad (10)$$

• Solve the Lyapunov equation (11) to find $P$:

$$A_{cl}^{\top}PA_{cl} - P + M = 0; \quad (11)$$

• Following Proposition 1 to find the largest ellipsoid:

$$\mathcal{E}_f = \{ \tilde{\mathbf{x}} : \tilde{\mathbf{x}}^{\top} \mathbf{P} \tilde{\mathbf{x}} \le \varepsilon_f \}, \quad (12)$$

such that $\mathcal{E}_f \subset \tilde{\mathcal{X}} \triangleq \{\tilde{\mathbf{x}} : K\tilde{\mathbf{x}} \in \mathcal{V}^*\}$.

Then, we have:

1) The control law $\tilde{v}_k = K\tilde{x}_k \in \mathcal{V}^*$, $\forall \tilde{x}_k \in \mathcal{E}_f$ and also renders the ellipsoid $\mathcal{E}_f$ forward invariant for the system (9) (i.e, $\tilde{x}_k \in \mathcal{E}_f \Rightarrow \tilde{x}_{k+1} \in \mathcal{E}_f, k \ge 0$ [15]);

2) $V_f(\tilde{x}_{k+1}) + V_s(\tilde{x}_k, K\tilde{x}_k) - V_f(\tilde{x}_k) \le 0 \quad \forall \tilde{x}_k \in \mathcal{E}_f$ with $$ \begin{cases} V_f(\tilde{x}_k) \triangleq \|\tilde{x}_k\|_P^2 \\ V_s(\tilde{x}_k, \tilde{v}_k) \triangleq \|\tilde{x}_k\|_Q^2 + \|\tilde{v}_k\|_R^2 \end{cases} \quad (13) $$

*Proof:* From the definition (12), $\tilde{v}_k = K\tilde{x}_k \in \mathcal{V}^*$, $\forall \tilde{x}_k \in \mathcal{E}_f$ always holds. Next, with condition (11), we have:

$$ \begin{aligned} \|\tilde{x}_{k+1}\|_P^2 &= \tilde{x}_k^\top A_{cl}^\top P A_{cl} \tilde{x}_k = \tilde{x}_k^\top (P - M) \tilde{x}_k \quad (14) \\ &\le (1 - \lambda_{min}(M)/\lambda_{max}(P)) \tilde{x}_k^\top P \tilde{x}_k \le \tilde{x}_k^\top P \tilde{x}_k \end{aligned} $$

Then, if $\tilde{x}_k \in \mathcal{E}_f$, $\|\tilde{x}_{k+1}\|_P^2 \le \|\tilde{x}_k\|_P^2 \le \varepsilon_f$, or $\tilde{x}_{k+1} \in \mathcal{E}_f$, namely $\mathcal{E}_f$ is forward invariant for the closed-loop.

Finally, with the conditions in (10) and (11), we have:

$$ \begin{aligned} V_f(\tilde{x}_{k+1}) + V_s(\tilde{x}_k, K\tilde{x}_k) - V_f(\tilde{x}_k) &= \tilde{x}_k^\top A_{cl}^\top P A_{cl} \tilde{x}_k + \tilde{x}_k^\top (Q + K^\top R K) \tilde{x}_k - \tilde{x}_k^\top P \tilde{x}_k \\ &= -\tilde{x}_k^\top (M - Q - K^\top R K) \tilde{x}_k \le 0, \forall \tilde{x}_k \in \mathbb{R}^n. \end{aligned} \quad (15) $$

**Corollary 1:** From the settings of Proposition 2, consider the standard optimization problem at time step $k$ [3]:

$$ \begin{aligned} V_{N_p}(\tilde{x}_k) &\triangleq \min \sum_{i=0}^{N_p-1} V_s(\tilde{x}_{k+i}, \tilde{v}_{k+i}) + V_f(\tilde{x}_{k+N_p}) \quad (16) \\ \text{s.t} \quad & \{ \tilde{x}_{i+k+1} = A_d \tilde{x}_{i+k} + B_d \tilde{v}_{i+k}; \tilde{x}_{k+N_p} \in \mathcal{E}_f \quad (17a) \\ & \tilde{v}_{i+k} \in \mathcal{V}^*, i \in \{0, 1, \dots, N_p - 1\}, \quad (17b) \end{aligned} $$

where $N_p$ denotes the prediction horizon, $\mathcal{E}_f$ is the terminal region; the polyhedron $\tilde{\mathcal{V}}$ describes the input constraints; $Q, P \succ 0, R \succeq 0$ are weighting matrices. Then, by finding $P$ and $\varepsilon_f$ as in Proposition 2, the problem (16)-(17) is recursively feasible, and the origin $0_{n \times 1}$ is asymptotically stable under the closed-loop system given by repetitively applying the first control action found in (16)-(17).

*Proof:* The properties achieved in Proposition 2 satisfy the conditions for feasibility and stability presented in Section 3.3 in [3] for a standard MPC scheme. ■

**Remark 3:** The existence of $\mathcal{E}_f$ in (12) simply requires the set $\tilde{\mathcal{X}}$ to include the origin as its interior point. In such case, there always exists a sufficiently small value of $\varepsilon_f$ so that $\mathcal{E}_f$ is inscribed in $\tilde{\mathcal{X}}$.

### III. TUBE MPC FORMULATION FOR A QUADCOPTER UNDER DISTURBANCES

In this part, we establish and implement the robust MPC strategy for the quadcopter's tracking error dynamics by stabilizing the tracking error dynamics with robust MPC.

#### A. Trajectory tracking error dynamics

First, consider the disturbed quadcopter model (1):

$$x_{k+1} = A_d x_k + B_d h_{\psi}(u_k) + w, \quad (18)$$

where $w \in \mathbb{R}^6$ denotes the system's disturbance which is assumed to be bounded, i.e.: $w \in \mathcal{W}$. Then, by using the mapping (4), the system (18) becomes:

$$x_{k+1} = A_d x_k + B_d v_k + w. \quad (19)$$

Next, we assume to have a well-defined reference trajectory of the quadcopter with all constraints satisfied, namely:

$$\left\{ \begin{array}{l} x_{k+1}^{\text{ref}} = A_d x_k^{\text{ref}} + B_d v_k^{\text{ref}}, \\ v_k^{\text{ref}} \in \mathcal{V}_c^{\text{ref}} \subset \mathcal{V}_c^{\text{in}} \text{ as in (7)}, \end{array} \right. \quad (20a)$$

$$\left\{ \begin{array}{l} v_k^{\text{ref}} \in \mathcal{V}_c^{\text{ref}} \subset \mathcal{V}_c^{\text{in}} \text{ as in (7)}, \\ \tilde{v}_k \in \tilde{\mathcal{V}}, \end{array} \right. \quad (20b)$$

where $\mathcal{V}_c^{\text{ref}}$ represents the polytope containing the input reference sequence $v_k^{\text{ref}}$. This set $\mathcal{V}_c^{\text{ref}}$ is assumed to be included by the constraint set $\mathcal{V}_c^{\text{in}}$ of $v_k$. Note that the dynamics expressed in (20a) simply represent the trajectory of three independent double integrators. Hence, effective time parameterization tools can be used to generate the reference signal satisfying the constraints (20b) [11]. Subsequently, from (20) and (19), we achieve the tracking error dynamics:

$$ \left\{ \begin{array}{l} \tilde{x}_{k+1} = A_d \tilde{x}_k + B_d \tilde{v}_k + w \quad (21a) \\ \text{s.t. } \tilde{v}_k \in \tilde{\mathcal{V}}, \quad (21b) \end{array} \right. $$

where $\tilde{x}_k \triangleq x_k - x_k^{\text{ref}}$ denotes the deviation of the state $x_k$ with respect to the reference $x_k^{\text{ref}}$; likewise, $\tilde{v}_k \triangleq v_k - v_k^{\text{ref}}$ denotes the control action employed to compensate such deviation and disturbances while $\tilde{\mathcal{V}} \triangleq \mathcal{V}_c^{\text{in}} \ominus \mathcal{V}_c^{\text{ref}}$ is the corresponding constraint set for the input $\tilde{v}_k$ of system (21a).

Prior to this point, the trajectory tracking problem has been reduced to the stabilization of the linear disturbed system (21a) towards the origin $\tilde{x}_e = \mathbf{0}_{6 \times 1}, \tilde{v}_e = \mathbf{0}_{3 \times 1}$. Therefore, in the next part, we synthesize the robust MPC [16] subject to the bounded disturbance $w$.

#### B. Robust model predictive control

As introduced above, let us formulate the robust MPC presented in [16] for the constrained and disturbed system (21). The online optimization at step $k$ is defined as follows:

$$V_{N_p}(\tilde{x}_k) \triangleq \min \sum_{i=0}^{N_p-1} V_s(\tilde{x}_{k+i}^*, \tilde{v}_{k+i}^*) + V_f(\tilde{x}_{k+N_p}^*) \quad (22)$$

$$ \begin{aligned} \text{s.t} \quad & \tilde{x}_{i+k+1}^* = A_d \tilde{x}_{i+k}^* + B_d \tilde{v}_{i+k}^*; \quad (23a) \\ & \tilde{x}_k^* \in \tilde{x}_k \oplus \mathcal{Z}; \tilde{x}_{N_p}^* \in \mathcal{E}_f; \quad (23b) \\ & \tilde{v}_{i+k}^* \in \mathcal{V}^* \triangleq \tilde{\mathcal{V}} \ominus K\mathcal{Z}, i \in \{0, 1, \dots, N_p - 1\}, \quad (23c) \end{aligned} $$

where $A_d, B_d$ is defined as in the model's parameters (2); $V_s(\cdot), V_f(\cdot)$ are the quadratic cost given in (13) with given weighting $Q \succ 0, R \succeq 0$ of choice; $N_p$ is the prediction horizon; $\tilde{x}_{i+k}^*$ and $\tilde{v}_{i+k}^*$ denote the predicted state and control action at time step $i+k$, respectively, for the nominal version of system (19) (i.e., without the effect of the disturbance $w$). The remaining ingredients in the constraints (23) are defined as follows. Firstly, given a state feedback gain $K$ that renders the closed-loop matrix $A_{cl} = A_d + B_dK$ a Schur matrix, then $\mathcal{Z}$ is a robust positively invariant (RPI) set [17], [18] for the autonomous system: $\tilde{x}_{k+1} = A_{cl}\tilde{x}_k + w$. In this setting, we employ the outer-approximation of the minimal RPI proposed in [17]. Then, the set $\mathcal{Z}$ plays a role
• Choose a feedback gain $K$ such that $A_{cl} \triangleq A_d+B_dK$ is a Schur matrix;

• Choose $M$ such that

$$M \geq Q + \lambda_{max}(R)K^{\top}K; \quad (10)$$

• Solve the Lyapunov equation (11) to find $P$:

$$A_{cl}^{\top}PA_{cl} - P + M = 0; \quad (11)$$

• Following Proposition 1 to find the largest ellipsoid:

$$\mathcal{E}_f = \{ \tilde{\mathbf{x}} : \tilde{\mathbf{x}}^{\top} \mathbf{P} \tilde{\mathbf{x}} \le \varepsilon_f \}, \quad (12)$$

such that $\mathcal{E}_f \subset \tilde{\mathcal{X}} \triangleq \{\tilde{\mathbf{x}} : K\tilde{\mathbf{x}} \in \mathcal{V}^*\}$.

Then, we have:

1) The control law $\tilde{v}_k = K\tilde{\mathbf{x}}_k \in \mathcal{V}^*$, $\forall \tilde{\mathbf{x}}_k \in \mathcal{E}_f$ and also renders the ellipsoid $\mathcal{E}_f$ forward invariant for the system (9) (i.e, $\tilde{\mathbf{x}}_k \in \mathcal{E}_f \Rightarrow \tilde{\mathbf{x}}_{k+1} \in \mathcal{E}_f, k \ge 0$ [15]);

2) $V_f(\tilde{\mathbf{x}}_{k+1}) + V_s(\tilde{\mathbf{x}}_k, K\tilde{\mathbf{x}}_k) - V_f(\tilde{\mathbf{x}}_k) \le 0 \quad \forall \tilde{\mathbf{x}}_k \in \mathcal{E}_f$ with $\begin{cases} V_f(\tilde{\mathbf{x}}_k) \triangleq \|\tilde{\mathbf{x}}_k\|_P^2 \\ V_s(\tilde{\mathbf{x}}_k, \tilde{v}_k) \triangleq \|\tilde{\mathbf{x}}_k\|_Q^2 + \|\tilde{v}_k\|_R^2 \end{cases}$ (13) $\square$

*Proof:* From the definition (12), $\tilde{v}_k = K\tilde{\mathbf{x}}_k \in \mathcal{V}^*$, $\forall \tilde{\mathbf{x}}_k \in \mathcal{E}_f$ always holds. Next, with condition (11), we have:

$$\begin{aligned} \|\tilde{\mathbf{x}}_{k+1}\|_P^2 &= \tilde{\mathbf{x}}_k^\top A_{cl}^\top P A_{cl} \tilde{\mathbf{x}}_k = \tilde{\mathbf{x}}_k^\top (P - M) \tilde{\mathbf{x}}_k \quad (14) \\ &\le (1 - \lambda_{min}(M)/\lambda_{max}(P)) \tilde{\mathbf{x}}_k^\top P \tilde{\mathbf{x}}_k \le \tilde{\mathbf{x}}_k^\top P \tilde{\mathbf{x}}_k \end{aligned}$$

Then, if $\tilde{\mathbf{x}}_k \in \mathcal{E}_f$, $\|\tilde{\mathbf{x}}_{k+1}\|_P^2 \le \|\tilde{\mathbf{x}}_k\|_P^2 \le \varepsilon_f$, or $\tilde{\mathbf{x}}_{k+1} \in \mathcal{E}_f$, namely $\mathcal{E}_f$ is forward invariant for the closed-loop.

Finally, with the conditions in (10) and (11), we have:

$$\begin{aligned} V_f(\tilde{\mathbf{x}}_{k+1}) + V_s(\tilde{\mathbf{x}}_k, K\tilde{\mathbf{x}}_k) - V_f(\tilde{\mathbf{x}}_k) \\ = \tilde{\mathbf{x}}_k^\top A_{cl}^\top P A_{cl} \tilde{\mathbf{x}}_k + \tilde{\mathbf{x}}_k^\top (Q + K^\top R K) \tilde{\mathbf{x}}_k - \tilde{\mathbf{x}}_k^\top P \tilde{\mathbf{x}}_k \\ = -\tilde{\mathbf{x}}_k^\top (M - Q - K^\top R K) \tilde{\mathbf{x}}_k \le 0, \forall \tilde{\mathbf{x}}_k \in \mathbb{R}^n. \end{aligned} \quad (15)$$

*Corollary 1:* From the settings of Proposition 2, consider the standard optimization problem at time step $k$ [3]:

$$\begin{aligned} V_{N_p}(\tilde{\mathbf{x}}_k) &\triangleq \min \sum_{i=0}^{N_p-1} V_s(\tilde{\mathbf{x}}_{k+i}, \tilde{v}_{k+i}) + V_f(\tilde{\mathbf{x}}_{k+N_p}) \quad (16) \\ \text{s.t} \quad &\{ \tilde{\mathbf{x}}_{i+k+1} = A_d \tilde{\mathbf{x}}_{i+k} + B_d \tilde{v}_{i+k}; \tilde{\mathbf{x}}_{k+N_p} \in \mathcal{E}_f \quad (17a) \\ &\tilde{v}_{i+k} \in \mathcal{V}^*, i \in \{0, 1, \dots, N_p - 1\}, \quad (17b) \end{aligned}$$

where $N_p$ denotes the prediction horizon, $\mathcal{E}_f$ is the terminal region; the polyhedron $\tilde{\mathcal{V}}$ describes the input constraints; $Q, P \succ 0, R \succeq 0$ are weighting matrices. Then, by finding $P$ and $\varepsilon_f$ as in Proposition 2, the problem (16)-(17) is recursively feasible, and the origin $0_{n \times 1}$ is asymptotically stable under the closed-loop system given by repetitively applying the first control action found in (16)-(17).

*Proof:* The properties achieved in Proposition 2 satisfy the conditions for feasibility and stability presented in Section 3.3 in [3] for a standard MPC scheme. ■

*Remark 3:* The existence of $\mathcal{E}_f$ in (12) simply requires the set $\tilde{\mathcal{X}}$ to include the origin as its interior point. In such case, there always exists a sufficiently small value of $\varepsilon_f$ so that $\mathcal{E}_f$ is inscribed in $\tilde{\mathcal{X}}$.

### III. TUBE MPC FORMULATION FOR A QUADCOPTER UNDER DISTURBANCES

In this part, we establish and implement the robust MPC strategy for the quadcopter's tracking error dynamics by stabilizing the tracking error dynamics with robust MPC.

#### A. Trajectory tracking error dynamics

First, consider the disturbed quadcopter model (1):

$$x_{k+1} = A_d x_k + B_d h_{\psi}(u_k) + w, \quad (18)$$

where $w \in \mathbb{R}^6$ denotes the system's disturbance which is assumed to be bounded, i.e.: $w \in \mathcal{W}$. Then, by using the mapping (4), the system (18) becomes:

$$x_{k+1} = A_d x_k + B_d v_k + w. \quad (19)$$

Next, we assume to have a well-defined reference trajectory of the quadcopter with all constraints satisfied, namely:

$$\left\{ \begin{array}{l} x_{k+1}^{\text{ref}} = A_d x_k^{\text{ref}} + B_d v_k^{\text{ref}}, \\ v_k^{\text{ref}} \in \mathcal{V}_c^{\text{ref}} \subset \mathcal{V}_c^{\text{in}} \text{ as in (7)}, \end{array} \right. \quad (20a)$$

$$v_k^{\text{ref}} \in \mathcal{V}_c^{\text{ref}} \subset \mathcal{V}_c^{\text{in}} \text{ as in (7)}, \quad (20b)$$

where $\mathcal{V}_c^{\text{ref}}$ represents the polytope containing the input reference sequence $v_k^{\text{ref}}$. This set $\mathcal{V}_c^{\text{ref}}$ is assumed to be included by the constraint set $\mathcal{V}_c^{\text{in}}$ of $v_k$. Note that the dynamics expressed in (20a) simply represent the trajectory of three independent double integrators. Hence, effective time parameterization tools can be used to generate the reference signal satisfying the constraints (20b) [11]. Subsequently, from (20) and (19), we achieve the tracking error dynamics:

$$\left\{ \begin{array}{l} \tilde{x}_{k+1} = A_d \tilde{x}_k + B_d \tilde{v}_k + w \quad (21a) \\ \text{s.t. } \tilde{v}_k \in \tilde{\mathcal{V}}, \quad (21b) \end{array} \right.$$

where $\tilde{x}_k \triangleq x_k - x_k^{\text{ref}}$ denotes the deviation of the state $x_k$ with respect to the reference $x_k^{\text{ref}}$; likewise, $\tilde{v}_k \triangleq v_k - v_k^{\text{ref}}$ denotes the control action employed to compensate such deviation and disturbances while $\tilde{\mathcal{V}} \triangleq \mathcal{V}_c^{\text{in}} \ominus \mathcal{V}_c^{\text{ref}}$ is the corresponding constraint set for the input $\tilde{v}_k$ of system (21a).

Prior to this point, the trajectory tracking problem has been reduced to the stabilization of the linear disturbed system (21a) towards the origin $\tilde{x}_e = 0_{6 \times 1}, \tilde{v}_e = 0_{3 \times 1}$. Therefore, in the next part, we synthesize the robust MPC [16] subject to the bounded disturbance $w$.

#### B. Robust model predictive control

As introduced above, let us formulate the robust MPC presented in [16] for the constrained and disturbed system (21). The online optimization at step $k$ is defined as follows:

$$V_{N_p}(\tilde{\mathbf{x}}_k) \triangleq \min \sum_{i=0}^{N_p-1} V_s(\tilde{\mathbf{x}}_{k+i}, \tilde{v}_{k+i}^*) + V_f(\tilde{\mathbf{x}}_{k+N_p}^*) \quad (22)$$

$$\begin{aligned} \text{s.t} \quad & \tilde{\mathbf{x}}_{i+k+1}^* = A_d \tilde{\mathbf{x}}_{i+k}^* + B_d \tilde{v}_{i+k}^*; \quad (23a) \\ & \tilde{\mathbf{x}}_k^* \in \tilde{\mathbf{x}}_k \oplus \mathcal{Z}; \tilde{\mathbf{x}}_{N_p}^* \in \mathcal{E}_f; \quad (23b) \\ & \tilde{v}_{i+k}^* \in \mathcal{V}^* \triangleq \tilde{\mathcal{V}} \ominus K\mathcal{Z}, i \in \{0, 1, \dots, N_p - 1\}, \quad (23c) \end{aligned}$$

where $A_d, B_d$ is defined as in the model's parameters (2); $V_s(\cdot), V_f(\cdot)$ are the quadratic cost given in (13) with given weighting $Q \succ 0, R \succeq 0$ of choice; $N_p$ is the prediction horizon; $\tilde{\mathbf{x}}_{i+k}^*$ and $\tilde{v}_{i+k}^*$ denote the predicted state and control action at time step $i+k$, respectively, for the nominal version of system (19) (i.e., without the effect of the disturbance $w$). The remaining ingredients in the constraints (23) are defined as follows. Firstly, given a state feedback gain $K$ that renders the closed-loop matrix $A_{cl} = A_d + B_d K$ a Schur matrix, then $\mathcal{Z}$ is a robust positively invariant (RPI) set [17], [18] for the autonomous system: $\tilde{\mathbf{x}}_{k+1} = A_{cl} \tilde{\mathbf{x}}_k + w$. In this setting, we employ the outer-approximation of the minimal RPI proposed in [17]. Then, the set $\mathcal{Z}$ plays a role
as the “origin” for our stabilizing problem [16]. Next, the terminal ingredients (P and the set $\mathcal{E}_f$) for the nominal MPC settings are found exactly with the Proposition 2 with the gain K chosen as aforementioned, the dynamics (23a) and the constraint set $\mathcal{V}^* \triangleq \tilde{\mathcal{V}} \ominus K\mathcal{Z}$. It is also assumed that the disturbance’s bound $\mathcal{W}$ is sufficiently manageable and hence results in non-empty feasible sets for the constraints in (23).

In short, the offline synthesis can be summarized as:

1) Choose $Q > 0$ and $R \ge 0$ to formulate the stage and terminal cost value function of the form (13);

2) Choose the gain K stabilizing the pair $(A_d, B_d)$;

3) Compute the mRPI set $\mathcal{Z}$ as proposed in [17];

4) Compute the terminal ingredients subject to the constraint $\mathcal{V}^* \triangleq \tilde{\mathcal{V}} \ominus K\mathcal{Z}$ as in Proposition 2.

**Remark 4:** To avoid computational issues caused by the system’s large dimensionality, we do not employ the computation of the maximal positively invariant set [19] for the closed-loop $\tilde{x}_{k+1} = A_{cl}\tilde{x}_k$ to play the role of the terminal region $\mathcal{E}_f$. More specifically, although proven finite, the number of equalities portraying such a set for this 6 dimensional problem may exceed the memory capacity provided for the computation software. Yet, if the polyhedral set can be computed, the terminal constraints $\mathcal{E}_f$ will become linear, as opposed to being quadratic as in (12), and reduce the complexity of the online problem (22)-(23).

Nevertheless, with these setups, the control input applied to the system (21a) is defined as:

$$ \tilde{v}_k = \tilde{v}_{N_p}^{\text{mpc}}(\tilde{x}_k) \triangleq \tilde{v}_k^* + K(\tilde{x}_k - \tilde{x}_k^*). \quad (24) $$

Consequently, with this scheme, as proven in [16], the set $\mathcal{Z}$ is robustly exponentially stable for the closed-loop:

$$ \tilde{x}_{k+1} = A_d \tilde{x}_k + B_d \tilde{v}_{N_p}^{\text{mpc}}(\tilde{x}_k) + w. \quad (25) $$

Namely, $\tilde{x}_k$ is maintained inside the “tube” $\tilde{x}_k^* \oplus \mathcal{Z}$ which converges to $\mathcal{Z}$ when $k \to \infty$. In other words, one can state:

$$ x_k - x_k^{\text{ref}} = \tilde{x}_k \in \tilde{x}_k^* \oplus \mathcal{Z} \Leftrightarrow x_k \in x_k^{\text{ref}} \oplus \tilde{x}_k^* \oplus \mathcal{Z} \quad (26) $$

with $\tilde{x}_k^*$ exponentially converges to $0_{6 \times 1}$. With the sequence of the reference for $x_k$ and $v_k$ denoted as $x^{\text{ref}}, v^{\text{ref}}$, respectively, the control implementation is given by Algorithm 1.

<table><caption>**Algorithm 1:** Robust MPC for trajectory tracking</caption><tr><td colspan="2"><b>Input:</b> Reference sequence $x_k^{\text{ref}}, v_k^{\text{ref}}$; the optimization setup as in (22)-(23).</td></tr><tr><td>1</td><td>for ($x_k^{\text{ref}}, v_k^{\text{ref}}$) in $x_k^{\text{ref}} \times v_k^{\text{ref}}$ do</td></tr><tr><td>2</td><td>&nbsp;&nbsp;&nbsp;&nbsp;Measure $x_k$ and $\psi$ at time step $k$;</td></tr><tr><td>3</td><td>&nbsp;&nbsp;&nbsp;&nbsp;Compute $\tilde{x}_k^*$ and $\tilde{v}_k^*$ by solving (22)-(23);</td></tr><tr><td>4</td><td>&nbsp;&nbsp;&nbsp;&nbsp;$\tilde{v}_{N_p}^{\text{mpc}} \leftarrow \tilde{v}_k^* + K(\tilde{x}_k - \tilde{x}_k^*)$ as in (24);</td></tr><tr><td>5</td><td>&nbsp;&nbsp;&nbsp;&nbsp;$u_k \leftarrow \varphi_{\psi}(v_k^{\text{ref}} + \tilde{v}_{N_p}^{\text{mpc}})$ as in (4);</td></tr><tr><td>6</td><td>&nbsp;&nbsp;&nbsp;&nbsp;Apply $u_k$ to the vehicle;</td></tr><tr><td>7</td><td>end</td></tr></table>

## IV. EXPERIMENTAL VALIDATION

The experiments are done over the Crazyflie 2.1 quadcopter platform where the drone’s inputs are the desired thrust, roll and pitch angles, gathered in the vector $u_k$, together with the desired yaw angle, assigned to zero.

<figure><img src="image_2.png" /><figcaption>Fig. 2. Two references employed for the experimental validation.</figcaption></figure>

Furthermore, the position and velocity of the quadcopter are acquired via a system of 8 Qualisys motion capture cameras. Note that the platform expects the normalized thrust $T(m/s^2)$ to be converted into the quadcopter’s unit which varies from 0 to 65535. Then, these signals will be tracked via a built-in inner-loop containing two cascade PID and a transformation from the required torque to PWM signals designed for the drone’s X-shape configuration [20].

### A. Parameter setup

Firstly, the reference is required to be at least two time differentiable to guarantee the continuity and boundedness of $v^{\text{ref}}$ as in (20b). Herein, two references are used as follows:

* Ref. 1: The position of the drone will be parameterized in time with the B-spline method [11]. More specifically, at timestamp $t_r(s)$, we imposed the position of the drone to pass through a waypoint $p_r$ as:

$$ p_r^\top \in \left\{ \begin{array}{l} [6, 0, 3]; [6, 3, 5]; [6, 6, 10]; \\ [0, 6, 10]; [-6, 6, 10]; [-6, -6, 10]; \\ [6, -6, 10]; [6, 0, 10]; [6, 6, 10] \end{array} \right\} \times 10(\text{cm}), \quad (27) $$

with $t_r \in \mathbb{N}^9(0, 45)$ (s). Then, by minimizing the curve’s length and restraining their second derivatives (which is the mathematical interpretation of $v_k^{\text{ref}}$) inside $\mathcal{V}_c^{\text{in}}$, we obtain the green solid curve depicted in Fig. 2.

* Ref. 2: The circular motion is parameterized as:

$$ \begin{aligned} x^{\text{ref}}(t) &= 0.6 \cos \omega t, & y^{\text{ref}}(t) &= \sin 0.6\omega t, \\ z(t) &= 0.5(\text{m}), & \omega &= 0.2\pi \text{ rad/s}, \end{aligned} \quad (28) $$

and illustrated as the purple curve in Fig. 2. Note that at the beginning and the end of the reference, we introduce smooth curves to reduce the overshoot effect caused by introducing a sudden reference.

Consequently, $\mathcal{V}_c^{\text{ref}}$ as in (20b) can be obtained by bounding the $v^{\text{ref}}$ with a box-type constraint (See Fig. 3).

The parameter of constraint set $\mathcal{U}$ as in (3) is provided in Table I while the corresponding constraint $\mathcal{V}_c$ is approximated by $\mathcal{V}_c^{\text{in}}$ as in (7) with $s_1 = 10, s_2 = 3$ which accounts for 91.9% of $\mathcal{V}_c$ as in (6). Finally, the disturbance set $\mathcal{W}$ as in (18) is considered to be bounded as:

$$ \mathcal{W} = \{ w : |w| \le 0.02, l = 1, \dots, 6 \}, \quad (29) $$

which is estimated from our data of the difference between the system predicted and actual behavior after the same control action [6]. The set was intentionally chosen to be symmetric so that the heavy computation of the mRPI set $\mathcal{Z}$
as the “origin” for our stabilizing problem [16]. Next, the terminal ingredients (P and the set $\mathcal{E}_f$) for the nominal MPC settings are found exactly with the Proposition 2 with the gain K chosen as aforementioned, the dynamics (23a) and the constraint set $\mathcal{V}^* \triangleq \tilde{\mathcal{V}} \ominus K\mathcal{Z}$. It is also assumed that the disturbance’s bound $\mathcal{W}$ is sufficiently manageable and hence results in non-empty feasible sets for the constraints in (23).

In short, the offline synthesis can be summarized as:

1) Choose $Q > 0$ and $R \succeq 0$ to formulate the stage and terminal cost value function of the form (13);

2) Choose the gain K stabilizing the pair $(A_d, B_d)$;

3) Compute the mRPI set $\mathcal{Z}$ as proposed in [17];

4) Compute the terminal ingredients subject to the constraint $\mathcal{V}^* \triangleq \tilde{\mathcal{V}} \ominus K\mathcal{Z}$ as in Proposition 2.

**Remark 4:** To avoid computational issues caused by the system’s large dimensionality, we do not employ the computation of the maximal positively invariant set [19] for the closed-loop $\tilde{x}_{k+1} = A_{cl}\tilde{x}_k$ to play the role of the terminal region $\mathcal{E}_f$. More specifically, although proven finite, the number of equalities portraying such a set for this 6 dimensional problem may exceed the memory capacity provided for the computation software. Yet, if the polyhedral set can be computed, the terminal constraints $\mathcal{E}_f$ will become linear, as opposed to being quadratic as in (12), and reduce the complexity of the online problem (22)-(23).

Nevertheless, with these setups, the control input applied to the system (21a) is defined as:

$$ \tilde{v}_k = \tilde{v}_{N_p}^{\text{mpc}}(\tilde{x}_k) \triangleq \tilde{v}_k^* + K(\tilde{x}_k - \tilde{x}_k^*). \quad (24) $$

Consequently, with this scheme, as proven in [16], the set $\mathcal{Z}$ is robustly exponentially stable for the closed-loop:

$$ \tilde{x}_{k+1} = A_d \tilde{x}_k + B_d \tilde{v}_{N_p}^{\text{mpc}}(\tilde{x}_k) + w. \quad (25) $$

Namely, $\tilde{x}_k$ is maintained inside the “tube” $\tilde{x}_k^* \oplus \mathcal{Z}$ which converges to $\mathcal{Z}$ when $k \to \infty$. In other words, one can state:

$$ x_k - x_k^{\text{ref}} = \tilde{x}_k \in \tilde{x}_k^* \oplus \mathcal{Z} \Leftrightarrow x_k \in x_k^{\text{ref}} \oplus \tilde{x}_k^* \oplus \mathcal{Z} \quad (26) $$

with $\tilde{x}_k^*$ exponentially converges to $\mathbf{0}_{6 \times 1}$. With the sequence of the reference for $x_k$ and $v_k$ denoted as $x^{\text{ref}}$, $v^{\text{ref}}$, respectively, the control implementation is given by Algorithm 1.

<table><caption>**Algorithm 1:** Robust MPC for trajectory tracking</caption><tr><td colspan="2"><b>Input:</b> Reference sequence $x_k^{\text{ref}}$, $v_k^{\text{ref}}$; the optimization setup as in (22)-(23).</td></tr><tr><td>1</td><td>for ($x_k^{\text{ref}}$, $v_k^{\text{ref}}$) in $x_k^{\text{ref}} \times v_k^{\text{ref}}$ do</td></tr><tr><td>2</td><td>&nbsp;&nbsp;Measure $x_k$ and $\psi$ at time step $k$;</td></tr><tr><td>3</td><td>&nbsp;&nbsp;Compute $\tilde{x}_k^*$ and $\tilde{v}_k^*$ by solving (22)-(23);</td></tr><tr><td>4</td><td>&nbsp;&nbsp;$\tilde{v}_{N_p}^{\text{mpc}} \leftarrow \tilde{v}_k^* + K(\tilde{x}_k - \tilde{x}_k^*)$ as in (24);</td></tr><tr><td>5</td><td>&nbsp;&nbsp;$u_k \leftarrow \varphi_{\psi}(v_k^{\text{ref}} + \tilde{v}_{N_p}^{\text{mpc}})$ as in (4);</td></tr><tr><td>6</td><td>&nbsp;&nbsp;Apply $u_k$ to the vehicle;</td></tr><tr><td>7</td><td>end</td></tr></table>

## IV. EXPERIMENTAL VALIDATION

The experiments are done over the Crazyflie 2.1 quadcopter platform where the drone’s inputs are the desired thrust, roll and pitch angles, gathered in the vector $u_k$, together with the desired yaw angle, assigned to zero.

<figure><img src="image_4.png" /><figcaption>Fig. 2. Two references employed for the experimental validation.</figcaption></figure>

Furthermore, the position and velocity of the quadcopter are acquired via a system of 8 Qualisys motion capture cameras. Note that the platform expects the normalized thrust $T(m/s^2)$ to be converted into the quadcopter’s unit which varies from 0 to 65535. Then, these signals will be tracked via a built-in inner-loop containing two cascade PID and a transformation from the required torque to PWM signals designed for the drone’s X-shape configuration [20].

### A. Parameter setup

Firstly, the reference is required to be at least two time differentiable to guarantee the continuity and boundedness of $v^{\text{ref}}$ as in (20b). Herein, two references are used as follows:

* Ref. 1: The position of the drone will be parameterized in time with the B-spline method [11]. More specifically, at timestamp $t_r(s)$, we imposed the position of the drone to pass through a waypoint $p_r$ as:

$$ p_r^\top \in \left\{ \begin{array}{l} [6, 0, 3]; [6, 3, 5]; [6, 6, 10]; \\ [0, 6, 10]; [-6, 6, 10]; [-6, -6, 10]; \\ [6, -6, 10]; [6, 0, 10]; [6, 6, 10] \end{array} \right\} \times 10(\text{cm}), \quad (27) $$

with $t_r \in \mathbb{N}^9(0, 45)$ (s). Then, by minimizing the curve’s length and restraining their second derivatives (which is the mathematical interpretation of $v_k^{\text{ref}}$) inside $\mathcal{V}_c^{\text{in}}$, we obtain the green solid curve depicted in Fig. 2.

* Ref. 2: The circular motion is parameterized as:

$$ \begin{aligned} x^{\text{ref}}(t) &= 0.6 \cos \omega t, & y^{\text{ref}}(t) &= \sin 0.6\omega t, \\ z(t) &= 0.5(\text{m}), & \omega &= 0.2\pi(\text{rad/s}), \end{aligned} \quad (28) $$

and illustrated as the purple curve in Fig. 2. Note that at the beginning and the end of the reference, we introduce smooth curves to reduce the overshoot effect caused by introducing a sudden reference.

Consequently, $\mathcal{V}_c^{\text{ref}}$ as in (20b) can be obtained by bounding the $v^{\text{ref}}$ with a box-type constraint (See Fig. 3).

The parameter of constraint set $\mathcal{U}$ as in (3) is provided in Table I while the corresponding constraint $\mathcal{V}_c$ is approximated by $\mathcal{V}_c^{\text{in}}$ as in (7) with $s_1 = 10, s_2 = 3$ which accounts for 91.9% of $\mathcal{V}_c$ as in (6). Finally, the disturbance set $\mathcal{W}$ as in (18) is considered to be bounded as:

$$ \mathcal{W} = \{w : |w| \le 0.02, l = 1, \dots, 6\}, \quad (29) $$

which is estimated from our data of the difference between the system predicted and actual behavior after the same control action [6]. The set was intentionally chosen to be symmetric so that the heavy computation of the mRPI set $\mathcal{Z}$
in 6 dimensional space is reduced to that of the three double integrators as in (19) and their Cartesian product.

<figure><img src="image_3.png" /><figcaption>Fig. 3. Two input $v^{\text{ref}}$ of the two references and their bounding boxes.</figcaption></figure>

## B. Controller tuning

First, $Q, R$ in (22) are chosen in the following form:

$$ \left\{ \begin{array}{l} Q = \operatorname{diag}(Q^*, Q^*, Q^*); Q^* \triangleq \operatorname{diag}(\mu_p, \mu_v); \\ R = \operatorname{diag}(\mu_r, \mu_r, \mu_r), \end{array} \right. \qquad (30) $$

where $\mu_p, \mu_v, \mu_r$ can be interpreted as the penalties for the deviation in position, velocity and input from the reference. The gain $K$ in (23c) is found by placing the poles $p_i, i \in \{1, ..., 6\}$ in continuous time and computing the corresponding poles via the Z-transformation (i.e., $z_i = e^{p_i t_s}$) for the closed-loop $A_{cl} = A_d + B_d K$. We proceed to choose the poles in continuous time as:

$$ p_1 = p_2 = p_3 = -4\mu_K \quad \text{and} \quad p_4 = p_5 = p_6 = -5\mu_K, \quad (31) $$

with the intention of analyzing the effect of choosing poles with different rise times or convergence rates. Then we provide some criteria for choosing the gain and matrices appropriately. Illustrative examples for the tracking problem of Ref. 1 are given to analyze the closed-loop characteristics.

<figure><img src="image_4.png" /><figcaption>Fig. 4. Effect of changing the gain $K$ towards fast closed-loop convergence, projected into the first two axes of $\tilde{x}_k$ ($^1\tilde{x}_k$-horizontal, $^2\tilde{x}_k$-vertical).</figcaption></figure>

On one hand, as mentioned in Remark 3 and Proposition 2, for the nominal system, the stabilizing gain $K$ also needs to be chosen so that $0_{6 \times 1} \in \tilde{\mathcal{X}} \triangleq \{\tilde{x} : K\tilde{x} \in \mathcal{V}^*\}$. Meanwhile, under the presence of $\mathcal{W}$, $K$ needs to stabilize the closed-loop robustly or fast enough so that the corresponding mRPI set $\mathcal{Z}$ in (23b) becomes satisfactory in terms of size. More specifically, as $\mathcal{Z}$ represents the bound of the tracking error, it reflects the controller's performance guarantee. With these

<table><caption>TABLE I
CONTROLLER'S PARAMETERS AND SYSTEM'S SPECIFICATION</caption><thead><tr><th>Parameters</th><th>Values</th></tr></thead><tbody><tr><td>Sampling time t<sub>s</sub> (s)</td><td>0.2</td></tr><tr><td>Prediction horizon N<sub>p</sub></td><td>10 steps</td></tr><tr><td>T<sub>max</sub>, ε<sub>max</sub> in (3)</td><td>1.45g ≈ 14.22 m/s<sup>2</sup>; 0.1745 rad</td></tr><tr><td>(μ<sub>K</sub>, μ<sub>p</sub>, μ<sub>v</sub>, μ<sub>r</sub>) in (30)-(31)</td><td>(0.5, 50, 15, 25)</td></tr></tbody></table>

observations, firstly, we would like to choose the gain $K$ as “aggressive” as possible, or in our settings, $\mu_K$ as large as possible. Secondly, the choice of gain $K$ is also tangled with the size of $\mathcal{E}_f$ which necessarily requires including $\mathcal{Z}$ (i.e., $\mathcal{Z} \subset \mathcal{E}_f$) so that the input constraints $K\tilde{x} \in \mathcal{V}^* = \tilde{\mathcal{V}} \oplus K\mathcal{Z}$ are also satisfied inside $\mathcal{Z}$, and so is the closed-loop performance. Therefore, as illustrated in Fig. 4, the two last choices of $\mu_K = 1.0$ and $1.25$ violate the latter, while the first choice $\mu_K = 0.25$ results in a poor guarantee for the closed-loop (with the weighting fixed as $\mu_q = 50, \mu_v = 15$ and $\mu_r = 25$) as stated in the former principle.

On the other hand, after the suitable choice of $K$ fixing the set $\mathcal{Z}$, it is also possible to modify the set $\mathcal{E}_f$ with $Q$ and $R$. The motivation for this adjustment comes from the fact that, for the stabilization of system (21), $\mathcal{E}_f \oplus \mathcal{Z}$ plays the role of the new terminal region for the system's tube trajectory [16]. Therefore, for a given initial tracking error point, the set can be modified to be “closer” or even to include the point, hence, resulting in a shorter prediction interval $N_p$. Depending on how the penalty weights are chosen, the set $\mathcal{E}_f$ can be characterized to some extent, while the condition $\mathcal{Z} \subset \mathcal{E}_f$ also needs to be ensured. For the experiments, the weighting matrices and gain are given in Table I.

## C. Experimental results and discussion

Fig. 5 and Fig. 6 show the position tracking performance and how the tracking error of the quadcopter is restrained inside the tube $\mathcal{Z}$, respectively. Numerical details are also given in Table II where, the matrix $P$ is represented as:

$$ P \triangleq \operatorname{diag}(P^*, P^*, P^*). \qquad (32) $$

<figure><img src="image_5.png" /><figcaption>Fig. 5. Position tracking with the two references.</figcaption></figure>

Overall, it can be seen that the controller is successfully implemented and verified within the flight tests with around 3-5 cm in root-mean-square (RMS) of tracking error<fnref n="1" /> and under 40ms for the average computation time at each control loop (see Fig. 7). These results show that not only is the performance guaranteed, but also that the method is computationally efficient thanks to the linear model prediction obtained via the coordinate change.

It is also noticeable that, although later adequately concealed in the tube, there are significant overshoots of errors at the beginning of both flights. This problem comes from the fact that the drone starts from a static state with zero

<footnote n="1">¹RMS = $\frac{1}{3} \sum_{q=1,3,5} \sqrt{N_s^{-1} \sum_{l=1}^{N_s} (q\mathbf{x}_k - q\mathbf{x}^{\text{ref}})^2}$, $N_s$ is the number of simulation steps.</footnote>
in 6 dimensional space is reduced to that of the three double integrators as in (19) and their Cartesian product.

<figure><img src="image_5.png" /><figcaption>Fig. 3. Two input $v^{\text{ref}}$ of the two references and their bounding boxes.</figcaption></figure>

## B. Controller tuning

First, $Q, R$ in (22) are chosen in the following form:

$$ \left\{ \begin{array}{l} Q = \operatorname{diag}(Q^*, Q^*, Q^*); Q^* \triangleq \operatorname{diag}(\mu_p, \mu_v); \\ R = \operatorname{diag}(\mu_r, \mu_r, \mu_r), \end{array} \right. \qquad (30) $$

where $\mu_p, \mu_v, \mu_r$ can be interpreted as the penalties for the deviation in position, velocity and input from the reference. The gain $K$ in (23c) is found by placing the poles $p_i, i \in \{1, ..., 6\}$ in continuous time and computing the corresponding poles via the Z-transformation (i.e., $z_i = e^{p_i t_s}$) for the closed-loop $A_{cl} = A_d + B_d K$. We proceed to choose the poles in continuous time as:

$$ p_1 = p_2 = p_3 = -4\mu_K \quad \text{and} \quad p_4 = p_5 = p_6 = -5\mu_K, \quad (31) $$

with the intention of analyzing the effect of choosing poles with different rise times or convergence rates. Then we provide some criteria for choosing the gain and matrices appropriately. Illustrative examples for the tracking problem of Ref. 1 are given to analyze the closed-loop characteristics.

<figure><img src="image_6.png" /><figcaption>Fig. 4. Effect of changing the gain $K$ towards fast closed-loop convergence, projected into the first two axes of $\tilde{x}_k$ ($^1\tilde{x}_k$-horizontal, $^2\tilde{x}_k$-vertical).</figcaption></figure>

On one hand, as mentioned in Remark 3 and Proposition 2, for the nominal system, the stabilizing gain $K$ also needs to be chosen so that $0_{6 \times 1} \in \tilde{\mathcal{X}} \triangleq \{\tilde{x} : K\tilde{x} \in \mathcal{V}^*\}$. Meanwhile, under the presence of $\mathcal{W}$, $K$ needs to stabilize the closed-loop robustly or fast enough so that the corresponding mRPI set $\mathcal{Z}$ in (23b) becomes satisfactory in terms of size. More specifically, as $\mathcal{Z}$ represents the bound of the tracking error, it reflects the controller's performance guarantee. With these

<table><caption>TABLE I
CONTROLLER'S PARAMETERS AND SYSTEM'S SPECIFICATION</caption><thead><tr><th>Parameters</th><th>Values</th></tr></thead><tbody><tr><td>Sampling time t<sub>s</sub> (s)</td><td>0.2</td></tr><tr><td>Prediction horizon N<sub>p</sub></td><td>10 steps</td></tr><tr><td>T<sub>max</sub>, ε<sub>max</sub> in (3)</td><td>1.45g ≈ 14.22 m/s<sup>2</sup>; 0.1745 rad</td></tr><tr><td>(μ<sub>K</sub>, μ<sub>p</sub>, μ<sub>v</sub>, μ<sub>r</sub>) in (30)-(31)</td><td>(0.5, 50, 15, 25)</td></tr></tbody></table>

observations, firstly, we would like to choose the gain $K$ as “aggressive” as possible, or in our settings, $\mu_K$ as large as possible. Secondly, the choice of gain $K$ is also tangled with the size of $\mathcal{E}_f$ which necessarily requires including $\mathcal{Z}$ (i.e., $\mathcal{Z} \subset \mathcal{E}_f$) so that the input constraints $K\tilde{x} \in \mathcal{V}^* = \tilde{\mathcal{V}} \ominus K\mathcal{Z}$ are also satisfied inside $\mathcal{Z}$, and so is the closed-loop performance. Therefore, as illustrated in Fig. 4, the two last choices of $\mu_K = 1.0$ and $1.25$ violate the latter, while the first choice $\mu_K = 0.25$ results in a poor guarantee for the closed-loop (with the weighting fixed as $\mu_q = 50, \mu_v = 15$ and $\mu_r = 25$) as stated in the former principle.

On the other hand, after the suitable choice of $K$ fixing the set $\mathcal{Z}$, it is also possible to modify the set $\mathcal{E}_f$ with $Q$ and $R$. The motivation for this adjustment comes from the fact that, for the stabilization of system (21), $\mathcal{E}_f \oplus \mathcal{Z}$ plays the role of the new terminal region for the system's tube trajectory [16]. Therefore, for a given initial tracking error point, the set can be modified to be “closer” or even to include the point, hence, resulting in a shorter prediction interval $N_p$. Depending on how the penalty weights are chosen, the set $\mathcal{E}_f$ can be characterized to some extent, while the condition $\mathcal{Z} \subset \mathcal{E}_f$ also needs to be ensured. For the experiments, the weighting matrices and gain are given in Table I.

## C. Experimental results and discussion

Fig. 5 and Fig. 6 show the position tracking performance and how the tracking error of the quadcopter is restrained inside the tube $\mathcal{Z}$, respectively. Numerical details are also given in Table II where, the matrix $P$ is represented as:

$$ P \triangleq \operatorname{diag}(P^*, P^*, P^*). \qquad (32) $$

<figure><img src="image_7.png" /><figcaption>Fig. 5. Position tracking with the two references.</figcaption></figure>

Overall, it can be seen that the controller is successfully implemented and verified within the flight tests with around 3-5 cm in root-mean-square (RMS) of tracking error<fnref n="1" /> and under 40ms for the average computation time at each control loop (see Fig. 7). These results show that not only is the performance guaranteed, but also that the method is computationally efficient thanks to the linear model prediction obtained via the coordinate change.

It is also noticeable that, although later adequately concealed in the tube, there are significant overshoots of errors at the beginning of both flights. This problem comes from the fact that the drone starts from a static state with zero

<footnote n="1">¹RMS = $\frac{1}{3} \sum_{q=1,3,5} \sqrt{N_s^{-1} \sum_{l=1}^{N_s} (q\mathbf{x}_k - q\mathbf{x}^{\text{ref}})^2}$, $N_s$ is the number of simulation steps.</footnote>
<figure><img src="image_6.png" /><figcaption>Fig. 6. Tracking error $\tilde{x}_k$ for Ref. 1 (top) and Ref. 2 (bottom).</figcaption></figure>

<table><caption>TABLE II
EXPERIMENTAL RESULTS AND CONTROL PARAMETERS</caption><thead><tr><th></th><th>Ref. 1</th><th>Ref. 2</th></tr></thead><tbody><tr><td>RMS of tracking error</td><td>3.25 cm</td><td>5.46 cm</td></tr><tr><td>Average computation time</td><td>39.40 ms</td><td>38.11 ms</td></tr><tr><td><b>P</b>*</td><td>513.54, 328.59<br>328.59, 356.89</td><td>542.18, 329.80<br>329.80, 368.92</td></tr><tr><td>ε<sub>f</sub> as in (12)</td><td>8.7104</td><td>6.0724</td></tr></tbody></table>

angular velocity of the propellers, which later entails a sudden acceleration of the vehicle. However, all three inputs are not saturated and remain constrained in their bounds, as can be seen from Fig. 8.

<figure><img src="image_7.png" /><figcaption>Fig. 7. Computation time for the proposed MPC scheme.</figcaption></figure>

V. CONCLUSION

This paper presented a procedure to implement the robust trajectory tracking for a quadcopter based on tube MPC. The main advantage came from the problem reformulation through differential flatness, which transformed the nonlinear system into chains of integrators in closed-loop. Furthermore, an approximation of the feasible domain was given to convert the original tracking problem into a constrained stabilization problem under disturbances. Then, with a robust MPC setup for linear systems, the tracking performance was guaranteed along the trajectory without any approximation. The controller was successfully applied and validated via experimental tests. Further work will concentrate on multi-quadcopters formation stability and robustness guarantees.

REFERENCES

[1] W. Xie, G. Yu, D. Cabecinhas, R. Cunha, and C. Silvestre, "Global saturated tracking control of a quadcopter with experimental validation," *IEEE Control Systems Letters*, vol. 5, no. 1, pp. 169–174, 2020.

<figure><img src="image_8.png" /><figcaption>Fig. 8. Quadcopter's inputs for the tracking of Ref. 1 and 2.</figcaption></figure>

[2] N. T. Nguyen and I. Prodan, "Stabilizing a multicopter using an nmpc design with a relaxed terminal region," *IFAC-PapersOnLine*, vol. 54, no. 6, pp. 126–132, 2021.

[3] D. Q. Mayne, J. B. Rawlings, C. V. Rao, and P. O. Scokaert, "Constrained model predictive control: Stability and optimality," *Automatica*, vol. 36, no. 6, pp. 789–814, 2000.

[4] D. Wang, Q. Pan, Y. Shi, J. Hu, and C. Zhao, "Efficient nonlinear model predictive control for quadrotor trajectory tracking: Algorithms and experiment," *IEEE Trans. on Cybernetics*, 2021.

[5] M. A. Santos, A. Ferramosca, and G. V. Raffo, "Tube-based MPC with nonlinear control for load transportation using a UAV," in *9th IFAC Symposium on Robust Control Design*, pp. 459–465, 2018.

[6] N. Michel, S. Bertrand, S. Olaru, G. Valmorbidà, and D. Dumur, "Design and flight experiments of a tube-based model predictive controller for the ar. drone 2.0 quadrotor," *IFAC-PapersOnLine*, vol. 52, no. 22, pp. 112–117, 2019.

[7] A. Didier, A. Parsi, J. Coulson, and R. S. Smith, "Robust adaptive model predictive control of quadrotors," in *European Cont. Conf., IEEE*, 2021.

[8] M. W. Mueller and R. D'Andrea, "A model predictive controller for quadrocopter state interception," in *2013 European Control Conference (ECC)*, pp. 1383–1389, IEEE, 2013.

[9] H.-T. Do and I. Prodan, "Indoor experimental validation of mpc-based trajectory tracking for a quadcopter via a flat mapping approach," in *2023 European Control Conference (ECC)*, 2023.

[10] J. Levine, *Analysis and control of nonlinear systems: A flatness-based approach*. Springer Science & Business Media, 2009.

[11] N. T. Nguyen, I. Prodan, and L. Lefèvre, "Flat trajectory design and tracking with saturation guarantees: a nano-drone application," *International Journal of Control*, pp. 1–14, 2018.

[12] S. Amini, B. Ahi, and M. Haeri, "Control of high order integrator chain systems subjected to disturbance and saturated control: A new adaptive scheme," *Automatica*, vol. 100, pp. 108–113, 2019.

[13] S. Boyd, S. P. Boyd, and L. Vandenberghe, *Convex optimization*. Cambridge university press, 2004.

[14] H. Chen and F. Allgöwer, "A quasi-infinite horizon nonlinear model predictive control scheme with guaranteed stability," *Automatica*, vol. 34, no. 10, pp. 1205–1217, 1998.

[15] F. Blanchini and S. Miani, *Set-theoretic methods in control*. Springer, 2008.

[16] D. Q. Mayne, M. M. Seron, and S. Raković, "Robust model predictive control of constrained linear systems with bounded disturbances," *Automatica*, vol. 41, no. 2, pp. 219–224, 2005.

[17] S. V. Rakovic, E. C. Kerrigan, K. I. Kouramas, and D. Q. Mayne, "Invariant approximations of the minimal robust positively invariant set," *IEEE Trans. on Auto. Control*, vol. 50, no. 3, pp. 406–410, 2005.

[18] I. Kolmanovsky and E. G. Gilbert, "Theory and computation of disturbance invariant sets for discrete-time linear systems," *Mathematical problems in engineering*, vol. 4, no. 4, pp. 317–367, 1998.

[19] E. G. Gilbert and K. T. Tan, "Linear systems with state and control constraints: The theory and application of maximal output admissible sets," *IEEE Trans. on Auto. Cont.*, vol. 36, no. 9, pp. 1008–1020, 1991.

[20] W. Giernacki, M. Skwierczyński, W. Witwicki, P. Wronski, and P. Kozierski, "Crazyfly 2.0 quadrotor as a platform for research and education in robotics and control engineering," in *2017 22nd International Conference on Methods and Models in Automation and Robotics*, pp. 37–42, IEEE, 2017.
<figure><img src="image_8.png" /><figcaption>Fig. 6. Tracking error $\tilde{x}_k$ for Ref. 1 (top) and Ref. 2 (bottom).</figcaption></figure>

<table><caption>TABLE II
EXPERIMENTAL RESULTS AND CONTROL PARAMETERS</caption><thead><tr><th></th><th>Ref. 1</th><th>Ref. 2</th></tr></thead><tbody><tr><td>RMS of tracking error</td><td>3.25 cm</td><td>5.46 cm</td></tr><tr><td>Average computation time</td><td>39.40 ms</td><td>38.11 ms</td></tr><tr><td><b>P</b>* as in (32)</td><td>513.54, 328.59<br>328.59, 356.89</td><td>542.18, 329.80<br>329.80, 368.92</td></tr><tr><td>ε<sub>f</sub> as in (12)</td><td>8.7104</td><td>6.0724</td></tr></tbody></table>

angular velocity of the propellers, which later entails a sudden acceleration of the vehicle. However, all three inputs are not saturated and remain constrained in their bounds, as can be seen from Fig. 8.

<figure><img src="image_9.png" /><figcaption>Fig. 7. Computation time for the proposed MPC scheme.</figcaption></figure>

## V. CONCLUSION

This paper presented a procedure to implement the robust trajectory tracking for a quadcopter based on tube MPC. The main advantage came from the problem reformulation through differential flatness, which transformed the nonlinear system into chains of integrators in closed-loop. Furthermore, an approximation of the feasible domain was given to convert the original tracking problem into a constrained stabilization problem under disturbances. Then, with a robust MPC setup for linear systems, the tracking performance was guaranteed along the trajectory without any approximation. The controller was successfully applied and validated via experimental tests. Further work will concentrate on multi-quadcopters formation stability and robustness guarantees.

## REFERENCES

[1] W. Xie, G. Yu, D. Cabecinhas, R. Cunha, and C. Silvestre, "Global saturated tracking control of a quadcopter with experimental validation," *IEEE Control Systems Letters*, vol. 5, no. 1, pp. 169–174, 2020.

<figure><img src="image_10.png" /><figcaption>Fig. 8. Quadcopter's inputs for the tracking of Ref. 1 and 2.</figcaption></figure>

[2] N. T. Nguyen and I. Prodan, "Stabilizing a multicopter using an nmpc design with a relaxed terminal region," *IFAC-PapersOnLine*, vol. 54, no. 6, pp. 126–132, 2021.

[3] D. Q. Mayne, J. B. Rawlings, C. V. Rao, and P. O. Scokaert, "Constrained model predictive control: Stability and optimality," *Automatica*, vol. 36, no. 6, pp. 789–814, 2000.

[4] D. Wang, Q. Pan, Y. Shi, J. Hu, and C. Zhao, "Efficient nonlinear model predictive control for quadrotor trajectory tracking: Algorithms and experiment," *IEEE Trans. on Cybernetics*, 2021.

[5] M. A. Santos, A. Ferramosca, and G. V. Raffo, "Tube-based MPC with nonlinear control for load transportation using a UAV," in *9th IFAC Symposium on Robust Control Design*, pp. 459–465, 2018.

[6] N. Michel, S. Bertrand, S. Olaru, G. Valmorbidà, and D. Dumur, "Design and flight experiments of a tube-based model predictive controller for the ar. drone 2.0 quadrotor," *IFAC-PapersOnLine*, vol. 52, no. 22, pp. 112–117, 2019.

[7] A. Didier, A. Parsi, J. Coulson, and R. S. Smith, "Robust adaptive model predictive control of quadrotors," in *European Cont. Conf., IEEE*, 2021.

[8] M. W. Mueller and R. D'Andrea, "A model predictive controller for quadrocopter state interception," in *2013 European Control Conference (ECC)*, pp. 1383–1389, IEEE, 2013.

[9] H.-T. Do and I. Prodan, "Indoor experimental validation of mpc-based trajectory tracking for a quadcopter via a flat mapping approach," in *2023 European Control Conference (ECC)*, 2023.

[10] J. Levine, *Analysis and control of nonlinear systems: A flatness-based approach*. Springer Science & Business Media, 2009.

[11] N. T. Nguyen, I. Prodan, and L. Lefèvre, "Flat trajectory design and tracking with saturation guarantees: a nano-drone application," *International Journal of Control*, pp. 1–14, 2018.

[12] S. Amini, B. Ahi, and M. Haeri, "Control of high order integrator chain systems subjected to disturbance and saturated control: A new adaptive scheme," *Automatica*, vol. 100, pp. 108–113, 2019.

[13] S. Boyd, S. P. Boyd, and L. Vandenberghe, *Convex optimization*. Cambridge university press, 2004.

[14] H. Chen and F. Allgöwer, "A quasi-infinite horizon nonlinear model predictive control scheme with guaranteed stability," *Automatica*, vol. 34, no. 10, pp. 1205–1217, 1998.

[15] F. Blanchini and S. Miani, *Set-theoretic methods in control*. Springer, 2008.

[16] D. Q. Mayne, M. M. Seron, and S. Raković, "Robust model predictive control of constrained linear systems with bounded disturbances," *Automatica*, vol. 41, no. 2, pp. 219–224, 2005.

[17] S. V. Rakovic, E. C. Kerrigan, K. I. Kouramas, and D. Q. Mayne, "Invariant approximations of the minimal robust positively invariant set," *IEEE Trans. on Auto. Control*, vol. 50, no. 3, pp. 406–410, 2005.

[18] I. Kolmanovsky and E. G. Gilbert, "Theory and computation of disturbance invariant sets for discrete-time linear systems," *Mathematical problems in engineering*, vol. 4, no. 4, pp. 317–367, 1998.

[19] E. G. Gilbert and K. T. Tan, "Linear systems with state and control constraints: The theory and application of maximal output admissible sets," *IEEE Trans. on Auto. Cont.*, vol. 36, no. 9, pp. 1008–1020, 1991.

[20] W. Giernacki, M. Skwierczyński, W. Witwicki, P. Wronski, and P. Kozierski, "Crazyflie 2.0 quadrotor as a platform for research and education in robotics and control engineering," in *2017 22nd International Conference on Methods and Models in Automation and Robotics*, pp. 37–42, IEEE, 2017.