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

HAL
open science

# Embedded Robust Model Predictive Path Integral Control using Sensitivity Tubes and GPU Acceleration

Frederik Falk Nyboe, Amr Afifi, Paolo Robuffo Giordano, Emad Ebeid, Antonio Franchi

► To cite this version:

Frederik Falk Nyboe, Amr Afifi, Paolo Robuffo Giordano, Emad Ebeid, Antonio Franchi. Embedded Robust Model Predictive Path Integral Control using Sensitivity Tubes and GPU Acceleration. ICRA 2025 - IEEE International Conference on Robotics & Automation, May 2025, Atlanta, United States. pp.7174-7180, 10.1109/icra55743.2025.11127520. hal-05066734

HAL Id: hal-05066734

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

Submitted on 14 May 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_2.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
# Embedded Robust Model Predictive Path Integral Control using Sensitivity Tubes and GPU Acceleration

Frederik Falk Nyboe<fnref n="1" />*, Amr Afifi<fnref n="2" />*, Paolo Robuffo Giordano<fnref n="3" />, Emad Ebeid<fnref n="1" />, Antonio Franchi<fnref n="2" />,<fnref n="4" />

**Abstract**— This paper proposes a method to robustify model predictive path integral (MPPI) control by directly taking into account the effects of parameter uncertainty into the controller formulation. Leveraging the recent notion of closed-loop state sensitivity, the proposed MPPI can consider the state sensitivity against parameter mismatch as a part of the system state, and consequently exploit this additional information to address the challenge of model mismatch in sampling-based model predictive control. Using an obstacle avoidance scenario, we demonstrate the use of our approach to control an aerial robot. We present an embedded implementation of our method, utilizing parallelization of computations on a GPU. Finally, we show the increased robustness of our approach over a standard MPPI controller through hardware-in-the-loop simulations and validate its embedded real-time properties.

## I. INTRODUCTION

Model Predictive Control (MPC) has emerged as a control methodology that can enable robots to achieve higher levels of autonomy and performance. It endows a robot with predictive capabilities using dynamic models, and exploits optimal control techniques to optimize the robot behavior with respect to a task. It does so by solving a finite horizon optimal control problem (OCP) in an iterative receding horizon fashion. There are different flavors of MPC algorithms such as gradient-based and sampling-based MPC. However, regardless of the chosen algorithm, the potential performance improvement of MPC is hindered by its sensitivity to mismatch between the robot model used for the prediction and the real system. This mismatch can lead to incorrect predictions which may cause failures during operation.

In this work, we focus on sampling-based MPC and, in particular, on the algorithm known as Model Predictive Path Integral (MPPI) control which was first proposed in [1]. We address the problem of the MPPI control's sensitivity to model mismatch. This problem has been addressed in the literature using a number of different approaches. In [2], a two-level control architecture is proposed, where the MPPI controller uses the nominal model of the system, while an ancillary controller tries to keep the real system's evolution close to the nominal state evolution. The MPPI

controller is unaware of the closed-loop dynamics of the plant, which can result in overly conservative behavior. In [3], the MPPI controller augments both the open-loop and closed-loop plant dynamics. The optimization attempts to bound the error between the open-loop state and the closed-loop state evolution by means of a controller for the closed-loop plant. However, satisfying the desired error bound is not trivial and requires a computationally expensive controller design procedure, which hinders the real-time applicability of the approach. In [4], the MPPI controller is used as a high-level planner providing motion references to a low-level impedance controller. Safety constraints are enforced reactively using a control barrier function outside the MPPI controller. The controller's sensitivity to model mismatch is not addressed explicitly and the authors of [4] report that the system's performance can degrade due to model uncertainty.

Contrarily to the state of the art where the robustification of the MPPI algorithm requires complex architectures, we propose an approach that considers the uncertainty directly within the MPPI formulation. We tackle the MPPI's sensitivity to model mismatch by making use of the recent notion of closed-loop state sensitivity [5]. This approach allows us to include the dynamics of the model's sensitivity to parameter mismatch within the OCP. This then allows the MPPI controller to naturally consider the effect of the uncertainty as a state of the system. Moreover, the closed-loop state sensitivity can be used to derive bounds on the state deviation from its nominal values due to parameter mismatch. These bounds are used to compute tubes referred to as sensitivity-tubes centered around the system's nominal evolution. We use the sensitivity-tubes to formulate the cost function within the MPPI controller to robustify its performance. We present a real-time implementation of our robust MPPI controller utilizing parallelization. We demonstrate that the propagation of the uncertainty information over the prediction horizon can be done on an embedded computer, exploiting hardware acceleration with a graphics processing unit (GPU). Finally, we show the efficacy and embedded, real-time performance of our approach using hardware-in-the-loop simulations of an aerial robot (AR) in an obstacle avoidance scenario.

The paper is divided as follows. Sec. II presents the preliminary notions needed for our approach. The example use case is presented in Sec. III. The robust MPPI formulation is described in Sec. IV, which is followed by the embedded implementation description in Sec. V. The hardware-in-the-loop simulations are presented in Sec. VI. Lastly, the conclusions of the work are outlined in Sec. VII.

