This article has been accepted for inclusion in a future issue of this journal.

Content is final as presented, with the exception of pagination.

IEEE TRANSACTIONS ON SYSTEMS, MAN, AND CYBERNETICS: SYSTEMS 1

Path Planning for Active SLAM Based on the D*

Algorithm With Negative Edge Weights
Ivan Maurovic, Member, IEEE, Marija Seder, Member, IEEE, Kruno Lenac, Member, IEEE,
and Ivan Petrovic, Member, IEEE

AbstractIn this paper, the problem of path planning is always combined with a higher priority module which sets
for active simultaneous localization and mapping (SLAM) is the goal positions for the robot. Very often it is the exploration
addressed. In order to improve its localization accuracy while module where the robot builds a map of an unknown environ-
autonomously exploring an unknown environment the robot
needs to revisit positions seen before. To that end, we propose ment by visiting unknown areas what increases localization
a path planning algorithm for active SLAM that continuously uncertainty since there are no landmarks and areas visited
improves robots localization while moving smoothly, without before [1][4]. We consider a moving sensor for exploration
stopping, toward a goal position. The algorithm is based on (sensor placement problem) where the sensor is mounted on
the D* shortest path graph search algorithm with negative edge a moving robot base, unlike static sensors for localization
weights for finding the shortest path taking into account local-
ization uncertainty. The proposed path planning algorithm is in indoor environments [5], or exploration performed with a
suitable for exploration of highly dynamic environments with sensor mounted on a manipulator with a fixed base [6].
moving obstacles and dynamic changes in localization demands. Active SLAM solutions are implemented by minimizing a
While the algorithm operation is illustrated in simulation exper- certain criterion consisting of information gain. In [1], the next
iments, its effectiveness is verified experimentally in real-world velocity input applied to the robot is obtained from a crite-
scenarios.
rion which combines localization uncertainty and the amount
Index TermsActive SLAM, dynamic environment, explo- of unexplored area in the next robot step. The authors use
ration, negative edge weight in a graph, path planning, extended Kalman filter (EKF) as a basic method. A variant
simultaneous localization and mapping (SLAM).
of model predictive control (MPC) with EKF where the robot
velocity inputs are obtained as a multi step optimization for
I. I NTRODUCTION active SLAM is demonstrated in [7]. The landmarks for the
HE simultaneous localization and mapping (SLAM) localization are fixed on the optimization horizon meaning that
T problem has been widely researched in the last two
decades and many different variations exist. Considering
no new landmarks are expected while the robot is moving.
The problem with prediction in active SLAM is that we do
SLAM as a passive system the robots path is not chosen by not have future measurements prediction. The authors assumed
taking into account the localization uncertainty improvement. that innovation in Kalman filter is Gaussian with zero mean.
Thus, SLAM algorithm does not control the robot motion in Simulation results show a comparison between one step and
any sense. If SLAM algorithm steers the robot to the areas multi step ahead optimization and a benefit of the later one.
which lower the pose uncertainty it becomes the active SLAM Leung et al. [2] incorporated exploration strategy in active
algorithm. In general, the idea is that the robot chooses a path SLAM using MPC where the exploration areas of interest are
which maximizes the information gain by moving to the areas included as artificial landmarks for SLAM what forces the
already visited from the previous navigation actions where it active SLAM path planning to explore the environment. MPC
can take measurements of already known landmarks (objects formulation of active SLAM for line-feature-based SLAM can
in the environment used for localization like points, lines, be found in [8]. Dellaert and Kaess [9] used iSAM instead
planes, and others). This is called loop closing in SLAM. of EKF SLAM. Kontitsis et al. [10] introduced an interest-
Active SLAM minimizes localization and map uncertainty. It ing idea of multiple robots active SLAM with relative entropy
optimization where the cost function consists of the trace of
Manuscript received November 28, 2016; accepted February 1, 2017.
This work was supported in part by the Unity through Knowledge the covariance matrix at the terminal state as a measure of the
Fund (Cooperative Cloud-Based Simultaneous Localization and Mapping in uncertainty. From the space of trajectory samples the optimal
Dynamic Environments) under Grant 25/15, and in part by the Ministry one is chosen. The EKF-based SLAM is used as a simula-
of Science, Education and Sports of the Republic of Croatia (Centre of
Research Excellence for Data Science and Cooperative Systems) under tion example. However, no real-time applicability discussion
Grant 533-19-15-0007. This paper was recommended by Associate Editor is given. Carrillo et al. [11] discussed a cost function used as
S. Nahavandi. a measure of the localization uncertainty.
The authors are with the Faculty of Electrical Engineering and Computing,
Department of Control and Computer Engineering, University of Zagreb, If the mobile robot path is planned only a few steps ahead
10000 Zagreb, Croatia (e-mail: [email protected]; [email protected]; there is no guaranty that the robot will reach the goal posi-
[email protected]; [email protected]). tion. There are also papers dealing with a complete path of
Color versions of one or more of the figures in this paper are available
online at http://ieeexplore.ieee.org. the robot. Stachniss et al. [12] used RaoBlackwellized par-
Digital Object Identifier 10.1109/TSMC.2017.2668603 ticle filter to solve SLAM problem with the exploration task.
2168-2216 c 2017 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission.
See http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
 This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.

2 IEEE TRANSACTIONS ON SYSTEMS, MAN, AND CYBERNETICS: SYSTEMS

between exploration and SLAM like in [12], [14], and [15]

