# Acceleration-based Inner-loop Control and MPC for Aerial Robots: Advantages and Drawbacks

Lorenzo Balandi, Paolo Robuffo Giordano, Marco Tognon

► To cite this version:

Lorenzo Balandi, Paolo Robuffo Giordano, Marco Tognon. Acceleration-based Inner-loop Control and MPC for Aerial Robots: Advantages and Drawbacks. ERF 2025 - European Robotics Forum, Mar 2025, Stuttgart, Germany. pp.1-6. hal-05022328

HAL Id: hal-05022328

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

Submitted on 6 Apr 2025

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
Acceleration-based Inner-loop Control
and MPC for Aerial Robots:
Advantages and Drawbacks

Lorenzo Balandi¹, Paolo Robuffo Giordano¹, and Marco Tognon¹

Univ Rennes, CNRS, Inria, IRISA, Rennes, 35042, France*

lorenzo.balandi@inria.fr, prg@irisa.fr, marco.tognon@inria.fr

**Abstract.** This paper studies the application of a fast inner-loop con-
troller on top of a Nonlinear Model Predictive Control (NMPC) to in-
crease tracking performances of aerial robots in presence of disturbances
and model mismatches. We investigate the use of Incremental Nonlinear
Dynamic Inversion (INDI) and Time Delay Control (TDC), discussing
their close relations. Finally, a TDC formulation is developed as inner
tracking loop running at 1 kHz, and applied to a NMPC. Close-to-real
simulations with a fully-actuated hexarotor demonstrates the effective-
ness of the inner-loop control and its main drawbacks in presence of input
saturation.

**Keywords:** aerial robotics, trajectory tracking, nonlinear control

# 1 Introduction

Model Predictive Control (MPC) and its nonlinear variant (NMPC) have been successfully used for accurate trajectory tracking in Micro Aerial Vehicles (MAVs) despite challenges like disturbances and input saturation [4, 5, 2]. NMPC, in particular, excels at handling these issues thanks to its predictive capabilities but comes with the trade-off of high computational demands, the need for precise model knowledge, and suitable software frameworks.

Some recent works highlighted the benefits of having an additional inner-loop controller [9, 8], running at higher frequency and able to compensate for model mismatches and external disturbances, such as aerodynamic forces which are very difficult to model and are thus often neglected. Incremental Nonlinear Dynamic Inversion (INDI) [7] is an effective inner-loop controller that delivers high-performance tracking for nonlinear systems without needing detailed model knowledge. Initially applied to attitude control, INDI uses measured or estimated acceleration to incrementally compensate for model uncertainties. The benefits of high-frequency direct acceleration feedback are discussed in [3]. A similar control method is Time Delay Control (TDC) [10], whose strong analogies with INDI have been discussed in [1]. TDC is basically an INDI with fixed gain, and the acceleration feedback plays a similar role to an integral action in the control.

<footnote>* This work was supported by the project ANR-23-CE33-0013 "FlyHandyBot".</footnote>
In this paper, we further discuss the relation between INDI and TDC for MAVs, we show how TDC may be more appropriate and we highlight its criticality when applied to NMPC in the presence of input saturation. In fact, we demonstrate that these low-level controllers do not provide guarantees about stability in presence of input saturation, which can happen due to disturbances, to dynamically infeasible trajectories that require motor thrusts exceeding their limits, or both. The results are shown through close-to-real simulations of a fully-actuated hexarotor [6] controlled by a NMPC with inner TDC loop running at 1 kHz, which is applied to the whole model and not only to the rotational part.

## 2 System Modeling

