# Stein Variational Ergodic Search

Darrick Lee  Mathematical Institute
University of Oxford
<EMAIL_ADDRESS>Cameron Lerch  Yale University
<EMAIL_ADDRESS>Fabio Ramos  NVIDIA, USA
The University of Sydney, Australia
<EMAIL_ADDRESS>Ian Abraham  Yale University
<EMAIL_ADDRESS>
###### Abstract

Exploration requires that robots reason about numerous ways to cover a space
in response to dynamically changing conditions. However, in continuous domains
there are potentially infinitely many options for robots to explore which can
prove computationally challenging. How then should a robot efficiently
optimize and choose exploration strategies to adopt? In this work, we explore
this question through the use of variational inference to efficiently solve
for distributions of coverage trajectories. Our approach leverages ergodic
search methods to optimize coverage trajectories in continuous time and space.
In order to reason about distributions of trajectories, we formulate ergodic
search as a probabilistic inference problem. We propose to leverage Stein
variational methods to approximate a posterior distribution over ergodic
trajectories through parallel computation. As a result, it becomes possible to
efficiently optimize distributions of feasible coverage trajectories for which
robots can adapt exploration. We demonstrate that the proposed Stein
variational ergodic search approach facilitates efficient identification of
multiple coverage strategies and show online adaptation in a model-predictive
control formulation. Simulated and physical experiments demonstrate
adaptability and diversity in exploration strategies online.

##  I Introduction

Effective robotic exploration requires robots to reason about different ways
to explore a space. In the presence of uncertainty and in unstructured
environments, having multiple exploration strategies for robots to quickly
choose from can be advantageous. However, optimizing for multiple exploratory
paths can be computationally challenging, especially in continuous domains
where trajectory solutions are infinite dimensional. Defining the problem of
exploration on a grid [11, 5, 14, 37] can provide the means to enumerate many
coverage paths; however, grid-based methods limit where the robot can visit in
continuous, unstructured environments.

Recent advancements in curiosity- and information- based exploration have
allowed robots to explore vast domains [37, 27, 24, 34, 9]. However, they are
often limited to exploring using one strategy, i.e., an information maximizing
strategy. As a result, exploring becomes myopic where immediate information
gain is sought after without regard to advantageous states in the future.
Ergodicity-based exploration techniques show promise in breaking away from
myopic strategies by posing exploration as a coverage problem based on time-
averaged trajectory visitation [23, 25]. More specifically, ergodic methods
optimize over where trajectories spend time on average as a function of the
expected measure of information. As a result, ergodic search methods allow
robots to optimize exploratory paths in multi-modal search problems [26].
While the literature has proven ergodic-methods to produce effective
exploration strategies, they only optimize one search strategy at any given
moment, thus limiting how robots can adapt.

Figure 1: The Stein Variational Ergodic Approach.  Robotic exploration is
challenging as there are many ways to effectively explore an area that robots
need to reason about. Calculating all the possible exploration strategies can
be computationally prohibitive, especially when there is no guarantee that
optimized solutions will coverage to a diverse set of strategies. We propose
to solve this problem by posing coverage and exploration as an inference
problem over distributions of trajectories. Our approach leverages ergodic
exploration techniques in conjunction with Stein variational methods to
efficiently optimize diverse exploration strategies in parallel over
continuous domains (see above). Illustrated is a set of $4$ exploration
strategies optimized to uniformly explore around the cylinders. The green
trajectory indicates the selected best strategy.

Prior work suggests that the non-convex form of ergodic search methods are
capable of identifying multiple optimal trajectories through variations in
initial condition [25]. The challenge with reasoning about multiple, i.e.,
distributions, of trajectories is that the optimization can become
computationally prohibitive. Furthermore, there is no guarantee that certain
initial conditions will not converge onto the same ergodic search strategy
which is not ideal, especially in online exploration scenarios where mode
collapse on trajectory solutions can have catastrophic consequences.

Stein variational inference methods show promise in providing the necessary
tools to approximate distributions of trajectories in a computationally
tractable manner [22]. These methods leverage approximate inference in a non-
parametric manner that 1) empirically estimates complex distributions, and 2)
can do so in a computationally efficient manner through parallel computation.
Thus, in this work, we propose a novel formulation of ergodic exploration
using Stein variational gradient decent methods [22] that solves an inference
problem on distributions of trajectories. We find that the spectral
construction of the ergodic metric promotes discovery of locally optimal
solutions which can be leveraged to guide robot exploration with multiple
redundancies. We demonstrate the efficacy of our approach in simulation and on
a physical drone system that can efficiently and effectively adapt exploration
in cluttered environments. Furthermore, we find that the synergy between Stein
variational methods and ergodic search methods promotes a diverse range of
exploration strategies which can be acquired in a computationally efficient
manner. In summary, our contributions are

  1. 1.

A Stein variational ergodic search method for trajectory optimization and
control;

  2. 2.

Demonstration of diverse exploration strategies in a computationally efficient
manner; and

  3. 3.

Real-time exploration and adaptation of multiple ergodic trajectories in
dynamic domains.

The remainder of the paper is structured as follows: Section §II provides an
overview of related work, Section §III-A introduces the problem of coverage
via ergodic search, Section §III-B introduces the Stein variational gradient
descent, Section §IV derives the proposed Stein variational ergodic search
approach, and Sections §V, and §VII presents the results and conclusions.

##  II Related Work

Coverage and Exploration. Robotic exploration seeks to guide robots towards
unexplored areas within a domain. Likewise, coverage is concerned with
generating paths and placement of a robot’s sensors such that it covers a
bounded domain [8, 3]. Early solutions to coverage and exploration are often
formulated over discrete grids (defined on continuous space) and shown to have
completeness guarantees through novel boustrophedon search patterns (which
originates the lawnmower pattern [11]) and the traveling salesperson problem
[4, 19]. Through grids, it is relatively straightforward to use exhaustive
computational techniques to enumerate the many different, feasible paths a
robot can take to explore and cover an area. Extending these ideas to
continuous domains (which provides infinite spatial resolution for the robot
to traverse) often proves challenging as the number of possible ways to
explore a domain becomes infinite.

Recent methods in reinforcement learning and information-based methods
circumvent the issues with transitioning to continuous domains by leveraging
“curiosity” measures [27, 7] derived from information theory [10] to encourage
exploration. These measures provide a signal which informs a robot where it is
beneficial to visit and have demonstrated comparability with continuous
domains [27, 24]. However, many of these methods tend to be myopic in nature,
only focusing on immediate information gain, and often limited to single-mode
solutions which can limit the adaptability of robots in the wild. More recent
efforts in ergodicity-based methods (also referred to as ergodic exploration,
coverage, or search) have demonstrated that it is possible to compute
intricate coverage patterns through a spectral-based metric over continuous
domains [26, 1, 12]. Ergodicity-based methods generate coverage by minimizing
the difference between the (expected) spatial distribution of information and
the time-averaged spatial distribution of an agent’s trajectory within the
domain [23]. Interestingly, prior work has provided evidence that suggests
ergodic coverage strategies produce optimal exploration strategies [13]. The
spectral, multi-scale composition of the ergodic metric [31] suggests there
exist many solutions that can be exploited by robots. However, it has yet to
be demonstrated how one can calculate sets of “good”, locally-optimal ergodic
trajectory solutions that a robot can adapt as its search strategy.

Control as Inference. The introduction of probabilistic inference to optimal
control has enhanced the capabilities of robots via sample-based methods.
Specifically, the non-convexity of many robot task specifications makes it
challenging for gradient-based methods to find reasonable trajectory solutions
that satisfy a task. Incidentally, this is caused by the non-convexity of many
robotic tasks due to its specification or the underlying complexity of the
interactions, e.g., contact dynamics. Sample-based methods derived from
probabilistic inference circumvent these issues through zero-order
optimization, e.g., predictive sampling [15], and model-predictive path
integral (MPPI) [39, 38], that are less sensitive to the non-convexity of
robotic tasks. Add in the significant technological leap of GPU-based
computation, and control as an inference problem becomes a powerful tool for
robotics. However, like with gradient-based methods, many of the sample-based
techniques typically solve for only one locally optimal solution, ignoring
other equally viable ways to solve a task.

Having more than one plan for which robots can switch between is highly
valuable for real-world systems that need to quickly change strategy. For
example, in crowd-based navigation, certain planned paths often become
infeasible due to the dynamically changing environment. Rather than having to
recalculate a new path, it is more efficient and robust for a robot to have
several paths to choose from [35, 18]. The recent adoption of optimal
transport and variational methods in optimal control has demonstrated promise
is isolating the many locally optimal solutions for robots to choose from [20,
18]. These approaches pose trajectory solutions to non-convex optimal control
problems as inference problems were solutions are approximated as
distributions. As a result, it becomes possible for robots to optimize for
many locally optimal solutions and adapt in real-time. However, most of the
applications have focused on obstacle avoidance and point-to-point navigation.
Exploration and coverage has not yet been explored primarily due to 1) the
difficulty of forming coverage objectives in continuous domains, and 2) stable
convergence of solutions over long-time horizons.

In this work, we demonstrate it is possible to solve for many coverage
trajectory solutions in continuous space that promote diverse and robust
exploration over long-time horizons by forming ergodic coverage methods as an
inference problem and leverage second-order Stein variational gradients [18,
22] to converge on solutions.

##  III Background and Preliminaries

###  III-A Ergodic Search

Let us first define a robot’s state space and control space as
$\mathcal{X}\subseteq\mathbb{R}^{n}$ and $\mathcal{U}\subseteq\mathbb{R}^{m}$.
Next let us define the robot’s state trajectory
$x(t):\mathbb{R}^{+}\to\mathcal{X}$ as the solution to the initial value
problem

$x(t)=x(0)+\int_{0}^{t}f(x(\tau),u(\tau))d\tau$ (1)

where $x(0)\in\mathcal{X}$ is an initial condition,
$u(t):\mathbb{R}^{+}\to\mathcal{U}$ is a control trajectory, and
$f(x,u):\mathcal{X}\times\mathcal{U}\to\mathcal{T}_{\mathcal{X}}$ is the
continuous-time (potentially nonlinear) dynamics of the robot. We denote

$\mathsf{Paths}(\mathcal{X})\coloneqq\bigcup_{T>0}\mathcal{C}([0,T],\mathcal{X})$
(2)

as the set of all continuous trajectories defined on arbitrary finite time
intervals. In addition, let us define a bounded domain where the robot
explores as $\mathcal{W}=[0,L_{0}]\times\ldots\times[0,L_{v-1}]$, where $v\leq
n$ and $L_{i}$ are the bounds of the workspace. We fix a map
$g(x):\mathcal{X}\to\mathcal{W}$ that projects state space $\mathcal{X}$ to
exploration space $\mathcal{W}$, e.g., a selection matrix $g(x)=\mathbf{S}x$
that isolates and scales certain states that correspond to exploration.

######  Definition 1.

Time-Averaged Trajectory Statistics. Let $m$ denote the Lebesgue measure on
$\mathbb{R}^{+}$, $x(t):\mathbb{R}^{+}\to\mathcal{X}$ be a trajectory, and
$g:\mathcal{X}\to\mathcal{W}$. For each $T\in\mathbb{R}^{+}$, let the
probability measure $\mathcal{Q}_{T}$ on $\mathcal{W}$ that defines the time-
averaged trajectory visitation statistics integrated along time $[0,T]$ be
defined by