what would interrupt the robot motion toward a goal in order
to improve localization. To address this problem we propose a
path planning algorithm that is able to continuously improve
localization without interrupting the main task. Together with
fast trajectory replanning in dynamic environment it represents
the main contribution of this paper. The algorithm is based
Fig. 1. Overall scheme of exploration incorporating active SLAM.
on the modified D* path planning algorithm [18]. Here it is
adopted for use with negative edge weights and negative cycles
in a graph. Additionally, the path planning algorithm can
Defining exploration and place revisiting goal positions they replan the shortest path in case of change of the localization
evaluate the path to each of the goal position by estimating the demands.
change of the entropy of the particle filter caused by moving
the robot to a certain goal position. Sim and Roy [13] intro- II. OVERALL P ROBLEM F ORMULATION
duced a global path planning based on the robots position
The active SLAM is often combined with exploration mod-
variance minimization using breadth first search over all robot
ule which gives a goal position where to go next. The map of
positions. However, the exploration task might not be optimal
the environment is unknown in the beginning and the robot is
as there is no exhaustive search of the space of all trajec-
given the task to explore the environment. Typical exploration
tories. Additionally, approaches where each of the potential
problem incorporating SLAM, path planning and path exe-
trajectories from the robot position to the set of goal positions
cuting is shown in Fig. 1. Connections between modules are
(specific for exploration tasks) are evaluated and the best tra-
represented with the arrows. The sensors mounted on the robot
jectory according to SLAM criterion is chosen usually have a
provide information about the robot and the environment like
limited trajectory space from which the best trajectory comes
obstacles and odometry measurements. Sensor used for the
from.
exploration is a 3-D lidar mounted on top of the autonomous
In some active SLAM approaches uncertainty improvement
mobile robot platform measuring distance to obstacles in the
is considered when the uncertainty of the robot pose is above
environment. The same sensor is also used for SLAM, together
a certain threshold [14], [15]. Since SLAM is often com-
with inertial measurement unit (IMU) and encoders. In our
bined with exploration, in these methods the robot movements
setup we also used a 2-D laser range sensor mounted in front
switch between exploration and SLAM tasks in a discrete
for obstacle avoidance. Based on the sensor data SLAM mod-
way depending on the uncertainty criterion value. Active
ule builds a map and localize the robot inside the map while
SLAM problem with multiple robots is considered, where
exploration module calculates the next goal position. SLAM
Pham and Juang [16] used predefined threshold for the switch-
module consists of two submodules: one is SLAM, giving the
ing between exploration and localization improvement. The
robot pose in the map according to sensors data and the other
multiple robot active SLAM is also used in [17]. A procedure
is active SLAM giving a discrete set of points that should be
of taking a fixed threshold for the loop closing is limited since
preferable to visit in order to increase the localization uncer-
it stops the robot from accomplishing the main task and con-
tainty. Navigation module generates robot trajectories which
trol the robot to the loop closing position causing time delay
take into account exploration regarding the goal position and
in the main task. The opportunity for loop closing perhaps
SLAM demands regarding preferable localization points. It
was better in some previous robot positions when the robot
consists of the path planning and path executing part respon-
did not have a problem with localization uncertainty but the
sible for generating robot trajectories. Active SLAM is also
detour could have been much shorter and smoother. On the
considered as a part of navigation module since it can redirect
other hand, setting a subgoal for the localization would cause
the robot path into the localization preferable area. The output
double path planning: from the robot position to the subgoal;
signal from the navigation module is calculated by the path
from the subgoal to the goal, what is not preferable due to
executing module by which the robot follows the given path
computation and time demands of the main task.
and takes into account the model of the robot. The proposed
All mentioned methods suffer from the problem of replan-
contributionpath planning for active SLAM algorithmis
ning a trajectory/path in the scenario with moving obstacles
affecting active SLAM and path planning module colored red
where the robot needs to quit executing the planned trajec-
in the figure. The rest of the modules are briefly described
tory and replan a new one in real time. In approaches like
bellow.
in [1], [2], and [7] the robots path is only locally optimal
where the path is planned only few steps ahead. The advan-
tage of here proposed algorithm is a complete path from the A. Exploration
robot position to the goal position. Active SLAM approach The robot starts with an unknown map of the environment
presented in [10] do not consider obstacles while the robot and takes an initial laser scan. Based on the first laser scan it
is moving, i.e., it is necessary to calculate the path from the builds an initial polygonal map of the environment and cal-
beginning if obstacles appear, unlike in our approach where culates the next positions from where to take the next scan
the path is efficiently recalculated in the presence of obstacles. to maximize the amount of unexplored area which can be
Other common approach to active SLAM includes switching seen from the next scan. The exploration is finished when the
 This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.

MAUROVIC et al.: PATH PLANNING FOR ACTIVE SLAM BASED ON THE D* ALGORITHM WITH NEGATIVE EDGE WEIGHTS 3

whole environment is covered by the laser sensor. The algo-

rithm gives a goal positions in front of the lines which divide
known and unknown areas. In our previous work, we tested
the algorithm on a complex 3-D environment. Details of the
exploration method can be found in [19].

B. SLAM
We have used graph-based SLAM algorithm developed
Fig. 2. Topological distance as a measure of information gain.
within our group [20]. It is based on exactly sparse delayed
state filter (ESDSF) [21]. ESDSF is used for estimation of
Gaussian vehicles trajectory X which consists of n pose
is in which state should the loop closing points be placed.
samples Xi = [qi ti ]T , i = 0, . . . , n 1
Although every loop closing would increase accuracy of the
X0 trajectory, SLAM update and the afterwards global map update
X1 are performance costly operations that should not be executed

. if the impact on the accuracy is too small. Moreover in order to
X= , X N (, ) = N 1 (, ) (1)

. close the loop, the robot will need to diverse from the shortest
. path planned to the exploration goal. This is why loop closing
Xn1 points should be placed only in states in which loop closing
with high enough impact on the overall trajectory accuracy
where qi represents mobile robots orientation in the global
can be achieved.
frame in the form of quaternion and ti its position in the frame.
Whenever a new state is added to the trajectory X, the active
The relation
 between and is: = ,and between covari-
SLAM algorithm begins to search for possible loop closings
ance and information matrix :  = 1 . Since our robot
between newly added state Xi and all other states Xj , j = i,
moved through one building with flat floor for the simplic-
in the trajectory. The state Xj is chosen for the loop clos-
ity we used 2-D motion model (ti = [xi , yi ], qi = [si , q3,i ])
ing if it satisfies two conditions. First condition is that the
described as nonlinear first order Markov process
Euclidean distance from Xi has to be lower than a predefined

xn + x value dmin . This condition imposes high probability of regis-

xn+1 yn + y tration between states Xj and Xi since it is likely that there are

yn+1 sn 12 q3,n y enough similar landmarks (planar surface segments) between
=
sn+1 1+ 4 2 + wn (2) local maps assigned to them. Second condition that the state