Consider two right-handed reference frames: a world frame $\mathcal{F}_W$ with z axis op-posing gravity and a robot frame $\mathcal{F}_R$ attached to the vehicle Center of Mass (CoM). The position of the origin of $\mathcal{F}_R$ expressed in $\mathcal{F}_W$ is denoted as $\mathbf{p}$, while the orientation (attitude) of robot frame with respect to (wrt) world frame is represented by the rotation matrix $\mathbf{R}$ and parametrized by the unit quaternion $\mathbf{q} = [q_w, q_x, q_y, q_z]^\top \in \mathbb{S}(3)$. The angular velocity of $\mathcal{F}_R$ wrt $\mathcal{F}_W$, expressed in $\mathcal{F}_R$, is $\omega$. Following the Newton-Euler formalism, expressing the translational dynamics in world frame and the rotational dynamics in robot frame, we can write the hexarotor model as $\dot{\mathbf{p}} = \mathbf{v}$, $\dot{\mathbf{q}} = \frac{1}{2}\mathbf{q} \otimes [0 \quad \omega^\top]^\top$ and

$$ \begin{bmatrix} m\mathbf{I}_3 & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{J} \end{bmatrix} \begin{bmatrix} \dot{\mathbf{v}} \\ \dot{\mathbf{\omega}} \end{bmatrix} = \begin{bmatrix} -mgz_W \\ -\omega \times \mathbf{J}\omega \end{bmatrix} + c_f \begin{bmatrix} \mathbf{R} & \mathbf{0}_3 \\ \mathbf{0}_3 & \mathbf{I}_3 \end{bmatrix} \mathbf{G}\mathbf{w}^{\circ 2}, \quad (1) $$

where $\mathbf{v}$ is the linear velocity, $\otimes$ is the quaternion product, $m$ is the robot mass, $g$ is the gravity, $\mathbf{J}$ is the constant inertia matrix, $\mathbf{I}_3$ and $\mathbf{0}_3$ are the identity and zero matrix respectively, $\mathbf{G}$ is the allocation matrix computed as in [2]. In (1) we have used the well known relation $\gamma = c_f \mathbf{w}^{\circ 2}$, where $\gamma \in \mathbb{R}^6$ is the vector of motors thrusts, $\cdot^\circ$ is the Hadamard power and $\mathbf{w} \in \mathbb{R}^6$ the rotors speeds vector, which is the final command sent to motors. The vector $\mathbf{G}\gamma = [\mathbf{f}^\top \tau^\top]^\top$ is the body wrench, namely forces and torques applied to the robot by the actuators.

## 3 Nonlinear Model Predictive Controller

The NMPC algorithm for trajectory tracking iteratively solves an Optimal Control Problem (OCP) and computes optimal states and inputs for the prediction horizon. State and input vectors are $\mathbf{x} = [\mathbf{p}^\top \mathbf{q}^\top \mathbf{v}^\top \mathbf{w}^\top]^\top \in \mathbb{R}^3 \times \mathbb{S}(3) \times \mathbb{R}^6$ and $\mathbf{u} = \mathbf{w}^{\circ 2} \in \mathbb{R}^6$, respectively. The control objective is to track as close as possible a desired state trajectory $\mathbf{x}_d(t) = [\mathbf{p}_d(t)^\top \mathbf{q}_d(t)^\top \mathbf{v}_d(t)^\top \omega_d(t)^\top]^\top$ which may not be dynamically feasible considering actuation constraints. States and inputs are discretized in N equally spaced intervals with discretization step $dt$, obtaining
the following Non Linear Program (NLP)

$$
\begin{equation}
\begin{array}{ll}
\displaystyle \min_{\mathbf{x}_0, \dots, \mathbf{x}_N, \mathbf{u}_0, \dots, \mathbf{u}_{N-1}} & \sum_{k=0}^{N-1} (\|\mathbf{x}_k - \mathbf{x}_{k,d}\|_{\mathbf{Q}}^2 + \|\mathbf{u}_k - \mathbf{u}_{k,d}\|_{\mathbf{U}}^2) + \|\mathbf{x}_N - \mathbf{x}_{N,d}\|_{\mathbf{Q}_N}^2 \\
\text{subject to} & \mathbf{x}_{k+1} = \mathbf{f}(\mathbf{x}_k, \mathbf{u}_k) \quad k \in [0, N-1] \\
& \mathbf{x}_0 = \hat{\mathbf{x}} \\
& \mathbf{u}_{\min} \le \mathbf{u}_k \le \mathbf{u}_{\max} \quad k \in [0, N-1],
\end{array}
\tag{2}
\end{equation}
$$