<footnote>*These authors contributed equally to this work.</footnote>

<footnote n="1">¹Digital and High-Frequency Electronics, Institute of Mechanics and Electronics, University of Southern Denmark, Odense, Denmark.</footnote>

<footnote n="2">²Robotics and Mechatronics, Faculty of Electrical Engineering, Mathematics & Computer Science, University of Twente, Enschede, Netherlands.</footnote>

<footnote n="3">³CNRS, Univ Rennes, Inria Campus de Beaulieu, Rennes, Cedex, France.</footnote>

<footnote n="4">⁴Department of Computer, Control and Management Engineering, Sapienza University of Rome, 00185 Rome, Italy.</footnote>

<footnote>This work was partially funded by the ANR-20-CE33-0003 "CAMP"; EU AUTOASSESS project (EU Grant agreement ID: 101120732); and IFD InnoExplorer AirOps.</footnote>
## II. PRELIMINARIES

In this section, we first define the basic setting used for system modeling throughout the paper. We then provide a brief description of the different methodologies which constitute the building blocks for our approach.

### A. General Modeling Formulation

Consider the following general representation for a non-linear system:

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

where $q(t) \in \mathbb{R}^{n_q}$ is the system state and $u(t) \in \mathbb{R}^{n_u}$ the system inputs. The model parameters are represented by $p \in \mathbb{R}^{n_p}$. The system input are obtained by a controller represented in a general form as

$$ \begin{aligned} \dot{\xi}(t) &= g(q(t), \xi(t), p_n) \\ u(t) &= h(q(t), \xi(t), p_n, r(t)). \end{aligned} \quad (2) $$

This representation considers the possible presence of internal controller states $\xi(t) \in \mathbb{R}^{n_\xi}$, while the state or output references to be tracked by the controller are represented by $r(t) \in \mathbb{R}^{n_r}$.

### B. Model Predictive Path Integral (MPPI) Control

MPC is an optimal control approach where a cost function is minimized along a time horizon. This is achieved by finding the optimal control sequence in the presence of dynamic, state and input constraints. MPPI control is an algorithm that solves an MPC problem with a sampling-based approach. The derivation of the algorithm starts from a stochastic optimal control setting, where we seek the optimal control sequence that minimizes the expectation of a cost function. The cost is evaluated with respect to stochastic dynamics. This can be stated mathematically as

$$ U^* = \underset{U}{\operatorname{argmin}} \mathbb{E}_{\pi(U)} \left[ l_T(q(T)) + \int_{t_0}^{T} l(q(\tau), u(\tau)) d\tau \right]. \quad (3) $$

The optimization problem expressed in eq. (3) can be solved by stochastic gradient descent. Instead, the MPPI approach uses information theoretic dualities between free energy and relative entropy in order to approximate the optimal control sequence, and in particular the gradient computation, using Monte Carlo sampling. The final discrete-time policy update rule for the optimal input sequence $[u_t \quad u_{t+\Delta t} \quad \dots \quad u_{t+T-\Delta t}]$ is

$$ \mathbf{u}_j^{i+1} = \mathbf{u}_j^i + \alpha \sum_{a=0}^{N_r-1} w_r \epsilon_j^a, \quad (4) $$

where

$$ w_r \approx \frac{\exp(-\lambda H)}{\sum_{b=0}^{N_r-1} \exp(-\lambda H)}. $$

For a detailed derivation we direct the interested reader to [1].

The jth input vector $u_j$ is updated in an iterative fashion: $N_r$ Monte-Carlo samples or roll-outs are used to approximate

the gradient. Every roll-out is propagated using a random input perturbation $\epsilon_j$. The cost $H$, computed as follows,

$$ H = \left[ l_T(q(T)) + \int_{t_0}^{T} l(q(\tau), u(\tau)) \right]. $$

is computed for each roll-out and then an importance sampling procedure is performed to update the weights $w$, where $\lambda$ is a tunable parameter.

### C. Closed-Loop State Sensitivity Dynamics & Tubes

The notion of closed-loop state sensitivity with respect to model parameters was initially introduced in [5]. It is defined as

$$ \mathbf{\Pi}(t) = \left. \frac{\partial q(t)}{\partial p} \right|_{p=p_n} \quad \mathbf{\Pi}(t) \in \mathbb{R}^{n_q \times n_p}, \quad (5) $$

and matrix $\Pi(t)$ represents the deviation of the closed-loop state from its nominal value (i.e., the state evolution obtained for the nominal values of the parameters $p = p_n$). The closed-loop state sensitivity is generally not available in closed-form, however its dynamics can be easily derived from eq. (1-2) as shown in [6]