q3,n+1 q3,n + sn 2 Xj has to satisfy, in order to be chosen for the loop closing is

1+ 4
2 that the resulting SLAM update will have high enough impact
on the trajectory accuracy. In general, update impact on the
where x, y, and  are changes in robots position and ori- trajectory accuracy is proportional to the sum J of the cost
entation in every step obtained from IMU and wheel encoders functions fc between all neighboring states in the trajectory
measurements and wn is the white noise with mean value 0 moving from the state Xj to the state Xi . The sum of cost
and the covariance Q. functions between states Xj and Xi is calculated as
As the robot moves through the environment new states are
added to the trajectory X when the pose difference fc between
i1
the last added state Xn1 and the current robot pose Xn is J= fc (m, m + 1). (4)
larger than the predefined threshold. Cost function fc takes m=j
into account both angle and distance differences between two However, the problem with using sum of all cost functions
states Xi , Xj and is calculated as between neighboring states to calculate the trajectory accuracy

 

fc (i, j) = d Xi , Xj + Yaw Xi , Xj  (3) impact of the loop closing, arises if there were previous loop
closing detections. For example, let us consider situation illus-
where d(Xi , Xj ) is the Euclidean distance between the states trated in Fig. 2, where we want to measure the accuracy impact
Xi and Xj , and Yaw is the difference of the angle between of loop closing between states X1 and X9 . Although the sum
those states and is a scaling factor. Whenever a new state is of cost functions between all states from X9 to X1 is high,
added, the current measurements from the robots sensors are since an update occurred between states X7 and X2 , the overall
recorded and associated with the state. Update of the trajectory impact on the trajectory accuracy from closing loop between
occurs by calculating relative pose between two states from states X9 and X1 is small. This is because a lot of informa-
the associated measurements. In order to calculate the relative tion that would be gained from loop closing between states
pose between two states the associated measurements need X1 and X9 was already gained by loop closing between states
to contain enough similar landmarks which means they need X2 and X7 . This is why we use approach similar to [14] as a
to be taken from approximately the same location. This is measure of accuracy impact of loop closing. Accuracy impact
why for a successful trajectory update the robot has to visit is determined using topological distance. Topological distance
already traversed areas and achieve loop closing. The question is calculated from the graph T g generated from the pose graph
 This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.

4 IEEE TRANSACTIONS ON SYSTEMS, MAN, AND CYBERNETICS: SYSTEMS

incidence matrix T I. Pose graph incidence matrix T I is n n edges and W referring to edge weights. The environment is
matrix, where n is the number of states in the trajectory, whose represented by a discrete grid where each grid cell is occu-
elements are given by pied or empty, representing nodes N in the graph. Such a
 grid is called the occupancy grid map created by approximate
1, if state Xi is connected with state Xj
T
Iij = (5) cell decomposition of the environment [25], [26]. The current
0, otherwise.
robot position is denoted with x = [x y ]T .
States are connected if they are neighboring states (i.e., state Xi Besides standard cells in a grid map with occupied and
is connected with states Xi+1 and Xi1 ) or if the pose constraint empty status for the purpose of active SLAM we add the local-
was formed between them (i.e., loop closing was detected and ization improvement cells L N as a loop closing points
trajectory update executed). Nodes in the graph T g are rep- from active SLAM and consider them as a special status of
resented by the states and connections between nodes i and the cell.
j exist if the element (i, j) in matrix T I is 1. Weight of the Search for a shortest path in a graph uses the criterion as
connection wi,j between states i and j is a sum of all edge weights from the robot position to the goal
 position. The edge weight of the empty, i.e., the cost of enter-
f (i, j), if |i j| = 1
wi,j = c (6) ing to an empty cell is set to the Euclidean distance between
0, otherwise
two adjacent cells depending on the cell size while the edge
this ensures that connections made by the previous loop clos- weight of an occupied cell is set to . The localization cells
ing have 0 weight and will not increase measured topological should attract the robot to go through the localization cell thus
distance between two states. Topological distance between the edge weight or the cost to enter the localization cells has
states (Xi ,Xj ) is calculated as the shortest path from the node to be lower than the other cells in the grid. Setting the edge
i to the node j in the graph T g. The shortest path is calculated weight to 0 is not giving anything to attract the robot. Actually
using the A* algorithm. For example, topological distance the edge weight needs to have a negative value. The edge
from the nodes 19 (Fig. 2) would be the sum of cost func- weight of the localization cell compensates a detour the robot
tions between the states (X1 , X2 ), (X7 , X8 ), and (X8 , X9 ). Now takes from the shortest path on the way to the goal position
when we can calculate topological distance between two states what can be done only using negative cell edge weight.
(Xi , Xj ), we can form the second condition that states have to A standard graph search in robotics like A , D or simple
satisfy in order to be chosen for the loop closing Dijkstra algorithm cannot be used since we have negative edge

 weights in the graph. Adding a negative edge weight cell in the
