# Where to Map? Iterative Rover-Copter Path Planning for Mars Exploration

Takahiro Sasaki1, Kyohei Otsu2, Rohan Thakker2, Sofie Haesaert3, and Ali-akbar
Agha-mohammadi2 Manuscript received: September 10, 2019; Revised: December 6,
2019; Accepted: December 28, 2019.This paper was recommended for publication
by Editor Jonathan Roberts upon evaluation of the Associate Editor and
Reviewers’ comments.The research was carried out at the Jet Propulsion
Laboratory and the California Institute of Technology, under a contract with
the National Aeronautics and Space Administration and funded through the
President’s and Director’s Fund Program. U.S. Government sponsorship
acknowledged.1T. Sasaki is with Japan Aerospace Exploration Agency, Tsukuba,
Japan, (e-mail: sasaki.takahiro@jaxa.jp)2K. Otsu, R. Thakker, and A. Agha-
Mohammadi are with Jet Propulsion Laboratory, California Institute of
Technology, Pasadena, CA, (e-mail<EMAIL_ADDRESS><EMAIL_ADDRESS>aliakbar.aghamohammadi@jpl.nasa.gov)3S. Haesaert
is with Eindhoven University of Technology, Eindhoven, The Netherlands,
(e-mail: s.haesaert@tue.nl)Digital Object Identifier (DOI): see top of this
page.

###### Abstract

In addition to conventional ground rovers, the Mars 2020 mission will send a
helicopter to Mars. The copter’s high-resolution data helps the rover to
identify small hazards such as steps and pointy rocks, as well as providing
rich textual information useful to predict perception performance. In this
paper, we consider a three-agent system composed of a Mars rover, copter, and
orbiter. The objective is to provide good localization to the rover by
selecting an optimal path that minimizes the localization uncertainty
accumulation during the rover’s traverse. To achieve this goal, we quantify
the localizability as a goodness measure associated with the map, and conduct
a joint-space search over rover’s path and copter’s perceptual actions given
prior information from the orbiter. We jointly address where to map by the
copter and where to drive by the rover using the proposed iterative copter-
rover path planner. We conducted numerical simulations using the map of Mars
2020 landing site to demonstrate the effectiveness of the proposed planner.

###### Index Terms:

Cooperating Robots, Motion and Path Planning, Multi-Robot Systems, Space
Robotics and Automation.

##  I Introduction

The National Aeronautics and Space Administration (NASA) has sent several
mobile rovers onto the surface of the red planet, to understand the Martian
geology and the habitability of the environment. Recently, besides the
conventional ground rovers, NASA decided to send a helicopter to the Martian
sky, which will be traveling with a rover in the Mars 2020 mission [1] (Fig.
1). Although the objective of this mission is limited to a technology
demonstration (with less than 90 seconds flight time per day), future rover
missions might be accompanied by copters, which work as low-flying scouts
providing rich information about locations ahead of the rover. Such data
provided by the copters could significantly increase the probability of
mission success. Currently, the highest resolution achieved by a satellite is
25 cm/px (HiRISE) [2], which means small rocks remain undetected and are not
taken into account by long-range strategic planners. These micro-structures
could be critical hazards to the rover, such as sharp pointy rocks that
significantly damaged the wheels in the Mars Science Laboratory (MSL) mission
[3]. The copter’s high-resolution data helps the rover to detect such small
hazards in advance. In addition, it could provide richer information that
further helps rover’s path planning. For example, high-resolution top-view
data will increase the confidence of localizability information with increased
resolution and contextual information.

Figure 1: Rover/copter team for Mars exploration.

Autonomy will play an important role in future complex missions where multiple
assets act independently[4]. Localization, or more generally state estimation,
is one of the key components in establishing autonomy. Due to the absence of
global positioning systems, dead-reckoning methods such as Wheel Odometry (WO)
and Visual Odometry (VO) are the major source of localization on Mars. The
rover’s remote sensing capability is strongly impacted by localization
performance since the accumulated errors in rover positions impose challenges
in targeted observations after a few drives. Perception-aware planning is one
of the effective methods to improve the performance of dead-reckoning-based
localization [5, 6, 7, 8, 9]. It aims at improving the perception results by
actively choosing future measurement targets. For example, the works in [8, 9]
improved the performance of VO localization by actively choosing timing and
camera direction to obtain an optimal image sequence using the predictive
perception technique. This problem is typically approached by Partially
Observable Markov Decision Process (POMDP), or belief-space planning [10, 11,
12, 13, 14], where the planner chooses optimal actions under motion and
sensing uncertainty.

In this paper, we consider a three-agent system composed of Mars rover,
copter, and orbiter. The objective is to compute an optimal rover path that
minimizes the localization uncertainty accumulation after a traverse. The key
observation is that the accuracy of dead-reckoning localization depends on
terrain types; for example, VO accuracy is degraded in feature-poor terrain
such as sand [15]. In other words, the localization performance can be boosted
by selectively driving over “good” terrain to localize, which we call
localizability in the rest of the paper. Given the prior localizability map
provided by remote satellite measurements, we conduct a joint-space search
over rover’s path and copter’s perceptive actions while taking dynamic map
updates from the copter into account. In other words, we jointly address where
to map by the copter and where to drive by the rover in order to minimize the
uncertainty accumulation in rover localization. The effectiveness of the
proposed method is demonstrated through numerical simulations using the map of
planned landing site.

###  I-A Related Work

####  I-A1 Multi-agent systems in terrestrial applications