$$ \begin{aligned} \dot{\mathbf{\Pi}} &= \frac{\partial f}{\partial q} \mathbf{\Pi} + \frac{\partial f}{\partial u} \mathbf{\Theta} + \frac{\partial f}{\partial p} \quad \mathbf{\Pi}(0) = \mathbf{0} \in \mathbb{R}^{n_q \times n_p} \\ \mathbf{\Theta} &= \frac{\partial h}{\partial q} \mathbf{\Pi} + \frac{\partial g}{\partial \xi} \mathbf{\Pi}_{\xi} \\ \dot{\mathbf{\Pi}}_{\xi} &= \frac{\partial g}{\partial q} \mathbf{\Pi} + \frac{\partial h}{\partial q} \mathbf{\Pi}_{\xi} \quad \dot{\mathbf{\Pi}}_{\xi}(0) = \mathbf{0} \in \mathbb{R}^{n_{\xi} \times n_p}. \end{aligned} \quad (6) $$

One can then obtain $\Pi$ by forward integrating the ODE (6) along the system's nominal trajectory. It is further possible to compute the closed-loop sensitivity of any differentiable functions of the state, e.g. $y(t) = f_y(q(t), p) \in \mathbb{R}^{n_y}$, as

$$ \mathbf{\Pi}_y(t) = \frac{\partial y(t)}{\partial p} = \frac{\partial f_y}{\partial q} \mathbf{\Pi} + \frac{\partial f_y}{\partial p}. \quad (7) $$

Matrix $\Pi$ can be used to compute sensitivity-tubes which are centered around the nominal evolution of the state (or a function of the state). The sensitivity-tubes capture the worst case effect of model mismatch on the evolution of the nominal state in closed-loop. The computation of the sensitivity-tubes is detailed in [7]. In the following, we provide a brief overview of the procedure. Denote the nominal closed-loop state trajectory $q_n(t)$ and let $\Delta q(t) = q(t) - q_n(t)$ and $\Delta p = p - p_n$. Then for small $\Delta p$, $\Delta q(t)$ can be approximated as

$$ \Delta q(t) \approx \mathbf{\Pi}(t) \Delta p. \quad (8) $$

Suppose it is possible to define the maximum parameter deviation $\Delta p_{\max} = (\Delta p_{\max,0}, \dots, \Delta p_{\max,n_p})$ and define an ellipsoid $\Delta p^T W^{-1} \Delta p = 1$ centered at $p_n$ with $W = \text{diag}\{\Delta p_{\max,i}\}$. Then one can obtain the corresponding ellipsoid in the state space by using eq. (8). The ellipsoid centered at $q_n(t)$ is given by

$$ \Delta q^T \Pi^{\dagger T} W^{-1} \Pi^{\dagger} \Delta q = \Delta q^T (\Pi W \Pi^T)^{\dagger} \Delta q = 1. $$
<figure><img src="image_3.png" /><figcaption>Fig. 1: The figure shows a simulated example that demonstrates the sensitivity-tubes. One the top row, a nominal x-y-z position trajectory for an aerial robot is shown in blue, with the sensitivity tubes are shown in dashed-red. In the bottom row, 500 simulations are performed with the a subset of the system parameters perturbed from their nominal values. The reader can appreciate the ability of the sensitivity-tubes to approximate very well the envelope of the perturbed trajectories.</figcaption></figure>

Defining the kernel of previous ellipsoid as $K_Π = ΠWΠ^T$, let the tube bounds be $\alpha_q(t) \in \mathbb{R}^{n_q}$, computed as

$$ \alpha_{q,i}(t) = \sqrt{e_i^T K_Π e_i} \quad i = 1, \dots, n_q, \ t \in [0, T], \quad (9) $$

where $(e_1, \dots, e_{n_q})$ represents the canonical basis of $\mathbb{R}^{n_q}$. This approach offers then a computationally tractable means to propagate the effects of model uncertainty on the nonlinear closed-loop system. Figure 1 shows an example of the sensitivity-tubes computed for the AR used later on in our example use-case. We will demonstrate that embedding the sensitivity-tubes in the MPPI control problem leads to an improved performance with respect to an obstacle avoidance objective.

### III. EXAMPLE USE CASE: AERIAL ROBOT MODELING, CONTROL & UNCERTAINTY DYNAMICS

In this section, we present the modeling and control for an aerial robot used as an example use-case for demonstrating the efficacy of our approach in robustifying a MPPI controller.

#### A. Aerial Robot Modeling & Control

The robotic system chosen for the example use case is a fully actuated (FA) multi-rotor AR [8]. This is a specific class of aerial systems which are optimized for aerial manipulation. These AR designs are able to independently control their position and orientation, unlike the more standard under-actuated multi-rotors (for example the quad-rotor). Full actuation is enabled on such platforms by optimizing the pose of the propellers. In particular we use the fully actuated hexa-rotor design known as the *Tilthex* [9]. The main motivation of using this system is due to the fact that model uncertainty plays a strong role in the robot's actuation model. The uncertainty in the robot's inertial and aerodynamic parameters affects the system's actuation model along all six degrees of freedom, unlike the more standard quad-rotor.