$\mathcal{Q}_{T}(A)\coloneqq\frac{1}{T}m\big{(}(g\circ
x)^{-1}(A)\cap[0,T]\big{)},$ (3)

where $A\subset\mathcal{W}$ is a Borel set.

######  Definition 2.

Ergodicity. A trajectory $x(t)$ is ergodic with respect to a Borel probability
measure $\mu$ on $\mathcal{W}$ if $\mathcal{Q}_{T}$ converges weakly to $\mu$
as $T\to\infty$. That is,

$\lim_{T\to\infty}\int_{\mathcal{W}}\phi(w)\,d\mathcal{Q}_{T}(w)=\int_{\mathcal{W}}\phi(w)\,d\mu(w)$
(4)

for all continuous functions $\phi\in\mathcal{C}(\mathcal{W})$. In particular,
the trajectory statistics measure can be viewed as an integral of delta
functions, where

$\int_{\mathcal{W}}\phi(w)\,d\mathcal{Q}_{T}(w)=\frac{1}{T}\int_{0}^{T}\phi(g\circ
x(t)))\,dt$ (5)

which is the common definition in literature [23, 25].

Roughly speaking, a trajectory is ergodic with respect to the measure $\mu$ if
it eventually (as $T\to\infty$) explores the workspace in a manner which is
commensurate with $\mu$. This is formalized by requiring the measure
$\mathcal{Q}_{T}$, which quantifies the proportion of time the trajectory is
contained in subsets $A\subset\mathcal{W}$ during the interval $[0,T]$, to
converge to $\mu$. Because $\mathcal{W}$ is compact, weak convergence
$\mathcal{Q}_{T}\xrightarrow{T\to\infty}\mu$ is defined by

$\lim_{T\to\infty}\int_{\mathcal{W}}\phi(w)\,d\mathcal{Q}_{T}(w)=\int_{\mathcal{W}}\phi(w)\,d\mu(w)$
(6)

for all integrable functions $\phi\in C(\mathcal{W})$. However, since robots
only run for finite time horizons, we must quantify the level of ergodicity of
finite trajectories.

######  Definition 3.

Ergodic Cost Function. Let $\mu$ be a probability measure on $\mathcal{W}$. A
_$\mu$ -ergodic cost function_ is a function
$\mathcal{E}_{\mu}:\mathsf{Paths}(\mathcal{X})\to\mathbb{R}$ such that for an
infinite trajectory $x(t):\mathbb{R}^{+}\to\mathcal{X}$, if
$\mathcal{E}_{\mu}(x|_{[0,T]})\to 0$ as $T\to\infty$ then $x(t)$ is ergodic.

To define the ergodic metric for trajectory optimization, we use spectral
methods and construct a metric in the Fourier space [23, 31, 26].

######  Definition 4.

Spectral Ergodic Cost Function. Let $\mu$ be a probability measure on
$\mathcal{W}$. Let $\mathcal{K}^{v}\subset\mathbb{N}^{v}$ be the set of all
integer $k$ fundamental frequencies that define the cosine Fourier basis
function

$F_{k}(w)=\frac{1}{h_{k}}\prod_{i=0}^{v-1}\cos\left(\frac{w_{i}k_{i}\pi}{L_{i}}\right)$
(7)

where $h_{k}$ is a normalizing factor (see [26, 23]). For a finite trajectory
$x(t):[0,T]\to\mathcal{X}$, let $\mathcal{Q}_{T}$ be the measure defined in
Eq. (3). The spectral ergodic cost function is defined as

$\displaystyle\mathcal{E}_{\mu}(x)=\sum_{k\in\mathcal{K}^{v}}\Lambda_{k}\left(Q^{k}_{T}-\mu^{k}\right)^{2}$
(8)
$\displaystyle=\sum_{k\in\mathcal{K}^{v}}\Lambda_{k}\left(\frac{1}{T}\int_{0}^{T}F_{k}(g\circ
x(t))dt-\int_{\mathcal{W}}F_{k}(w)d\mu(w)\right)^{2}$

where $\mathcal{Q}_{T}^{k}$ and $\mu^{k}$ are the $k^{\text{th}}$ Fourier
decomposition modes of $\mathcal{Q}_{T}$ and $\mu$, respectively (using Eq.
(5)), and $\Lambda_{k}=(1+\left\|k\right\|_{2})^{-\frac{v+1}{2}}$ is a weight
coefficient that places higher importance on lower-frequency modes.

In particular, the spectral ergodic cost function defined for a probability
measure $\mu$ forms a metric [23] which is able to generate coverage
trajectories at arbitrary spacial scales [31] (which we additionally prove in
Appendix A-A). This is advantageous as 1) the metric defines a proper coverage
distances using spectral modes in continuous space; and 2) specifies
infinitely many ways to explore with respect to the spectral decomposition.
Note that while the ergodic metric forms a non-convex objective in the
trajectory space, the non-convexity specifies many trajectory solutions that
robots can take leverage to improve exploration. The challenge is then
optimizing for the distribution of feasible ergodic trajectory solutions.

We can formulate an optimization problem over trajectories $x(t)$ through the
ergodic metric. Consider that $x(t)$ is constrained (either through dynamics
(1) or through some other differential constraints). Then the ergodic
trajectory optimization is given as

$\displaystyle\min_{\begin{subarray}{c}x(t)\in\mathsf{Paths}(\mathcal{X})\\\
u(t)\in\mathcal{U}\,\forall t\in[0,T]\end{subarray}}$
$\displaystyle\mathcal{E}_{\mu}(x(t))$ (9) subject to $\displaystyle
h_{1}(x)=0\,\,\forall t\in[0,T]$ $\displaystyle h_{2}(x)\leq 0\,\,\forall
t\in[0,T]$

where initial conditions and input trajectory constraints $u(t)$ can be
accounted for in the equality and inequality constraints $h_{1},h_{2}$ , e.g.,
$\dot{x}=f(x,u)$ and $u\in\mathcal{U}$.

###  III-B Stein Variational Gradient Descent

A powerful method to perform probabilistic inference is variational inference
(VI), where one aims to approximate a complex target distribution $p(x)$ with
a candidate distribution $q(x)$ from a parameterized family of distributions
$\mathcal{D}$. This is typically achieved by minimizing the Kullback-Leibler
(KL) divergence,

$\displaystyle
q^{*}=\operatorname*{arg\,min}_{q\in\mathcal{D}}D_{\text{KL}}(q||p).$ (10)

However, finding an appropriate family $\mathcal{D}$ which balances accuracy
with tractable computations is often challenging. Stein variational gradient
descent (SVGD) [22] addresses this issue by providing a nonparametric method
to sample from the target distribution $p(x)$ via kernel methods and gradient
descent.

The Stein variational method is derived by sampling a collection of points
$\\{x^{i}_{0}\\}_{i=1}^{N}$ using a prior distribution on $\mathcal{X}$, and
then iteratively updating them by the gradient descent

$\displaystyle x^{i}_{r+1}=x_{r}^{i}+\epsilon\phi^{*}_{r}(x_{r}^{i}).$ (11)

In particular, $\epsilon>0$ is the step size, and
$\phi^{*}_{r}:\mathcal{X}\to\mathcal{T}_{\mathcal{X}}$ is the vector field
which maximally decreases the KL-divergence at the $r^{th}$ step (not to be
confused with fundamental frequency).

Suppose $k:\mathcal{X}\times\mathcal{X}\to\mathbb{R}$ is a positive definite
kernel, and $\mathcal{H}$ is its corresponding reproducing kernel Hilbert
space (RKHS). We restrict the vector field to be the function class
$\mathcal{H}^{n}$, and therefore

$\displaystyle\phi^{*}_{r}=\operatorname*{arg\,max}_{\phi\in\mathcal{H}^{n}}\\{-\nabla_{\epsilon}D_{\text{KL}}(\hat{q}_{r}||p)\,:\,\|\phi\|_{\mathcal{H}^{n}}\leq
1\\},$ (12)

where $\hat{q}_{r}=\frac{1}{N}\sum_{i=1}^{N}\delta_{x^{i}_{r}}$ denotes the
empirical distribution at step $r$. Note that if the kernel $k$ is universal,
$\mathcal{H}^{n}$ is dense in the space of continuous functions
$C(\mathcal{X},\mathbb{R}^{n})$, and does not result in a loss of generality.
Furthermore, from [22], we have an exact form for $\phi^{*}_{r}$,

$\displaystyle\phi^{*}_{r}(\cdot)=\mathbb{E}_{x\sim\hat{q}_{r-1}}\left[k(x,\cdot)\nabla_{x}\log
p(x)+\nabla_{x}k(x,\cdot)\right]$ (13)

which can be approximated by samples $\\{x_{r}^{i}\\}^{N}_{i=1}$

$\displaystyle\phi^{*}_{r}(\cdot)=\frac{1}{N}\sum_{n=1}^{N}k(x^{i}_{r},\cdot)\nabla_{x}\log
p(x^{i}_{r})+\nabla_{x}k(x^{i}_{r},\cdot).$ (14)

Thus, the sampled points converge onto an approximation of the distribution
$p$. In this work, we leverage the Stein variational gradients to define and
solve for distributions of ergodic trajectories for robot exploration.

##  IV Ergodic Coverage as Inference

In this section, we pose the problem of computing ergodic trajectories as an
inference problem over a distribution of paths. We then derive an algorithm
for optimizing approximate distributions on ergodic paths and prove optimality
of the posterior distribution of paths in the ergodic coverage inference
problem.

###  IV-A The likelihood of ergodicity and Stein variations

In order to explore the landscape of trajectories which minimize an ergodic
cost function, we will formulate the ergodic search problem in terms of
variational inference. Adapting [18], which formulated motion planning
problems using variational inference, we introduce a binary optimality
criterion $\mathcal{O}:\mathsf{Paths}(\mathcal{X})\to\\{0,1\\}$. Suppose $\mu$
is a probability measure on $\mathcal{W}$. Then let use this optimality to
encode a $\mu$-ergodic cost likelihood function $\mathcal{E}_{\mu}$ by
defining

$\displaystyle p(\mathcal{O}|x)\coloneqq\exp(-\lambda\mathcal{E}_{\mu}(x)),$
(15)

where $\lambda>0$ is a hyperparameter and we simplify the notation by using
$\mathcal{O}$ to denote the optimal condition $\\{\mathcal{O}=1\\}$. Given a
prior distribution $p$ on $\mathsf{Paths}(\mathcal{X})$, and a positive
definite kernel $k:\mathsf{Paths}(\mathcal{X})^{2}\to\mathbb{R}$ on path
space, we can use SVGD to sample from the posterior distribution
$p(x|\mathcal{O})$. In particular, by applying Bayes’ rule, the optimal vector
field from Eq. (13) for a $\mu$-ergodic cost likelihood function
$\mathcal{E}_{\mu}$ is given by

$\phi^{*}_{r}=\mathbb{E}_{x\sim\hat{q}_{r-1}}[k(x,\cdot)(\nabla_{x}\log
p(x)-\\\ \lambda\nabla_{x}\mathcal{E}_{\mu}(x))+\nabla_{x}k(x,\cdot)],$ (16)

where $x\in\mathsf{Paths}(\mathcal{X})$ and $p(x)$ is a prior over
trajectories. The main benefit of forming an ergodic trajectory optimization
through Stein variational descent is the ability to optimize multiple
trajectories in parallel.

###  IV-B Ergodic Stein variational trajectory optimization