There are a lot of studies on multi-vehicles for both ground and aerial
systems. Compared to ground vehicles, typically, aerial vehicles can cover
larger search areas in a given time. For UAV teams, [16] studies use of
Dijkstra algorithm for real-time motion control. [17] utilizes multiple active
sensors to avoid mid-air collisions. Recent methods study decentralized visual
simultaneous localization and mapping (SLAM) (e.g., [18]) and planning under
uncertainty (e.g., [19]). However, UAVs are typically highly resource limited,
ranging from their sensory limitation in their localization accuracy of ground
targets as well as computational and battery charge limitations. Multi-
ground/aerial system is focused on a combined system to address some of these
limitations [20, 21].

####  I-A2 Multi-agent systems in planetary exploration

Unlike terrestrial exploration, Mars surface makes it more difficult for the
exploration vehicles to estimate their locations. [22] describes an integrated
system for coordinating multi-rover behavior with the overall goal of
collecting planetary surface data. [23] shows how to construct evaluation
functions in dynamics, noisy and communication-limited collective
environments. [4] focuses on navigation and coordination of the Mars rover and
copter teams such as the one shown in Fig. 1, and aims at bridging the gap
between high-level mission specifications and low-level navigation and control
techniques under the environment uncertainty. Also, there is a large body of
work studying the Mars rover/orbiter collaboration and data sharing (e.g.,
[24])

###  I-B Mission Concept

Our conceptual mission treats the copter as a scout to collect data before the
beginning of rover’s traverse on each Martian day. The rover plans an optimal
path that avoids critical terrain hazards and regions with low localizability
using the updated map information by the copter. We assume a copter deployment
scenario similar to the upcoming Mars 2020 rover/copter mission. The copter is
first deployed from the rover to the ground, and then it takes off directly
from the ground after the rover drives away for a certain distance. This is to
protect the precious rover from accidental damage caused by copter’s
misbehavior. Mission assumptions are:

  * •

The copter can only fly 90 seconds per day due to power constraints [1]

  * •

The rover has more energy and operation time but can only traverse at a very
low speed ($\sim$4 cm/s)

  * •

The copter can move fast and is not hindered by obstacles

  * •

The copter cannot get closer than a certain distance to the rover for safety.

###  I-C Contributions

In this work, we aim to develop a joint rover and copter motion planner that
minimizes the localization uncertainty of the rover in a noisy localizability
map. The contribution of this paper is summarized as follows:

  * •

Define hyper-belief map representation for the VO odometry error uncertainty;

  * •

Solve the copter’s where-to-map problem for minimizing the rover localization
uncertainty

  * •

Solve for optimal image capture point for the copter, while considering the
trade-off between field-of-view and resolution of the camera;

  * •

Propose an iterative rover/copter path planning method and demonstrate the
effectiveness through simulations.

##  II Problem Description

Figure 2: Belief propagation in rover’s state estimation. Since odometry error
is dependent on terrain localizability, the belief evolution depends on the
rover’s path.

The main problem that we are going to address is a two-agent motion planning
problem where one agent is expected to assist the other for the mission to
succeed. The objective of the primary agent is to reach the goal with high
localization confidence, while the secondary agent works as a scout to
increase the information quantity about the environment to help primary
agent’s motion planning. In our problem setup, two agents are Mars rover and
Mars helicopter. The prior imperfect information comes from the Mars orbiter.

####  II-1 Primary agent (rover)

Rovers’ mobility is limited to relatively flat surfaces. We denote the rover
state by $x^{r}$ which is typically parameterized by a position in X-Y plane
and its heading. The rover moves with a slow speed traveling from the initial
state $x_{0}^{r}$ to the goal state $x_{g}^{r}$. Due to the absence of global
positioning methods on Mars, a rover has to estimate its pose incrementally by
propagating relative transformations between infrequent pose updates available
through local bundle adjustment or satellite image matching. This incremental
dead-reckoning approach is prone to error accumulation. Typically, such error
is captured by propagating the covariance of state estimation as shown in Fig.
2. The rover has to keep its state uncertainty small enough to avoid obstacle
collisions or entering keep-out-zones.

####  II-2 Secondary agent (copter)

The Mars copter has an ability to fly and explore larger areas with down-
facing cameras. The copter state is denoted by $x^{c}$ which is composed of a
3D position and a heading angle. The difference in altitude affects the
quality of sensor measurements such as resolution and estimation confidence,
as well as the size of the area it can observe. While copter is much faster
than the rover, its operation time is limited to a single 90-second flight per
day due to the severe power constraints. In addition, the copter’s should
constantly keep a certain distance from the rover to satisfy the safety
constraint for the rover.

####  II-3 Environment

On parts of Mars, we have surface data available from satellite imagery up to
25cm/px resolution. This limited information needs to be complemented by
onboard sensing for any surface navigation. For the current navigation
methods, the terrain type is an important factor to determine localizability.
In case of WO, significant error is expected on loose sandy terrain due to
wheel slippage, and VO is affected by visual texture on the ground. Therefore,
a localizability index can be computed using terrain classification techniques
such as [25]. However, the resolution of satellite data is not high enough to
confidently assess the localizability on the Mars surface. Adding copter’s
data helps increasing the accuracy of localizability information with
increased resolution and contextual information.

####  II-4 Problem

