# Trajectory Optimization for Adaptive
Informative Path Planning with Multimodal Sensing

Joshua Ott1, Edward Balaban2, and Mykel J. Kochenderfer1 1Department of
Aeronautics & Astronautics, Stanford University
({joshuaott, mykel}@stanford.edu).2NASA Ames Research Center
(edward.balaban@nasa.gov).

###### Abstract

We consider the problem of an autonomous agent equipped with multiple sensors,
each with different sensing precision and energy costs. The agent’s goal is to
explore the environment and gather information subject to its resource
constraints in unknown, partially observable environments. The challenge lies
in reasoning about the effects of sensing and movement while respecting the
agent’s resource and dynamic constraints. We formulate the problem as a
trajectory optimization problem and solve it using a projection-based
trajectory optimization approach where the objective is to reduce the variance
of the Gaussian process world belief. Our approach outperforms previous
approaches in long horizon trajectories by achieving an overall variance
reduction of up to 85% and reducing the root-mean square error in the
environment belief by 50%. This approach was developed in support of rover
path planning for the NASA VIPER Mission.

##  I Introduction

Adaptive informative path planning with multimodal sensing (AIPPMS) considers
the problem of an agent tasked with exploring an unknown environment with
finite energy resources [1]. The agent is equipped with multiple sensors each
with different sensing cost and sensing precision. The agent must not only
reason about where to explore but also what measurements to take at a specific
location. In addition, the agent must maintain a belief about the true state
of the environment since it can only partially observe the underlying state
through noisy observations. To solve an AIPPMS problem, the agent must develop
a flexible decision making strategy that navigates towards the desired
destination, balancing the need for gathering information with utilizing the
gathered information, and taking into account resource limitations along the
way.

Many real-world situations involve agents equipped with multiple sensors which
cannot operate concurrently due to limitations related to energy, sensor
interference, or motion constraints. Consider an agent with multimodal sensing
capabilities. Here, multimodal refers to the presence of numerous sensors,
each with differing degrees of precision and cost; for instance, a rover
equipped with an array of sensors like CCD cameras, radar, mass spectrometers,
and drills. Certain sensors like the cameras consume less energy yet yield
less dependable measurements since they can’t completely examine the
subsurface environment. In contrast, other sensors, such as the drill, consume
significantly more energy but provide almost perfect measurements of the
subsurface environment. This dichotomy between sensor efficiency and precision
is also observable in numerous other domains, including ocean monitoring,
mineral prospecting, aerial surveillance, and wildfire mapping [2, 3, 4, 5].

Figure 1: High-level overview of the projection-based trajectory optimization
approach. The trajectory and sample locations are initialized. A descent
direction is then computed to modify the trajectory in the direction that
maximally reduces the objective function. This trajectory may violate the
dynamics of the agent so the candidate trajectory is then projected back into
feasibility. This process is repeated until convergence.

The informative path planning (IPP) problem is NP-hard [6] and involves an
agent planning a path without accounting for noisy observations of the
environment. The adaptive problem extends the IPP problem by seeking a policy
that changes in response to received observations [7, 8, 9, 10, 11]. The
AIPPMS problem further extends the adaptive problem by reasoning over multiple
sensing modalities with different sensing costs corresponding to sensor
precision and movement costs [1].

A variety of approaches have been proposed for both the adaptive informative
path planning (AIPP) and non-adaptive IPP problems [12, 13, 14, 15, 16, 17].
The main focus in the majority of this previous work is on the case where the
agent is equipped with a single sensor. In this work, we are interested in the
case where the agent can choose between multiple sensing modalities. This
extension adds additional complexity by requiring that the agent reasons about
the cost-benefit trade-off associated with the different sensing modalities.
Choudhury et al. and Ott et al. both considered rollout-based partially
observable Markov decision process (POMDP) solvers [18, 19, 20, 21, 22].
Rollout-based POMDP approaches suffer from near-sightedness induced by the
rollout depth. In longer trajectories, the agent may use more valuable
resources at the beginning of the trajectory due to the limited rollout depth.
As a result, valuable samples may be concentrated at the start of the
trajectory instead of being distributed throughout the environment. To solve
this problem, we propose optimizing the whole trajectory using a Gaussian
process belief objective. Our holistic approach is able to better allocate
valuable sensing resources across the entire trajectory and outperforms
rollout-based solvers in trajectories with greater energy resources.

The key contributions of this work are:

  1. 1.

The formulation of the AIPPMS problem as a trajectory optimization problem
where the objective is based on the Gaussian process world belief.

  2. 2.

The introduction of a projection-based trajectory optimization method that
uses Gaussian process information objectives.

  3. 3.

The extensive evaluation of our method in simulation, along with directly
comparing to previous AIPPMS methods from the literature.

  4. 4.

The release of our implementation as an open-source software package for use
and further development by the
community.222https://github.com/josh0tt/TO˙AIPPMS