Consider an inertial frame $\mathcal{F}_I$ and a robot frame $\mathcal{F}_r$. Using the same notation defined in Sec. II-A, we define the AR's state as $q = [\mathbf{p}_r^T \ \phi_r^T \ \nu_r^T]^T$, where $\mathbf{p}_r \in \mathbb{R}^3$ is the robot's position, $\phi_r \in \mathbb{R}^3$ is the robot's orientation wrt to the inertial frame, represented using the minimal representation of orientation roll-pitch-yaw, $\phi - \theta - \psi$, and lastly, $\nu_r \in \mathbb{R}^6$ represents the robot's translation velocity $\dot{\mathbf{p}}_r$ and angular velocity $\omega_r$.

The AR's dynamic equations can be derived using the Newton-Euler Formalism [10] as

$$ \dot{\nu}_r = -M^{-1}h(q, \dot{q}) + M^{-1}F(q)u, \quad (10) $$

where $M \in \mathbb{R}^{6 \times 6}$ is the AR's positive definite inertia matrix, the gravity and Coriolis effects are represented by the vector $h(q, \dot{q}) \in \mathbb{R}^6$ and the actuation map is represented by $F(q) \in \mathbb{R}^{6 \times 6}$. The robot's control input $u \in \mathbb{R}^6$ represents the square of the propeller spinning velocity, which is related, in good approximation, quadratically to the propeller generated thrust [9].

We will assume the robot has a low-level controller which will receive position reference trajectories from the MPPI controller. Due to the system's FA, the low-level controller is a static feedback linearization with virtual input as a PID controller.

$$ u = F^{-1}(q)(M\dot{\nu}_r^* + h(q, \dot{q})) \quad (11) $$

with

$$ \dot{\nu}_r^* = \begin{bmatrix} \ddot{\mathbf{p}}_r^d + \mathbf{K}_p(\mathbf{p}_r^d - \mathbf{p}_r) + \mathbf{K}_d(\dot{\mathbf{p}}_r^d - \dot{\mathbf{p}}_r) \\ \dot{\omega}_r^d + \mathbf{K}_R e_R + \mathbf{K}_\omega e_\omega \end{bmatrix} $$

Where, $\ddot{\mathbf{p}}_r^d, \dot{\mathbf{p}}_r^d, \mathbf{p}_r^d$ are the position trajectory references to be computed by the MPPI. It is also possible to use the MPPI to command the platforms 3D orientation as well, however to simplify our robustness analysis that will follow, we assume that the desired orientation of the platform are given for our scenario and are used to compute $e_R$ and $e_\omega$ as in [11]. More details about the low-level controller can be found in [9]. The control architecture is visualized in the top part of Fig. 2.

#### B. Parametric Uncertainty Representation & Dynamics

At this point, it is interesting to highlight a subset of the AR's physical parameters that we will consider uncertain in the robustness considerations to follow. We choose to highlight the robot's mass $m_r \in \mathbb{R}^+$, center of mass position $\mathbf{r}_c \in \mathbb{R}^3$ and the aerodynamic parameters $k_f \in \mathbb{R}^+$ and $k_\tau \in \mathbb{R}^+$, which represent the propeller coefficient for thrust and drag, respectively. The choice of these parameters is motivated by two different scenarios; the first of which is the AR carrying a payload and thus affecting its mass and center of mass; the second scenario constitutes flying close to surfaces like a wall or the ground, which changes the propeller efficiency, i.e, the values of $k_f$ and $k_\tau$. We re-write eq. (10), exposing the dependency of the system matrices on such parameters:

$$ \dot{\nu}_r = -M^{-1}(m_r, \mathbf{r}_c) [\mathbf{h}(m_r, \mathbf{r}_c) + \mathbf{F}(k_f, k_\tau)u] . \quad (12) $$
Consequently, by defining the vector of uncertain parameters $\mathbf{p} = [m_r \ r_c^T \ k_f \ k_\tau]^T$, we can compute the closed-loop state sensitivity dynamics $\Pi$ as in eq. (6), leading to a set of ordinary differential equations representing the dynamics of the outlined uncertainty. Additionally, in Fig. 1, an example of the sensitivity-tube computation is described.

## IV. ROBUST MPPI OPTIMAL CONTROL PROBLEM

In this section, we describe our approach to robustify MPPI control using the sensitivity-tubes. We consider the control architecture presented in [4] and shown in Fig. 2, where the MPPI controller is used as a high-level receding horizon planner that provides position references to a low-level controller. In particular, the MPPI will be used to compute the reference accelerations $\mathbf{a} = \ddot{\mathbf{p}}_r^d$, which will then be integrated to obtain the position and velocity references $\mathbf{r} = [\mathbf{p}_r^d \ \dot{\mathbf{p}}_r^d]^T$ for the low level controller. The proposed MPPI scheme considers the full closed-loop system dynamics and additionally, in this work we propose to consider the sensitivity dynamics, in particular eq. (1), (2) and (6).

We can then formulate the following OCP:

$$
\begin{array}{ll}
\min_{\mathbf{a}} & \displaystyle \int_{T_0}^{T} (\mathbf{q}_T, \mathbf{\Pi}_T) + \int_{T_0}^{T} l(\mathbf{q}, \mathbf{\Pi}, \mathbf{a}) d\tau \\
\text{s.t.} & \dot{\mathbf{q}}(t) = \mathbf{f}(\mathbf{q}, \mathbf{u}, \mathbf{p}) \\
& \dot{\mathbf{\xi}} = \mathbf{g}(\mathbf{q}, \mathbf{\xi}, \mathbf{p}_n) \\
& \mathbf{u}(t) = \mathbf{h}(\mathbf{q}, \mathbf{\xi}, \mathbf{p}_n, \mathbf{a}) \\
& \dot{\mathbf{\Pi}} = \frac{\partial \mathbf{f}}{\partial \mathbf{q}} \mathbf{\Pi} + \frac{\partial \mathbf{f}}{\partial \mathbf{u}} \mathbf{\Theta} + \frac{\partial \mathbf{f}}{\partial \mathbf{p}} \\
& \mathbf{\Theta} = \frac{\partial \mathbf{h}}{\partial \mathbf{q}} \mathbf{\Pi} + \frac{\partial \mathbf{g}}{\partial \mathbf{\xi}} \mathbf{\Pi}_{\mathbf{\xi}} \\
& \dot{\mathbf{\Pi}}_{\mathbf{\xi}} = \frac{\partial \mathbf{g}}{\partial \mathbf{q}} \mathbf{\Pi} + \frac{\partial \mathbf{h}}{\partial \mathbf{q}} \mathbf{\Pi}_{\mathbf{\xi}}
\end{array}
\qquad (13)
$$

The OCP in eq. (13) evaluates the cost with respect to the combined dynamics. Naturally, the dynamics will be computed using the nominal values of a robot's parameters $\mathbf{p}_n$. Indeed, this is the reason why a standard OCP is sensitive to model mismatch since the real robot trajectory will unavoidably evolve in a different way than the predicted one whenever $\mathbf{p} \neq \mathbf{p}_n$, as seen in the Fig. 1. However, the OCP proposed in eq. (13) is made aware of such sensitivity to parameter perturbations by means of the closed-loop state-sensitivity dynamics, which are embedded in the problem. In this way, the cost function can take into account such information, for example the sensitivity-tubes, to robustify the performance of the MPPI controller. The OCP in eq. (13) is solved in an iterative receding horizon fashion using Monte-Carlo sampling as described in Sec. II-B.

In the following, we present our particular cost function design for the scenario used in the approach validation. The aerial robot is tasked to track a trajectory in the presence of an obstacle (static or moving). The trajectory is not obstacle free, therefore it is up to the MPPI controller to refine the trajectory in real-time and avoid the obstacle. Moreover, as we will show, due to the MPPI awareness of the closed-loop state sensitivity, by embedding the sensitivity tubes in the

cost function leads to a significantly improved performance in avoiding the obstacle despite of the model uncertainty.

### A. MPPI Cost Function Design

For the design of the MPPI cost function, we consider the objective of tracking a desired position trajectory while penalizing collisions along the prediction horizon. We choose this objective that simplifies the cost function tuning procedure and allows us to analyze the effect of embedding the sensitivity-tubes in MPPI problems without the added complexity of a complex cost function. A complex objective might have an indirect influence on the system's robustness by itself and would affect our interpretation of the results. We consider a prediction horizon $T$ and propose a cost function composed of the following terms.

1) **Position tracking running cost:** Considering a set of desired position way-points given by $\mathbf{p}_v^r(t), t \in [t_0, T]$ that are assumed to be provided by a simple high-level planner, which does not consider the presence of obstacles. We define the following quadratic position tracking running cost as

$$H_{\mathbf{p}_v}(t) = \int_{t_0}^{T} (\mathbf{p}_v - \mathbf{p}_v^r)^T \mathbf{W}_a (\mathbf{p}_v - \mathbf{p}_v^r) d\tau \quad (14)$$

where $\mathbf{W}_a \in \mathbb{R}^{3 \times 3}$ is a diagonal positive definite weighting matrix for the running trajectory tracking cost.

2) **Position tracking terminal cost:** Similarly, we define the quadratic position tracking terminal cost as

$$H_{\mathbf{p}_v}(T) = (\mathbf{p}_{v_T} - \mathbf{p}_{v_T}^r)^T \mathbf{W}_b (\mathbf{p}_{v_T} - \mathbf{p}_{v_T}^r) \quad (15)$$

where $\mathbf{W}_b \in \mathbb{R}^{3 \times 3}$ is the diagonal positive definite weighting matrix for the terminal trajectory tracking cost.

3) **Robust collision penalization cost:** Consider $N_{cp}$ control points along the AR structure and define the euclidean distance between the position of each control point $\mathbf{p}_{cp_j}(\mathbf{q})$ and the position of an obstacle $\mathbf{p}_o$ as

$$d_{cp_j}(\mathbf{p}_{cp_j}, \mathbf{p}_o) = \|\mathbf{p}_{cp_j} - \mathbf{p}_o\| \quad j = 1, \dots, 6. \qquad (16)$$