Given the partial prior map from satellite, we simultaneously solve for the
rover path planning and optimal locations for copter measurements acquisition
to minimize the state uncertainty accumulation in rover localization (i.e.,
error ellipse in Fig. 2). The main challenge here is that we cannot precisely
predict how the covariance evolves along a given path before traversal. What
we can do instead is to have a belief on covariance evolution, given the prior
localizability estimate. We call this a hyper-belief as it is a belief over
state beliefs. The hyper-belief propagation process is depicted in Fig. 3. In
the top figure, the hyper covariance along a path is shown by yellow regions,
which is regarded as the distribution over the state error ellipse. To reduce
the size of the terminal covariance, we specifically consider two approaches:
1) plan a rover’s path to avoid excessive error accumulation, and 2) update
the localizability map by copter measurements to change the hyper-belief
evolution along paths. The second approach is presented in Fig. 3b. After the
map update by the copter, the uncertainty in covariance evolution is reduced.
As a result, the covariance in hyper-beliefs becomes smaller (the area of
yellow region), and hence the confidence on state uncertainty increases.

(a) The propagation of state hyper-belief along a path. The yellow region
represents the size of the state covariance in a distribution of error
ellipse. This is described as a donut-shaped region around error ellipse.

(b) Hyper-belief update after copter’s measurement. The covariance of terminal
hyper-belief is reduced due to the localizability update on the rover path.

Figure 3: Hyper-belief propagation in rover’s state estimation.

##  III Localization Error Propagation

In this section, we describe how we model the propagation of pose estimates
and their uncertainty, along with how we encode localizability into a map.

###  III-A Pose belief propagation

To capture uncertainty in the robot pose, we directly work and plan over pose
beliefs. A pose belief is a probability distribution over the possible states
of the robot. Below, we will present the mathematical definition of pose
beliefs and propagation process.

####  III-A1 Stochastic pose representation

We treat the pose of the rover as a member of Lie group $T\in SE(3)$, which
encodes the rover’s 3D pose and orientation in a $4\times 4$ transformation
matrix. To capture the stochasticity of the rover pose in $SE(3)$, we rely on
perturbation models on this Lie group [26]. Denote the small pose perturbation
by $\xi=(\rho,\phi)\in\mathbb{R}^{6}$, where $\rho\in\mathbb{R}^{3}$ is
perturbation in position and $\phi\in\mathbb{R}^{3}$ in orientation. Using
this perturbation representation, the noisy rover pose is expressed as

$\displaystyle T\equiv\exp(\xi^{\wedge})\bar{T},$ (1)

where $\bar{T}\in SE(3)$ is the nominal noise-free pose, and the noise vector
$\xi$ is mapped to a member of $SE(3)$ using the exponential map through the
Lie algebra. Note that $\exp(\cdot)$ is an operator producing the matrix
exponential which gives the connection between a matrix Lie algebra and the
corresponding Lie group. More precisely, we map a vector $\xi$ to a member of
the Lie algebra using the operator $\wedge$

$\displaystyle\xi^{\wedge}=\begin{bmatrix}\phi^{\times}&\rho\\\
0^{T}&0\end{bmatrix}$ (2)

where the operator $\times$ produces a skew-symmetric matrix from a vector,
i.e.,

$\displaystyle\bm{\phi}^{\times}=\begin{bmatrix}\phi_{1}\\\ \phi_{2}\\\
\phi_{3}\end{bmatrix}^{\times}=\begin{bmatrix}0&-\phi_{3}&\phi_{2}\\\
\phi_{3}&0&-\phi_{1}\\\ -\phi_{2}&\phi_{1}&0\end{bmatrix}\>.$ (3)

This perturbation approach ensures that noisy variables also belong to the
same group (i.e., $T,\bar{T}\in SE(3)$). Equation (1) can be interpreted as a
noise injection into the group $SE(3)$. We assume that the perturbation
follows a zero-mean Gaussian distribution

$\displaystyle\xi\sim\mathcal{N}(0,\Sigma)$ (4)

with covariance matrix $\Sigma\in\mathbb{R}^{6\times 6}$. In the sequel, we
use the pair $\\{\bar{T},\Sigma\\}$ to characterize the distribution of the
stochastic pose.

####  III-A2 Pose belief evolution.

Using the above definition, we represent the robot pose and covariance at time
$t$ by $\\{\bar{C}_{t},P_{t}\\}$, and the relative transformation and its
uncertainty between time $t$ and $t+1$ by
$\\{\bar{T}_{t+1,t},\Sigma_{t+1,t}\\}$. Similar to the work [26], we propagate
the mean and covariance of the robot pose up to the second-order term:

$\displaystyle\bar{C}_{t+1}$ $\displaystyle=\bar{C}_{t}\bar{T}_{t+1,t}$ (5)
$\displaystyle P_{t+1}$
$\displaystyle=P_{t}+\mathcal{T}_{t}\Sigma_{t+1,t}\mathcal{T}_{t}^{T}$ (6)

where the adjoint
$\mathcal{T}_{t}=\mathrm{adj}(\bar{C}_{t})\in\mathbb{R}^{6\times 6}$ can be
computed from the current pose. Note that $\mathrm{adj}(\cdot)$ is an operator
producing the adjoint of an element from the Lie group for rotations and
poses.

###  III-B Map representation

We approximate the environment as a map represented by a regular grid with $n$
cells. This allows us to analyze the localizability, or equivalently the
expected perturbation, for the individual cells. Localization methods such as
WO and VO are always contaminated with odometry error or perturbation $\xi$ as
in Eq. (1). It is reasonable to assume that the intensity of odometry error,
i.e., $\Sigma=Cov[\xi]$, depends on environmental factors such as terrain
types. As our focus is on the path selection in a global scale, we simplified
the error model by assuming the error accumulation is independent of local
motion types and is proportional to the distance traveled. With this, we model
the environment as a set of normalized motion estimation covariances, denoted
by $m=\\{\tilde{\Sigma}_{1},\cdots,\tilde{\Sigma}_{n}\\}$. Simply put, if the
rover traverses on $i$-th cell for $d$ meters, the covariance of motion
estimation becomes $d\tilde{\Sigma}_{i}$ and state estimation uncertainty
grows accordingly with Eq. (6). Thus, we approximate the environment with a
regular grid composed of $n$ cells, and associate with each cell a
localizability index.