Instead of defining a distribution of infinite-dimensional trajectories, we
discretize the path to facilitate numerical optimization. Let
$\mathbf{x}=[x_{0},x_{1},\ldots,x_{T-1}]$, where $x_{t}\in\mathcal{X}$, be a
collection of points representing a trajectory along a discrete time horizon
$T$ indexed by discrete time $t$ (which we purposefully overload the notation
to keep consistent with the stated definitions). Note that the definition of
paths in Eq. (2), ergodicity in Def. 1, and as a cost function Def. 4 are now
calculated on discrete paths and still hold. Next, let us define an empirical
distribution over $N$ discrete paths $\mathbf{x}$ as
$\hat{q}=\frac{1}{N}\sum_{i=1}^{N}\delta_{\mathbf{x}^{i}}$. Following Eq.
(14), we approximate $p(\mathbf{x})$ using $\hat{q}$ where we assign
$p(\mathbf{x})=\mathcal{N}(\hat{\mathbf{x}},\sigma^{2})$ where
$\hat{\mathbf{x}}=\text{interp}(x_{\text{init}},x_{\text{final}})$, and
$\sigma^{2}$ is the variance. Given a kernel function $k(\mathbf{x},\cdot)$ on
discrete paths $\mathcal{X}^{T}$, the ergodic Stein variational step is given
by

$\phi^{*}_{r}(\cdot)=\frac{1}{N}\sum_{i=1}^{N}[k(\mathbf{x}^{i}_{r},\cdot)(\nabla_{\mathbf{x}}\log
p(\mathbf{x}_{r}^{i})-\\\
\lambda\nabla_{\mathbf{x}}\mathcal{E}_{\mu}(\mathbf{x}_{r}^{i}))+\nabla_{\mathbf{x}}k(\mathbf{x}_{r}^{i},\cdot)]$
(17)

where
$-k(\mathbf{x}^{i}_{r},\cdot)\lambda\nabla_{\mathbf{x}}\mathcal{E}_{\mu}(\mathbf{x}_{r}^{i})$
minimizes the ergodic cost over the trajectory sample, and
$k(\mathbf{x}_{r}^{i},\cdot)$ is a repulsive force that pushes trajectory
solutions. We can view the kernel as a similarity measure between two
trajectories $\mathbf{x},\mathbf{x}^{\prime}$. Specifically, as
$\mathbf{x}\to\mathbf{x}^{\prime}$ so does
$k(\mathbf{x},\mathbf{x}^{\prime})\to 1$ and as
$\|\mathbf{x}-\mathbf{x}^{\prime}\|\to\infty$,
$k(\mathbf{x},\mathbf{x}^{\prime})\to 0$ (when choosing a radial basis kernel
(24)). Thus, one can measure diversity of a set of particles
$\\{\mathbf{x}^{i}\\}$ using a kernel by forming a matrix with entries
$K_{ij}=k(\mathbf{x}^{i},\mathbf{x}^{j})$, (where $K_{ii}=1$) and computing
the determinant of $K$ we can establish a measure of diversity. In particular,
given a set of trajectories $\\{\mathbf{x}^{i}\\}$, $\det(K)\to 0$ as
$\mathbf{x}^{i}\to\mathbf{x}^{j}\,\,\forall i,j,\,\,i\neq j$ and $\det(K)\to
1$ as $\|\mathbf{x}^{i}-\mathbf{x}^{j}\|\to\infty\,\,\forall i,j,\,\,i\neq j$.

Constraints on the trajectory and additional terms that shape trajectory
solutions can be added by extending the cost likelihood function in the
following form

$\mathcal{L}_{\mu}(\mathbf{x})=\mathcal{E}_{\mu}(\mathbf{x})+\rho(\mathbf{x})+c_{1}h_{1}(\mathbf{x})^{2}+c_{2}\text{max}(0,h_{2}(\mathbf{x}))$
(18)

where $c_{1},c_{2}$ are positive penalty weights111$c_{1},c_{2}$ are chosen
arbitrarily, but can be treated as Lagrange multipliers in an Augmented
Lagrange formulation. that form an inner product with equality and inequality
functions $h_{1},h_{2}$, and $\rho:\mathsf{Paths}(\mathcal{X})\to\mathbb{R}$
is any additional penalty terms on the trajectory $\mathbf{x}$. We can rewrite
the ergodic Stein variational step using $\mathcal{L}_{\mu}$ as

$\phi^{*}_{r}(\cdot)=\frac{1}{N}\sum_{i=1}^{N}[k(\mathbf{x}^{i}_{r},\cdot)(\nabla_{\mathbf{x}}\log
p(\mathbf{x}_{r}^{i})-\\\
\lambda\nabla_{\mathbf{x}}\mathcal{L}_{\mu}(\mathbf{x}_{r}^{i}))+\nabla_{\mathbf{x}}k(\mathbf{x}_{r}^{i},\cdot)].$
(19)

Selection of trajectories can be done through a heuristic
$\operatorname*{arg\,max}$ operation

$i^{\star}=\operatorname*{arg\,max}_{i}\exp(-\lambda\mathcal{L}_{\mu}(\mathbf{x}_{r}^{i})).$
(20)

In Alg. 1 we outline the Stein variational ergodic trajectory optimization
algorithm.

input: measure $\mu$, domain $\mathcal{W}$, map $g:\mathcal{X}\to\mathcal{W}$,
cost $\mathcal{L}_{\mu}$, prior $p(\mathbf{x})$, kernel $k(\mathbf{x},\cdot)$,
step size $\epsilon$, iteration $r=0$, initial trajectory samples
$\\{\mathbf{x}_{0}^{i}\\}^{N}_{i=1}$, termination condition $\gamma$

while  _$\|\phi^{\star}_{r}(\mathbf{x}_{r}^{i})\forall i\|\geq\gamma$ or $r<$
max iterations_ do

for  _each sample $i$ in parallel_ do

$\mathbf{x}_{r+1}^{i}\leftarrow\mathbf{x}_{r}^{i}+\epsilon\phi^{\star}_{r}(\mathbf{x}_{r}^{i})$;

end for

$r\leftarrow r+1$

end while

return: $\\{\mathbf{x}_{r}^{i}\\}_{i=1}^{N}$,
$\operatorname*{arg\,max}_{i}\exp(-\lambda\mathcal{L}_{\mu}(\mathbf{x}_{r}^{i}))$

Algorithm 1 Stein Variational Ergodic Trajectory Opt.

###  IV-C Ergodic Stein variational control

The ergodic Stein step Eq. (19) acts on discrete points that represent robot
trajectories. We can readily extend this formulation over discrete control
inputs $\mathbf{u}=[u_{0},u_{1},\ldots,u_{T-1}]$. Consider the discrete-time
transition dynamics

$x_{t+1}=F(x_{t},u_{t}),$ (21)

then, given an initial condition $x_{0}$, and a sequence of controls
$\mathbf{u}$, $\mathbf{x}$ is calculated through recursive application of Eq.
(21) starting from $x_{0}$. Using this formulation, we can define the problem
over control inputs $\mathbf{u}$ where we optimize over
$p(\mathbf{u}|\mathcal{O})$ and by an approximate distribution over control
sequences $\hat{q}=\frac{1}{N}\sum_{i=1}^{N}\delta_{\mathbf{u}^{i}}$. We can
then optimize strictly over controls by using a kernel $k$ on discrete control
paths $\mathcal{U}^{T}$, where the Stein variational step is

$\phi^{*}_{r}(\cdot)=\frac{1}{N}\sum_{i=1}^{N}[k(\mathbf{u}^{i}_{r},\cdot)(\nabla_{\mathbf{u}}\log
p(\mathbf{u}_{r}^{i})-\\\
\lambda\nabla_{\mathbf{u}}\mathcal{E}_{\mu}(\mathbf{x}_{r}^{i}|\mathbf{u}_{r}^{i},x_{0}))+\nabla_{\mathbf{u}}k(\mathbf{u}_{r}^{i},\cdot)]$
(22)

where $\mathcal{E}_{\mu}(\mathbf{x}_{r}^{i}|\mathbf{u}_{r}^{i},x_{0})$ is the
ergodic metric of trajectory $\mathbf{x}$ given initial condition $x_{0}$ and
control sequence $\mathbf{u}$. This form is useful in adaptive model-
predictive control (MPC) where direct control values are replanned online. As
with trajectory optimization, we can rewrite the Stein step using Eq. (18) and
introduce constraints on the control and trajectory as needed. In most MPC
formulations, after the control trajectory is optimized, it is common to pass
to the robot the first control value and shift the sequence of controls, i.e.,
$u_{0:T-2}=u_{1:T-1}$ to warm start the optimization. We follow [22] for
updating the controls. We outline the Stein variational ergodic control in
Alg. 2.

input: initial state $x_{0}$, time horizon $T$, measure $\mu$, domain
$\mathcal{W}$, map $g:\mathcal{X}\to\mathcal{W}$, cost $\mathcal{L}_{\mu}$,
prior $p(\mathbf{u})$, kernel $k(\mathbf{u},\cdot)$, step size $\epsilon$,
prior control samples $\\{\mathbf{u}_{0}^{i}\\}^{N}_{i=1}$, termination
condition $\gamma$

iteration $r=0$;

while  _$\|\phi^{\star}_{r}(\mathbf{u}_{r}^{i})\forall i\|\geq\gamma$ or $r<$
max iterations_ do

for  _each sample $i$ in parallel_ do

$\mathbf{u}_{r+1}^{i}\leftarrow\mathbf{u}_{r}^{i}+\epsilon\phi^{\star}_{r}(\mathbf{u}_{r}^{i})$;

end for

$r\leftarrow r+1$

end while

return: $\\{\mathbf{u}_{r}^{i}\\}_{i=1}^{N}$,
$i^{\star}=\operatorname*{arg\,max}_{i}\exp(-\lambda\mathcal{L}_{\mu}(\mathbf{u}_{r}^{i}))$
;

apply $u_{0}^{i^{\star}}$ to robot;

;

/*shift controls */

for  _each sample $i$ in parallel_ do

$u_{0:T-2}^{i}\leftarrow u_{1:T-1}^{i}$;

end for

;

/*sample state and return to input */

Algorithm 2 Stein Variational Ergodic Control Figure 2: Stein Ergodic
Solutions Converge to Similar Ergodicity. (a) Log of mean ergodic losses for
$50$ trajectories optimized by the Stein variational ergodic search on a
uniform $\mu$. Note the tight bound of 2-standard deviation on the ergodic
losses suggests trajectory solutions are close in optimality. (b) Overlap of
$50$ ergodic trajectories that provide uniform coverage over the $1m\times 1m$
domain. Figure 3: Measured Kernel Ergodic Trajectory Diversity. Illustrated is
a comparison of trajectory diversity using the Stein variation ergodic
approach and the vanilla ergodic trajectory optimization. (a) Ergodic losses
for $20$ trajectories using the proposed Stein variational ergodic method and
the vanilla ergodic Trajectories. (b) Trajectory diversity measured using the
determinant of the RBF matrix kernel
$K_{ij}=k(\mathbf{x}^{i},\mathbf{x}^{j})$, (where $K_{ii}=1$ and $k$ is given
by Eq. (24)) with a fixed $h=0.01$. The initial sampled trajectories are drawn
from a zero-mean Gaussian distribution with $\sigma^{2}=0.01$. Values
approaching $1$ suggest diversity of trajectories (the Kernel matrix is
strongly diagonal with values $1$). Smaller values suggest similar
trajectories (the Kernel matrix consists of identical values).