In a standard non-robust setting, we can define the following collision checking condition:

$$\text{collision} = \begin{cases} \text{true} & \text{if } d_{cp_j} < \sigma \\ \text{false} & \text{otherwise,} \end{cases} \qquad (17)$$

where $\sigma$ represents a safe distance. However, within the context of a predictive controller as MPPI, the previous condition can mistakenly be considered false while it is in fact true, due to model mismatch between the real robot and the nominal model used by the MPPI controller for the prediction. This might lead to a real-world collision.

As described in Sec. IV, in our robust MPPI approach we approximate the error in the model prediction using the sensitivity-tubes. Since $d_{cp_j}(\mathbf{p}_{cp_j}, \mathbf{p}_o)$ is a function of the state, we can compute its sensitivity-tube centered around its nominal evolution as described in eq. (7). We can then redefine the collision checking condition as

$$\text{collision} = \begin{cases} \text{true} & \text{if } d_{cp_j} < \sigma + \alpha_{d_{cp_j}}(t) \\ \text{false} & \text{otherwise,} \end{cases} \qquad (18)$$

where $\alpha_{d_{cp_j}}(t)$ is the radius of a tube centered around the nominal evolution of $d_{cp_j}(t)$, while $\sigma$ is the previously
<figure><img src="image_4.png" /><figcaption>**Fig. 2:** System diagram showing the control architecture (top) and the MPPI controller implementation details (bottom).</figcaption></figure>

defined safe distance. The sensitivity tube is used to effectively tighten the obstacle avoidance constraint based on the uncertainty dynamics given by $\mathbf{\Pi}$. We can then define the robust collision penalization cost as

$$w_{\text{col}}(t) = \begin{cases} c_{\text{col}} & \text{if collision = true} \\ 0 & \text{otherwise.} \end{cases} \qquad (19)$$

where $c_{col}$ is an additional cost incurred if the obstacle is in collision along the prediction horizon, which is incorporated in the cost function as

$$H_{\text{col}}(t) = \int_{t_0}^{T} w_{\text{col}}(\tau) \ d\tau. \qquad (20)$$

It is important to mention that such discontinuous cost functions are easily dealt with in MPPI control due to its sampling-based nature.

The cost function for our OCP is summation of the previously defined terms $H(t) = H_{p_v}(T) + H_{p_v}(t) + H_{\text{col}}(t)$

$$l_T(\mathbf{q}(T), \mathbf{\Pi}(T)) + \int_{t_0}^{T} l(\mathbf{q}(\tau), \mathbf{\Pi}(\tau)) d\tau = H(t). \quad (21)$$

## V. EMBEDDED IMPLEMENTATION

The MPPI controller is implemented on an Nvidia Jetson AGX Xavier (Xavier) embedded computer [12] which has a form-factor feasible for use as onboard computer on an AR while featuring a sufficiently sized GPU. We utilize the ROS2 Humble middleware [13] to handle the communication between the various nodes in the system. Each call to the MPPI controller receives as input the robot state $q_t$, the goal position $p_{goal}$ and the obstacle position $p_o$, and yields the optimal control sequence $a_{opt}$.

The MPPI is implemented using C++ and CUDA [14]. We parallelize the roll-out computations by deploying $N_r$ concurrent threads on the GPU of the Xavier, with $N_r = 250$ being within the available number of CUDA cores. A diagram detailing the implementation is shown in Fig. 2.

The simple high-level planner is used to warm-start the MPPI controller. When invoking the MPPI control node at time $t$, $N_r$ perturbations are sampled uniformly from a range $[\Delta a_{min}, \Delta a_{max}]$ on the nominal trajectory references across the horizon $N_h$. The $N_r$ roll-outs are initialized and

<table><caption>**TABLE I:** The uncertain parameter nominal values and perturbation ranges. Note that we are considering non-trivial and realistic uncertainty ranges which can affect the real robotic system.</caption><thead><tr><th>Parameter</th><th>$m_r$ [kg]</th><th>$||r_c||$ [m]</th><th>$k_f$</th><th>$\frac{N \cdot s^2}{\text{rad}^2}$</th><th>$k_\tau$</th><th>$\frac{N \cdot m \cdot s^2}{\text{rad}^2}$</th></tr></thead><tbody><tr><td>Nominal value</td><td>2.35</td><td>0</td><td>11.5e-04</td><td></td><td>2.38e-05</td><td></td></tr><tr><td>Upper bound</td><td>2.82</td><td>2.45e-02</td><td>14.0e-04</td><td></td><td>2.866e-05</td><td></td></tr><tr><td>Lower bound</td><td>1.88</td><td>-2.45e-02</td><td>9.2e-04</td><td></td><td>1.9107e-05</td><td></td></tr><tr><td>Max uncertainty</td><td>20 %</td><td>NA</td><td>20 %</td><td></td><td>20 %</td><td></td></tr></tbody></table>