The localizability map cannot be built before actual traversal on every
location. Instead, we keep a probability distribution (belief) on map $m$
based on remote sensing data. In our problem, we build a map belief based on
satellite measurements $z^{s}$, copter measurements $z^{c}_{k}$ and position
$x^{c}_{k}$:

$\displaystyle b^{m}_{k}=p(m|z^{s},z^{c}_{1:k},x^{c}_{1:k})$ (7)

where $z^{c}_{1:k}=\\{z^{c}_{1},\cdots,z^{c}_{k}\\}$ and
$x^{c}_{1:k}=\\{x^{c}_{1},\cdots,x^{c}_{k}\\}$. By choosing to model the
environment as a collection of cells, the belief $b^{m}_{k}$ contains $n$
individual beliefs.

$\displaystyle b^{m}_{k}$
$\displaystyle\approxeq\\{b^{m^{1}}_{k},\cdots,b^{m^{n}}_{k}\\}$ (8)
$\displaystyle b^{m^{i}}_{k}$
$\displaystyle=p(m^{i}|z^{s},z^{c}_{1:k},x^{c}_{1:k})$ (9)

Specifically, the prior map belief obtained from satellite data is represented
as $b^{m^{i}}_{0}=p(m^{i}|z^{s})$. Note that modelling the belief as a
collection of beliefs over map cells offers a computationally-tractable
approximation to the evolution of the full joint map belief.

###  III-C Pose hyper-belief propagation

The pose belief is propagated with Eqs. (5) and (6) if the relative
transformation is given in a deterministic form. In our problem, we plan
rover’s path on a map belief and the uncertainty accumulation only given as a
stochastic form. Given an initial pose covariance $P_{0}$, and the rover’s
path $X^{r}$ expressed as a list of cell indices, the pose covariance $P_{g}$
at the terminal pose $x^{r}_{g}$ is predicted as

$\displaystyle P_{g}$ $\displaystyle=P_{0}+\sum_{i\in
X^{r}}d_{i}\mathcal{T}_{i}\tilde{\Sigma}_{i}\mathcal{T}_{i}^{\top}$ (10)

where $d_{i}$ is the travel distance on $i$-th cell. At time instant $k$, the
local motion estimation variance $\tilde{\Sigma}_{i}$ is unknown and modeled
with the belief distribution $b^{m^{i}}_{k}$.

##  IV Map Update

The localizability map is first initialized with the satellite measurements,
and then dynamically updated by the copter. In this section, we describe the
measurement models and the prediction of map belief update by future copter
measurements.

###  IV-A Measurements

In Mars missions, there are various sensing modalities, including measurements
from orbiter, copter, and rover’s onboard sensors. Each sensor differently
contributes to the map update with various resolution and confidence.

####  IV-A1 Satellite observation

The satellite observation is considered to be the lowest in resolution and
fidelity, but the highest in areal coverage. We assume that all locations in
the map are measured by satellites. The satellite-based measurements usually
come with large uncertainty. The uncertainty is also terrain type dependent.

####  IV-A2 Copter observation

The copter observation has higher resolution and fidelity than satellites, but
the scope is smaller than the entire map. The intensity of measurement noise
varies with the copter’s 3D location as illustrated in Fig. 4. The blue square
in this figure represents the field of view (FOV) of the copter’s camera. At
higher altitudes, copter can observe larger regions but the noise intensity is
higher. In contrast, at lower altitudes, the copter can observe smaller
regions but more accurately. In addition to the copter altitude, its 2D
location can cause variation in noise intensity depending on the texture of
the underlying terrain.

####  IV-A3 Copter Measurement Model

The copter’s measurement model for the $i$-th map cell is described as a
probabilistic distribution over copter measurements $z^{c}$ given copter pose
$x^{c}$ and map cell $m^{i}$

$\displaystyle p(z^{c}|m^{i},x^{c}).$ (11)

For simplicity, we assume that the copter can directly observe the
localizability index of a location, and the Gaussian noise is injected to the
measurement:

$\displaystyle p(z^{c}|m^{i},x^{c})=\mathcal{N}(m^{i},w(x^{c}))$ (12)

where $w(\cdot)$ is a function representing the noise intensity that varies
depending on the 3D location of the copter such as altitude.

Figure 4: Trade-off between FOV and resolution of copter measurements.

####  IV-A4 Map update

After each copter measurement, we update the map $m$; precisely speaking, we
update the beliefs over map cells using the Bayesian update rule:

$\displaystyle b^{m^{i}}_{k+1}=\tau(b^{m^{i}}_{k},z^{c}_{k+1},x^{c}_{k+1}).$
(13)

We discuss this update rule further in the next subsection.

###  IV-B Predictive map update

To understand how the map belief will evolve, it is essential to evaluate and
predict the impact of the copter observations on the map.

####  IV-B1 Belief over copter measurement

Given the prior knowledge, we can model the distribution of future copter
observations on the $i$-th cell by marginalizing over the prior map
distribution:

$\displaystyle b^{z^{c}}_{k+1}$
$\displaystyle=p({z}^{c}_{k+1}|z^{s},z^{c}_{1:k},x^{c}_{1:k+1})$ (14)
$\displaystyle=\int
p(m^{i}|z^{s},z^{c}_{1:k},x^{c}_{1:k})p(z^{c}_{k+1}|m^{i},x^{c}_{k+1})dm^{i}.$
(15)

####  IV-B2 Predictive map update

Based on the belief over copter’s measurement, we predict the map belief after
the measurement by:

$\displaystyle
b_{k+1}^{m^{i}}=\tau(b^{m^{i}}_{k},z^{c,ml}_{k+1},x^{c}_{k+1}),~{}~{}~{}z^{c,ml}_{k+1}=\arg\max
b^{z^{c}}_{k+1}$ (16)

Note that we use the most likely copter measurement $z^{c,ml}_{k+1}$ to
predict the map belief. This recursion can be computed as:

$\displaystyle b^{m^{i}}_{k+1}$
$\displaystyle=p(m^{i}|z^{s},z_{0:k+1}^{c},x_{0:k+1}^{c})$ (17)
$\displaystyle=\frac{p(z_{k+1}^{c}|m^{i},z^{s},z_{0:k}^{c},x_{0:k+1}^{c})p(m^{i}|z^{s},z_{0:k}^{c},x_{0:k}^{c})}{p(z_{k+1}^{c}|z^{s},z_{0:k}^{c},x_{0:k+1}^{c})}$
(18) $\displaystyle=\eta p(z_{k+1}^{c}|m^{i},x_{k+1}^{c})b^{m^{i}}_{k}$ (19)

We assume Gaussian distributions provides a good approximation for the map
belief. This assumption can lead to a computationally tractable update rule,
given $b_{0}^{m^{i}}=p(m^{i}|z^{s})=\mathcal{N}(\mu^{i},\sigma^{i^{2}})$ and
Eq. (12).

##  V Planning

In this section, we present an approximate joint-space search method for
rover’s path and copter’s measurements given a prior localizability map from
the satellite. The ultimate objective is to navigate the rover to the goal
with low localization error accumulation.

Algorithm 1 Iterative Rover/Copter Optimization

Generate initial map belief from satellite measurement $z^{s}$
For $j=1$ to max iteration

Find the optimal rover path $X^{r*}_{j}$ given map belief (Section V-C)

Find the optimal copter path $X^{c*}_{j}$ given map belief and rover path
(Section V-D)

Update map belief based on copter measurements (Section IV-B)

Return rover and copter paths $({X}^{r},X^{c})$

###  V-A Approximate Joint-space Search

Searching the joint space of rover path and copter measurements is
computationally demanding. We introduce an approximate solution to the joint-
space search.

####  V-A1 Search space

Let $X^{r}=\\{x^{r}_{0},x^{r}_{1},\cdots,x^{r}_{g}\\}$ denote a smooth rover
path that connects start and goal positions, and
$X^{c}=\\{x^{c}_{1},x^{c}_{2},\cdots\\}$ denote ordered copter measurement
locations. The entire search spaces are represented by $\mathcal{X}^{r}$ and
$\mathcal{X}^{c}$, respectively. The joint space is defined as a product of
two spaces, $\mathcal{X}^{r}\times\mathcal{X}^{c}$.

####  V-A2 Optimization problem

The joint-space optimization problem is formulated as follows. Given a cost
function $J$ and prior map belief $b^{m}_{0}$, find optimal $X^{r}$ and
$X^{c}$ that minimize $J$:

$\displaystyle(X^{r*},X^{c*})=\operatorname*{arg\,min}_{(X^{r},X^{c})\in\mathcal{X}^{r}\times\mathcal{X}^{c}}J(X^{r},X^{c};b^{m}_{0})$
(20)

The detail of the cost function is given in the next section.

####  V-A3 Iterative programming

Searching the full joint space is computationally demanding. We introduce an
approximate solution to the joint-space search by decomposing the problem into
two: rover path planning and copter perception planning. We approximately
solve the optimization problem by iteratively solving these two problems until
it converges (see Algorithm 1).

###  V-B Optimization Criteria

In this section, we discuss the optimization objective mathematically.

####  V-B1 Cost for pose belief

We first define a scalar cost $\Omega$ for evaluating the terminal pose belief
after driving path $X^{r}$. Consider the pose belief represented by a pair
$\\{\bar{C}_{g},P_{g}\\}$. We model the cost for the pose estimate by
computing the trace of covariance matrix:

$\displaystyle\Omega=\operatorname{tr}(P_{g})$ (21)

Note that our objective is to reduce the accumulation error in rover pose
estimation, i.e., minimizing $\Omega$.

####  V-B2 Cost for pose hyper-belief

Due to the uncertainty in the localizability map, the pose covariance cannot
be obtained in a deterministic form prior to driving. Instead, it is given as
a stochastic prediction as formulated in Eq. (10). Accordingly, the cost
$\Omega$ is given as a random variable which is computed from a map belief as

$\displaystyle\Omega=\operatorname{tr}(P_{0})+\sum_{i\in
X^{r}}d_{i}\operatorname{tr}\left(\mathcal{T}_{i}\tilde{\Sigma}_{i}\mathcal{T}_{i}^{\top}\right)$
(22)

To determinize the cost, we introduce the inverse cumulative distribution
function $\Phi_{\Omega}^{-1}(p)$ for $\Omega$ that returns a threshold value
at which the cumulative probability of random variable is equal to the given
probability $p$. We define the cost function as

