# 3D Multi-Robot Exploration with a Two-Level Coordination
Strategy and Prioritization

Luigi Freda1 Tiago Novo2 David Portugal2 Rui P. Rocha2
<EMAIL_ADDRESS>of Systems and Robotics of the University of
Coimbra, 3030-290 Coimbra, Portugal {rprocha<EMAIL_ADDRESS>
###### Abstract

This work presents a 3D multi-robot exploration framework for a team of UGVs
moving on uneven terrains. The framework was designed by casting the two-level
coordination strategy presented in [1] into the context of multi-robot
exploration. The resulting distributed exploration technique minimizes and
explicitly manages the occurrence of conflicts and interferences in the robot
team. Each robot selects where to scan next by using a receding horizon next-
best-view approach [2]. A sampling-based tree is directly expanded on
segmented traversable regions of the terrain 3D map to generate the candidate
next viewpoints. During the exploration, users can assign locations with
higher priorities on-demand to steer the robot exploration toward areas of
interest. The proposed framework can be also used to perform coverage tasks in
the case a map of the environment is a priori provided as input. An open-
source implementation is available online.

##  I Introduction

(a)

(b)

(c)

Figure 1: (a) The volumetric map and the frontier tree built by a team of 3
robots. The volumetric map represents free and occupied space and is used to
compute the information gain of each candidate next viewpoint. Nodes with high
information gain are inserted into the frontier tree and updated for planning.
(b) and (c): The traversability and volumetric maps obtained at the end of the
exploration process. The traversability map is used to expand a search tree of
candidate viewpoints and generate new safe paths at any given time.

In the near future, fleets of autonomous robots will be able to flexibly and
cooperatively perform multiple tasks such as exploration, coverage and
patrolling [3, 4]. Amongst these tasks, an exploration mission is crucial for
first assessments and to preliminarily build a model of an unknown
environment. This operation typically requires a higher level of autonomy and
robustness, especially with UGVs or UAVs operating in complex 3D environments
[5, 6, 2, 7, 8].

Specifically, the goal of a team of exploring robots is to cooperatively cover
an unknown environment with sensory perceptions [9]. Typically, the expected
output of exploration is a 3D map of the environment [6] or the discovery of
interesting information/objects [10, 11]. Indeed, multi-robot exploration has
a wide range of potential applications, spanning from search-and-rescue
operations [12, 13, 14] to surveillance [15], mapping [16] and planetary
missions.

In general, a multi-robot system presents many advantages over a single robot
system [17]: completion time is reduced, information sharing and redundancy
can be used to increase the map accuracy and localization quality [18, 16].
Nonetheless, taking advantage of a multi-robot architecture and actually
attaining performance improvements requires the design of sophisticated
strategies which can guarantee _cooperation_ (avoid useless actions) and
_coordination_ (avoid conflicts). In the realm of UGVs, these strategies are
particularly crucial. Indeed, narrow passages (due for example to collapsed
infrastructures or debris [5, 13]) typically generate spatial conflicts
amongst teammates. A suitable strategy is required to _(i)_ minimize
interferences and _(ii)_ recognize and resolve possible incoming deadlocks.
Neglecting the occurrence of such events can hinder UGVs activities or even
provoke major failures.

A team of exploring UGVs has to face many challenges. For instance, the UGVs
are required to navigate over a 3D uneven and complex terrain. Moreover, the
environment may be dynamic and large-scale [19]. In this case, UGVs must
continuously and suitably update their internal representations of the
surrounding scenario to robustly localize and to best adapt their behaviors to
environment changes. Last but not least, in order to collaborate, UGVs must
continuously exchange coordination messages and share their knowledge over an
unreliable network infrastructure. The design of a robust communication
protocol is crucial.

In this paper, we address the aforementioned challenges. We present a multi-
robot exploration framework designed by casting the two-level coordination
strategy presented in [1] into a 3D exploration context. The resulting
distributed technique minimizes and explicitly manages the occurrence of
conflicts and interferences in the team. Each robot selects where to scan next
by using a receding horizon next-best-view approach [2]. Here, a sampling-
based tree is directly expanded on segmented traversable regions of the
terrain 3D map (see Fig. 1). During the exploration, users can assign
locations with higher priorities on-demand to steer the exploration toward
areas of interest. The proposed framework can be also used to perform coverage
tasks in the case a 3D map of the environment is provided a priori as input.
We evaluate our system through both simulation analyses and real-world
experiments. An open-source implementation is available at
github.com/luigifreda/3dmr.

The remainder of this paper is organized as follows. First, Section II
presents and discusses related works. Then, Section III describes the
exploration problem. Next, an overview of our approach is presented in Section
IV. Our two-level exploration strategy is then described in Sections V-B and
VI. Subsequently, Section VII explains how our framework can be used for
coverage tasks. Results are presented in Section VIII. Finally, Section IX
presents our conclusions.

##  II Related Works

In the literature, the problem of covering a scene with sensory perceptions
comes in many flavors. Essentially, a sequence of viewpoints must be planned
in order to gather sensory perceptions over a scene of interest. Three main
specializations can be considered for this _viewpoint planning_ problem: next-
best-view, exploration and coverage.

###  II-A Next-best-view

If the scene consists of a single-object without obstacles, _next-best-view_
planning algorithms are in order [20, 21, 22, 23]. In this case, the goal is
to obtain an accurate 3D reconstruction of an arbitrary object of interest.
These approaches typically sample candidate view configurations within a
sphere around the target object and select the view configuration with the
highest information gain. The downside of these strategies is they do not
scale well in multi-objects scenes.

In this work, the proposed exploration framework uses the same philosophy of
next-best-view approaches. First, candidate views are generated. In our case,
we locally expand a sampling-based tree of candidate views over the
traversable terrain surrounding the robot. Then, the next best view is
selected. As in [2], we first identify the branch $b^{*}$ of the expanded tree
which maximizes the total information gain (as in a receding-horizon scheme)
and then select as next view the nearby node along $b^{*}$ (as in a motion-
predictive approach).

###  II-B Exploration

When the scene is an unknown environment, viewpoint planning is referred to as
_exploration_.

The vast majority of exploration strategies are _frontier-based_ [24]. Here,
the _frontier_ is defined as the boundary between known and unknown territory
and is approached in order to maximize the expected information gain. In most
strategies, a team of robots cooperatively build a metric representation of
the environment, where frontier segments are extracted and used as prospective
viewpoints.

In [25], Burgard et al. presented a frontier-based decentralized strategy
where robots negotiate targets by optimizing a mixed utility function which
takes into account the expected information gain, the cost-to-go and the
number of negotiating robots. In [26], the same decentralized frontier-based
approach extended to a large-scale heterogeneous team of mobile robots.

An incremental deployment algorithm is presented in [27], where robots
approach the frontier while retaining visual contact with each other.

In [28, 29], a sensor-based random graph is expanded by the robots by using a
randomized local planner that automatically performs a trade-off between
information gain and navigation cost.

A interesting class of exploration methods fall under the hat of _active SLAM_
[30, 31, 32, 33] (or integrated exploration [34]), which considers the
correlation between viewpoint selection and the onboard localization/mapping
quality. For instance, in [35, 36], the utility of a particular frontier
region is also considered from the viewpoint of relative robot localization.
Similarly, _belief-space planning_ was proposed to integrate the expected
robot belief into its motion planning [37, 38].

An interesting multi-robot architecture is presented in [39], where robots are
guided through the exploration by a market economy. Similarly, in [40], a
centralized frontier-based approach is proposed in which a bidding protocol is
used to assign frontier targets to the robots.

In [41], an exploration strategy is driven by the resolution of a partial
differential equation. A similar concept is presented in [8]. Here, in order
to solve a stochastic differential equation, Shen et al. use particles as an
efficient sparse representation of the free space and for identifying
frontiers.

Biological-inspired strategies based on Particle Swarm Optimization (PSO) are
presented in [42], where an exploration task is defined through the
distributed optimization of a suitable sensing function.

In [43], an efficient exploration strategy exploits background information
(i.e. a topo-metric map) of the environment in order to improve time
performances.

###  II-C Coverage

Finally, when a prior 3D model of the scene is assigned, the viewpoints
planning problem is known as _coverage_ [44, 45]. In [3], a special issue
collected new research frontiers and advancements on multi-robot coverage
along with multi-robot search and exploration. In [46], Agmon et al. proposed
a strategy for efficiently building a coverage spanning tree for both online
and offline coverage, aiming to minimize the time to complete coverage. In
[47], an efficient frontier-based approach was proposed to solve at the same
time the problems of exploration (maximize the size of the reconstructed
model) and coverage (observe the entire surface of the environment, maximize
the model completeness). In [48, 49], coverage strategies were presented for
inspecting complex structures on the ocean floor by using autonomous
underwater vehicles.

##  III Problem Setup

Table I: Table of symbols. Symbol | Description
---|---
${\cal W}$ | Environment
$T$ | Time interval $[t_{0},\infty)$
${\cal S}$ | Surface terrain in ${\cal W}$
${\cal O}$ | Obstacle region
${\cal C}$ | Configuration space of each robot
${\cal A}_{j}(\bm{q})$ | Region occupied by robot $j$ at configuration $\bm{q}\in{\cal C}$
${\cal V}_{j}(\bm{q})$ | View of robot $j$ at $\bm{q}$
${\cal E}_{j}^{k}$ | Explored region of robot $j$ at step $k$
${\cal E}^{k}$ | Region explored by the robot team at step $k$
${\cal R}^{k}_{j}$ | Exploration-safe region of robot $j$ at step $k$
${\cal Q}_{j}^{k}$ | Informative-safe region of robot $j$ at step $k$

A team of $m\geq 2$ ground exploration robots are deployed in an unknown
environment and are constrained to move on an uneven terrain. The team has to
perform an exploration mission, i.e. cooperatively cover the largest possible
part of the environment with sensory perceptions [9]. The output of the
exploration process is a 3D model of the environment.

###  III-A Notation

In the remainder of this paper, we will use the following convention: Given a
symbol $a^{k}_{j}$, unless otherwise specified, the superscript $k$ denotes a
time index while the subscript $j$ refers to robot $j$. For instance,
$\bm{q}_{j}^{k}$ is the configuration of robot $j$ at exploration step $k$.

A list of the main symbols introduced hereafter is reported in Table I.

###  III-B 3D Environment, Terrain and Robot Configuration Space

Let $T=[t_{0},\infty)\subset\mathbb{R}$ denote a _time interval_ , where
$t_{0}\in\mathbb{R}$ is the starting time. The 3D _environment_ ${\cal W}$ is
a compact connected region in $\mathbb{R}^{3}$. The obstacle region is denoted
by ${\cal O}\subset\mathbb{R}^{3}$. For ease of presentation, we assume ${\cal
O}$ to be static for the moment111In our experiments, we relaxed this
assumption by considering an environment populated by low-dynamic objects
[50]. This is possible by representing the environment with a suitable dynamic
model..

The robots move on a 3D terrain, which is identified as a compact and
connected manifold ${\cal S}$ in ${\cal W}$. The _free environment_ is ${\cal
W}_{\rm free}={\cal W}\setminus\big{(}{\cal O}\cup{\cal S}\big{)}$.

The configuration space $\cal C$ of each robot is the special Euclidean group
$SE(3)$ [51]. In particular, a robot configuration
$\bm{q}=[\bm{p},\bm{\phi}]^{T}\in{\cal C}$ consists of a 3D position $\bm{p}$
of the robot representative centre and a 3D orientation $\bm{\phi}$. ${\cal
A}_{j}(\bm{q})\subset\mathbb{R}^{3}$ denotes the compact region occupied by
robot $j$ at $\bm{q}\in{\cal C}$.

A robot configuration $\bm{q}\in{\cal C}$ is considered _valid_ if the robot
at $\bm{q}$ is safely placed over the 3D terrain $\cal S$. This requires
$\bm{q}$ to satisfy some validity constraints defined according to [52].

A robot path is a continuous function $\bm{\tau}:[0,1]\to{\cal C}$. A path
$\bm{\tau}$ is _safe_ for robot $j$ if for each $s\in[0,1]$: ${\cal
A}_{j}(\bm{\tau}(s))\cap{\cal O}=\emptyset$ and $\bm{\tau}(s)\in{\cal C}$ is a
valid configuration.

We assume each robot in the team is _path controllable_ , that is each robot
can follow any assigned safe path in $\cal C$ with arbitrary accuracy [29].

Figure 2: Left: robot $j$ with its sensor center $s_{j}(\bm{q})$ and the
associated field of view ${\cal F}_{j}(\bm{q})$ at configuration $\bm{q}$.
Right: the view ${\cal V}_{j}(\bm{q})$ and the visible obstacle boundary
${\cal B}_{j}(\bm{q})$.

###  III-C Sensor Model

Each robot of the team is equipped with a 3D laser range-finder rigidly
attached to its body. Teammates are able to localize in a common global map
frame (see [1]). We formalize the laser sensor operation as follows.

Assuming that robot $j$ is at $\bm{q}$, denote by ${\cal
F}_{j}(\bm{q})\subset\mathbb{R}^{N}$ the compact region occupied by its sensor
_field of view_ , which is _star-shaped_ with respect to its _sensor center_
$s_{j}(\bm{q})\in{\cal W}$. In $\mathbb{R}^{2}$, for instance, ${\cal
F}_{j}(\bm{q})$ can be a circular sector with apex $s_{j}(\bm{q})$, opening
angle $\alpha_{s}$ and radius $R_{s}$, where the latter is the perception
range (see Fig. 2, left).

With robot $j$ at $\bm{q}$, a point $\bm{p}\in{\cal W}$ is said to be _visible
from the sensor_ if $\bm{p}\in{\cal F}_{j}(\bm{q})$ and the open line segment
joining $\bm{p}$ and $s_{j}(\bm{q})$ does not intersect $\partial{\cal
O}\cup{\cal S}\cup\partial{\cal A}_{j}(\bm{q})$. At each configuration
$\bm{q}$, the robot sensory system returns a 3D scan with the following
information (see Fig. 2, right):

  * •

the _visible free region_ (or _view_) ${\cal V}_{j}(\bm{q})$, i.e. all the
points of ${\cal W}_{\rm free}$ that are visible from the sensor of robot $j$;

  * •

the _visible obstacle boundary_ ${\cal B}_{j}(\bm{q})=\big{(}\partial{\cal
O}\cup{\cal S}\big{)}\cap\partial{\cal V}_{j}(\bm{q})$, i.e., all points of
$\partial{\cal O}\cup{\cal S}$ that are visible from the sensor.

The above sensor is an idealization of a ‘continuous’ range finder. In our
case, it is a 3D laser range finder, which returns the distance to the nearest
obstacle point along the directions (_rays_) contained in its field of view
(with a certain resolution). Indeed, other sensory systems, such a
stereoscopic camera, satisfy the above description.

###  III-D Exploration task

Robot $j$ explores the world through a sequence of _view-plan-move_ actions.
In order to simplify the notation, we assume the robots synchronize their
actions, i.e. a step $k$ identifies a common time frame $t_{k}\in T$ for all
the robots. The following presentation can be easily adapted to the general
case.

Each configuration where a view is acquired is called a _view configuration_.
Let $\bm{q}_{j}^{0}$ be the initial configuration of robot $j$. Denote by
$\bm{q}_{j}^{1},\bm{q}_{j}^{2},...,\bm{q}_{j}^{k}$ the sequence of view
configurations planned by robot $j$ up to the $k$-th exploration step. When
the exploration starts, all the initial robot $j$ endogenous knowledge can be
expressed as:

${\cal E}_{j}^{0}={\cal A}_{j}(\bm{q}_{j}^{0})\cup{\cal
V}_{j}(\bm{q}_{j}^{0}),$ (1)

where ${\cal A}_{j}(\bm{q}_{j}^{0})$ represents the free volume222Often,
${\cal A}_{j}(\bm{q}_{j}^{0})$ in (1) is replaced by a larger free volume
$\widetilde{{\cal A}}_{j}$ whose knowledge comes from an external source. This
may be essential for planning safe motions in the early stages of an
exploration. that robot $j$ body occupies (computed on the basis of
proprioceptive sensors) and ${\cal V}_{j}(\bm{q}_{j}^{0})$ is the view at
$q_{j}^{0}$ (provided by the exteroceptive sensors). At step $k\geq 1$, the
_explored region_ of robot $j$ is:

${\cal E}_{j}^{k}={\cal E}_{j}^{k-1}\cup{\cal V}_{j}(\bm{q}_{j}^{k}).$

At each step $k$, ${\cal E}_{j}^{k}\subseteq{\cal W}_{\rm free}$ is the
current estimate of the free world that robot $j$ has built. Since safe
planning requires ${\cal A}_{j}(\bm{q}^{k})\subset{\cal E}_{j}^{k-1}$ for any
$k$, we have:

${\cal E}_{j}^{k}={\cal
A}_{j}(\bm{q}_{j}^{0})\cup\left(\bigcup\limits_{i=0}^{k}{{\cal
V}_{j}(\bm{q}_{j}^{i})}\right).$ (2)

If we consider the whole robot team, the overall explored region at step $k$
is:

${\cal E}^{k}=\bigcup\limits_{j=1}^{m}{{\cal
E}_{j}^{k}}=\bigcup\limits_{j=1}^{m}{\cal
A}_{j}(\bm{q}_{j}^{0})\cup\left(\bigcup\limits_{j=1}^{m}\bigcup\limits_{i=0}^{k}{{\cal
V}_{j}(\bm{q}_{j}^{i})}\right).$ (3)

In the above equations, the union operator $\cup$ denotes a data-integration
operation which depends on the adopted spatial model (see Sect. IV-C).

A point $\bm{p}\in{\cal W}_{\rm free}$ is defined _explored_ at step $k$ if it
is contained in ${\cal E}^{k}$ and _unexplored_ otherwise. A configuration
$\bm{q}$ is _exploration-safe_ at step $k$ if ${\cal A}_{j}(\bm{q})\subset{\rm
cl}({\cal E}^{k})$, where cl$(\cdot)$ indicates the set closure operation
(configurations that bring the robot in contact with obstacles are allowed). A
path $\bm{\tau}$ in $\cal C$ is _exploration-safe_ for robot $j$ at step $k$
if for each $s\in[0,1]$ the configuration $\bm{\tau}(s)\in{\cal C}$ is
exploration-safe.

The _exploration-safe region_ ${\cal R}_{j}^{k}$ of robot $j$ collects all the
configurations that are reachable from $\bm{q}_{j}^{0}$ through an
exploration-safe path at step $k$. Indeed, ${\cal R}_{j}^{k}$ represents a
configuration space image of ${\cal E}^{k}$ for robot $j$.

The _view plan_ $\pi_{j}$ of robot $j$ is a finite sequence of view
configuration $\pi_{j}=\\{\bm{q}_{j}^{k}\\}_{k=0}^{l_{j}}$, where $l_{j}$ is
the length of $\pi_{j}$. An _exploration plan_ is a view plan such that
$\bm{q}_{j}^{k}\in{\cal R}_{j}^{k}$ at each step $k$. A _team exploration
strategy_ $\Pi=\\{\pi_{1},...,\pi_{m}\\}$ collects the exploration plans of
the robots in the team. Here, $l={\rm max}(l_{1},...,l_{m})$ is the length of
the strategy $\Pi$.

Exploration objective. The robots must cooperatively plan an exploration
strategy $\Pi$ of minimum length $l$ such that ${\cal E}^{l}$ is maximized. In
particular, ${\cal E}^{l}$ is _maximized_ at step $l$ if there is no robot $j$
that can plan a new view configuration $\bm{q}\in{\cal R}_{j}^{l}$ such that
${\cal E}^{l}\subset({\cal E}^{l}\cup{\cal V}_{j}(\bm{q}))$.

Other factors, such as the resulting map “accuracy” could be taken into
account in the exploration objective. We further develop this in the next
subsection.

General Exploration Method
---
if ${\cal Q}_{j}^{k}\cap{\cal D}(\bm{q}_{j}^{k},k)\neq\emptyset$ %forwarding%
choose new $\bm{q}_{j}^{k+1}$ in ${\cal Q}_{j}^{k}\cap{\cal
D}(\bm{q}_{j}^{k},k)$
move to $\bm{q}_{j}^{k+1}$ and acquire sensor view
else %backtracking%
choose visited $\bm{q}_{j}^{b}$ ($b<k$) such that ${\cal Q}_{j}^{k}\cap{\cal
D}(\bm{q}_{j}^{b},k)\neq\emptyset$
move to $\bm{q}_{j}^{b}$
Figure 3: The $k$-th step of a general exploration method.

###  III-E Exploration Methods

Assume that each robot can associate an information gain $I(\bm{q},k)$ to any
(safe) $\bm{q}$ at step $k$. This is an estimate of the world information
which can be discovered at the current step by acquiring a view from $\bm{q}$.

Consider the $k$-th exploration step, which starts with the robot $j$ at
$\bm{q}_{j}^{k}$. Let ${\cal Q}_{j}^{k}\subset{\cal R}_{j}^{k}$ be the
_informative safe region_ of robot $j$, i.e. the set of configurations which
(i) have non-zero information gain, and (ii) can be reached333The reachability
requirement accounts for possible kinematic constraints to which robot may be
subject to. from $\bm{q}_{j}^{k}$ through a path that is exploration-safe at
step $k$. A general exploration method (Fig. 3) searches for the next view
configuration in ${\cal Q}_{j}^{k}\cap{\cal D}(\bm{q}_{j}^{k},k)$, where
${\cal D}(\bm{q}_{j}^{k},k)\subseteq{\cal C}$ is a compact _admissible set_
around $\bm{q}_{j}^{k}$ at step $k$, whose size determines the scope of the
search. For example, if ${\cal D}(\bm{q}_{j}^{k},k)={\cal C}$, a global search
is performed, whereas the search is local if ${\cal D}(\bm{q}_{j}^{k},k)$ is a
neighborhood of $\bm{q}_{j}^{k}$.

If ${\cal Q}_{j}^{k}\cap{\cal D}(\bm{q}_{j}^{k},k)$ is not empty,
$\bm{q}_{j}^{k+1}$ is selected in ${\cal Q}_{j}^{k}\cap{\cal
D}(\bm{q}_{j}^{k},k)$ according to some criterion (e.g., information gain
maximization). The robot then moves to $\bm{q}_{j}^{k+1}$ to acquire a new
view (_forwarding_). Otherwise, the robot returns to a previously visited
$\bm{q}_{j}^{b}$ ($b<k$) such that ${\cal Q}_{j}^{k}\cap{\cal
D}(\bm{q}_{j}^{b},k)$ is not empty (_backtracking_).

The exploration can be considered _completed_ at step $k$ if ${\cal
Q}_{j}^{k}=\emptyset$, i.e., no informative configuration can be safely
reached.

To specify an exploration method, one must define:

  * •

an information gain;

  * •

a forwarding selection strategy;

  * •

an admissible set ${\cal D}(\bm{q}_{j}^{k},k)$;

  * •

a backtracking selection strategy.

##  IV System Overview

Figure 4: The exploration robot model. Table II: Table of environment models. Environment Models |  Description
---|---
Exploration tree ${\cal K}_{j}$ |  Topological map of the environment: it stores the history of view configurations $\\{\bm{q}^{k}_{j}\\}$ in the form of a tree.
Volumetric map ${\cal H}_{j}$ |  Representation of the explored region ${\cal E}^{k}$: it is used to compute the information gains of candidate view configurations.
Point cloud map ${\cal M}_{j}$ |  Representation of the detected surfaces of the environment: it is used by the path planner to compute safe paths over segmented traversable regions and by the exploration agent to generate candidate view configurations.

In this section, we introduce our approach. Fig. 4 presents the main
components of an exploration robot. This interacts with its environment
through observations and actions, where an observation consists of a vector of
sensor measurements and an action corresponds to a robot actuator command.
Team messages are exchanged with teammates over a network for sharing
knowledge and decisions in order to attain team collaboration.

Decision-making is achieved by the exploration agent and the path planner,
basing on the available information stored in the environment model and the
team model. In particular, the _environment model_ consists of a topological
map, aka exploration tree, and two different metric maps: the volumetric map
and the point cloud map. The _team model_ represents the robot belief about
the current plans of teammates (goals and planned paths).

The main components of the exploration robots are introduced in the following
subsections. Table II reports their symbol along with a short description.

Table III: Table of broadcast messages. Broadcast message |  Content description |  Affected data in the recipient robot $h$
---|---|---
$\langle j,reached,\bm{g}\rangle$ |  robot $j$ has reached goal position $\bm{g}$ |  $j$-th tuple $(\bm{g}_{j},\bm{\tau}_{j},c_{j})$ in team model ${\cal T}_{h}$ is reset
$\langle j,planned,\bm{g}\rangle$ |  robot $j$ has planned node $\bm{g}$ as goal |  $j$-th tuple $(\bm{g}_{j},\bm{\tau}_{j},c_{j})$ in team model ${\cal T}_{h}$ is reset
$\langle j,selected,\bm{g},\bm{\tau},c\rangle$ |  robot $j$ has selected node $\bm{g}$ as goal, $\bm{\tau}$ is the planned path and $c$ the corresponding path length |  $j$-th tuple in team model ${\cal T}_{h}$ is filled with $(\bm{g},\bm{\tau},c)$
$\langle j,aborted,\bm{g}\rangle$ |  robot $j$ has aborted goal node $\bm{g}$ |  $j$-th tuple $(\bm{g}_{j},\bm{\tau}_{j},c_{j})$ in team model ${\cal T}_{h}$ is reset
$\langle j,position,\bm{p}\rangle$ |  robot $j$ shares its current position $\bm{p}$ |  $\bm{p}_{j}$ is set to $\bm{p}$ in the $j$-th tuple of ${\cal T}_{h}$
$\langle j,\emph{scan},\bm{v},\bm{q}\rangle$ |  robot $j$ has acquired new 3D scan data $\bm{v}$ at $\bm{q}$ |  3D scan data is integrated in volumetric map ${\cal H}_{h}$
$\langle j,\emph{tree},{\cal K}_{j}\rangle$ |  robot $j$ shares its exploration tree ${\cal K}_{j}$ |  ${\cal K}_{h}$ is compared with ${\cal K}_{j}$ (cfr. Algorithm 1) to check if a map synchronization with robot $j$ is needed

###  IV-A Exploration Tree and Exploration Agent

During the exploration process (see Sect. III-E), an _exploration tree_ ${\cal
K}_{j}$ is built by robot $j$. A node of ${\cal K}_{j}$ is referred to as
_view node_ and represents a view configuration. An edge between two view
nodes corresponds to a safe path joining them. ${\cal K}_{j}$ is rooted at
$\bm{q}_{j}^{0}$. At each forwarding step, a new node corresponding to
$\bm{q}_{j}^{k+1}$ and a new edge representing a path from $\bm{q}_{j}^{k}$ to
$\bm{q}_{j}^{k+1}$ are added in ${\cal K}_{j}$.

We consider an exploration tree as a topological map representation of the
explored environment. Each node of the tree represents an explored local
region contained within a ball of radius $R_{s}$ centred at the associated
view configuration.

The _exploration agent_ is responsible of online generating an exploration
plan (cfr. Sect. III-E) according to the defined exploration objective (cfr.
Sect. III-E).

###  IV-B Point Cloud Map and Path-Planning

Robot $j$ incrementally builds a 3D point cloud map ${\cal M}_{j}$ as a metric
representation of the detected environment surfaces
$\bigcup\limits_{j=1}^{m}\bigcup\limits_{i=0}^{k}{\cal
B}_{j}(\bm{q}_{j}^{i})$. The points of map ${\cal M}_{j}$ are segmented and
partitioned into two main sets: _obstacles_ points ${\cal M}^{obs}_{j}$ and
_traversable_ points ${\cal M}^{trav}_{j}$ (which can be “back-projected” to a
valid and safe robot configuration [1]). Each point of ${\cal M}_{j}$ is
associated to a _multi-robot traversability cost_ function
${trav:\mathbb{R}^{3}\to\mathbb{R}}$ [1]. In this regard, ${\cal M}_{j}$ and
the traversability cost are used to associate a _navigation cost_
$J(\bm{\tau})$ to each safe path $\bm{\tau}$ [1].

Given the current robot position $\bm{p}_{r}\in\mathbb{R}^{3}$ and a goal
position $\bm{p}_{g}\in{\cal S}$, the _path planner_ computes the safe path
$\bm{\tau}*$ which minimizes the navigation cost $J(\bm{\tau})$ and connect
$\bm{p}_{r}$ with $\bm{p}_{g}$. The path planner reports a failure if a safe
path connecting $\bm{p}_{r}$ with $\bm{p}_{g}$ is not found. See [1] for
further details.

In this work, for simplicity, in order to reach a desired view configuration
$\bm{q}=[\bm{p},\bm{\phi}]^{T}\in{\cal C}$, a robot first plans and follows a
path towards $\bm{p}$, then it turns on itself on ${\cal S}$ so as to assume
the closest orientation to $\bm{\phi}$.

###  IV-C Volumetric Map and Information Gain

A volumetric map ${\cal H}_{j}$ is incrementally built by robot $j$ to
represent the explored region ${\cal E}^{k}$ and associate an information gain
to each safe configuration. Specifically, ${\cal H}_{j}$ is a probabilistic
occupancy gridmap, stored in the form of an Octomap [53]. This partitions the
environment in _free_ , _occupied_ and _unknown_ cells with a pre-fixed
resolution. Such a model allows to appropriately model an environment
populated by low-dynamic objects [50].

The information gain is defined and computed as follows. At step $k$, the
boundary of the explored region $\partial{\cal E}^{k}$ is the union of two
_disjoint_ sets:

  * •

the _obstacle boundary_ $\partial{\cal E}^{k}_{\rm obs}$, i.e., the part of
$\partial{\cal E}$ which consists of detected obstacle surfaces;

  * •

the _free boundary_ $\partial{\cal E}^{k}_{\rm free}$, i.e., the complement of
$\partial{\cal E}^{k}_{\rm obs}$, which leads to potentially explorable areas.

One has $\partial{\cal E}^{k}_{\rm obs}=\bigcup_{i=0}^{k}{\cal B}(\bm{q}^{i})$
and $\partial{\cal E}^{k}_{\rm free}=\partial{\cal
E}^{k}\setminus\partial{\cal E}^{k}_{\rm obs}$.

Let ${\cal V}(\bm{q},k)$ be the _simulated view_ , i.e., the view which would
be acquired from $\bm{q}$ if the obstacle boundary were $\partial{\cal
E}^{k}_{\rm obs}$. The information gain $I(\bm{q},k)$ is a measure of the set
of unexplored points lying in ${\cal V}(\bm{q},k)$. We compute $I(\bm{q},k)$
as the total volume of the unknown cells of ${\cal H}_{j}$ lying in ${\cal
V}(\bm{q},k)$, following the approaches in [54, 55].

MapOverlapCheckAndSync($\langle j,\emph{tree},{\cal K}_{j}\rangle$)

// robot $h$ checks map overlap with robot $j$; robot $h$ compares received
${\cal K}_{j}$ with ${\cal K}_{h}$ and sends robot $j$ missing parts

1 $\\{^{j}\bm{q}_{k}\\}_{k=1}^{l_{j}}\leftarrow{\cal K}_{j}$ // extract view
configurations from ${\cal K}_{j}$; $l_{j}$ is the number of extracted
configurations

2 $\\{^{h}\bm{q}_{k}\\}_{k=1}^{l_{h}}\leftarrow{\cal K}_{h}$ // extract view
configurations from ${\cal K}_{h}$; $l_{h}$ is the number of extracted
configurations

// iterate over extracted configurations of ${\cal K}_{h}$

3 for  _ $r=1$ to $l_{h}$_ do

// given ${}^{h}\bm{q}_{r}$ of ${\cal K}_{h}$, find its closest configuration
in ${\cal K}_{j}$ and the corresponding Euclidean distance

4
$(^{j}\bm{q}^{*},d^{*})\leftarrow\text{arg}\underset{k\in\\{1,...,l_{j}\\}}{\text{min}}\leavevmode\nobreak\
\text{dist}_{3D}(^{h}\bm{q}_{r},^{j}\bm{q}_{k})$ //
$\text{dist}_{3D}(\bm{q}_{1},\bm{q}_{2})$ is the 3D Euclidean distance
$|\bm{q}_{1}^{x,y,z}-\bm{q}_{2}^{x,y,z}|$

5 if  _ $d^{*}>R_{s}$_ then

6 send robot $j$ scan at ${}^{h}\bm{q}_{r}$ // robot $j$ is missing the scan
acquired at ${}^{h}\bm{q}_{r}$

7

8 end if

9

10 end for

return _false_ ;

Algorithm 1 MapOverlapCheckAndSync

###  IV-D Network Model

Each robot can broadcast messages in order to share knowledge, decisions and
achievements with teammates. We assume each of these messages can be lost
during its transmission with a given probability $P_{c}$.

Different types of broadcast messages are used by the robots to convey various
information and attain coordination. In this process, the identification
number (ID) of the emitting robot is included in the heading of any broadcast
message. In particular, a _coordination broadcast message_ is emitted by a
robot in order to inform teammates when it reaches a goal node (_reached_),
planned a goal node (_planned_), selected a goal node (_selected_), abort a
goal node (_aborted_), acquires new laser data (_scan_). Additionally, a
message _tree_ is broadcast in order to enforce the synchronization of the
volumetric maps and point cloud maps amongst teammates (see Sect. IV-E). Table
III summarizes the used broadcast messages along with the conveyed
information/data. The general message format is $\langle
robot\\_id,message\\_type,data\rangle$.

###  IV-E Shared Knowledge Representation and Update

Each robot of the team stores and updates its individual representation of the
world state. In particular, robot $j$ incrementally builds its individual
point cloud map ${\cal M}_{j}$ by using the acquired scans (see [1]). This
allows the path planner to safely take into account explored terrain
extensions and possible environment changes.

At the same time, robot $j$ updates its volumetric map ${\cal H}_{j}$ by
integrating its acquired 3D scans and the _scan_ messages received from
teammates. As mentioned above, some of the scan messages can be lost and
${\cal H}_{j}$ may only partially represent the actual explored region ${\cal
E}^{k}$.

In order to mitigate this problem, each robot continuously broadcasts a _tree_
message at a fixed frequency $1/T_{M}$. In particular, a recipient robot $h$
uses a _tree_ message from robot $j$ to estimate if the map ${\cal H}_{h}$
sufficiently overlaps with ${\cal H}_{j}$ or it missed the integration of some
_scan_ message data. A pseudocode description of this procedure is reported in
Algorithm 1: this verifies if the Euclidean projections of ${\cal K}_{j}$ and
${\cal K}_{h}$ sufficiently overlap each other. If a sufficient overlap is not
verified, a synchronization and merge procedure is triggered between the maps
of the two robots $i$ and $j$ by requesting the missing scans (line 4,
Algorithm 1).

The above information sharing mechanism allows to implement a _shared
information gain_ , i.e. each robot computes the information gain on the basis
of a distributed shared knowledge. This enforces team cooperation by
minimizing unnecessary actions such as exploring regions already visited by
teammates.

###  IV-F Team Model

In order to cooperate with its teammates and manage conflicts, robot $j$
maintains an internal representation of the _planning state of the team_ by
using a dedicated table:

${\cal
T}_{j}=\langle(\bm{p}_{1},\bm{g}_{1},\bm{\tau}_{1},c_{1},t_{1}),...,(\bm{p}_{m},\bm{g}_{m},\bm{\tau}_{m},c_{m},t_{m})\rangle.$
(4)

This stores for each robot $h$: its current position
$\bm{p}_{h}\in\mathbb{R}^{3}$, its selected goal position $\bm{g}_{h}\in{\cal
S}$, the last computed safe path $\bm{\tau}_{h}$ to $\bm{g}_{h}$, the
associated travel cost-to-go $c_{h}\in\mathbb{R}^{+}$ (i.e. the length of
$\bm{\tau}_{h}$), and the timestamp $t_{h}\in T$ of the last message used to
update any portion of $(\bm{p}_{h},\bm{g}_{h},\bm{\tau}_{h},c_{h})$. The table
${\cal T}_{j}$ is updated by using _reached_ , _planned_ , _selected_ ,
_aborted_ and _position_ messages. In particular, _reached_ , _planned_ and
_aborted_ messages received from robot $h$ are used to reset the tuple
$(\bm{g}_{h},\bm{\tau}_{h},c_{h})$ to empty (i.e. no information available). A
_selected_ message sets the tuple $(\bm{g}_{h},\bm{\tau}_{h},c_{h},t_{h})$.
Furthermore, a _position_ message from robot $h$ is used to update its
position estimate in ${\cal T}_{j}$.

An _expiration time_ $\delta t_{e}$ is used to clean ${\cal T}_{j}$ of old and
invalid information. In fact, part of the information stored in ${\cal T}_{j}$
may refer to robots that underwent critical failures or whose connections have
been down for a while. A tuple
$(\bm{p}_{h},\bm{g}_{h},c_{h},\bm{\tau}_{h},t_{h})$ is reset to zero if
$(t-t_{h})>\delta t_{e}$, where $t\in T$ denotes the current time.

##  V Two-Level Coordination Strategy

This section first introduces the notions of topological and metric conflicts
and then presents our two-level coordination strategy (see Sect. V-B). In this
context, _coordination_ (avoid conflicts) and _cooperation_ (avoid inefficient
actions) are crucial team objectives.

###  V-A Topological and Metric conflicts

A _topological conflict_ between two robots $i$ and $j$ is defined with
respect to their exploration trees ${\cal K}_{i}$ and ${\cal K}_{j}$.
Specifically, a _node conflict_ occurs when robot $i$ and $j$ attempt to add a
new view node in the same spatial region at the same time, respectively
$n_{i}$ in ${\cal K}_{i}$ and $n_{j}$ in ${\cal K}_{j}$. We identify this
situation when the corresponding positions
$\bm{p}(n^{i}_{g})\in\mathbb{R}^{3}$ and $\bm{p}(n^{j}_{g})\in\mathbb{R}^{3}$
are closer than a certain distance $D_{g}\leq R_{s}$.

On the other hand, metric conflicts are directly defined in the 3D Euclidean
space where two robots are referred to be in _interference_ if their centres
are closer than a pre-fixed _safety distance_ $D_{s}$. It must hold $D_{s}\geq
2R_{b}$, where $R_{b}$ is the common bounding radius of robots, i.e. the
radius of their minimal bounding sphere. A _metric conflict_ occurs between
two robots if they are in interference or if their planned paths may bring
them in interference444That is, the distance between the closest pair of
points of the two planned paths is smaller than $D_{s}$. .

It is worth noting that topological conflicts may not correspond to metric
conflicts. In our framework, a node may represent a large region that could be
visited by two or more robots at the same time.

Figure 5: The two-level strategy implemented on each robot.

###  V-B Coordination Strategy

Our exploration strategy is distributed and supported on two levels:
topological and metric (see Figure 5).

The exploration agent acts on the _topological_ strategy level by suitably
planning and adding a new view node $n_{g}$ (corresponding to a view
configuration) on its exploration tree. _Cooperation_ is attained by planning
$n_{g}$ on the basis of the shared information gain (see Sect. IV-E).

The path planner acts on the _metric_ strategy level (see Figure 5) by
computing the best safe path from the current robot position to the goal
position $\bm{p}(n_{g})$ using its individual traversable map (see [1]).

The exploration agent guarantees _topological coordination_ by continuously
monitoring and negotiating possibly incoming node conflicts (see Sect. VI). In
case two or more robots plan view configurations close to each other (node
conflict), the robot with the smaller path length actually goes, while the
other robots stop and re-plan a new view node.

The path planner guarantees _metric coordination_ by applying a multi-robot
traversability function. This induces prioritized path planning, in which
robots negotiate metric conflicts by preventing their planned paths from
intersecting (see [1]).

The continuous interaction between the exploration agent and the path planner
plays a crucial role. The exploration agent assigns desired _goals_ to the
path planner. The latter in turn continuously replies with a _feedback_
informing the former about its state and computations. In particular, when the
robot moves towards $\bm{p}(n_{g})$, the path planner continuously re-plans
the best traversable path until the robot reaches the assigned goal. During
this process, if a safe path is not found, the path planner stops the robot,
informs the exploration agent of a _path planning failure_ , and then the
exploration agent re-plans a new view node. On the other hand, every time the
path planner computes a new safe path, its length is used by the exploration
agent to resolve possible node conflicts.

In our view, the two-way strategy approach allows to reduce interferences and
manage possible deadlocks. In fact, while the exploration agent focuses on the
most important exploration aspects (shared information gain maximization and
node conflicts resolution), the path planner takes care of possible incoming
metric conflicts. Moreover, where the path planner strategy may fail alone in
arbitrating challenging conflicts, the exploration agent intervenes and
reassigns tasks to better redistribute robots over the environment. These
combined strategies minimize interferences by explicitly controlling node
conflicts and planning on the multi-robot traversable map.

##  VI The Exploration Agent

ExplorationAgent(_robot_id_ $j$)

1 _current_node_ $\leftarrow$ new node at current robot configuration // init
current node, it represents $\bm{q}^{k}$

2 ${\cal K}_{j}$._root_ $\leftarrow$ _current_node_ // init exploration tree
${\cal K}_{j}$, set its root node

3 _goal_ $\leftarrow\emptyset$ // init goal, it represents the next view node
$\bm{q}^{k+1}$

4 _is_goal_reached_ $\leftarrow$ _true_

5 while  _true_ do

6 Update() // update data structures and boolean variables

7 if  _is_goal_reached_ then

8 ${\cal K}_{j}$ $\leftarrow$ add _goal_ as new node and create a new edge
between _current_node_ and _goal_

9 _current_node_ $\leftarrow$ _goal_ // set goal as current node in the
exploration tree

10 broadcast _goal_ is _reached_

11 _goal_ $\leftarrow$ PlanNextBestView(_current_node_) // plan next view node

12 broadcast _goal_ is _planned_

13 send _goal_ to path planner as goal

14

15 else

16 if  _(is_path_planning_failure or is_node_conflict) _ then

17 send _goal-abort_ to path planner

18 broadcast _goal_ is _aborted_

19 _goal_ $\leftarrow$ PlanNextBestView(_current_node_) // plan next view node

20 broadcast _goal_ is _planned_

21 send _goal_ to path planner as goal

22

23 else

24 broadcast _goal_ is _selected_ // emit the select message at each step for
robustness

25 sleep for $T_{sleep}$

26

27 end if

28

29 end if

30

31 end while

Algorithm 2 ExplorationAgent (in robot $j$)

Update()

1 update volumetric map ${\cal H}_{j}$ by using received _scan_ messages and
last acquired scan

2 update point cloud map ${\cal M}_{j}$ by using last acquired scan

3 update team model ${\cal T}_{j}$ by using _reached_ , _planned_ , _selected_
, _aborted_ messages

4 _is_node_conflict_ $\leftarrow$ check if the goal of another robot is closer
than $D_{g}$ to current _goal_

5 _is_goal_reached_ $\leftarrow$ check if current goal node has been reached

6 _is_path_planning_failure_ $\leftarrow$ check if path planner failed to
compute a path to current goal node

Algorithm 3 Update (in robot $j$)

PlanNextBestView(_current_node_)

1 _next_node_ $\leftarrow\emptyset$ // init next view node

2 _box_ $\leftarrow\emptyset$ // init search bounding box where to constrain
admissible set construction

3 _bounding_boxes_
$\leftarrow\\{\emph{box}_{1},\emph{box}_{2},...,\emph{box}_{l}\\}$ // pre-
defined sequence of increasing bounding boxes

4 foreach  _box in bounding_boxes _ do

5 ${\cal D}$ $\leftarrow$ BuildSearchTree(_current_node_ , _box_) // build the
admissible set as a tree contained in bounding box

6 $\bm{q}^{*}$ $\leftarrow$ select node with maximum utility in ${\cal D}$ //
receding-horizon optimization, equivalent to select "best" branch

7 if  _ $U(\bm{q}^{*})>U_{min}$_ then

8 branch $\leftarrow$ extract the branch of ${\cal D}$ containing $\bm{q}^{*}$
// "best" branch extraction

9 _next_node_ $\leftarrow$ select the node of _branch_ at prefixed graph-
distance $\delta_{D}$ from ${\cal D}$._root_ // forwarding step

10 break // exit from foreach loop

11

12 end if

13

14 end foreach

15if  _next_node $==\emptyset$_ then

16 _next_node_ $\leftarrow$ BackTrackingStrategy() // backtracking step

17

18 end if

19return _next_node_

Algorithm 4 PlanNextBestView (in robot $j$)

BuildSearchTree(_current_node_ , _box_)

1 ${\cal D}$._root_ $\leftarrow$ _current_node_ // init search tree ${\cal
D}$, set its root node