started with the sampled perturbation trajectories, each roll-out propagating the system dynamics for a single instance of perturbations on the high-level trajectory accelerations. We compute a single roll-out as a sequential GPU thread where the dynamics are propagated iteratively alongside the accumulation of the roll-out cost. Finally, when the perturbed accelerations weighted by the roll-out costs have been returned from the GPU, the optimal trajectory accelerations are computed.

We build the system by applying a tool-chain consisting of MATLAB and CasADI [15], a set of custom scripts, and the Ament/colcon build system of ROS2 with the nvcc Nvidia CUDA C++ compiler [16]. We discretize the dynamic equations of the AR, eq. (1) and (10), implement the low-level feedback linearization PID controller, eq. (2), and discretize and implement the sensitivity dynamics of eq. (5) to (9) in MATLAB using CasADI. Individual standalone C++ code is generated for each of the implemented functions using CasADI code generation.

The MPPI controller is called with a frequency of 5 Hz with an internal discretization period of 0.02 s and a horizon of 0.5 s, yielding $N_h = 25$. Filtering and linear interpolation is applied to the optimal acceleration sequence to decouple the frequency of the MPPI controller from the frequency of the underlying low-level controller.

<figure><img src="image_5.png" /><figcaption>**Fig. 3:** The magnitudes of the sampled uncertainties used in the experiments as fractions of the parameter ranges for the uncertain payload scenario (top) and close-to-surface scenario (bottom).</figcaption></figure>

## VI. HARDWARE-IN-THE-LOOP SIMULATIONS

We tested the MPPI implementation extensively in simulations of the two scenarios described in Sec. III-A, with
<figure><img src="image_6.png" /><figcaption>**Fig. 4:** The number of successful (obstacle avoided) test runs with and without tubes for each uncertainty sample index for the uncertain payload scenario (left) and the close-to-surface scenario (right) with static obstacle (top) and dynamic obstacle (bottom).</figcaption></figure>

uncertainty on the parameters $m_r$ and $r_c$, as well as $k_f$ and $k_τ$, respectively. A total of **320** test runs were performed. The MPPI controller was asked to guide the robot from the initial position to a goal position while avoiding an obstacle. We considered both the cases of a static or a dynamic obstacle. For both scenarios, the parameters were randomly sampled from a uniform distribution with the ranges in Tab. I. However, for the parameters $m_r$ and $k_f$, the uncertainties were sampled only from either the positive or negative range. This was done so because a smaller than nominal value of $m_r$ as well as a larger than nominal value of $k_f$ would yield the robot to fly further away from the obstacle in the static case, and vice versa for the dynamic case, not contributing to the test scenario. Between the static and dynamic cases, the same parameter values were used across the experiments, with flipped sign on $m_r$ and $k_f$.

The magnitudes of the sampled parameter uncertainties used in the experiments displayed as fractions of the parameter ranges are seen in Fig. 3 with the uncertainty sample index identifying a set of sampled parameters.

For each experiment configuration and uncertainty sample index, the experiment was conducted twice with and without inclusion of the tubes $α_{d_{cp}}$ in the collision cost computation, eq. (17-19). Each run was evaluated by the metric of whether the MPPI controller was able to bring the AR to the goal point without collision with the obstacle. Tracking performance was not considered in the evaluation as our emphasis is studying the effects of parameter uncertainty.

We ran the simulations in MATLAB's Simulink on a desktop computer. Using the MATLAB ROS2 Toolbox, the simulation interfaced with the MPPI controller running on the Xavier, connected to the desktop computer through ethernet. The results of all test runs are presented in Fig. 4. Each test run can be related to level of induced uncertainty by crosschecking the uncertainty sample indices with those of Fig. 3. The results show the performance improvement gained when considering uncertainty in the MPPI controller via the sensitivities. The success rates are summarized in

Tab. II. Finally, the average MPPI controller computation times without tubes and with tubes for each of scenario are listed in Tab. III, showing significantly lower than required computation time indicating that an even less powerful embedded computer could run the system sufficiently.

<table><thead><tr><th>Uncertain payload scenario</th><th>w/o α<sub>d<sub>cp</sub></sub></th><th>w/ α<sub>d<sub>cp</sub></sub></th></tr></thead><tbody><tr><td>Static obstacle</td><td>50%</td><td>100%</td></tr><tr><td>Dynamic obstacle</td><td>37.5%</td><td>90%</td></tr></tbody></table>

<table><caption>**TABLE II:** Percentage successful runs for the various experiment configurations. Note the performance improvement that results from the inclusion of the sensitivity dynamics within the MPPI control</caption><thead><tr><th>Close-to-surface scenario</th><th></th><th></th></tr></thead><tbody><tr><td>Static obstacle</td><td>55%</td><td>100%</td></tr><tr><td>Dynamic obstacle</td><td>42.5%</td><td>80%</td></tr></tbody></table>