T
d Xi , Xj > Dmax (7)
graph causes problems with finding a shortest path using com-
where T d is the topological distance between the states Xi and mon graph search algorithms in robotics, especially since there
Xj , and Dmax is a predefined topological distance threshold. For are also negative cycles where shortest path does not exist. The
all states that satisfy both conditions, the state with the highest shortest path problem with the negative edge weights is used
topological distance from Xi is selected for loop closing. in [27], where the problem is solved using dynamic program-
1) Path Planning: When the robot has all information about ming BellmanFord algorithm for the shortest path planning.
the static and dynamic obstacles in its surrounding it obtains BellmanFord algorithm can solve the problem with the nega-
a shortest, obstacle free path to reach the goal. In the pro- tive edge weights but has higher complexity than Dijkstra and
posed method the additional input to the path planning module it is not suitable for dynamic environment [28]. If something
are loop closing points from the active SLAM module. If the changes in the environment or dynamic obstacle appears the
cost of the detour the robot takes to reach localization points whole computational process has to be repeated. The nega-
is acceptable according to localization accuracy increase, the tive cycle problem with the shortest path still remains since
path will be planned through the localization point. The path BellmanFord can only detect a negative cycle but it does not
planning module is based on the D* path planning algorithm solve it in any way. Therefore, we proposed an extension of
and it is the main contribution of this paper. It is explained in the D algorithm which solves the shortest path problem with
detail in the next section. negative edge weights.
2) Path Executing: Path executing module receives planned
path from the path planning module and brings signals to con-
trol the robots wheels to reach the goal position. The outputs A. Graph Creation and Search
are linear and angular velocities of the robot in each time Weighted undirected graph G(N , E, W) is created in the
step. We used our receding horizon control approach for the occupancy grid map. Two nodes i, j N in the graph are
trajectory execution based on the planned D* path [22]. neighbors if corresponding Cartesian coordinates of cell cen-
ters ci , cj R2 have L distance equal to the length of the cell
III. D W ITH N EGATIVE E DGE width ecell , i.e., ci cj = ecell . The set of edges is defined
W EIGHT PATH P LANNING as E = {ei,j := {i, j} | i, j N , i and j are neighbors}. The set
There has been a large amount of approaches on plan- of edge weights W = {wi,j | i, j N , i and j are neighbors}
ning the robot trajectories, path planning, and path follow- is defined as the cost of transition between neighbors.
ing [23], [24]. In this paper, we use a graph representation In standard binary occupancy grid maps there are two val-
of the environment as a collection of nodes and edges, ues of transitions between neighbors: straight and diagonal
G(N , E, W) with N representing nodes and E representing transition what could present real Euclidean distance, i.e.,
 This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.

MAUROVIC et al.: PATH PLANNING FOR ACTIVE SLAM BASED ON THE D* ALGORITHM WITH NEGATIVE EDGE WEIGHTS 5

wi,j = ci cj 2 . For the purpose of active SLAM negative and the value of the key function k(Z) for the replanning
cost transitions between the negative cell from L and a free process, which stores the old values h(Z) before changes of
cell from N \ L is introduced in the graph. The negative value weights in the graph happened.
is a function of the expected localization gain from the SLAM The execution of the D* algorithm can be divided into
update at a certain negative cell position. For the problem, we initial planning and replanning phases. Initial planning is per-
solve the edge weight has the following structure: formed if the robot is standstill at the start position (R = start)
  and replanning is performed if the robot detects nodes with
wi,j = ci cj 2 (8)
changed occupancy values during its motion. D* uses the
where = 1 for empty cells, for occupied cells and < 0 set OPEN as a temporary storage for the currently examined
for localization (negative) cells. For the negative values, is nodes. Details can be found in [18].
a function of a topological distance T d(Xn , Xj ) between the
current robots state Xn in SLAM and the state Xj in SLAM.
C. Negative Edge Weight D* AlgorithmND* Algorithm
State Xj in SLAM is represented with the negative cell in the
graph The proposed algorithm is given in Algorithm 1 where the


 main differences from the original D* algorithm are high-
= f T d Xn , Xj . (9) lighted and explained in this section. We named the proposed
Assume that the start node and the goal node are specified graph search algorithm the negative edge weight D* algo-
in the graph G(N , E, W). The search for the shortest path rithm (ND* algorithm) the main D* function is renamed as
from the start node to the goal node has to be performed. A PROCESS-STATE-NEG-EDGE().
path P = P(start, goal) is in the graph G(N , E, W) is defined In PROCESS-STATE-NEG-EDGE() algorithm lines 57 are
as different from the standard D* algorithm. Before the initial
search the set of all graph nodes with negative edge weight is
P[1] = start, P[|P|] = goal created, called NEGATIVE_CELL.
P[i] N , i = 1, . . . , |P| The algorithm starts by adding the goal node to the OPEN
   list. Every node Z has associated tag t(Z) with a value NEW
P[j], P j + 1 E, j = 1, . . . , |P| 1
if the node has never been on the open list, OPEN if it is
P[i] = P[j], i, j = 1, . . . , |P|, i = j (10) currently on the OPEN list and CLOSED if the node was
where |P| represents the path length in the number of cells. on the OPEN list. In the initial step t(Z) for all nodes is set
The cost of the path P is defined as the sum of weights of to NEW. PROCESS-STATE-NEG-EDGE() function is invoked
edges along the path, that is repeatedly until OPEN list is empty. The rest of the embedded
functions are MIN-STATE() which returns the node with min-
|P
|1
imum k value on the OPEN list, DELETE(Z) which removes
h(P) := wP [i],P [i+1] . (11) the node from the OPEN list and INSERT(Z,h(Z)) which
i=1
inserts the node Z on the OPEN list and computes h(Z).
Let (start, goal) be the set of all possible paths between the INSERT(Z,h(Z)) is given in Algorithm 2. At the end of ini-
start node and the goal node in the graph G(N , E, W). The tial path planning process every node has a parent node, i.e.,
optimal cost of the path from the start node to the goal node b(Y) = Z, where Y is a parent node of Z or we can say Y
that has to be searched for is defined as is pointing at Z. The shortest path for the node Y is obtained
following pointers through parents nodes to the goal node.
h (start, goal) := min h(P)
P Executing CREATE-BLACKLIST() in Algorithm 1 enables
s.t. P (start, goal) (12) handling negative edge weights in the graph. If the negative
cell is on the OPEN list and has minimum k value among
with implicit assumption that h (start, goal) = if the other cells on the list, the BLACKLIST is created by the
(start, goal) = . The shortest path from the start node to following backpointers starting from the negative edge weight
the goal node is the path (or more than one path) that has the node. Creating the BLACKLIST on a such way it actually
optimal cost h (start, goal). represents the shortest path from the negative edge weight cell
to the goal node. In the standard D* algorithm with no negative
B. D* Algorithm cells the initial step is the Dijkstra shortest path search. Only
The D* algorithm is a well known graph search algorithm line segment from 1519 is executed. Dijkstra algorithm does
capable of fast replanning in dynamic environments [18]. It not work with negative edge weights, i.e., does not guaranties
is also known as the dynamic version of the Dijkstras algo- optimality with negative cells. In the proposed algorithm when
rithm or dynamic version of the A* algorithm without the condition Z NEGATIVE_CELL is true the path from the
heuristic function [23]. The D* algorithm finds the shortest negative cell to the goal position is the shortest path. We use
path in graphs in which weights change during the time, i.e., that path fixed during the rest of the ND* search. Creating
occupancy values become higher or lower due to obstacles BLACKLIST we do not allow nodes on the list to change the
movement. pointer while the rest of nodes will be changed and redirected
For every searched node Z, the D* algorithm computes the according to negative cell node. By this we prevent the loop
cost value h(Z) of the path from the node Z to the goal node cycles, or the negative cycles in the graph.
 This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.