2 ${\cal D}$ $\leftarrow$ expand a sampling-based tree on the surface ${\cal
M}^{trav}_{j}\cap\emph{box}$

3 compute information $I(\bm{q})$ and utility $U(\bm{q})$ for each sampled
configuration $\bm{q}$ in ${\cal D}$

4 if $I(\bm{q})>I_{min}$ then add $\bm{q}$ to the frontier tree ${\cal Y}_{j}$

5 return ${\cal D}$

Algorithm 5 BuildSearchTree (in robot $j$)

BacktrackingStrategy(_current_node_)

1 update information gain of each frontier node in the frontier tree ${\cal
Y}_{j}$

2 build frontier node clusters and compute the cluster centroids in ${\cal
Y}_{j}$

3 compute utility of each frontier cluster centroid // according to Sect. VI-E

4 return best frontier cluster centroid

Algorithm 6 BacktrackingStrategy (in robot $j$)

In this section, we present in detail the exploration agent algorithm. A
pseudocode description is reported in Algorithm 2. Some comments accompany the
pseudocode with the aim of rendering it self-explanatory.

An exploration agent instance runs on each robot. It takes as input the robot
ID $j$. A main loop supports the exploration algorithm (lines 5–26). First,
all the main data structures along with the boolean variables555We use an “
_is__ ” prefix to denote boolean variables. are updated (line 6). This update
takes into account all the information received from teammates and recast the
distributed knowledge. When the current goal is reached (line 7), it is added
as a new node in ${\cal K}_{j}$. A new edge is created from the current node
$\bm{q}^{k}$ to the new node $\bm{q}^{k+1}$. Next, a new node is planned and
sent to the path planner as the next goal position (line 13). During this
steps, the robot informs the team about its operations by using broadcast
messages (line 10 and 12).