##  II Related Work

The AIPP problem has been widely studied and a common approach is to represent
the world belief as a Gaussian process. Marchant et al. formulate the AIPP
problem as a POMDP and solve it using sequential Bayesian optimization through
Monte Carlo tree search upper confidence bound for trees (MCTS-UCT) [3]. This
work has been extended to modify the reward function for achieving monitoring
behavior that exploits areas with high gradients [23], and for additional
reasoning over continuous action spaces through Bayesian optimization [16].
Fernández et al. use partially observable Monte Carlo planning (POMCP) with
Gaussian process beliefs for estimating quantiles of the underlying world
state [24]. Rückin et al. present a hybrid approach that combines tree search
with an offline-learned neural network to predict informative sensing actions
[17].

The AIPPMS problem is less widely studied. Choudhury et al. introduced the
AIPPMS problem where the agent must make decisions about where to visit and
what sensors to use [1]. Greater energy cost is incurred for using more
precise sensors. Choudhury et al. formulate the AIPPMS problem as a POMDP and
use POMCP to produce a solution. Ott et al. extend this work by formulating
the AIPPMS problem as a belief MDP and use belief-dependent rewards to
penalize uncertainty in the belief through sequential Bayesian optimization
using Monte Carlo tree search with double progressive widening (MCTS-DPW)
[20].

While sequential rollout methods have seen great success in application to the
AIPPMS problem, they can also suffer from myopia resulting from the limited
rollout depth. In trajectories with longer horizons, this myopia can lead to
valuable sensing measurements being frontloaded earlier on in the trajectory
which can be detrimental to performance depending on the objective function
being used. In applications such as planetary exploration, frontloading can
weaken the performance in characterizing the presence of quantities across the
subsurface exploration region [4, 5].

Projection-based trajectory optimization was originally proposed by Hauser as
an iterative first order optimization method which allowed the descent
direction at each iteration to be calculated using linear quadratic regulator
techniques [25]. Miller and Murphey used projection-based trajectory
optimization with an ergodic objective based on an underlying expected
information density distribution [26]. An ergodic trajectory is one that
spends time in a region proportional to the expected information density
distribution [27]. Dressel and Kochenderfer extended this work and applied it
to signal localization on drones [28, 29]. We directly compare our method with
that of Ott et al. and Dressel and Kochenderfer [20, 28].

##  III Preliminaries

###  III-A Adaptive Informative Path Planning with Multimodal Sensing

The objective in the AIPPMS problem is to determine the optimal sequence of
actions, denoted as $\psi^{*}=(a_{1},...,a_{N})$, where each action $a_{i}$ is
a member of a set of potential actions $\mathcal{A}$. Crucially, these actions
comprise a hybrid set of both discrete and continuous elements. Discrete
elements include choices like the type of sensors used, while continuous
elements represent actions such as acceleration commands or future agent
positions. The action sequence $\psi^{*}$ is chosen to maximize an
information-theoretic criterion $I(\cdot)$. This can be represented as:

$\psi^{*}=\operatorname*{argmax}_{\psi\in\Psi}I(\psi),\hskip 5.0pt\text{s.t.
}C(\psi)\leq B,$ (1)

where $\Psi$ denotes the set of all possible hybrid action sequences of
discrete and continuous actions, the cost function $C:\Psi\to\mathbb{R}$ maps
these hybrid action sequences to execution costs, $B\in\mathbb{R}^{+}$ is the
agent’s budget limit, such as time or energy, and $I:\Psi\to\mathbb{R}$ is the
information criterion, computed from the new sensor measurements obtained by
executing the actions $\psi$ [13, 1, 17, 20].

###  III-B Gaussian Processes

We use a Gaussian process to represent the belief over the true environment
state. A Gaussian process is a distribution over functions that can be used to
predict an underlying function $f$ given some previously observed noisy
measurements at location $x$. That is, $y=f(x)+\epsilon$ where $f$ is
deterministic but $\epsilon$ is zero-mean Gaussian noise,
$\epsilon\sim\mathcal{N}(0,\nu)$. The distribution of functions conditioned on
the previously observed measurements $\hat{y}\mid
y,\nu\sim\mathcal{N}(\mu^{\ast},\Sigma^{\ast})$ is given by:

$\displaystyle\mu^{\ast}$ $\displaystyle=m(X^{\ast})+K(X^{\ast},X)(K(X,X)+\nu
I)^{-1}(y-m(X))$ (2) $\displaystyle\Sigma^{\ast}$
$\displaystyle=K(X^{\ast},X^{\ast})-K(X^{\ast},X)(K(X,X)+\nu
I)^{-1}K(X,X^{\ast})$ (3)

where $X$ is the set of measured locations, $X^{\ast}$ is the set of locations
to predict the values $\hat{y}$, $m(X)$ is the mean function and
$K(X,X^{\prime})$ is the covariance matrix constructed with the kernel
$k(x,x^{\prime})$ [22].