6 IEEE TRANSACTIONS ON SYSTEMS, MAN, AND CYBERNETICS: SYSTEMS

Algorithm 1 PROCESSSTATENEGEDGE() Algorithm 2 INSERT(Z,hn )

1: Z=MINSTATE(O) 1: if t(Z)=NEW then
2: if Z=NULL then 2: k(Z)=hn
3: return -1 3: else if t(Z)=OPEN then
4: end if 4: k(Z)=min(k(Z),hn )
5: if Z NEGATIVE_CELL then 5: else if t(Z)=CLOSED then
6: CREATEBLACKLIST() 6: k(Z)=min(h(Z),hn )
7: end if 7: end if
8: kold =GETKMIN(); DELETE (Z) 8: h(Z)=hn
9: if kold < h(Z) then 9: t(Z)=OPEN
10: for each neighbor Y of Z
11: if h(Y) kold and h(Z) > h(Y) + wY,Z and (Y /
BLACKLIST) then Algorithm 3 MAPCHANGE(Z,w_val)
12: b(Z) = Y; h(Z) = h(Y) + wY,Z 1: for each neighbor Y of Z do
13: end if 2: wZ,Y =w_val
14: end if 3: end for
15: if kold = h(Z) then 4: if t(Z)=CLOSED then
16: for each neighbor Y of Z: 5: INSERT(Z,h(Z))
17: if t(Y) = NEW or (b(Y) = Z and h(Y) = h(Z) + wZ,Y ) 6: end if
or (b(Y) = Z and h(Y) > h(Z) + wZ,Y ) and (Y / 7: if Z BLACK_LIST then
BLACKLIST) then 8: RESET_BLACKLIST()
18: b(Y) = Z; INSERT(Y, h(Z) + wZ,Y ) 9: end if
19: end if
20: else
21: for each neighbor Y of Z
22: if t(Y) = NEW or (b(Y) = Z and h(Y) = h(Z) + wZ,Y )
and (Y / BLACKLIST) then
23: b(Y) = Z; INSERT(Y, h(Z) + wZ,Y )
24: else if b(Y) = Z and h(Y) > h(Z) + wZ,Y and (Y /
BLACKLIST) then
25: INSERT(Z, h(Z))
26: else if b(Y) = Z and h(Z) > h(Y) + wY,Z and t(Y) =
CLOSED and h(Y) > kold and (Y / BLACKLIST)
then
27: INSERT(Y, h(Y))
28: end if
29: end if

When initial phase calculates the shortest path from each

node to the goal the robot starts to execute the path fol-
lowing task. When the change of the map is detected with
sensors on the robot the function MAP-CHANGE() is called. Fig. 3. Beginning of initial step.
The part of the proposed algorithm is given in Algorithm 3.
Map change could be caused by a dynamic obstacle, creating
a new localization point or removing already visited local-
ization point. The function MAP-CHANGE() changes edge D. Illustration of Operation
weights and enters affected cell on the open list. The dif- In this section, we illustrate how the algorithm operates
ference from the standard D* is that it also checks if the in a simple environment. Starting from the initial step where
modification in the map is on the shortest path from the neg- the map of the environment and the robots goal position is
ative cell to the goal position, i.e., on the BLACKLIST. If given. The algorithm starts by putting a goal node on the open
that happens all cells with the shortest path going through the list and expanding it with its neighbor cells. The number of
BLACKLIST are reset by invoking RESET_BLACKLIST(). neighbor cells depends on a realization of the graph from the
Status of the cell is set to NEW, their h value is set to occupancy grid map. Here we use four neighbors due to sim-
obstacle and pointers are deleted. Also, all cells on the plicity but the algorithm works independently from the number
BLACKLIST are set on the OPEN list while deleting the of neighbors. Fig. 3 represents the map showing pointers at
BLACKLIST. the moment when the negative cell has been expanded and
 This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.

MAUROVIC et al.: PATH PLANNING FOR ACTIVE SLAM BASED ON THE D* ALGORITHM WITH NEGATIVE EDGE WEIGHTS 7

Fig. 4. Final path planning of the initial step. Fig. 6. Path planning with an obstacle on the blacklist.

Fig. 5. Reset the map after obstacles on the blacklist. Fig. 7. Two negative cells in the map.

removed from the open list. The robots start and goal posi- status already have the shortest path and are not considered.
tion are denoted with R and G, respectively. The obstacles are In Fig. 4, the final, initial path planning with the shortest path
colored gray while negative edge weight cells are green. Each with thicker arrows is shown. As it can be seen in Fig. 4 all
cell which has been examined at that moment would have the cells in the upper part of the map are attracted by the negative
shortest path if there was no negative cell in the map. When cell. For the simulation purpose negative edge weight is set to
the algorithm reaches the negative cell (line 5) it activates and 15 where the other cells edge weight is set to 1.
generates the blacklist to fix the shortest path from the nega- When the robot has the shortest path it starts to follow the
tive cell to the goal position. The pointers from the blacklist path. If nothing changes on the way the whole pointer struc-
remain unchanged during the path planning while the other ture remains the same and the robot reaches the goal. Since the
cells change the pointers after the negative cell is detected. dynamic environment is highly expected where the robot has
This way the graph cycles are avoided. Line 17 in Algorithm 1 any kind of interaction with humans or other moving obstacles
with condition b(Y) = Z AND h(Y) > h(Z)+c(Z, Y) activates the algorithm begins with replanning phase when a change in
and redirect the cells. This is not the case with the standard the environment is detected. Three typical map change scenar-
D* algorithm where in the initial step the cells with CLOSED ios are possible. When a dynamic obstacle is in the map area
 This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.