$\displaystyle J(X^{r},X^{c};b^{m}_{0},p)=\Phi_{\Omega}^{-1}(p).$ (23)

The threshold parameter $p$ can be arbitrarily set. Qualitatively, by
selecting a higher value, the planner tries to minimize the worst-case cost,
while a smaller value (e.g., 0.5) optimizes for nominal performance.

###  V-C Rover Planning

The objective of rover path planning is to solve the optimal path that
minimize the objective function in Eq. (23) given a map belief. As presented
in Algorithm 1, the rover path planning is based on the current best estimate
of map belief that takes future copter measurements into account. Let
$X^{c}_{j}$ be the optimal copter measurements after $j$-th iteration. The
$(j+1)$-th rover pose optimization is formulated as

$\displaystyle
X^{r*}_{j+1}=\operatorname*{arg\,min}_{X^{r}\in\mathcal{X}^{r}}J(X^{r};X^{c}_{j},b^{m}_{0})$
(24)

or more directly,

$\displaystyle
X^{r*}_{j+1}=\operatorname*{arg\,min}_{X^{r}\in\mathcal{X}^{r}}J(X^{r};\grave{b}^{m}_{K})$
(25)

where the predictive map belief $\grave{b}^{m}_{K}$ is built based on the
prior and copter’s $K$ measurements. We solve this problem by casting it as a
graph search problem. Objective criteria in Eq. (22) allow us to set positive
uncertainty accumulation costs to graph edges. The minimum uncertainty
accumulation path can be computed by the Dijkstra’s shortest path algorithm.

###  V-D Copter Planning

The copter’s perception planning problem also aims at improving the objective
function in Eq. (23). Although the rover localization error has no direct
connection to the copter’s measurements, the updated localizability map by
copter’s measurements can improve localization error by indirectly affecting
rover’s path planning process.

####  V-D1 Objective

We rewrite the objective function as the difference in cost by incorporating
copter’s measurements $X^{c}$

$\displaystyle
X^{c*}_{j+1}=\operatorname*{arg\,min}_{X^{c}\in\mathcal{X}^{c}}J^{\prime}(X^{c};X^{r}_{j+1},b^{m}_{0})$
(26)

where

$\displaystyle J^{\prime}(X^{c};X^{r},b^{m}_{0})$
$\displaystyle=J(X^{c};X^{r},b^{m}_{0})-J(\O;X^{r},b^{m}_{0})$ (27)
$\displaystyle=-\sum_{i\in X^{r}}\max_{k}v_{ik}.$ (28)

Note that $v_{ik}$ is the cost improvement in the $i$-th cell along rover’s
path by copter’s $k$-th measurement. If the $i$-th location is not observed by
any copter measurement, the cost improvement is zero. If the location is
observed by multiple measurements, the most effective update is taken.

####  V-D2 Constraints

There are severe operational constraints for the Mars helicopter. In this
paper, the following constraints are considered.

$\displaystyle h_{\min}\leq x_{z}^{c}\leq h_{\max}$ (29) $\displaystyle 0\leq
K\leq k_{\max}$ (30) $\displaystyle d(x^{c}_{K},x^{r}_{0:T})>\Delta$ (31)

where $x_{z}^{c}$ is the altitude of the copter. The first constraint in Eq.
(29) comes from both copter FOV constraint and camera resolution constraint.
This constraint has the trade-off between FOV and resolution of the camera. In
Eq. (30), the limitation of the copter battery constrains copter’s flight time
or distance. The positive scalar $k_{\max}$ is the maximum time step of the
copter and should be larger than the final step of the copter $K$. The final
constraint in the planning framework comes from the copter position at the $K$
(final step) in Eq. (31). Parameter $\Delta$ is the distance margin between
rover and copter for their safety.

####  V-D3 Search Method

To efficiently solve the minimization problem in Eq. (27), we formulate the
problem as a variant of knapsack problem. We discretize the copter’s state
space as $\mathcal{X}^{c}_{search}$. Then, we find the optimal combination of
measurements that maximize the total value while satisfing various
constraints. The value of a measurement is computed as the sum of cost
improvement along a path

$\displaystyle V_{k}=\sum_{i\in X^{r}}v_{ik}.$ (32)

Analogous to the capacity constraint of knapsack problem, this problem poses
the flight distance constraint

$\displaystyle\mathrm{PathLength}(X^{c})\leq l_{\max}.$ (33)

Note that, unlike the capacity constraint, the distance constraint is order-
sensitive. Therefore, before testing the constraint, we solve the travel
salesman problem (TSP) to sort the locations so that the entire path has
shortest distance. The algorithm for the TSP-based knapsack problem is
presented in Algorithm 2.

Algorithm 2 TSP-based knapsack problem

Maximum path length $l_{\max}$
Set initial cost $c_{\max}=0$
For $j=1$ to max iteration

$X^{c}=\mathrm{SamplePathWithTSP}()$

$c=Cost(X^{c})$

$l=PathLength(X^{c})$

If $c>c_{\max}$ and $l\leq l_{\max}$

Update max cost $c_{\max}=c$

Store optimal copter path $X^{c*}=X^{c}$

Return Optimal copter path $X^{c*}$

##  VI Simulation Results

Figure 5: Mars map by the HiRISE.

In our simulation, we use the HiRISE map of Jezero crater (see Fig. 5), the
landing site for the Mars 2020 mission. We manually annotated the map for
feature-rich and poor areas in the localizability context.

###  VI-A Single-run Simulation Results

