# Experimental Validation of Sensitivity-Aware Trajectory Planning for a Quadrotor UAV Under Parametric Uncertainty

Ali Srour, Salvatore Marcellini, Tommaso Belvedere, Marco Cognetti, Antonio Franchi, Paolo Robuffo Giordano

► To cite this version:

Ali Srour, Salvatore Marcellini, Tommaso Belvedere, Marco Cognetti, Antonio Franchi, et al., Experimental Validation of Sensitivity-Aware Trajectory Planning for a Quadrotor UAV Under Parametric Uncertainty. ICUAS 2024 - International Conference on Unmanned Aircraft Systems, Jun 2024, Chania Crete, Greece. pp.1-7. hal-04553932

HAL Id: hal-04553932

https://hal.science/hal-04553932

Submitted on 21 Apr 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_1.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
# Experimental Validation of Sensitivity-Aware Trajectory Planning for a Quadrotor UAV Under Parametric Uncertainty

Ali Srour, Salvatore Marcellini, Tommaso Belvedere, Marco Cognetti, Antonio

Franchi, Paolo Robuffo Giordano

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

► To cite this version:

Ali Srour, Salvatore Marcellini, Tommaso Belvedere, Marco Cognetti, Antonio Franchi, et al., Experimental Validation of Sensitivity-Aware Trajectory Planning for a Quadrotor UAV Under Parametric Uncertainty. ICUAS 2024 - International Conference on Unmanned Aircraft Systems, Jun 2024, Chania Crete, Greece. pp.572-578, 10.1109/ICUAS60882.2024.10556958. hal-04553932

HAL Id: hal-04553932

https://hal.science/hal-04553932v1

Submitted on 21 Apr 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
# Experimental Validation of Sensitivity-Aware Trajectory Planning for a Quadrotor UAV Under Parametric Uncertainty

Ali Srour<fnref n="1" />, Salvatore Marcellini<fnref n="2" />, Tommaso Belvedere<fnref n="3" />, Marco Cognetti<fnref n="4" />,
Antonio Franchi<fnref n="3" />,<fnref n="5" />, Paolo Robuffo Giordano<fnref n="1" />

**Abstract**— In this work, we provide an experimental validation of the recent concepts of closed-loop state and input sensitivity in the context of robust flight control for a quadrotor (UAV) equipped with the popular PX4¹ controller. Our objective is to experimentally assess how the optimization of the reference trajectory w.r.t. these sensitivity metrics can improve the closed-loop system performance against model uncertainties commonly affecting the quadrotor systems. To accomplish this, we present a series of experiments designed to validate our optimization approach on two distinct trajectories, with the primary aim of assessing its precision in guiding the quadrotor through the center of a window at relatively high speeds. This approach provides some interesting insights for increasing the closed-loop robustness of the robot state and inputs against physical parametric uncertainties that may degrade the system's performance.

## I. INTRODUCTION

Aerial robotics has experienced a growing interest propelled by advancements in research, resulting in numerous practical uses. A primary concern with aerial robots is to improve system autonomy and ensure safety during motion tasks. Nevertheless, attaining high levels of autonomy for flying robots remains an enduring challenge due to the need to operate in unpredictable and uncertain real-world conditions. Despite efforts to create precise models, real-world complexities introduce several uncertainties among which inaccuracies in the model parameters, which can potentially disrupt the system behavior during task execution. The pursuit of greater autonomy, precision, and safety in aerial robotics requires bridging the gap between theoretical models and practical real-world conditions. Adaptive [1]–[3] or robust control [4], [5] are typical methods to deal with parametric uncertainty, either through online parameter

<figure><img src="image_2.png" /><figcaption>Fig. 1. Drone trajectory tracking under parametric uncertainties (green) to pass through a target $s_d(t_w)$ (blue sphere). Non-optimized (INIT, left) and optimized (OPTa, right) trajectories are compared. The closest positions reached to the desired target after each experiment (small red sphere) are visualized in PyBullet where the drone passes through a circular window at a relatively high speed. A video of the experiments is available at https://youtu.be/QFnrQ_O2BiU.</figcaption></figure>

estimation or by introducing trade-offs between performance and robustness vs. parametric uncertainty. Another approach involves employing Model Predictive Control, as discussed in [6], [7], which predicts the system behavior using a dynamic model. However, the MPC effectiveness depends on the model, and thus parameters, accuracy. Uncertainties in these parameters can significantly impact both the controller performance and the robot behavior. Another possibility is to plan feedforward trajectories with minimal state sensitivity, as in [8]–[10]. However, these approaches operate in an open-loop fashion and thus do not consider the presence and strengths/weaknesses of the motion controller that is eventually implemented on the robot.

Recent strategies for addressing the parameter uncertainty problem have been introduced in [11]–[15] where metrics based on closed-loop state/input sensitivity assess how uncertainties in robot model affect robots behavior during closed-loop control. To enhance robustness vs. parametric uncertainties, one can plan a feedforward desired trajectory that minimizes these sensitivity metrics, creating a motion plan that is inherently robust. These concepts have been further developed in [16] for optimal initialization of an energy tank in the context of passivity-based control, and in [17] where an extension has been proposed to evaluate the impact of the controller choice, gain tuning and shape of reference trajectory for minimizing the parametric sen-

<footnote n="1">¹A. Srour and P. Robuffo Giordano are with CNRS, Univ Rennes, Inria, IRISA, Campus de Beaulieu, 35042 Rennes Cedex, France. email: {ali.srour, prg}@irisa.fr</footnote>

<footnote n="2">²S. Marcellini is with the PRISMA Lab, Department of Electrical Engineering and Information Technology, University of Naples Federico II, Via Claudio 21, Naples, 80125, Italy. email: salvatore.marcellini@unina.it</footnote>

<footnote n="3">³T. Belvedere and A. Franchi are with the Department of Computer, Control and Management Engineering, Sapienza University of Rome, 00185 Rome, Italy. email: belvedere@diag.uniroma.it; antonio.franchi@uniroma.it</footnote>

<footnote n="4">⁴M. Cognetti is with LAAS-CNRS, Université de Toulouse, CNRS, UPS, Toulouse, France LAAS-CNRS, email: mcognetti@laas.fr</footnote>

<footnote n="5">⁵A. Franchi is also with the Robotics and Mechatronics Department, Electrical Engineering, Mathematics, Computer Science (EEMCS) Faculty, University of Twente, 7500 AE Enschede, The Netherlands. email: a.franchi@utwente.nl</footnote>

<footnote>This work was supported by the project ANR-20-CE33-0003 "CAMP", the Horizon EU research and innovation programme [grant agreement No. 101120732] AUTOASSESS, and the Chaire de Professeur Junior grant no. ANR-22-CPJ1-0064-01.</footnote>

<footnote n="1">¹https://px4.io/</footnote>
# Experimental Validation of Sensitivity-Aware Trajectory Planning for a Quadrotor UAV Under Parametric Uncertainty

Ali Srour<fnref n="1" />, Salvatore Marcellini<fnref n="2" />, Tommaso Belvedere<fnref n="3" />, Marco Cognetti<fnref n="4" />,
Antonio Franchi<fnref n="3" />,<fnref n="5" />, Paolo Robuffo Giordano<fnref n="1" />

**Abstract**— In this work, we provide an experimental validation of the recent concepts of closed-loop state and input sensitivity in the context of robust flight control for a quadrotor (UAV) equipped with the popular PX4¹ controller. Our objective is to experimentally assess how the optimization of the reference trajectory w.r.t. these sensitivity metrics can improve the closed-loop system performance against model uncertainties commonly affecting the quadrotor systems. To accomplish this, we present a series of experiments designed to validate our optimization approach on two distinct trajectories, with the primary aim of assessing its precision in guiding the quadrotor through the center of a window at relatively high speeds. This approach provides some interesting insights for increasing the closed-loop robustness of the robot state and inputs against physical parametric uncertainties that may degrade the system's performance.

## I. INTRODUCTION

Aerial robotics has experienced a growing interest propelled by advancements in research, resulting in numerous practical uses. A primary concern with aerial robots is to improve system autonomy and ensure safety during motion tasks. Nevertheless, attaining high levels of autonomy for flying robots remains an enduring challenge due to the need to operate in unpredictable and uncertain real-world conditions. Despite efforts to create precise models, real-world complexities introduce several uncertainties among which inaccuracies in the model parameters, which can potentially disrupt the system behavior during task execution. The pursuit of greater autonomy, precision, and safety in aerial robotics requires bridging the gap between theoretical models and practical real-world conditions. Adaptive [1]–[3] or robust control [4], [5] are typical methods to deal with parametric uncertainty, either through online parameter

<figure><img src="image_3.png" /><figcaption>Fig. 1. Drone trajectory tracking under parametric uncertainties (green) to pass through a target $s_d(t_w)$ (blue sphere). Non-optimized (INIT, left) and optimized (OPTa, right) trajectories are compared. The closest positions reached to the desired target after each experiment (small red sphere) are visualized in PyBullet where the drone passes through a circular window at a relatively high speed. A video of the experiments is available at https://youtu.be/QFnrQ_O2BiU.</figcaption></figure>

estimation or by introducing trade-offs between performance and robustness vs. parametric uncertainty. Another approach involves employing Model Predictive Control, as discussed in [6], [7], which predicts the system behavior using a dynamic model. However, the MPC effectiveness depends on the model, and thus parameters, accuracy. Uncertainties in these parameters can significantly impact both the controller performance and the robot behavior. Another possibility is to plan feedforward trajectories with minimal state sensitivity, as in [8]–[10]. However, these approaches operate in an open-loop fashion and thus do not consider the presence and strengths/weaknesses of the motion controller that is eventually implemented on the robot.

Recent strategies for addressing the parameter uncertainty problem have been introduced in [11]–[15] where metrics based on closed-loop state/input sensitivity assess how uncertainties in robot model affect robots behavior during closed-loop control. To enhance robustness vs. parametric uncertainties, one can plan a feedforward desired trajectory that minimizes these sensitivity metrics, creating a motion plan that is inherently robust. These concepts have been further developed in [16] for optimal initialization of an energy tank in the context of passivity-based control, and in [17] where an extension has been proposed to evaluate the impact of the controller choice, gain tuning and shape of reference trajectory for minimizing the parametric sen-

<footnote n="1">¹A. Srour and P. Robuffo Giordano are with CNRS, Univ Rennes, Inria, IRISA, Campus de Beaulieu, 35042 Rennes Cedex, France. email: {ali.srour, prg}@irisa.fr</footnote>

<footnote n="2">²S. Marcellini is with the PRISMA Lab, Department of Electrical Engineering and Information Technology, University of Naples Federico II, Via Claudio 21, Naples, 80125, Italy. email: salvatore.marcellini@unina.it</footnote>