###  IV-D Kernel design for Ergodic trajectories

Exploration requires reasoning about long time horizons to promote effective
coverage. This poses a problem as Stein variational methods tend to perform
poorly in high-dimensional problems, e.g., $T=100$ for $n=2$ results in $200$
dimensional inference problem [40]. The repulsive terms that provide the
diversity in solutions vanishes at higher dimensions (due to the derivative of
the kernel diminishing with respect to its input). The most common way to
effectively deal with the computational issues is by choosing a kernel of the
form

$k(\mathbf{x},\mathbf{x}^{\prime})=\sum_{t}k(x_{t},x_{t})+\sum_{(t,t^{\prime})\in\mathcal{G}}k(x_{t},x_{t}^{\prime})$
(23)

where $\mathcal{G}$ is a graph of connected points separated by time and
$k(\mathbf{x},\mathbf{x}^{\prime})$ is a sum of positive semi-definite kernels
that leverages the Markov property of state trajectories [18] which we refer
to as a Markov Kernel. As with prior work, a good choice of kernel is the
smooth radial basis function (RBF)

$k(x,x^{\prime})=\exp\left(-\|x-x^{\prime}\|_{2}^{2}/h\right)$ (24)

where $h=\text{med}\left(\\{\mathbf{x}\\}\right)/\log N$ is a heuristic that
takes the median of the particles. It is possible to choose the kernel to be a
composition, e.g., a Markov RBF kernel, which we evaluate the effect of
kernels on trajectory diversity in Section §V.

In this work, we leverage the multiscale aspect of ergodicity and the spectral
ergodic metric and define the kernel and trajectories in the workspace
$\mathcal{W}=[0,L_{0}]\times[0,L_{1}],\ldots\times[0,L_{v-1}]$ where $L_{i}$
can be arbitrarily chosen to satisfy numerical conditioning, e.g., $L_{i}=1$,
and $\forall w\in\mathcal{W},w\in[0,1]^{v}$. Given an invertible and linear
map $g:\mathcal{X}\to\mathcal{W},g^{-1}:\mathcal{W}\to\mathcal{X}$, we can
rewrite the trajectories in terms of the composition
$\mathbf{w}=g\circ\mathbf{x}=[g(x_{0}),\ldots,g(x_{T-1})]$ which significantly
improves numerical conditioning of the kernel without loss of generality. Note
that if $k(x,x^{\prime})=1\,\forall
x,x^{\prime}\in\mathsf{Paths}(\mathcal{X})$ then the Stein variational ergodic
optimization problem reduces to a parallel trajectory optimization over random
samples $\\{\mathbf{x}^{i}\\}_{i=1}^{N}$. Other choices can provide better
options, but in this work we only consider the smooth RBF and the Markov RBF
kernel.

###  IV-E Convergence

The convergence of SVGD has been actively studied, primarily in the population
limit with infinitely many samples [21, 16, 30]. A non-asymptotic analysis of
convergence was carried out in [16], and finite sample bounds for propagation
of chaos have also been considered [33]. We show in Appendix A-B that SVGD
converges in the present setting of ergodic search by applying [16, Corollary
6].

######  Theorem 1 (Informal).

Given the assumptions in this section and in the infinite particle limit, SVGD
converges, where the gradient update steps are bounded by

$\displaystyle\frac{1}{r}\sum_{i=1}^{r}\|\phi^{*}_{i}\|^{2}_{\mathcal{H}^{Tv}}\leq\frac{\text{KL}(p||\pi)}{c_{\epsilon}r},$
(25)

where $p$ is a smooth prior distribution and $c_{\epsilon}>0$ is a constant.

###### Proof.

See Appendix A-B for formal proof. ∎

Convergence of SVGD subject to an ergodic metric is beneficial towards
guaranteeing trajectory solutions can be acquired. In the following section we
provide analysis and empirical results for our proposed approach.

Figure 4: Effect of Kernel on Ergodic Trajectory Diversity . Trajectories are
optimized to provide uniform coverage over domain. All $50$ initial trajectory
particles are initialized as an interpolating function between initial and
final poses additive zero mean noise with $\sigma^{2}=0.01$. Each (a) Radial
basis function (RBF) kernels produce smooth diverse paths. (b) Using Eq. (23)
which relies on the Markov property of trajectories $x(t)$, uniform ergodic
trajectories can be arbitrarily diverse. (c) Setting the kernel
$k(x_{i},x_{j})=1$ reduces the Stein ergodic gradient descent to parallel
gradient descent on independent trajectory particles $\mathbf{x}_{i}$. Note
that particles collapse on a single ergodic path.  Figure 5: The many ways to
explore a forest. Here, we demonstrate the many different solutions to uniform
exploration of a $100m\times 100m$ forest that our proposed method produces.
(a) Trajectories are optimized through the Stein variational ergodic approach
starting from the same initial conditions. (b) Each solution is shown to be a
locally optimal solution to the ergodic metric that provides uniform coverage
which can be used for robust environmental monitoring.

##  V Results

In this section, we provide analysis and validation of our proposed approach.
Specifically, we are interested in addressing the following:

  1. 1.

How diverse are Stein variational ergodic trajectories?

  2. 2.

Do optimized trajectories yield similar ergodic losses?

  3. 3.

How does the choice of kernel effect solutions?

  4. 4.

Can the approach handle additional constraints?

  5. 5.

Can the proposed approach adapt exploration in a real-time control setting?

Additional information regarding parameters, kernels, assumptions, and
implementation details are provided in Appendix B. Multimedia for the results
can be found in the supplementary material.

###  V-A Convergence and Trajectory Diversity

First, we are interested in evaluating whether the proposed Stein variational
ergodic trajectory optimization method converges and provides diverse
trajectories. In particular, we are interested in empirically evaluating how
ergodic trajectories can be under a Stein variational approach. We consider
the case of Alg. 1 in a 2-D planar uniform coverage problem in a bounded
domain (see Fig. 2).