On the other hand, if the robot is still reaching the current goal node, lines
15–24 are executed. If either a path planner failure or a node conflict (see
Section VI-B) occurs (line 15) then the exploration agent sends a goal abort
to the path planner and broadcasts its decision (lines 16–17). This triggers a
new node selection (line 18). Otherwise (line 22), a selected message is
emitted for sake of robustness (aiming at maximizing the probability the
message is actually received by all teammates). Then, a sleep for a pre-fixed
time $T_{sleep}$ allows the robot to continue its travel towards the planned
goal (line 23).

It is worth noting that the condition at line 15 of Algorithm 2 implements
continuous monitoring and allows robots to modify their plan while moving.

###  VI-A Data Update

The Update() function is summarized in Algorithm 3. This is in charge of
refreshing the robot data structures presented in Sect. IV. Indeed, these
structures are asynchronously updated by callbacks which are independently
triggered by new teammates broadcast messages or path planner feedback
messages.

Lines 1–3 of Algorithm 3 represent the asynchronous updates of the volumetric
map ${\cal H}_{j}$, the point cloud map ${\cal M}_{j}$ and of the team model
${\cal T}_{j}$. The remaining lines describe how the reported boolean
variables are updated depending on the information stored in the team model
and received from path planner feedback.

###  VI-B Node Conflict Management