<footnote n="3">³T. Belvedere and A. Franchi are with the Department of Computer, Control and Management Engineering, Sapienza University of Rome, 00185 Rome, Italy. email: belvedere@diag.uniroma.it ; antonio.franchi@uniroma.it</footnote>

<footnote n="4">⁴M. Cognetti is with LAAS-CNRS, Université de Toulouse, CNRS, UPS, Toulouse, France LAAS-CNRS, email: mcognetti@laas.fr</footnote>

<footnote n="5">⁵A. Franchi is also with the Robotics and Mechatronics Department, Electrical Engineering, Mathematics, Computer Science (EEMCS) Faculty, University of Twente, 7500 AE Enschede, The Netherlands. email: a.franchi@utwente.nl</footnote>

<footnote>This work was supported by the project ANR-20-CE33-0003 "CAMP", the Horizon EU research and innovation programme [grant agreement No. 101120732] AUTOASSESS, and the Chaire de Professeur Junior grant no. ANR-22-CPJ1-0064-01.</footnote>

<footnote n="1">¹https://px4.io/</footnote>
sitivity for quadrotor flight control. Similarly to some of these previous works, [18] proposes an offline optimization scheme in which ellipsoidal tubes on the state are used to provide approximate robustness of state constraints against disturbances. Notably, the propagation of the uncertainty ellipsoids exploited in [14]–[17] and used in this work takes a different expression due to being oriented towards parametric uncertainties instead of using a stochastic modeling of disturbances.

While these methods have shown promise in simulated case studies, only in [16] the notion of closed-loop state/input sensitivity was applied to real-world experiments with a robotic manipulator. Notably, there have been no real implementations using quadrotors executing a motion task. A major contribution of this work is to provide for the first time a quadrotor-focused experimental assessment and validation of a sensitivity-based trajectory generation. The employed quadrotor is controlled by the widely-used open-source autopilot PX4. While PX4 is used by numerous research groups, companies, and enthusiasts for its valuable features, there are several instances where its control performance can fall short of meeting user requirements. Users often attempt to enhance control by customizing the autopilot, sometimes opting for a model-based approach. However, such customization is not straightforward and demands a strong grasp of firmware architecture and proficient programming skills. Our objective is then to employ the closed-loop state/input sensitivity metric (and derived quantities) on a drone controlled by a PX4 default controller to assess how a proper reference trajectory planning, designed to be robust against parametric uncertainties, can enhance the performance of a standard drone.

The rest of the paper is structured as follows: In Sect. II, we recall the main notions of closed-loop state/input sensitivity. In Sect. III, we present the reader with the optimization problem proposed with the details of the quadrotor model and tracking controller used. Sect. IV discusses the simulation and the experimental results on the controller performance, and Sect. V concludes the paper.

## II. PRELIMINARIES

In this section, we recall for the reader's convenience the main notions related to the closed-loop state/input sensitivity introduced in [11], [12] and recently exploited in [13]–[17]. We consider a robot model

$$ \dot{x} = f(x, u, p) \quad (1) $$

where $x \in \mathbb{R}^{n_q}$ is the state, $u \in \mathbb{R}^{n_u}$ the control inputs and $p \in \mathbb{R}^{n_p}$ a vector of (possibly uncertain) model parameters. Letting $s(x) \in \mathbb{R}^{n_s}$ be a quantity of interest (e.g., the position of the robot center), we consider the tracking task of a desired trajectory $s_d(a, t)$ defined for $t \in [t_0, t_f]$ and function of a finite set of trajectory parameters $a \in \mathbb{R}^{n_a}$. The tracking task is realized by a controller having the following generic form