8 IEEE TRANSACTIONS ON SYSTEMS, MAN, AND CYBERNETICS: SYSTEMS

Fig. 8. Experimental resultsexploration with the loop closing. Red line is the planned robot path, green line is the executed path and red x are the
localization points.

not affected by the negative cell or cells on the blacklist the the shortest path from the negative cell to the goal needs
algorithm performs as it is expected from standard D* algo- to be found to create the blacklist. When a new localiza-
rithm. The second scenario appears when the obstacle is on tion cell appears it already has the shortest path to the goal
the blacklist which forces the algorithm to allow changes on position due to the initial planning. Following the shortest
the blacklist and the third scenario that activates replanning path the blacklist is created and replanning executes invoking
is when a new localization point appears or when an already PROCESS-STATE-NEG-EDGE(). Some nodes redirect their
existing localization point is removed from the map. The latest pointer so that path goes through the new localization cell.
two are considered in the next example since the first one is Fig. 7 shows the situation when two negative cells are active
standard as in the D* algorithm. in the environment. A new negative cell edge weight is set to
1) Obstacle on the Blacklist: When an obstacle is on the 10 what results with the shortest path going through the both
blacklist the pointers on the blacklist need to be changed what negative cells. Note that the blacklist consists of the shortest
would cause cycles in the graph with negative edge weight. To path with both negative cells inside. In a different arrangement
avoid cycles in the graph we implemented a special care for of the negative cell it could happen that the shortest path goes
that case in the function MAP-CHANGE() in line 7. Calling only through one cell what would create two blacklists for
RESET_BLACKLIST() resets only cells for which the short- each of the negative cell. The same scenario happens if there
est path goes through the blacklist, i.e., cells affected by the are more than two negative cells. On the other hand, when the
obstacle on the blacklist. Fig. 5 shows the map after MAP- robot closes the loop the negative cell needs to be removed and
CHANGE() in line 8 execution. The robot starts moving and at not used any more. The edge weight is set to positive value
the position denoted with C detects the obstacle on the black- and the cell is put on the OPEN list. The blacklist is deleted
list. It can be seen that complete upper part of the map is reset for that cell and PROCESS-STATE-NEG-EDGE() recalculates
since the negative edge value is set to 15 and attracts rela- the path.
tively large area in that example map. In real scenarios affected
area are usually local cells around the negative cell what is
more acceptable from the algorithm complexity point of view. IV. E XPERIMENTAL R ESULTS
Path planning continue with setting the obstacle cell on the The proposed algorithm was tested experimentally on a dif-
open list and invoking PROCESS-STATE-NEG-EDGE(). The ferential drive mobile robot Husky equipped with 2-D SICK
result is shown in Fig. 6, where the robot position is denoted LMS100-10000 laser and 3-D Velodyne LiDAR. The main
with C. task was exploration of the environment based on the 2-D
2) New Localization Cell: The number of localization cells data available from the laser. The exploration algorithm used
considered in the map depends on the area where the robot in the experiment can be found in [19] and is shortly described
is expected to go. Thus, when the robot moves it enters a in Section II-A. The next robot goal positions are chosen to
new area where new localization cells appear. All cells with maximize unexplored area which can be seen from the next
changed status are set on a OPEN list with calling MAP- robots positions. The overall system was implemented under
CHANGE(). In order to guaranty absence of negative cycles the robot operating system.
 This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.

MAUROVIC et al.: PATH PLANNING FOR ACTIVE SLAM BASED ON THE D* ALGORITHM WITH NEGATIVE EDGE WEIGHTS 9

We did experiments under various conditions including no

obstacles and dynamic obstacles on the robots path, appearing
a group of the people around the robot, varying with obstacles
size, etc. The negative cells were added dynamically when
the robot entered in the vicinity of the loop closing points.
However, not all negative cells were part of the robots path
to the goal. Fig. 8 shows a few typical exploration steps from
one of the experiments. The polygon shown in each figure rep-
resents the explored environment from the beginning until the
current step labeled in the figure. The robots current position
is denoted by a rectangular shape object. Two paths con-
necting consecutive goal points represent planned path and
executed path colored red and green, respectively. The local-
ization points are marked with red x inside the exploration
polygon. The robot starts exploring the environment and makes
an initial map of the explored area and chooses the next explo-
ration goal. On the way to the next goal, what is shown in (a)
step 2, the planned and executed paths are similar. The differ-
ence in the beginning is due to path executing module which
takes into account the mobile robot dynamics and initial ori-
entation. As can be seen in step 2 there are no localization
points on the way since the robot has entered a new area for
the first time and the loop closing cannot be achieved if the
robot has not been there before. Step 3 brings the robot back to
the area visited before and shows the ND* algorithm steering
the robot from the planned trajectory to lower the localiza-
tion uncertainty. While the robot moves following an already
planned path the localization points appear one by one since
the robot was already moving in that area. The area in which
we search for a loop closing position is a circle with a radius
of 4 m around the robot. Negative value for the current
robots SLAM state Xn used in the experiment is calculated
using the following equation:

Td X , X
 (b)
n j
(X) = (13)
500 Fig. 9. Velocity comparison. (a) ND* active SLAMlinear velocity (top)
and angular velocity (bottom). (b) Switching active SLAMlinear velocity
where T d(X , X ) refers to a topological distance for the SLAM
n j (top) and angular velocity (bottom).
state Xn , explained in Section II-B, while Xj is SLAM state
inside 4 m radius circle. We used 500 to scale the topological Steps 5 and 7 show longer paths where can be seen how the
distance to use it as a edge weight in the graph. The value active SLAM affected robots path according to the initially
was chosen experimentally so the topological distance of 500 planned path. In step 5, one localization point was on the
means the negative cell attracts only one cell around. The new robots initial path while the others were not chosen by the
negative cell points are accepted for ND* path planning only ND* algorithm as a good enough to take a detour to close the
if the negative value of a new points is smaller than the current loop. Step 7 of the exploration process clearly shows deviation
active point or when there is no negative cell in the graph. The from the initially shortest path due to negative cells appeared
first point that appeared is the middle one with the negative close to the robots path. The exploration algorithm finished
value of 15 and the robot was attracted by the negative cell in 13 steps and the whole environment was explored.
and loop was closed. As can be seen the robot did not go The algorithm was implemented on a ThinkPad P50 with
exactly through the marked negative point because we set the Intel Core i7 2.60 GHz processor and 8 GB of RAM. The
loop closing tolerance to 1 m since the robot can successfully algorithm processing time is shown in Fig. 10. For each step
close the loop when it is inside the circle with radius of 1 m of the ND* algorithm, i.e., for each call of the ND* algorithm,
around the loop closing point. The next localization point was the processing time is presented in ms. It can be seen that all
the left one with the negative value of 25 and the robot also calculations were done within 26 ms.
went through that negative point to improve the localization. We have explored the same environment with a switch-
When the loop was closed the most right loop closing points ing active SLAM concept from our previous work described
appeared with the negative edge weight of 21 but the path in [15]. Fig. 9 shows robots angular and linear velocity com-
planning did not change the trajectory since the point was far parison for the switching active SLAM and ND* active SLAM
away from the current robot position. proposed in this paper. For both algorithms the velocities are
 This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.