Under the deployment scenario discussed earlier, the start and goal positions
of two agents are different. Assuming the bottom-left corner of the map in
Fig. 5 represents $(0,0,0)$ coordinate, the initial positions of the rover and
copter are set to $(20,~{}20)$ and $(30,~{}70,~{}2)$, and the goal positions
of the rover and copter are set to $(140,~{}140)$ and $(120,~{}140,~{}2)$,
respectively.

Figure 6 shows the selected rover paths and copter measurements planned by the
proposed method under different assumptions on measurement opportunities and
qualities. The rover and copter are operated on 800-by-800 grid in 0.2 [m]
resolution. $n$ denotes the number of the images copter can take. The
resolution of the camera influences the amount of uncertainty reduction given
the measurement altitude. The blue solid line and the orange dashed line
represent the initial rover path and the updated rover path using iterative
planner, respectively. The red solid line shows the copter body path. The red
point and the red square represent the copter position and FOV of the copter’s
camera, respectively.

The observation positions (OP) $\\#1$, $\\#2$, and $\\#3$ in Table I denote
the locations where the copter will take images. Table I also shows the
associated reduction rates of the rover’s pose hyper-belief in Eq. (22). Note
that we show two types of copter’s updates with high- and low-resolution
cameras. The reported results show the reduction in rover’s worst-case
odometry error uncertainty. As the number of images captured by the copter
increases, the rover uncertainty decreases.

Figure 6: Where-to-map and iterative rover path results updated by (a) the high-resolution camera and (b) the low-resolution camera of the copter. TABLE I: Observation positions (OP) by the copter and reduction rates of rover’s pose hyper-belief $n$ | OP$\\#1$ [m] | OP$\\#2$ [m] | OP$\\#3$ [m] | Reduction rate [$\%$]
---|---|---|---|---
(High-resolution updates) |
$1$ | (80, 90, 10) | - | - | 14.15
$2$ | (50, 50, 10) | (80, 90, 10) | - | 27.49
$3$ | (50, 50, 10) | (80, 90, 10) | (120, 120, 10) | 40.66
(Low-resolution updates) |
$1$ | (80, 90, 10) | - | - | 7.35
$2$ | (80, 90, 10) | (130, 130, 5) | - | 14.19
$3$ | (50, 50, 10) | (80, 90, 10) | (130, 130, 5) | 20.93

###  VI-B Monte Carlo Simulation Result

This subsection shows a Monte Carlo simulation result of collaborative rover
and copter planning. The number of copter measurements is fixed to $n=1$. We
ran a simulation for each initial position of the rover
$s_{1},s_{2},\cdots,s_{10}$ listed in Table II. We also compared our algorithm
with the baseline random mapping approach in which the copter randomly selects
a point on the rover’s trajectory. We present the average result of 100 runs
for the baseline. Figure 7 and Table II show the optimal paths of rover and
copter and reduction rates by copter observations. Nominally, we obtained
10–20% gain in uncertainty reduction with the proposed algorithm, while the
random mapping approach only gives less than 10% gain, highlighting the
effectiveness of the proposed approach in reducing rover’s localization error
by utilizing the limited resource of the copter.

TABLE II: Reduction rates in MC simulation result Rover’s initial position [m] | Reduction rate [$\%$]
---|---
| Proposed | Baseline
$s_{1}$ | (10, 20) | 13.36 | (6.04)
$s_{2}$ | (20, 20) | 14.15 | (6.59)
$s_{3}$ | (30, 20) | 15.12 | (7.25)
$s_{4}$ | (40, 20) | 15.83 | (7.73)
$s_{5}$ | (50, 20) | 16.37 | (8.16)
$s_{6}$ | (60, 20) | 16.42 | (7.62)
$s_{7}$ | (70, 20) | 15.67 | (7.36)
$s_{8}$ | (80, 20) | 18.38 | (8.39)
$s_{9}$ | (90, 20) | 18.18 | (9.51)
$s_{10}$ | (100, 20) | 18.41 | (10.19)
Figure 7: Monte Carlo Simulation result with various initial rover’s
positions.

##  VII Conclusion

In this paper, we addressed the copter’s where-to-map problem for reducing the
rover odometry error uncertainty using the introduced hyper-covariance
concept. We proposed a copter path planner for determining the points where
the copter will take images while considering the trade-off between the field-
of-view and resolution of its camera. An iterative rover/copter path planning
method is proposed to solve the problem. Finally, we have demonstrated the
effectiveness of the proposed method through the numerical simulation results.
The battery of Mars copter is strictly limited since the flight on Mars needs
high-speed rotor which will consume a lot of power. This leads to strict time
constraints on when the copter flies. As a future work, we will consider when-
to-map problem by the copter and conduct an experiment on the collaborative
rover/copter system.