The concept of topological node conflict was defined in Section V-A. During an
exploration process, this occurs when (i) two robots select their goals at a
distance closer than a pre-fixed value $D_{g}$ or (ii) when a robot selects
its goal at distance closer than $D_{g}$ from a teammate position. The first
case is referred to as _goal-goal conflict_ , while the latter is called
_goal-start conflict_.

A robot checks for node conflicts by solely using the information stored in
its individual team model. Robot $j$ can detect a node conflict with robot $i$
only if, in its team model ${\cal T}_{j}$, the tuple
$(\bm{p}_{i},\bm{g}_{i},c_{i},\bm{\tau}_{i},t_{i})$ is valid (see Sect. IV-F).

In particular, robot $j$ detects a goal-goal conflict with robot $i$ if the
following conditions are verified:

  1. 1.

a path connecting the goals $\bm{g}_{j}$ with $\bm{g}_{i}$ exists on the local
traversability map of robot $j$, and its length is smaller than $D_{g}$.

  2. 2.

$c_{j}$ is higher than $c_{i}$ in ${\cal T}_{j}$, or $j>i$ in the unlikely
case the navigation costs are equal (robot priority by ID as fallback).

This entails that the priority is given to the robot with the smallest
navigation cost.

Robot $j$ detects a goal-start conflict with robot $i$ if a path connecting
the goal $\bm{g}_{j}$ with the position $\bm{p}_{i}$ exists on the local
traversability map of robot $j$, and its length is smaller than $D_{g}$.