The trajectories are constrained such that each consecutive time step is
regularized (ensuring smoothness of the paths). In addition, we bound the
trajectories within the $\mathcal{W}=[0,1]^{2}$ domain through a barrier
function (see Appendix B). We evaluate a discrete sample of $N=50$
trajectories of length $T=100$ (initialized with a mean trajectory
interpolating from some initial point to a final point on $\mathcal{W}$ with
additive zero mean Gaussian noise with variance $\sigma^{2}=0.01$. We set
$\mu=1\,\forall w\in\mathcal{W}$, i.e., a uniform distribution, to specify the
ergodic metric.

Stein Ergodic Trajectory Convergence. As demonstrated in Fig. 2, the proposed
Stein variational ergodic trajectory optimization converges each of the $50$
trajectories towards a minimum ergodic loss (shown in log form).
Interestingly, the standard deviation of the ergodic losses for the trajectory
remains significantly minimal (where we show $2$ standard deviation in Fig.
2). Visual inspection of the trajectories in Fig. 2 demonstrates the diversity
in the optimized solutions which produce uniform coverage over the domain.
This result emphasizes that synergy between the spectral ergodic metric and
the Stein variational gradient descent, offering the effectiveness of non-
parametric optimization in approximating a posterior
$p(\mathbf{x}|\mathcal{O})$ over trajectories, and the ergodic metric
producing complex trajectories that facilitates discovery of many exploration
strategies.

Figure 6: 3D Diverse Coverage with Aircraft Dynamics. Here, we show that our
proposed Stein ergodic controller (Alg. 2) can take into consideration
nonlinear aircraft dynamics in a 3D search environment with an obstacle. The
underlying distribution represent $\mu$ as a bimodal distribution with two
peaks projected on the 2D plane. The spherical obstacle is placed between the
two modes for which the controller has to construct $4$ trajectories to
explore around the obstacle while spending most of its time around the two
peaks.  Figure 7: Stein Variational Ergodic Model-Predictive Control with
Dynamic Obstacles. Here, we demonstrate the adaptiveness of our proposed
method in an online exploration problem with dynamics obstacles. The goal is
to visit the dark areas (defining $\mu$) while avoiding the dynamic obstacles
(green) which move based on zero mean, $\sigma=0.1$ normally distributed
velocity inputs which is unknown to the controller (only instantaneous
position of obstacles are known). Trajectory planning (total of $12$ plans are
optimized) is calculated using single-integrator dynamics with control
constraints so a drone can track the inputs. The red line indicates the best
possible solution according to our heuristic in Eq. (20), black lines indicate
planned path, and blue line indicates visited areas. The proposed Stein
variational ergodic controller optimizes several paths that can be used as
redundancies in case plans become infeasible due the dynamic environment.
Figure 8: Stein Variational Ergodic Sensor-based Exploration with Attentive
Kernel. Here, we demonstrate our approach can readily integrate sensors and
information measures is this sonar-based mapping task [9]. The target
distribution $\mu$ is specified by the entropy of the attentive kernel (AK)
Gaussian process model (developed in [9]) as data is collected (dark circles).
A total budget of $700$ points with an initial $50$ provided to initialize
model fitting of the underlying terrain (the N17E073 described in [9]). Sensor
location plans (total of $4$ plans shown in cyan where the red plan is chosen
by a heuristic) are optimized in receding horizon at a $1$ Hz rate for $20s$
into the future. The sensor location is tracked by a Dubins car dynamical
system which commits a sensor measurement at the magenta point. We compare
against the myopic planner used in [9] where red areas have high entropy and
blue areas have low entropy. The Stein variational ergodic approach spreads
out the sample points while planning several strategies for maneuvering the
sensor location.

Stein Kernel Effect on Ergodic Trajectories. Analyzing the kernel gives us
more insight as to its effect on producing diverse trajectories. In Fig. 4, we
evaluate the proposed approach in Alg. 1 on three kernels, the RBF kernel
(24), the Markov RBF [18] (23), and a kernel $k(x^{i},x^{j})=1$ that reduces
the Stein variational gradient to independent parallel gradient descent on a
set of trajectories, i.e., a benchmark on the canonical vanilla ergodic
trajectory optimization. We configure the problem as we did in the convergence
analysis presented in Fig. 2 where $\mu$ is a uniform distribution and the
trajectories (totalling $50$) are regularized to be Markovian. Each kernel is
optimized until convergence of the ergodic metric (based on the gradient
condition as described in Alg. 1). We find that the Markov RBF kernel produced
significantly more diverse trajectories than the RBF kernel alone. This is
expected as the Markov RBF kernel repels pair-wise trajectory points where RBF
produces compares whole trajectories (which results in more regular
trajectories). Compared to the base vanilla ergodic trajectory optimization,
the proposed Stein variational ergodic approach significantly outperforms the
benchmark, generating a diverse set of ergodic trajectories. Under the same
initial conditions, the benchmark ergodic trajectory optimization produces the
same ergodic trajectory, resulting in mode-collapse over the posterior and
further supporting the need for the proposed Stein variational ergodic method.

Stein Ergodic Trajectory Diversity. We can see the diversity of the
trajectories more clearly in Fig. 3 where we compare the Stein variational
ergodic approach with the vanilla ergodic metric. The kernel is given by Eq.
(24) with $h=0.01$. As seen in in Fig. 3, both methods converge towards an
ergodic trajectory local minima. The Stein variational approach produces a
wider range of local minima (based on standard deviation) and initially
produces worse ergodicity (due to the trajectory diversification). The initial
bump in the Stein variational ergodic losses are due to the initial trajectory
diversification, as shown in Fig. 3 (b). The diversity measure is calculated
using the determinant of the kernel matrix
$K_{ij}=k(\mathbf{x}^{i},\mathbf{x}^{j})$ which converges on a diverse set of
trajectories $\det(K)=1$. Interestingly, this behavior does not influence the
rate of change in the ergodic loss as both the Stein variational ergodic
approach and the vanilla ergodic trajectory optimization converge onto the
same levels of ergodicity within the same number of iterations. The
trajectories calculated from parallel ergodic trajectory optimizations
(without the repulsive force) collapses onto a single local minima which
results in $\det(K)=0$ and non-unique trajectory solutions.

Figure 9: Stein Variational Mode Collapse. Here, we demonstrate mode collapse
of Stein variational inference, i.e., when the repulsive force reduces to
zero. (Left) Ergodic trajectories randomly initialized converge to similar
solutions when the repulsive force from Stein variational is set to zero.
(Right) When the repulsive force is non-zero, the trajectory solutions
leverage the multimodal nature of the ergodic metric, yielding diverse
solutions. Mode collapse can occur with increased dimensionality of the
optimization variables which can be mitigated with the right choice of kernel
[40].

###  V-B Large-Scale and Constrained Exploration

Next, we consider real-world problem scenarios where exploration occurs in a
constrained environment (with obstacles and dynamic constraints). Our first
examples looks at generating diverse exploratory trajectories in a simulated
forest. The goal is to generate uniform coverage over a larger domain
($100m\times 100m$) and demonstrate the flexibility of the proposed approach
when dealing with arbitrary scales, and introduce several obstacles
representing trees. The trajectories are constrained to be Markovian (via a
penalty term, see Appendix B) to match velocity constraints that are feasible
for a drone to follow. The trajectories are solved over seconds to indicate
the larger-scale aspect of the example, and are required to start and end at
the same point (to further emphasize the diversity of solutions). The trees
are represented as disk penalty terms that encompass the width of the tree
(and is known at optimization time).

Multiscale Coverage in a Forest.  As illustrated in Fig. 5 is an example of
$4$ solutions to the Stein ergodic trajectory optimization in the cluttered
forest environment. Note that the initial and final positions are same for
each trajectory (due to the cost-terms, see Appendix B), and yet the proposed
approach is capable of generating highly diverse solutions (with just $4$
trajectory samples). In addition, the computation of the trajectory incurred
no additional cost as the function map $g$ was the only element that was
altered (other than the cost terms). As a result, the trajectories were
produced with relative ease owing to the synergy between the ergodic metric
with Stein variational methods, especially in terms of problem scale.

Figure 10: Computational Analysis of Stein Variational Ergodic Search. Here,
we show the computational scaling of the Stein variational ergodic search
gradient step as a function of the number of particles $N$. Note that with the
CPU (AMD Threadripper 3960X), the computational scales linearly. With GPU
computation (NVIDIA RTX 3080) we get constant computational scaling (between
$57$ and $66\mu s$). The additional parallelization is afforded by the
spectral composition of ergodic search which further improves GPU computation.
Figure 11: Scale Analysis of Stein Variational Ergodic Search. Here, we show
the computational scaling of the Stein variational ergodic search gradient
step as a function the time horizon $T$ and the exploration dimensionality
$v$. Note that due to the added parallelization, time horizon scales
computation approximately linearly. With increasing exploration
dimensionality, computation increases quadratically (which is common to most
ergodic exploration methods [26, 31]). Both examples are done with the same
number of basis functions per dimensions.  Figure 12: 3D Coverage Drone
Experiment. Here, we show experimental validation of the proposed Stein
variational ergodic search. The trajectories are optimized ($4$ total with
$T=50$) to be uniformly ergodic in the domain with three obstacles known to
the solver. The green trajectory is the most optimal according to the
heuristic (20). (a) Trajectory constraints satisfying the 3D velocity limits
are used to produce feasible paths for the Crazyflie 2.1 Drone to follow. (b)
Rendering of Stein variational ergodic trajectory optimization through the
three obstacles. Note that the trajectories appear symmetric about the first
obstacle, but are forced to be diverse through the Stein variational kernels
which produce unique trajectories.

Diverse Coverage with Nonlinear Dynamics.  We additionally evaluate our
proposed method to generate diverse coverage with nonlinear robot dynamic
constraints (see Fig. 6). The dynamics of the robot are given by a nonlinear
aircraft model (see Appendix B). The trajectory length is given by $T=150$
with $dt=0.1s$ and the Markov RBF kernel (23) was used in this example. We use
the Stein variational ergodic controller (Alg. 2) to produce a distribution of
diverse exploratory trajectories that avoids the spherical obstacle in the
middle. The generated trajectories leverage the nonlinear aircraft dynamics to
go around the obstacle and avoid collision while slowing down around the
bimodal peaks to generate coverage.

###  V-C Online Planning and Adaptive Exploration

Last, we explore the capability of the proposed approach for online planning
and adaptive exploration through the use of the Stein variational ergodic
model-predictive controller (Alg. 1). In particular, we are interested in
leveraging the parallel nature of the proposed approach for exploration to
efficiently compute exploration strategies in dynamic environments. We
evaluate the proposed controller as a receding-horizon planner, which
recomputes control strategies based on new information, in a exploration
problem with dynamic obstacles. The obstacles are assumed to be observable
after they move (and thus only their positions are known to the solver). We
plan with a fixed time horizon of $T=20$ steps starting from the robot’s
current initial condition and plan using $12$ trajectories with an RBF kernel.
The dynamics are given by a single-integrator system with velocity limits that
match the Crazyflie 2.1 drone [29]. The underlying information measure $\mu$
is given by a mixture of $4$ Gaussians placed asymmetrically over a bounded
2-D domain.

Online Planning and Control. In Fig. 7 we demonstrate the proposed receding-
horizon Stein variational ergodic controller. Illustrated is a time-series
snapshot of the controller producing several paths at various stages that
avoids obstacles and provides alternative routes to explore the high-density
areas. The selected path (in red) is based on the maximum of the cost
likelihood model at the given planning time. As a result, it is typical that
the approach switches between coverage strategies based on the immediate
information obtained form observing the environment.

Interestingly, we find the proposed control approach is still able to reason
about complex trajectories that visit multiple modal peaks within a single
path. This is an exceptionally advantageous capability that provide within-
trajectory diversity without resorting to heuristics or sample-based
augmentations e.g., control randomness [39]. This can be seen through the
smoothness of the planned trajectory paths (note in this example a trajectory
smoothness penalty was not used as the underlying dynamic constraints
naturally induce Markov trajectories). Furthermore the proposed approach
visited each of the modal peaks within a $10s$ run of the controller, even
when the obstacles cluttered around one another and cut off certain paths. The
proposed controller generates coverage trajectories around the free space
proportional to the utility of information in that space. In this scenario,
the trajectories will remain in the collision-free space until a path opens,
leading to a fast traversal as the ergodic metric will penalize staying in the
same area for too long.

Sensor-based Exploration.  We also demonstrate how our proposed approach can
readily incorporate sensors and information measures to guide exploration. A
sonar-based mapping problem as described in [9] is used to demonstrate this
added capability. The goal is to map an environment using a sonar sensor (see
[9] Fig. 7). We adopt the Attentive Kernel (AK) model where we compare against
the myopic, sample-based planner that was used for finding where to sample
data (as described in [9]). The information measure is defined as the entropy
of the Attentive Kernel Gaussian process and is inserted into the Stein
variational MPC as the utility measure $\mu$. A total of $1000$ samples are
used to compute the Fourier transform of $\mu$ numerically based on the
attentive kernel model (as done with the myopic planner). The same experiment
settings are maintained where we choose sample locations based on the
exploration path determined by the Stein variational ergodic approach (with a
budget of 700 data points). Standard Gaussian process metrics are used to
compare the quality of the Gaussian process as a function of data like,
standardized mean square error (SMSE), mean standardized log loss (MSLL),
root-mean-square error (RMSE), mean negative log-likelihood (MNLL), and mean
absolute error (MAE) as done in [9].

Empirical results for the resulting Gaussian process metrics are reported in
Table I. The results are averaged over 5 runs of each algorithm subject to the
same random seed. Note that the proposed Stein variational ergodic approach
improves the each metric without explicit fine-tuning to the sensor. As
demonstrated in Fig. 8, the proposed Stein variational ergodic approach
generates several exploration plans that plan useful data acquisition across
multiple high-information peaks. In contrast, the myopic planner sets goal
points based on a single plan but does not consider the effect collecting the
data may have on the overall information landscape. As a result, how data
points are distributed is significantly different across methods as our
proposed approach spreads out data proportional to the entropy measure rather
than densely sampling in only high-entropy areas. It is worth noting that our
approach is not specific to the exact sensor or how we define information. The
only restriction is that we are able to at least sample from the information
distribution to compute the Fourier transform.

TABLE I: Standard Gaussian Process Metrics for Training Data using Stein Erg. and Myopic Planners (lower is better). | SMSE | MSLL | NLPD | RMSE | MAE
---|---|---|---|---|---
Stein. Erg. | 0.0669 | -1.6556 | 4.0504 | 16.6373 | 11.6971
Myopic Plan. | 0.0765 | -1.6498 | 4.1785 | 17.7783 | 12.2943

Computational Analysis.  We additionally analyze the computational time of the
proposed method. In Fig. 10, we find that CPU-based computation (using an AMD
Threadripper 3960X) and acceleration through JAX [6] we get linear
computational scaling with respect to the number of samples (holding the time
horizon $T=100$ constant). With GPU-based computation and JAX (using an NVIDIA
RTX 3080) we get constant scaling (with the worse computation time of $67\mu
s$. The increase in computation is as a result of the spectral nature of the
ergodic metric. Specifically, one can additionally distribute the ergodic
metric and gradient computation quite effectively which further improves the
computation time, allowing for real-time control.

Furthermore, we provide a brief computational complexity analysis of our
methods; in particular for one SVGD step described in Eq. (19), where we
assume the paths $\mathbf{x}$ are valued in an exploration space of dimension
$v$. This Stein variational step requires the computation of both the kernel
matrix $k(\mathbf{x}_{r}^{i},\mathbf{x}_{r}^{j})$, which has a cost of
$\mathcal{O}((NvT)^{2})$, and the gradient $\nabla_{\mathbf{x}}\log
p(\mathbf{x}_{r}^{i})$, which requires the computation of $N$ gradients on an
$vT$-dimensional space. We must also compute the spectral ergodic cost
function, which is common to all ergodic search methods. For $k$ basis
functions in each dimension, the ergodic cost function from Eq. (8) has a
complexity of $\mathcal{O}(NTk^{v})$. We emphasize that all of these
computations are parallelized in practice, and Fig. (11) demonstrates the
effective parallelized scaling behavior.

###  V-D Experimental Validation

We demonstrate empirical validation of the proposed Stein variational ergodic
search method on a 3D uniform coverage problem in a crowded domain (see Fig.
12). We solve for a set of $4$ ergodic trajectories (using Alg. 1) at a length
of $T=50$ with a time step $dt=0.1s$. The coverage problem is defined over the
3D domain $\mathcal{W}=[0,3]\times[0,3]\times[0.5,1.5]$. Trajectories are
constrained to satisfy feasible drone velocities according to the
specifications of the Crazyflie 2.1 drone [29]. The best trajectory is
selected based off the heuristic (20). The path was tracked using a low-level
PID controller We find close correspondence with the planned trajectories and
what the drone was able to execute in the in a physical experiment.
Furthermore, the diversity in coverage solution can be used to readily adapt
exploration as needed.

##  VI Limitations and Best Practises

Mode Collapse.  A well-known issue with SVGD is the reduction of the repulsive
force in Eq. (19) as the dimensionality of the input space increases. This
causes the particles to concentrate in the modes of the target distribution
potentially diminishing its exploration capabilities and accuracy to capture
long-tail distributions. Fortunately, there are several ways to deal with high
dimensional inputs by exploiting the structure of our problem. In particular,
since we are planning over the space of trajectories, there is an implicit
Markovian assumption over the inference variables. This allow us to construct
structured kernels such as the sum of kernels defined over subsets of
variables that minimize the impact of performing inferences over long
sequences [40]. We demonstrate an example of mode collapse in Fig. (9) by
explicitly setting the repulsive force in Eq. (19) to zero.

Scaling.  We demonstrate in Fig. (11) that the Stein variational gradient step
empirically scales linearly in the time horizon $T$ due to the added
parallelization, despite the $\mathcal{O}(T^{2})$ complexity of the kernel
matrix evaluation. However, we empirically observe quadratic scaling with
respect to the scaling dimension $v$ due to the exponential number of basis
functions required in the ergodic cost function (8). This scaling behavior is
inherently a part of the definition of the spectral cost function and is
independent to our primary contribution of incorporating SVGD into ergodic
search. This is a well-known limitation in the literature [26, 31], and
various methods to address this have recently been studied [2, 32, 36]. Our
Stein methodology is already able to generate multiple diverse trajectories
within these limitations, and we plan to address these scaling limitations in
future work.

Best Practises.  We found that the less nonlinear the constraints are the
easier it was to produce ergodic trajectories. This is true in most trajectory
optimization and model-predictive control approaches. However, certain aspects
and choices have more influence as a result of the limitations discussed
earlier. For example, longer time horizons can be optimized and yield non-
myopic search strategies with the appropriate choice of time step $dt$ for the
dynamics. Similarly, one can achieve very poor and myopic search strategies
when the step-size is chosen too small (which is highly dependent on the time-
scale of the dynamics). Prior work on the impact of integration scheme and
step-size has been explored [28] in isolation and should be referred to for
best practises in choosing an appropriate time step and time horizon.

Another consideration is the number of basis functions used for computing the
ergodic spectrum. The more features that the underlying information measure
$\mu$ has, the more beneficial it is to have a high number of basis functions
(see Fig. 8). This can incur additional computational costs, especially in
high-dimensional exploration tasks. Specialized order reduction techniques,
e.g., [32], can be used to effectively reduce the required number of basis
functions and parallelization can further reduce computational overhead. A
related consideration is the step-size of the stein variational gradient. We
found that a step-size $\epsilon\leq 0.5$ was sufficient for most problems and
varied depending on the problem complexity and numerical scale, i.e., the more
nonlinear the constraints and objective the smaller $\epsilon$ has to be to
converge. A line search can be performed to find the optimal step-size for the
specific problem.

##  VII Conclusion

In summary, we present a novel formulation of coverage and exploration as a
variational inference problem on distributions of ergodic trajectories.
Efficient computation of posterior ergodic trajectories was facilitated
through Stein variational inference that provided sufficient parallelization
and approximate inference to make the problem tractable. We found that the
Stein variational gradients were well suited to being formulated with ergodic
metrics as the natural multiscale aspect of ergodicity produces numerically
well-behaved gradients for trajectory optimization and model-based control. As
an outcome, we proposed two algorithms: 1) for directly computing
distributions of ergodic trajectories; and 2) for calculating robust online
model-predictive controller for robotic exploration.