where $Q = \text{diag}(Q_p, Q_q, Q_v, Q_\omega)$, $Q_*$ and $U$ are positive definite weight matrices, $Q_N = Q$ because we are equally interested in tracking every point of the reference trajectory, $f(\mathbf{x}_k, \mathbf{u}_k)$ is the discretized version of the dynamics obtained with, e.g., RK4 integrators, $\hat{\mathbf{x}}$ is the current state from state estimation, $\mathbf{u}_{\min}$ and $\mathbf{u}_{\max}$ are the minimum and maximum squared rotors velocities, respectively. In the cost function of (2) a reference input $\mathbf{u}_{k,d}$ is also specified.

4 TDC Inner-Loop Tracking Controller

Here we derive an expression of the TDC applied to the considered fully-actuated
vehicle, and we compare it to the INDI controller. Finally, application to NMPC
and practical consideration are drawn.

4.1 Time Delay Control

Model (1) can be rewritten as a nonlinear system in control affine form

$$
\dot{x}_1 = h(x_1, x_2) \tag{3a}
$$

$$
\dot{x}_2 = \mathbf{f}(x) + \mathbf{F}(x)u, \quad (3b)
$$

where $\mathbf{x}_1 = [\mathbf{p}^\top \mathbf{q}^\top]^\top \in \mathbb{R}^3 \times \mathbb{S}(3)$, $\mathbf{x}_2 = [\mathbf{v}^\top \mathbf{\omega}^\top]^\top \in \mathbb{R}^6$, $\mathbf{u} = \mathbf{w}^{\circ 2} \in \mathbb{R}^6$, $\mathbf{x} = [\mathbf{x}_1^\top \mathbf{x}_2^\top]^\top$ and $\mathbf{h}$ is a function expressing the non-linearity of the quater-
nion derivative. Introducing the matrix $\tilde{\mathbf{G}} = \tilde{\mathbf{G}}\tilde{\mathbf{R}}\mathbf{G}$ with $\tilde{\mathbf{G}}$ a constant diagonal
positive-definite gain matrix, $\tilde{\mathbf{R}} = \text{diag}(\mathbf{R}, \mathbf{I}_3)$, $\mathbf{G}$ the allocation matrix, (3b)
can be rewritten as

$$
\tilde{\mathbf{G}}^{-1} \dot{\mathbf{x}}_2 = \mathbf{H}(\mathbf{x}, \dot{\mathbf{x}}) + \mathbf{u}, \quad (4)
$$

where $\mathbf{H}(\mathbf{x}, \dot{\mathbf{x}}) = (\tilde{\mathbf{G}}^{-1} - \mathbf{F}^{-1}(\mathbf{x}))\dot{\mathbf{x}}_2 + \mathbf{F}^{-1}(\mathbf{x})\mathbf{f}(\mathbf{x})$ includes all unknown dynamics. $\mathbf{F}(\mathbf{x})$ is full-rank thanks to the full actuation of the system. Considering a sufficiently small time interval $\Delta t$, the value of $\mathbf{H}(\mathbf{x}, \dot{\mathbf{x}})$ at time $t$ is close to that at time $t - \Delta t$. By defining $\dot{\mathbf{x}}_{2,0} = \dot{\mathbf{x}}_2(t - \Delta t)$, $\mathbf{u}_0 = \mathbf{u}(t - \Delta t)$, uncertain terms can be measured from sensors and/or state estimation algorithms, obtaining the Time Delay Estimation (TDE) form of $\mathbf{H}(\mathbf{x}, \dot{\mathbf{x}})$ as

$$
\hat{\mathbf{H}}(\mathbf{x}, \dot{\mathbf{x}}) = \tilde{\mathbf{G}}^{-1} \dot{\mathbf{x}}_{2,0} - \mathbf{u}_0. \quad (5)
$$
Note that (5) uses the most recent sampled values of $\dot{x}_2$ and $u$ to approximate the unknown dynamics, thus making this approach sensor-based. By substituting (5) in (4), considering the virtual control input $\dot{x}_2 = \nu$ as in [1], we obtain

