IEEE Robotics & Automation Magazine - September 2019 - 81

toposemantic map can be used to represent the interior of
a building.
Planning Under Temporal Constraints
We now briefly recap our planning approach based on
CMDPs. Because of space limitations, we refer those who
are interested to [13] and [17] for more details. We assume
that the reader is familiar with MDPs and related terminology [18]. A finite, stationary, discrete-time CMDP is
defined as (X, A, b, c, {c i} iL= 1, P, {D i} iL= 1), where X is a discrete set of states; A is a finite set of actions; b is the probability distribution of the initial state; c: X # A " R $ 0 is a
primary nonnegative cost function, with c ^x, ah being the
cost of executing action a in state x; c i: X # A " R $ 0 are L
secondary cost functions, with c i ^x, ah being the ith cost
incurred when executing action a in state x; P ^x, a, y h is the
transition probability, i.e., the probability of transitioning
from state x to state y when action a is executed; and D i are
L nonnegative constants.
A policy r ^ x h defines with which probability each action
should be taken when in state x. Let c ^rh be the overall primary cost incurred when following policy r, and let c i ^rh be
the overall L additional costs. The CMDP problem is therefore defined as follows:
r

destination by a given deadline. The IDS module determines
when a robot has reached a vertex (either new or formerly
visited) and how many outgoing edges are emanating from
the vertex. The createNewNode (sketched in algorithm 1) is
called every time the IDS identifies a corridor intersection or
a room that was not formerly visited. The function takes as
parameters the new vertex vl to be added and the set of edges
emanating from it and updates the set of vertices and edges,
assigning labels. Unvisited edges, i.e., edges toward parts of
the graph still to be explored, are of the type ^vl , v zh, to indicate they lead to unexplored areas (v z is used to model unexplored space). In the function, labels are set to satisfy the
constraints listed in definition 1.
The algorithm to navigate from the current vertex v c to a
desired target vertex v is sketched in algorithm 2, which
takes two parameters, i.e., v and a temporal deadline t d . Later
on, we discuss how these are selected as part of the exploration module. To navigate to the vertex, a CMDP is built, considering the given temporal deadline and a probability of
failure P f (which, for simplicity, is assumed to be constant in
the following). At each step, the policy r * is used to

= arg r min c ^rh,

s.t. c i ^rh # D i

1 # i # L;

c3d

Right
Left

that is, we aim at following a policy r that minimizes the primary cost subject to bounds on the L secondary costs. The
primary and secondary costs are defined as
c ^rh = E ; / c ^ X t, r ^ X t hhE
3

t =0

R8
N/Right

c i ^rh = E ; / c i ^ X t, r ^ X t hhE .
3

t =0

To fulfill a navigation task with temporal deadlines, we use
one primary cost and two secondary costs. The primary cost
is set to zero in the goal state and to one elsewhere. The two
secondary costs are the global failure probability and the
cumulative time to reach the target vertex. Accordingly, the
problem is then to compute a policy reaching the goal state
while limiting the time spent and the failure probability. To
apply the CMDP planner to the exploration task, the state set
X will be the set of vertices V such that the policy can be
used to navigate the robot to a vertex. The set of actions A is
instead a finite set of maneuvers representing basic motion
abilities. Each maneuver is characterized by an expected time
spent to execute it and a failure probability. In the "Experimental Validation" section, we describe the maneuvers and
associated parameters.
Incremental Map Construction,
Navigation, and Exploration
We now describe how a toposemantic map M can be incrementally built and used for navigation by a robot exploring an
unknown indoor environment while attempting to reach a

c2f

c2e

c2d

c3c
c3b

W/Left E/Right

S/Left

R6b

c1a

c2c
c2b

c3a R6a

c1b

c1c

c1d

Figure 2. A toposemantic map. Vertices labeled Rx are rooms,
while corridors are denoted by Cx. Edges with an R label are
purple while those with an L label are green.

Algorithm 1: Adding a new node to the map
1.
2.
3.
4.
5.
6.
7.
8.
9.

Algorithm createNewNode ^v l, edges h
Let v be the vertex the robot came from
V ! V , " v l,
E ! E , "^v, v lh, ^v l , v h,
Set L^v lh as per the IDS (room or corridor)
Set D ^v l , v h and D ^v l , v h as per the compass
Set S^v l , v h and S^v l , v h as per the compass
for all unvisited outgoing edges from vl do
Add edges of type ^v l , v zh and set labels S, D

SEPTEMBER 2019

IEEE ROBOTICS & AUTOMATION MAGAZINE

IEEE Robotics & Automation Magazine - September 2019

Table of Contents for the Digital Edition of IEEE Robotics & Automation Magazine - September 2019