## References

  * [1] B. Balaram et al., “Mars Helicopter Technology Demonstrator,” in Proc. AIAA Atmos. Flight Mech. Conf., 2018.
  * [2] A. S. McEwen et al., “Mars Reconnaissance Orbiter’s High Resolution Imaging Science Experiment (HiRISE),” J. Geophys. Res. E: Planets, vol. 112, no. 5, 2007.
  * [3] R. E. Arvidson et al., “Relating Geologic Units and Mobility System Kinematics Contributing to Curiosity Wheel Damage at Gale Crater, Mars,” J. Terramechanics, vol. 73, pp. 73–93, 2017.
  * [4] P. Nilsson et al., “Toward Specification-Guided Active Mars Exploration for Cooperative Robot Teams,” Robot.: Sci. Syst., 2018.
  * [5] A.-A. Agha-Mohammadi, S. Agarwal, S.-K. Kim, S. Chakravorty, and N. M. Amato, “SLAP: Simultaneous Localization and Planning for Physical Mobile Robots via Enabling Dynamic Replanning in Belief Space,” IEEE Trans. Robot., vol. 34, no.5, pp. 1195-1214, 2018.
  * [6] H. Inoue, M. Ono, S. Tamaki, and S. Adachi, “Active localization for planetary rovers,” in Proc. IEEE Aerosp. Conf., June, 2016.
  * [7] G. Costante, J. Delmerico, M. Werlberger, P. Valigi, and D. Scaramuzza, “Exploiting Photometric Information for Planning under Uncertainty,” Robot. Res., Springer Proc. in Adv. Robot., vol. 2, 2018.
  * [8] K. Otsu, A.-A. Agha-Mohammadi, and M. Paton, “Where to Look? Predictive Perception With Applications to Planetary Exploration,” IEEE Robot. Autom. Lett., vol. 3, no. 2, pp. 635–642, 2018.
  * [9] J Strader, K. Otsu, and A.-A. Agha-Mohammadi, “Perception-aware Autonomous Mast Motion Planning for Planetary Exploration Rovers,” J. Field Robot., 2020, accepted.
  * [10] R. D. Smallwood and E. J. Sondik, “The Optimal Control of Partially Observable Markov Processes over a Finite Horizon,” Oper. Res., vol. 21, no. 5, pp. 1019–1175, 1973.
  * [11] L. p. Kaelbling, M. L. Littman, and A. R. Cassandra, “Planning and acting in partially observable stochastic domains,” J. Artif. Intell., vol. 101, issues 1-2, pp. 99-134, 1998.
  * [12] A.-A. Agha-Mohammadi et al., “Robust online belief space planning in changing environments: Application to physical mobile robots,” in Proc. IEEE Int. Conf. Robot. Autom., pp. 149–156, 2014.
  * [13] A.-A. Agha-Mohammadi, S. Chakravorty, and N. Amato, “FIRM: Sampling-based Feedback Motion Planning Under Motion Uncertainty and Imperfect Measurements,” Int. J. Robot. Res., vol. 33, no. 2, pp. 268-304, 2014.
  * [14] S.-K. Kim, R. Thakker, and A.-A. Agha-Mohammadi, “Bi-directional Value Learning for Riskaware Planning Under Uncertainty,” IEEE Robot. Autom. Lett., vol. 4, no. 3, pp. 2493-2500, March 2019.
  * [15] M. Maimone, Y. Cheng, and L. Matthies, “Two years of Visual Odometry on the Mars Exploration Rovers,” J. Field Robot., vol. 24, issue 3, pp. 169–186, 2007.
  * [16] H. Hexmoor, S. Rahimi, and J. T. Little, “Coordinated UAV Manoeuvring Flight Formation,” Informatica, vol. 33, no. 3, pp. 375–383, 2009.
  * [17] A. Moses, M. J. Rutherford, and K. P. Valavanis, “Radar-based detection and identification for miniature air vehicles,” in Proc. IEEE Int. Conf. Contr. Appl., 2011, pp. 933–940.
  * [18] C. Titus, C. Siddharth, and S. Davide, “Data-Efficient Decentralized Visual SLAM,” in Proc. IEEE Int. Conf. Robot. Autom., 2018, pp. 933–940.
  * [19] S. Omidshafiei, A.-A. Agha-Mohammadi, C. Amato, S.-Y.. Liu, J. P. How, and J. Vian, “Decentralized Control of Partially Observable Markov Decision Processes using Belief Space Macro-actions,” Int. J. Robot. Res., vol. 36, no. 2, pp. 231-258, 2017.
  * [20] B. Grocholsky, J. Keller, V. Kumar, and G. Pappas, “Cooperative air and ground surveillance - A scalable approach to the detection and localization of targets by a network of UAVs and UGVs,” IEEE Robot. Autom. Mag., vol. 13, no. 3, pp. 16–26, 2006.
  * [21] S. L. Waslander, “Unmanned Aerial and Ground Vehicle Teams: Recent Work and Open Problems,” Autonomous Control Systems and Vehicles, in Springer 2013, pp. 21–36.
  * [22] T. Estlin et al., “An Integrated system for mulit-rover scientific exploration,” in Proc. Nat. Conf. Artif. Intell., 1999, pp. 541–548.
  * [23] K. Tumer and A. Agogino, “Coordinating Multi-Rover Systems: Evaluation Functions for Dynamic and Noisy Environments,” in Proc. 7th Annu. Conf. Genetic Evol. Comput., June 2005, pp. 591–598.
  * [24] M. Pflueger, A.-A. Agha-Mohammadi, and G. S. Sukhatme, “Rover-IRL: Inverse Reinforcement Learning with Soft Value Iteration Networks for Planetary Rover Path Planning,” IEEE Robot. Autom. Lett., vol. 4, no. 2, pp. 1387-1394, 2019.
  * [25] B. Rothrock, J. Papon, R. Kennedy, M. Ono, M. Heverly, and C. Cunningham, “SPOC: Deep learning-based terrain classification for Mars rover missions,” in Proc. AIAA SPACE, 2016\.
  * [26] T. D. Barfoot and P. T. Furgale, “Associating uncertainty with three-dimensional poses for use in estimation problems,” IEEE Trans. Robot., vol. 30, no. 3, pp. 679–693, 2014.

  *[WO]: Wheel Odometry
  *[VO]: Visual Odometry
  *[POMDP]: Partially Observable Markov Decision Process