We demonstrated empirical evidence that showed our proposed approach generated
diverse trajectories that can be used for exploration and proved convergence
of our approach to optimal ergodic trajectories. Constraints and multiscale
implementations were demonstrated as an advantage of our approach through
several simulated examples. Furthermore, we show the computational efficiency
of our approach through parallel compute which was further improved by the
spectral composition of computing ergodic trajectories. Physical experiments
on a drone provided additional validation of our approach on real-world
systems for robust and adaptive exploration.

## Acknowledgments

DL was supported by the Hong Kong Innovation and Technology Commission (InnoHK
Project CIMDA). CL, IA are supported by Yale University.

## References

  * Abraham et al. [2020] Ian Abraham, Ahalya Prabhakar, and Todd D Murphey.  Active area coverage from equilibrium.  In _Algorithmic Foundations of Robotics XIII: Proceedings of the 13th Workshop on the Algorithmic Foundations of Robotics 13_ , pages 284–300. Springer, 2020.
  * Abraham et al. [2021] Ian Abraham, Ahalya Prabhakar, and Todd D Murphey.  An ergodic measure for active learning from equilibrium.  _IEEE Transactions on Automation Science and Engineering_ , 18(3):917–931, 2021.
  * Acar et al. [2002] Ercan U Acar, Howie Choset, Alfred A Rizzi, Prasad N Atkar, and Douglas Hull.  Morse decompositions for coverage tasks.  _The international journal of robotics research_ , 21(4):331–344, 2002.
  * Applegate et al. [2011] David L Applegate, Robert E Bixby, Vašek Chvátal, and William J Cook.  The traveling salesman problem.  In _The Traveling Salesman Problem_. Princeton university press, 2011\.
  * Bähnemann et al. [2021] Rik Bähnemann, Nicholas Lawrance, Jen Jen Chung, Michael Pantic, Roland Siegwart, and Juan Nieto.  Revisiting boustrophedon coverage path planning as a generalized traveling salesman problem.  In _Field and Service Robotics: Results of the 12th International Conference_ , pages 277–290. Springer, 2021.
  * Bradbury et al. [2018] James Bradbury, Roy Frostig, Peter Hawkins, Matthew James Johnson, Chris Leary, Dougal Maclaurin, George Necula, Adam Paszke, Jake VanderPlas, Skye Wanderman-Milne, and Qiao Zhang.  JAX: composable transformations of Python+NumPy programs, 2018\.  URL http://github.com/google/jax.
  * Burda et al. [2018] Yuri Burda, Harri Edwards, Deepak Pathak, Amos Storkey, Trevor Darrell, and Alexei A Efros.  Large-scale study of curiosity-driven learning.  In _International Conference on Learning Representations_ , 2018.
  * Butler et al. [1999] Zack J Butler, Alfred A Rizzi, and Ralph L Hollis.  Contact sensor-based coverage of rectilinear environments.  In _Proceedings of the 1999 IEEE international symposium on intelligent control intelligent systems and semiotics (Cat. No. 99CH37014)_ , pages 266–271. IEEE, 1999.
  * Chen et al. [2022] Weizhe Chen, Roni Khardon, and Lantao Liu.  AK: Attentive Kernel for Information Gathering.  In _Proceedings of Robotics: Science and Systems_ , New York City, NY, USA, June 2022.  doi: 10.15607/RSS.2022.XVIII.047.
  * Chirikjian [2011] Gregory S Chirikjian.  _Stochastic models, information theory, and Lie groups, volume 2: Analytic methods and modern applications_ , volume 2.  Springer Science & Business Media, 2011.
  * Choset [2000] Howie Choset.  Coverage of known spaces: The boustrophedon cellular decomposition.  _Autonomous Robots_ , 9:247–253, 2000.
  * Dong et al. [2023] Dayi E Dong, Henry P Berger, and Ian Abraham.  Time Optimal Ergodic Search.  In _Proceedings of Robotics: Science and Systems_ , Daegu, Republic of Korea, July 2023.  doi: 10.15607/RSS.2023.XIX.082.
  * Dressel and Kochenderfer [2018] Louis Dressel and Mykel J Kochenderfer.  On the optimality of ergodic trajectories for information gathering tasks.  In _2018 Annual American Control Conference (ACC)_ , pages 1855–1861. IEEE, 2018.
  * Galceran and Carreras [2013] Enric Galceran and Marc Carreras.  A survey on coverage path planning for robotics.  _Robotics and Autonomous systems_ , 61(12):1258–1276, 2013.
  * Howell et al. [2022] Taylor Howell, Nimrod Gileadi, Saran Tunyasuvunakool, Kevin Zakka, Tom Erez, and Yuval Tassa.  Predictive sampling: Real-time behaviour synthesis with mujoco.  _arXiv preprint arXiv:2212.00541_ , 2022.
  * Korba et al. [2020] Anna Korba, Adil Salim, Michael Arbel, Giulia Luise, and Arthur Gretton.  A Non-Asymptotic Analysis for Stein Variational Gradient Descent.  In _Advances in Neural Information Processing Systems_ , volume 33, pages 4672–4682. Curran Associates, Inc., 2020.
  * Krengel [2011] Ulrich Krengel.  _Ergodic Theorems_.  De Gruyter, 2011.  ISBN 978-3-11-084464-1.  doi: 10.1515/9783110844641.
  * Lambert et al. [2021] Alexander Lambert, Fabio Ramos, Byron Boots, Dieter Fox, and Adam Fishman.  Stein variational model predictive control.  In _Conference on Robot Learning_ , pages 1278–1297. PMLR, 2021.
  * Laporte and Nobert [1983] Gilbert Laporte and Yves Nobert.  Generalized travelling salesman problem through n sets of nodes: an integer programming approach.  _INFOR: Information Systems and Operational Research_ , 21(1):61–75, 1983.
  * Le et al. [2023] An Le, Georgia Chalvatzaki, Armin Biess, and Jan Peters.  Accelerating motion planning via optimal transport.  In _NeurIPS 2023 Workshop Optimal Transport and Machine Learning_ , 2023.
  * Liu [2017] Qiang Liu.  Stein Variational Gradient Descent as Gradient Flow.  In _Advances in Neural Information Processing Systems_ , volume 30. Curran Associates, Inc., 2017.
  * Liu and Wang [2016] Qiang Liu and Dilin Wang.  Stein variational gradient descent: A general purpose bayesian inference algorithm.  _Advances in neural information processing systems_ , 29, 2016.
  * Mathew and Mezić [2011] George Mathew and Igor Mezić.  Metrics for ergodicity and design of ergodic dynamics for multi-agent systems.  _Physica D: Nonlinear Phenomena_ , 240(4-5):432–442, 2011.
  * Mazzaglia et al. [2022] Pietro Mazzaglia, Ozan Catal, Tim Verbelen, and Bart Dhoedt.  Curiosity-driven exploration via latent bayesian surprise.  In _Proceedings of the AAAI Conference on Artificial Intelligence_ , volume 36, pages 7752–7760, 2022.
  * Miller and Murphey [2013] Lauren M Miller and Todd D Murphey.  Trajectory optimization for continuous ergodic exploration.  In _2013 American Control Conference_ , pages 4196–4201. IEEE, 2013\.
  * Miller et al. [2015] Lauren M Miller, Yonatan Silverman, Malcolm A MacIver, and Todd D Murphey.  Ergodic exploration of distributed information.  _IEEE Transactions on Robotics_ , 32(1):36–52, 2015.
  * Pathak et al. [2017] Deepak Pathak, Pulkit Agrawal, Alexei A Efros, and Trevor Darrell.  Curiosity-driven exploration by self-supervised prediction.  In _International conference on machine learning_ , pages 2778–2787. PMLR, 2017.
  * Prabhakar et al. [2015] Ahalya Prabhakar, Kathrin Flaßkamp, and Todd D Murphey.  Symplectic integration for optimal ergodic control.  In _2015 54th IEEE Conference on Decision and Control (CDC)_ , pages 2594–2600. IEEE, 2015.
  * Preiss* et al. [2017] James A. Preiss*, Wolfgang Hönig*, Gaurav S. Sukhatme, and Nora Ayanian.  Crazyswarm: A large nano-quadcopter swarm.  In _IEEE International Conference on Robotics and Automation (ICRA)_ , pages 3299–3304. IEEE, 2017.  doi: 10.1109/ICRA.2017.7989376.  URL https://doi.org/10.1109/ICRA.2017.7989376.  Software available at https://github.com/USC-ACTLab/crazyswarm.
  * Salim et al. [2022] Adil Salim, Lukang Sun, and Peter Richtarik.  A convergence theory for svgd in the population limit under talagrand’s inequality t1.  In _International Conference on Machine Learning_ , pages 19139–19152. PMLR, 2022.
  * Scott et al. [2009] Sherry E Scott, Thomas C Redd, Leonid Kuznetsov, Igor Mezić, and Christopher KRT Jones.  Capturing deviation from ergodicity at different scales.  _Physica D: Nonlinear Phenomena_ , 238(16):1668–1679, 2009.
  * Shetty et al. [2021] Suhan Shetty, João Silvério, and Sylvain Calinon.  Ergodic exploration using tensor train: Applications in insertion tasks.  _IEEE Transactions on Robotics_ , 38(2):906–921, 2021.
  * Shi and Mackey [2022] Jiaxin Shi and Lester Mackey.  A Finite-Particle Convergence Rate for Stein Variational Gradient Descent.  In _OPT 2022: Optimization for Machine Learning (NeurIPS 2022 Workshop)_ , 2022.
  * Silverman et al. [2013] Yonatan Silverman, Lauren M Miller, Malcolm A MacIver, and Todd D Murphey.  Optimal planning for information acquisition.  In _2013 IEEE/RSJ International Conference on Intelligent Robots and Systems_ , pages 5974–5980. IEEE, 2013.
  * Sun et al. [2021] Muchen Sun, Francesca Baldini, Peter Trautman, and Todd Murphey.  Move Beyond Trajectories: Distribution Space Coupling for Crowd Navigation.  In _Proceedings of Robotics: Science and Systems_ , Virtual, July 2021\.  doi: 10.15607/RSS.2021.XVII.053.
  * Sun et al. [2024] Muchen Sun, Ayush Gaggar, Peter Trautman, and Todd Murphey.  Fast Ergodic Search with Kernel Functions.  _arXiv preprint arXiv.2403.01536_ , 2024.
  * Sung et al. [2023] Yoonchang Sung, Zhiang Chen, Jnaneshwar Das, Pratap Tokekar, et al.  A survey of decision-theoretic approaches for robotic environmental monitoring.  _Foundations and Trends in Robotics_ , 11(4):225–315, 2023.
  * Williams et al. [2016] Grady Williams, Paul Drews, Brian Goldfain, James M Rehg, and Evangelos A Theodorou.  Aggressive driving with model predictive path integral control.  In _2016 IEEE International Conference on Robotics and Automation (ICRA)_ , pages 1433–1440. IEEE, 2016.
  * Williams et al. [2017] Grady Williams, Andrew Aldrich, and Evangelos A Theodorou.  Model predictive path integral control: From theory to parallel computation.  _Journal of Guidance, Control, and Dynamics_ , 40(2):344–357, 2017.
  * Zhuo et al. [2018] Jingwei Zhuo, Chang Liu, Jiaxin Shi, Jun Zhu, Ning Chen, and Bo Zhang.  Message passing stein variational gradient descent.  In Jennifer Dy and Andreas Krause, editors, _Proceedings of the 35th International Conference on Machine Learning_ , volume 80 of _Proceedings of Machine Learning Research_ , pages 6018–6027. PMLR, 10–15 Jul 2018.