10 IEEE TRANSACTIONS ON SYSTEMS, MAN, AND CYBERNETICS: SYSTEMS

robots speed does not rapidly change what gives faster robot
motion and faster exploration while simultaneously improv-
ing the localization uncertainty. The shortest path is changed
only locally when the dynamic obstacle or dynamic localiza-
tion pose appears thus the time needed to travel to the goal
position is decreased.

R EFERENCES
[1] Y. Liu, F. Sun, T. Tao, J. Yuan, and C. Li, A solution to active simul-
taneous localization and mapping problem based on optimal control,
in Proc. Int. Conf. Mechatronics Autom. (ICMA), Harbin, China, 2007,
pp. 314319.
[2] C. Leung, S. Huang, and G. Dissanayake, Active SLAM using model
predictive control and attractor based exploration, in Proc. IEEE Int.
Conf. Intell. Robots Syst., Beijing, China, 2006, pp. 50265031.
[3] J. Vallv and J. Andrade-Cetto, Active pose SLAM with RRT*,
in Proc. IEEE Int. Conf. Robot. Autom., Seattle, WA, USA, 2015,
pp. 21672173.
Fig. 10. Processing time for ND* algorithm. [4] R. Valencia, J. V. Mir, G. Dissanayake, and J. Andrade-Cetto, Active
pose SLAM, in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), 2012,
pp. 18851891.
[5] I. Vlasenko, I. Nikolaidis, and E. Stroulia, The smart-condo:
taken at the point when the robot decides to turn from the Optimizing sensor placement for indoor localization, IEEE Trans. Syst.,
planned path to close the loop. The arrows are pointing at the Man, Cybern., Syst., vol. 45, no. 3, pp. 436453, Mar. 2015.
moment when the localization point appears. x-axis represents [6] S. Salan, E. Drumwright, and K.-I. Lin, Minimum-energy robotic
exploration: A formulation and an approach, IEEE Trans. Syst., Man,
discrete time where each time step corresponds to a period of Cybern., Syst., vol. 45, no. 1, pp. 175182, Jan. 2015.
100 ms. It can be seen that with the proposed algorithm the [7] S. Huang, N. M. Kwok, G. Dissanayake, Q. P. Ha, and G. Fang,
robot has continuous and smooth movements in comparison Multi-step look-ahead trajectory planning in SLAM: Possibility and
necessity, in Proc. IEEE Int. Conf. Robot. Autom., Barcelona, Spain,
to the switching SLAM algorithm where the robot needs to 2005, pp. 10911096.
stop, set velocity to zero and then continue toward the loop [8] C. Leung, S. Huang, and G. Dissanayake, Active SLAM in structured
closing point. For each of the velocity profile two loop closing environments, in Proc. IEEE Int. Conf. Robot. Autom., Pasadena, CA,
USA, 2008, pp. 18981903.
points appear while the robot was moving to the goal position. [9] F. Dellaert and M. Kaess, Square root SAM: Simultaneous localization
Besides the velocity profile difference the path planning for the and mapping via square root information smoothing, Int. J. Robot. Res.,
switching active SLAM is done from the beginning when a vol. 25, no. 12, pp. 11811203, 2006.
[10] M. Kontitsis, E. A. Theodorou, and E. Todorov, Multi-robot active
new localization point appears while with ND* we have only SLAM with relative entropy optimization, in Proc. Amer. Control
replanning. The average velocity is higher with ND* what Conf. (ACC), Washington, DC, USA, 2013, pp. 27572764.
brings faster robots movement and together with less time [11] H. Carrillo, Y. Latif, M. L. Rodriguez-Arevalo, J. Neira, and
J. A. Castellanos, On the monotonicity of optimality criteria during
needed for path planning implies faster exploration. With ND* exploration in active SLAM, in Proc. IEEE Int. Conf. Robot. Autom.,
algorithm we had more often loop closing what keeps localiza- Seattle, WA, USA, Jun. 2015, pp. 14761483.
tion uncertainty inside boundaries unlike in switching SLAM [12] C. Stachniss, G. Grisetti, and W. Burgard, Information gain-based
exploration using RaoBlackwellized particle filters, in Proc. Int. Conf.
where the localization uncertainty rapidly drops when the loop Robot. Sci. Syst. (RSS), vol. 1. Cambridge, MA, USA, 2005, pp. 6572.
is closed. [13] R. Sim and N. Roy, Global a-optimal robot exploration in SLAM,
in Proc. IEEE Int. Conf. Robot. Autom., Barcelona, Spain, Apr. 2005,
pp. 661666.
V. C ONCLUSION [14] C. Stachniss, D. Hahnel, and W. Burgard, Exploration with active loop-
closing for FastSLAM, in Proc. IEEE/RSJ Int. Conf. Intell. Robots
In this paper, we presented the path planning for active Syst. (IROS), vol. 2. Sendai, Japan, 2004, pp. 15051510.
SLAM loop closing. The path planning is based on the D* [15] K. Lenac, A. Kitanov, I. Maurovic, M. Dakulovic, and I. Petrovic, Fast
algorithm modification where we introduced negative edge active SLAM for accurate and complete coverage mapping of unknown
environments, in Proc. 13th Int. Conf. Intell. Auton. Syst., Padua, Italy,
weights in a graph for the shortest path planning. Standard 2014, pp. 415428.
graph search algorithms in robotics cannot handle negative [16] V.-C. Pham and J.-C. Juang, An improved active SLAM algorithm
edge weights due to negative cycles which would appear as for multi-robot exploration, in Proc. SICE Annu. Conf., Tokyo, Japan,
2011, pp. 16601665.
a shortest path solution. The active SLAM solution presented [17] K. K. Leung and G. Gallagher, Multi-robot localization and mapping
in this paper can effectively deal with dynamic changes in strategy: Utilizing behavior based dynamic tree structure and observer-
the environment including moving obstacles and localization explorer routine, in Proc. 3rd IEEE Int. Conf. Autom. Sci. Eng. (CASE),
Scottsdale, AZ, USA, 2007, pp. 881886.
demands which can appear while robot is moving. Throughout [18] A. Stentz, Optimal and efficient path planning for partially-known envi-
a simple environment simulation it was shown how negative ronments, in Proc. IEEE Int. Conf. Robot. Autom., San Diego, CA,
cycles are handled with a modification of the D* algorithm. USA, 1994, pp. 33103317.
[19] D. Borrmann et al., A mobile robot based system for fully automated
In the experiment we have shown in a real-world scenario thermal 3D mapping, Adv. Eng. Informat., vol. 28, no. 4, pp. 425440,
with exploration task that the proposed path planning algo- 2014.
rithm for active SLAM efficiently closes a loop planning the [20] A. Kitanov and I. Petrovic, Exactly sparse delayed state filter based
robust SLAM with stereo vision, in Proc. 41st Int. Symp. 6th
path which goes through already visited area. The robots German Conf. Robot. (ISR) Robot. (ROBOTIK), Munich, Germany,
path is continuous and when a localization point appears the 2010, pp. 17.
 This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.