When one of the two above conflict cases occurs, the boolean variable
is_node_conflict is set to true by robot $j$ (line 3 of Alg. 3). As a
consequence, its current goal is aborted and a new node is planned (lines
16–20 of Alg. 2).

(a)

(b)

Figure 6: (a) A robot, its point cloud map and the expanded search tree during
an exploration step. Red points represent the segmented obstacles points
${\cal M}^{obs}_{j}$. Green points represent the traversable points ${\cal
M}^{trav}_{j}$. The volumetric map is not shown. The small arrows represent
candidate view configurations: the lighter the yellow, the higher the
corresponding utility value. (b) Another example with a pioneer robot and its
seach tree. In both pictures, the nodes marked with a yellow disk represent
the candidate view nodes with the highest utility (best branch) and the nodes
marked in green represent the selected next view nodes at pre-fixed graph
distance.

###  VI-C Next Best View Selection Strategy

This section describes the PlanNextBestView() function, whose pseudocode is
reported in Algorithm 4. This function is responsible of generating an
admissible set ${\cal D}$ and selecting the next best view configuration in
${\cal D}$ (forwarding strategy, cfr. Sect. III-E).

In order to achieve efficiency and a quick response time, a windowed search
strategy is adopted. In particular, in Algorithm 4, the main loop (lines 4–12)
guides the search of the next best view through an increasing sequence of
bounding boxes $\\{\emph{box}_{i}\\}_{i=1}^{l}$, where
$\emph{box}_{i}\subset\emph{box}_{i+1}$ (lines 4–9).

At each iteration (line 4, Algorithm 4), an admissible set ${\cal D}$ is
created in the current bounding box (line 5), in the form of a tree by using
the BuildSearchTree() function. In the following, we refer to ${\cal D}$ as
the _search tree_. A utility $U(\bm{q})$ (see Sect. VI-D) and an information
gain $I(\bm{q})$ (see Sect. IV-B) are associated to each candidate
configuration $\bm{q}$ in ${\cal D}$. The next best view configuration
$\bm{q}^{*}$ is computed as the one maximizing the utility in ${\cal D}$ (line
7).

If the utility $U(\bm{q}^{*})$ is greater than a minimum threshold $U_{min}$,
then $\bm{q}^{*}$ is actually used for a forwading step (line 8–9). On the
other hand, if a valid next view configuration is not found (line 13) then the
robot performs a backtracking step (line 14, Algorithm 6, Sect. VI-E).