##  Appendix A Proofs

###  A-A Spectral Ergodic Metric

In ergodic theory, ergodicity is typically defined for an entire dynamical
systems on a probability space rather than pathwise, which is the approach
required here. Indeed, suppose $(\mathcal{X},\mathcal{F},\mu)$ is a
probability space and $f_{t}:\mathcal{X}\to\mathcal{X}$ is an ergodic flow.
Then Birkhoff’s Ergodic Theorem in continuous time (see [17, Section 1.2.2])
states that for any $\phi\in\mathcal{L}^{1}(\mu)$, time averages of $\phi$
converge to space averages of $\phi$ for $\mu$-almost every initial
$x_{0}\in\mathcal{X}$; in other words,

$\displaystyle\lim_{T\to\infty}\frac{1}{T}\int_{0}^{T}\phi(f_{t}(x_{0}))dt=\int_{\mathcal{X}}\phi
d\mu.$ (26)

However, for an individual path $x(t)=f_{t}(x_{0})$, there is no guarantee
this holds. In fact, the left hand side may not even be well-defined since
$\phi\in\mathcal{L}^{1}(\mu)$ is only defined $\mu$-almost everywhere and the
time averaged measure $\mathcal{Q}_{T}$ from Eq. (3) may not be absolutely
continuous with respect to $\mu$. Thus, we use an alternative definition,
replacing the $\mathcal{L}^{1}(\mu)$ functions with continuous functions
$\mathcal{C}(\mathcal{X})$ for the pathwise Definition 1.

This pathwise definition generalizes the spectral cost function, which has
previously been used as a measure of pathwise ergodicity [23, 12].

######  Theorem 2.

Let $\mathcal{W}=[0,L_{0}]\times\ldots\times[0,L_{v-1}]\subset\mathbb{R}^{v}$.
The spectral cost function is an ergodic cost function.

###### Proof.

Here, we view $\mathcal{W}$ with periodic boundaries, and is thus a
$v$-dimensional torus (ie. $v$ copies of the circle $S^{1}$). Suppose
$\mathcal{E}_{\mu}$ is the spectral cost function defined in Eq. (8), and let
$x(t):\mathbb{R}^{+}\to\mathcal{X}$ be a trajectory such that
$\mathcal{E}_{\mu}(x|_{[0,T]})\to 0$ as $T\to\infty$. This implies that the
Fourier coefficients converge $\mathcal{Q}_{T}^{k}\to\mu^{k}$ as $T\to\infty$,
and thus the Fourier transforms of $\mathcal{Q}_{T}$ converge to the Fourier
transform of $\mu$. Finally, by Levy’s convergence theorem for the torus,
$\mathcal{Q}_{T}$ converges weakly to $\mu$ as $T\to\infty$. ∎

###  A-B Convergence of SVGD

In this appendix, we show that the general convergence results for SVGD from
[16] hold in the ergodic search setting. We consider discretized ergodic
search on a bounded state space $\mathcal{X}\subset\mathbb{R}^{n}$ and
normalized workspace $\mathcal{W}=[0,1]^{v}$ with $T$ time points. Without
loss of generality we set $\mathcal{X}=[0,1]^{n}$ and thus, we consider SVGD
on the space $\mathcal{X}^{T}=[0,1]^{Tn}$. We fix a function
$g:\mathcal{X}\to\mathcal{X}$. Moreover, we work in the population limit of
infinitely many initial samples from the prior distribution.

Let $\mathcal{E}_{\mu}$ be the spectral ergodic cost function with respect to
a measure $\mu$ on $\mathcal{W}$ and $p$ be the prior distribution on
$\mathcal{X}^{T}$. Then, the target distribution we aim to approximate is the
posterior $\pi\coloneqq p(\mathbf{x}|\mathcal{O})\sim
p(\mathcal{O}|\mathbf{x})p(\mathbf{x})$ which has the form

$\displaystyle\pi\sim\exp\left(-\lambda\mathcal{E}_{\mu}(\mathbf{x})+\log(p(\mathbf{x}))\right).$
(27)

The function

$\displaystyle
V(\mathbf{x})=\lambda\mathcal{E}_{\mu}(\mathbf{x})-\log(p(\mathbf{x}))$ (28)

is called the _potential function_ in the SVGD literature. Furthermore,
suppose $k:\mathcal{X}^{T}\times\mathcal{X}^{T}\to\mathbb{R}$ is the RBF
kernel and $\mathcal{H}$ is its corresponding RKHS.

Convergence of SVGD is framed in terms of the kernel Stein discrepancy (KSD).
In particular, the KSD of a measure $q$ with respect to the target measure
$\pi$ is

$\displaystyle\text{KSD}_{\pi}(q)\coloneqq\|\phi^{*}(q)\|^{2}_{\mathcal{H}^{Tn}},$
(29)

where $\phi^{*}_{q}\in\mathcal{H}^{Tn}$ (note that
$\mathcal{X}^{T}\subset\mathbb{R}^{Tn}$),

$\phi^{*}(q)=\mathbb{E}_{\mathbf{x}\sim
q}[k(\mathbf{x},\cdot)(\nabla_{\mathbf{x}}\log p(\mathbf{x})-\\\
\lambda\nabla_{\mathbf{x}}\mathcal{E}_{\mu}(\mathbf{x}))+\nabla_{\mathbf{x}}k(\mathbf{x},\cdot)].$
(30)