MAUROVIC et al.: PATH PLANNING FOR ACTIVE SLAM BASED ON THE D* ALGORITHM WITH NEGATIVE EDGE WEIGHTS 11

[21] M. R. Walter, R. M. Eustice, and J. J. Leonard, Exactly sparse extended Kruno Lenac (M13) received the M.Sc. degree
information filters for feature-based SLAM, Int. J. Robot. Res., vol. 26, in electrical engineering from the University of
no. 4, pp. 335359, 2007. Zagreb, Zagreb, Croatia, in 2013.
[22] M. Seder, M. Baotic, and I. Petrovic, Receding horizon control for He is a Doctoral Research Fellow with the
convergent navigation of a differential drive mobile robot, IEEE Trans. Faculty of Electrical Engineering and Computing,
Control Syst. Technol., vol. 25, no. 2, pp. 653660, Mar. 2017. Department of Control and Computer Engineering,
[23] S. M. LaValle, Planning Algorithms. New York, NY, USA: Cambridge University of Zagreb. His current research inter-
Univ. Press, 2006. ests include mobile robotics with a focus on
[24] A. Konar, I. G. Chakraborty, S. J. Singh, L. C. Jain, and A. K. Nagar, robot localization and map building of unknown
A deterministic improved Q-learning for path planning of a mobile environmentsSLAM, problem of connecting
robot, IEEE Trans. Syst., Man, Cybern., Syst., vol. 43, no. 5, SLAM with path planning process, known as active
pp. 11411153, Sep. 2013. SLAM.
[25] J.-C. Latombe, Robot Motion Planning. Dordrecht, The Netherlands:
Kluwer, 1991.
[26] S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics. Cambridge,
MA, USA: MIT Press, 2005.
[27] W. A. Kamal and R. Samar, A mission planning approach for UAV
applications, in Proc. IEEE Conf. Decis. Control (CDC), Cancn,
Mexico, 2008, pp. 31013106.
[28] J. Kleinberg and va Tardos, Algorithm Design. Boston, MA, USA:
Pearson, 2006.

Ivan Maurovic (M11) received the B.Sc. and M.Sc.

degrees in electrical engineering from the University
of Zagreb, Zagreb, Croatia, in 2008 and 2010,
respectively.
He joined the Department of Control and
Computer Engineering, University of Zagreb, in
2010, as a Research Assistant of SEE-ERA.NET
PLUS Project ThermalMapper, where he has been
a Researcher of ACROSS Project, since 2012, and
is currently a Researcher with the LAMOR Research Ivan Petrovic (M97) received the B.Sc., M.Sc.,
Group. His current research interests include mobile and Ph.D. degrees in electrical engineering from
robotics, exploration of unknown environmets, path planning, and localization. the University of Zagreb, Zagreb, Croatia, in 1983,
1989, and 1998, respectively.
He was an Research and Development Engineer
Marija Seder (M05) received the M.Sc. and with the Institute of Electrical Engineering, Koncar
Ph.D. degrees in electrical engineering from the Corporation, Zagreb, from 1985 to 1994. Since
University of Zagreb, Zagreb, Croatia, in 2004 and 1994, he has been with the Faculty of Electrical
2010, respectively. and Computing Engineering, University of Zagreb,
She was a Visiting Researcher with the where he is currently a Full Professor. He teaches a
Autonomous Intelligent System Group, University number of undergraduate and graduate courses in the
of Freiburg, Freiburg im Breisgau, Germany, field of control systems and mobile robotics. He has published over 40 journal
from 2012 to 2013, under the supervision of and 160 conference papers, and results of his research have been implemented
Prof. W. Burgard. She is currently an Assistant in several industrial products. His current research interests include various
Professor with the Faculty of Electrical Engineering advanced control strategies and their applications to control of complex sys-
and Computing, Department of Control and tems and mobile robots navigation.
Computer Engineering, University of Zagreb. Her current research interests Dr. Petrovic is the Editor-in-Chief of the Automatika Journal. He is a
include mobile robotics, especially motion planning, path planning, coverage member of the IFAC Technical Committee on Robotics, the FIRA Executive
planning, obstacle avoidance, and environment exploration. Committee, and the Croatian Academy of Engineering.