<table><caption>**TABLE III:** Average computation time of the MPPI controller running on the embedded computer with and without tubes. It can be seen that with the inclusion of the sensitivity dynamics, the MPPI can be solved in real-time.</caption><thead><tr><th></th><th>w/o α<sub>d<sub>cp</sub></sub></th><th>w/ α<sub>d<sub>cp</sub></sub> on<br>m<sub>r</sub> and r<sub>c</sub></th><th>w/ α<sub>d<sub>cp</sub></sub> on<br>k<sub>f</sub> and k<sub>τ</sub></th></tr></thead><tbody><tr><th>Avg. MPPI<br>computation time</th><td>15.60 ms</td><td>37.46 ms</td><td>39.69 ms</td></tr></tbody></table>

## VII. CONCLUSION

In this paper, we proposed a method to robustify an MPPI controller using closed-loop state sensitivity dynamics. The proposed formulation considers the sensitivity to parameter mismatch as a part of the state. This is then exploited to design a cost function that uses the sensitivity to robustify performance against model parameter uncertainties. We demonstrate our approach using an obstacle avoidance scenario, leading to improved collision avoidance despite parameter uncertainty. We validated that the implementation could satisfy the real-time requirements through hardware-in-the-loop simulations. One interesting direction for future works is considering the online minimization of state's sensitivity to parameter uncertainty within the MPPI cost function, thus generating intrinsically robust motion plans online.
REFERENCES

[1] G. Williams, P. Drews, B. Goldfain, J. M. Rehg, and E. A. Theodorou, "Aggressive driving with model predictive path integral control," in *IEEE Int. Conf. on Robotics and Automation*. IEEE, 2016, pp. 1433-1440.

[2] G. Williams, B. Goldfain, P. Drews, K. Saigol, J. M. Rehg, and E. A. Theodorou, "Robust sampling based model predictive control with sparse objective information." in *Robotics: Science and Systems*, vol. 14, 2018, p. 208.

[3] M. S. Gandhi, B. Vlahov, J. Gibson, G. Williams, and E. A. Theodorou, "Robust model predictive path integral control: Analysis and performance guarantees," *IEEE Robotics and Automation Letters*, vol. 6, no. 2, pp. 1423-1430, 2021.

[4] G. Rizzi, J. J. Chung, A. Gawel, L. Ott, M. Tognon, and R. Siegwart, "Robust sampling-based control of mobile manipulators for interaction with articulated objects," *IEEE Trans. on Robotics*, vol. 39, no. 3, pp. 1929-1946, 2023.

[5] P. Robuffo Giordano, Q. Delamare, and A. Franchi, "Trajectory generation for minimum closed-loop state sensitivity," in *2018 IEEE Int. Conf. on Robotics and Automation*, Brisbane, Australia, May 2018, pp. 286-293.

[6] P. Brault, Q. Delamare, and P. Robuffo Giordano, "Robust trajectory planning with parametric uncertainties," in *IEEE Int. Conf. on Robotics and Automation*, 2021, pp. 11095-11101.

[7] P. Brault and P. Robuffo Giordano, "Tube-based trajectory optimization for robots with parametric uncertainty," http://rainbow-doc.irisa.fr/pdf/tube_based_traj_opt.pdf.

[8] M. Hamandi, F. Usai, Q. Sable, N. Staub, M. Tognon, and A. Franchi, "Design of multirotor aerial vehicles: a taxonomy based on input allocation," *The International Journal of Robotics Research*, vol. 40, no. 8-9, pp. 1015-1044, 2021.

[9] M. Ryll, G. Muscio, F. Pierri, E. Cataldi, G. Antonelli, F. Caccavale, D. Bicego, and A. Franchi, "6D interaction control with aerial robots: The flying end-effector paradigm," *The International Journal of Robotics Research*, vol. 38, no. 9, pp. 1045-1062, 2019.

[10] R. Featherstone, *Rigid body dynamics algorithms*. Springer, 2014.

[11] T. Lee, M. Leoky, and N. H. McClamroch, "Geometric tracking control of a quadrotor UAV on SE(3)," in *49th IEEE Conf. on Decision and Control*, Atlanta, GA, Dec. 2010, pp. 5420-5425.

[12] Nvidia, "Nvidia Jetson AGX Xavier product page," https://www.nvidia.com/en-us/autonomous-machines/embedded-systems/jetson-xavier-series/, accessed: 04/03/2024.

[13] S. Macenski, T. Foote, B. Gerkey, C. Lalancette, and W. Woodall, "Robot Operating System 2: Design, architecture, and uses in the wild," *Science Robotics*, vol. 7, no. 66, p. eabm6074, 2022. [Online]. Available: https://www.science.org/doi/abs/10.1126/scirobotics.abm6074

[14] NVIDIA, P. Vingelmann, and F. H. Fitzek, "Cuda," https://developer.nvidia.com/cuda-toolkit, 2020, accessed: 04/03-2024.

[15] J. A. E. Andersson, J. Gillis, G. Horn, J. B. Rawlings, and M. Diehl, "CasADi – A software framework for nonlinear optimization and optimal control," *Mathematical Programming Computation*, vol. 11, no. 1, pp. 1-36, 2019.

[16] Nvidia, "NVIDIA CUDA Compiler Driver nvcc," https://docs.nvidia.com/cuda/cuda-compiler-driver-nvcc/index.html, accessed: 04/03/2024.