In particular, $\text{KSD}_{\pi}(q)$ is determined by the norm of the gradient
in the SVGD update step (compare with the finite particle gradient
$\phi^{*}_{r}$ in Eq. (16). Here, we define SVGD in the population limit in
the same manner as the finite particle setting. The population gradient at
step $r$ is $\phi^{*}_{r}\coloneqq\phi^{*}(q_{r})$, where the step $r$ measure
$q_{r}$ on $\mathcal{X}^{T}$ is defined as the pushforward of $q_{r-1}$ along
the function $U_{r}:\mathcal{X}^{T}\to\mathcal{X}^{T}$,

$\displaystyle
U_{r}(\mathbf{x})\coloneqq\mathbf{x}+\epsilon\phi^{*}_{r-1}(\mathbf{x})$ (31)

with step size $\epsilon>0$ and initial condition $q_{0}=p$.

Furthermore, the SVGD convergence results from [16] rests on three assumptions
on the kernel $k$, the potential function $V$, and moments of the step $r$
measures $q_{r}$. In particular, there exist constants $B,C,M>0$ such that the
following holds.

  * ($A_{1}$)

$\|k(\mathbf{x},\cdot)\|_{\mathcal{H}},\|\nabla_{\mathbf{x}}k(\mathbf{x},\cdot)\|_{\mathcal{H}^{Tn}}\leq
B$.

  * ($A_{2}$)

The Hessian $H_{V}$ of the potential function $V$ from Eq. (28) is well-
defined as $\|H_{V}\|_{op}\leq M$.

  * ($A_{3}$)

For all $r$, $\text{KSD}_{\pi}(q_{r})<C$.

######  Theorem 1.

(Formal). Let $\mathcal{X}=[0,1]^{n}$, $\pi=p(\mathbf{x}|\mathcal{O})$ be the
target distribution with potential $V$ from Eq. (28), $p$ be smooth prior
distribution on $\mathcal{X}^{T}$, $k$ be the RBF kernel on $\mathcal{X}^{T}$
with RKHS $\mathcal{H}$. There exists a step size $\epsilon<S$, where $S$ is a
constant which depends on $B,C,M$ such that

$\displaystyle\frac{1}{r}\sum_{i=1}^{r}\text{KSD}_{\pi}(q_{i})\leq\frac{\text{KL}(p|\pi)}{c_{\epsilon}r},$
(32)

where $c_{\epsilon}$ is a constant which depends on $\epsilon,M,B$.

###### Proof.

This result is a special case of [16, Corollary 6], and in order to prove this
result, we must show that assumptions $(A_{1}),(A_{2}),(A_{3})$ above hold.
First, $(A_{1})$ holds since the RBF kernel is differentiable, and
$\mathcal{X}^{T}$ is a bounded domain. Next, we note that for a discrete path
$\mathbf{x}=[x_{0},\ldots,x_{T-1}]$, the spectral ergodic cost fucntion has
the form

$\displaystyle\mathcal{E}_{\mu}(\mathbf{x})=\sum_{k\in\mathcal{K}^{v}}\Lambda_{k}\left(\frac{1}{T}\sum_{t=0}^{T-1}F_{k}(g(x_{t}))-\int_{\mathcal{W}}F_{k}(w)d\mu(w)\right)^{2},$
(33)

which is smooth since $F_{k}$ from Eq. (7) is smooth. Because the prior $p$ is
also smooth, the potential $V$ is smooth, and the Hessian is well-defined.
Furthermore, the Hessian is bounded since $\mathcal{X}^{T}$ is a bounded
domain, so $(A_{2})$ is satisfied. Finally, since $(A_{1})$ and $(A_{2})$ is
satisfied, it suffices to show that

$\displaystyle\sup_{r}\int_{\mathcal{X}^{T}}\|\mathbf{x}\|\,dq_{r}(\mathbf{x})<\infty,$
(34)

to show $(A_{3})$, from the discussion in [16, Section 5]. However, this is
satisfied since $q_{r}$ is a probability measure and $\mathcal{W}^{T}$ is a
bounded domain, so $(A_{3})$ is satisfied. ∎

##  Appendix B Implementation Detail & Additional Results

Here, we provide additional implementation details for the experimental
results in Section §V. For all methods, the Stein variational gradient step
size is given by $\epsilon=0.5$ unless otherwise specified. In addition, the
RBF kernel is used in all examples unless otherwise specified. Convergence
condition is the same for all examples $\|\phi_{r}\|\leq 10^{-3}$ unless
specified.

Convergence and Diversity Results. For all examples we assume a bounded 2D
domain of size $\mathcal{W}=[0,1]\times[0,1]$. All trajectories are of length
$T=100$ with $n=2$ and an assumed $dt=0.1s$. The solvers are all optimized
until the gradient direction $\|\phi_{r}\|\leq 10^{-3}$. The cost function
$\mathcal{L}_{\mu}$ is given as

$\displaystyle\mathcal{L}_{\mu}(\mathbf{x})=\mathcal{E}_{\mu}(\mathbf{x})+0.1c_{\mathcal{W}}(\mathbf{x})+\sum_{t}15\|x_{t+1}-x_{t}\|_{2}^{2}$
$\displaystyle+0.1\|x_{0}-x_{\text{init}}\|_{2}^{2}+0.1\|x_{T}-x_{\text{final}}\|_{2}^{2}$

where $c_{\mathcal{W}}$ is a cost on staying within the boundaries of
$\mathcal{W}$ ($0$ within the boundaries and proportional square of the
distance outside the boundaries), and $x_{\text{init}}$ and $x_{\text{final}}$
are the initial and final state conditions. All initial samples
$\\{\mathbf{x}^{i}\\}_{i=1}^{N}$ are drawn from
$p(\mathbf{x})=\mathcal{N}(\hat{\mathbf{x}},\sigma^{2})$ where
$\sigma^{2}=0.01$, and
$\hat{\mathbf{x}}=\text{interp}(x_{\text{init}},x_{\text{final}})$. A
temperature value $\lambda=10$ was used for all experiments. The maximum
number of basis functions used was $k_{\text{max}}=8$ for all dimensions.

Multiscale Constrained Exploration. This experiment was done using Alg. 1
subject to a uniform $\mu$ in 2D. The function $g(x)$ would map the trajectory
within the $100m\times 100m$ domain onto a $[0,1]\times[0,1]$ domain for
improved numerical conditioning. Note that this did not change the underlying
problem as the ergodic metric is defined over the Fourier spectral domain
which can be defined on any periodic domain. The trajectories are of length
$T=100$ with a time step of $dt=1s$ and a dimension of $n=2$. A maximum of
$k_{\text{max}}=8$ basis functions were used. The cost function is given as

$\displaystyle\mathcal{L}_{\mu}(\mathbf{x})=\mathcal{E}_{\mu}(\mathbf{x})+0.1c_{\mathcal{W}}(\mathbf{x})+\sum_{t}15\|x_{t+1}-x_{t}\|_{2}^{2}$
$\displaystyle+0.1\|x_{0}-x_{\text{init}}\|_{2}^{2}+0.1\|x_{T}-x_{\text{final}}\|_{2}^{2}+0.01c_{\text{obs}}(\mathbf{x})$

where $c_{\text{obs}}(\mathbf{x})$ is a obstacle constraint given by
$\text{max}(0,\|x-x_{c}\|_{2}-r)$ where $r$ is the radius of the obstacle. The
initial and final positions of the trajectory are specified by the boundaries
of the domain.

3D Constrained Exploration with Nonlinear Dynamics. In this example the state
of the aircraft dynamics are given as
$x=[p_{x},p_{y},p_{z},\psi,\phi,v]^{\top}$ where $p_{i}$ is the position in
3D, $\phi,\phi$ are the roll and pitch, and $v$ is the forward velocity of the
aircraft. The control vector is given by $u=[u_{1},u_{2},u_{3}]^{\top}$ where
the dynamics is defined as

$\frac{d}{dt}\begin{bmatrix}p_{x}\\\ p_{y}\\\ p_{z}\\\ \psi\\\ \phi\\\
v\end{bmatrix}=\begin{bmatrix}v\cos(\phi)\cos(\psi)\\\
v\cos(\phi)\sin(\psi)\\\ v\sin(\phi)\\\ u_{1}\\\ u_{2}\\\ u_{3}\end{bmatrix}.$
(35)

The distribution $\mu$ is given by a sum of two Gaussians with equal weights.
The space is defined by $\mathcal{W}=[0,3]\times[0,3]\times[0,3]$ and $g(x)$
takes the states and returns the position vector of the aircraft. The obstacle
is represented by a distance penalty function
$\text{max}(0,\|x-x_{c}\|_{2}-r)$ where $r$ is the radius of the obstacle.
Trajectories are of length $T=150$. Initial controls are sampled from
$p(\mathbf{u})=\mathcal{N}(0,\sigma^{2})$ where $\sigma^{2}=0.1$. The cost
function is given as

$\displaystyle\mathcal{L}_{\mu}(\mathbf{u})=\mathcal{E}_{\mu}(\mathbf{u})+c_{\mathcal{W}}(\mathbf{x})+\sum_{t}\|p_{z,t}\|_{2}^{2}+100c_{\text{obs}}(\mathbf{x})$

and $\mathbf{x}$ is computed using $x_{t+1}=F(x_{t},u_{t})$ from the initial
condition. The temperature $\lambda$ has value $20$ and $N=5$ samples are
computed. The step size of the Stein variational algorithm is given as
$\epsilon=0.1$.

Stein Variational Model-Predictive Control. In this example, we consider
coverage over a quad-modal distribution $\mu$ which is composed of $4$
Gaussians with equal weights. The domain is specified over
$\mathcal{W}=[0,1]\times[0,1]$ and the dynamics are given as a single
integrator $x_{t+1}=x_{t}+dtu_{t}$. The planning horizon is given as $T=20$
with $N=20$ trajectory samples. We run each planning loop for a total of $100$
iterations or unless tolerance was achieved of $\gamma=10^{-2}$. A total of
$k_{\text{max}}=10$ basis functions are used for each dimension. The $10$
obstacles are randomly placed using a uniform distribution on $\mathcal{W}$
and move according to random velocity directions sampled from a normal
distribution with standard deviation of $\sigma=0.01$ at the same control rate
as the robot ($dt=0.1$). The cost function is given as

$\displaystyle\mathcal{L}_{\mu}(\mathbf{u})$
$\displaystyle=\mathcal{E}_{\mu}(\mathbf{u})+c_{\mathcal{W}}(\mathbf{x})+0.01\|\mathbf{u}\|_{2}^{2}$
$\displaystyle+\sum_{t}0.001\|x_{t+1}-x_{t}\|_{2}^{2}+100c_{\text{obs}}(\mathbf{x})$

with $\lambda=10$. The initial control prior distribution is given as
$p(\mathbf{u})=\mathcal{N}(0,\sigma^{2}=0.01)$.

Sensor-based Exploration. In this example, we consider coverage over the
entropy of a Gaussian process using the AK attentive kernel as $\mu$. The
domain is specified over $\mathcal{W}=[-10,10]\times[-10,10]$ and the sensor
planning dynamics are given as a single integrator $x_{t+1}=x_{t}+dtu_{t}$.
The trajectory is tracked using a Dubins car dynamics model with a feedback
controller rate of $10$ Hz as done in [9]. The planning horizon is given as
$T=20$ with $N=4$ trajectory samples. We run each planning loop for a total of
$100$ iterations or unless tolerance was achieved of $\gamma=10^{-2}$. A total
of $k_{\text{max}}=20$ basis functions are used for each dimension. Samples
are committed using a sonar sensor model at each $1$ Hz rate location. The
cost function is given as

$\displaystyle\mathcal{L}_{\mu}(\mathbf{u})$
$\displaystyle=\mathcal{E}_{\mu}(\mathbf{u})+0.1c_{\mathcal{W}}(\mathbf{x})+0.001\|\mathbf{u}\|_{2}^{2}$

with $\lambda=100$ and $p(\mathbf{u})=\mathcal{N}(0,\sigma^{2}=0.01)$.

3D Coverage using the Crazyflie Drone. This example was done on a 3D domain
$\mathcal{W}=[0,3]\times[0,3]\times[0.5,1.5]$ with $\mu$ as a uniform
distribution. Trajectories were calculated using Alg. 1 with cost functional
similar to the multiscale forest example

$\displaystyle\mathcal{L}_{\mu}(\mathbf{x})=\mathcal{E}_{\mu}(\mathbf{x})+0.1c_{\mathcal{W}}(\mathbf{x})+\sum_{t}15\|x_{t+1}-x_{t}\|_{2}^{2}$
$\displaystyle+0.1\|x_{0}-x_{\text{init}}\|_{2}^{2}+0.1\|x_{T}-x_{\text{final}}\|_{2}^{2}+0.01c_{\text{obs}}(\mathbf{x}).$

The trajectory length was given by $T=50$ with $dt=0.1s$. The total number of
basis functions was given as $k_{\text{max}}=8$ for each dimension (totalling
$512$ basis functions).

Once trajectories were computed, tracking was done through the Crazyflie 2.1
API which sends way-point commands as a specified interval (equal to the
trajectory time step). Drone position tracking was done using two Lighthouse
VR trackers distributed across the domain. The obstacles are defined using an
obstacle penalty function given by $\text{max}(0,\|x-x_{c}\|_{2}-r)$ where $r$
is the radius of the obstacle. The planner was given the positions of the
obstacles at planning time. Trajectories are optimized until convergence. Once
trajectories are solved, each is send to the drone to execute. The one that
satisfies the heuristic in Eq. (20) signals the drone to turn a LED green to
indicate the optimal solution.