The selection of the next view node is performed at line 9 of Algorithm 4)
along the best branch of the search tree (see next section).

###  VI-D Search Tree Construction

The construction of the admissible set ${\cal D}$ (line 5, Algorithm 4) is
performed by the BuildSearchTree() function, whose pseudocode is introduced in
Algorithm 5. Each node $n_{h}$ of the search tree ${\cal D}$ represents a
candidate view configuration $\bm{q}_{h}$, while an edge represents a safe
path $\bm{\tau}$ in ${\cal C}$ between the two joined nodes (see Fig. 6).

The function BuildSearchTree() expands a sampling-based tree $\cal D$ on the
set of traversable points ${\cal M}^{trav}_{j}\cap\emph{box}$. The root of
$\cal D$ is initialized at the _current_node_. The tree is grown by using a
breadth-first expansion over ${\cal M}^{trav}_{j}$ (line 2, Algorithm 5). The
use of the traversable points constrains the tree to automatically grow over a
cloud of reachable candidate configurations.

Let $n_{h}$ denote a node in $\cal D$ and denote by $b$ its corresponding
branch, i.e. the path in $\cal D$ which brings from the root to $n_{h}$. The
utility of $n_{h}\in{\cal D}$ is computed as the total information gain which
would be attained by a robot when moving towards $n_{h}$ along $b$.
Specifically, the _utility_ $U(n_{h})$ of a node $n_{h}\in{\cal D}$ with
parent node $n_{h-1}$ is recursively defined as follows:

$U(n_{h})=U(n_{h-1})+I(n_{h})e^{-\lambda\sigma(n_{h})},$ (5)

where $\sigma(n_{h})=\sigma(n_{h-1})+\|\bm{q}_{h}-\bm{q}_{h-1}\|$ represents
the geometric length of the path connecting the root of $\cal D$ to the node
$n_{h}$ [2].

Such a definition of the utility function entails a receding-horizon
optimization approach where, in fact, the “best” branch (and not necessarily
the best configuration) is selected (line 8, Algorithm 4). Next, the robots
moves along a pre-fixed number of edges along the selected branch.

###  VI-E Backtracking Strategy

The backtracking strategy is achieved by building and updating the following
data structures.

  * •

_Frontier nodes_ : nodes whose information gain is greater than a prefixed
threshold $I_{min}$ at the present time. During each BuildSearchTree()
expansion, each new node of ${\cal D}$ that is a frontier node is added to the
frontier tree.

  * •

_Frontier tree_ ${\cal Y}_{j}$: it is incrementally expanded by attaching each
new found frontier node to its nearest neighbor in the tree. To this aim, a
k-d tree is efficiently used and maintained. Frontier nodes that are inserted
in the frontier tree and subsequently become normal nodes (i.e. with $I\leq
I_{min}$) are kept within the frontier tree to preserve its connectivity.

  * •

_Frontier clusters_ : Euclidean clusters of close frontier nodes belonging to
the frontier tree. These clusters are computed by using a pre-fixed clustering
radius. Each cluster has an associated total information gain (the sum of the
information gains of all the nodes in the cluster) and a corresponding
representative centroid.

When a backtracking step is in order (see Algorithm 6), first the frontier
clusters are built. Then, for each frontier cluster $j$, the navigation cost-
to-go $c_{j}$ of its centroid (from the current robot position) and its total
information gain $I_{j}$ are computed. Next, the utility of each cluster is
computed as:

$U_{j}=I_{j}e^{-\lambda c_{j}}$ (6)

similarly to eq. (5). Finally, the cluster centroid with the maximum utility
is selected and planned as next best view node.

While moving towards a selected backtracking node, the robot periodically runs
the backtracking strategy (Algorithm 6) to confirm its last backtracking plan
remains valid and informative along the way. This allows the robot to promptly
take into account new covered regions (due to new asynchronous teammate scans)
and avoid unnecessary travels.

###  VI-F Prioritized Exploration

Robot $j$ uses a _priority queue_ ${\cal P}_{j}$ in order to store a set of
Point Of Interests (POI) with their associated priorities. The higher the
priority the more important the POI in ${\cal P}_{j}$. At each exploration
step, the most important point of interest is selected in ${\cal P}_{j}$ and
used to bias the robot search for the next view configuration.

In particular, the priority queue can be manually created by the end-user or
can be autonomously generated by an object detection module. In this second
case, the module automatically detects and classifies objects in the
environment and assigns them priorities according to a pre-fixed class-
priority table.

Let $\bm{p}^{*}$ be the POI with the highest priority in ${\cal P}_{j}$. In
BuildSearchTree() (see Algorithm 5), a randomized A* expansion is biased
towards $\bm{p}^{*}$. Specifically, at each step of such A* expansion, a coin
toss with probability 0.5 decides if a heuristic function with input
$\bm{p}^{*}$ is used or not to bias the breadth-first expansion. In this
process, we use a simple Euclidean distance as heuristic function $h(\bm{p})$.
The point $\bm{p}^{*}$ is deleted from ${\cal P}_{j}$ once the distance
between the robot and $\bm{p}^{*}$ is smaller than the sensor perception range
$R_{s}$.

##  VII Coverage Task

Coverage and exploration tasks have the same objective: robots have to cover
the environment with sensory perceptions. But while coverage robots have a
full prior knowledge of the environment ${\cal W}$ (which can be used to plan
safe motions), exploration robots must discover ${\cal W}$ online and restrict
their motions within the known terrain.

A _coverage plan_ is a view plan $\pi_{j}$ (see Sect. III-D) composed of view
configurations $\bm{q}_{j}^{k}$ which are valid and reachable through a safe
path from $\bm{q}^{0}_{j}$ (see Sect. III-B). Note that, given the a priori
knowledge of the full environment, a coverage plan
$\pi_{j}=\\{\bm{q}_{j}^{k}\\}$ includes view configurations which are not
necessarily exploration-safe at each step $k$. A _team coverage strategy_
$\Pi=\\{\pi_{1},...,\pi_{m}\\}$ collects the coverage plans of the robots in
the team.

Coverage objective. The robots must cooperatively plan a team coverage
strategy $\Pi$ of minimum length $l$ such that ${\cal E}^{l}$ is coverage-
maximized. In particular, ${\cal E}^{l}$ is _coverage-maximized_ at step $l$
if there is no robot $j$ which can plan a valid configuration $\bm{q}$
reachable through a safe path from $\bm{q}^{0}_{j}$ such that ${\cal
E}^{l}\subset({\cal E}^{l}\cup{\cal V}_{j}(\bm{q}))$.

The team of exploration robots can be used to perform a coverage task. To this
aim, we provide them with a point cloud map ${\cal M}$ representing the full
environment. Specifically, at $t_{0}$, each robot $j$ sets ${\cal M}_{j}={\cal
M}$ and ${\cal H}_{j}=\emptyset$. During the coverage task, the volumetric map
${\cal H}_{j}$ is grown and maintained as in a normal exploration process: it
is incrementally built in order _(i)_ to represent the regions of the
environment covered so far and _(ii)_ to compute the information gain of
candidate view configurations. The map ${\cal M}_{j}$ is used by the path
planner to plan safe paths all over the modeled environment terrain.

Clearly, such an approach does not take advantage of the full prior knowledge
of the environment, which in principle allows to pre-compute an optimal team
plan. Nonetheless, our framework can be conveniently used when the environment
undergoes low-dynamic changes. In fact, our 3D mapping is capable of
continuously integrating detected changes and our online decision-making
correspondingly adapts robot behaviour to the new environment model.

##  VIII Results

(a)

(b)

Figure 7: (a) The team of 3 pioneer 3-DX robots used in our experiments at the
University of Coimbra. The robots are equipped with two RGBD cameras: the
Orbbec Astra S and the Mynt Eye S1030. (b) A pioneer 3-DX sensor equipment
together with the sensor FOVs. Figure 8: TRADR UGV equipped with multiple
encoders, an IMU, and a rotating laser-scanner.

(a)

(b)

Figure 9: (a): One of the environments that was used for conducting
experiments. (b): The map and robot traveled paths obtained after an
exploration process with a team of 3 pioneer 3-DX robots. Figure 10: A
snapshot of a V-REP simulation while a team of two TRADR robots is exploring
an environment. The two different volumetric maps of the robots are overlapped
and shown in the picture. The robots start at different locations, knowing
their initial mutual positions. As for the volumetric map, only the obstacle
cells are shown. The search tree of each robot is also shown. Each search tree
is expanded only on the traversable portion of the point cloud map.

In this section, we present experimental results we obtained with a ROS-based
implementation of the proposed method.

Before conducting real-world experiments, we used V-REP as simulation
framework [56]. V-REP allows to simulate laser range finders, depth cameras,
odometry noise, and robot tracks with grousers to obtain realistic robot
interactions with the terrain. Fig. 10 shows a phase of a simulation test with
an exploring team of two robots. A video showing a simulated exploration
process with 3 robots is available at www.youtube.com/watch?v=UDddKJck2yE.

###  VIII-A Evaluations at the University of Coimbra

An analysis of the proposed method via both simulation and real-world
experiments was conducted in a Master Thesis project [57]. In this work, a
team of Pioneer 3-DX robots was used to perform both simulations and real-
world experiments (see Fig. 7). In particular, the Pioneer robots were
equipped with two RGBD cameras, the Orbbec Astra S and the Mynt Eye S1030.

Figure 9 shows one of the testing environments together with one of the
obtained maps and the corresponding paths traveled by the robots. A video
showing some phases of the performed exploration experiments is available at
youtu.be/xwCnQtLC0RI.

(a)

(b)

(c)

(d)

Figure 11: (a): the RDM training plant (Rotterdam) used for our first
evaluations. (b): a fireman operating the GUI interface for monitoring path
planning and exploration. (c) and (d): two maps obtained in two different
phases of a collaborative exploration process. Figure 12: An experiment phase
at the RDM training plant. In this figure, point cloud maps and volumetric
maps are overlaid. A team of two UGVs was used during the mission. The cubes
represent the obstacle cells of the volumetric map ${\cal H}_{j}$ built by the
right robot. The green points represent the segmented traversable points
${\cal M}^{trav}_{j}$. The red points represent the segmented obstacle points
${\cal M}^{obs}_{j}$. The two search trees of the robots are shown: these
collect candidate view configurations, represented as small yellow arrows. In
particular the lighter the yellow color of the arrows, the higher the
associated utility value.

###  VIII-B Evaluations with TRADR UGVs

