Uncertainty-based Online Mapping and Motion Planning
for Marine Robotics Guidance
Èric Pairet
1
, Juan David Hernández
2
, Morteza Lahijanian
3
, and Marc Carreras
4
Abstract— In real-world robotics, motion planning remains
to be an open challenge. Not only robotic systems are required
to move through unexplored environments, but also their ma-
noeuvrability is constrained by their dynamics and often suffer
from uncertainty. One approach to overcome this problem is
to incrementally map the surroundings while, simultaneously,
planning a safe and feasible path to a desired goal. This
is especially critical in underwater environments, where au-
tonomous vehicles must deal with both motion and environment
uncertainties. In order to cope with these constraints, this
work proposes an uncertainty-based framework for mapping
and planning feasible motions online with probabilistic safety-
guarantees. The proposed approach deals with the motion,
probabilistic safety, and online computation constraints by
(i) incrementally representing the environment as a collection
of local maps, and (ii) iteratively (re)planning kinodynamically-
feasible and probabilistically-safe paths to goal. The proposed
framework is evaluated on the Sparus II, a nonholonomic
torpedo-shaped AUV, by conducting simulated and real-world
trials, thus proving the efficacy of the method and its suitability
even for systems with limited on-board computational power.
I. INTRODUCTION
The usage of autonomous underwater vehicles (AUVs)
has considerably increased in the last decades for scien-
tific, military, and commercial purposes [1]. Some of the
most outstanding applications are in-water ship hull inspec-
tions [2], underwater archaeology [3], seabed exploration [4],
and exploration of inaccessible areas to vessel-based in-
struments [5]. A common issue in all these applications
is the lack of prior information about the environment in
which AUVs have to operate. On top of that, underwater
vehicles suffer from motion uncertainty and, in most cases,
limited manoeuvrability. Thus, addressing the motion con-
straints, and the uncertainties on the vehicle’s motion and
This research project has been conducted in the Computer Vision and
Robotics Institute (VICOROB) at University of Girona (Spain) with the
support of the EXCELLABUST and ARCHROV projects under the Grant
agreements H2020-TWINN-2015, CSA, ID: 691980 and DPI2014-57746-
C3-3-R, respectively. Additionally, this work has been partially supported
by ORCA Hub EPSRC (EP/R026173/1) and consortium partners.
1
È. Pairet is with VICOROB at University of Girona (Spain) and is
currently with the Edinburgh Centre for Robotics at University of Edinburgh
and Heriot-Watt University (UK). eric.pairet@ed.ac.uk
2
J. D. Hernández is with VICOROB at University of Girona (Spain)
and is currently with the Department of Computer Science at Rice Univer-
sity, Houston (TX, USA). His work while at Rice was supported in part by
NSF IIS 1317849. juandhv@rice.edu
3
M. Lahijanian was with the Department of Computer Science at
University of Oxford (UK) and is currently with the Aerospace Engineering
Sciences Department at University of Colorado Boulder (CO, USA). His
work while at Oxford was supported in part by EPSRC Mobile Autonomy
Grant (EP/M019918/1). morteza.lahijanian@colorado.edu
4
M. Carreras is with the VICOROB at University of Girona (Spain).
marc.carreras@udg.edu
Fig. 1: Sparus II AUV, a nonholonomic vehicle.
the environment awareness jointly in a planner is essential
to guarantee the vehicle’s safety. This is a challenging task
because such a planner must be able to continuously modify
the vehicle’s motion plan towards a desired goal according
to the incremental environment awareness, while maintaining
safety guarantees in face of the uncertainties.
In order to deal with kinodynamic motion constraints,
sampling-based motion planners offer great capabili-
ties (e.g., [6]–[8]). At the same time, these planners also
bring the chance to deal with motion uncertainty, for which
there are generally two approaches. One approach that is
popular among existing planners is based on discrete Markov
modelling of the evolution of the system in the environment
and generating a policy over the approximated Markov states.
Examples of such motion planners include stochastic motion
roadmap (SMR) [9] and incremental Markov decision pro-
cess (iMDP) [10]. These methods have shown to be effective
and provide optimality guarantees in terms of probability
of reaching a desired goal; however, they assume perfect
knowledge about the environment. Works such as [11] have
extended these techniques to partially unknown environ-
ments. Nonetheless, their large computational times remain
to be the main hurdle in applications with fully-unknown
environments or online planning requirements.
Another approach to deal with motion uncertainty in plan-
ning is based on chance-constraint method, which focuses on
finding a motion plan that satisfies a given (minimum) safety
probability bound. The challenge lies in the computation of
the safety probability over plans. In [12], linear chance con-
straints are combined with disjunctive linear programming to
perform probabilistic convex obstacle avoidance. This con-
cept was extended and integrated into a sampling-based plan-
ner, leading to the chance constrained RRT (CC-RRT) [13]
and the CC-RRT* [14]. The advantage of these methods is
that satisfying plans can be computed quickly, making them
desirable for online applications. They are, however, built
on strong assumptions and rely on the prior knowledge of a
convex environment with convex obstacles.
To appear in the Proc. of the 2018 IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems.
In the field of underwater robotics, online computation
techniques have been studied to address the problem of
planning in unknown environments, e.g., [15]–[18]. Petil-
lot et al. [15] tracked nearby underwater objects by using
a multi-beam forward-looking sonar to then perform online
obstacle avoidance. Along in this line, Maki et al. [16]
proposed a method to conduct online planning by guiding
an AUV with landmarks. These methods do not provide
any theoretical performance or safety guarantees, and their
evaluations are conducted either in simulation or in a highly
controlled environment. Hence, their performance in real-
istic scenarios is yet to be tested. More recently, Hernán-
dez et al. [17], [18] presented an online framework to plan
paths under motion constraints for AUVs. Notwithstanding
their framework succeeded in solving start-to-goal queries
in unexplored real-world environments, their planner uses
ad-hoc heuristics to estimate the risk associated with the
solution path, and approximates the system’s dynamics with
the Dubins curves. Consequently, that framework also lacks
theoretical analysis and does not provide a measure of
robustness or quantified safety guarantees.
This paper presents a new framework that seeks to over-
come some of the limitations of the preceding works, namely,
the lack of exploitation of the whole system’s dynamic
range and the use of ad-hoc heuristics to ensure the robot’s
safety [17], [18], and the conservativeness and the need of
having a prior convex representation of the environment [12],
[13]. Aiming to guarantee the AUV’s safety when solving
start-to-goal queries in unexplored environments, the pro-
posed framework incrementally maps and plans paths online,
while considering the vehicle’s kinodynamic constraints,
uncertainties in its motion and mapping, and a user-defined
minimum probability of safety. The framework is twofold:
(i) incrementally mapping the vehicle’s surroundings to build
an uncertain environment representation, and (ii) planning
feasible paths (according to the AUV’s motion constraints)
with probabilistic safety guarantees (according to the uncer-
tainty of the system’s motion and environment awareness).
The main contribution of this work is an end-to-end
framework that allows AUVs to navigate online in unknown
environments with safety guarantees by means of online
mapping, online motion planning, and efficient evaluation of
uncertainties. The efficacy of this method has been demon-
strated in both simulated and real-world scenarios on the
Sparus II (Fig. 1), a nonholonomic torpedo-shaped AUV.
The experimental results demonstrate the suitability of the
method to address the aforementioned challenges.
II. SYSTEM DESCRIPTION AND PROBLEM
FORMULATION
The aim of this paper is a framework that allows au-
tonomous navigation of AUVs in unexplored environments
with safety guarantees. Such a framework needs to be able to
simultaneously map the environment and plan paths towards
a desired goal, while accounting for the motion and mapping
uncertainties. These requirements are jointly addressed by
formulating a motion planning problem in the belief space.
That is, given an instance of an uncertain map that is in-
crementally being constructed, find a set of feasible motions
according to the system’s dynamics, drive the system to the
desired goal, and satisfy a minimum probability of safety
in every point in time. Bearing the problem requirements
in mind, this section firstly describes the nonholonomic
robotic system and its associated uncertainty. It then explains
the construction of the uncertain map before detailing the
computation of the probability of collision between these
two uncertain agents. Finally, this section provides a formal
definition of the motion planning problem.
A. Motion Constraints and Uncertainty
In its most generic formulation, the motion model of a
nonholonomic system is defined as:
˙
x = f (x, u), (1)
where x ∈ X ⊆ R
n
x
is the state, u ∈ U ⊆ R
n
u
is
the control input, and f is a smooth differentiable function.
Although in some cases the uncertainty associated with the
motion is not considered, more elaborated formulations that
include the uncertainty allow to better control the system.
In the particular case of a nonholonomic AUV, such an
uncertainty can be estimated as additive Gaussian noise in
the linearized, discrete-time form of (1). That is, by assuming
that each choice of control u is applied for a small or multiple
durations of ∆t, the uncertain motion can be represented by:
x
k+1
= Ax
k
+ Bu
k
+ Gw
k
, (2)
where subscript k represents time t = k∆t, A ∈ R
n
x
×n
x
,
B ∈ R
n
x
×n
u
, G ∈ R
n
x
×n
w
, and w ∈ R
n
w
is a random
variable with normal distribution defined by mean zero and
covariance P
w
, i.e., w ∼ N (0, P
w
).
For the particular case of a torpedo-shaped AUV that
operates at a constant depth, i.e. in a two-dimensional (2D)
workspace W = R
2
, the vehicle’s motion model can be
approximated by a unicycle system:
˙x = v cos(θ), ˙y = v sin(θ),
˙
θ = ω, (3)
where x and y correspond to the Cartesian coordinates of
the system with respect to a predefined reference frame, θ
is the system’s orientation with respect to the x-axis, v is
the vehicle’s forward velocity, and ω is the vehicle’s turning
rate. Thus, the system’s state is defined as x = (x, y, θ)
T
,
and the system’s control input is defined as u = (v, ω)
T
.
Although (3) approximates the AUVs’ behaviour, it does
not consider any source of uncertainty. In coping with this
limitation, (3) is first linearised by using a dynamic feedback
linearisation controller as presented in [19]. This technique
(i) increases the order of the system and defines its state as
z = (x, ˙x, y, ˙y)
T
, and (ii) applies a proportional derivative
(PD) controller on the model to drive the system towards a
desired state r = (x
r
, ˙x
r
, y
r
, ˙y
r
)
T
. Then, the differences
between the real system and the discretised closed-loop
model can be approximated by a Gaussian distribution, and
the closed-loop system can be represented as in (2) with
states z ∈ X = R
n
z
, and controls r ∈ U = X . Thus, state
z
k
is best described by its probability distribution:
z
k
∼ b
k
= N (
ˆ
z
k
, P
z
k
), (4)
where b is referred to as the belief of z, and
ˆ
z ∈ R
n
z
and
P
z
∈ R
n
z
×n
z
are the mean and covariance of b. The set
of all beliefs is called the belief space and denoted by B.
The evolution of belief is then given by the independent
propagation of its mean and covariance as:
ˆ
z
k+1
= A
ˆ
z
k
+ Br
k
,
P
z
k+1
= AP
z
k
A
T
+ P
w
,
(5)
where A ∈ R
n
z
×n
z
and B ∈ R
n
z
×n
z
define the closed-loop
linearised equation of motion with the PD controller, and P
w
is the covariance of noise w as in (2).
B. Environment Uncertainty
Some applications in robotics, such the ones in the under-
water domain, lack a complete awareness of the environment,
either because there is no information of the surroundings
or because of the presence of dynamic elements in the
workspace. This work scopes the mapping requirements to
undiscovered static environments. In order to reveal the
obstacles in the environment, the robot is equipped with a
range-based perception sensor. This setup allows the AUV
to incrementally explore the surroundings as it moves, i.e.
observe and integrate into the map the obstacles when they
are inside the sensor’s detection range.
The range-based sensor uniquely detects points on the
boundary of a nearby obstacle. Assuming no uncertainty
on these observations, yet bearing in mind that the robot’s
location is uncertain with respect to the global frame, the
observed point is represented in the global frame as:
x
O
∼ N (
ˆ
z
k
⊕ d
x
O
, P
z
k
), (6)
where d
x
O
is the detected obstacle point in the robot’s
local frame which is projected to the global frame with the
composition function ⊕. From these uncertain points x
O
, the
robot constructs an uncertain map consisting of the obstacle
space and the free space, i.e. M = X
O
∪ X
F
. The obstacle
density function for point x ∈ X denoted by F
O
(x) is
then the sum of the normally distributed densities for each
detected obstacle, which itself is a normal distribution.
C. Probabilistic Safety Guarantee
The system’s and environment’s uncertainties are jointly
considered to guarantee the vehicle’s safety. More specifi-
cally, the probability of the system being in collision with
an obstacle in the environment at time k is characterised as:
p
collision
(b
k
, M) ≤
Z
X
b
k
(x) F
O
(x) dx
=
Z
X
N (x |
ˆ
z
k
, P
z
k
) F
O
(x) dx. (7)
Given a minimum probability of safety p
safe
, we require
1 − p
collision
(b, M) > p
safe
for every belief b on the path in
order to probabilistically guarantee the robot’s safety.
D. Planning Problem
The planning problem considered in this work seeks a
dynamically feasible path in the belief space B that is prob-
abilistically safe. Formally, let B
goal
⊆ B denote the set of all
belief states that correspond to the desired goal region in the
environment. Then, the constrained planning problem is to
compute a dynamically-feasible path ξ : [0, T ] → B for sys-
tem (2) such that ξ(0) = b
start
∈ B, ξ(T ) ∈ B
goal
, u(t) ∈ U,
and 1 − p
collision
(ξ(t), M) > p
safe
for all t ∈ [0, T ].
III. FRAMEWORK FOR MULTI-CONSTRAINED
ONLINE PATH PLANNING
To efficiently plan paths that meet motion and probabilistic
constraints, while a robot moves through an unexplored envi-
ronment, this work presents the framework depicted in Fig. 2.
Such a framework is threefold: (i) the mapping module
that incrementally builds an uncertainty-aware map, (ii) the
planning module that finds a safe and feasible path towards
the goal, and (iii) the framework manager that coordinates the
framework’s execution while interacting with the vehicle’s
perception sensors and trajectory tracking control.
Fig. 2: Framework for incrementally mapping and planning
under motion and uncertainty constraints.
A. Mapping Module
Incrementally exploring the surroundings with a location-
uncertain system leads to an uncertain map. Under these
conditions, obtaining a consistent representation of the entire
environment M is a challenging task, which goes beyond
the scope of this work. Instead, this work represents M as
a sequence of independent local stochastic maps [20]:
M = {LM
0
, . . . , LM
n
}, (8)
where LM
i
is a local map, i.e. an area m
i
∈ M assumed to
have a uniform and null uncertainty (due to zero-uncertainty
assumption on observations) with respect to the local map’s
reference frame {LM
i
}. To make the uniformity assumption
consistent within each LM
i
, a new local map is started
when the relative uncertainty between the system and the
current {LM
n
} is over a predefined maximum value, where
n corresponds to the total number of local maps describing
M. Because the system in (5) holds linear time-invariant
(LTI) properties, the local maps management consists in
periodically defining a new local map as the triplet:
LM
i
= {{LM
i
}, k
i
, m
i
} , (9)
where k
i
is the time at which the LM
i
is created.
While a local map LM
n
is active, i.e. when the un-
certainty is within the permitted limits, the environment
information is directly represented with an Octomap [21].
Octomaps permit fusing range-based data into a probabilistic
voxel representation, which generates a three-dimensional
(3D) occupancy grid map with adjustable resolution. Thus,
an area m
i
is discretely represented as a set of voxels
{v
1
, . . . , v
m
} ∈ V
i
∼ m
i
. The Octomap directly permits to
differentiate between free, occupied, and unknown voxels
v
j
∈ LM
n
. The local map LM
n
expands the environment
awareness with the occluded (hidden) regions, i.e. unseen
voxels that are within the maximum sensor’s range but
located behind the obstacles (occupied space). This expanded
representation establishes three different labels with their
corresponding probability of occupancy: p
occupied
(v
j
): free
voxels v
F
are set to p
occupied
(v
F
) = 0, occluded voxels
v
H
with p
occluded
(v
H
) = 0.5 and occupied voxels v
O
with
p
occupied
(v
O
) = 1. This interpretation of the environment
awareness lets us consider those zones that are likely to be
occupied. All the information related to a local map is stored
in an octree data structure, which provides fast access time
while, at the same time, optimising the use of memory.
B. Planning Module
The planning problem defined in Section II-D has three
main requirements: to consider the vehicle’s motion capa-
bilities, to validate probabilistic constraints, and to meet
online computation limitations. Sampling-based methods,
particularly tree-based, provide a good alternative to meet
these requirements. Tree-based algorithms are commonly
composed by two procedures: sample and extend [7], [8]. The
former one randomly samples states from the state space X ,
towards which the latter one expands the tree. This twofold
procedure can also be conducted in the belief space B, by not
only considering the system’s motion capabilities, but also by
incorporating the uncertainties associated with the system’s
motion and the perception of the surroundings (Fig. 3).
1) Planning Under Uncertain Motion Constraints: the
system’s motion capabilities are considered in the planner
by expanding the tree with the system’s closed-loop model
defined in (5). The randomly sampled states are used as input
references r for the closed-loop system, which allows us to
guide the tree growth towards such a state. This expansion
is done for a period of time T
prop
. This strategy of using
the closed-loop model to expand the tree has been adopted
from the work presented by Kuwata et al. [22]. Furthermore,
since the proposed formulation presented in (5) includes the
system’s uncertainty, each obtained belief (node) corresponds
to a vehicle’s state with its associated uncertainty.
Most of the tree-based algorithms usually do not optimize
any metric or function. One alternative to cope with this
issue is to use a asymptotic optimal RRT (RRT*) while
approximating the dynamic range of the system to Dubins
curves. This alternative has already been explored in [17],
[18]. Instead, aiming to preserve the entire system’s dynamic
range, our planning module uses a shooting approach, which
consists of expanding the tree from the node with the lowest
Fig. 3: Tree expansion under motion and probabilistic con-
straints. The states (nodes) of the tree are obtained by con-
sidering the motion capabilities. The spheres surrounding the
states represent their uncertainty, where green corresponds to
those states satisfying the probabilistic safety constraints, and
red those that do not.
cost within a neighbourhood of δ-radius. This strategy is also
adopted in the stable sparse RRT (SST) planner to obtain
asymptotically near-optimal solutions [23].
2) Planning Under Probabilistic Constraints: the previ-
ously explained tree expansion leads to a tree of motions
(edges) and states (nodes) which are uncertain with respect
to its root, i.e. the state where all the planning tree is rooted
to. However, the uncertainty of all these elements can be
referenced to the global frame by taking into account the
uncertain location of the root into the world. Once all the
states and their associated uncertainties are referenced to
the global frame, then p
collision
can be bounded as presented
in (7). Nonetheless, this computation results conservative
because it does not consider that the relative uncertainty
between the robot and its observation is zero. In fact, the
relative uncertainty between the system and the observed
obstacle will only start growing once the obstacle goes
beyond the sensor detection range. Then, a less conservative
formulation of (7) that already considers the discrete nature
of the environment representation can be defined as:
p
collision
(b
k
, M) ≤
X
x∈v
H
,v
O
p
occupied
(x ∈ {v
H
|v
O
}) K, (10)
where K is a kernel containing the discretised version of
N (x |
ˆ
z
k
, P
relative
), where P
relative
is the relative uncertainty
between the state x located in a hidden (v
H
) or occupied
(v
O
) voxel in any of the local maps, and the state
ˆ
z
k
that
wants to be checked for collision. Retrieving the voxel’s
timestamp k
i
and the timestamp k
j
of the state
ˆ
z
k
, the
relative uncertainty between these elements is defined as:
P
relative
=
k
j
−1
X
m=k
i
A
k
j
−m−1
P
w
(A
T
)
k
j
−m−1
, (11)
where P
w
is, as introduced in (2), the covariance matrix
estimating the AUV’s motion uncertainty.
The formulation in (10) is less computationally expensive
than (7) since it does not integrate over the entire state space,
but instead it uniquely considers those states in collision.
3) Online Planning: the planning module uses different
strategies to meet online computation constraints. The first
of them is anytime calculation, which allows the planner
to provide the best available solution at any moment [24].
Another strategy seeks to decrease the computation time
when checking the validity of the motions during the tree
expansion. To do so, the planning module opportunistically
evaluates (10), avoiding to check the validity of states that
belong to the unknown space. Such a strategy was originally
presented in [17], [18]. Furthermore, for those states that
must be evaluated, the kernel K is verified by following a
spiral pattern, i.e., moving from the kernel’s centre outwards.
This heuristic seeks to firstly check the voxels where the sys-
tem has the highest changes. Finally, because the Gaussian
estimation of the system’s state extends infinitely over the
whole environment, the kernel K is bounded at 3.03σ, thus
being 99% certain of the state validation decision.
C. Framework manager
The framework manager is in charge of controlling the
previously presented mapping and planning modules. These
modules are coordinated following the pipeline presented in
Algorithm 1 until reaching a predefined goal region B
goal
(line 7). In this iterative process, the algorithm firstly requests
an update of the environment awareness M (line 8). Then,
if a previous solution old_path is available, it is probabilis-
tically checked for collision according to the new map M. If
old_path is no longer valid, the framework manager sends to
the controller the portion of old_path that is still safe, while
the planner finds a new valid solution path (line 10 and 11).
This approach prevents the vehicle stopping every time that
a path gets partially invalidated while ensuring its safety.
The planning stage starts by placing the root of the new
tree on a predicted position along the current valid path
(line 12). This (i) guarantees the feasibility of reaching the
root and (ii) prevents sudden changes in the vehicle’s direc-
tion of motion when a replanning manoeuvre is required.
Once the root is defined, the planner grows a new tree in B
for a specific amount of time planning_time (line 13).
The planner tries to find a near-optimal path within the
allocated planning_time, and returns a new_path if one
has been found (line 14). However, considering that the
tree expands faster in undiscovered areas rather than in the
presence of obstacles, the new_path is only executed under
two circumstances (line 15 to 17): (i) if the new path has a
lower cost than the path that is currently being followed, or
(ii) if the new path does not drastically change the trajectory
of the invalidated path. This last heuristic promotes the
exploration of paths through challenging scenarios.
IV. EXPERIMENTAL EVALUATION
The proposed framework has been implemented in robot
operating system (ROS) and uses the facilities provided by
the OMPL [25] to ease the motion planning requirements. To
conduct the experiments reported in this section, the frame-
work has been integrated with the component oriented layer-
based architecture for autonomy (COLA2) [26], a control
Algorithm 1: MANAGER(B
goal
, p
safe
)
1 Input:
2 B
goal
: Goal region.
3 p
safe
: Desired level of probabilistic safety guarantees.
4 begin
5 {M, B, old_path} ← ∅
6 planner.loadProblem(B
goal
, p
safe
)
7 while not isGoalAchieved() do
8 M ← mapper.getMap()
9 planner.updateMap(M)
10 if not planner.isValid(old_path) then
11 sendPath(old_path)
12 planner.setNewRoot(old_path)
13 planner.solve(planning_time)
14 new_path ← planner.getSolution()
15 if bestPath() = new_path then
16 sendPath(new_path)
17 old_path ← new_path
software architecture fully developed in ROS that controls
the AUVs from the Underwater Vision and Robotics Re-
search Centre (CIRS). One of such vehicles is the Sparus II, a
nonholonomic AUV used in this work (Fig. 1). The Sparus II
is a torpedo-shaped vehicle with hovering capabilities rated
for depths up to 200m [27]. For the trials presented in this
paper, the AUV was equipped with a mechanical scanned
imaging sonar (MSIS) to perceive the surroundings.
The test-bed to evaluate the proposed approach consists of
two environments located in Sant Feliu de Guíxols (Spain):
(a) breakwater structure that is composed of a series of
concrete blocks (14.5m long by 12m width), which are
separated by four-metre gaps (Fig. 4a and Fig. 4b), and
(a) Real breakwater (b) Simulated breakwater
(c) Real canyon (d) Simulated canyon
Fig. 4: Evaluation scenarios.
![Fig. 5: Comparison of the proposed probabilistic collision checking method with the approaches in [12] and [13]. (a) to (f) illustrate the XF (green) and the XO (red) in the breakwater structure scenario (blue lines) according to different methods and values of psafe. (g) shows the average (green diamonds) and quartiles of the computational cost for each considered method; the proposed one has been evaluated with two kernel-tranversing approaches: row or column-wise and spiral-like.](/figures/fig-5-comparison-of-the-proposed-probabilistic-collision-1vuw66ps.webp)