$$u = u_0 + \tilde{G}^{-1}(\nu - \dot{x}_{2,0}), \quad (6)$$

where, as discussed in [1], $\nu$ is a virtual control input that includes a reference acceleration and a stabilizing PD action. Note that (6) slightly misuses the term TDC, as $\tilde{G}$ depends on the state via the rotation matrix. However, since the rotation matrix only changes the direction of a vector and not its magnitude, we still consider it a TDC. The key property of a fixed, tunable gain remains by assuming no uncertainty in the attitude provided by the state estimator. In (6), mass and inertia are absent, but there is a dependency on the allocation matrix, thrust coefficient, and geometric parameters. Indeed, unlike manipulators, UAVs cannot directly control the body wrench; the controlled variables are the motors' rotational speeds.

## 4.2 Analogies with INDI

As discussed in [1], INDI can be viewed as TDC control with a state-dependent gain, $\tilde{G}(x_0)$ instead of $G$, which enhances system stability. However, for UAVs with fixed geometry, mass, and inertia, INDI would have a fixed gain, making it less flexible and more reliant on hard-to-obtain parameters like inertia [9, 8]. For MAVs, TDC is more suitable, as it allows greater tuning flexibility through the gain matrix $\tilde{G}$, similar to the integral gain approach in [3]. This does not apply to platforms like aerial manipulators, where inertia changes with the state. A comparison between TDC and incremental PID is also provided in [1].

## 4.3 Application to NMPC and Practical Implementation

In (6), $\nu$ includes a desired acceleration and a PD action on the state as stated in [1]. To apply TDC as inner loop, the reference values are obtained from NPMC: at each control step the algorithm outputs optimal states and inputs along the prediction horizon, namely $x_k^{opt}$ for $k = 0, \dots, N$ and $u_k^{opt}$ for $k = 0, \dots, N-1$. Since the TDC inner-loop runs at higher frequency than NMPC, the optimal states and inputs are interpolated between the first and the second shooting nodes, namely between $x_0^{opt}$, $u_0^{opt}$ and $x_1^{opt}$, $u_1^{opt}$ considering the current time. Linear and angular reference accelerations are computed using optimal interpolated states and inputs and inverting the model (1). In practical implementations, as discussed in [7, 9, 8], the noisy acceleration feedback signals must be low-pass filtered with the same filters in order to accumulate the same amount of delay. Moreover, the angular acceleration in (6) (last three elements of $\dot{x}_{2,0}$) cannot be directly measured by sensors and it is obtained by numerically differentiating the angular velocity. Concerning the input feedback $u_0$ in (6), if it cannot be measured, it is necessary to estimate it considering the actuation delay as explained in [7]. We use a first-order filter with time constant $\tau$ applied to the most recent
computed input. The delayed input, according to [7], is further filtered with the
same filter used for the accelerations in order to synchronize the signals.

5 Numerical Results

In this section, we report the comparison between NMPC (2) and NMPC + TDC (6) without saturation, then we show that NMPC + TDC is unstable in case of infeasible reference trajectory. We simulate the fully-actuated hexarotor [6] using Gazebo simulator, with a plugin<fnref n="1" /> that allows to simulate motors dynamics with vibrational radial and axial forces and torques and noise in the thrust direction. The NMPC runs at 100 Hz, while state estimation and TDC are implemented as GenoM3² modules written in C/C++ running at 1 kHz. In order to have more realistic simulations, we also add model mismatches between nominal parameters used by the controllers and real model parameters. In particular, the real mass and inertia are 30% higher, the real $c_f$ is 10% lower, and the tilting angle of some motors is varied of ± 1 or 2 degrees to simulate an imprecise assembly.