Our exploration framework was also tested on the TRADR UGV robots [58] during
some TRADR exercises in Rotterdam and Mestre. The TRADR UGVs are skid-steered
and satisfy the path controllability assumption. Amongst other sensors, the
robots are equipped with a $360^{\circ}$ spherical camera and a rotating laser
scanner as shown in Fig. 8. The full TRADR system was evaluated by the GB
(Gezamenlijke Brandweer) firefighters end-users in the RDM training plant in
Rotterdam (see Fig. 11a). In particular, Fig. 11b shows the TRADR OCU
(Operator Control Unit) displaying the GUI with a map and robot camera
feedback.

The real multi-robot system is implemented in ROS by using a multi-master
architecture. In particular, the NIMBRO network [59] is used for efficiently
transporting ROS topics and services over a WIFI network. Indeed, NIMBRO
allows to fully leverage UDP and TCP protocols in order to control bandwidth
consumption and avoid network congestion. This design choice was made by the
whole TRADR consortium with the aim of developing a robust multi-robot
architecture, which can flexibly offer many heterogeneous functionalities
[58].

We used the same ROS C++ code in order to run both simulations and
experiments. Only ROS launch scripts were adapted in order to interface the
modules with the multi-master NIMBRO transport layer.

We were able to run different exploration experiments and build a map of the
plant with two UGVs. Figures 11 and 12 show different stages of an exploration
process. During those runs the two UGVs started at the same location. In order
to attain a multi-robot localization, a background process was designed to
globally align the two robot maps in 3D and, in case of success, provide
corrections to the mutual position estimates. This system allowed the robots
to generate new maps of the facility in minutes. The pose graphs of the two
registered maps were mutually consistent (up to a roto-translation) and did
not present strong distributed deformations.

###  VIII-C Code Implementation

All the modules are implemented in C++ and use ROS as a middleware. The code
has been designed to seamlessly interface with both simulated and real robots.
This allows to use the same code both in simulations and experiments.

For the implementation of our exploration agent, we used the ROS package
nbvplanner as a starting point [60]. This was specifically designed for 3D
exploration with UAVs. In particular, in the accompanying paper [2], the
Authors present a single-robot strategy. We significantly modified the core of
this package in order to manage a team of UGVs, implement our exploration
agent algorithm, tightly integrate the agent module with the path planner and
traversability analysis, and interface the 3D GUI in our network architecture.

An open-source implementation is available at github.com/luigifreda/3dmr.

###  VIII-D Software Design

Figure 13: The functional diagram of the multi-robot system. Robots share the
same internal software architecture. The legend on the top left represents the
different kind of exchanged messages.

A functional diagram of the presented multi-robot system is reported in Fig.
13. The main blocks are listed below.

The robots, each one with its own ID $\in\\{1,...,m\\}$, have the same
internal architecture and host the on-board functionalities that concern
decision and processing aspects both at the topological level and at the
metric level.

The core services, hosted in the main central computer, represent the multi-
robot system persistence. These allow specific modules to load and save maps
and robot trajectories (along with other TRADR data structures).

The core modules, also hosted in the central computer, include the exploration
monitor. This continuously checks the current status of the exploration
activities and records relevant data.

The multi-robot 3D GUI hosted on one OCU is based on rviz and allows the user
_(i)_ to build an _exploration fence_ , which consists of a set of points
defining a polygonal bounding region and limiting the explorable area _(ii)_
to visualize relevant point cloud data, maps, and robot models _(iii)_ to
stop/restart robots when needed _(iv)_ to trigger the loading/saving of maps
and robot trajectories.

Each robot hosts its instance of the exploration agent and of the path-
planner. Hence, the implemented exploration algorithm is fully distributed.

As illustrated in Fig. 13, the various modules in the architecture exchange
different kinds of messages. These can be mainly grouped into the following
types.

  * •

Coordination messages: these are mainly exchanged amongst robots in order to
achieve coordination and cooperation. For convenience, the exploration monitor
records an history of these messages.

  * •

GUI messages: these are exchanged with the 3D GUI and include both control
messages and visualization data.

  * •

Load/save messages: these are exchanged with the core services and contain
both loaded and saved data.

##  IX Conclusions

We have presented a 3D multi-robot exploration framework for a team of UGVs
navigating in unknown harsh terrains with possible narrow passages. We cast
the two-level coordination strategy presented in [1] in the context of
exploration. An analysis of the proposed method through both simulations and
real-world experiments was conducted [57].

We extended the receding-horizon next-best-view approach [2] to the case of
UGVs by suitably adapting the expansion of sampling-based trees directly on
traversable regions. In this process, an iterative windowed search strategy
was used to optimize the computation time. In our tests, the proposed frontier
trees and backtracking strategy proved to be crucial to effectively complete
exploration experiments without leaving unvisited regions. The approach of
separating the resolution of topological and metric conflicts showed to be
successful also in this context [1]. The resulting strategy is distributed and
succeeds to minimize and explicitly manage the occurrence of conflicts and
interferences in the robot team.

A prioritization scheme was integrated into the framework to steer the robot
exploration toward areas of interest on-demand. This design was suggested by
TRADR end-users to improve their control of the exploration progress and
better integrate human and robot decision-making during a real mission. The
presented framework can be used to perform coverage tasks in the case a map of
the environment is a priori provided as input.

The conducted experiments proved that our exploration method has a competitive
performance and allows appropriate management of a team of robots.
Decentralization allows task reallocation and recovery when robot failures
occur. Notably, the proposed approach showed to be agnostic and not specific
to a given sensing configuration or robotic platform.

We publish the source code of the presented framework with the aim of
providing a useful tool for researchers in the Robotics Community. In the
future, we plan to improve the system communication protocol and to validate
the framework on larger teams of robots. Moreover, we aim to conduct a
comprehensive comparison and performance evaluation of the proposed
exploration method against other state-of-the-art 3D exploration approaches
under the same conditions.

