# Towards Efficient MPPI Trajectory Generation with Unscented Guidance: U-MPPI
Control Strategy

Ihab S. Mohamed1, Junhong Xu1, Gaurav Sukhatme2, and Lantao Liu1 1Ihab S.
Mohamed, Junhong Xu, and Lantao Liu (corresponding author) are with the Luddy
School of Informatics, Computing, and Engineering, Indiana University,
Bloomington, IN 47408 USA (e-mail: {mohamedi, xu14, lantao}@iu.edu)2Gaurav
Sukhatme is with the Department of Computer Science, University of Southern
California, Los Angeles, CA 90089, USA (e-mail<EMAIL_ADDRESS>
###### Abstract

The classical Model Predictive Path Integral (MPPI) control framework lacks
reliable safety guarantees since it relies on a risk-neutral trajectory
evaluation technique, which can present challenges for safety-critical
applications such as autonomous driving. Additionally, if the majority of MPPI
sampled trajectories concentrate in high-cost regions, it may generate an
infeasible control sequence. To address this challenge, we propose the U-MPPI
control strategy, a novel methodology that can effectively manage system
uncertainties while integrating a more efficient trajectory sampling strategy.
The core concept is to leverage the Unscented Transform (UT) to propagate not
only the mean but also the covariance of the system dynamics, going beyond the
traditional MPPI method. As a result, it introduces a novel and more efficient
trajectory sampling strategy, significantly enhancing state-space exploration
and ultimately reducing the risk of being trapped in local minima.
Furthermore, by leveraging the uncertainty information provided by UT, we
incorporate a risk-sensitive cost function that explicitly accounts for risk
or uncertainty throughout the trajectory evaluation process, resulting in a
more resilient control system capable of handling uncertain conditions. By
conducting extensive simulations of 2D aggressive autonomous navigation in
both known and unknown cluttered environments, we verify the efficiency and
robustness of our proposed U-MPPI control strategy compared to the baseline
MPPI. We further validate the practicality of U-MPPI through real-world
demonstrations in unknown cluttered environments, showcasing its superior
ability to incorporate both the UT and local costmap into the optimization
problem without introducing additional complexity.

###### Index Terms:

Autonomous vehicle navigation, MPPI, unscented transform, occupancy grid map
path planning.

##  I Introduction

Figure 1: Our proposed sampling strategy, for a ground vehicle model, under
the U-MPPI control strategy based on unscented transform; such a sampling
strategy propagates both the mean $\bar{\mathbf{x}}_{k}$ (blue dots) and
covariance $\mathbf{\Sigma}_{k}$ (gray ellipses) of the state vector at each
time-step $k$; to generate $M$ sampled trajectories, we propagate $M_{\sigma}$
sets of batches, where each batch contains $n_{\sigma}$ trajectories
corresponding to the $n_{\sigma}$ sigma points, where
$M=n_{\sigma}M_{\sigma}$, $n_{\sigma}=2n_{x}+1$, and red lines refer to
$2n_{x}$ sigma-point trajectories surrounding the nominal trajectories (blue
lines); for our validation-used robot, $n_{x}=3$.

Planning and controlling autonomous vehicles under uncertainty is a partially-
solved yet highly challenging problem in robotics. The uncertainty or risk can
arise from multiple sources, such as the vehicle’s dynamics, the dynamics of
other objects in the environment, the presence of unexpected obstacles, and
the accuracy of the sensors used to perceive the environment. These factors
can lead to unpredictable vehicle behavior, making it challenging to
anticipate its movements and actions. Therefore, it is crucial for motion
planning and control algorithms to account for the uncertainties of the
system’s states, sensing, and control actions. This eventually enables
autonomous vehicles to adapt to unexpected environmental changes and make
reliable decisions in real-time [1].

Model Predictive Control (MPC), also referred to as receding-horizon optimal
control, has been introduced as an effective solution for improving system
safety and managing random disturbances and uncertainties, owing to its
flexibility and ability to handle system constraints and nonlinearities while
optimizing system performance. It plans a sequence of optimal control inputs
over a finite time-horizon by repeatedly solving the optimal control problem
using the receding-horizon principle, with the first control input applied to
the system. There are two main categories of MPC frameworks that can handle
various forms of uncertainty: Robust MPC (RMPC) and Stochastic MPC (SMPC).
RMPC considers worst-case scenarios of uncertainty to ensure stability and
constraint satisfaction, while SMPC takes into account expected costs and
probabilistic constraints (e.g., chance constraints) to optimize performance
under uncertain conditions. RMPC is known to generate the safest solutions
among MPC categories due to its incorporation of worst-case scenarios into the
optimization problem and its emphasis on minimizing worst-case objective
functions. However, it may lead to excessively conservative control actions,
resulting in low system performance [2, 3].

On the other hand, despite the superior ability of SMPC to leverage the
probabilistic nature of uncertainties, many SMPC approaches have performance
limitations, including being tailored to specific forms of stochastic noise,
requiring dynamics linearization to ensure a real-time performance and
implementation of the control strategy, and employing chance constraints that
may not accurately reflect the severity of constraint violations or potential
accidents and can be computationally demanding to evaluate, particularly for
complex or high-dimensional probability distributions [4, 5]. Additionally,
SMPC has a further limitation in that it relies on a risk-neutral expectation
to predict future uncertain outcomes, which may not be reliable in the case of
a tail-end probability event (i.e., a rare event with a low probability of
occurring) actually happening [6]. To address the challenges posed by
uncertainties, Risk-Sensitive MPC (RSMPC) approaches have gained traction in
recent years, thanks to their ability to balance the benefits and drawbacks of
robust and stochastic MPC methods. By integrating the concept of risk measures
or risk metrics into the optimization problem, RSMPC can evaluate the impact
of uncertainty and adjust responses accordingly to different levels of
uncertainty [7, 8, 9].

The Model Predictive Path Integral (MPPI) framework, a type of SMPC method,
has emerged as a promising control strategy for complex robotics systems with
stochastic dynamics and uncertainties [10, 11, 12, 13]. Such a method solves
the stochastic optimal control problem in a receding-horizon control setting
by: (i) leveraging Monte Carlo simulation to rollout real-time simulated
trajectories propagated from the system dynamics, (ii) evaluating these
trajectories, (iii) computing the optimal control sequence by taking the
weighted average of the costs of the sampled trajectories, and (iv) applying
the first control input to the system while using the remaining control
sequence to warm-start the optimization in the next time-step, enabling the
method to solve the optimization problem effectively [11]. MPPI stands out
among alternative MPC methods due to its attractive features, such as being a
sampling-based and derivative-free optimization method, not relying on
assumptions or approximations of objective functions and system dynamics,
being effective for highly dynamic systems, and benefiting from parallel
sampling and the computational capabilities of Graphics Processing Units
(GPUs) to achieve optimized and real-time performance [14].111It is worth
noting that a CPU-based MPPI, optimized using vectorization and tensor
operations, is now available. See the link for further information:
https://github.com/artofnothingness/mppic

While MPPI has appealing characteristics, it may also pose challenges in
practice. One particular concern is that, much like any sampling-based
optimization algorithm, it could generate an infeasible control sequence if
all the resulting MPPI sampled trajectories are concentrated in a high-cost
region, which may lead to violations of system constraints or a higher
likelihood of being trapped in local minima [15, 16]. In [17], Tube-MPPI was
proposed as a solution to alleviate the situation by incorporating an
iterative Linear Quadratic Gaussian (iLQG) controller (which, unfortunately,
requires the linearization of dynamics) as an ancillary controller to track
the MPPI-generated nominal trajectory. Similarly, in [13], an augmented
version of MPPI is employed, which includes a nonlinear $\mathcal{L}_{1}$
adaptive controller to address model uncertainty. Recently, novel sampling
techniques have been introduced to enhance the performance of MPPI, as
discussed in [15] and [16]. In [15], the MPPI algorithm is enhanced by
incorporating the covariance steering (CS) principle, while [16] proposes
sampling trajectories from a product of normal and log-normal distributions
(NLN mixture), instead of solely using a Gaussian distribution. These methods
result in more efficient trajectories than the vanilla MPPI, leading to better
exploration of the system’s state-space and reducing the risk of encountering
local minima.

Another constraint of MPPI is its inability to explicitly incorporate risk
levels during planning due to its risk-neutral technique in evaluating sampled
trajectories during the optimization process, making it challenging to achieve
the desired balance between risk and robustness. Additionally, the MPPI
optimization problem concentrates solely on minimizing the objective function,
which is influenced by a minor perturbation injected into the control input,
without explicitly considering any uncertainties or risks associated with the
system dynamics or the environment. This can eventually lead to sub-optimal or
overly aggressive control actions, as MPPI may select trajectories that appear
to have a low expected cost but may actually be riskier or less robust in
practice. Consequently, MPPI cannot guarantee safety when environmental
conditions change, which limits its applicability for safety-critical
applications such as autonomous driving. The Risk-aware MPPI (RA-MPPI)
algorithm [18] is a more recent approach that addresses this issue by
utilizing Conditional Value-at-Risk (CVaR) to generate risk-averse controls
that evaluate real-time risks and account for systematic uncertainties.
However, such a method employs Monte-Carlo sampling to estimate the CVaR,
which can be computationally intensive and time-consuming since generating a
large number of random samples is required for accurate estimation using Monte
Carlo methods.

In contrast to existing solutions aimed at mitigating the shortcomings of
MPPI, our proposed solution can effectively reflect the uncertainties of
system states, sensing, and control actions, in addition to integrating a
novel and more efficient trajectory sampling strategy. To this end, we
introduce the U-MPPI control strategy, a novel methodology that enhances the
classical MPPI algorithm by combining the Unscented Transform (UT) with
standard optimal control theory (also known as unscented guidance [19]) to
effectively manage system uncertainties. Such a control strategy leverages the
UT for two purposes: regulating the propagation of the dynamical system and
proposing a new state-dependent cost function formulation that incorporates
uncertainty information. To the best of the authors’ knowledge, the proposed
control strategy has not been previously discussed in the literature. In
summary, the contributions of this work can be summarized as follows:

  1. 1.

While vanilla MPPI variants propagate only the mean value of the system
dynamics, as depicted in Fig. 2, we propose a novel trajectory sampling
technique equipped with a more effective sampling distribution policy based on
UT, this technique utilizes UT to propagate both the mean and covariance of
the system dynamics at each time-step, as demonstrated in Figs. 1 and 3, and
explained in detail in Section IV-A; by doing so, our new sampling method
achieves significantly better exploration of the state-space of the given
system and reduces the risk of getting trapped in local minima.

  2. 2.

Then, by utilizing the propagated uncertainty information (i.e., state
covariance matrix), we introduce a risk-sensitive cost function that
explicitly considers risk or uncertainty during the trajectory evaluation
process, leading to a safer and more robust control system, especially for
safety-critical applications, as discussed in Section IV-B.

  3. 3.

In Section V, we validate the effectiveness of our U-MPPI control strategy for
aggressive collision-free navigation in both known and unknown cluttered
environments using intensive simulations; by comparing it with the baseline
MPPI, we demonstrate its superiority in producing more efficient trajectories
that explore the state-space of the system better, resulting in higher success
and task completion rates, reducing the risk of getting trapped in local
minima, and ultimately leading the robot to find feasible trajectories that
avoid collisions.

##  II Stochastic Optimal Control

This section aims to establish the problem statement of stochastic optimal
control and present a concise overview of MPPI as a potential solution to
address this problem.

###  II-A Problem Formulation

Within the context of discrete-time stochastic systems, let us consider the
system state $\mathbf{x}_{k}\in\mathbb{R}^{n_{x}}$, the control input
$\mathbf{u}_{k}\in\mathbb{R}^{n_{u}}$, and the underlying non-linear dynamics

$\mathbf{x}_{k+1}=f\left(\mathbf{x}_{k},\mathbf{w}_{k}\right).$ (1)

The actual (i.e., disturbed) control input, $\mathbf{w}_{k}$, is represented
as
$\mathbf{w}_{k}=\mathbf{u}_{k}+\delta\mathbf{u}_{k}\sim\mathcal{N}(\mathbf{u}_{k},\Sigma_{\mathbf{u}})$,
where $\delta\mathbf{u}_{k}\sim\mathcal{N}(\mathbf{0},\Sigma_{\mathbf{u}})$ is
a zero-mean Gaussian noise with covariance $\Sigma_{\mathbf{u}}$ which
represents the injected disturbance into the control input. Within a finite
time-horizon $N$, we denote the control sequence
$\mathbf{U}=\left[\mathbf{u}_{0},\mathbf{u}_{1},\dots,\mathbf{u}_{N-1}\right]^{\top}\in\mathbb{R}^{n_{u}N}$
and the corresponding state trajectory
$\mathbf{x}=\left[\mathbf{x}_{0},\mathbf{x}_{1},\dots,\mathbf{x}_{N}\right]^{\top}\in\mathbb{R}^{n_{x}(N+1)}$.
Moreover, let $\mathcal{X}^{d}$ denote the $d$ dimensional space with
$\mathcal{X}_{rob}\left(\mathbf{x}_{k}\right)\subset\mathcal{X}^{d}$ and
$\mathcal{X}_{obs}\subset\mathcal{X}^{d}$ represent the area occupied by the
robot and obstacles, respectively. In this scenario, the objective of the
stochastic optimal control problem is to find the optimal control sequence,
$\mathbf{U}$, that generates a collision-free trajectory, guiding the robot
from its initial state, $\mathbf{x}_{s}$, to the desired state,
$\mathbf{x}_{f}$, under the minimization of the cost function, $J$, subject to
specified constraints. The optimization problem at hand can be formulated
using the vanilla MPPI control strategy as

$\displaystyle\min_{\mathbf{U}}\,\;J(\mathbf{x},\mathbf{u})$
$\displaystyle=\mathbb{E}\left[\phi\left(\mathbf{x}_{N}\right)+\\!\\!\sum_{k=0}^{N-1}\\!\\!\left(\\!q\\!\left(\mathbf{x}_{k}\right)+\frac{1}{2}\mathbf{u}_{k}^{\top}R\mathbf{u}_{k}\\!\\!\right)\\!\\!\right]\\!,$
(2a) s.t.
$\displaystyle\mathbf{x}_{k+1}=f\left(\mathbf{x}_{k},\mathbf{w}_{k}\right),\delta\mathbf{u}_{k}\sim\mathcal{N}(\mathbf{0},\Sigma_{\mathbf{u}}),$
(2b)
$\displaystyle\mathcal{X}_{rob}\left(\mathbf{x}_{k}\right)\cap\mathcal{X}_{obs}=\emptyset,\;h(\mathbf{x}_{k},\mathbf{u}_{k})\leq
0,$ (2c)
$\displaystyle\mathbf{x}_{0}=\mathbf{x}_{s},\;\mathbf{u}_{k}\in\mathbb{U},\;\mathbf{x}_{k}\in\mathbb{X},$
(2d)

where $R\in\mathbb{R}^{n_{u}\times n_{u}}$ is a positive-definite control
weighting matrix, $\mathbb{U}$ denotes the set of admissible control inputs,
and $\mathbb{X}$ denotes the set of all possible states $\mathbf{x}_{k}$; the
state terminal cost function, $\phi\left(\mathbf{x}_{N}\right)$, and the
running cost function, $q\left(\mathbf{x}_{k}\right)$, can be defined as
arbitrary functions, offering a more flexible and dynamic approach to cost
modeling that can be adapted to meet the specific requirements of the system
being controlled.

###  II-B Overview of MPPI Control Strategy

MPPI solves the optimization problem defined in (2) by minimizing the
objective function $J$ (2a), taking into account the system dynamics (2b) and
constraints, including collision avoidance and control constraints, detailed
in (2c). To this end, at each time-step $\Delta t$, MPPI employs the Monte
Carlo simulation to sample thousands of real-time simulated trajectories,
represented by $M$, propagated from the underlying system dynamics, as
illustrated in Fig. 2.

Figure 2: Schematic illustration of system dynamics propagation in MPPI for
$M$ sampled trajectories over a finite time-horizon $N$.

Subsequently, within the time-horizon $N$, the cost-to-go of each trajectory
$\tau_{i}$ can be evaluated as

$\tilde{S}\left(\tau_{i}\right)=\phi\left(\mathbf{x}_{N}\\!\right)+\\!\\!\sum_{k=0}^{N-1}\\!\tilde{q}\left(\mathbf{x}_{k},\mathbf{u}_{k},\delta\mathbf{u}_{k,i}\right),\forall
i\in\\!\\{1,\cdots\\!,M\\},$ (3)

where $\phi(\mathbf{x}_{N})$ refer to the terminal state cost, while the
instantaneous running cost
$\tilde{q}\left(\mathbf{x}_{k},\mathbf{u}_{k},\delta\mathbf{u}_{k}\right)$
encompasses both the state-dependent running cost
$q\left(\mathbf{x}_{k}\right)$ and the quadratic control cost
$q\left(\mathbf{u}_{k},\delta\mathbf{u}_{k}\right)$ and is formulated as

$\tilde{q}\\!=\\!\underbrace{\vphantom{\gamma_{\mathbf{u}}\delta\mathbf{u}_{k}^{\top}R\delta\mathbf{u}_{k}+\mathbf{u}_{k}^{\top}R\delta\mathbf{u}_{k}+\frac{1}{2}\mathbf{u}_{k}^{\top}R\mathbf{u}_{k}}q\left(\mathbf{x}_{k}\\!\right)}_{\text{\color[rgb]{0,0,0}\definecolor[named]{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@gray@stroke{0}\pgfsys@color@gray@fill{0}{{State-
dep.}}}}\\!+\underbrace{\gamma_{\mathbf{u}}\delta\mathbf{u}_{k,i}^{\top}R\delta\mathbf{u}_{k,i}\\!+\mathbf{u}_{k}^{\top}R\delta\mathbf{u}_{k,i}\\!+\frac{1}{2}\mathbf{u}_{k}^{\top}R\mathbf{u}_{k}}_{\text{\color[rgb]{0,0,0}\definecolor[named]{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@gray@stroke{0}\pgfsys@color@gray@fill{0}{{$q\left(\mathbf{u}_{k},\delta\mathbf{u}_{k}\right)$:
Quadratic Control Cost}}}},$ (4)

where $\gamma_{\mathbf{u}}=\frac{\nu-1}{2\nu}$, and $\nu\in\mathbb{R}^{+}$
determines the level of aggressiveness in exploring the state-space.

As stated in [11], the vanilla MPPI algorithm updates the optimal control
sequence $\left\\{\mathbf{u}_{k}\right\\}_{k=0}^{N-1}$ by considering a
weighted average cost from all of the simulated trajectories; mathematically,
this control sequence can be expressed as

$\mathbf{u}_{k}\leftarrow\mathbf{u}_{k}+\frac{\sum_{m=1}^{M}\exp\Bigl{(}\frac{-1}{\lambda}\left[\tilde{S}\left(\tau_{m}\right)-\tilde{S}_{\min}\right]\Bigr{)}\delta\mathbf{u}_{k,m}}{\sum_{m=1}^{M}\exp\Bigl{(}\frac{-1}{\lambda}\left[\tilde{S}\left(\tau_{m}\right)-\tilde{S}_{\min}\right]\Bigr{)}},$
(5)

which incorporates the cost-to-go of the $m^{\text{th}}$ trajectory
$\tilde{S}\left(\tau_{m}\right)$, the minimum cost trajectory among all
simulated rollouts $\tilde{S}_{\min}$ that prevents numerical overflow or
underflow, without affecting the optimality of the algorithm, and the inverse
temperature $\lambda$ that governs the selectiveness of the weighted average
of trajectories. The resulting sequence is then smoothed with a Savitzky-Galoy
filter [20], followed by applying the first control $\mathbf{u}_{0}$ to the
system, while the remaining sequence is utilized for warm-starting the next
optimization step.

##  III Unscented Optimal Control

In this section, we present a comprehensive overview of the unscented
transform, followed by the unscented guidance technique, which combines the
unscented transform with standard stochastic optimal control theory to
generate a robust open-loop method for managing system uncertainties.

###  III-A Unscented Transform

The unscented transform (UT), proposed by Julier and Uhlmann [21], aims to
create nonlinear filters without the need for linearization. It approximates a
probability distribution function (PDF) after it passes through a non-linear
transformation using a set of sampled points, known as sigma points [22].

Formally, given the mean $\bar{\mathbf{x}}_{k}$ and covariance
$\mathbf{\Sigma}_{k}$ of a Gaussian-distributed system state $\mathbf{x}_{k}$,
with
$\mathbf{x}_{k}\sim\mathcal{N}(\bar{\mathbf{x}}_{k},\mathbf{\Sigma}_{k})$, UT
approximates the distribution over the next state, $\mathbf{x}_{k+1}$, by
first introducing a set of sigma points
$\big{\\{}\mathcal{X}_{k}^{(i)}\big{\\}}^{2n_{x}}_{i=0}\in\mathbb{R}^{n_{\sigma}}$
around the mean $\bar{\mathbf{x}}_{k}$ and the corresponding weights
$\big{\\{}w^{(i)}\big{\\}}^{2n_{x}}_{i=0}\in\mathbb{R}^{n_{\sigma}}$, where
$n_{\sigma}=2n_{x}+1$. These sigma points are designed to capture the
covariance of the distribution at time-step $k$ as follows

$\displaystyle\mathcal{X}_{k}^{(0)}$ $\displaystyle=\bar{\mathbf{x}}_{k},$ (6)
$\displaystyle\mathcal{X}_{k}^{(i)}$
$\displaystyle=\bar{\mathbf{x}}_{k}+\Big{(}\sqrt{(n_{x}+\lambda_{\sigma})\mathbf{\Sigma}_{k}}\Big{)}_{\\!i},\;\forall
i=\\{1,\dots,n_{x}\\},$ $\displaystyle\mathcal{X}_{k}^{(i)}$
$\displaystyle=\bar{\mathbf{x}}_{k}-\Big{(}\\!\sqrt{(n_{x}\\!+\\!\lambda_{\sigma}\\!)\mathbf{\Sigma}_{k}}\Big{)}_{\\!i},\;\forall
i\\!=\\!\\{n_{x}\\!+\\!1,\dots,2n_{x}\\!\\},$

where $\Big{(}\sqrt{(n_{x}+\lambda_{\sigma})\mathbf{\Sigma}_{k}}\Big{)}_{i}$
represents the $i^{\text{th}}$ row or column of the square root of the
weighted covariance matrix $(n_{x}+\lambda_{\sigma})\mathbf{\Sigma}_{k}$, and
$\lambda_{\sigma}=\alpha^{2}(n_{x}+k_{\sigma})-n_{x}$ is influenced by the
scaling parameters $k_{\sigma}\geq 0$ and $\alpha\in(0,1]$ that determine how
far the sigma points are spread from the mean [23], as demonstrated in Fig. 4.
Each $\mathcal{X}_{k}^{(i)}$ is associated with two weights, $w^{(i)}_{m}$ for
computing the mean and $w^{(i)}_{c}$ for determining the covariance of the
transformed distribution, computed as

$\displaystyle w_{m}^{(0)}$
$\displaystyle=\frac{\lambda_{\sigma}}{n_{x}+\lambda_{\sigma}},$ (7)
$\displaystyle w_{c}^{(0)}$ $\displaystyle=w_{m}^{(0)}+(1-\alpha^{2}+\beta),$
$\displaystyle w_{m}^{(i)}$
$\displaystyle=w_{c}^{(i)}=\frac{1}{2(n_{x}+\lambda_{\sigma})},$

where $\beta$ is a hyper-parameter controlling the relative importance of the
mean and covariance information. In other words, $\beta$ is employed to
incorporate prior knowledge about the distribution of the state $\mathbf{x}$.
For Gaussian distributions, the optimal value for $\beta$ is 2 [24]. In the
second step, we propagate the $(2n_{x}+1)$ sigma points through the non-linear
system to produce the transformed sigma points
$\mathcal{X}_{k+1}^{(i)}=f(\mathcal{X}_{k}^{(i)})$ at the next time-step.
Finally, the mean and covariance of $\mathbf{x}_{k+1}$ can be estimated using
the transformed sigma points and their corresponding weights, as follows

$\displaystyle\mathbf{\bar{x}}_{k+1}$
$\displaystyle=\sum_{i=0}^{2n_{x}}w_{m}^{(i)}\mathcal{X}_{k+1}^{(i)},$ (8)
$\displaystyle\mathbf{\Sigma}_{k+1}$
$\displaystyle=\sum_{i=0}^{2n_{x}}w_{c}^{(i)}(\mathcal{X}_{k+1}^{(i)}-\mathbf{\bar{x}}_{k+1})(\mathcal{X}_{k+1}^{(i)}-\mathbf{\bar{x}}_{k+1})^{\top}.$

###  III-B Unscented Optimal Control

Figure 3: Schematic illustration of nonlinear dynamical system propagation
under the proposed U-MPPI control strategy for $M$ sampled trajectories over a
finite time-horizon $N$, where $M=n_{\sigma}M_{\sigma}$.

By incorporating the unscented transform with standard optimal control, the
unscented optimal control, also referred to as unscented guidance [19],
presents a novel methodology for addressing the uncertainties in non-linear
dynamical systems within an open-loop framework [25, 26]. Given the sigma
points $\mathcal{X}_{k}^{(i)}$ and disturbed control input $\mathbf{w}_{k}$ at
time-step $k$, each sigma point can be propagated through the underlying non-
linear dynamics given in (1), as follows

$\mathcal{X}_{k+1}^{(i)}=f\left(\mathcal{X}_{k}^{(i)},\mathbf{w}_{k}\right),\forall
i=0,\cdots,2n_{x}\in\mathbb{R}^{n_{\sigma}}.$ (9)

Consider an $n_{\sigma}n_{x}$-dimensional vector $\mathbf{X}$, defined as
$\mathbf{X}=\left[\mathcal{X}^{(0)},\mathcal{X}^{(1)},\ldots,\mathcal{X}^{\left(2n_{x}\right)}\right]^{\top}\in\mathbb{R}^{n_{x}n_{\sigma}}$.
Then, the dynamics of $\mathbf{X}$ are characterized by $n_{\sigma}$ instances
of the function $f$, specified as

$\bm{X}_{k+1}=\left[\begin{array}[]{c}f\left(\mathcal{X}_{k}^{(0)},\mathbf{w}_{k}\right)\\\
f\left(\mathcal{X}_{k}^{(1)},\mathbf{w}_{k}\right)\\\ \vdots\\\
f\left(\mathcal{X}_{k}^{(2n_{x})},\mathbf{w}_{k}\right)\end{array}\right]:=\mathbf{f}(\bm{X}_{k},\mathbf{w}_{k}).$
(10)

With these preliminaries, the original stochastic optimal control problem
described in (2) can be re-formulated within the context of the unscented
guidance framework as

$\displaystyle\min_{\mathbf{U}}\;\\!\mathbf{J}(\mathbf{X}$
$\displaystyle,\mathbf{u})=\mathbb{E}\\!\left[\mathbf{\Phi}\\!\left(\mathbf{X}_{N}\\!\right)\\!+\\!\\!\\!\sum_{k=0}^{N-1}\\!\\!\left(\\!\mathbf{q}\\!\left(\mathbf{X}_{k}\right)\\!+\\!\frac{1}{2}\mathbf{u}_{k}^{\top}R\mathbf{u}_{k}\\!\\!\right)\\!\\!\right]\\!\\!,$
(11a) s.t.
$\displaystyle\mathbf{X}_{k+1}=\mathbf{f}\left(\mathbf{X}_{k},\mathbf{w}_{k}\right),\delta\mathbf{u}_{k}\sim\mathcal{N}(\mathbf{0},\Sigma_{\mathbf{u}}),$
(11b)
$\displaystyle\mathcal{X}_{rob}\left(\mathbf{X}_{k}\right)\cap\mathcal{X}_{obs}=\emptyset,\;\mathbf{h}(\mathbf{X}_{k},\mathbf{u}_{k})\leq
0,$ (11c)
$\displaystyle\mathbf{X}_{0}=\left[\mathcal{X}_{0}^{(0)}\\!,\ldots,\mathcal{X}_{0}^{(2n_{x})}\right]^{\top}\\!\\!,\mathbf{u}_{k}\\!\in\mathbb{U},\mathbf{X}_{k}\\!\in\mathbb{X},$
(11d)

where
$\mathbf{\Phi}\left(\mathbf{X}_{N}\right)\\!\\!=\\!\\!\left[\phi\left(\mathcal{X}_{N}^{(0)}\right),\ldots,\phi\left(\mathcal{X}_{N}^{(2n_{x})}\right)\right]^{\top}\\!\\!\\!\in\\!\mathbb{R}^{n_{\sigma}}$,
and
$\mathbf{q}\left(\mathbf{X}_{k}\right)=\left[q\left(\mathcal{X}^{(0)}_{k}\right),\ldots,q\left(\mathcal{X}^{(2n_{x})}_{k}\right)\right]^{\top}\\!\\!\in\mathbb{R}^{n_{\sigma}}$.
The objective of our proposed U-MPPI control strategy, as detailed in Section
IV, is to minimize the objective function, $\mathbf{J}$, in (11a) by finding
the optimal control sequence,
$\mathbf{U}=\left\\{\mathbf{u}_{k}\right\\}_{k=0}^{N-1}$, while taking into
account (i) the system constraints previously discussed in Section II-A, and
(ii) the uncertainties associated with both system states and control actions.

##  IV U-MPPI Control Strategy

As previously outlined in Section II-B, the control noise variance
$\Sigma_{\mathbf{u}}$ is not updated by MPPI, and the state-space exploration
is performed by adjusting $\nu$ (refer to (4)). Nevertheless, a too-high value
of $\nu$ can cause control inputs with considerable chatter. Similarly,
increasing $\Sigma_{\mathbf{u}}$ might violate system constraints and eventual
divergence from the desired state [16]. Additionally, the MPPI problem, as
stated in (2), is focused solely on minimizing the cost function that is
affected by a minor perturbation in the control input, represented by
$\delta\mathbf{u}_{k}$, without explicitly incorporating the uncertainties
that may be associated with either the system states or the surrounding
environment. To ensure effectiveness in practice, the motion control strategy
should be able to reflect the uncertainties of system states, sensing, and
control actions. To this end, we introduce the U-MPPI control strategy, a new
technique that leverages unscented transform to deal with these uncertainties.
More precisely, the Unscented Transform (UT) is utilized for the purpose of
regulating the propagation of the dynamical system, thereby introducing a
novel and more efficient trajectory sampling strategy than the standard MPPI
variants, as demonstrated in Section IV-A. Furthermore, as discussed in
Section IV-B, UT proposes a new cost function formulation incorporating
uncertainty information, leading to a safer and more robust control system,
especially for safety-critical applications.

###  IV-A Unscented-Based Sampling Strategy

By leveraging Monte Carlo simulation, the vanilla MPPI algorithm simulates a
large number of real-time trajectories $M$, propagated from the system
dynamics defined in (1), by solely manipulating the injected Gaussian noise
into the mean control sequence (see Fig. 2). Additionally, the computation of
the propagated states during the time period of $N$ is restricted to the mean
value or the first moment with respect to the initial state $\mathbf{x}_{0}$,
without propagating the covariance of $\mathbf{x}_{k}$. Therefore, to enhance
the performance of the classic MPPI algorithm, we propose a new trajectory
sampling technique that utilizes the Unscented Transform (UT) to propagate
both the mean $\bar{\mathbf{x}}_{k}$ and covariance $\mathbf{\Sigma}_{k}$ of
the state vector $\mathbf{x}_{k}$ at each time-step $k$.

In Fig. 3, we illustrate how the sigma points propagate through the nonlinear
dynamical system within the U-MPPI control framework, leading to a total of
$M$ rollouts. These rollouts are achieved by sampling $M_{\sigma}$ sets of
batches, also referred to as cones, with each batch comprising $n_{\sigma}$
trajectories, such that $M=n_{\sigma}M_{\sigma}$. The propagation process of
our proposed sampling strategy can be summarized in the following steps. At
time-step $k=0$, we compute $n_{\sigma}$ sigma points
$\big{\\{}\mathcal{X}_{0}^{(i)}\big{\\}}^{2n_{x}}_{i=0}$ using (6), given the
initial state
$\mathbf{x}_{0}\sim\mathcal{N}(\bar{\mathbf{x}}_{0},\mathbf{\Sigma}_{0})$. We
then apply the underlying non-linear dynamics expressed in (9) to the sigma
points, which can be merged into a single vector using (10). Finally, we use
the resulting sigma points $\mathbf{X}_{1}$ at $k=1$ to estimate the first and
second moments, namely $\bar{\mathbf{x}}_{1}$ and $\mathbf{\Sigma}_{1}$, of
the propagated state vector $\mathbf{x}_{1}$ by applying (8). This propagation
process is repeated until $k=N-1$, resulting in a sequence of state vectors
denoted as $\big{\\{}\mathbf{X}_{k}\big{\\}}^{N}_{i=0}$, which represent the
$n_{\sigma}$ propagated sigma points. This entire process is carried out for
each batch, resulting in a total of $M$ trajectories. In this study, during
time-step $k$, we assume that all sigma points belonging to the
$m^{\text{th}}$ batch are being affected by the same Gaussian control noise
represented by $\delta\mathbf{u}_{k,m}$. Figure 1 depicts the visual
representation of our proposed unscented-based sampling strategy displayed in
Fig. 3.

(a)

(b)

(c)

(d)

Figure 4: Distribution of $210$ sampled trajectories generated by (a) MPPI
with $\delta\mathbf{u}_{k}\sim\mathcal{N}(\mathbf{0},0.025\mathbf{I}_{2})$ and
(b) U-MPPI with the same perturbation in the control input
$\delta\mathbf{u}_{k}$ but with different UT parameters; in both methods, the
robot is assumed to be initially located at
$\mathbf{x}=[{x},{y},\theta]^{\top}=[0,0,0]^{\top}$ in ([$\mathrm{m}$],
[$\mathrm{m}$], [$\deg$]), with a commanded control input
$\mathbf{u}=[v,\omega]^{\top}=[1,0]^{\top}$ in
([$\mathrm{m}\mathrm{/}\mathrm{s}$], [$\mathrm{rad}\mathrm{/}\mathrm{s}$]).

The primary objective of the new sampling strategy is to achieve significantly
better exploration of the state-space of the given system and more efficient
sampling of trajectories compared to MPPI, while utilizing the same injected
control noise $\Sigma_{\mathbf{u}}$. To exemplify how leveraging the UT for
trajectory sampling can enhance the performance of the MPPI algorithm and to
investigate the effect of UT parameters on the distribution of sampled
rollouts, we present a concrete example in Fig. 4. In particular, using the
discrete-time kinematics model of a differential wheeled robot from [16] and
control schemes parameters listed in Section V-A1, we generate $210$ rollouts
by sampling $\delta\mathbf{u}_{k}$ from a zero-mean Gaussian distribution with
a covariance of $0.025\mathbf{I}_{2}$ under the classical MPPI framework, as
illustrated in Fig. 4(a), where $\mathbf{I}_{n}$ denotes an $n\times n$
identity matrix. Similarly, we employ the unscented-based sampling strategy to
draw $210$ trajectories, considering: (i) the same injected disturbance into
the control input, i.e.,
$\delta\mathbf{u}_{k}\sim\mathcal{N}(\mathbf{0},0.025\mathbf{I}_{2})$, and
(ii) setting the UT scaling parameters to $\alpha=1$ and $k_{\sigma}=0$, along
with an initial state covariance matrix $\mathbf{\Sigma}_{0}$ of
$0.01\mathbf{I}_{3}$, as illustrated in Fig. 4(b). It is noteworthy to observe
in Fig. 4(b) that our proposed sampling strategy is more efficient than the
classical MPPI sampling strategy, as it generates more spread-out trajectories
that cover a larger state space. This enables the robot to explore the
environment more extensively and find better solutions, thereby reducing the
likelihood of getting trapped in local minima, as revealed in Section V-A4.
Nevertheless, as depicted in Fig. 4(c), employing higher values of UT
parameters (specifically, $\alpha,k_{\sigma},\mathbf{\Sigma}_{0}$) may result
in a loss of precision and continuity in the distribution of trajectories
across the state-space, as the sigma points become more spread out from the
mean $\mathcal{X}_{k}^{(0)}$. This can impact the resulting control actions of
the system and, in the context of autonomous navigation, potentially lead to
collisions with obstacles. In contrast, Fig. 4(d) demonstrates that using
lower values of $\alpha$ and $k_{\sigma}$ results in a sampling strategy that
closely resembles MPPI. Similarly, if trajectories are sampled only from
$\mathcal{X}_{k}^{(0)}$ (as depicted by the blue trajectories in Fig. 1),
while excluding other sigma-point trajectories, the same sampling strategy can
be achieved. We refer to this approach as sampling mode 0 ($\texttt{SM}_{0}$),
while the default strategy that includes all sigma points is referred to as
$\texttt{SM}_{1}$.

###  IV-B Risk-Sensitive Cost

One of the main limitations of the vanilla MPPI is that it typically assumes a
risk-neutral approach when assessing the sampled trajectories during the
optimization process, without explicitly considering risk or uncertainty in
the trajectory evaluation process, as outlined in Section II-B, particularly
in (4). The commonly employed method in sampling-based MPC algorithms involves
using a quadratic cost function to guide the current state $\mathbf{x}_{k}$
towards its desired state $\mathbf{x}_{f}$, denoted as
$q_{\text{state}}(\mathbf{x}_{k})$, and expressed mathematically as follows

$\displaystyle
q_{\text{state}}(\mathbf{x}_{k})=\left(\mathbf{x}_{k}-\mathbf{x}_{f}\right)^{\top}\mathrm{Q}\left(\mathbf{x}_{k}-\mathbf{x}_{f}\right)=\left\|\mathbf{x}_{k}-\mathbf{x}_{f}\right\|_{\mathrm{Q}}^{2},$
(12)

where $\mathrm{Q}$ is a positive definite weighting matrix. Whittle introduced
in [27] an interesting method for incorporating risk in decision-making by
replacing the expected quadratic cost with a risk-sensitive benchmark in the
form of an exponential-quadratic function. The risk-sensitive (RS) cost
$q_{\mathrm{rs}}\left(\mathbf{x}_{k}\right)$ can be obtained by evaluating the
log-expectation of the exponentiated quadratic cost as follows

$\displaystyle q_{\mathrm{rs}}(\mathbf{x}_{k})$
$\displaystyle=-\frac{2}{\gamma}\log\mathbb{E}\left[\exp\left(-\frac{1}{2}\gamma
q_{\text{state}}(\mathbf{x}_{k})\right)\right]$ (13)
$\displaystyle=-\frac{2}{\gamma}\log\mathbb{E}\left[\exp\left(-\frac{1}{2}\gamma\left\|\mathbf{x}_{k}-\mathbf{x}_{f}\right\|_{\mathrm{Q}}^{2}\right)\right],$

where $\gamma$ is a real scalar denoted as the risk-sensitivity parameter,
dictating how the controller reacts to risk or uncertainty. For example, when
$\gamma>0$, the controller exhibits risk-seeking or risk-preferring behavior.
Conversely, when $\gamma<0$, the controller demonstrates risk-averse or risk-
avoiding behavior. When $\gamma=0$, the controller is considered risk-neutral.
We refer to [7] for more details and clear visualizations demonstrating the
relationship between the sign of $\gamma$ and the RS cost $q_{\mathrm{rs}}$.

Our proposed U-MPPI control strategy assumes that the system state
$\mathbf{x}_{k}\sim\mathcal{N}(\mathbf{\bar{\mathbf{x}}}_{k},\mathbf{\Sigma}_{k})$
follows Gaussian distribution. With this assumption, we can approximate the RS
state-dependent cost expressed in (13) as

$q_{\mathrm{rs}}(\mathbf{x}_{k})=\frac{1}{\gamma}\log\operatorname{det}\left(\mathbf{I}+\gamma
Q\mathbf{\Sigma}_{k}\right)+\left\|\mathbf{\bar{\mathbf{x}}}_{k}-\mathbf{x}_{f}\right\|_{Q_{\mathrm{rs}}}^{2},\vspace*{-3pt}$
(14)

where $Q_{\mathrm{rs}}$ represents the risk-sensitive penalty coefficients
matrix or adaptive weighting matrix, given by
$Q_{\mathrm{rs}}(\mathbf{\Sigma}_{k})={\left(Q^{-1}+\gamma\mathbf{\Sigma}_{k}\right)^{-1}}$.
The derivation of (14) is given in Appendix A. Such a new formulation is
employed by U-MPPI to assess each batch of sigma-point trajectories, where the
predicted mean state $\bar{\mathbf{x}}_{k}$ is replaced with the predicted
sigma points $\big{\\{}\mathcal{X}_{k}^{(i)}\big{\\}}^{2n_{x}}_{i=0}$.
Therefore, the modified RS cost for the $i^{\text{th}}$ U-MPPI sampled
trajectory within a certain batch is defined by

$q_{\mathrm{rs}}\\!\left(\\!\mathcal{X}_{k}^{(i)}\\!\\!,\mathbf{\Sigma}_{k}\\!\right)=\frac{1}{\gamma}\log\operatorname{det}\left(\mathbf{I}\\!\,+\\!\,\gamma
Q\mathbf{\Sigma}_{k}\right)+\left\|\mathcal{X}_{k}^{(i)}\\!\\!-\mathbf{x}_{f}\right\|_{Q_{\mathrm{rs}}}^{2}\\!\\!\\!\\!.\vspace*{-2pt}$
(15)

It is noteworthy to observe in (15) that $q_{\mathrm{rs}}(\cdot)$ incorporates
uncertainty information $\mathbf{\Sigma}_{k}$ as a feedback into the weighting
matrix $Q$, which measures the difference between the predicted sigma point
$\mathcal{X}_{k}^{(i)}$ and the desired state $\mathbf{x}_{f}$. By
incorporating this uncertainty feedback mechanism, the proposed control
strategy exhibits a risk-sensitive behavior that effectively minimizes the RS
cost over the time-horizon $N$. This, in turn, enables the development of a
more robust and risk-conscious control policy, as empirically validated
through intensive simulations outlined in Section V. To be more precise, if
$\gamma<0$, the penalty coefficients matrix $Q_{\mathrm{rs}}$ utilized for
tracking the desired state increases with the level of system uncertainty
$\mathbf{\Sigma}_{k}$. This can be expressed mathematically over a finite
time-horizon $N$ as
$Q_{\mathrm{rs}}(\mathbf{\Sigma}_{0})<Q_{\mathrm{rs}}(\mathbf{\Sigma}_{1})<\dots<Q_{\mathrm{rs}}(\mathbf{\Sigma}_{N-1})$
for all $\gamma<0$. On the other hand, if $\gamma>0$, the penalty matrix
decreases as the uncertainty level increases. Additionally, when $\gamma=0$,
the penalty remains constant and equal to the weighting matrix $Q$, regardless
of the level of the uncertainty [8]. More in-depth analysis of how the U-MPPI
performance is influenced by the sign of $\gamma$ is discussed in Section
V-A1.

By incorporating the system uncertainty $\mathbf{\Sigma}_{k}$ into (3), we can
obtain the modified cost-to-go for each trajectory $\tau^{(i)}_{m}$ in batch
$m$ as

$\displaystyle\tilde{S}\left(\tau^{(i)}_{m}\right)=$
$\displaystyle\,\phi\left(\mathcal{X}_{N}^{(i)}\right)+\sum_{k=0}^{N-1}\tilde{q}\left(\mathcal{X}_{k}^{(i)},\mathbf{\Sigma}_{k},\mathbf{u}_{k},\delta\mathbf{u}_{k,m}\right),$
(16) $\displaystyle\forall m\in\\{1,\cdots,M_{\sigma}\\},\quad\forall
i\in\\{0,\cdots,2n_{x}\\},$

where the instantaneous running cost $\tilde{q}$ is a combination of the
state-dependent running cost
$q\left(\mathcal{X}_{k}^{(i)},\mathbf{\Sigma}_{k}\right)$, which relies on the
RS cost defined in (15) (as shown in (18) as an example), as well as the
quadratic control cost $q(\mathbf{u}_{k},\delta\mathbf{u}_{k})$ from (4). Note
that the cost-to-go for all sigma-point trajectories in the $m^{\text{th}}$
batch can be expressed in vector form as
$\mathbf{\tilde{S}}\left(\footnotesize\scalerel*{\tau}{T}_{m}\right)=\left[\tilde{S}\left(\tau^{(0)}_{m}\right),\dots,\tilde{S}\left(\tau^{(2n_{x})}_{m}\right)\right]^{\top}\in\mathbb{R}^{n_{\sigma}}$.
Similarly,
$\mathbf{\tilde{q}}\left(\mathbf{X}_{k},\mathbf{\Sigma}_{k},\mathbf{u}_{k},\delta\mathbf{u}_{k,m}\right)=\left[\tilde{q}\left(\mathcal{X}_{k}^{(0)},\cdot\right),\dots,\tilde{q}\left(\mathcal{X}_{k}^{(2n_{x})},\cdot\right)\right]^{\top}$.

Algorithm 1 Real-Time U-MPPI Control Algorithm

Given:
$M,M_{\sigma},N$: $\\#$ of trajectories, batches, time-horizon,
$\mathbf{f},n_{x},\Delta t$: Dynamics, state dimension, time-step size,
$\phi,q,Q,\gamma,\lambda,\nu,\Sigma_{\mathbf{u}},R$: Cost/Control parameters,
$\lambda_{\sigma},k_{\sigma},n_{\sigma},\alpha,\beta,\mathbf{\Sigma}_{0}$: UT
parameters,
SGF: Savitzky-Galoy (SG) convolutional filter,
Input:
$\mathbf{U}=\left[\mathbf{u}_{0},\mathbf{u}_{1},\ldots,\mathbf{u}_{N-1}\right]^{\top}$:
Initial control sequence,
SM: Set U-MPPI sampling mode ($\texttt{SM}_{0}$ OR $\texttt{SM}_{1}$),

1:while task not completed do

2: $\bar{\mathbf{x}}_{0}\leftarrow$ StateEstimator(), $\triangleright$
$\bar{\mathbf{x}}_{0}\in\mathbb{R}^{n_{x}}$

3: $\delta\mathbf{u}\leftarrow$
RandomNoiseGenerator$(\mathbf{0},\Sigma_{\mathbf{u}}\\!)$, $\triangleright$
$\delta\mathbf{u}\\!\in\\!\mathbb{R}^{N\\!\,\times M_{\sigma}}$

4: for $m\leftarrow 0$ to $M_{\sigma}$ in parallel do

5:
$\left(\bar{\mathbf{x}},\mathbf{\Sigma}\right)\\!\leftarrow\\!\left(\bar{\mathbf{x}}_{0},\mathbf{\Sigma}_{0}\right)\\!,$
$\triangleright$ Actual state
$\mathbf{x}_{0}\\!\sim\\!\mathcal{N}\\!(\\!\bar{\mathbf{x}}_{0},\mathbf{\Sigma}_{0}\\!)$

6:
$\mathbf{\tilde{S}}\left(\footnotesize\scalerel*{\tau}{T}_{m}\right)\leftarrow[0,\dots,0]^{\top},$
$\triangleright$
$\mathbf{\tilde{S}}\left(\footnotesize\scalerel*{\tau}{T}_{m}\right)\in\mathbb{R}^{n_{\sigma}}$

7: for $k\leftarrow 0$ to $N-1$ do

8: $\mathbf{X}_{k}\leftarrow$
Moments2SigmaPoints$(\bar{\mathbf{x}}_{k},\mathbf{\Sigma}_{k})$,

9:
$\mathbf{X}_{k+1}\leftarrow\mathbf{X}_{k}+\mathbf{f}\left(\mathbf{X}_{k},\mathbf{u}_{k}+\delta\mathbf{u}_{k,m}\right)\Delta
t$,

10:
$\mathbf{\tilde{S}}\left(\footnotesize\scalerel*{\tau}{T}_{m}\right)\\!\leftarrow\\!\mathbf{\tilde{S}}\left(\footnotesize\scalerel*{\tau}{T}_{m}\right)+\mathbf{\tilde{q}}\left(\mathbf{X}_{k},\mathbf{\Sigma}_{k},\mathbf{u}_{k},\delta\mathbf{u}_{k,m}\right)$,

11:
$\left(\\!\bar{\mathbf{x}}_{k+1}\\!\,,\mathbf{\Sigma}_{k+1}\\!\right)\\!\leftarrow$
SigmaPoints2Moments$(\mathbf{X}_{k+1}\\!)$,

12: end for

13:
$\mathbf{\tilde{S}}\left(\footnotesize\scalerel*{\tau}{T}_{m}\right)\leftarrow\mathbf{\tilde{S}}\left(\footnotesize\scalerel*{\tau}{T}_{m}\right)+\mathbf{\Phi}\left(\mathbf{X}_{N}\right)$,

14: end for

15:
$\tilde{S}_{\min}\leftarrow\min_{m}[\mathbf{\tilde{S}}\left(\footnotesize\scalerel*{\tau}{T}_{m}\right)]$,
$\forall m=\\{1,\dots,M_{\sigma}\\}$

16: for $k\leftarrow 0$ to $N-1$ do

17:
$\mathbf{u}_{k}\\!\leftarrow\\!\textit{SGF}\\!\left(\\!\\!\mathbf{u}_{k}\\!+\\!\frac{\sum_{m=0}^{M_{\sigma}}\exp\bigl{(}\\!\frac{-1}{\lambda}\\!\left[\mathbf{\tilde{S}}\left(\footnotesize\scalerel*{\tau}{T}_{m}\right)-\tilde{S}_{\min}\right]\bigr{)}\delta\mathbf{u}_{k,m}}{\sum_{m=0}^{M_{\sigma}}\exp\bigl{(}\frac{-1}{\lambda}\left[\mathbf{\tilde{S}}\left(\footnotesize\scalerel*{\tau}{T}_{m}\right)-\tilde{S}_{\min}\right]\bigr{)}}\\!\\!\right)$,

18: end for

19: $\mathbf{u}_{0}\leftarrow$ SendToActuators($\mathbf{U}$),

20: for $k\leftarrow 1$ to $N-1$ do

21: $\mathbf{u}_{k-1}\leftarrow\mathbf{u}_{k}$,

22: end for

23: $\mathbf{u}_{N-1}\leftarrow$
ControlSequenceInitializer($\mathbf{u}_{N-1}$),

24: Check for task completion

25:end while

###  IV-C Real-Time U-MPPI Control Algorithm

We are now prepared to describe the real-time control-loop of our U-MPPI
algorithm, as depicted in Algorithm 1, employing the default sampling strategy
that considers all sigma points (referred to as $\texttt{SM}_{1}$). At every
time-step $\Delta t$, the algorithm estimates the current system state
$\bar{\mathbf{x}}_{0}$ with an external state estimator, generates $N\times
M_{\sigma}$ random control perturbations $\delta\mathbf{u}$ on the GPU using
CUDA’s random number generation library, and then produces $M_{\sigma}$ sets
of batches in parallel on the GPU (lines $2:4$). Subsequently, for each batch
and starting from the actual (i.e., initial) state
$\mathbf{x}_{0}\sim\mathcal{N}(\bar{\mathbf{x}}_{0},\mathbf{\Sigma}_{0})$, the
algorithm samples and propagates $n_{\sigma}$ sigma-point trajectories by
applying the non-linear dynamics in (10) to the sigma points computed via (6)
(lines $5:9$). These trajectories are then evaluated using (16), and the first
and second moments of the propagated state are estimated by applying (8)
(lines $10:15$). Then, the algorithm updates the optimal control sequence
$\left\\{\mathbf{u}_{k}\right\\}_{k=0}^{N-1}$, applies a Savitzky-Galoy filter
for smoothing, and applies the first control $\mathbf{u}_{0}$ to the system
(lines $16:19$). It then slides down the remaining sequence of length $N-1$ to
be utilized at the next time-step (lines $20:23$). It is noteworthy that when
executing the algorithm in sampling mode 0 ($\texttt{SM}_{0}$), where only the
mean $\mathcal{X}_{k}^{(0)}$ is used for trajectory sampling, it is essential
to set $M_{\sigma}$ to $M$ instead of $int(\frac{M}{n_{\sigma}})$.
Additionally, it is necessary to evaluate solely the cost-to-go of the nominal
trajectory $\tilde{S}\left(\tau^{(0)}_{m}\right)$.

##  V Simulation-Based Evaluation

In this section, we evaluate the effectiveness of our proposed control
strategy by comparing it to the standard MPPI control framework. Herein, we
focus on two goal-oriented autonomous ground vehicle (AGV) navigation tasks in
2D cluttered environments. The first task, presented in Section V-A, involves
maneuvering the AGV within a given map. This experiment allows us to
understand the algorithmic advantages of the proposed U-MPPI control scheme
without adding real-world complexity. Section V-B introduces a more complex
and realistic scenario where the map is unknown a priori. It examines the
adaptability and robustness of our proposed control strategy, providing a
thorough evaluation of its potential in real-world applications.

###  V-A Aggressive Navigation in Known Cluttered Environments

#### 1) Simulation Setup:

In this experiment, we use the kinematics model of a differential drive robot
presented in [16] for sampling trajectories in the conventional MPPI and
propagating sigma points in the proposed U-MPPI technique, as outlined in (9).
The model’s state includes its position and orientation in the world frame,
given by $\mathbf{x}=[{x},{y},\theta]^{\top}\in\mathbb{R}^{3}$. The control
input consists of the robot’s desired linear and angular velocities, denoted
by $\mathbf{u}=[v,\omega]^{\top}\in\mathbb{R}^{2}$.

To ensure a fair comparison, both MPPI and U-MPPI simulations were conducted
under consistent parameters. A time prediction of $8\text{\,}\mathrm{s}$ and a
control rate of $30\text{\,}\mathrm{Hz}$ were employed, resulting in $240$
control time-steps (i.e., $N=240$). At each time-step $\Delta t$, a total of
$2499$ rollouts were sampled, accompanied by an exploration noise of
$\nu=1200$. To account for control weighting, a control weighting matrix $R$
was utilized, which was formulated as
$\lambda\Sigma_{\mathbf{u}}^{-\frac{1}{2}}$. Additionally, the inverse
temperature parameter was set to $\lambda=0.572$, while the control noise
variance matrix
$\Sigma_{\mathbf{u}}=\operatorname{Diag}\left(\sigma_{v}^{2},\sigma_{w}^{2}\right)$
was defined as
$\Sigma_{\mathbf{u}}=\operatorname{Diag}\left(0.023,0.028\right)$. To smooth
the control sequence, we utilized the Savitzky-Galoy (SG) convolutional filter
with a quintic polynomial function, i.e., $n_{sg}=5$, and a window length
$l_{sg}$ of $61$. As described in Section IV-A, U-MPPI has additional
parameters in the unscented transform to regulate the spread of the sigma
points. These parameters are set to $\alpha=1,k_{\sigma}=0.5,\text{
and},\beta=2$. Additionally, the initial state covariance matrix
$\mathbf{\Sigma}_{0}$ is set to $0.001\mathbf{I}_{3}$. The baseline and U-MPPI
are implemented using Python and incorporated within the Robot Operating
System (ROS) framework. They are executed in real-time on an NVIDIA GeForce
GTX 1660 Ti laptop GPU.

In the context of the 2D navigation task, MPPI employs a commonly-used
instantaneous state-dependent cost function described in (17). This cost
function consists of two terms. The first term, denoted as
$q_{\text{state}}(\mathbf{x}_{k})$, encourages the robot to reach the desired
state; its formulation, which employs a quadratic expression, is provided in
(12). The second term,
$q_{\text{crash}}(\mathbf{x}_{k})=w_{\text{crash}}\mathbb{I}_{\text{crash}}$,
serves as an indicator function that imposes a high penalty when the robot
collides with obstacles, with $\mathbb{I}_{\text{crash}}$ being a Boolean
variable and $w_{\text{crash}}$ representing the collision weighting
coefficient. While implementing the proposed U-MPPI approach, we replace the
quadratic cost $q_{\text{state}}(\mathbf{x}_{k})$, as depicted in (18), with
our newly introduced RS cost denoted as
$q_{\mathrm{rs}}\left(\mathcal{X}_{k}^{(i)},\mathbf{\Sigma}_{k}\right)$ and
defined in (15). Within the scope of this work, we set the values of $Q$ and
$w_{\text{crash}}$ as follows: $Q=\operatorname{Diag}(2.5,2.5,2)$ and
$w_{\text{crash}}=10^{3}$. It is worth noting that in this particular task,
assigning positive values to $\gamma$ (i.e., $\gamma>0$) ensures a trade-off
between compelling the robot to reach its desired state and minimizing the
risk of collisions with obstacles. This trade-off arises from the fact that
the penalty coefficients matrix $Q_{\mathrm{rs}}$, utilized for tracking the
desired state, decreases as the system uncertainty $\mathbf{\Sigma}_{k}$
increases over the time-horizon $N$. Therefore, we have chosen $\gamma=1$ to
maintain this trade-off and strike a balance between task completion and
collision avoidance. On the other hand, we believe that assigning negative
values to $\gamma$ in applications such as autonomous racing [28] and visual
servoing [14] could enhance the performance of U-MPPI. In such tasks, it is
crucial to prioritize forcing the current state to reach its desired state,
which can be achieved by assigning a higher penalty coefficients matrix
$Q_{\mathrm{rs}}$.

$q(\mathbf{x}_{k})=q_{\text{state}}(\mathbf{x}_{k})+q_{\text{crash}}(\mathbf{x}_{k}).$
(17)
$q\left(\mathcal{X}_{k}^{(i)},\mathbf{\Sigma}_{k}\right)=q_{\mathrm{rs}}\\!\left(\\!\mathcal{X}_{k}^{(i)}\\!\\!,\mathbf{\Sigma}_{k}\\!\right)+q_{\text{crash}}(\mathbf{x}_{k}).$
(18)

#### 2) Simulation Scenario:

In order to assess the effectiveness of the proposed control framework within
cluttered environments, three distinct scenarios with different difficulty
levels were examined. In each scenario, we randomly generate one unique forest
type consisting of $25$ individual forests, resulting in a total of
$\mathcal{N}_{T}=25$ tasks. Each forest represents a cluttered environment
with dimensions of $$50\text{\,}\mathrm{m}$\times$50\text{\,}\mathrm{m}$$. In
the first scenario (referred to as Scenario #1), the average distance between
obstacles was $1.5\text{\,}\mathrm{m}$, indicated as
$d^{\text{obs}}_{\min}=$1.5\text{\,}\mathrm{m}$$; while in the second and
third scenarios (i.e., Scenario #2 and Scenario #3), they were placed at
average distances of $2\text{\,}\mathrm{m}$ and $3\text{\,}\mathrm{m}$,
respectively. Additionally, we set the maximum desired velocity $v_{\max}$ of
the robot based on the degree of clutter in each scenario. Specifically,
$v_{\max}$ is set to $2\text{\,}\mathrm{m}\mathrm{/}\mathrm{s}$,
$3\text{\,}\mathrm{m}\mathrm{/}\mathrm{s}$, and
$4\text{\,}\mathrm{m}\mathrm{/}\mathrm{s}$ in Scenario #1, #2, and #3,
respectively.

#### 3) Performance Metrics:

To achieve a fair comparison between the two control strategies, we use the
following criteria: (i) firstly, in all simulation instances, the robot is
required to reach the designated desired pose, denoted as
$\mathbf{x}_{f}=[50,50,0]^{\top}$, from a predetermined initial pose
$\mathbf{x}_{0}=[0,0,0]^{\top}$, measured in
$([$\mathrm{m}$],[$\mathrm{m}$],[$\deg$])$; (ii) secondly, a comprehensive set
of metrics are defined to evaluate the overall performance [16], including the
task completion percentage $\mathcal{T}_{\text{c}}$, the success rate
$\mathcal{S}_{R}$, the average number of collisions $\mathcal{N}_{\text{c}}$,
the average number of local minima occurrences $\mathcal{R}_{\text{lm}}$, the
average distance traversed by the robot $d_{\text{av}}$ to reach the desired
state $\mathbf{x}_{f}$ from its initial state $\mathbf{x}_{0}$, the average
linear velocity $v_{\text{av}}$ of the robot during the execution of its task
in the cluttered environment, and the average execution time per iteration
$t_{\text{exec.}}$ of the control algorithm. Successful task completion is
characterized by the robot reaching the desired pose without colliding with
obstacles within a predefined finite time, i.e.,
$\mathcal{T}_{\text{c}}=100\%,\mathcal{N}_{\text{c}}=0$, and
$\mathcal{R}_{\text{lm}}=0$. Furthermore, in all the given scenarios, if the
robot fails to reach the desired pose within a duration of
$70\text{\,}\mathrm{s}$ while successfully avoiding collisions with obstacles,
we classify the simulation episode as reaching a local minimum, indicated by
$\mathcal{R}_{\text{lm}}=1$.

TABLE I: Performance comparisons of the two control schemes, where the gray cells represent better results. Scheme |  $\mathcal{R}_{\text{lm}}$$(\mathcal{N}_{c})$ |  $\mathcal{S}_{R}$ [%] |  $\mathcal{T}_{c}$ [%] |  $d_{\text{av}}$ [$\mathrm{m}$] |  $v_{\text{av}}$ [$\mathrm{m}\mathrm{/}\mathrm{s}$] |  $t_{\text{exec.}}$ [$\mathrm{ms}$]
---|---|---|---|---|---|---
Scenario #1: $v_{\text{max}}=$2\text{\,}\mathrm{m}\mathrm{/}\mathrm{s}$$ &
$d^{\text{obs}}_{\min}=$1.5\text{\,}\mathrm{m}$,\gamma=1,w_{\text{crash}}=10^{3}$
MPPI | 9 (2) | 78 | 92.86 | 75.18 | $1.85\pm 0.21$ | 9.68
U-MPPI | 2 (0) | 96 | 98.78 | 75.34 | $\pagecolor{gray!20}1.85\pm 0.18$ | 9.12
Scenario #2: $v_{\text{max}}=$3\text{\,}\mathrm{m}\mathrm{/}\mathrm{s}$$ &
$d^{\text{obs}}_{\min}=$2\text{\,}\mathrm{m}$,\gamma=1,w_{\text{crash}}=10^{3}$
MPPI | 2 (1) | 94 | 98 | 75.31 | $2.49\pm 0.74$ | 10.03
U-MPPI | 0 (0) | 100 | 100 | 75.78 | $\pagecolor{gray!20}2.53\pm 0.49$ | 8.87
Scenario #3: $v_{\text{max}}=$4\text{\,}\mathrm{m}\mathrm{/}\mathrm{s}$$ &
$d^{\text{obs}}_{\min}=$3\text{\,}\mathrm{m}$,\gamma=1,w_{\text{crash}}=10^{3}$
MPPI | 0 (0) | 100 | 100 | 72.19 | $3.52\pm 0.74$ | 8.55
U-MPPI | 0 (0) | 100 | 100 | 71.98 | $\pagecolor{gray!20}3.54\pm 0.59$ | 7.7

#### 4) Simulation Results:

Table I presents the performance analysis of the proposed U-MPPI and the
baseline MPPI control strategies, considering the three predefined scenarios.
For each scenario, two trials were conducted over the 25 individual forests,
resulting in a total of 50 tasks ($\mathcal{N}_{T}=50$). In Scenario #1, where
$d^{\text{obs}}_{\min}=$1.5\text{\,}\mathrm{m}$$, it is noteworthy that U-MPPI
outperforms MPPI. Specifically, U-MPPI achieves a notably higher task
completion percentage ($\mathcal{T}_{c}=98.78\%$) compared to MPPI
($\mathcal{T}_{c}=92.86\%)$, effectively avoids collisions
($\mathcal{N}_{c}=0$), mitigates local minimum occurrences
($\mathcal{R}_{\text{lm}}=2$), and achieves a significantly higher success
rate ($\mathcal{S}_{R}=96\%$ vs. $\mathcal{S}_{R}=78\%$ when MPPI is
utilized). Furthermore, it successfully navigates the cluttered environment
with a slightly improved average linear velocity $v_{\text{av}}$, which
exhibits a very low standard deviation and approaches the maximum desired
speed $v_{\max}$ of $2\text{\,}\mathrm{m}\mathrm{/}\mathrm{s}$ (likewise
observed in the other two scenarios). Similarly, in Scenario #2, with a
minimum obstacle distance of $d^{\text{obs}}_{\min}=$2\text{\,}\mathrm{m}$$,
U-MPPI achieves a perfect task completion rate of $100\%$, outperforming
MPPI’s $98\%$, as it successfully avoids collisions and local minima,
surpassing the baseline MPPI that experienced one collision with obstacles
($\mathcal{N}_{c}=1$) and encountered two instances of local minima
($\mathcal{R}_{\text{lm}}=2$).

In the least cluttered scenario, Scenario #3, both control strategies
effectively complete all assigned tasks while successfully avoiding obstacles
in the cluttered environment. However, U-MPPI stands out by offering a
slightly more direct route towards the desired pose, with the robot traveling
an average distance $d_{\text{av}}$ of approximately
$71.98\text{\,}\mathrm{m}$, compared to $72.19\text{\,}\mathrm{m}$ when
utilizing MPPI. On the contrary, in Scenarios #1 and #2, MPPI demonstrates an
enhanced performance in terms of the average distance traveled $d_{\text{av}}$
by the robot when compared to our proposed U-MPPI. In the last column of Table
I, despite both control methods ensuring real-time performance (since
$t_{\text{exec.}}<$33.33\text{\,}\mathrm{ms}$$), it is worth emphasizing that
the average execution time $t_{\text{exec.}}$ of our proposed U-MPPI control
strategy is slightly shorter than that of MPPI. This can be attributed to the
parallel implementation of the U-MPPI algorithm on GPU, where each thread is
responsible for computing the dynamics and costs of the entire batch when
sampling from all sigma points ($\texttt{SM}_{1}$). On the other hand, the
parallel implementation of MPPI, as well as U-MPPI with sampling mode 0
($\texttt{SM}_{0}$), employs a single thread to compute each sampled
trajectory, resulting in a relatively longer execution time, as evidenced by
the intensive simulations in Table I, as well as Tasks #3 and #4 in Table II,
where only the mean $\mathcal{X}_{k}^{(0)}$ is used for trajectory sampling
(i.e., $\texttt{SM}_{0}$).

To summarize, the intensive simulations clearly demonstrate that our U-MPPI
method consistently outperforms the baseline MPPI control framework in all
tested scenarios, particularly in environments with higher levels of clutter.
These remarkable results can be credited to two key factors: the effective
utilization of an unscented-based sampling strategy, which provides more
flexible and efficient trajectories, and the incorporation of a risk-sensitive
(RS) cost function that explicitly takes into account risk and uncertainty
during the trajectory evaluation process; thanks to the incorporation of these
crucial components, our approach ensures a significantly enhanced exploration
of the state-space of the controlled system, even while leveraging the same
injected Gaussian noise $\delta\mathbf{u}_{k}$ into the mean control sequence,
effectively reducing the likelihood of being trapped in local minima and
yielding a safer and more resilient control system that is suitable for
aggressive navigation in highly complex cluttered environments.

TABLE II: Influence of collision weighting coefficient $w_{\text{crash}}$, sampling mode (i.e., $\texttt{SM}_{0}$ & $\texttt{SM}_{1}$), and UT parameters (namely, $\mathbf{\Sigma}_{0},k_{\sigma},\alpha$) on U-MPPI performance. Test No. |  $\mathcal{R}_{\text{lm}}$$(\mathcal{N}_{c})$ |  $\mathcal{S}_{R}$ [%] |  $\mathcal{T}_{c}$ [%] |  $d_{\text{av}}$ [$\mathrm{m}$] |  $v_{\text{av}}$ [$\mathrm{m}\mathrm{/}\mathrm{s}$] |  $t_{\text{exec.}}$ [$\mathrm{ms}$]
---|---|---|---|---|---|---
Scenarios #1 and #2: $w_{\text{crash}}=500$ instead of
$w_{\text{crash}}=10^{3}$
Test #1 | 2 (0) | 96 | 99.4 | 74.43 | $1.86\pm 0.15$ | 8.69
Test #2 | 0 (0) | 100 | 100 | 74.55 | $2.56\pm 0.56$ | 8.9
Scenarios #1 and #2: $\texttt{SM}_{0}$ instead of $\texttt{SM}_{1}$
Test #3 | 3 (0) | 94 | 98.24 | 75.88 | $1.84\pm 0.23$ | 12.87
Test #4 | 1 (0) | 98 | 99.8 | 76.87 | $2.47\pm 0.68$ | 12.59
Scenario #1: Impact of UT parameters ($\mathbf{\Sigma}_{0},k_{\sigma},\alpha$)
Test #5 | 0 (0) | 100 | 100 | 77.19 | $1.79\pm 0.24$ | 8.80
Test #6 | 1 (0) | 98 | 98.7 | 74.65 | $1.80\pm 0.28$ | 8.88
Test #7 | 4 (0) | 92 | 96.88 | 75.75 | $1.81\pm 0.28$ | 9.46
Test #8 | 9 (0) | 82 | 92.91 | 75.25 | $1.83\pm 0.24$ | 9.02

To achieve a comprehensive understanding of how the behavior of the U-MPPI
control strategy is affected by integrating the proposed sampling strategy and
RS cost function, we expanded our intensive simulations in Table II to include
varying operating conditions and hyper-parameters, differing from those
utilized in Section V-A1. More precisely, in the first four intensive
simulations (namely, Test #1 to Test #4), we investigate the potential
benefits of integrating the RS cost function into the U-MPPI control strategy
through two approaches: (i) reducing the collision weighting coefficient
$w_{\text{crash}}$ (specifically, Tests #1 and #2), and (ii) adopting sampling
mode 0 ($\texttt{SM}_{0}$) as an alternative to the default mode
$\texttt{SM}_{1}$ (i.e., Tests #3 and #4). Additionally, in the subsequent
four tests, we extensively analyze the influence of the UT parameters on the
performance of U-MPPI. For Tests #1 and #2, we replicated the U-MPPI
simulations presented in Table I, specifically for Scenarios #1 and #2, by
assuming a reduced collision weighting coefficient $w_{\text{crash}}$ of
$500$, representing half of its nominal value. We can clearly observe that
lowering the value of $w_{\text{crash}}$ has no significant impact on the
success rate $\mathcal{S}_{R}$ (as also depicted in Fig. 5(a)) and task
completion rate $\mathcal{T}_{c}$. Nevertheless, it demonstrates improved
performance in the robot’s average travel distance $d_{\text{av}}$ for
completing the assigned tasks in both scenarios, outperforming both U-MPPI and
MPPI as indicated in Table I. As an example, in Scenario #2 shown in Fig.
5(b), we can observe that $d_{\text{av}}$ is approximately
$1.23\text{\,}\mathrm{m}$ shorter than that of U-MPPI when $w_{\text{crash}}$
is set to $10^{3}$. On the contrary, we empirically observed that reducing
$w_{\text{crash}}$ in the case of MPPI, which utilizes a risk-neutral
technique for evaluating sampled trajectories (as expressed in (17)), does not
lead to a performance improvement, as depicted in Fig. 55(c). For instance, in
Scenario #1, it can be noted from Fig. 55(c) that the success rate
$\mathcal{S}_{R}$ experiences a decline from 78% to 72% with the reduction of
$w_{\text{crash}}$.

(a)

(b)

(c)

(d)

Figure 5: Impact of decreasing collision weighting coefficient
$w_{\text{crash}}$ on (a) U-MPPI success rate $\mathcal{S}_{R}$, (b) average
distance traveled by the robot $d_{\text{av}}$ in U-MPPI, and (c) MPPI success
rate $\mathcal{S}_{R}$, as well as (d) the effect of utilizing sampling mode 0
($\texttt{SM}_{0}$) on U-MPPI success rate $\mathcal{S}_{R}$ compared to MPPI
success rate.

By employing sampling mode 0 ($\texttt{SM}_{0}$) in Tests #3 and #4 as an
alternative to the default sampling $\texttt{SM}_{1}$ in U-MPPI, a slight
decrease in performance is observed. However, U-MPPI continues to demonstrate
impressive capabilities in successfully accomplishing assigned tasks and
navigating around obstacles, outperforming the classical MPPI, particularly in
Scenario #1 (refer to Fig. 5(d)), owing to the integration of our efficient RS
cost function for trajectory assessment. Furthermore, the comprehensive
simulations performed in Tests #3 and #4 highlight the importance of employing
the default sampling strategy $\texttt{SM}_{1}$, which takes into account all
sigma points, in extremely challenging scenarios. This strategy leads to
enhancements in both the success rate and the trajectory quality of the robot,
as depicted in the illustrative example presented in Fig. 6(a). Now, it is
time to delve into investigating how the key UT parameters (namely,
$\mathbf{\Sigma}_{0},k_{\sigma},\alpha$) affect the distribution of sampled
rollouts and their subsequent influence on the performance of the U-MPPI
algorithm; to achieve this, we will explore their effects in highly cluttered
environments, with a specific focus on Scenario #1. In Test #5, the initial
state covariance matrix $\mathbf{\Sigma}_{0}$ is increased from
$0.001\mathbf{I}_{3}$ to $0.005\mathbf{I}_{3}$. It is observed that this
increase in $\mathbf{\Sigma}_{0}$ results in a more conservative yet safer
trajectory, with a success rate $\mathcal{S}_{R}$ of 100% and an average
traveled distance $d_{\text{av}}$ of $77.19\text{\,}\mathrm{m}$, compared to a
success rate of 96% and an average distance of $75.34\text{\,}\mathrm{m}$ when
$\mathbf{\Sigma}_{0}$ is set to $0.001\mathbf{I}_{3}$, as shown in Table I.
During Tests #6 and #7, we adjust the UT scaling parameters ($k_{\sigma}$ and
$\alpha$), which control the spread of sigma points; specifically, we increase
$k_{\sigma}$ from $0.5$ to $3$ in Test #6 and decrease $\alpha$ from $1$ to
$0.1$ in Test #7, while keeping all other parameters constant. It is
noteworthy to observe in Test #6 that assigning a higher value to
$k_{\sigma}$, along with $\alpha=1$, generates more widely spread trajectories
that cover a larger state space. This enables the robot to explore the
environment more extensively and find better solutions, while improving the
quality of the robot trajectory (as
$d_{\text{av}}=$74.65\text{\,}\mathrm{m}$$), thereby reducing the likelihood
of getting trapped in local minima. Conversely, reducing the spread of sigma
points by assigning very low values to $\alpha$ increases the likelihood of
the robot becoming trapped in local minima, as illustrated in the simulations
conducted during Test #7. In Test #8, we replicated the simulations from Test
#7 using the same reduced UT scaling parameters ($k_{\sigma}=0.5$ and
$\alpha=0.1$), while excluding the risk-sensitive behavior by setting the
risk-sensitive parameter $\gamma$ to 0, resulting in a constant penalty
coefficients matrix $Q_{\mathrm{rs}}$ that is equal to the weighting matrix
$Q$. Such a setup yields a sampling strategy closely resembling that of MPPI,
resulting in the worst performance achieved, albeit only marginally better
than the performance of MPPI illustrated in Table I.

(a)

(b)

(c)

(d)

Figure 6: Performance analysis of MPPI and U-MPPI in a
$$50\text{\,}\mathrm{m}$\times$50\text{\,}\mathrm{m}$$ cluttered environment
with $2\text{\,}\mathrm{m}$ obstacle spacing (Scenario #2), utilizing U-MPPI-
specific sampling modes ($\texttt{SM}_{0}$ and $\texttt{SM}_{1}$).

In Fig. 6, we showcase the behavior of MPPI and U-MPPI (utilizing U-MPPI-
specific sampling modes) in one of the randomly generated
$$50\text{\,}\mathrm{m}$\times$50\text{\,}\mathrm{m}$$ cluttered environments
with $2\text{\,}\mathrm{m}$ obstacles apart, i.e., Scenario #2. As shown in
Fig. 6(a), both control strategies successfully achieve collision-free
navigation in the cluttered environment. However, U-MPPI in the default
sampling mode 1 ($\texttt{SM}_{1}$) demonstrates a significantly shorter route
to the desired pose, with a robot trajectory length $d_{\text{av}}$ of
$75.45\text{\,}\mathrm{m}$, compared to $77.39\text{\,}\mathrm{m}$ for the
classical MPPI and $78.33\text{\,}\mathrm{m}$ for U-MPPI in sampling mode 0
($\texttt{SM}_{0}$). In the given cluttered environment, as depicted in Fig.
6(b), MPPI demonstrates a smoother velocity profile compared to U-MPPI (see
Figs. 6(c) and 6(d)), with an average traveling speed $v_{\text{av}}$ of
$2.63\text{\,}\mathrm{m}\mathrm{/}\mathrm{s}$, as opposed to
$v_{\text{av}}=$2.52\text{\,}\mathrm{m}\mathrm{/}\mathrm{s}$$ for U-MPPI
($\texttt{SM}_{1}$) and
$v_{\text{av}}=$2.57\text{\,}\mathrm{m}\mathrm{/}\mathrm{s}$$ for U-MPPI
($\texttt{SM}_{0}$). Furthermore, it is worth noting that none of the control
strategies violate the control (namely, velocity) constraint, which is defined
as $v\leq v_{\text{max}}=$3\text{\,}\mathrm{m}\mathrm{/}\mathrm{s}$$, as
observed from the velocity profiles.

###  V-B Aggressive Navigation in Unknown Environments

In Section V-A, although extensive simulations of our proposed control
strategies are performed using the fully autonomous ClearPath Jackal robot to
ensure their effectiveness and validate their performance in realistic
scenarios, the autonomous navigation tasks in cluttered environments are
typically carried out under the assumption that the costmap representing the
environment is known beforehand. Such a setup or configuration could be
limited in many real-world scenarios where autonomous robot systems often need
to operate in partially observed environments due to the constraints of
limited sensor range and the impracticality of obtaining a complete map before
task deployment [10]. To tackle this challenge, the robot needs to construct a
real-time 2D local costmap, also known as a 2D occupancy grid map, centered at
its current state; this grid map is utilized to store information about
obstacles in the robot’s surrounding area, gathered from the incoming sensory
data acquired by its on-board sensor. To this end, we utilize the costmap_2d
ROS package to generate the occupancy grid based on the sensory data, as
depicted in Fig. 7(b) [29]. Afterward, the local costmap is incorporated into
the optimization problem of the sampling-based MPC algorithm to assess sampled
trajectories, aiming to achieve collision-free navigation. We refer to our
previous work [16] and the corresponding open-source
code.222https://github.com/IhabMohamed/log-MPPI_ros In this work, we
successfully integrated the 2D grid map into the MPPI algorithm.

#### 1) Simulation Setup:

We employed the same simulation setup for both control strategies previously
presented in Section V-A1, with the exception that: (i) the collision
indicator function $q_{\text{crash}}(\mathbf{x})$ is herein calculated based
on the robot-centered 2D grid map, and (ii) the time prediction is reduced to
$6\text{\,}\mathrm{s}$, resulting in $N=180$, to be compatible with the size
of the grid map. In this study, a 16-beam Velodyne LiDAR sensor mounted on the
Clearpath Jackal AGV is used to construct the grid map (local costmap), with
the costmap having dimensions of
$$240\text{\,}\mathrm{cell}$\times$240\text{\,}\mathrm{cell}$$ and a
resolution of $0.05\text{\,}\mathrm{m}\mathrm{/}\mathrm{cell}$.

(a) (b)

Figure 7: Snapshot capturing (a) our Jackal robot operating in a forest-like
environment, equipped with a Velodyne VLP-16 LiDAR sensor, and (b) the
corresponding 2D local costmap generated by the on-board Velodyne LiDAR
sensor, where purple and cyan cells depict obstacles and their inflation.

#### 2) Simulation Scenarios:

For the performance evaluation, we utilize two types of forest-like cluttered
environments within the Gazebo simulator, each measuring $50\mathrm{m}\times
50\mathrm{m}$. The first type, referred to as Forest #1, consists of trees of
different sizes with a density of
$0.1\text{\,}\mathrm{trees}\mathrm{/}\mathrm{{}^{2}}\mathrm{m}$, whereas the
second type, known as Forest #2, contains tree-shaped obstacles with a density
of $0.2\text{\,}\mathrm{trees}\mathrm{/}\mathrm{{}^{2}}\mathrm{m}$. For Forest
#1, the desired velocity $v_{\text{max}}$ is set to
$3\text{\,}\mathrm{m}\mathrm{/}\mathrm{s}$, while for Forest #2, it is set to
$2\text{\,}\mathrm{m}\mathrm{/}\mathrm{s}$.

#### 3) Performance Metrics:

To assess the efficiency of the U-MPPI control scheme in unknown environments,
we compare it with the baseline MPPI by evaluating the predefined set of
performance metrics outlined in Section V-A3, namely:
$\mathcal{R}_{\text{lm}}$, $\mathcal{N}_{c}$, $\mathcal{S}_{R}$,
$\mathcal{T}_{c}$, $d_{\text{av}}$, $v_{\text{av}}$, and $t_{\text{exec.}}$.
In the first forest-like environment (Forest #1), the robot is directed to
autonomously navigate from an initial pose $G_{0}=[0,0,0]^{\top}$ to a
sequence of desired poses expressed in ([$\mathrm{m}$], [$\mathrm{m}$],
[$\deg$]):
$G_{1}=[20,20,45]^{\top},G_{2}=[-18,2,0]^{\top},G_{3}=[20,-21,90]^{\top}$,
$G_{4}=[20,20,0]^{\top}$, and ultimately reaching a stop at
$G_{5}=[0,0,100]^{\top}$. Meanwhile, in Forest #2, for the sake of simplicity,
the robot navigates solely from $G_{0}$ to $G_{3}$, where it comes to a stop.

TABLE III: Performance statistics of the two control strategies over ten trials in Forest #1 ($0.1\text{\,}\mathrm{trees}\mathrm{/}\mathrm{{}^{2}}\mathrm{m}$) and Forest #2 ($0.2\text{\,}\mathrm{trees}\mathrm{/}\mathrm{{}^{2}}\mathrm{m}$). | Forest #1 | Forest #2
---|---|---
Indicator | MPPI | U-MPPI | MPPI | U-MPPI
$\mathcal{R}_{\text{lm}}$$(\mathcal{N}_{c})$ | 2 (0) | 0 (0) | 4 (1) | 1 (0)
$\mathcal{S}_{R}$ [%] | 80 | 100 | 50 | 90
$\mathcal{T}_{c}$ [%] | 91.01 | 100 | 88.08 | 97.91
$d_{\text{av}}$[$\mathrm{m}$] | $\pagecolor{gray!20}185.33\pm 0.43$ | $185.37\pm 0.22$ | $118.25\pm 1.7$ | $\pagecolor{gray!20}116.76\pm 0.43$
$v_{\text{av}}$ [$\mathrm{m}\mathrm{/}\mathrm{s}$] | $\pagecolor{gray!20}2.45\pm 0.17$ | $2.44\pm 0.02$ | $1.51\pm 0.16$ | $\pagecolor{gray!20}1.64\pm 0.16$
$t_{\text{exec.}}$ [$\mathrm{ms}$] | $8.52\pm 0.62$ | $9.87\pm 0.76$ | $8.08\pm 0.91$ | $10.75\pm 0.75$

#### 4) Simulation Results:

In Table III, we present a comparison of performance statistics for the
proposed control strategies in achieving goal-oriented autonomous navigation
in both Forest #1 and Forest #2, where the statistics are averaged over $10$
trials for each environment. The obtained results validate the anticipated
superiority of U-MPPI over MPPI in all scenarios, owing to its efficient
unscented-based sampling distribution policy and risk-sensitive based
trajectory evaluation technique. This superiority is evident in several
aspects, including: (i) achieving a higher task completion rate
$\mathcal{T}_{c}$, such as $\mathcal{T}_{c}=100\%$ with U-MPPI versus 91.01%
when using MPPI in Forest #1, (ii) reducing the probability of encountering
local minima, as demonstrated by the comparison in Forest #2, where U-MPPI
leads to $\mathcal{R}_{\text{lm}}=1$ compared to $\mathcal{R}_{\text{lm}}=4$
with MPPI, and (iii) improving the quality of the generated robot trajectory,
as evidenced by a significantly shorter average distance traveled by the robot
$d_{\text{av}}$, particularly noticeable in Forest #2. Furthermore, it is
worth emphasizing that both control methods guarantee real-time performance,
highlighting the superiority of the sampling-based MPC algorithm, particularly
our proposed U-MPPI, in incorporating not only the local costmap but also the
unscented transform into the optimization problem without introducing
additional complexity.

##  VI Conclusion and Future Work

In this paper, we proposed the U-MPPI control strategy, a novel methodology
that enhances the vanilla MPPI algorithm by leveraging the unscented transform
for two primary objectives. Firstly, it regulates the propagation of the
dynamical system, resulting in a more effective sampling distribution policy
that effectively propagates both the mean $\bar{\mathbf{x}}_{k}$ and
covariance $\mathbf{\Sigma}_{k}$ of the state vector $\mathbf{x}_{k}$ at each
time-step $k$. Secondly, it incorporates a risk-sensitive cost function that
explicitly accounts for risk or uncertainty throughout the trajectory
evaluation process. Through extensive simulations and real-world
demonstrations, we demonstrated the effectiveness of U-MPPI in achieving
aggressive collision-free navigation in both known and unknown cluttered
environments. By comparing it to MPPI, our approach accomplished a substantial
improvement in state-space exploration while utilizing the same injected
Gaussian noise $\delta\mathbf{u}_{k}$ in the mean control sequence. As a
result, it yielded higher success and task completion rates, effectively
minimizing the likelihood of getting trapped in local minima, and enabling the
robot to identify feasible trajectories that avoid collisions. Our future plan
involves incorporating chance constraints [4] into the U-MPPI control
architecture to effectively address uncertainties in system dynamics and the
environment, including moving obstacles, resulting in an enhanced safety and
robustness of the control system to handle uncertain conditions, especially in
safety-critical applications.

## References

  * [1] H. Kurniawati, Y. Du, D. Hsu, and W. S. Lee, “Motion planning under uncertainty for robotic tasks with long time horizons,” _The International Journal of Robotics Research_ , vol. 30, no. 3, pp. 308–323, 2011\.
  * [2] A. Bemporad and M. Morari, “Robust model predictive control: A survey,” in _Robustness in identification and control_. Springer, 2007, pp. 207–226.
  * [3] A. Mesbah, “Stochastic model predictive control: An overview and perspectives for future research,” _IEEE Control Systems Magazine_ , vol. 36, no. 6, pp. 30–44, 2016.
  * [4] N. E. Du Toit and J. W. Burdick, “Probabilistic collision checking with chance constraints,” _IEEE Transactions on Robotics_ , vol. 27, no. 4, pp. 809–815, 2011.
  * [5] L. Hewing and M. N. Zeilinger, “Stochastic model predictive control for linear systems using probabilistic reachable sets,” in _2018 IEEE Conference on Decision and Control (CDC)_. IEEE, 2018, pp. 5182–5188.
  * [6] S. A. Sajadi-Alamdari, H. Voos, and M. Darouach, “Risk-averse stochastic nonlinear model predictive control for real-time safety-critical systems,” _IFAC-PapersOnLine_ , vol. 50, no. 1, pp. 5991–5997, 2017.
  * [7] X. Yang and J. Maciejowski, “Risk-sensitive model predictive control with gaussian process models,” _IFAC-PapersOnLine_ , vol. 48, no. 28, pp. 374–379, 2015.
  * [8] E. Hyeon, Y. Kim, and A. G. Stefanopoulou, “Fast risk-sensitive model predictive control for systems with time-series forecasting uncertainties,” in _2020 59th IEEE Conference on Decision and Control (CDC)_. IEEE, 2020, pp. 2515–2520.
  * [9] M. Schuurmans, A. Katriniok, H. E. Tseng, and P. Patrinos, “Learning-based risk-averse model predictive control for adaptive cruise control with stochastic driver models,” _IFAC-PapersOnLine_ , vol. 53, no. 2, pp. 15 128–15 133, 2020.
  * [10] I. S. Mohamed, G. Allibert, and P. Martinet, “Model predictive path integral control framework for partially observable navigation: A quadrotor case study,” in _16th Int. Conf. on Control, Automation, Robotics and Vision (ICARCV)_ , Shenzhen, China, Dec. 2020, pp. 196–203.
  * [11] G. Williams, A. Aldrich, and E. A. Theodorou, “Model predictive path integral control: From theory to parallel computation,” _Journal of Guidance, Control, and Dynamics_ , vol. 40, no. 2, pp. 344–357, 2017.
  * [12] I. S. Mohamed, G. Allibert, and P. Martinet, “Sampling-based MPC for constrained vision based control,” in _IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS)_ , 2021, pp. 3753–3758.
  * [13] J. Pravitra, K. A. Ackerman, C. Cao, N. Hovakimyan, and E. A. Theodorou, “$\mathcal{L}_{1}$-adaptive MPPI architecture for robust and agile control of multirotors,” in _IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS)_ , 2020, pp. 7661–7666.
  * [14] I. S. Mohamed, “MPPI-VS: Sampling-based model predictive control strategy for constrained image-based and position-based visual servoing,” _arXiv preprint arXiv:2104.04925_ , 2021.
  * [15] J. Yin, Z. Zhang, E. Theodorou, and P. Tsiotras, “Trajectory distribution control for model predictive path integral control using covariance steering,” in _IEEE Int. Conf. on Robotics and Automation (ICRA)_ , 2022, pp. 1478–1484.
  * [16] I. S. Mohamed, K. Yin, and L. Liu, “Autonomous navigation of AGVs in unknown cluttered environments: log-MPPI control strategy,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 4, pp. 10 240–10 247, 2022.
  * [17] 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_ , Pittsburgh, Pennsylvania, USA, 2018, pp. 42––51.
  * [18] J. Yin, Z. Zhang, and P. Tsiotras, “Risk-aware model predictive path integral control using conditional value-at-risk,” _arXiv preprint arXiv:2209.12842_ , 2022.
  * [19] I. M. Ross, R. J. Proulx, and M. Karpenko, “Unscented guidance,” in _American Control Conference (ACC)_. IEEE, 2015, pp. 5605–5610.
  * [20] A. Savitzky and M. J. Golay, “Smoothing and differentiation of data by simplified least squares procedures.” _Analytical chemistry_ , vol. 36, no. 8, pp. 1627–1639, 1964.
  * [21] S. J. Julier, J. K. Uhlmann, and H. F. Durrant-Whyte, “A new approach for filtering nonlinear systems,” in _Proceedings of 1995 American Control Conference-ACC’95_ , vol. 3. IEEE, 1995, pp. 1628–1632.
  * [22] J. Xu, K. Yin, and L. Liu, “Online planning in uncertain and dynamic environment in the presence of multiple mobile vehicles,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2020, pp. 2410–2416.
  * [23] R. Van Der Merwe, “Sigma-point kalman filters for probabilistic inference in dynamic state-space models,” Ph.D. dissertation, Oregon Health & Science University, 2004.
  * [24] S. J. Julier, “The scaled unscented transformation,” in _American Control Conference_ , vol. 6. IEEE, 2002, pp. 4555–4559.
  * [25] I. M. Ross, R. J. Proulx, and M. Karpenko, “Unscented optimal control for space flight,” in _International Symposium on Space Flight Dynamics (ISSFD)_ , 2014, pp. 1–12.
  * [26] N. Ozaki, S. Campagnola, and R. Funase, “Tube stochastic optimal control for nonlinear constrained trajectory optimization problems,” _Journal of Guidance, Control, and Dynamics_ , 2020.
  * [27] P. Whittle, “Risk-sensitive linear/quadratic/gaussian control,” _Advances in Applied Probability_ , vol. 13, no. 4, pp. 764–777, 1981.
  * [28] G. Williams, P. Drews, B. Goldfain, J. M. Rehg, and E. A. Theodorou, “Information-theoretic model predictive control: Theory and applications to autonomous driving,” _IEEE Transactions on Robotics_ , vol. 34, no. 6, pp. 1603–1622, 2018.
  * [29] E. Marder-Eppstein, D. V. Lu!!, and D. Hershberger. Costmap_2d package. [Online]. Available: http://wiki.ros.org/costmap_2d
  * [30] M. A. Woodbury, _Inverting modified matrices_. Department of Statistics, Princeton University, 1950.

##  Appendix A Derivative of Risk-Sensitive Cost

Considering the risk-sensitive cost defined as:

$q_{\text{rs}}(\mathbf{x}_{k})=-\frac{2}{\gamma}\log\mathbb{E}\left[\exp\left(-\frac{1}{2}\gamma\left\|\mathbf{x}_{k}-\mathbf{x}_{f}\right\|_{Q}^{2}\right)\right],$
(19)

our objective is to obtain the following formulation:

$q_{\text{rs}}(\mathbf{x}_{k})=\frac{1}{\gamma}\log\operatorname{det}\left(\mathbf{I}+\gamma
Q\mathbf{\Sigma}_{k}\right)+\left\|\mathbf{\bar{\mathbf{x}}}_{k}-\mathbf{x}_{f}\right\|_{Q_{\mathrm{rs}}}^{2},$
(20)

where $Q_{\mathrm{rs}}={\left(Q^{-1}+\gamma\mathbf{\Sigma}_{k}\right)^{-1}}$,
$Q$ is a diagonal matrix, and $\mathbf{\Sigma}_{k}$ is a symmetric matrix. We
assume that the system state $\mathbf{x}_{k}$ follows a Gaussian distribution
$\mathcal{N}(\bar{\mathbf{x}}_{k},\mathbf{\Sigma}_{k})$, and $\mathbf{x}_{f}$
represents the reference (desired) state. To derive the expression for
$q_{\text{rs}}(\mathbf{x}_{k})$ as presented in (20), we will proceed with a
step-by-step derivation as follows. We will start by expanding the quadratic
form in (19):

$\displaystyle q_{\text{rs}}(\mathbf{x}_{k})$
$\displaystyle=-\frac{2}{\gamma}\log\mathbb{E}\left[\exp\left\\{-\frac{\gamma}{2}\left\|\mathbf{x}_{k}-\mathbf{x}_{f}\right\|_{Q}^{2}\right\\}\right]$
(21)
$\displaystyle=-\frac{2}{\gamma}\log\mathbb{E}\left[\exp\left\\{-\frac{\gamma}{2}\left(\mathbf{x}_{k}-\mathbf{x}_{f}\right)^{\top}Q\left(\mathbf{x}_{k}-\mathbf{x}_{f}\right)\right\\}\right].$

Since
$\mathbf{x}_{k}\sim\mathcal{N}(\bar{\mathbf{x}}_{k},\mathbf{\Sigma}_{k})$, we
can assume that $\mathbf{x}_{k}=\bar{\mathbf{x}}_{k}+\mathbf{e}_{k}$, where
$\mathbf{e}_{k}\sim\mathcal{N}(0,\mathbf{\Sigma}_{k})$. Therefore, by
substituting $\mathbf{x}_{k}$ in (21), we can express the expectation as
follows:

$\displaystyle q_{\text{rs}}$
$\displaystyle=-\frac{2}{\gamma}\log\mathbb{E}\left[\exp\left\\{-\frac{\gamma}{2}\overline{\mathbf{X}}_{k}^{\top}Q\overline{\mathbf{X}}_{k}-\gamma\mathbf{e}_{k}^{\top}Q\overline{\mathbf{X}}_{k}-\frac{\gamma}{2}\mathbf{e}_{k}^{\top}Q\mathbf{e}_{k}\right\\}\right]$
(22)
$\displaystyle=-\frac{2}{\gamma}\log\mathbb{E}\left[\exp\left\\{-\frac{\gamma}{2}\overline{\mathbf{X}}_{k}^{\top}Q\overline{\mathbf{X}}_{k}\right\\}\right.$
$\displaystyle\qquad\times\left.\exp\left\\{-\gamma\mathbf{e}_{k}^{\top}Q\overline{\mathbf{X}}_{k}-\frac{\gamma}{2}\mathbf{e}_{k}^{\top}Q\mathbf{e}_{k}\right\\}\right]$
$\displaystyle=\left\|\overline{\mathbf{X}}_{k}\right\|_{Q}^{2}-\frac{2}{\gamma}\log\mathbb{E}\left[\exp\left\\{-\frac{\gamma}{2}\left\|\mathbf{e}_{k}\right\|_{Q}^{2}-\gamma\mathbf{e}_{k}^{\top}Q\overline{\mathbf{X}}_{k}\right\\}\right],$

where $\overline{\mathbf{X}}_{k}=\bar{\mathbf{x}}_{k}-\textbf{x}_{f}$. Now, to
compute the expectation $\mathbb{E}[\cdot]$ in (22), given by

$-\frac{2}{\gamma}\log\mathbb{E}\left[\exp\left\\{-\frac{\gamma}{2}\mathbf{e}_{k}^{\top}Q\mathbf{e}_{k}-\gamma\mathbf{e}_{k}^{\top}Q\overline{\mathbf{X}}_{k}\right\\}\right],$
(23)

we introduce a new term $\mathbf{y}_{k}$ to simplify and express the
expectation in the quadratic form. To achieve this, let us define
$\mathbf{y}_{k}=-\gamma\mathbf{K}^{-1}Q\overline{\mathbf{X}}_{k}$, where
$\mathbf{K}=\gamma Q+\Sigma_{k}^{-1}$. By using this definition, we can
accordingly prove that

$\displaystyle\frac{1}{2}\left(\mathbf{e}_{k}-\mathbf{y}_{k}\right)^{\top}\mathbf{K}\left(\mathbf{e}_{k}-\mathbf{y}_{k}\right)-\frac{1}{2}\mathbf{y}_{k}^{\top}\mathbf{K}\mathbf{y}_{k}$
(24)
$\displaystyle=\frac{\gamma}{2}\mathbf{e}_{k}^{\top}Q\mathbf{e}_{k}+\frac{1}{2}\mathbf{e}_{k}^{\top}\mathbf{\Sigma}_{k}^{-1}\mathbf{e}_{k}+\gamma\mathbf{e}_{k}^{\top}Q\overline{\mathbf{X}}_{k}.$

Building on the equivalence in (24), we can approximate the expectation in
(23) by integrating with respect to the distribution of the random variable
$\mathbf{e}_{k}$. Since
$\mathbf{e}_{k}\sim\mathcal{N}(\mathbf{0},\mathbf{\Sigma}_{k})$, we can
express it as follows:

$\displaystyle-\frac{2}{\gamma}\log\mathbb{E}\left[\exp\left\\{-\frac{\gamma}{2}\mathbf{e}_{k}^{\top}Q\mathbf{e}_{k}-\gamma\mathbf{e}_{k}^{\top}Q\overline{\mathbf{X}}_{k}\right\\}\right]$
(25)
$\displaystyle=-\frac{2}{\gamma}\log\left(\int_{\mathbb{R}^{n}}\exp\left\\{\frac{-1}{2}\left(\mathbf{e}_{k}-\mathbf{y}_{k}\right)^{\top}\mathbf{K}\left(\mathbf{e}_{k}-\mathbf{y}_{k}\right)\right.\right.$
$\displaystyle\qquad+\left.\left.\frac{1}{2}\mathbf{y}_{k}^{\top}\mathbf{K}\mathbf{y}_{k}+\frac{1}{2}\mathbf{e}_{k}^{\top}\mathbf{\Sigma}_{k}^{-1}\mathbf{e}_{k}\right\\}p(\mathbf{e}_{k})\mathrm{d}\mathbf{e}_{k}\right),$

where
$p(\mathbf{e}_{k})=\frac{1}{\sqrt{\mathbf{A}}}\exp\left\\{-\frac{1}{2}\mathbf{e}_{k}^{\top}\mathbf{\Sigma}_{k}^{-1}\mathbf{e}_{k}\right\\}$
represents the probability density function of $\mathbf{e}_{k}$. By
substituting $p(\mathbf{e}_{k})$ in (25) and simplifying it further, we
obtain:

$\displaystyle-\frac{2}{\gamma}\log\mathbb{E}\left[\exp\left\\{-\frac{\gamma}{2}\mathbf{e}_{k}^{\top}Q\mathbf{e}_{k}-\gamma\mathbf{e}_{k}^{\top}Q\overline{\mathbf{X}}_{k}\right\\}\right]$
(26)
$\displaystyle=-\frac{2}{\gamma}\log\left(\frac{1}{\sqrt{\mathbf{A}}}\int_{\mathbb{R}^{n}}\exp\left\\{-\frac{1}{2}\left(\mathbf{e}_{k}-\mathbf{y}_{k}\right)^{\top}\mathbf{K}\left(\mathbf{e}_{k}-\mathbf{y}_{k}\right)\right.\right.$
$\displaystyle\qquad+\left.\left.\frac{1}{2}\mathbf{y}_{k}^{\top}\mathbf{K}\mathbf{y}_{k}\right\\}\mathrm{d}\mathbf{e}_{k}\right)$
$\displaystyle=-\frac{2}{\gamma}\log\left(\frac{1}{\sqrt{\mathbf{A}}}\exp\left(\frac{1}{2}\mathbf{y}_{k}^{\top}\mathbf{K}\mathbf{y}_{k}\right)\right.$
$\displaystyle\left.\qquad\times\int_{\mathbb{R}^{n}}\exp\left\\{\frac{-1}{2}\left(\mathbf{e}_{k}-\mathbf{y}_{k}\right)^{\top}\mathbf{K}\left(\mathbf{e}_{k}-\mathbf{y}_{k}\right)\right\\}\mathrm{d}\mathbf{e}_{k}\right),$

where $\textbf{A}=(2\pi)^{n}\det\mathbf{\Sigma}_{k}$. We can now evaluate the
integral. Since the integrand is a Gaussian distribution with mean
$\mathbf{y}_{k}$ and covariance matrix $\mathbf{K}^{-1}$, the integral
evaluates to $\sqrt{\frac{(2\pi)^{n}}{\det\mathbf{K}}}$. Substituting this
back into the expression given in (26), we have:

$\displaystyle-\frac{2}{\gamma}\log\mathbb{E}\left[\exp\left\\{-\frac{\gamma}{2}\mathbf{e}_{k}^{\top}Q\mathbf{e}_{k}-\gamma\mathbf{e}_{k}^{\top}Q\overline{\mathbf{X}}_{k}\right\\}\right]$
(27)
$\displaystyle=-\frac{2}{\gamma}\log\left(\frac{\exp\left(\frac{1}{2}\mathbf{y}_{k}^{\top}\mathbf{K}\mathbf{y}_{k}\right)}{\sqrt{(2\pi)^{n}\det\mathbf{\Sigma}_{k}}}\sqrt{\frac{(2\pi)^{n}}{\det\mathbf{K}}}\right)$
$\displaystyle=-\frac{2}{\gamma}\log\left(\frac{\exp\left(\frac{1}{2}\mathbf{y}_{k}^{\top}\mathbf{K}\mathbf{y}_{k}\right)}{\sqrt{\det(\mathbf{\Sigma}_{k}\mathbf{K})}}\right)$
$\displaystyle=\frac{1}{\gamma}\log\det(\mathbf{\Sigma}_{k}\mathbf{K})-\frac{1}{\gamma}\mathbf{y}_{k}^{\top}\mathbf{K}\mathbf{y}_{k}$
$\displaystyle=\frac{1}{\gamma}\log\operatorname{det}\left(\mathbf{I}+\gamma
Q\mathbf{\Sigma}_{k}\right)-\frac{1}{\gamma}\mathbf{y}_{k}^{\top}\mathbf{K}\mathbf{y}_{k}.$

By substituting
$\mathbf{y}_{k}=-\gamma\mathbf{K}^{-1}Q\overline{\mathbf{X}}_{k}$, and
$\mathbf{K}=\gamma Q+\mathbf{\Sigma}_{k}^{-1}$ into (27), we can simplify the
second term $-\frac{1}{\gamma}\mathbf{y}_{k}^{\top}\mathbf{K}\mathbf{y}_{k}$
as follows:

$-\frac{1}{\gamma}\mathbf{y}_{k}^{\top}\mathbf{K}\mathbf{y}_{k}=-\gamma\overline{\mathbf{X}}_{k}^{\top}Q\left(\gamma
Q+\mathbf{\Sigma}^{-1}_{k}\right)^{-1}Q\overline{\mathbf{X}}_{k},$ (28)

assuming that $Q$ is a diagonal matrix and $\mathbf{\Sigma}_{k}$ is a
symmetric matrix. Afterward, we can utilize the Woodbury matrix identity to
simplify the expression $\left(\gamma Q+\mathbf{\Sigma}_{k}^{-1}\right)^{-1}$,
where the Woodbury matrix identity is defined as [30]:

$(A+UCV)^{-1}=A^{-1}-A^{-1}U(C^{-1}+VA^{-1}U)^{-1}VA^{-1}.$ (29)

By applying the Woodbury identity expressed in (29) to the given expression
with $A=\gamma Q$, $C=\mathbf{\Sigma}_{k}^{-1}$, and $U=V=\mathbf{I}$ (where
$\mathbf{I}$ denotes the identity matrix), we can rewrite $\left(\gamma
Q+\mathbf{\Sigma}_{k}^{-1}\right)^{-1}$ as follows:

$\left(\gamma
Q+\mathbf{\Sigma}_{k}^{-1}\right)^{-1}=\frac{1}{\gamma}Q^{-1}\Bigr{[}\mathbf{I}-Q_{\mathrm{rs}}Q^{-1}\Bigl{]},$
(30)

where $Q_{\mathrm{rs}}={\left(Q^{-1}+\gamma\mathbf{\Sigma}_{k}\right)^{-1}}$.
By substituting (30) back into (28), we obtain:

$-\frac{1}{\gamma}\mathbf{y}_{k}^{\top}\mathbf{K}\mathbf{y}_{k}=-\left\|\overline{\mathbf{X}}_{k}\right\|_{Q}^{2}+\left\|\overline{\mathbf{X}}_{k}\right\|_{Q_{\mathrm{rs}}}^{2}.$
(31)

Subsequently, by substituting (31) into (27) and further substituting the
obtained results into (22), while replacing $\overline{\mathbf{X}}_{k}$ with
$\bar{\mathbf{x}}_{k}-\mathbf{x}_{f}$, the desired result can be obtained as
follows:

$q_{\text{rs}}(\mathbf{x}_{k})=\frac{1}{\gamma}\log\operatorname{det}\left(\mathbf{I}+\gamma
Q\mathbf{\Sigma}_{k}\right)+\left\|\mathbf{\bar{\mathbf{x}}}_{k}-\mathbf{x}_{f}\right\|_{Q_{\mathrm{rs}}}^{2}.$
(32)

It is worth emphasizing that a similar form of (32), along with its
derivation, can be found in [8]. However, the presented derivation process in
[8] is brief and lacks detailed explanations and clarifications.