We propose three simulations where the desired state trajectories are sym-
metric chirps. In the first two simulations, the maximum chirp frequency is
0.4 Hz, in the last it is 0.6 Hz and it is dynamically unfeasible (it leads to in-
put saturation). The NPMC costs in (2) are tuned in order to give more im-
portance to position tracking. Moreover, $dt = 50$ ms and $N = 20$. The de-
sired input $\mathbf{u}_{k,d}$ is set to the hovering value. The TDC parameters are $\mathbf{G} = \text{diag}(0.001, 0.001, 0.0004, 0.01, 0.01, 0.01)$, $\tau = 0.02$, and the low pass filters are
second order with cutoff frequency 8 Hz.

In Fig. 1 we show position and attitude errors, with normalized inputs. It is
clear that the inner-loop controller helps in reducing the position error, but it is
unable to manage input saturation leading to instability in this case (Fig. 1(c)).

6 Conclusion

We examined the application of a fast inner-loop controller to an outer-loop lower frequency controller to increase tracking performances in presence of disturbances. The relation between INDI and TDC have been discussed relative to MAVs and the application of TDC to a fully-actuated hexarotor with NMPC outer loop are reported. Finally, realistic simulations demonstrate advantages and main limitation of this approach. We are currently working on the solution of this problem and performing real-world experiments with the hexarotor.

References

1. Acquatella, B.P., Chu, Q.P.: Agile spacecraft attitude control: An incremental non-linear dynamic inversion approach. IFAC-PapersOnLine **53**(2), 5709–5716 (2020)

<footnote n="1">¹ https://git.openrobots.org/projects/mrsim-gazebo</footnote>

<footnote n="2">² https://git.openrobots.org/projects/genom3</footnote>
<figure><img src="image_2.png" /><figcaption>**Fig. 1.** Simulation results: (a) just NMPC, (b) NMPC+TDC, (c) NMPC+TDC with input saturation. Red dashed lines represent lower and upper actuation limits.</figcaption></figure>

2. Bicego, D., Mazzetto, J., Carli, R., Farina, M., Franchi, A.: Nonlinear model predictive control with enhanced actuator model for multi-rotor aerial vehicles with generic designs. Journal of Intelligent & Robotic Systems **100**(3), 1213–1247 (2020)

3. Hamandi, M., Tognon, M., Franchi, A.: Direct acceleration feedback control of quadrotor aerial vehicles. In: 2020 IEEE International Conference on Robotics and Automation (ICRA), pp. 5335–5341. IEEE (2020)

4. Nguyen, H., Kamel, M., Alexis, K., Siegwart, R.: Model predictive control for micro aerial vehicles: A survey. In: 2021 European Control Conference (ECC), pp. 1556–1563. IEEE (2021)

5. Peric, L., Brunner, M., Bodie, K., Tognon, M., Siegwart, R.: Direct force and pose nmpc with multiple interaction modes for aerial push-and-slide operations. In: 2021 IEEE International Conference on Robotics and Automation (ICRA), pp. 131–137. IEEE (2021)

6. Rajappa, S., Ryll, M., Bülthoff, H.H., Franchi, A.: Modeling, control and design optimization for a fully-actuated hexarotor aerial vehicle with tilted propellers. In: 2015 IEEE international conference on robotics and automation (ICRA), pp. 4006–4013. IEEE (2015)

7. Smeur, E.J., Chu, Q., De Croon, G.C.: Adaptive incremental nonlinear dynamic inversion for attitude control of micro air vehicles. Journal of Guidance, Control, and Dynamics **39**(3), 450–461 (2016)

8. Sun, S., Romero, A., Foehn, P., Kaufmann, E., Scaramuzza, D.: A comparative study of nonlinear mpc and differential-flatness-based control for quadrotor agile flight. IEEE Transactions on Robotics **38**(6), 3357–3373 (2022)

9. Tal, E., Karaman, S.: Accurate tracking of aggressive quadrotor trajectories using incremental nonlinear dynamic inversion and differential flatness. IEEE Transactions on Control Systems Technology **29**(3), 1203–1218 (2020)

10. Youcef-Toumi, K., Wu, S.T.: Input/output linearization using time delay control (1992)