###  III-C Variance Reduction

Our approach uses the reduction in variance of the Gaussian process as the
exploration objective. Intuitively, this means we want our prediction error
about the unseen locations in the environment to be as small as possible.

We denote $\mathcal{X}_{\mathcal{V}}$ as the set of random variables at all
locations in the environment and $M_{t}$ as the locations that have been
observed by the agent at time $t$ as well as the corresponding measurement
received and sensor type that was used at each of the observed locations. That
is, $M_{t}=\left[x_{0:t},y_{0:t},\nu_{0:t}\right]$ where
$x_{i}\in\mathbb{R}^{n}$ is a point location in the environment,
$y_{i}\in\mathbb{R}$ is the measurement value received and
$\nu_{i}\in\mathbb{R}$ corresponds to the measurement noise in Eq. (2). The
measurement type is characterized by its level of noise with more precise
sensors incurring greater costs. We denote the set of random variables that
have been observed by the agent at time $t$ as $\mathcal{X}_{M_{t}}$ which is
a subset of all of the random variables at all locations in the environment
$\mathcal{X}_{M_{t}}\subseteq\mathcal{X}_{\mathcal{V}}$. The variance
reduction of the Gaussian process between the point locations in $M_{t}$ and
the rest of the world $\mathcal{X}_{\mathcal{V}}$ given previous measurement
locations and sensor types $M_{t-1}$ is

$I(\mathcal{X}_{\mathcal{V}\mid M_{t}};\mathcal{X}_{\mathcal{V}\mid
M_{t-1}})=\text{Tr}\left(\boldsymbol{\Sigma^{\ast}_{M_{t-1}}}\right)-\text{Tr}\left(\boldsymbol{\Sigma^{\ast}_{M_{t}}}\right)$
(4)

where $\boldsymbol{\Sigma^{\ast}_{M_{t}}}$ is the covariance of all points in
the environment after measuring at $M_{t}$ and the trace of the covariance
matrix corresponds to the total variance in the world belief-state.

##  IV Methods

###  IV-A Projection-based Trajectory Optimization

The discrete dynamics of a general nonlinear agent can be expressed as
$x_{t+1}=x_{t}+h(x_{t},u_{t})\Delta t$ where $x_{t}\in\mathbb{R}^{n}$
represents the state, $u_{t}\in\mathbb{R}^{m}$ the control inputs, and $h$ is
the nonlinear dynamics function of the agent. In this work, we focus on AIPPMS
problems and use relatively simple dynamics models to allow for more even
comparison with other methods; however, our method can be extended to more
complex dynamics following the integration methods discussed by Dressel and
Kochenderfer [30]. The locations that have been observed by the agent at time
$T$ where $T$ is the horizon of the trajectory are given by
$M_{T}=\left[x_{0:T},y_{0:T},\nu_{0:T}\right]$ and $M_{0}$ is the empty set.
The objective function $J(\cdot)$ is defined as a function of the Gaussian
process variance, control effort, and a goal and boundary penalty:

$\displaystyle
J(x,u)=-q\underbrace{I(\mathcal{X}_{M_{T}};\mathcal{X}_{\mathcal{V}\mid
M_{0}})}_{\text{variance reduction}}+\underbrace{\lVert
x_{T}-x_{f}\rVert^{2}_{Q_{f}}}_{\text{goal penalty}}$ (5) $\displaystyle\hskip
50.00008pt+\underbrace{\sum_{0}^{T}\frac{1}{2}u^{T}_{t}R_{t}u_{t}}_{\text{control
effort}}+\underbrace{J_{b}(x_{0:T})}_{\text{boundary penalty}}$

where $q\in\mathbb{R}^{+}$, $R_{t}\in\mathbb{R}^{m\times m}$, and
$Q_{f}\in\mathbb{R}^{n\times n}$ are arbitrary design parameters that affect
the relative importance of minimizing the negative Gaussian process variance,
control effort, and distance from the goal location respectively. Note that we
include a negative sign on the variance reduction since we seek to minimize
$J(x_{0:T},u_{0:T})$ and therefore maximize the variance reduction. The goal
penalty $\lVert x_{T}-x_{f}\rVert^{2}_{Q_{f}}$ is computed using the
Mahalanobis norm with respect to the positive definite matrix $Q_{f}$.
$J_{b}(x_{0:T})$ is the boundary penalty that penalizes the trajectory for
leaving the optimization region as defined by Dressel and Kochenderfer [30].
The goal is to solve for the feasible continuous-time trajectory that
minimizes the objective function, i.e.

$\displaystyle\underset{x_{0:T},u_{0:T}}{\text{minimize}}$ $\displaystyle
J(x_{0:T},u_{0:T})$ (6) subject to $\displaystyle
x_{t+1}=x_{t}+h(x_{t},u_{t})\Delta t,\hskip 10.00002ptx_{0}=x_{\text{init}}.$