$$ \left\{ \begin{array}{l} \dot{\xi} = g(\xi, x, a, p_c, k_c, t) \\ u = h(\xi, x, a, p_c, k_c, t), \end{array} \right. \quad (2) $$

which is evaluated at a nominal value $p_c$ for the uncertain parameters $p$. Vector $\xi \in \mathbb{R}^{n_\xi}$ represents the possible internal

controller states (e.g., an integral action), and $k_c \in \mathbb{R}^{n_k}$ is the vector of control gains.

Following [11], [12], we define the state sensitivity for the closed-loop system (1–2) as

$$ \Pi(t) = \left. \frac{\partial x(t)}{\partial p} \right|_{p=p_c} \quad (3) $$

and the input sensitivity as

$$ \Theta(t) = \left. \frac{\partial u(t)}{\partial p} \right|_{p=p_c} \quad (4) $$

Matrix $\Pi(t)$ quantifies how variations of the parameters $p$ around a nominal value $p_c$ will affect the evolution of the state $x$ (in closed-loop). Analogously, matrix $\Theta(t)$ relates variations of $p$ to variations of the inputs $u$. Although a closed-form expression for matrices $\Pi(t)$ and $\Theta(t)$ is in general not available, it is possible to easily obtain these two quantities via forward integration of a differential equation along the system trajectories [11], [12].

Matrices $\Pi(t)$ and $\Theta(t)$ can then be used for several purposes in the context of trajectory optimization, for instance for obtaining suitable sensitivity metrics to be optimized or an estimation of the envelope of 'perturbed trajectories' for the states/inputs. To this end, assume that each parameter $p_i$ can vary in a given range $\delta p_i$ centered at a nominal $p_{c_i}$

$$ p_i \in [p_{c_i} - \delta p_i, p_{c_i} + \delta p_i] \quad (5) $$

and define the diagonal weight matrix $W = \text{diag}(\delta p_i^2)$. Letting $\Delta p = p - p_c$, an ellipsoid in parameter space centered at $p_c$ and with semi-axes $\delta p_i$ has equation

$$ \Delta p^T W^{-1} \Delta p = 1. \quad (6) $$

Following the derivations in [14], from (6) and (3–4) one can obtain the corresponding ellipsoids in state space

$$ \Delta x^T (\Pi W \Pi^T)^{-1} \Delta x = 1 \quad (7) $$

and in input space

$$ \Delta u^T (\Theta W \Theta^T)^{-1} \Delta u = 1, \quad (8) $$

where $\Delta x = x - x_{nom}$ and $x_{nom}$ is the state evolution of (1–2) in the unperturbed case $p = p_c$, and analogously for $\Delta u = u - u_{nom}$.

The state and input space ellipsoids can be exploited for defining a “weighted sensitivity norm” by considering the eigenvalues $\lambda_i$ of the kernel matrix $\Pi W \Pi^T$ in (7). In particular, in [14] and in this work we consider the following matrix norm

$$ \|\Pi\|_W = \max(\lambda_i (\Pi W \Pi^T)) \quad (9) $$

which represents the largest (worst-case) deviation of the state $x$ assuming a parametric uncertainty as in (5). Furthermore, one can also exploit (7–8) for obtaining the tubes of perturbed trajectories for the individual components of the states and the inputs. By referring to [14] for all details, for each direction of interest in the input space, one can obtain the “tube radius” $r_i(t)$ such that

$$ u_{nom,i}(t) - r_i(t) \le u_i(t) \le u_{nom,i}(t) + r_i(t). \quad (10) $$
sitivity for quadrotor flight control. Similarly to some of these previous works, [18] proposes an offline optimization scheme in which ellipsoidal tubes on the state are used to provide approximate robustness of state constraints against disturbances. Notably, the propagation of the uncertainty ellipsoids exploited in [14]–[17] and used in this work takes a different expression due to being oriented towards parametric uncertainties instead of using a stochastic modeling of disturbances.

While these methods have shown promise in simulated case studies, only in [16] the notion of closed-loop state/input sensitivity was applied to real-world experiments with a robotic manipulator. Notably, there have been no real implementations using quadrotors executing a motion task. A major contribution of this work is to provide for the first time a quadrotor-focused experimental assessment and validation of a sensitivity-based trajectory generation. The employed quadrotor is controlled by the widely-used open-source autopilot PX4. While PX4 is used by numerous research groups, companies, and enthusiasts for its valuable features, there are several instances where its control performance can fall short of meeting user requirements. Users often attempt to enhance control by customizing the autopilot, sometimes opting for a model-based approach. However, such customization is not straightforward and demands a strong grasp of firmware architecture and proficient programming skills. Our objective is then to employ the closed-loop state/input sensitivity metric (and derived quantities) on a drone controlled by a PX4 default controller to assess how a proper reference trajectory planning, designed to be robust against parametric uncertainties, can enhance the performance of a standard drone.

The rest of the paper is structured as follows: In Sect. II, we recall the main notions of closed-loop state/input sensitivity. In Sect. III, we present the reader with the optimization problem proposed with the details of the quadrotor model and tracking controller used. Sect. IV discusses the simulation and the experimental results on the controller performance, and Sect. V concludes the paper.

## II. PRELIMINARIES

In this section, we recall for the reader's convenience the main notions related to the closed-loop state/input sensitivity introduced in [11], [12] and recently exploited in [13]–[17]. We consider a robot model

$$ \dot{x} = f(x, u, p) \quad (1) $$

where $x \in \mathbb{R}^{n_q}$ is the state, $u \in \mathbb{R}^{n_u}$ the control inputs and $p \in \mathbb{R}^{n_p}$ a vector of (possibly uncertain) model parameters. Letting $s(x) \in \mathbb{R}^{n_s}$ be a quantity of interest (e.g., the position of the robot center), we consider the tracking task of a desired trajectory $s_d(a, t)$ defined for $t \in [t_0, t_f]$ and function of a finite set of trajectory parameters $a \in \mathbb{R}^{n_a}$. The tracking task is realized by a controller having the following generic form

$$ \left\{ \begin{array}{l} \dot{\xi} = g(\xi, x, a, p_c, k_c, t) \\ u = h(\xi, x, a, p_c, k_c, t), \end{array} \right. \quad (2) $$

which is evaluated at a nominal value $p_c$ for the uncertain parameters $p$. Vector $\xi \in \mathbb{R}^{n_\xi}$ represents the possible internal

controller states (e.g., an integral action), and $k_c \in \mathbb{R}^{n_k}$ is the vector of control gains.

Following [11], [12], we define the state sensitivity for the closed-loop system (1–2) as

$$ \Pi(t) = \left. \frac{\partial x(t)}{\partial p} \right|_{p=p_c} \quad (3) $$

and the input sensitivity as

$$ \Theta(t) = \left. \frac{\partial u(t)}{\partial p} \right|_{p=p_c} \quad (4) $$

Matrix $\Pi(t)$ quantifies how variations of the parameters $p$ around a nominal value $p_c$ will affect the evolution of the state $x$ (in closed-loop). Analogously, matrix $\Theta(t)$ relates variations of $p$ to variations of the inputs $u$. Although a closed-form expression for matrices $\Pi(t)$ and $\Theta(t)$ is in general not available, it is possible to easily obtain these two quantities via forward integration of a differential equation along the system trajectories [11], [12].

Matrices $\Pi(t)$ and $\Theta(t)$ can then be used for several purposes in the context of trajectory optimization, for instance for obtaining suitable sensitivity metrics to be optimized or an estimation of the envelope of 'perturbed trajectories' for the states/inputs. To this end, assume that each parameter $p_i$ can vary in a given range $\delta p_i$ centered at a nominal $p_{c_i}$

$$ p_i \in [p_{c_i} - \delta p_i, p_{c_i} + \delta p_i] \quad (5) $$

and define the diagonal weight matrix $W = \text{diag}(\delta p_i^2)$. Letting $\Delta p = p - p_c$, an ellipsoid in parameter space centered at $p_c$ and with semi-axes $\delta p_i$ has equation

$$ \Delta p^T W^{-1} \Delta p = 1. \quad (6) $$

Following the derivations in [14], from (6) and (3–4) one can obtain the corresponding ellipsoids in state space

$$ \Delta x^T (\Pi W \Pi^T)^{-1} \Delta x = 1 \quad (7) $$

and in input space

$$ \Delta u^T (\Theta W \Theta^T)^{-1} \Delta u = 1, \quad (8) $$

where $\Delta x = x - x_{nom}$ and $x_{nom}$ is the state evolution of (1–2) in the unperturbed case $p = p_c$, and analogously for $\Delta u = u - u_{nom}$.

The state and input space ellipsoids can be exploited for defining a “weighted sensitivity norm” by considering the eigenvalues $\lambda_i$ of the kernel matrix $\Pi W \Pi^T$ in (7). In particular, in [14] and in this work we consider the following matrix norm

$$ \|\Pi\|_W = \max(\lambda_i (\Pi W \Pi^T)) \quad (9) $$

which represents the largest (worst-case) deviation of the state $x$ assuming a parametric uncertainty as in (5). Furthermore, one can also exploit (7–8) for obtaining the tubes of perturbed trajectories for the individual components of the states and the inputs. By referring to [14] for all details, for each direction of interest in the input space, one can obtain the “tube radius” $r_i(t)$ such that

$$ u_{nom,i}(t) - r_i(t) \le u_i(t) \le u_{nom,i}(t) + r_i(t). \quad (10) $$
where $u_{nom,i}(t)$ is the behavior of the input $u_i(t)$ in the unperturbed case $p = p_c$. Equation (10) bounds from above/below the envelope of perturbed inputs when the parameter uncertainty is bounded as in (5), and an analogous upper/lower bound can also be obtained for the generic state component $x_i(t)$.

### III. OPTIMIZATION PROBLEM

In this work, we consider the following trajectory optimization problem

$$ 
\begin{aligned}
\boldsymbol{a}^* &= \arg\min_{\boldsymbol{a}} \|\boldsymbol{\Pi}(t_w)\|_W \\
&\text{s.t.} \quad \boldsymbol{M}\boldsymbol{a} = \boldsymbol{b} \\
U_{\text{min},i} &\le u_{\text{nom},i}(t) - r_i(t) \quad \forall i \forall t \in [t_0, t_f] \\
u_{\text{nom},i}(t) &+ r_i(t) \le U_{\text{max},i} \quad \forall i \forall t \in [t_0, t_f].
\end{aligned}
\quad (11) $$

We seek the optimal value $\boldsymbol{a}^*$ of the shape parameter $\boldsymbol{a}$ of the reference trajectory $s_d(\boldsymbol{a}, t)$ for minimizing the weighted norm (9) at a specific time $t_w$ (e.g., for passing through a desired point like the center of a window). Minimization of this cost will increase the robustness (thus reducing the sensitivity) of the closed-loop system against parameter uncertainties at $t_w$. The constraints consist of given initial/final conditions for $s_d(\boldsymbol{a}, t)$, represented by the linear constraints $Ma = b$, and constraints that bound the envelope of perturbed inputs within actuation limits $U_{\text{min},i} \le U_{\text{max},i}$, ensuring that the tracking of the optimized reference trajectory will be feasible for any value of the uncertain parameters $p$ in the range (5). Note that these constraints leverage the “input tubes” as described in (10). Of course, other objective functions can be considered such as the integral of the sensitivity norm along the trajectory to enhance tracking accuracy during motion. Furthermore, one can easily include extra constraints like obstacle avoidance by exploiting the tubes on the states, as shown in [15]. We note that this optimization problem is non-convex (as any other trajectory optimization problem of this type), and therefore one can only guarantee convergence to a local minimum for a given initial guess.

#### A. Quadrotor Model

Concerning the quadrotor dynamics, we adopted the model from [14] that considers a displaced center of mass (CoM). Let $\mathcal{F}_W = \{O_W, x_W, y_W, z_W\}$ denote the world inertial frame, and $\mathcal{F}_B = \{O_B, x_B, y_B, z_B\}$ represent the body frame attached to the drone geometric center. We define: $\boldsymbol{r} = (x, y, z) \in \mathbb{R}^3$ as the drone position, $\boldsymbol{v} = (v_x, v_y, v_z) \in \mathbb{R}^3$ as its linear velocity in the world frame $\mathcal{F}_W$, $\boldsymbol{q} = (q_w, q_x, q_y, q_z) \in \mathbb{S}^3$ as the unit-norm quaternion representing the orientation of $\mathcal{F}_B$ relative to $\mathcal{F}_W$, and $\boldsymbol{\omega} = (\omega_x, \omega_y, \omega_z) \in \mathbb{R}^3$ as the angular velocity of $\mathcal{F}_B$ with respect to $\mathcal{F}_W$, expressed in $\mathcal{F}_B$. The quadrotor state vector is $\boldsymbol{x} = (\boldsymbol{r}, \boldsymbol{v}, \boldsymbol{q}, \boldsymbol{\omega}) \in \mathbb{R}^6 \times \mathbb{S}^3 \times \mathbb{R}^3$.

Let $w_i$ be the squared velocity of the $i$-th propeller and define the quadrotor control input $\boldsymbol{u} = (w_1, \dots, w_4)$. An allocation matrix is used to relate the inputs $\boldsymbol{u}$ (i.e., the squared propeller speeds) to the thrust/torques ($f, \tau$)

$$ \begin{bmatrix} f \\ \tau \end{bmatrix} = k_f \begin{bmatrix} 1 & 1 & 1 & 1 \\ 0 & l & 0 & -l \\ -l & 0 & l & 0 \\ k_m & -k_m & k_m & -k_m \end{bmatrix} \boldsymbol{u} = T \boldsymbol{u} \quad (12) $$

<figure><img src="image_3.png" /><figcaption>Fig. 2. Block diagram of the PX4 controller, composed of four stages: the position controller (red), the attitude generator (blue), the attitude controller (yellow), and the control allocation (green).</figcaption></figure>

where $l$ is the arm length of the quadrotor arm, and $k_f$ and $k_m$ are the thrust and the drag aerodynamic coefficients of the propellers [19], [20]. We consider that the quadrotor center of mass $G_B$ is displaced w.r.t. the geometric center $O_B$ by a displacement denoted as $g_C = (g_x, g_y, g_z)$ in $\mathcal{F}_B$. The parameters considered uncertain are then the set$^2$ $\boldsymbol{p} = (k_f, g_x, g_y, g_z, m) \in \mathbb{R}^5$. The total force $\boldsymbol{f}_{tot}$ acting on the quadrotor in $\mathcal{F}_B$ includes the propeller thrust $f$, gravitational effects, and an additional fictitious force due to displacement $g_C$:

$$ \boldsymbol{f}_{tot} = f z_W - m \boldsymbol{R}(\boldsymbol{q})^T z_W - m[\boldsymbol{\omega}]_\times [\boldsymbol{\omega}]_\times g_C $$

For the total torque $\tau_{tot}$ (expressed in $\mathcal{F}_B$), we have:

$$ \tau_{tot} = \tau - m[g_C]_\times \boldsymbol{R}(\boldsymbol{q})^T z_W - [\boldsymbol{\omega}]_\times \boldsymbol{J}\boldsymbol{\omega} $$

where $\tau$ represents the propeller torque as in (12), $\boldsymbol{R}(\boldsymbol{q}) \in SO(3)$ is the rotational matrix associated with the quaternion $\boldsymbol{q}$ and $\boldsymbol{J}$ stands for the body frame inertia matrix about the geometric center $\mathcal{F}_B$. By defining the spatial inertia matrix

$$ M = \begin{bmatrix} m \boldsymbol{I}_3 & -m[g_C]_\times \\ m[g_C]_\times & \boldsymbol{J} \end{bmatrix} $$

one can finally obtain the following relation:

$$ \begin{bmatrix} \boldsymbol{\nu} \\ \boldsymbol{\alpha} \end{bmatrix} = M^{-1} \begin{bmatrix} f_{tot} \\ \tau_{tot} \end{bmatrix} $$

which leads to the quadrotor model with displaced center of mass

$$ \begin{cases} \dot{\boldsymbol{r}} = \boldsymbol{R}(\boldsymbol{q})\boldsymbol{v} \\ \dot{\boldsymbol{v}} = \boldsymbol{\nu}(\boldsymbol{x}, \boldsymbol{u}, \boldsymbol{p}) \\ \dot{\boldsymbol{q}} = \frac{1}{2} \begin{bmatrix} 0 \\ \boldsymbol{\omega} \end{bmatrix} \otimes \boldsymbol{q} \\ \dot{\boldsymbol{\omega}} = \boldsymbol{\alpha}(\boldsymbol{x}, \boldsymbol{u}, \boldsymbol{p}) \end{cases} \quad (13) $$

#### B. PX4 Controller

It is well known that the position $\boldsymbol{r}$ and the yaw angle $\boldsymbol{\psi}$ are flat outputs for the quadrotor model (13), as explained in, e.g., [21]. Based on this fact, the PX4 controller is designed to track a desired position $\boldsymbol{r}_d(t)$ and a yaw angle $\boldsymbol{\psi}_d(t)$ trajectories by generating suitable propeller speeds $\boldsymbol{u}$. More in detail, the controller has a cascaded structure consisting of four stages (as illustrated in Fig. 2): (i) the position controller in which the desired position $\boldsymbol{r}_d$ is transformed into an equivalent acceleration setpoint $a_{sp}$, (ii) the attitude generator responsible for producing both a combined thrust $f$ and an attitude setpoint $q_{sp}$ derived from the desired yaw angle $\boldsymbol{\psi}_d$ and the previously computed acceleration setpoint,

<footnote>$^2$We did not include $k_m$ since uncertainties in this parameter have a negligible impact compared to the other parameters as shown in, e.g., [17].</footnote>
where $u_{nom,i}(t)$ is the behavior of the input $u_i(t)$ in the unperturbed case $p = p_c$. Equation (10) bounds from above/below the envelope of perturbed inputs when the parameter uncertainty is bounded as in (5), and an analogous upper/lower bound can also be obtained for the generic state component $x_i(t)$.

### III. OPTIMIZATION PROBLEM

In this work, we consider the following trajectory optimization problem

$$ 
\begin{aligned}
\mathbf{a}^* &= \arg \min_{\mathbf{a}} \|\mathbf{\Pi}(t_w)\|_W \\
&\quad \text{s.t. } \mathbf{M}\mathbf{a} = \mathbf{b} \\
U_{\text{min},i} &\le u_{\text{nom},i}(t) - r_i(t) \quad \forall i \forall t \in [t_0, t_f] \\
u_{\text{nom},i}(t) + r_i(t) &\le U_{\text{max},i} \quad \forall i \forall t \in [t_0, t_f].
\end{aligned} 
\quad (11) $$

We seek the optimal value $\mathbf{a}^*$ of the shape parameter $\mathbf{a}$ of the reference trajectory $s_d(\mathbf{a}, t)$ for minimizing the weighted norm (9) at a specific time $t_w$ (e.g., for passing through a desired point like the center of a window). Minimization of this cost will increase the robustness (thus reducing the sensitivity) of the closed-loop system against parameter uncertainties at $t_w$. The constraints consist of given initial/final conditions for $s_d(\mathbf{a}, t)$, represented by the linear constraints $Ma = b$, and constraints that bound the envelope of perturbed inputs within actuation limits $U_{\text{min},i} \le U_{\text{max},i}$, ensuring that the tracking of the optimized reference trajectory will be feasible for any value of the uncertain parameters $p$ in the range (5). Note that these constraints leverage the “input tubes” as described in (10). Of course, other objective functions can be considered such as the integral of the sensitivity norm along the trajectory to enhance tracking accuracy during motion. Furthermore, one can easily include extra constraints like obstacle avoidance by exploiting the tubes on the states, as shown in [15]. We note that this optimization problem is non-convex (as any other trajectory optimization problem of this type), and therefore one can only guarantee convergence to a local minimum for a given initial guess.

#### A. Quadrotor Model

Concerning the quadrotor dynamics, we adopted the model from [14] that considers a displaced center of mass (CoM). Let $\mathcal{F}_W = \{O_W, x_W, y_W, z_W\}$ denote the world inertial frame, and $\mathcal{F}_B = \{O_B, x_B, y_B, z_B\}$ represent the body frame attached to the drone geometric center. We define: $\mathbf{r} = (x, y, z) \in \mathbb{R}^3$ as the drone position, $\mathbf{v} = (v_x, v_y, v_z) \in \mathbb{R}^3$ as its linear velocity in the world frame $\mathcal{F}_W$, $\mathbf{q} = (q_w, q_x, q_y, q_z) \in \mathbb{S}^3$ as the unit-norm quaternion representing the orientation of $\mathcal{F}_B$ relative to $\mathcal{F}_W$, and $\mathbf{\omega} = (\omega_x, \omega_y, \omega_z) \in \mathbb{R}^3$ as the angular velocity of $\mathcal{F}_B$ with respect to $\mathcal{F}_W$, expressed in $\mathcal{F}_B$. The quadrotor state vector is $\mathbf{x} = (\mathbf{r}, \mathbf{v}, \mathbf{q}, \mathbf{\omega}) \in \mathbb{R}^6 \times \mathbb{S}^3 \times \mathbb{R}^3$.

Let $w_i$ be the squared velocity of the $i$-th propeller and define the quadrotor control input $\mathbf{u} = (w_1, \dots, w_4)$. An allocation matrix is used to relate the inputs $\mathbf{u}$ (i.e., the squared propeller speeds) to the thrust/torques ($f, \tau$)

$$ \begin{bmatrix} f \\ \tau \end{bmatrix} = k_f \begin{bmatrix} 1 & 1 & 1 & 1 \\ 0 & l & 0 & -l \\ -l & 0 & l & 0 \\ k_m & -k_m & k_m & -k_m \end{bmatrix} \mathbf{u} = T \mathbf{u} \quad (12) $$

<figure><img src="image_4.png" /><figcaption>Fig. 2. Block diagram of the PX4 controller, composed of four stages: the position controller (red), the attitude generator (blue), the attitude controller (yellow), and the control allocation (green).</figcaption></figure>

where $l$ is the arm length of the quadrotor arm, and $k_f$ and $k_m$ are the thrust and the drag aerodynamic coefficients of the propellers [19], [20]. We consider that the quadrotor center of mass $G_B$ is displaced w.r.t. the geometric center $O_B$ by a displacement denoted as $g_C = (g_x, g_y, g_z)$ in $\mathcal{F}_B$. The parameters considered uncertain are then the set$^2$ $\mathbf{p} = (k_f, g_x, g_y, g_z, m) \in \mathbb{R}^5$. The total force $\mathbf{f}_{tot}$ acting on the quadrotor in $\mathcal{F}_B$ includes the propeller thrust $f$, gravitational effects, and an additional fictitious force due to displacement $g_C$:

$$ \mathbf{f}_{tot} = f z_W - m \mathbf{R}(\mathbf{q})^T z_W - m[\mathbf{\omega}]_\times [\mathbf{\omega}]_\times g_C $$

For the total torque $\tau_{tot}$ (expressed in $\mathcal{F}_B$), we have:

$$ \tau_{tot} = \tau - m g [g_C]_\times \mathbf{R}(\mathbf{q})^T z_W - [\mathbf{\omega}]_\times \mathbf{J} \mathbf{\omega} $$

where $\tau$ represents the propeller torque as in (12), $\mathbf{R}(\mathbf{q}) \in SO(3)$ is the rotational matrix associated with the quaternion $\mathbf{q}$ and $\mathbf{J}$ stands for the body frame inertia matrix about the geometric center $\mathcal{F}_B$. By defining the spatial inertia matrix

$$ M = \begin{bmatrix} m \mathbf{I}_3 & -m[g_C]_\times \\ m[g_C]_\times & \mathbf{J} \end{bmatrix} $$

one can finally obtain the following relation:

$$ \begin{bmatrix} \nu \\ \alpha \end{bmatrix} = M^{-1} \begin{bmatrix} f_{tot} \\ \tau_{tot} \end{bmatrix} $$

which leads to the quadrotor model with displaced center of mass

$$ \begin{cases} \dot{\mathbf{r}} = \mathbf{R}(\mathbf{q})\mathbf{v} \\ \dot{\mathbf{v}} = \nu(\mathbf{x}, \mathbf{u}, \mathbf{p}) \\ \dot{\mathbf{q}} = \frac{1}{2} \begin{bmatrix} 0 \\ \mathbf{\omega} \end{bmatrix} \otimes \mathbf{q} \\ \dot{\mathbf{\omega}} = \alpha(\mathbf{x}, \mathbf{u}, \mathbf{p}) \end{cases} \quad (13) $$

#### B. PX4 Controller

It is well known that the position $\mathbf{r}$ and the yaw angle $\psi$ are flat outputs for the quadrotor model (13), as explained in, e.g., [21]. Based on this fact, the PX4 controller is designed to track a desired position $\mathbf{r}_d(t)$ and a yaw angle $\psi_d(t)$ trajectories by generating suitable propeller speeds $\mathbf{u}$. More in detail, the controller has a cascaded structure consisting of four stages (as illustrated in Fig. 2): (i) the position controller in which the desired position $\mathbf{r}_d$ is transformed into an equivalent acceleration setpoint $a_{sp}$, (ii) the attitude generator responsible for producing both a combined thrust $f$ and an attitude setpoint $q_{sp}$ derived from the desired yaw angle $\psi_d$ and the previously computed acceleration setpoint,

<footnote>$^2$We did not include $k_m$ since uncertainties in this parameter have a negligible impact compared to the other parameters as shown in, e.g., [17].</footnote>
(iii) the attitude controller determining the desired body torques and, finally, (iv) the control allocation where thrust and body torques are transformed in propeller speeds via the inverse of the nominal allocation matrix (12).

Following the available documentation and open-source code<fnref n="3" />, we have implemented an equivalent continuous time controller and integrated it into our sensitivity-aware planning scheme. Note that, in the PX4 controller, the inertial and the body frames are expressed using the North-East-Down (NED) convention, so trivial conversions are needed if one chooses to express the dynamic model in different frames.

The position controller in the PX4 consists of a P and a PID action on the position and velocity errors, respectively. The latter introduces two 3-dimensional internal states $\xi_v$ and $\xi_a$ to realize the integral and the low-pass-filtered derivative actions. Denoting with $\Omega_p$ the low-pass filter cutoff pulsation and with $P_r, P_v, I_v, D_v$ the diagonal matrix gains, the position controller equations can be expressed as

$$ 
\begin{aligned}
\dot{\xi}_v &= \dot{r}_d - v \\
\dot{\xi}_a &= -\Omega_p \xi_a - \Omega_p^2 v \\
a_{sp} &= \ddot{r}_d + P_v(\dot{r}_d - v + P_r(r_d - r)) \\
&\quad - D_v(\xi_a + \Omega_p v) + I_v \xi_v.
\end{aligned}
$$

The attitude generator receives the acceleration setpoint $a_{sp} = (\ddot{x}_{sp}, \ddot{y}_{sp}, \ddot{z}_{sp})$ and computes the collective thrust $f$. Let $b_z = \frac{(-\ddot{x}_{sp}, -\ddot{y}_{sp}, g)}{\|(-\ddot{x}_{sp}, -\ddot{y}_{sp}, g)\|}$ and $t_{sp} = \frac{b_z}{b_{z,z}}(\ddot{z}_{sp} \frac{h_t}{g} - h_t)$, where $h_t \in (0, 1)$ denotes the nominal normalized hovering thrust, so that the computed thrust is $f = -\|t_{sp}\|$. The attitude setpoint $q_d$ is then computed by aligning the z-axis of the body frame to the desired thrust $t_{sp}$ and rotating of an angle $\psi_d$ around such axis [22]. To generate the body torques, let the quaternion error be $q_e = q^{-1} \otimes q_{sp}$. The angular rate setpoint is then computed proportionally to the error as $\omega_{sp} = 2P_q \text{sign}(q_{e,w})q_{e,xyz}$, with $P_q$ denoting the proportional gains and $q_e = [q_{e,w}, q_{e,xyz}]^T$. Finally, the equations of the PID controller tracking the attitude rate $\omega_{sp}$ are

$$ 
\begin{aligned}
\dot{\xi}_{\omega} &= \omega_{sp} - \omega \\
\dot{\xi}_{\alpha} &= -\Omega_{a} \xi_{\alpha} - \Omega_{a}^{2} \omega \\
\tau &= P_{\omega}(\omega_{sp} - \omega) - D_{\omega}(\xi_{\alpha} + \Omega_{a}\omega) + I_{\omega}\xi_{\omega},
\end{aligned}
$$

where $\xi_\omega$ and $\xi_\alpha$ are the 3-dimensional internal states of the controller, $\Omega_a$ is the cutoff pulsation of the low-pass filter, and $P_\omega, I_\omega, D_\omega$ are the diagonal matrix gains.

Lastly, the control input $u$ is reconstructed from the allocation matrix as

$$ u = T^{-1} \begin{bmatrix} f \\ \tau \end{bmatrix} $$

where, of course, the matrix $T$ from (12) is evaluated on the nominal values of the parameters $p_c$ (which may differ from the actual $p$ because of inaccuracies in the quadrotor model).

## IV. EXPERIMENTAL ANALYSIS

A series of experiments and simulations were conducted to assess the effectiveness of the proposed trajectory generation

obtained by solving (11). Two distinct trajectories $s_d(a, t) = (r_d(a, t), \psi_d(a, t))$, denoted as Traj1 and Traj2 in the following, were used to guide the drone through a circular window at a relatively high speed ($2.0 \le v \le 2.5$ m/s) (see Fig. 3) while complying with the initial/final state constraints (rest-to-rest motions) and input saturations as in (11). Note that the actual window was not introduced during the real experiments for safety reasons, but it was used in PyBullet<fnref n="4" /> with the real flight data for visualization.

We start by generating a first trajectory – that will be referred to as INIT in the following – constructed using piecewise Bezier curves, as discussed in [11], [12], [17], [23], by performing a preliminary trajectory optimization that minimizes the snap of $r_d(a, t)$ over the whole time interval with the aim of obtaining a smooth trajectory with minimal curvature changes and satisfying nominal constraints over the state and inputs. Thus, all initial non-optimized trajectories INIT use the minimum snap cost function defined as

$$ 
\begin{aligned}
\mathbf{a} = \arg \min_{\mathbf{a}} & \left( \int_{t_0}^{t_f} \left\| \frac{d^4 r_d(\mathbf{a}, \tau)}{dt^4} \right\|^{2} d\tau \right) \\
& \text{s.t. } M\mathbf{a} = \mathbf{b} \\
U_{\min,i} \le u_{\text{nom},i}(t) \le U_{\max,i} & \quad \forall i \forall t \in [t_0, t_f].
\end{aligned}
\quad (14)
$$

Our framework then modifies this trajectory by solving (11) with INIT as an initial guess. The resulting trajectory is denoted as $OPT_a$. More in detail, our framework is implemented in Python and utilizes the COBYLA [24] nonlinear optimizer from the nlopt toolbox by employing the symbolic toolbox for symbolic system representation and making use of the Jitcode [25] framework for in-time compilation of ordinary differential equations. Within this framework, optimizing trajectories with the PX4 controller typically requires 4 to 6 minutes per trajectory.

Our main goal is to evaluate how close the drone gets to a specific target location, labeled as $r_d(t_w)$, which is right at the center of a predefined window. We want to find out if the drone can safely go through the window, even when in presence of uncertainties in its parameters. Additionally, we want to know how close the drone can get to the center of the window. Traj1, having a duration of 7 seconds, requires a relatively low initial acceleration to reach the target. Conversely, Traj2, which has a duration of 5 seconds, requires a more significant initial acceleration to reach the target. Both trajectories ensure a minimum speed of 2 m/s when they reach the target.

### A. Simulation Results

After obtaining trajectories using our sensitivity Python framework, dynamic simulations have been carried out in Gazebo thanks to the software in the loop (SITL) feature of PX4, which allows to simulate the autopilot and communicate through a ROS 2 application. This application reads the desired trajectory from a csv file, and it sends the series of setpoints (position, velocity, and acceleration) to the drone.

Starting from the default model of the Iris quadrotor, we performed a simulation running with the nominal parameters. Then, we carried out $N_p = 11$ simulations using uncertain parameters $p$ generated by uniformly sampling the inner

<footnote n="3">³https://github.com/PX4/PX4-Autopilot</footnote>

<footnote n="4">⁴https://pybullet.org/</footnote>
(iii) the attitude controller determining the desired body torques and, finally, (iv) the control allocation where thrust and body torques are transformed in propeller speeds via the inverse of the nominal allocation matrix (12).

Following the available documentation and open-source code<fnref n="3" />, we have implemented an equivalent continuous time controller and integrated it into our sensitivity-aware planning scheme. Note that, in the PX4 controller, the inertial and the body frames are expressed using the North-East-Down (NED) convention, so trivial conversions are needed if one chooses to express the dynamic model in different frames.

The position controller in the PX4 consists of a P and a PID action on the position and velocity errors, respectively. The latter introduces two 3-dimensional internal states $\xi_v$ and $\xi_a$ to realize the integral and the low-pass-filtered derivative actions. Denoting with $\Omega_p$ the low-pass filter cutoff pulsation and with $P_r, P_v, I_v, D_v$ the diagonal matrix gains, the position controller equations can be expressed as

$$ 
\begin{aligned}
\dot{\xi}_v &= \dot{r}_d - v \\
\dot{\xi}_a &= -\Omega_p \xi_a - \Omega_p^2 v \\
a_{sp} &= \ddot{r}_d + P_v(\dot{r}_d - v + P_r(r_d - r)) \\
&\quad - D_v(\xi_a + \Omega_p v) + I_v \xi_v.
\end{aligned}
$$

The attitude generator receives the acceleration setpoint $a_{sp} = (\ddot{x}_{sp}, \ddot{y}_{sp}, \ddot{z}_{sp})$ and computes the collective thrust $f$. Let $b_z = \frac{(-\ddot{x}_{sp}, -\ddot{y}_{sp}, g)}{\|(-\ddot{x}_{sp}, -\ddot{y}_{sp}, g)\|}$ and $t_{sp} = \frac{b_z}{b_{z,z}}(\ddot{z}_{sp} \frac{h_t}{g} - h_t)$, where $h_t \in (0, 1)$ denotes the nominal normalized hovering thrust, so that the computed thrust is $f = -\|t_{sp}\|$. The attitude setpoint $q_d$ is then computed by aligning the z-axis of the body frame to the desired thrust $t_{sp}$ and rotating of an angle $\psi_d$ around such axis [22]. To generate the body torques, let the quaternion error be $q_e = q^{-1} \otimes q_{sp}$. The angular rate setpoint is then computed proportionally to the error as $\omega_{sp} = 2P_q \text{sign}(q_{e,w})q_{e,xyz}$, with $P_q$ denoting the proportional gains and $q_e = [q_{e,w}, q_{e,xyz}]^T$. Finally, the equations of the PID controller tracking the attitude rate $\omega_{sp}$ are

$$ 
\begin{aligned}
\dot{\xi}_{\omega} &= \omega_{sp} - \omega \\
\dot{\xi}_{\alpha} &= -\Omega_{a} \xi_{\alpha} - \Omega_{a}^{2} \omega \\
\tau &= P_{\omega}(\omega_{sp} - \omega) - D_{\omega}(\xi_{\alpha} + \Omega_{a}\omega) + I_{\omega}\xi_{\omega},
\end{aligned}
$$

where $\xi_{\omega}$ and $\xi_{\alpha}$ are the 3-dimensional internal states of the controller, $\Omega_a$ is the cutoff pulsation of the low-pass filter, and $P_{\omega}, I_{\omega}, D_{\omega}$ are the diagonal matrix gains.

Lastly, the control input $u$ is reconstructed from the allocation matrix as

$$ u = T^{-1} \begin{bmatrix} f \\ \tau \end{bmatrix} $$

where, of course, the matrix $T$ from (12) is evaluated on the nominal values of the parameters $p_c$ (which may differ from the actual $p$ because of inaccuracies in the quadrotor model).

## IV. EXPERIMENTAL ANALYSIS

A series of experiments and simulations were conducted to assess the effectiveness of the proposed trajectory generation

obtained by solving (11). Two distinct trajectories $s_d(a, t) = (r_d(a, t), \psi_d(a, t))$, denoted as Traj1 and Traj2 in the following, were used to guide the drone through a circular window at a relatively high speed ($2.0 \le v \le 2.5$ m/s) (see Fig. 3) while complying with the initial/final state constraints (rest-to-rest motions) and input saturations as in (11). Note that the actual window was not introduced during the real experiments for safety reasons, but it was used in PyBullet<fnref n="4" /> with the real flight data for visualization.

We start by generating a first trajectory – that will be referred to as INIT in the following – constructed using piecewise Bezier curves, as discussed in [11], [12], [17], [23], by performing a preliminary trajectory optimization that minimizes the snap of $r_d(a, t)$ over the whole time interval with the aim of obtaining a smooth trajectory with minimal curvature changes and satisfying nominal constraints over the state and inputs. Thus, all initial non-optimized trajectories INIT use the minimum snap cost function defined as

$$ 
\begin{aligned}
a &= \arg \min_a \left( \int_{t_0}^{t_f} \left\| \frac{d^4 r_d(a, \tau)}{dt^4} \right\|^{2} d\tau \right) \\
&\quad \text{s.t. } Ma = b \\
U_{\text{min},i} &\le u_{\text{nom},i}(t) \le U_{\text{max},i} \quad \forall i \forall t \in [t_0, t_f].
\end{aligned}
\quad (14) $$

Our framework then modifies this trajectory by solving (11) with INIT as an initial guess. The resulting trajectory is denoted as $OPT_a$. More in detail, our framework is implemented in Python and utilizes the COBYLA [24] nonlinear optimizer from the nlopt toolbox by employing the symbolic toolbox for symbolic system representation and making use of the Jitcode [25] framework for in-time compilation of ordinary differential equations. Within this framework, optimizing trajectories with the PX4 controller typically requires 4 to 6 minutes per trajectory.

Our main goal is to evaluate how close the drone gets to a specific target location, labeled as $r_d(t_w)$, which is right at the center of a predefined window. We want to find out if the drone can safely go through the window, even when in presence of uncertainties in its parameters. Additionally, we want to know how close the drone can get to the center of the window. Traj1, having a duration of 7 seconds, requires a relatively low initial acceleration to reach the target. Conversely, Traj2, which has a duration of 5 seconds, requires a more significant initial acceleration to reach the target. Both trajectories ensure a minimum speed of 2 m/s when they reach the target.

### A. Simulation Results

After obtaining trajectories using our sensitivity Python framework, dynamic simulations have been carried out in Gazebo thanks to the software in the loop (SITL) feature of PX4, which allows to simulate the autopilot and communicate through a ROS 2 application. This application reads the desired trajectory from a csv file, and it sends the series of setpoints (position, velocity, and acceleration) to the drone.

Starting from the default model of the Iris quadrotor, we performed a simulation running with the nominal parameters. Then, we carried out $N_p = 11$ simulations using uncertain parameters $p$ generated by uniformly sampling the inner

<footnote n="3">³https://github.com/PX4/PX4-Autopilot</footnote>

<footnote n="4">⁴https://pybullet.org/</footnote>
<figure><img src="image_4.png" /><figcaption>Fig. 3. Trajectory tracking results by PX4 controller for two trajectory types (1st row: Traj1, 2nd row: Traj2) in simulation (first two columns) and experiments (last two columns). All trajectories start from the black sphere at (-2, -2, 1). Cases include INIT and OPT<sub>a</sub>. Perturbed runs consist of N<sub>p</sub> green trajectories, while the nominal trajectory tracking (i.e. when **p** = **p**<sub>c</sub>) is denoted in red, all sharing an origin which is the black sphere. Parameters **p** were randomly drawn from (6) in Gazebo simulation where it was implemented by modifying the Iris URDF model. However, the experiments were carried out as in (Fig. 7) where actual modifications were done on the drone that resemble the perturbations that will affect parameters **p**. Red spheres at *t* = *t*<sub>w</sub> mark closest points to desired target *r*<sub>d</sub>(*t*<sub>w</sub>), forming a point cloud around it.</figcaption></figure>

<figure><img src="image_5.png" /><figcaption>Fig. 4. Histograms of two trajectory types (left: Traj1, right: Traj2) for both cases (INIT in blue, OPT<sub>a</sub> in red) showing the distances to the desired target (|r<sub>d</sub>(t<sub>w</sub>) − r(t<sub>w</sub>)|) in [m] and its average across N<sub>p</sub> runs in Gazebo.</figcaption></figure>

volume of the ellipsoid (6). This is done for Traj1 and Traj2 and for both INIT and OPT<sub>a</sub> cases. The adjustments of the parameters were made by modifying the Iris URDF model. The uncertainty intervals δp<sub>i</sub> for the matrix W are chosen relative to the nominal parameters p<sub>c</sub> as δk<sub>f</sub> = 0.1k<sub>f</sub>, δg<sub>x,y</sub> = 0.1l, δg<sub>z</sub> = 0.1h, and δm = 0.1m, where h is the distance between the top and bottom plates of the drone chassis. Thus, the optimization problem of (11) will aim at minimizing the sensitivity of the quadrotor position r(t<sub>w</sub>) at the instant of passing through the window center t<sub>w</sub> against variations in the parameters k<sub>f</sub>, g<sub>x</sub>, g<sub>y</sub>, g<sub>z</sub> and m.

In our initial assessment, as depicted in Fig. 3, we present the results for the $N_p$ “perturbed runs” for both Traj1 and Traj2 (1st and 2nd row, respectively) under the Gazebo label (first two columns). The plots report the different targets

reach r(t<sub>w</sub>) (red dots) that are the closest to r<sub>d</sub>(t<sub>w</sub>) (window center) for each of the perturbed simulations (in green). The result is a point cloud around the desired target r<sub>d</sub>(t<sub>w</sub>). It is worth noticing that, when there is no parameter uncertainty affecting the system (i.e. when **p** = **p**<sub>c</sub>) at nominal tracking (in red), the drone successfully traverses the window with an accuracy below 0.12 [m] for all the trajectories and in both the optimized and the non-optimized cases. However, the outcomes differ significantly for the perturbed simulations. Specifically, the non-optimized INIT case displays greater deviations from the desired target location r<sub>d</sub>(t<sub>w</sub>), when compared to the optimized OPT<sub>a</sub> case, as expected.

This difference is particularly pronounced in the 2nd trajectory, which is more challenging (due to its higher accelerations). By referring to Fig. 4, which displays the distance to the desired target for both trajectories, we observe that the INIT case shows a larger average deviation of 0.14 [m] for Traj1 and 0.3 [m] for Traj2, in contrast to the OPT<sub>a</sub> cases with averages of 0.09 [m] for Traj1 and 0.12 [m] for Traj2. Furthermore, the INIT case exhibits a significantly larger variance, with certain samples deviating 0.42 [m] in Traj1 and 0.75 [m] in Traj2. In contrast, OPT<sub>a</sub> samples tend to cluster around the average. These findings emphasize the effectiveness of the optimization problem presented in (11), which is able to capture how variations in the parameters affect deviations in the considered states.

## B. Experimental Results

After the validation of the optimization problem (11) in simulation, we performed real experiments using the Acanthis drone (see Fig. 5). Acanthis is an experimental
<figure><img src="image_5.png" /><figcaption>Fig. 3. Trajectory tracking results by PX4 controller for two trajectory types (1st row: Traj1, 2nd row: Traj2) in simulation (first two columns) and experiments (last two columns). All trajectories start from the black sphere at (-2, -2, 1). Cases include INIT and OPT<sub>a</sub>. Perturbed runs consist of N<sub>p</sub> green trajectories, while the nominal trajectory tracking (i.e. when **p** = **p**<sub>c</sub>) is denoted in red, all sharing an origin which is the black sphere. Parameters **p** were randomly drawn from (6) in Gazebo simulation where it was implemented by modifying the Iris URDF model. However, the experiments were carried out as in (Fig. 7) where actual modifications were done on the drone that resemble the perturbations that will affect parameters **p**. Red spheres at *t* = *t*<sub>w</sub> mark closest points to desired target *r*<sub>d</sub>(*t*<sub>w</sub>), forming a point cloud around it.</figcaption></figure>

<figure><img src="image_6.png" /><figcaption>Fig. 4. Histograms of two trajectory types (left: Traj1, right: Traj2) for both cases (INIT in blue, OPT<sub>a</sub> in red) showing the distances to the desired target (|r<sub>d</sub>(t<sub>w</sub>) − r(t<sub>w</sub>)|) in [m] and its average across N<sub>p</sub> runs in Gazebo.</figcaption></figure>

volume of the ellipsoid (6). This is done for Traj1 and Traj2 and for both INIT and OPT<sub>a</sub> cases. The adjustments of the parameters were made by modifying the Iris URDF model. The uncertainty intervals δp<sub>i</sub> for the matrix W are chosen relative to the nominal parameters p<sub>c</sub> as δk<sub>f</sub> = 0.1k<sub>f</sub>, δg<sub>x,y</sub> = 0.1l, δg<sub>z</sub> = 0.1h, and δm = 0.1m, where h is the distance between the top and bottom plates of the drone chassis. Thus, the optimization problem of (11) will aim at minimizing the sensitivity of the quadrotor position r(t<sub>w</sub>) at the instant of passing through the window center t<sub>w</sub> against variations in the parameters k<sub>f</sub>, g<sub>x</sub>, g<sub>y</sub>, g<sub>z</sub> and m.

In our initial assessment, as depicted in Fig. 3, we present the results for the $N_p$ “perturbed runs” for both Traj1 and Traj2 (1st and 2nd row, respectively) under the Gazebo label (first two columns). The plots report the different targets

reach r(t<sub>w</sub>) (red dots) that are the closest to r<sub>d</sub>(t<sub>w</sub>) (window center) for each of the perturbed simulations (in green). The result is a point cloud around the desired target r<sub>d</sub>(t<sub>w</sub>). It is worth noticing that, when there is no parameter uncertainty affecting the system (i.e. when **p** = **p**<sub>c</sub>) at nominal tracking (in red), the drone successfully traverses the window with an accuracy below 0.12 [m] for all the trajectories and in both the optimized and the non-optimized cases. However, the outcomes differ significantly for the perturbed simulations. Specifically, the non-optimized INIT case displays greater deviations from the desired target location r<sub>d</sub>(t<sub>w</sub>), when compared to the optimized OPT<sub>a</sub> case, as expected.

This difference is particularly pronounced in the 2nd trajectory, which is more challenging (due to its higher accelerations). By referring to Fig. 4, which displays the distance to the desired target for both trajectories, we observe that the INIT case shows a larger average deviation of 0.14 [m] for Traj1 and 0.3 [m] for Traj2, in contrast to the OPT<sub>a</sub> cases with averages of 0.09 [m] for Traj1 and 0.12 [m] for Traj2. Furthermore, the INIT case exhibits a significantly larger variance, with certain samples deviating 0.42 [m] in Traj1 and 0.75 [m] in Traj2. In contrast, OPT<sub>a</sub> samples tend to cluster around the average. These findings emphasize the effectiveness of the optimization problem presented in (11), which is able to capture how variations in the parameters affect deviations in the considered states.

## B. Experimental Results

After the validation of the optimization problem (11) in simulation, we performed real experiments using the Acanthis drone (see Fig. 5). Acanthis is an experimental
<figure><img src="image_6.png" /><figcaption>Fig. 5. Image of the experimental drone Acanthis.</figcaption></figure>

<figure><img src="image_7.png" /><figcaption>Fig. 6. Histograms of two trajectory types (left: Traj1, right: Traj2) for both cases (INIT in blue, OPT<sub>a</sub> in red) showing the distances to the desired target (|r<sub>d</sub>(t<sub>w</sub>) − r(t<sub>w</sub>)|) in [m] and its average across N<sub>p</sub> experiments with Acanthis drone.</figcaption></figure>

drone platform developed and maintained at INRIA/IRISA
Rennes that utilizes the PX4 firmware. N<sub>p</sub> = 11 perturbed
experiments were performed by physically altering the sys-
tem as depicted in Fig. 7. These perturbations were obtained
by adding some tools and weights on the extension rod of
the Acanthis drone. This allowed us to modify the set of
parameters p = (k<sub>f</sub>, g<sub>x</sub>, g<sub>y</sub>, g<sub>z</sub>, m) at each experiment. In
particular, the parameter k<sub>f</sub> is affected mainly by the tools
that we added under the motors that obstruct the airflow,
while the other parameters are affected by the weights and
location of the tools on the extension rod.

<figure><img src="image_8.png" /><figcaption>Fig. 7. Acanthis drone in its nominal state (top left), and subject to 11 distinct physical perturbations labeled as p₁, ..., p₁₁.</figcaption></figure>

<figure><img src="image_9.png" /><figcaption>Fig. 8. Experimental actuator speed comparison for Traj2: INIT (top) vs. OPT<sub>a</sub> (bottom). The solid red line denotes nominal actuator speed, while the dashed black line signifies the input tube upper bound (eq. 10). The horizontal dashed red line represents the actuator limit. The dashed green and solid orange lines depict N<sub>p</sub> perturbation runs as in Fig. 7: the solid orange lines indicate the cases in which the actuation was saturated, and the green dashed lines the cases in which no saturation occurred. Note how many fewer saturation cases are present in the OPT<sub>a</sub> case vs. the INIT case thanks to the use of the 'input tubes' in the constraints (11).</figcaption></figure>

In the last two columns of Fig. 3, we present the results
for Traj1 and Traj2 (1st and 2nd row, respectively), both
for the optimized and non-optimized cases. Notably, it is
evident from Traj1 that the deviation is more pronounced in
the INIT case. In fact, the point cloud – representing the
different target reach locations r(t<sub>w</sub>) and denoted with red
dots in Fig. 3 – is larger when compared to the OPT<sub>a</sub> case.
Fig. 6 provides insights for Traj1 in the experiments, where
we find that the average distance to target for the INIT
case is 0.09 [m], while it reduces to 0.07 [m] for the OPT<sub>a</sub>
case. This reduction signifies an improvement of 2 [cm]
on average. Additionally, the OPT<sub>a</sub> case demonstrates less
variance, further supporting its effectiveness.

Similar results are confirmed for Traj2. As it can be
observed in Fig. 3, the deviation from the target is both
larger and more spread in the INIT case compared to the
OPT<sub>a</sub> case. Returning to Fig. 6, for Traj2, we find that the
average distance to target is about 0.19 [m] for the INIT
case while it is around 0.16 [m] for the OPT<sub>a</sub> case. However,
it is important to note that the INIT case exhibits a higher
variance with deviations reaching up to 0.31 [m], whereas the
maximum deviation in the OPT<sub>a</sub> case is around 0.21 [m].
These results highlight the significant improvements in terms
of the average distance to the target and the reduced spread
achieved with the optimized OPT<sub>a</sub> approach. In addition,
Fig. 1 visualizes the flight logs data in PyBullet for Traj2.
Collisions with the virtual window occurred 50% of the times
in the INIT case while they are reduced to 25% for the
optimized OPT<sub>a</sub> case across the 12 experiments. For a more
detailed visualization, please refer to the attached video also
available at this link.

To illustrate the impact of the input sensitivity on the
constraints in the optimization problem (11), we refer to
Fig. 8. This figure reports the normalized actuator speed
u<sub>3</sub>(t) for Traj2 in real experiments, featuring both the INIT
case (top) and the OPT<sub>a</sub> case (bottom). In the nominal
case **p** = **p**<sub>c</sub> (in red), both INIT and OPT<sub>a</sub> maintained
the actuator speeds well below their maximum limit, thus
ensuring satisfactory reaching of the target. However, the
<figure><img src="image_7.png" /><figcaption>Fig. 5. Image of the experimental drone Acanthis.</figcaption></figure>

<figure><img src="image_8.png" /><figcaption>Fig. 6. Histograms of two trajectory types (left: Traj1, right: Traj2) for both cases (INIT in blue, OPT<sub>a</sub> in red) showing the distances to the desired target (|r<sub>d</sub>(t<sub>w</sub>) − r(t<sub>w</sub>)|) in [m] and its average across N<sub>p</sub> experiments with Acanthis drone.</figcaption></figure>

drone platform developed and maintained at INRIA/IRISA
Rennes that utilizes the PX4 firmware. N<sub>p</sub> = 11 perturbed
experiments were performed by physically altering the sys-
tem as depicted in Fig. 7. These perturbations were obtained
by adding some tools and weights on the extension rod of
the Acanthis drone. This allowed us to modify the set of
parameters p = (k<sub>f</sub>, g<sub>x</sub>, g<sub>y</sub>, g<sub>z</sub>, m) at each experiment. In
particular, the parameter k<sub>f</sub> is affected mainly by the tools
that we added under the motors that obstruct the airflow,
while the other parameters are affected by the weights and
location of the tools on the extension rod.

<figure><img src="image_9.png" /><figcaption>Fig. 7. Acanthis drone in its nominal state (top left), and subject to 11 distinct physical perturbations labeled as p₁, ..., p₁₁.</figcaption></figure>

<figure><img src="image_10.png" /><figcaption>Fig. 8. Experimental actuator speed comparison for Traj2: INIT (top) vs. OPT<sub>a</sub> (bottom). The solid red line denotes nominal actuator speed, while the dashed black line signifies the input tube upper bound (eq. 10). The horizontal dashed red line represents the actuator limit. The dashed green and solid orange lines depict N<sub>p</sub> perturbation runs as in Fig. 7: the solid orange lines indicate the cases in which the actuation was saturated, and the green dashed lines the cases in which no saturation occurred. Note how many fewer saturation cases are present in the OPT<sub>a</sub> case vs. the INIT case thanks to the use of the 'input tubes' in the constraints (11).</figcaption></figure>

In the last two columns of Fig. 3, we present the results
for Traj1 and Traj2 (1st and 2nd row, respectively), both
for the optimized and non-optimized cases. Notably, it is
evident from Traj1 that the deviation is more pronounced in
the INIT case. In fact, the point cloud – representing the
different target reach locations r(t<sub>w</sub>) and denoted with red
dots in Fig. 3 – is larger when compared to the OPT<sub>a</sub> case.
Fig. 6 provides insights for Traj1 in the experiments, where
we find that the average distance to target for the INIT
case is 0.09 [m], while it reduces to 0.07 [m] for the OPT<sub>a</sub>
case. This reduction signifies an improvement of 2 [cm]
on average. Additionally, the OPT<sub>a</sub> case demonstrates less
variance, further supporting its effectiveness.

Similar results are confirmed for Traj2. As it can be
observed in Fig. 3, the deviation from the target is both
larger and more spread in the INIT case compared to the
OPT<sub>a</sub> case. Returning to Fig. 6, for Traj2, we find that the
average distance to target is about 0.19 [m] for the INIT
case while it is around 0.16 [m] for the OPT<sub>a</sub> case. However,
it is important to note that the INIT case exhibits a higher
variance with deviations reaching up to 0.31 [m], whereas the
maximum deviation in the OPT<sub>a</sub> case is around 0.21 [m].
These results highlight the significant improvements in terms
of the average distance to the target and the reduced spread
achieved with the optimized OPT<sub>a</sub> approach. In addition,
Fig. 1 visualizes the flight logs data in PyBullet for Traj2.
Collisions with the virtual window occurred 50% of the times
in the INIT case while they are reduced to 25% for the
optimized OPT<sub>a</sub> case across the 12 experiments. For a more
detailed visualization, please refer to the attached video also
available at this link.

To illustrate the impact of the input sensitivity on the
constraints in the optimization problem (11), we refer to
Fig. 8. This figure reports the normalized actuator speed
u<sub>3</sub>(t) for Traj2 in real experiments, featuring both the INIT
case (top) and the OPT<sub>a</sub> case (bottom). In the nominal
case **p** = **p**<sub>c</sub> (in red), both INIT and OPT<sub>a</sub> maintained
the actuator speeds well below their maximum limit, thus
ensuring satisfactory reaching of the target. However, the
situation changes when the perturbations in Fig. 7 act on the drone. In the non-optimized INIT case, a considerable number of experimental perturbed runs (in orange) quickly reached saturation of the maximum speed limit. In contrast, the OPT<sub>a</sub> case displayed a quite better behavior, with only one of the N<sub>p</sub> perturbations reaching saturation at the beginning (corresponding to p<sub>8</sub> in Fig. 7), potentially exceeding the predefined range of 10% deviation as per eq. (5). Furthermore, we observed that the actuator behavior (normalized speed) in the OPT<sub>a</sub> case generally remained within the upper bound of the input tube (indicated by the dashed black line) obtained from the input sensitivity matrix (4). This highlights the significance of including input constraints (with their ‘tubes’) to ensure robustness against parameter variations of the inputs, particularly for aggressive trajectories such as Traj2.

## V. CONCLUSIONS

In this paper, we have experimentally validated the optimization problem in which the effect of parametric uncertainties in a quadrotor model (quantified by the notion of closed-loop state sensitivity) can be minimized by acting on the reference trajectory to be tracked. We selected two distinct trajectories to be tracked by the UAV “Acanthis”, introducing real-world physical perturbations to generate parameter uncertainties. The experimental results demonstrate the effectiveness of the proposed optimization in reducing the effects of uncertainties stemming from physical parameter perturbations, enabling more precise target attainment at relatively high speeds. We also showcased the significance of input sensitivity within the constraints of our optimization problem. Indeed, by using the input tube in the constraints, one can ensure the robustness of inputs against parametric uncertainty. Future work will consider the possibility of online replanning (e.g., within a MPC scheme) for runtime generation of robust trajectories during flight.

## REFERENCES

[1] K. J. Astrom and B. Wittenmark, "Adaptive control 2nd edition," *Addison-Wesley Pub Co.*, vol. 1994, 1994.

[2] M. Achtelik, T. Bierling, J. Wang, L. Höcht, and F. Holzapfel, "Adaptive control of a quadcopter in the presence of large/complete parameter uncertainties," in *Infotech@Aerospace 2011*, 2011, p. 1485.

[3] A. Haseltalab and R. R. Negenborn, "Adaptive control for autonomous ships with uncertain model and unknown propeller dynamics," *Control Engineering Practice*, vol. 91, p. 104116, 2019.

[4] K. Zhou and J. C. Doyle, *Essentials of robust control*. Prentice hall Upper Saddle River, NJ, 1998, vol. 104.

[5] R. Sanz, P. Garcia, Q.-C. Zhong, and P. Albertos, "Robust control of quadrotors based on an uncertainty and disturbance estimator," *Journal of Dynamic Systems, Measurement, and Control*, vol. 138, no. 7, p. 071006, 2016.

[6] S. Sun, A. Romero, P. Foehn, E. Kaufmann, and D. Scaramuzza, "A comparative study of nonlinear mpc and differential-flatness-based control for quadrotor agile flight," *IEEE Transactions on Robotics*, vol. 38, no. 6, pp. 3357-3373, 2022.

[7] D. Hanover, P. Foehn, S. Sun, E. Kaufmann, and D. Scaramuzza, "Performance, precision, and payloads: Adaptive nonlinear mpc for quadrotors," *IEEE Robotics and Automation Letters*, vol. 7, no. 2, pp. 690-697, 2021.

[8] A. Ansari and T. Murphey, "Minimum Sensitivity Control for Planning with Parametric and Hybrid Uncertainty," *The International Journal of Robotics Research (IJRR)*, vol. 35, no. 7, pp. 823-839, October 2016.

[9] S. Candido and S. Hutchinson, "Minimum Uncertainty Robot Path Planning using a POMDP Approach," in *2010 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, December 2010, pp. 1408-1413.

[10] ——, "Minimum uncertainty robot navigation using information-guided pomdp planning," in *2011 IEEE International Conference on Robotics and Automation*. IEEE, 2011, pp. 6102-6108.

[11] P. Robuffo Giordano, Q. Delamare, and A. Franchi, "Trajectory generation for minimum closed-loop state sensitivity," in *2018 IEEE International Conference on Robotics and Automation (ICRA)*. IEEE, 2018, pp. 286-293.

[12] P. Brault, Q. Delamare, and P. Robuffo Giordano, "Robust trajectory planning with parametric uncertainties," in *2021 IEEE International Conference on Robotics and Automation (ICRA)*. IEEE, 2021, pp. 11095-11101.

[13] C. Bohm, P. Brault, Q. Delamare, P. Robuffo Giordano, and S. Weiss, "Cop: Control & observability-aware planning," in *2022 IEEE Int. Conf. on Robotics and Automation*, 2022.

[14] P. Brault and P. Robuffo Giordano, "Tube-based trajectory optimization for robots with parametric uncertainty," submitted to IEEE Robotics and Automation Letters, http://rainbow-doc.irisa.fr/pdf/tube-based_traj_opt.pdf, 2023.

[15] S. Wasiela, P. Robuffo Giordano, J. Cortes, and T. Simeon, "A Sensitivity-Aware Motion Planner (SAMP) to Generate Intrinsically-Robust Trajectories," in *2023 IEEE Int. Conf. on Robotics and Automation*, 2023.

[16] A. Pupa, P. Robuffo Giordano, and C. Secchi, "Optimal energy tank initialization for minimum sensitivity to model uncertainties," in *2023 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, 2023.

[17] A. Srour, A. Franchi, and P. Robuffo Giordano, "Controller and trajectory optimization for a quadrotor uav with parametric uncertainty," in *Proceedings of the 2023 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS2023)*, Detroit, Michigan, United States, October 2023, uRL: https://hal.science/hal-04192321.

[18] F. Messerer and M. Diehl, "An Efficient Algorithm for Tube-based Robust Nonlinear Optimal Control with Optimal Linear Feedback," in *2021 60th IEEE Conference on Decision and Control (CDC)*, Dec. 2021, pp. 6714-6721, iSSN: 2576-2370.

[19] R. Mahony, V. Kumar, and P. Corke, "Multirotor Aerial Vehicles: Modeling, Estimation, and Control of Quadrotor," *IEEE Robotics Automation Magazine (RAM)*, vol. 19, no. 3, pp. 20-32, Sep. 2012.

[20] G. Antonelli, E. Cataldi, F. Arrichiello, P. Robuffo Giordano, S. Chaverini, and A. Franchi, "Adaptive trajectory tracking for quadrotor mavs in presence of parameter uncertainties and external disturbances," *IEEE Trans. on Control Systems Technology*, vol. 26, no. 1, pp. 248-254, 2018.

[21] M. J. Van Nieuwstadt and R. M. Murray, "Real-time trajectory generation for differentially flat systems," *International Journal of Robust and Nonlinear Control*, vol. 8, no. 11, pp. 995-1020, 1998.

[22] D. Brescianini, M. Hehn, and R. D'Andrea, "Nonlinear quadrocopter attitude control: Technical report," ETH Zurich, Tech. Rep., 2013.

[23] F. Zhou, B. Song, and G. Tian, "Bézier curve based smooth path planning for mobile robot," *Journal of Information & Computational Science*, vol. 8, no. 12, pp. 2441-2450, December 2011.

[24] A. R. Conn, K. Scheinberg, and P. L. Toint, "On the convergence of derivative-free methods for unconstrained optimization," *Approximation theory and optimization: tributes to MJD Powell*, pp. 83-108, 1997.

[25] G. Ansmann, "Efficiently and easily integrating differential equations with jitcode, jitcdde, and jitsde," *Chaos: An interdisciplinary journal of nonlinear science*, vol. 28, no. 4, 2018.
situation changes when the perturbations in Fig. 7 act on the drone. In the non-optimized INIT case, a considerable number of experimental perturbed runs (in orange) quickly reached saturation of the maximum speed limit. In contrast, the OPT<sub>a</sub> case displayed a quite better behavior, with only one of the N<sub>p</sub> perturbations reaching saturation at the beginning (corresponding to p<sub>8</sub> in Fig. 7), potentially exceeding the predefined range of 10% deviation as per eq. (5). Furthermore, we observed that the actuator behavior (normalized speed) in the OPT<sub>a</sub> case generally remained within the upper bound of the input tube (indicated by the dashed black line) obtained from the input sensitivity matrix (4). This highlights the significance of including input constraints (with their 'tubes') to ensure robustness against parameter variations of the inputs, particularly for aggressive trajectories such as Traj2.

## V. CONCLUSIONS

In this paper, we have experimentally validated the optimization problem in which the effect of parametric uncertainties in a quadrotor model (quantified by the notion of closed-loop state sensitivity) can be minimized by acting on the reference trajectory to be tracked. We selected two distinct trajectories to be tracked by the UAV “Acanthis”, introducing real-world physical perturbations to generate parameter uncertainties. The experimental results demonstrate the effectiveness of the proposed optimization in reducing the effects of uncertainties stemming from physical parameter perturbations, enabling more precise target attainment at relatively high speeds. We also showcased the significance of input sensitivity within the constraints of our optimization problem. Indeed, by using the input tube in the constraints, one can ensure the robustness of inputs against parametric uncertainty. Future work will consider the possibility of online replanning (e.g., within a MPC scheme) for runtime generation of robust trajectories during flight.

## REFERENCES

[1] K. J. Astrom and B. Wittenmark, "Adaptive control 2nd edition," *Addison-Wesley Pub Co.*, vol. 1994, 1994.

[2] M. Achtelik, T. Bierling, J. Wang, L. Höcht, and F. Holzapfel, "Adaptive control of a quadcopter in the presence of large/complete parameter uncertainties," in *Infotech@ Aerospace 2011*, 2011, p. 1485.

[3] A. Haseltalab and R. R. Negenborn, "Adaptive control for autonomous ships with uncertain model and unknown propeller dynamics," *Control Engineering Practice*, vol. 91, p. 104116, 2019.

[4] K. Zhou and J. C. Doyle, *Essentials of robust control*. Prentice hall Upper Saddle River, NJ, 1998, vol. 104.

[5] R. Sanz, P. Garcia, Q.-C. Zhong, and P. Albertos, "Robust control of quadrotors based on an uncertainty and disturbance estimator," *Journal of Dynamic Systems, Measurement, and Control*, vol. 138, no. 7, p. 071006, 2016.

[6] S. Sun, A. Romero, P. Foehn, E. Kaufmann, and D. Scaramuzza, "A comparative study of nonlinear mpc and differential-flatness-based control for quadrotor agile flight," *IEEE Transactions on Robotics*, vol. 38, no. 6, pp. 3357-3373, 2022.

[7] D. Hanover, P. Foehn, S. Sun, E. Kaufmann, and D. Scaramuzza, "Performance, precision, and payloads: Adaptive nonlinear mpc for quadrotors," *IEEE Robotics and Automation Letters*, vol. 7, no. 2, pp. 690-697, 2021.

[8] A. Ansari and T. Murphey, "Minimum Sensitivity Control for Planning with Parametric and Hybrid Uncertainty," *The International Journal of Robotics Research (IJRR)*, vol. 35, no. 7, pp. 823-839, October 2016.

[9] S. Candido and S. Hutchinson, "Minimum Uncertainty Robot Path Planning using a POMDP Approach," in *2010 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, December 2010, pp. 1408-1413.

[10] ——, "Minimum uncertainty robot navigation using information-guided pomdp planning," in *2011 IEEE International Conference on Robotics and Automation*. IEEE, 2011, pp. 6102-6108.

[11] P. Robuffo Giordano, Q. Delamare, and A. Franchi, "Trajectory generation for minimum closed-loop state sensitivity," in *2018 IEEE International Conference on Robotics and Automation (ICRA)*. IEEE, 2018, pp. 286-293.

[12] P. Brault, Q. Delamare, and P. Robuffo Giordano, "Robust trajectory planning with parametric uncertainties," in *2021 IEEE International Conference on Robotics and Automation (ICRA)*. IEEE, 2021, pp. 11095-11101.

[13] C. Bohm, P. Brault, Q. Delamare, P. Robuffo Giordano, and S. Weiss, "Cop: Control & observability-aware planning," in *2022 IEEE Int. Conf. on Robotics and Automation*, 2022.

[14] P. Brault and P. Robuffo Giordano, "Tube-based trajectory optimization for robots with parametric uncertainty," submitted to IEEE Robotics and Automation Letters, http://rainbow-doc.irisa.fr/pdf/tube-based_traj_opt.pdf, 2023.

[15] S. Wasiela, P. Robuffo Giordano, J. Cortes, and T. Simeon, "A Sensitivity-Aware Motion Planner (SAMP) to Generate Intrinsically-Robust Trajectories," in *2023 IEEE Int. Conf. on Robotics and Automation*, 2023.

[16] A. Pupa, P. Robuffo Giordano, and C. Secchi, "Optimal energy tank initialization for minimum sensitivity to model uncertainties," in *2023 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, 2023.

[17] A. Srour, A. Franchi, and P. Robuffo Giordano, "Controller and trajectory optimization for a quadrotor uav with parametric uncertainty," in *Proceedings of the 2023 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS2023)*, Detroit, Michigan, United States, October 2023, uRL: https://hal.science/hal-04192321.