## References

  * [1] L. Freda, M. Gianni, F. Pirri, A. Gawel, R. Dubé, R. Siegwart, and C. Cadena, “3d multi-robot patrolling with a two-level coordination strategy,” _Autonomous Robots Journal_ , vol. 43, 10 2019.
  * [2] A. Bircher, M. Kamel, K. Alexis, H. Oleynikova, and R. Siegwart, “Receding horizon ”next-best-view” planner for 3d exploration,” in _Robotics and Automation (ICRA), 2016 IEEE International Conference on_. IEEE, 2016, pp. 1462–1468.
  * [3] G. A. Kaminka and A. Shapiro, “Editorial: Annals of mathematics and artificial intelligence special issue on multi-robot coverage, search, and exploration,” _Annals of Mathematics and Artificial Intelligence_ , vol. 52, no. 2, pp. 107–108, Apr 2008.
  * [4] D. Portugal and R. P. Rocha, “Performance estimation and dimensioning of team size for multirobot patrol,” _IEEE Intelligent Systems_ , vol. 32, no. 6, pp. 30–38, 2017.
  * [5] I. Kruijff-Korbayová, L. Freda, M. Gianni, V. Ntouskos, V. Hlaváč, V. Kubelka, E. Zimmermann, H. Surmann, K. Dulic, W. Rottner, _et al._ , “Deployment of ground and aerial robots in earthquake-struck amatrice in italy (brief report),” in _Safety, Security, and Rescue Robotics (SSRR), 2016 IEEE International Symposium on_. IEEE, 2016, pp. 278–279.
  * [6] H. Surmann, A. Nüchter, and J. Hertzberg, “An autonomous mobile robot with a 3d laser range finder for 3d exploration and digitalization of indoor environments,” _Robotics and Autonomous Systems_ , vol. 45, no. 3-4, pp. 181–198, 2003.
  * [7] C. Papachristos, S. Khattak, and K. Alexis, “Uncertainty-aware receding horizon exploration and mapping using aerial robots,” in _Robotics and Automation (ICRA), 2017 IEEE International Conference on_. IEEE, 2017, pp. 4568–4575.
  * [8] S. Shen, N. Michael, and V. Kumar, “Stochastic differential equation-based exploration algorithm for autonomous indoor 3d exploration with a micro-aerial vehicle,” _The International Journal of Robotics Research_ , vol. 31, no. 12, pp. 1431–1444, 2012.
  * [9] L. Freda, G. Oriolo, and F. Vecchioli, “Exploration strategies for general robotic systems,” _DIS Robotics Laboratory Working Paper_ , 2009. [Online]. Available: http://www.dis.uniroma1.it/labrob/pub/papers/SET-WorkingPaper.pdf
  * [10] A. Das, S. Datta, G. Gkioxari, S. Lee, D. Parikh, and D. Batra, “Embodied question answering,” _CoRR_ , vol. abs/1711.11543, 2017. [Online]. Available: http://arxiv.org/abs/1711.11543
  * [11] A. Aydemir, A. Pronobis, M. Göbelbecker, and P. Jensfelt, “Active visual object search in unknown environments using uncertain semantics,” _IEEE Transactions on Robotics_ , vol. 29, no. 4, pp. 986–1002, 2013.
  * [12] F. Colas, S. Mahesh, F. Pomerleau, M. Liu, and R. Siegwart, “3d path planning and execution for search and rescue ground robots,” in _Intelligent Robots and Systems (IROS), 2013 IEEE/RSJ International Conference on_. IEEE, 2013, pp. 722–727.
  * [13] R. R. Murphy, _Disaster robotics_. MIT press, 2014.
  * [14] A. Kleiner, F. Heintz, and S. Tadokoro, “Special issue on safety, security, and rescue robotics (ssrr), part 2,” _Journal of Field Robotics_ , vol. 33, no. 4, pp. 409–410, 2016.
  * [15] S. Bhattacharya, S. Candido, and S. Hutchinson, “Motion strategies for surveillance.” in _Robotics: Science and Systems_ , 2007.
  * [16] R. Dubé, A. Gawel, H. Sommer, J. Nieto, R. Siegwart, and C. Cadena, “An online multi-robot slam system for 3d lidars,” in _Intelligent Robots and Systems (IROS), 2017 IEEE/RSJ International Conference on_. IEEE, 2017, pp. 1004–1011.
  * [17] Z. Yan, N. Jouandeau, and A. A. Cherif, “A survey and analysis of multi-robot coordination,” _International Journal of Advanced Robotic Systems_ , vol. 10, no. 12, p. 399, 2013.
  * [18] I. Rekleitis, G. Dudek, and E. Milios, “Multi-robot collaboration for robust exploration,” _Annals of Mathematics and Artificial Intelligence_ , vol. 31, no. 1-4, pp. 7–40, 2001.
  * [19] C. Cadena, L. Carlone, H. Carrillo, Y. Latif, D. Scaramuzza, J. Neira, I. Reid, and J. Leonard, “Past, present, and future of simultaneous localization and mapping: Towards the robust-perception age,” _IEEE Transactions on Robotics_ , vol. 32, no. 6, p. 1309–1332, 2016.
  * [20] C. Connolly, “The determination of next best views,” in _Robotics and automation. Proceedings. 1985 IEEE international conference on_ , vol. 2. IEEE, 1985, pp. 432–435.
  * [21] J. I. Vasquez-Gomez, L. E. Sucar, R. Murrieta-Cid, and E. Lopez-Damian, “Volumetric next-best-view planning for 3d object reconstruction with positioning error,” _International Journal of Advanced Robotic Systems_ , vol. 11, no. 10, p. 159, 2014.
  * [22] E. Dunn, J. Van Den Berg, and J.-M. Frahm, “Developing visual sensing strategies through next best view planning,” in _Intelligent Robots and Systems, 2009. IROS 2009. IEEE/RSJ International Conference on_. IEEE, 2009, pp. 4001–4008.
  * [23] M. Krainin, B. Curless, and D. Fox, “Autonomous generation of complete 3d object models using next best view manipulation planning,” in _Robotics and Automation (ICRA), 2011 IEEE International Conference on_. IEEE, 2011, pp. 5031–5037.
  * [24] B. Yamauchi, “Frontier-based exploration using multiple robots,” in _2nd Int. Conf. on Autonomous Agents_ , 1998, pp. 47–53.
  * [25] W. Burgard, M. Moors, C. Stachniss, and F. Schneider, “Coordinated multi-robot exploration,” _IEEE Trans. on Robotics and Automation_ , vol. 1, no. 3, pp. 376–386, 2005.
  * [26] A. Howard, L. Parker, and G. Sukhatme, “Experiments with a large heterogeneous mobile robot team: Exploration, mapping, deployment and detection,” _Int. J. Robotics Research_ , vol. 25, no. 5–6, pp. 431–447, 2006.
  * [27] A. Howard, M. Mataric, and S. Sukhatme, “An incremental deployment algorithm for mobile robot teams,” in _2003 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems_ , 2003, pp. 2849–2854.
  * [28] L. Freda and G. Oriolo, “Frontier-based probabilistic strategies for sensor-based exploration,” in _Robotics and Automation, 2005. ICRA 2005\. Proceedings of the 2005 IEEE International Conference on_. IEEE, 2005, pp. 3881–3887.
  * [29] A. Franchi, L. Freda, G. Oriolo, and M. Vendittelli, “The sensor-based random graph method for cooperative robot exploration,” _IEEE/ASME Transaction on Mechatronics_ , vol. 14, no. 2, pp. 163–175, 2009.
  * [30] C. Leung, S. Huang, and G. Dissanayake, “Active slam using model predictive control and attractor based exploration,” in _Intelligent Robots and Systems, 2006 IEEE/RSJ International Conference on_. IEEE, 2006, pp. 5026–5031.
  * [31] A. Kim and R. M. Eustice, “Active visual slam for robotic area coverage: Theory and experiment,” _The International Journal of Robotics Research_ , vol. 34, no. 4-5, pp. 457–475, 2015.
  * [32] L. Carlone, J. Du, M. K. Ng, B. Bona, and M. Indri, “Active slam and exploration with particle filters using kullback-leibler divergence,” _Journal of Intelligent & Robotic Systems_, vol. 75, no. 2, pp. 291–311, 2014.
  * [33] R. Valencia and J. Andrade-Cetto, “Active pose slam,” in _Mapping, Planning and Exploration with Pose SLAM_. Springer, 2018, pp. 89–108.
  * [34] A. Makarenko, S. B. Williams, F. Bourgault, and H. F. Durrant-Whyte, “An experiment in integrated exploration,” in _IEEE/RSJ Int. Conf. on Intelligent Robots and Systems_ , vol. 1, 2002, pp. 534–539.
  * [35] J. Ko, B. Stewart, D. Fox, K. Konolige, and B. Limketkai, “A practical, decision-theoretic approach to multi-robot mapping and exploration,” in _2003 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems_ , 2003, pp. 3232–3238.
  * [36] L. Freda, F. Loiudice, and G. Oriolo, “A randomized method for integrated exploration,” in _Intelligent Robots and Systems, 2006 IEEE/RSJ International Conference on_. IEEE, 2006, pp. 2457–2464.
  * [37] G. Costante, C. Forster, J. Delmerico, P. Valigi, and D. Scaramuzza, “Perception-aware path planning,” _arXiv preprint arXiv:1605.04151_ , 2016\.
  * [38] V. Indelman, L. Carlone, and F. Dellaert, “Planning in the continuous domain: A generalized belief space approach for autonomous navigation in unknown environments,” _The International Journal of Robotics Research_ , vol. 34, no. 7, pp. 849–882, 2015.
  * [39] R. Zlot, A. Stenz, M. Dias, and S. Thayer, “Multi-robot exploration controlled by a market economy,” in _2002 IEEE Int. Conf. on Robotics and Automation_ , 2002, pp. 3016–3023.
  * [40] R. Simmons, D. Apfelbaum, W. Burgard, D. Fox, M. Moors, S. Thrun, and H. Younes, “Coordination for multi-robot exploration and mapping,” in _17th AAAI/12th IAAI_ , 2000, pp. 852–858.
  * [41] R. Shade and P. Newman, “Choosing where to go: Complete 3d exploration with stereo,” in _Robotics and Automation (ICRA), 2011 IEEE International Conference on_. IEEE, 2011, pp. 2806–2811.
  * [42] M. S. Couceiro, R. P. Rocha, and N. M. Ferreira, “A novel multi-robot exploration approach based on particle swarm optimization algorithms,” in _Safety, Security, and Rescue Robotics (SSRR), 2011 IEEE International Symposium on_. IEEE, 2011, pp. 327–332.
  * [43] S. Oßwald, M. Bennewitz, W. Burgard, and C. Stachniss, “Speeding-up robot exploration by exploiting background information,” _IEEE Robotics and Automation Letters_ , vol. 1, no. 2, pp. 716–723, 2016.
  * [44] H. Choset, “Coverage for robotics–a survey of recent results,” _Annals of mathematics and artificial intelligence_ , vol. 31, no. 1-4, pp. 113–126, 2001\.
  * [45] N. Hazon and G. A. Kaminka, “On redundancy, efficiency, and robustness in coverage for multiple robots,” _Robotics and Autonomous Systems_ , vol. 56, no. 12, pp. 1102–1114, 2008.
  * [46] N. Agmon, N. Hazon, G. A. Kaminka, M. Group, _et al._ , “The giving tree: constructing trees for efficient offline and online multi-robot coverage,” _Annals of Mathematics and Artificial Intelligence_ , vol. 52, no. 2-4, pp. 143–168, 2008.
  * [47] L. Heng, A. Gotovos, A. Krause, and M. Pollefeys, “Efficient visual exploration and coverage with a micro aerial vehicle in unknown environments,” in _Robotics and Automation (ICRA), 2015 IEEE International Conference on_. IEEE, 2015, pp. 1071–1078.
  * [48] E. Galceran, R. Campos, N. Palomeras, D. Ribas, M. Carreras, and P. Ridao, “Coverage path planning with real-time replanning and surface reconstruction for inspection of three-dimensional underwater structures using autonomous underwater vehicles,” _Journal of Field Robotics_ , vol. 32, no. 7, pp. 952–983, 2015.
  * [49] B. Englot and F. S. Hover, “Three-dimensional coverage planning for an underwater inspection robot,” _The International Journal of Robotics Research_ , vol. 32, no. 9-10, pp. 1048–1073, 2013.
  * [50] A. Walcott-Bryant, M. Kaess, H. Johannsson, and J. J. Leonard, “Dynamic pose graph slam: Long-term mapping in low dynamic environments,” in _Intelligent Robots and Systems (IROS), 2012 IEEE/RSJ International Conference on_. IEEE, 2012, pp. 1871–1878.
  * [51] S. M. LaValle, _Planning Algorithms_. Cambridge, U.K.: Cambridge University Press, 2006, available at http://planning.cs.uiuc.edu/.
  * [52] A. Haït, T. Simeon, and M. Taïx, “Algorithms for rough terrain trajectory planning,” _Advanced Robotics_ , vol. 16, no. 8, pp. 673–699, 2002.
  * [53] A. Hornung, K. M. Wurm, M. Bennewitz, C. Stachniss, and W. Burgard, “OctoMap: An efficient probabilistic 3D mapping framework based on octrees,” _Autonomous Robots_ , 2013.
  * [54] E. Kruse, R. Gutsche, and F. Wahl, “Efficient, iterative, sensor based 3-D map building using rating functions in configuration space,” in _1996 IEEE Int. Conf. on Robotics and Automation_ , 1996, pp. 1067–1072.
  * [55] H. Gonzalez-Banos and J. Latombe, “Robot navigation for automatic model construction using safe regions,” in _Experimental Robotics VII_ , ser. Lecture Notes in Control and Information Sciences, D. Russ and S. Singh, Eds., vol. 271. Springer, 2000, pp. 405–415.
  * [56] M. F. E. Rohmer, S. P. N. Singh, “V-rep: a versatile and scalable robot simulation framework,” in _Proc. of The International Conference on Intelligent Robots and Systems (IROS)_ , 2013.
  * [57] T. de Brito Novo, “3d multi-robot exploration in irregular terrains,” Master’s thesis, University of Coimbra, Faculty of Sciences and Technology, 2021\. [Online]. Available: https://drive.google.com/file/d/1Iad2mOjQv3iKjawEVX92jozUo4hoOp5o/view
  * [58] I. Kruijff-Korbayová, F. Colas, M. Gianni, F. Pirri, J. Greeff, K. Hindriks, M. Neerincx, P. Ögren, T. Svoboda, and R. Worst, “Tradr project: Long-term human-robot teaming for robot assisted disaster response,” _KI-Künstliche Intelligenz_ , vol. 29, no. 2, pp. 193–201, 2015.
  * [59] M. Schwarz, “nimbro_network - ROS transport for high-latency, low-quality networks,” https://github.com/AIS-Bonn/nimbro_network, 2017, [Online; accessed 20-February-2017].
  * [60] A. Bircher, “nbvplanner - Receding Horizon Next Best View Planning.” https://github.com/ethz-asl/nbvplanner, 2018, [Online; accessed 20-February-2018].