The optimization of Equation 6 can be carried out iteratively in the following
steps [30].

####  IV-A1 Linearize

To efficiently compute the descent direction, we can linearize the dynamics
and control at each state and control input in our current trajectory. That is
$A_{t}=\nabla_{x}h(x_{t},u_{t})$ and $B_{t}=\nabla_{u}h(x_{t},u_{t})$. A
perturbation of the discrete trajectory is then given by $\delta
x_{t+1}=\delta x_{t}+(A_{t}\delta x_{t}+B_{t}\delta u_{t})\Delta t$ where the
perturbations $\delta x_{t}$ and $\delta u_{t}$ are small changes in the state
and control at the time $t$. By introducing $\tilde{A}_{t}=I+A_{t}\Delta t$
and $\tilde{B}_{t}=B_{t}\Delta t$ we have $\delta x_{t+1}=\tilde{A}_{t}\delta
x_{t}+\tilde{B}_{t}\delta u_{t}$.

Figure 2: Examples of trajectories produced from each of the six methods
considered in this work. The true map is shown on the left. The top row shows
the posterior mean and the bottom row shows the posterior variance of the
Gaussian process world belief after the agent has reached the goal state. The
agent starts in the bottom left corner. The pink line indicates the path the
agent took and the green dots represent drill sites. These trajectories were
generated with $\sigma_{s}=1.0$ and $b=60.0$.

####  IV-A2 Descent Direction

The trajectory and control descent directions are given by $z_{0:T}$ and
$v_{0:T}$ respectively. To compute these descent directions, we can rewrite
Equation 6 as shown by Miller and Murphey [26] as

$\displaystyle\underset{z_{0:T},v_{0:T}}{\text{minimize}}\sum_{0}^{T}\Bigl{(}a^{T}_{t}z_{t}+b^{T}_{t}v_{t}+\frac{1}{2}z^{T}_{t}Q_{n}z_{t}+\frac{1}{2}v^{T}_{t}R_{n}v_{t}\Bigr{)}$
$\displaystyle\text{subject to}\hskip
10.00002ptz_{t+1}=\tilde{A}_{t}z_{t}+\tilde{B}_{t}v_{t},\hskip
10.00002ptz_{0}=0.$

In the above expression, $a_{t}$ and $b_{t}$ are the gradients of the
objective $J$ with respect to the state and control. That is
$a_{t}=\nabla_{x}J(x_{t},u_{t})$ and $b_{t}=\nabla_{u}J(x_{t},u_{t})$. $Q_{n}$
is an arbitrary positive semi-definite matrices and $R_{n}$ is positive
definite [25]. The initial state descent direction $z_{0}$ is constrained to
zero because the agent’s starting state is fixed and cannot be changed. The
above expression is a linear quadratic optimal control problem that can be
solved using standard discrete algebraic Riccati equations [25, 31].
Intuitively, $z_{t}$ and $v_{t}$ tell us how we should modify $x_{t}$ and
$u_{t}$.

####  IV-A3 Candidate Trajectory & Projection

Applying the descent direction with step size $\gamma$ will result in a
candidate trajectory $\tilde{\alpha}_{0:T}$ and candidate control sequence
$\tilde{\mu}_{0:T}$ that may not be dynamically feasible. That is,
$\tilde{\alpha}_{0:T}=x_{0:T}+\gamma z_{0:T}$ and
$\tilde{\mu}_{0:T}=u_{0:T}+\gamma v_{0:T}$. Therefore, we must project the
candidate $(\tilde{\alpha}_{0:T},\tilde{\mu}_{0:T})$ into a feasible space
with the projection operator $\mathscr{P}$:

$\mathscr{P}\left(\tilde{\alpha}_{0:T},\tilde{\mu}_{0:T},x_{0}\right):\left\\{\begin{array}[]{l}\tilde{u}_{t}=\tilde{\mu}_{t}+\mathscr{K}_{t}\left(\tilde{\alpha}_{t}-\tilde{x}_{t}\right)\\\
\tilde{x}_{t+1}=\tilde{x}_{t}+h\left(\tilde{x}_{t},\tilde{u}_{t}\right)\Delta
t\end{array}\right.$ (7)

where $\mathscr{K}_{t}$ is the Riccati gain computed from the linear quadratic
regulator problem with respect to the reference candidate trajectory [26, 30].
A high-level overview of this approach is shown in Fig. 1. For implementation
details, we refer the reader to our open-source repository.

Previous projection-based trajectory optimization methods have focused solely
on ergodic objectives, while our contribution distinctively expands the scope
to Gaussian process-based objectives and incorporates multimodal sensor
selection [26, 30]. A comparative analysis of these methodologies is presented
in the subsequent section.

###  IV-B Sample Selection

The projection-based trajectory optimization controls how the shape of the
trajectory evolves, but the variance reduction term in Equation 5 is also
dependent on the sample type. Therefore, during the iterative optimization, we
perturb one sensor type $\nu_{i}$ in
$M_{t}=\left[x_{0:t},y_{0:t},\nu_{0:t}\right]$ with probability $p$ where $i$
is chosen from a uniform distribution at each iteration. If the overall
objective decreased, then the perturbation is kept, otherwise, it is
discarded. Note that this random perturbation allows for both continuous and
discrete sensor types since the effect of continuous sensor types can be
analyzed analytically and discrete sensor types can be perturbed randomly by
uniformly sampling an integer from the range of sample types available. Due to
energy constraints, inserting a more precise sensor type will lead to the
trajectory being shortened since more energy resources will be needed for that
specific sensor type. As a result, inserting valuable sensor types will not
always necessarily lead to a decrease in the objective function. Previous
projection-based trajectory optimization approaches have not incorporated
multimodal sensor selection.

##  V Results

Figure 3: The trace of the Gaussian process covariance matrix is shown on the
left and the RMSE of the beliefs with respect to the true map is shown on the
right. For even comparison we evaluated $\text{Tr}(\Sigma)$ and the RMSE all
using the same Gaussian process setup, even though the Ergodic and Random
methods do not use the Gaussian process for decision making. Each subplot
shows the average and standard deviation from 50 simulation runs. The budget
was varied from $b=30$ to $100$ and the spectrometer noise was varied from
$\sigma_{s}=0.1$ to $1.0$.

###  V-A Rover Exploration Problem

To evaluate our approach, we use a modified version of the Rover Exploration
problem which was a benchmark AIPPMS problem introduced by Ott et al. and
inspired by multiple planetary rover exploration missions [4, 5, 32, 20]. The
rover begins at a specified starting location and has a fixed amount of energy
available to explore the environment and reach the goal location. The rover is
equipped with a spectrometer and a drill. Drilling reveals the true state of
the environment at the location the drill sample was taken and is a more
costly action to take from a resource budget perspective. Conversely, the
spectrometer provides a noisy observation of the environment and uses less of
the resource budget. Since the rover does not have access to the ground truth
state of the environment during execution, the rover’s goal is to reduce the
uncertainty in its belief about the environment as much as possible.

The environment is modeled as an $n\times n$ grid with $\beta$ unique
measurement types in the environment. To construct a spatially-correlated
environment, we first start by sampling each grid cell value from an
independent and identically distributed uniform distribution of the $\beta$
unique measurement types. Then, each cell takes on the average value of its
neighboring cells with probability $p_{g}$. This process creates environments
with spatial correlation as well as some random noise to simulate geospatial
environments where the amount of spatial correlation is controlled by $p_{g}$.

###  V-B Approaches Considered

We directly compare our approach with five other methods.

  1. 1.

MCTS-DPW. Ott et al. used MCTS-DPW with Gaussian process beliefs which was
able to significantly outperform previous AIPPMS approaches [20, 1].

  2. 2.

Ergodic. Projection-based trajectory optimization has previously been applied
to ergodic objectives introduced by Miller and adapted by Dressel [26, 28]. We
use evenly spaced drill measurements since the ergodic metric does not
consider multimodal sensing capabilities.

  3. 3.

Ergodic-GP. The Ergodic Gaussian process (Ergodic-GP) method is a modified
ergodic control approach that also uses a Gaussian process to update the
information distribution as samples are received online. The updated
information distribution is then used to plan an ergodic trajectory at the
next iteration. This approach allows the agent to combine the ergodic metric
for trajectory generation with the Gaussian process for drill site selection.

  4. 4.

GP-PTO. The Gaussian Process Projection-based Trajectory Optimization (GP-PTO)
method is our contribution outlined in the previous section.

  5. 5.

GP-PTO Offline. The GP-PTO Offline method is a slight modification of GP-PTO
that allows the optimization to run longer and then executes the best
trajectory without replanning online.

  6. 6.

Random. The random method was implemented as a baseline. At each step, the
agent chooses randomly from a set of actions. The action space is constrained
so that the agent will always be able to reach the goal location within the
specified energy resource constraints.

MCTS-DPW, GP-PTO, Ergodic-GP and Random all replan after each step of the
trajectory. This means that they receive a sample, update their belief, and
then replan accounting for this updated belief. The Ergodic and GP-PTO Offline
methods do all of their planning offline and then execute the trajectory
without accounting for the sample types that were received.

###  V-C Trajectory Comparison

An example of trajectories generated with each of the six approaches is shown
in Fig. 2. We highlight a few key observations that emerge from these
trajectories. MCTS-DPW often revisits locations in the environment that it has
already explored. MCTS-DPW uses rollouts to estimate the value of each action
available to it. As a result, exploration is limited to its rollout depth and
therefore it cannot take into consideration the entire trajectory during
planning, but instead only considers the next several steps in the trajectory.

The Ergodic approach produces a sinusoidal trajectory, which is often
desirable in exploration and search-based problems [27, 26, 28]. However,
since the ergodic metric has no knowledge of the underlying Gaussian process,
it is not able to adjust the spacing of these sinusoidal waves accordingly,
which leaves many gaps unexplored.

We see relatively poor results from the Ergodic-GP approach. The issue arises
due to oscillations in the ergodic metric. In the Fourier decomposition method
used by Miller and Dressel, the trajectories can have significant changes in
direction from one planning iteration to the next, causing oscillations [26,
28]. While the trajectories planned at each iteration are feasible, what ends
up being executed is often of poor quality. This poor performance under rapid
replanning has also been noted by Dressel [29].

The GP-PTO and GP-PTO Offline methods are able to smartly place drill sites on
the outer regions of the trajectory. This emergent behavior results from the
fact that the GP-PTO methods recognize they will not be able to visit the
entire environment so they dedicate their most valuable samples to the
boundaries and accept a greater quantity of noisier samples on the interior.
Placing valuable measurement types at the boundary of the explored region will
decrease the variance more compared to placing the valuable measurements on
the inner regions of previously explored space.

###  V-D Simulation Results

For the Rover Exploration problem, we focus on the interplay between the
resource budget allotted to the agent and the sensing quality of the
spectrometer, where $\sigma_{s}$ denotes the standard deviation of a Gaussian
sensor model. The results from our large scale simulation comparison across
varying $\sigma_{s}$ and $b$ are summarized in Figs. 3 and 4.

GP-PTO outperforms MCTS-DPW in total variance reduction in roughly four of the
nine cases considered. They approximately tie in three cases and MCTS-DPW
outperforms in two cases. Of particular significance is that GP-PTO tends to
outperform as the resource budget is increased. GP-PTO considers the entire
trajectory during the optimization, while MCTS-DPW only rolls out a few steps
into the future. Therefore, as the resource budget is increased, accounting
for future actions becomes more important to performance. MCTS-DPW
consistently has a steeper slope in variance reduction which is a direct
result of the differences between the rollout-based methods and projection-
based methods. MCTS-DPW will take the actions that have the greatest variance
reduction based on its rollout horizon and therefore will prioritize more
near-term variance reduction whereas the GP-PTO methods will prioritize the
long-term variance reduction. In trajectories with less energy budget, MCTS-
DPW does not have an issue because there are fewer opportunities to re-explore
areas it already visited. However, in longer trajectories, MCTS-DPW overlaps
itself much more frequently as shown in Fig. 2 due to its limited rollout
depth. Additionally, MCTS-DPW uses a discrete action space so all of its
movements are of unit size. Conversely, the projection-based methods have a
hybrid action space and therefore reason over variable step sizes.

Figure 4: Drill placements along the trajectory from 50 simulations.

The GP-PTO methods perform relatively similarly, and in one case, the online
version outperforms the offline version. GP-PTO offline was allowed to run for
$5,000$ iterations whereas GP-PTO ran until convergence criteria were met or a
maximum of $50$ iterations was reached. Additionally, the GP-PTO method can be
terminated at any point based on planning time, iterations, or other
convergence criteria and the best solution found up to that point will be
used.

The Ergodic approach is clearly inferior but still performs relatively well in
comparison with the other methods. This is a very interesting result since the
Ergodic method has no knowledge of the Gaussian process world belief, yet is
still able to perform well indicating that there are some similarities present
between the objective of reducing the variance of a Gaussian process and the
measure of ergodicity. The Ergodic-GP approach is clearly the worst performer
– sometimes even being outperformed by the Random policy.

Fig. 4 shows the distribution of drill sites along all of the trajectories
from the simulation runs. We can see that when the spectrometer produces very
noisy measurements, $\sigma_{s}=1.0$, MCTS-DPW and GP-PTO tend to drill more
frequently with the drills distributed throughout the trajectory.

It is also important to note the differences in planning time across the
different approaches considered. In all of our experiments, the average
planning time for MCTS-DPW, Ergodic, Ergodic-GP, GP-PTO, and GP-PTO Offline
were on the order of $0.50,0.001,3.0,10.0,30.0$ seconds respectively. Note
that the increased planning time in the GP-PTO methods is a result of
conducting inference over the Gaussian process belief for the whole
trajectory. This is the main bottleneck in the planning process, but
additional planning time constraints can be imposed and the GP-PTO methods
will stop execution and return the best solution found so far.

###  V-E Expected Improvement Metric

The predictive variance in Gaussian processes depends only on the locations of
measurements. We have also evaluated our method using the expected improvement
metric which does take sample values into account. These results are shown in
Figure 5. The expected improvement metric is given by:

$\mathbb{E}[I_{p}(y)]=(y_{\text{min}}-\hat{\mu})P(y\leq
y_{\text{min}})+\hat{\sigma}^{2}\mathcal{N}(y_{\text{min}}\mid\hat{\mu},\hat{\sigma}^{2})$

where $I_{p}(y)$ is the improvement over the current minimum found so far,
$y_{\text{min}}$ is the minimum value sampled so far, and $\hat{\mu}$ and
$\hat{\sigma}^{2}$ are the predicted mean and variance of the Gaussian process
[22]. We see that GP-PTO is also able to outperform the other methods
considered on this adaptive metric.

Figure 5: Expected improvement results with respect to the true map. The
average and standard deviation from 50 simulation runs are shown.

##  VI Conclusion

This work focused on a variant of the adaptive informative path planning
problem where the agent is equipped with multiple sensors. We introduced a
Gaussian process projection-based trajectory optimization approach where we
considered both sensor distribution and sensor type. We showed that our
approach demonstrates performance comparable to previous state-of-the-art
AIPPMS methods, but offers scalability advantages in long horizon trajectories
by reasoning about the entire trajectory during the optimization process.
Future work includes integrating more efficient Gaussian process
representations to decrease planning time, such as adaptive query resolutions
and expanding the belief representation to reason over Gaussian process
parameters. Additionally, alternative sample injection techniques and
objective functions can be explored for improved performance. Lastly, hybrid
approaches have the potential to present unique solutions. For example,
combining MCTS-DPW with GP-PTO could result in more efficient solutions. These
areas of future research would allow a similar formulation to be extended to a
variety of real-world problems.

## References

  * [1] Shushman Choudhury, Nate Gruver and Mykel J Kochenderfer  “Adaptive informative path planning with multimodal sensing”  In _International Conference on Automated Planning and Scheduling (ICAPS)_ 30, 2020, pp. 57–65
  * [2] Jean-Raymond Melingui Betterton  “Reinforcement Learning for Adaptive Sampling in X-Ray Applications”, 2022
  * [3] Roman Marchant, Fabio Ramos and Scott Sanner  “Sequential Bayesian optimisation for spatial-temporal monitoring.”  In _Conference on Uncertainty in Artificial Intelligence (UAI)_ , 2014, pp. 553–562
  * [4] Raymond Francis, T Estlin, Gary Doran, Stephen Johnstone, D Gaines, Vandi Verma, M Burl, Jens Frydenvang, Suzanne Montaño and Roger C Wiens  “AEGIS autonomous targeting for ChemCam on Mars Science Laboratory: Deployment and results of initial science team use”  In _Science Robotics_ 2.7 American Association for the Advancement of Science, 2017, pp. eaan4582
  * [5] N Mangold, S Gupta, O Gasnault, G Dromart, JD Tarnas, SF Sholes, B Horgan, C Quantin-Nataf, AJ Brown and S Le Mouélic  “Perseverance rover reveals an ancient delta-lake system and flood deposits at Jezero crater, Mars”  In _Science_ 374.6568 American Association for the Advancement of Science, 2021, pp. 711–717
  * [6] Alexandra Meliou, Andreas Krause, Carlos Guestrin and Joseph M Hellerstein  “Nonmyopic informative path planning in spatio-temporal models”  In _AAAI Conference on Artificial Intelligence (AAAI)_ , 2007, pp. 16–7
  * [7] Amarjeet Singh, Andreas Krause and William J Kaiser  “Nonmyopic adaptive informative path planning for multiple robots”  In _International Joint Conference on Artificial Intelligence (IJCAI)_ , 2009
  * [8] Geoffrey A Hollinger, Brendan Englot, Franz S Hover, Urbashi Mitra and Gaurav S Sukhatme  “Active planning for underwater inspection and the benefit of adaptivity”  In _The International Journal of Robotics Research_ 32.1 SAGE Publications Sage UK: London, England, 2013, pp. 3–18
  * [9] Gregory Hitz, Alkis Gotovos, Marie-Éve Garneau, Cédric Pradalier, Andreas Krause and Roland Y Siegwart  “Fully autonomous focused exploration for robotic environmental monitoring”  In _IEEE International Conference on Robotics and Automation (ICRA)_ , 2014, pp. 2658–2664  IEEE
  * [10] Zhan Wei Lim, David Hsu and Wee Sun Lee  “Adaptive informative path planning in metric spaces”  In _The International Journal of Robotics Research_ 35.5 SAGE Publications Sage UK: London, England, 2016, pp. 585–598
  * [11] Yogesh Girdhar and Gregory Dudek  “Modeling curiosity in a mobile robot for long-term autonomous exploration and monitoring”  In _Autonomous Robots_ 40.7 Springer, 2016, pp. 1267–1278
  * [12] Marc Toussaint  “The Bayesian search game”  In _Theory and Principled Methods for the Design of Metaheuristics_ Springer, 2014, pp. 129–144
  * [13] Marija Popović, Teresa Vidal-Calleja, Gregory Hitz, Jen Jen Chung, Inkyu Sa, Roland Siegwart and Juan Nieto  “An informative path planning framework for UAV-based terrain monitoring”  In _Autonomous Robots_ 44.6 Springer, 2020, pp. 889–911
  * [14] Daniel A Duecker, Benedikt Mersch, Rene C Hochdahl and Edwin Kreuzer  “Embedded stochastic field exploration with micro diving agents using Bayesian optimization-guided tree-search and GMRFs”  In _IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2021, pp. 8649–8656  IEEE
  * [15] Joshua Ott, Sung-Kyun Kim, Amanda Bouman, Oriana Peltzer, Mamoru Sobue, Harrison Delecki, Mykel J. Kochenderfer, Joel Burdick and Ali-akbar Agha-mohammadi  “Risk-aware Meta-level Decision Making for Exploration Under Uncertainty”, 2022  arXiv:2209.05580
  * [16] Philippe Morere, Roman Marchant and Fabio Ramos  “Continuous state-action-observation POMDPs for trajectory planning with Bayesian optimisation”  In _IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2018, pp. 8779–8786  IEEE
  * [17] Julius Rückin, Liren Jin and Marija Popović  “Adaptive informative path planning using deep reinforcement learning for UAV-based active sensing”  In _IEEE International Conference on Robotics and Automation (ICRA)_ , 2022  IEEE
  * [18] Leslie Pack Kaelbling, Michael L Littman and Anthony R Cassandra  “Planning and acting in partially observable stochastic domains”  In _Artificial Intelligence_ 101.1-2 Elsevier, 1998, pp. 99–134
  * [19] Adrien Couëtoux, Jean-Baptiste Hoock, Nataliya Sokolovska, Olivier Teytaud and Nicolas Bonnard  “Continuous upper confidence trees”  In _Learning and Intelligent Optimization (LION)_ , 2011, pp. 433–445  Springer
  * [20] Joshua Ott, Edward Balaban and Mykel J Kochenderfer  “Sequential Bayesian Optimization for Adaptive Informative Path Planning with Multimodal Sensing”  In _arXiv preprint arXiv:2209.07660_ , 2022
  * [21] Mykel J Kochenderfer, Tim A Wheeler and Kyle H Wray  “Algorithms for Decision Making”  MIT Press, 2022
  * [22] Mykel J Kochenderfer and Tim A Wheeler  “Algorithms for Optimization”  MIT Press, 2019
  * [23] Philippe Morere, Roman Marchant and Fabio Ramos  “Sequential Bayesian optimization as a POMDP for environment monitoring with UAVs”  In _IEEE International Conference on Robotics and Automation (ICRA)_ , 2017, pp. 6381–6388
  * [24] Isabel M Rayas Fernández, Christopher E Denniston, David A Caron and Gaurav S Sukhatme  “Informative path planning to estimate quantiles for environmental analysis”  In _IEEE Robotics and Automation Letters_ IEEE, 2022
  * [25] John Hauser  “A Projection Operator Approach to the Optimization of Trajectory Functionals”  In _IFAC Proceedings Volumes_ 35.1 Elsevier, 2002, pp. 377–382
  * [26] Lauren M Miller and Todd D Murphey  “Trajectory optimization for continuous ergodic exploration”  In _American Control Conference (ACC)_ , 2013, pp. 4196–4201  IEEE
  * [27] George Mathew and Igor Mezić  “Metrics for Ergodicity and Design of Ergodic Dynamics for Multi-agent Systems”  In _Physica D: Nonlinear Phenomena_ 240.4-5 Elsevier, 2011, pp. 432–442
  * [28] Louis Dressel and Mykel J Kochenderfer  “On the Optimality of Ergodic Trajectories for Information Gathering Tasks”  In _American Control Conference (ACC)_ , 2018, pp. 1855–1861  IEEE
  * [29] Louis Kenneth Dressel  “Efficient and Low-cost Localization of Radio Sources with an Autonomous Drone”, 2018
  * [30] Louis Dressel and Mykel J Kochenderfer  “Tutorial on the Generation of Ergodic Trajectories with Projection-based Gradient Descent”  In _IET Cyber-Physical Systems: Theory & Applications_ 4.2 Wiley Online Library, 2019, pp. 89–100
  * [31] Anderson DO Brian and John B Moore  “Optimal Control”  In _Linear Quadratic Methods_ Citeseer, 1990
  * [32] Jennifer L Heldmann, Anthony Colaprete, Richard C Elphic, Ben Bussey, Andrew McGovern, Ross Beyer, David Lees and Matt Deans  “Site selection and traverse planning to support a lunar polar rover mission: A case study at Haworth Crater”  In _Acta Astronautica_ 127 Elsevier, 2016, pp. 308–320