[18] F. Messerer and M. Diehl, "An Efficient Algorithm for Tube-based Robust Nonlinear Optimal Control with Optimal Linear Feedback," in *2021 60th IEEE Conference on Decision and Control (CDC)*, Dec. 2021, pp. 6714-6721, iSSN: 2576-2370.

[19] R. Mahony, V. Kumar, and P. Corke, "Multirotor Aerial Vehicles: Modeling, Estimation, and Control of Quadrotor," *IEEE Robotics Automation Magazine (RAM)*, vol. 19, no. 3, pp. 20-32, Sep. 2012.

[20] G. Antonelli, E. Cataldi, F. Arrichiello, P. Robuffo Giordano, S. Chaverini, and A. Franchi, "Adaptive trajectory tracking for quadrotor mavs in presence of parameter uncertainties and external disturbances," *IEEE Trans. on Control Systems Technology*, vol. 26, no. 1, pp. 248-254, 2018.

[21] M. J. Van Nieuwstadt and R. M. Murray, "Real-time trajectory generation for differentially flat systems," *International Journal of Robust and Nonlinear Control*, vol. 8, no. 11, pp. 995-1020, 1998.

[22] D. Brescianini, M. Hehn, and R. D'Andrea, "Nonlinear quadrocopter attitude control: Technical report," ETH Zurich, Tech. Rep., 2013.

[23] F. Zhou, B. Song, and G. Tian, "Bézier curve based smooth path planning for mobile robot," *Journal of Information & Computational Science*, vol. 8, no. 12, pp. 2441-2450, December 2011.

[24] A. R. Conn, K. Scheinberg, and P. L. Toint, "On the convergence of derivative-free methods for unconstrained optimization," *Approximation theory and optimization: tributes to MJD Powell*, pp. 83-108, 1997.

[25] G. Ansmann, "Efficiently and easily integrating differential equations with jitcode, jitcdde, and jitsde," *Chaos: An interdisciplinary journal of nonlinear science*, vol. 28, no. 4, 2018.