Expert Systems with Applications 159 (2020) 113547
Contents lists available at ScienceDirect
Expert Systems with Applications
journal homepage: www.elsevier.com/locate/eswa
Simultaneous localization and mapping using swarm intelligence based
methods
Nadia Nedjah a,⇑, Luiza Macedo Mourelle de b, Pedro Jorge Albuquerque de Oliveira a
a
b
Department of Electronics Engineering and Telecommunication, Engineering Faculty, State University of Rio de Janeiro, RJ, Brazil
Department of System Engineering and Computation, Engineering Faculty, State University of Rio de Janeiro, RJ, Brazil
a r t i c l e
i n f o
Article history:
Received 9 September 2019
Revised 8 May 2020
Accepted 9 May 2020
Available online 13 May 2020
Keywords:
SLAM
Scan matching
PSO
FA
ABC
a b s t r a c t
The problem known as simultaneous localization and mapping is of fundamental importance both in its
own right and because of its potential applications in the development of autonomous robots. While
many solutions exist that are based on classical Newton-like optimization techniques regarding scan
matching, relatively no work has been done with respect to the application of derivative free bioinspired techniques to this particular area of robotics. That being said, we propose a novel approach to
the scan-matching step within the Simultaneous Localization And Mapping (SLAM) problem based on
the exploitation of swarm intelligence. For this purpose, we have chosen three swarm intelligence optimization methods to be the subjects of our research investigation, namely particle swarm optimization,
artificial bee colony and the firefly algorithm. Aiming at reducing further the translational and rotational
scan alignment errors, the proposed scan matching proceeds in two main steps, namely scan-to-scan and
scan-to-map matching. Furthermore, we have made use of a pose graph based approach as a means of
maintaining the consistency of our estimates across the mapping process. Systems designed to perform
simultaneous localization and mapping using pose graphs are currently the state of the art and it is our
belief that a robust scan-matching system is currently of the utmost importance to further the field. We
tested the proposed solution in many scenarios. We conclude that the artificial bee colony strategy is efficient in a large range of circumstances. This affirmation is backed by the fact that in the best case scenarios, we have obtained good and some times better accuracy gains, regarding the translational and
rotational estimates of the robot’s trajectory, by using the this meta-heuristic, when compared to state
of the art SLAM systems. The firefly algorithm, while not as accurate as the artificial bee colony technique,
was faster on 7 out of the 8 public domain datasets. The firefly algorithm consumed, on average less time
spent per scan than by the artificial bee colony optimization technique. Particle swarm optimization has
shown an inferior accuracy when compared to the artificial bee colony optimization technique and an
intermediate processing time when compared to the other two optimization methods. So, firefly based
meta-heuristic can be considered as the technique that provides a good trade-off between the high accuracy offered by the artificial bee colony and the high execution speed of the particle swarm optimization.
Ó 2020 Elsevier Ltd. All rights reserved.
1. Introduction
The Simultaneous Localization and Mapping (SLAM) problem
deals with the intertwined tasks of estimating the trajectory of a
moving agent and a map of the environment in which it takes
place. This area of research has received a great deal of attention
from the robotics research community in particular and the expert
and intelligent systems research community in general, over the
⇑ Corresponding author.
E-mail addresses: nadia@eng.uerj.br (N. Nedjah), ldmm@eng.uerj.b (L. Macedo
Mourelle de), pedro.jorge@uerj.br (P.J. Albuquerque de Oliveira).
https://doi.org/10.1016/j.eswa.2020.113547
0957-4174/Ó 2020 Elsevier Ltd. All rights reserved.
past few decades due to its fundamental role in the construction
of truly autonomous robots (Thrun, Burgard, and Fox, 2005). The
problem itself has been cast into many different forms. The abundance of formulations is due in great part to apply specific techniques aiming at maximizing the performance of each solution in
its particular application niche or perhaps in an exploratory fashion, thus enabling the analysis of the behavior of specific methods.
In this work we present a system capable of solving the SLAM
problem by making use of scan-matching procedure based on
swarm intelligence optimization meta-heuristics.
In the case of SLAM a map takes the role of world model, and its
estimation is conjugated to the inference of the robot’s trajectory
2
N. Nedjah et al. / Expert Systems with Applications 159 (2020) 113547
therein. The map may take many different forms such as feature
maps, occupancy grid maps, and point clouds. Independently of
its form, the construction of a coherent world model, however it
may present itself, begets the need to obtain information from various locations in the environment one seeks to model. For the purpose of analyzing the relations between the data available, the data
may be regarded as belonging to data frames, where each data
frame could be, for example, a set of laser range scan data, a set
of photographs taken from the same vantage point, a group of
sonar readings or whatever other form of data the sensors may
provide. The acquisition of these data frames leads to another sort
of challenge because the measurements gathered are always
expressed in a local reference frame of an entity whose position
is unknown and whose very task requires motion. In the case of
laser scan data, as was used in the present work, the sensor returns
a set of distances to objects that lie within a specific angular sector
around the robot, and it is the role of the SLAM back end to take
that information and in as efficient and accurate a manner possible
return a data frame that may be utilized to update the global
model, wherefore a procedure capable of performing the alignment
of data frames (scan matching) must be employed (LLu and Miliosu
and Milios, 1997).
Scan matching and the closely related image registration problem are a fundamental part of many methods developed to address
the problems of simultaneous localization and mapping, structure
from motion, object recognition, image fusion and 3D modeling. In
most cases, the goal is to find a transformation that leads to maximum overlap between two sets of measurements taken from different viewpoints, or perhaps, from a single vantage point at two
different instants in time or even between a set of measurements
and a global model being constructed. In the context of SLAM, such
a procedure is usually employed either to recover, or to enhance a
gross estimate of the relative motion observed between two data
frames. In the specific case of laser range data, the problem of
aligning measurements is often called scan alignment or scan
matching. There are many different formulations of the scanmatching problem, as touching the form taken by the data and
the pattern to be aligned. The most commonly encountered such
variations are: scan-to-scan; scan-to-line; line-to-line; scan-tomap; map-to-map. The distinction between these formulations,
and especially their reason for being, is but one of type, and level
of complexity, of the pre-processing performed on the sensed data.
It is thus sufficient to note that each one tries to exploit a particular
characteristic of the data in order to get a better result under a
specific set of conditions. For instance, the scan-to-line formulation
is expected to have a better performance than the simple alignment of raw scan data, if the data in question has a strong linear
trend, but may perform poorly if the entries of the covariance
matrix of the scan data are of similar magnitude. The map-tomap approach tries to exploit all of the available data and is usually
seen in the form of correlative matching (Thrun et al., 2005).
Nonetheless, for all its advantages, like less sensitivity to outliers
and more effective usage of past information, the large number
of floating point operations it requires, makes it computationally
expensive to perform, in some cases prohibitively so. This is said,
it is of utmost importance to emphasize that the performance of
the implementation of the scan-matching step is the main bottleneck for any SLAM solution. Hence, it is safe to establish that any
improvement in terms of accuracy and/or execution speed regarding scan matching is most welcome in any SLAM system.
In this paper we present a solution to the SLAM problem that is
based on scan matching through bio-inspired derivative free optimization methods. The advantage of such an approach is apparent
from the well-known multi-modality of the scan-matching problem. As far as we are concerned, this approach has not been investigated before. Furthermore, we have employed a graph-based
back-end to allow this system to be consistent in exploration sessions that are either too long or cover too large an area. In both
cases, a solution based exclusively on scan matching is known to
fail due to the incremental nature of the estimation error. In addition, graph-based SLAM systems are currently considered state of
the art due to their accuracy, capacity to recover from association
errors and the speed of execution, permitted by recent advances in
computational methods used in sparse linear algebra.
This work provides contributions both in the form of a comparison of the methods employed, which opens new exploration paths
within this relatively unexplored sub-field of the SLAM research,
and in the system developed itself that, as will become apparent,
has demonstrated a comparable performance to well-known
approaches to the SLAM problem. Indeed, our proposal has, in
the scenarios we tested, either matched the accuracy of other systems within this particular field or exceeded it. Moreover, in all
cases over the course of the experiments conducted we have
attained a more consistent response, as demonstrated by the fact
that the coefficient of variation of the error metric employed has
been smaller than all other systems. This substantiates our claim
that a potential application of the method presented herein would
be to treat SLAM where consistency of the responses, and the ability to correctly predict the performance of the system, is more
important than raw speed or accuracy.
The rest of this paper is organized in sections. First, in Section 2,
we make the tour of relevant existing works related to solving the
SLAM problem. Then, in Section 3, we give a general overview of
the slam problem and related challenges, including localization
and mapping. After that, in Section 4, we describe the main steps
of the optimization techniques used, which are based on swarm
intelligence. Subsequently, in Section 5, we explain the proposed
method to perform robot SLAM efficiently yet accurately using
computational swarm intelligence. There follows, in Section 6,
the performance results and their analysis, discussion and comparison with existing state-of-the-art solutions. Finally, in Section 7,
we draw some conclusions of the described works and point out
some promising future directions to improve the proposed
methodology.
2. Related works
In robotics, the SLAM problem was introduced through a series
of papers by Cheeseman et Al., in which the extended Kalman filter
(EKF) SLAM algorithm was first described (SSmith and
Cheesemanmith and Cheeseman, 1986) and revisited in Smith,
Self, and Cheeseman (1990).
The first mention of relative graph-like constraints in the SLAM
literature date back to the publications by Cheeseman and Smith in
1986, SSmith and Cheesemanmith and Cheeseman (1986). These
approaches, however, lacked any form of global relaxation or optimization in their approach to solve the problem. Historically, the
representation of the SLAM prior as a set of links between robot
poses was first presented in a seminal paper by LLu and Miliosu
and Milios (1997). It is, indeed, in LLu and Miliosu and Milios
(1997) that, for the first time, the SLAM problem was cast in the
form of a global optimization problem and where the first algorithm to derive a map from such a set of constraints was first presented. Unique to their approach was the usage of the robot’s pose
variables as the frame of reference, which greatly diverges from the
point of view of standard EKF based solutions where the poses are
integrated out. In the rest of this section, we give a chronological
retrospective of the main related works of the state-of-the-art.
In BBesl and McKayesl and McKay (1992), the most frequently
cited reference for the Iterative Closest Point (ICP) algorithm, the
authors present numerous formulae for calculating the distance
N. Nedjah et al. / Expert Systems with Applications 159 (2020) 113547
between different representations of the geometric data under
consideration and proceed to show in what manner those distance
measures can be utilized within the framework of the ICP algorithm. In addition, the authors also propose an Accelerated ICP
and perform an analysis of the convergence properties of both
algorithms (Luo, 2017).
In LLu and Miliosu and Milios (1997), the authors present a
comprehensive body of work on the subject of consistent laser scan
alignment and registration employing the concept of pose graphs,
and thus utilizing Pose Graph Optimization (PGO). The authors do
affirm that by maintaining a complete record of all the data frames
acquired while a mapping agent moves through an environment it
is possible to utilize the information there contained to construct a
network of relations between those sets of sensed data and, by
combining odometric information and scan matching based constraints, a map of said environment. The map construction procedure is made possible by assuming that the trajectory most likely
to be correct is the one that minimizes what they call the ‘‘network
energy”.
In BBiber and Strasseriber and Strasser (2003), the authors propose an alternative representation for a range scan, which termed
the Normal Distributions Transform (NDT), which can be considered as a point-cloud matching method. This representation is similar to an occupancy grid as it subdivides the 2D plane into cells,
and associates to each entry a normal distribution that locally
models the probability of measuring a point. This results into a
piecewise continuous and differentiable probability density that
can be used to match a given scan using Newton’s algorithm. So,
no explicit correspondences have to be established.
In Nieto, Bailey, and Nebot (2007), the authors describe an
approach to SLAM based on the joint application of extended Kalman filters and scan matching. The method, called Scan-SLAM, was
devised with the intent of generalizing EKF-SLAM’s applicability to
environments with markers that could be defined by simple geometrical models. The authors also proposed that the markers could
be defined by templates comprised of unprocessed measurements.
They defend the idea that templates, thus defined, may exploit
most of the information contained within scans to, for example,
improve the information contained in the templates along the
operation of the system. Moreover, given the way the authors
defined these templates, it is possible to envision the application
of data fusion to create higher dimensional templates, an example
of which could be the fusion of camera image and metric data from
laser range finders.
In Grisetti, Stachniss, and Burgard (2007), a system called
GMapping based on Rao-Blackwell particle filters (RBPF) is proposed. However, it is noteworthy to point out that an RBPF has
been exploited earlier in the FastSLAM algorithm (Montemerlo,
Thrun, Koller, and Wegbreit, 2002), wherein landmark maps are
used instead of grid maps. In general, Rao-Blackwell particle filters
based algorithms break down the state space into a analytical part
and a non-analytical part, which are interconnected to form a filter
bank. It is a well-known fact that several implementations of RaoBlackwell particle filters require a high memory consumption, considering that each particle must store an environment map with it.
It is therefore of great interest to use the smallest number of particles necessary for the effective operation of the SLAM system.
The system proposed in Grisetti et al. (2007) uses a occupancy grid
map environment representation. The fundamental notion used to
reduce the number of particles needed is the Rao-Blackwell theorem, which claims that it is possible to initially estimate the robot’s
trajectory and then use it to generate the map, which is strongly
dependent on it. In effect, this factoring makes it possible to obtain
the map efficiently, since both the pose and the observations are
known. A significant improvement offered by GMapping is its proposed distribution for sampling. In general, systems based on par-
3
ticle filters use only information from the odometry-based motion
model. However, the proposed system in Grisetti et al. (2007) uses
in addition to these data, the last measurement performed, thus
focusing on sampling in the most likely regions.
In DDiosi and Kleemaniosi and Kleeman (2007), the authors
propose an algorithm, which they denote as polar scan matching
(PSM). In this algorithm, the measurements obtained by means
of a laser sensor are aligned as polar coordinates. As this kind of
sensor measures distances to the nearest obstacles in a scan along
a predetermined angular space, the use of the proposed method
makes the repeated transformation of the measured values for a
Cartesian system unnecessary. Furthermore, it was possible to take
advantage of the fact that in a polar system, displacements along
the angular coordinate correspond to rotations in a Cartesian system. This fact that can be used to estimate orientation in a SLAM
system that applies this technique.
In Censi (2008), the authors describe an algorithm, called pointto-line ICP (PLICP), for scan matching based on the usage of a point
to line metric which, they claim, allows them to better gauge the
distance between the point and the surface whereof the scan was
obtained. The authors proceed to describe a closed form solution
to the minimization of this metric and affirm the algorithm that
employs it converges quadratically in the number of iterations,
unlike the classical ICP that converges linearly in the number of
iterations (BBesl and McKayesl and McKay, 1992).
In Segal, Haehnel, and Thrun (2009), the authors combine the
Iterative Closest Point ICP (BBesl and McKayesl and McKay, 1992)
and point-to-plane ICP (CChen and Medionihen and Medioni,
1992) algorithms into a single probabilistic framework, called Generalized ICP (GICP). Then, they use this framework to model locally
the planar surface structure obtained from both scans instead of
just the model scan as is typically done with the point-to-plane
method (CChen and Medionihen and Medioni, 1992). This
approach can be thought of as plane-to-plane scan matching. This
approach proved to be more robust to inaccurate associations, and
thus allowed a better tunning regarding the maximum match distance parameter present in most variants of ICP.
In Olson (2009), the computational power available to modern
robots is taken advantage of as it motivates the investigation of
correlative scan-matching solutions that examine quality vs. complexity trade-offs. The authors propose a probabilistic-based scanmatching algorithm that produces higher quality and more robust
results at the cost of additional computation time. The approach
casts the problem in a probabilistic, attempting to find the correct
transformation that maximizes the probability of having the
observed data. They describe several novel implementations of
the proposed approach that achieve real-time performance on
modern hardware, including a multi-resolution implementation
for conventional CPUs and a parallel implementation for graphics
processing units (GPUs).
In CChatterjee and Matsunohatterjee and Matsuno (2010), the
authors exploit a proposed modified Particle Swarm Optimization,
called Geese PSO algorithm, to tune a fuzzy supervisor for an adaptive EKF based approach to solve the SLAM problems for mobile
robots. They demonstrate that the proposed approach provides
better estimation and map-building performance in comparison
with those fuzzy supervisors for the adaptive EKF algorithm, where
the free parameters of the fuzzy systems are tuned using basic PSO
based algorithm. The utility of the proposed approach is aptly
demonstrated by employing it for several benchmark environment
situations with various numbers of waypoints and landmarks,
where the Geese PSO algorithm could tune the fuzzy supervisor
better than the canonical PSO based algorithm.
In Chatterjee, Ray, Chatterjee, and Rakshit (2011), the authors
demonstrates a successful real implementation of an EKF based
SLAM algorithm for indoor environments, utilizing two web-cam
4
N. Nedjah et al. / Expert Systems with Applications 159 (2020) 113547
based stereo-vision sensing mechanism. The vision-sensing mechanism used was a successful development of a real algorithm for
image feature identification in frames grabbed from continuously
running videos on 2 cameras, tracking of the identified features
in input frames and incorporation of these landmarks in the
yielded map. They utilize a 3D distance computation method.
The system was test-run in laboratory environments wherein the
robot is commanded to navigate through a given specified trajectory defined by waypoints and create a map of its surrounding
environment.
In Mingas, Tsardoulias, and Petrou (2012) the authors propose a
solution to the SLAM problem via scan matching performed by
means of a Genetic Algorithm (GA). In their work, the authors
applied a raw scan to global occupancy grid map matching procedure. Furthermore, a Field Programmable Gate Array (FPGA) based
implementation is proposed and compared to a pure software version running on a general-purpose computer. The results presented
were promising, but after analysis, the authors claimed that the
method weaknesses regarding rotational error accumulation of
and the difficulty to correctly determine rigid transformations in
environments with little visual changes may be verified in spite
of large motions.
In Mutz et al. (2016), the authors present an end-to-end framework for precise large-scale mapping with applications in autonomous car driving. In special, they study the problem of mapping
complex environments, with features changing from tree-lined
streets to urban areas with dense traffic. The used robotic car is
equipped with an odometry sensor, a 3D light detection and ranging device (LiDAR) and a low cost global positioning system (GPS).
The proposed approach integrates the data generated by these sensors into a pose-based GraphSLAM estimator. They contributed a
new strategy for identification and correction of odometry data
using evolutionary algorithms is presented. This new strategy
makes odometry data significantly more consistent with GPS. Loop
closures are detected using GPS data, and Generalized ICP (GICP), a
3D point cloud scan-matching registration algorithm to estimate
the displacement between the different travels over the same
region (Segal et al., 2009). After path estimation, the 3D LiDAR data
is used to build an occupancy grid mapping of the environment.
The proposed framework is tested in real-world environments
with different characteristics, whereby the results were satisfactory regarding precision in loop closures even when the vehicle
traveled long distances.
In Akai, Morales, Takeuchi, Yoshihara, and Ninomiya (2017),
NDT (BBiber and Strasseriber and Strasser, 2003) is extended to
3D NDT, wherein it is proposed to estimate the error of NDT scan
matching beforehand (i.e. off-line), and while the vehicle navigates
in the environment, the appropriate uncertainty is assigned to the
scan matching. The off-line estimate is combined with a roadmarker matching approach using a particle-filtering algorithm. It
further improved the localization estimates in the final SLAM.
In Jeong, Yoon, and Park (2018), the authors propose a new
approach to address the semantic 3D mapping, which is mainly
composed of 3D reconstruction and semantic segmentation. They
use 2D convolutional neural network for semantic segmentation.
To build a semantic 3D map, they integrate the 3D map with
semantic information, using coordinate transformation and Bayes’
update scheme, which is improved via a 3D refinement process to
correct inaccurate segmented voxels and remove traces of moving
vehicles in the 3D map. Through experiments on challenging
sequences, they demonstrate that the proposed method outperforms state-of-the-art methods in terms of accuracy and intersection over union (IoU).
In Zhang, Wang, and hai Chen (2019), the authors address the
self-localization that allows a mobile robot to identify and keep
track of its own position and orientation as the robot moves
through the environment. They propose a hybrid localization
approach based on the particle filter and particle swarm optimization algorithm. Their methodology is focused on the localization
tasks when an a priori environment map is available. The work
yielded an accurate and robust particle filter based localization
algorithm that is able to work in symmetrical environments. The
performance of the proposed approach is efficient in indoor robot
localization.
3. The SLAM problem
The SLAM problem consists of the concurrent estimation of the
robot trajectory and a map of the environment in which a robotic
entity moves in the course of its operation. A solution to this problem is required in situations in which a robot has no access to its
trajectory and/or to a map of the environment wherein it moves.
Any SLAM system needs to provide a solution to the trajectory estimation, i.e. solve the localization problem, and to the approximate
environment map building, i.e. solve the mapping problem. In Section 3.2, we introduce the required concepts to tackle the localization problem. Subsequently, in Section 3.3, define the necessary
elements to approach the mapping problem. It is noteworthy to
point out that the purpose of this section is completeness. It points
to the main techniques that have been applied later to form the
basis concepts for understanding the proposed SLAM implementation, described in Section 5.
3.1. Coordinate systems
SLAM aims at describing some region’s map and the robot’s trajectory during the mapping operation. To this end, it is essential
that we are able to adequately express the full range of movements
and coordinate changes necessary both by the view change and
restrictions due to the used sensors. The task establishes a dichotomy between the robot’s local coordinate system and the one in
which the map (the global system) is expressed. The analysis of
the relationship between these two systems is of fundamental
importance, not only because it is essential to new data integration, as obtained during robot movement, but also because the
relationship between two sets of measurements containing overlap determines, at least approximately, the relationship between
the coordinate systems in which both are obtained. Determining
a transformation that maximizes the overlap between two data
sets allows for a rotational translation that approximates the
movement performed by the robot (or its reverse). Generally
speaking, the set of isometries that represent rigid movements in
n dimensions has nðn1Þ
degrees of freedom. In the case of SLAM,
2
this means that the complete determination of a pose in n dimenrotational coorsions requires n translational coordinates and nðn1Þ
2
dinates (Woods, 2005). In this work, we exploit the homogeneous
coordinates used in projective geometry, with wide application in
the field of computer graphics and computer vision (Zhang, 1994).
3.2. Localization
This section deals with the theoretical rationale underlying the
use of projective coordinate systems to define the pose of the
robot, which represents the effective robot pose at a given time
instant, as the graph of poses, which registers the pose history
and allows the map construction. In Section 3.2.1, we provide the
formal definition and characteristics of a robot pose and the usual
operations that apply. Subsequently, in Section 3.2.2, we describe
and define the pose graph, as used in this work and how it can
be optimized.
5
N. Nedjah et al. / Expert Systems with Applications 159 (2020) 113547
3.2.1. Pose
The concept of pose refers to a set of coordinates that are sufficient to determine the location and orientation of the robot in relation to the environment. In the case of confined systems moving in
a two-dimensional R3 subspace, the pose must contain three coordinates. If a Cartesian system is considered to be R2 , the pose can
be completely determined by two translational coordinates and
an angle specifying the orientation of the system. However, if the
robot moves in a three-dimensional subspace of R3 , it is necessary
to specify six coordinates so that the position and orientation of the
robot is completely determined. In this case, the complete determination is given by three translational coordinates and three rotational coordinates.
Let V b ¼ ðxb ; yb ; hb Þ be a two-dimensional pose. If we consider a
pose D ¼ ðx; y; hÞ, which represents a variation of pose V b that
transforms it into pose V a ¼ ðxa ; ya ; ha Þ, we would have the composition of V b with D to be denoted by V a ¼ V b D, and defined as in
Eq. 1:
3 2 3 2 3 2 3 2
3
xb
xb
x
xa
x cos hb y sin hb
6 7 6 7 6 7 6 7 6
7
4 ya 5 ¼ 4 yb 5 4 y 5 ¼ 4 yb 5 þ 4 x sin hb þ y cos hb 5:
h
ha
hb
hb
h
2
ð1Þ
Considering that an absolute pose defines a coordinate system,
and that a relative pose defines a change in the coordinate system,
composed of a translation followed by a rotation, the pose composition operation determines the new coordinate system, obtained
after the transformation. Similarly, it is possible to define the
inverse operation of pose composition. This operation requires
two poses and returns the relative pose. Such an operation is
denoted by D ¼ V a V b , and its effect is defined by Eq. 2:
3
2 3 2 3 2 3 2
x
xb
xa
ðxa xb Þ cosðhb Þ þ ðya yb Þ sinðhb Þ
7
6 7 6 7 6 7 6
4 y 5 ¼ 4 ya 5 4 yb 5 ¼ 4 ðxa xb Þ sinðhb Þ þ ðya yb Þ cosðhb Þ 5;
ha
h
mp ðha hb Þ
hb
ð2Þ
wherein function mp ðdÞ is required because the orientation of the
resulted pose need to be normalized to ½p; pÞ. It produces the
smallest angle needed to rotate ha into hb , respecting the periodic
interpretation of angles, and is defined as in Eq. 3 (Hertzberg,
Wagner, Frese, and Schroder, 2013):
mp ðdÞ ¼ d 2p
dþp
:
2p
ð3Þ
With the pose difference operation, it is also possible to define
for a relative pose Dab ¼ V a V b its inverse relative pose Dba using
the definition of Eq. 4:
Dba ¼ Dab ¼ ð0; 0; 0Þ Dab ;
ð4Þ
wherein the notation Dab is taken directly from LLu and Miliosu
and Milios (1997) and, even though it may be considered a notation
abuse, has been retained for historical reasons.
We need also to define the composition between a pose, say
Va ¼ ðxa ; ya ; ha Þ and a point, say u ¼ ðx; yÞ, denoted as
u0 ¼ ðx0; y0Þ ¼ V a u, which results in the same point expressed in
the coordinate system. The transformation will be analogous to
that given in Eq. 1. The composition operation between a pose
and a point in two dimensions is defined by Eq. 5:
x0
y0
¼
xa
ya
x
y
¼
xa
ya
þ
x cos ha y sin ha
x sin ha þ y cos ha
:
ð5Þ
Having in mind possible transformations in a projective space, it is
interesting to note that a pose can be specified as a rigid body transformation. Likewise, operations between poses can be specified as
transformation compositions. For instance, if we have pose
V ¼ ðx; y; hÞ, it can be specified in an equivalent manner by the
transformation matrix displayed in Eq. 6:
2
cosðhÞ sinðhÞ x
6
M ¼ 4 sinðhÞ
0
3
cosðhÞ
7
y 5:
0
1
ð6Þ
So, given the definition in Eq. 6, it is possible to express all operations between poses by matrix operations.
3.2.2. Pose graph
The methods used to solve the SLAM problem that are take
advantage of a pose graph are based on a formulation of the SLAM
problem in terms of a graph of relationships between various
frames in which the robot has met over time. These methods
address the problem of state estimation as an optimization problem (LLu and Miliosu and Milios, 1997; Kaess, Ranganathan, and
Dellaert, 2008; Grisetti, Kummerle, Stachniss, and Burgard, 2010;
Kümmerle, Grisetti, Strasdat, Konolige, and Burgard, 2011). Generally, these kind of solutions deal with the full SLAM problem,
where the objective is to estimate the complete trajectory of the
robot that is performing the mapping task in addition to determining the current pose and the environment map. These techniques
tend to produce more consistent estimations than those based on
statistical filtering techniques. This is because techniques such as
Kalman filters (Jiang, Li, and Yu, 2016), particle filters (Grisetti
et al., 2007), information filters (Walter, Eustice, and Leonard,
2007), and their counterparts are inherently subject to inconsistencies arising from linearization errors caused by inaccurate, or even
erroneous, estimates of previous poses. The key point is that filtering techniques perpetuate these inaccuracies or errors, which is
intrinsic to the way they operate. In contrast, graph based techniques, sometimes called smoothers, are able, by virtue of their
solution to the full SLAM problem, to revise estimates for the entire
trajectory based on new evidence, a hence producing results of
higher accuracy and consistency. It is worth mentioning here, as
an addendum, that information filters can also make this kind of
correction by operating off-line and over the entire dataset
(Thrun et al., 2005). Nevertheless, this may take a reasonably long
time because of the combinatorial nature of the problem.
Most graph-based SLAM algorithms are structured into two
main and complementary components: a front-end, which is also
called the scan alignment system; and a back-end, which is also
called graph manipulation and optimization system. The scan
alignment system is able to extract constraints between problem
variables, such as poses and landmarks, from the sensory information by detecting points of interest and aligning the corresponding
scans. The graph manipulation and optimization system is responsible for optimizing the set of constraints, which is actually finding
the configuration of the problem variables that occasion the smallest possible error. In effect, the system that optimizes the set of
constraints is the one that performs the optimization of the pose
graph system optimization. Constraints in the context of graphbased SLAM algorithms are sets of isometries that relate poses or
observations. Regarding the relations between poses, there are
both odometry-derived constraints, which can either be enhanced
or not by the scan matching, and constraints imposed by observations. Restrictions imposed by observations are created by observing the same location from two different poses. In fact, if we adopt
the convention that poses are expressed as transformations in a
projective space, and assume that pose X i corresponds to the i-th
pose, then an odometer constraint between poses X i and
X j ¼ X iþ1 would take the form X 1
X iþ1 . In the case of constraints
i
issued from observations, we would have the constraint defined
X j , given two poses X i and X j that are related by one obseras X 1
i
vation. We also take advantage of the graph-based SLAM method
6
N. Nedjah et al. / Expert Systems with Applications 159 (2020) 113547
contributed in Grisetti et al. (2010). It works iteratively to optimize
the most likely node configuration. The algorithm requires the initial node pose estimates, the constraints, and their respective
covariance matrices (uncertainties). It returns the updated set of
poses x and the information matrix H . The procedure iterates
the given steps until convergence occurs. At each iteration, an
information matrix and an information vector are computed. At
each iteration, it starts with the null matrix H and a null vector
b. Then, for each constraint, vector b and matrix H are updated.
The linear system HDx ¼ b is solved, which allows to yield the
value of the upgrade step for node configuration Dx. At the end
of each iteration, we have H ¼ H and x ¼ x. Once there is convergence the process is terminated and x and H are returned. This
kind of technique usually produces topological maps, i.e. maps that
are focused on the representation of the relationship between
places of interest rather than their metrical representation.
Although it is possible to work the later to obtain other types of
spatial representations such as described in Section 3.3.2.
3.3. Mapping
This section deals with the theoretical rationale underlying the
use the current pose as well as the pose graph assembled so far to
map the environment where the robot is inserted. This includes the
scan alignment, the map internal representation and update, and
detection of previously visited locations to achieve loop closure
properly. First, in Section 3.3.1, we discuss the required concepts
related to the scan-matching problem. After that, in Section 3.3.2,
we explain how maps can be represented and analyze pros and
cons. Then, in Section 3.3.3, we briefly describe the search for nearest neighbors problem.
3.3.1. Scan matching
Because scan matching is at the heart of this work, we dedicate
this section to explaining the idea behind such a process and its
functionality as well as the different methodologies taken to
approach it. It is noteworthy to point out that here we use a Light
Detection And Ranging device (LiDAR).
The scan-matching process aims to determine the best transformation between two point clouds obtained by some sensing
instrument. Scan-matching usage, as a way of estimating the
movement between two poses, from which measurements were
taken, is effective as long as we can rely on the accuracy of both
measurements and on the convergence of the underlying optimization process, within a reasonable time.
There are several effective scan-matching algorithms. These
algorithms employ heuristics in order to quickly to reach a matching result. In Cox (1991) points are used as primitives and matched
to lines that are included in the model. In BBesl and McKayesl and
McKay (1992), the Iterative Closest Point (ICP) algorithm is proposed. Therein, the authors present several ways for calculating
the distance between different representations of the geometric
data under consideration. In CChen and Medionihen and Medioni
(1992) considered the more specific problem of aligning range data
for object modeling. The proposed approach therein takes advantage of the tendency of most range data to be locally planar. This
algorithm is considered to be the point-to-plane variant of ICP. In
GGutmann and Schlegelutmann and Schlegel (1996), the lines are
extracted from the scans. This work was later revisited, wherein
the lines extracted from a scan are matched to those in the model
(Gutmann, Weigel, and Nebel, 1999). In LLu and Miliosu and Milios
(1997), a general approach, which matches points to points, is
introduced. This is essentially a variant of the ICP (Iterated Closest
Point) algorithm (BBesl and McKayesl and McKay, 1992) applied to
laser scan matching. In BBiber and Strasseriber and Strasser (2003),
the NDT algorithm is proposed. It models the distribution of all
reconstructed 2D points of one laser scan by a collection of local
normal distributions. In Olson (2009), a probabilisticallymotivated scan-matching algorithm that produces higher quality
and more robust results at the cost of additional computation time
is presented. In Akai et al. (2017), the accuracy of the NDT algorithm is improved allowing an off-line estimation of the matching
error, which is corrected later in the scan-matching process. In
Segal et al. (2009), the Generalized-ICP is based on attaching a
probabilistic model to the last minimization step used in ICP, keeping the rest of the algorithm unchanged to reduce complexity and
maintain computation speed. So it only deals with the iterative
computation of the transformation to improve accuracy. In
DDiosi and Kleemaniosi and Kleeman (2007), the scan matching
PSM algorithm aligns the measurements obtained by a laser sensor
with polar coordinates, making the repeated transformation of the
measured values for a Cartesian system unnecessary. In Censi
(2008), the scan matching algorithm PLICP, which is based on the
usage of a point-to-line metric, allows for a more accurate distance
evaluation between the point and the surface whereof the scan
was obtained.
Existing scan-matching algorithms, although reasonably efficient, they are very sensitive to initialization. In BBesl and
McKayesl and McKay (1992), it is ensured that the ICP process
always converges to a local maximum. In addition, it is noteworthy
to point out that an inadequate initialization can cause the alignment process to fail or to be inefficient. One way to mitigate this
problem, and to prevent these algorithms from converging to optimal locations, is to initialize the algorithm with an estimate that is
believed to be close to the overall optimal alignment. Another way
is to use a more robust alignment search method rather than simply applying the method described in BBesl and McKayesl and
McKay (1992).
Given the advantages of applying techniques that are not only
able to fully exploit the search space but also to exploit the solution space independently of function derivatives, and taking into
account the fact that different heuristics present characteristics
that make them better suited to solving particular types of problems, we decided that it would be advantageous and positively
contributive for our work to investigate swarm optimization techniques to implement the scan-matching problem.
In line with the objective of comparing the performance of
swarm optimization meta-heuristics, we selected the part of the
system, which is responsible for scan alignment to be implemented
in such a way in order to allow the interchangeable application of
any selected techniques. In this work, a map representation is used
as an occupancy grid, and the scans are represented as point
clouds. This option was taken because it allows the possibility to
align scans (with each other) with less effort than necessary to construct two local maps and later align them. Moreover, this type of
representation allows the advantages of both representations to be
used. Given that the system preserves all information regarding
the trajectory, it is always possible to perform the conversion
between a points cloud in an occupancy map. The disadvantage,
however, is that it is necessary to perform frequent searches for
correspondence between clouds that can entail a high computational cost. The computational burden of searching for closer
neighbors has been reduced by using KD-tree of posses instead
of a simple graph (Friedman, Bentley, and Finkel, 1977; BBentley
and Friedmanentley and Friedman, 1979; Bentley, 1980). We use
the so-called Cauchy loss function, which is presented, in terms
of the quadratic error.
3.3.2. Occupancy maps
Occupancy maps are a form of spatial representation based on
space discretization i.e. associates one cell to each space region,
N. Nedjah et al. / Expert Systems with Applications 159 (2020) 113547
each of which can be classified either as full or empty. This type of
spatial representation is based on the hypotheses of world staticity, independence between random variables that represent the
map, and the fact that a cell can be completely full or completely
empty. In other words, the presence of an obstacle in a part of a
given space region makes one consider the region as entirely filled
and what is estimated is the probability that this fact is true. The
hypothesis of independence between the various cells is based
on the probability distribution of the map given by the product
of the probabilities of all its cells (Thrun et al., 2005). In general,
the problem of estimating an occupancy map is divided into smaller sub-problems, such as the estimation related to the state of
individual cells (Thrun et al., 2005). In this work, we will adopt
the convention used in Thrun et al. (2005), wherein the algorithm
that allows to perform cells mapping in this kinds of representation can be found.
3.3.3. Nearest neighbor search
A fundamental part of several of the steps of this work consists
of searching for the nearest neighbors to a given point in a set of
points. IN order to give a formal definition of the problem, let
P ¼ fp1 ; p2 ; ; pN g a set of points in R2 , and a point q. The problem
of searching for the nearest neighbor of point q in P is to find point
p 2 P, such that p is the closest to q. The simplest way to accomplish such a task would be to perform an exhaustive search computing the distance between any point of P to q and choosing the
one that occasion the smallest distance. However, this approach
is the least efficient and it is evident when the cardinality of P is
large. In the proposed SLAM system, we are led to perform multiple
searches for nearest neighbors, each with neighbors of entire point
sets in a counterpart set, we proposed to store P in a structure of
data that facilitates a fast search. One of the possible data structures we use in the proposed method is the KD-tree, which
improves the search process (Friedman et al., 1977). Pose bookkeeping in a KD-tree requires the pose set to be divided. One of
the most common forms of division makes use of the median of
the coordinates of the poses to be stored. The usage of KD-trees
reduces significantly the time required to find a closer set of neighbors (BBesl and McKayesl and McKay, 1992).
One of the most traditional methods for approach the aforementioned task is called Iterative Closest Point (ICP) (BBesl and
McKayesl and McKay, 1992). The main advantages of ICP-based
methods are their simplicity of implementation and agility when
implemented using KD-Trees (Friedman et al., 1977; BBentley
and Friedmanentley and Friedman, 1979; Bentley, 1980; Zhang,
1994). However, ICP assumes that the forms whose correspondence is sought can be completely and perfectly superimposed.
Of course, it is more common for the samples under analysis to
have only partial overlap, such as when they are obtained with different sampling rates or from different points of view. It is interesting to note, however, that in order to improve the robustness of the
technique, in the proposed implementation, several changes have
been proposed which range from suggestions of point association
heuristics to the implementation of hybrid methods that employ
global optimization techniques to provide ICP with best initial estimate. The algorithm for ICP classical method can be found in BBesl
and McKayesl and McKay (1992). In general terms, the underlying
operation can be described as an iterated application of the following steps:
1. Matching: Associate to each pose in the source dataset its nearest neighbor in the destination set.
7
2. Minimization: Minimize the error measure according to the
cost function used.
3. Transformation: Transform the poses in the source dataset
using the result of the minimization process.
4. Swarm intelligence optimization
Swarm intelligence is a field within artificial intelligence that
draws inspiration from collective behavior. The most traditional
optimization meta-heuristics that belong to this category are based
on the interactions of creatures, such as ants, bees, worms, birds,
fishes, among others. From the coordination, or apparent coordination, of the individual behavior of entities, which is of little sophistication in their modus operandi, a complex pattern usually
emerges. The search process, employed by all swarm intelligence
based techniques consists of an interaction and a dichotomy
between two, well known, processes: exploration and exploitation
(Kennedy, 2006). Exploration consists of the discovery of new
potential solutions within the search space, and typically leads to
a growth of the known part of the search space. Exploitation is
the process of intensifying the search within the neighborhood of
known solutions, thus making use of the information gathered by
the swarm up to the present moment to try and improve the quality of the known solutions.
In this work we use three swarm intelligence techniques: Particle Swarm Optimization (PSO), which we describe in Section 4.1;
Artificial Bee Colony Optimization (ABC), which is described in
4.2; Firefly behavior-based Optimization Algorithm (FA), which is
described in Section 4.3.
4.1. Particle swarm based optimization
The PSO is an algorithm that keeps a set of entities known as
particles, each particle position represents a codified solution to
the underlying optimization problem. The key idea of all variants
of this algorithm is that the interplay between each particle individual experience along the search process and the collective experience of the swarm is what guides the search through the solution
space. The search procedure occurs in an iterative fashion and each
particle, along its trajectory through the search space, keeps in its
memory the best solution it has thus far attained. The interaction
between each individual particle, and the swarm to which it
belongs, is controlled by the modulation of the velocity of the
members of the swarm. Each particle velocity is defined as the
composition of three distinct components: social, cognitive and
inertial. The social component is responsible for guiding each particle in a direction that approaches the best solution found by the
swarm, or by a subset of the swarm depending on the topology
that is employed. The cognitive component purpose is to guide
every particle towards the best solution they have found themselves. Finally, the inertial component has a role that, much like
the corresponding force in Newtonian mechanics, opposes any
change in the tendency of motion. As such, the inertia effectively
limits the particles ability to change its path through the search
space in a drastic manner.
Let xi ðtÞ be the position of the particle i at the instant t; v i ðtÞ the
component of the velocity of the particle i at instant t; w the coefficient of inertia, c1 the cognitive coefficient, c2 the social coefficient, n the swarm size and f the objective function. The basic
algorithm for the PSO with global best communication strategy
can then be stated in Algorithm 1 (KKennedy and
Eberhartennedy and Eberhart, 1995; Kennedy, 2006).
8
N. Nedjah et al. / Expert Systems with Applications 159 (2020) 113547
Algorithm 1 PSO
Require f , n, w, c1 and c2
1: Initialize the swarm
2: repeat
3: for i :¼ 1; 2; . . . ; n do
4:
if f ðparticlei Þ < f ðpbesti Þ then
5:
pbesti :¼ particlei
6:
end if
7:
if f ðpbest i Þ < f ðgbestÞ then
8:
gbest :¼ pbest i
9:
end if
10:
end for
11:
for i :¼ 1; 2; . . . ; 3 do
12:
Update velocity according to Eq. 7.
13:
Update position according to Eq. 8
14: end for
15: until Stopping criteria are met
16: return gbest
The operation of Algorithm 1 can be briefly explained as the
succession of the following steps: starting and iterating a swarm
moving process in the search of good quality solutions. Swarm
initialization consists of random generation of particles (coded
solutions) that are spread throughout the search space. As long
as no stopping conditions have been met, the swarm movement
process is repeated. The swarm movement process depends on
the quality of the swarm members and the past experiences of
each particle. In each iteration, the quality of the solution, as
coded by each particle, is evaluates. Whenever this is the best
solution found by the particle, its position is used to update pbest.
Each time the particle quality is evaluated, the particle’s pbest is
compared to the best position found so far by swarm as a whole.
Whenever the quality of the particle’s pbest is found to be better
than of the global best, gbest is then updated using pbest. During
the process, each particle has its velocity and position updated. At
the end of the optimization process gbest is returned as the solution to the problem. In Algorithm 1, the particle velocity is computed following Eq. 7:
v i ðt þ 1Þ ¼ w v i ðtÞ þ c1 r1 ðpbesti ðtÞ xi ðtÞÞ þ c2 r2
ðgbestðtÞ xi ðtÞÞ;
ð7Þ
wherein pbesti ðtÞ represents the best position in the history of the
particle until instant t; gbestðtÞ is the best global to moment t and
r 1 and r 2 are pseudo-random numbers sampled using a uniform distribution in the range ½0; 1. Regarding random numbers r 1 and r 2 ,
we further state that for each dimension of the problem the update
step is to generate one of these numbers independently. The position update of each particle is performed according to Eq. 8:
xi ðt þ 1Þ ¼ xi ðtÞ þ v i ðt þ 1Þ:
ð8Þ
In Algorithm 1, we did not specify which stopping conditions to
satisfy, although this is fundamental for any approximate optimization technique. Defining an appropriate stopping condition
can be done in several ways, the most common of which are: specifying a maximum number of iterations, setting a tolerated error
for the objective function value regarding to the found best solution, setting a value change threshold of the objective function
and the definition of a variation threshold of the objective function
argument. Cases where thresholds of variation are specified are
actually measures of stagnation, and these thresholds are typically
associated with a maximum number of iterations so that the limited amounts they violate before the whole optimization process
is completed.
With these definitions established, we can then try to quantify
the importance of each of the parameters mentioned for the convergence and efficiency of the process. Several studies have been
performed on parameter selection for an efficient optimization
process using PSO. The parameters denoted c1 and c2 are also
known as acceleration coefficients. As this name implies, low values of c1 and c2 lead to smoother trajectories, and allow particles
to explore the search space longer, temporarily moving away from
good regions before being attracted to a balance condition. Conversely, high values of these two parameters can lead to steeper
trajectories and cause particles to converge faster for good solutions, but they can also bounce them beyond good regions of the
search space.
In Engelbrecht (2006), one can find reference to simplified
canonical PSO models, wherein certain components of the velocity
formula are suppressed. Indeed, the suppression of certain components that influence the particle trajectory in the search space
allows a better notion of their contribution to the quality of the
results obtained. To these simplified models the author of
Engelbrecht (2006) names Cognition-Only Model, Social-Only Model
and Selfless Model (altruistic model). These simplified models are
instructive in that they unequivocally illustrate to what extent
the alteration of the three parameters mentioned above, namely:
the coefficient of inertia, the cognitive coefficient and the social
coefficient affects swarm behavior and overall algorithm performance. According to KKennedy and Eberhartennedy and Eberhart
(1995), the purely cognitive model is more susceptible to failure
than the complete model. Also according to KKennedy and
Eberhartennedy and Eberhart (1995), this type of velocity model
leads to an intense search behavior close to the initially populated
regions, degenerating the PSO into a case of parallel random
search. According to Kennedy (1997), Engelbrecht (2006) the
purely cognitive model still has a slower rate of convergence, as
confirmed in CCarlisle and Dozierarlisle and Dozier (2000). The
so-called purely social model has been lauded in KKennedy and
Eberhartennedy and Eberhart (1995), CCarlisle and Dozierarlisle
and Dozier (2000), Engelbrecht (2006) for being faster and more
efficient than the full model and the purely cognitive model. This
model represents the tendency of particles to mimic their most
successful neighbors. Finally, Kennedy mentions the altruistic
model. The latter is nothing but the social model under the restriction that the best particle in a neighborhood cannot be the particle
itself. That is, when a member of a neighborhood seeks out the
most successful particle in the vicinity, it abstains of the competition. This model has been described as being faster than purely
social for some problems in Kennedy (1997), although unsatisfactory results have been reported for the optimization case in
CCarlisle and Dozierarlisle and Dozier (2000).
4.2. Artificial bee colony based optimization
The ABC algorithm was first proposed by Karaboga (2005) and
draws its inspiration from the foraging behavior of honey bees.
In the model used by the algorithm the hive is divided into three
classes of bees: worker, onlookers and scouts. The fundamental
notion of the algorithm is that each food source represents a potential solution and the amount of nectar in the food source represents
its quality, as measured by the objective function. It should be
noted that the number of employed bees is always equal to the
number of potential solutions in the swarm, denoted food sources
within the meta-heuristic. The main step of the ABC optimization
9
N. Nedjah et al. / Expert Systems with Applications 159 (2020) 113547
strategy are given in Algorithm 2. It is essential to make it clear
that, in this algorithm, each food source represents a possible solution to the optimization problem and the nectar quantity represents the quality of the solution, which is computed according to
the objective function.
Algorithm 2 ABC
require f , n, N and l
1: Send scout bees to initial food sources, as chosen using Eq.
9.
2: repeat
3: Send worker bees to food sources to assess nectar
quantity.
4: Select neighboring sources for each worker bee using Eq.
9.
5: Compute preference probability of food source by
onlooker bees using Eq. 10.
6: Send onlooker bees to food sources to assess nectar
quantity.
7: Stop exploring food sources when these have been
completely exploited.
8: Move randomly scouts bees using Eq. 9.
9: Update s the best food source as discovered so far.
10: until Stopping criteria are met
11: return s
Every cycle of the ABC search procedure consists of three steps:
dispatching the employed bees to the food sources and the subsequent evaluation of the nectar collected by each of them, the selection of new promising food sources based on the information that
was shared by the returning employed bees and the selection, by
the onlookers, of promising food sources to where they are sent.
The food source selection process, by the bees, is controlled by a
parameter denominated limit. This limit is a predefined number
of attempts to improve the quality of a food source, after which
it is abandoned and the employed bee that was in charge of the
aforementioned food source becomes a scout bee. These details
show that the exploration stage of the search is carried out by
the scouts while the exploitation stage is performed by the
employed bees and the onlookers. The ABC algorithm uses as its
control parameters, such as the swarm size, the limit of attempts
to improve a food source, the number of onlookers and the number
of scouts. Usually, the employed bees compose half of the swarm.
In Algorithm 2, the initialization of food sources can be done as
defined by Eq. 9:
xij ¼ lj þ r1 ðuj lj Þ;
ð9Þ
wherein xij is the coordinate j of food source i; r 1 is a uniformly distributed pseudo-random number within the range ½0; 1; lj ¼ l is the
lower bound along the j direction of the subset of the objective
function regarding the considered space search and uj is the upper
bound of the aforementioned set.
Upon their return to the hive, the bees share their discovery
about the food sources they explored with the other bees in the
hive dance region. After the sharing of information regarding the
quality of the discovered food sources is completed, worker bees
return to the food sources they visited in the previous iteration,
given the fact that such locality is recorded in the worker bee’s
memory. Then the bee choses a new food source in the vicinity
of the previous one. Subsequently, each onlooker bee chooses a
food source based on the worker bee dance.
Importantly, the probability of selecting a food source increases
with the increase in the amount of nectar detected over there.
Indeed, the probability of selecting a particular source pi is set
according to Eq. 10:
f ðxi Þ
pi ¼ PN
;
k¼1 f ðxk Þ
ð10Þ
wherein f ðxi Þ denotes the value of the objective function of food
source i and N represents the number of food sources.
It is important to note that each worker or onlooker bee alters
the food source in its memory with a given probability. This modification is kept as long as the new food source is more fruitful than
the original. The selection algorithm can therefore be characterized
as greedy. The change of the food source is determined as in Eq. 11:
v ij ¼ xij þ rij ðxij xkj Þ;
ð11Þ
wherein r ij is a uniformly distributed pseudo-random number such
that rij 2 ½1; 1 R; k 2 f1; ; Ng N; i 2 f1; ; Ng N; j 2 f1; ; Ng N; xij
is coordinate j of food source i, and xkj represents coordinate j of
food source that is randomly selected.
4.3. Firefly based optimization
The FA is swarm intelligence method (Fister, Yang, and
Brest, 2013). In essence it is a bio-inspired stochastic metaheuristic. The inspiration of FA comes from the bioluminescence of fireflies. In fact, it is precisely the periodic
flickering of lights, a striking feature of these insects, which,
together with the physical characteristic of the decay of light
intensity, was used to elaborate this optimization strategy. In
Fister et al. (2013), it is shown that the decay of the luminous
intensity I follows a relation of the type I / r12 , where r indicates the distance to the source of luminescence. The communication between social insects has been successfully used
several times as an inspiration for the development of optimization techniques, such as in the cases of ants and bees
(Dorigo, Caro, and Gambardella, 1999; Karaboga, 2005). It is
important to emphasize that in swarm algorithms, the
decision-making processes of each entity typically occur in a
decentralized way, and for this reason it is essential that there
is communication between the members of the swarm in order
to have a cohesive behavior of the whole.
The parameters of the FA are b0 ; c and n. The coefficient b0 is the
attractiveness of a firefly when the distance to it is zero. The constant c determines the decay rate of the attractiveness of a firefly
in terms of the distance to it. In the Algorithm 3, each firefly was
denoted by s and the objective function by f.
Algorithm 3 FA
Require b0 ,c,n
1: Initialize the swarm
2: repeat
3: aðtÞ ¼ newAlfaðÞ
4: for i :¼ 1; 2; . . . ; n do
5:
Compute f ðsi Þ
6: end for
7: Sort the fireflies s of the swarm according to their quality
f ðsÞ.
8: Assign the fittest firefly to s .
9: Move the fireflies according to Eq. 12.
10: Until Stopping criteria are met
11: return s
10
N. Nedjah et al. / Expert Systems with Applications 159 (2020) 113547
The essence of FA operation is the combination of random
motion with the attraction of each firefly by the other brighter
members of the swarm. In fact, firefly sj is attracted to the firefly
si and its motion is defined by Eq. 12:
stþ1
¼ sti þ b0 expðc r2ij Þ ðsj si Þ þ a i ;
i
ð12Þ
wherein c represents the light absorption factor, that is how light
diffuses through the medium in which it propagates between one
firefly and another. The parameter a 2 ½0; 1 is responsible for the
random term a i of Eq. 12, and is sampled from a uniform distribution in that interval. The value i is sampled from a Gaussian
distribution. The value of r ij is the distance between the fireflies
si and sj , as defined by the two-dimensional Euclidean metric of
Eq. 13:
rffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi
X2
r ij ¼ ksj si k ¼
ðs sik Þ2 :
k¼1 jk
ð13Þ
Algorithm 4 Proposed SLAM Solution
1: G :¼ £; lv :¼ £
2: lk :¼ 0; rk :¼ 0 ;k :¼ 0
3: DP :¼ ½ 0 0 0 T
4: lv ð0Þ :¼ Acquire_ScanðÞ
5: t :¼ 1
6: repeat
7: lv ðtÞ :¼ Acquire_ScanðÞ
8: DP :¼ Align_Scansðlv ðtÞ; lv ðt 1ÞÞ
9: G :¼ Update_GraphðG; DP; t 1; tÞ
10: G :¼ Seek_Loop_ClosureðG; lv Þ
11: G :¼ Optimize_GraphðGÞ
12: t :¼ t þ 1
13: until Halt command is received.
14: M :¼ Generate_Global_Map ðG; lv ÞÞ
15; return M
It is important to note that in Algorithm 3, index k in the sum of
the Eq. 13 always varies in the range of 1 to the maximum number of dimensions of the problem. Function newAlphaðÞ is used to
allow an adaptive variation of a, in order to reduce the degree of
randomness of the movement of the fireflies as the iterations
pass.
In FA, the swarm of fireflies is initialized randomly, and the
search process is guided by the luminous intensity of the swarm’s
fireflies, which is proportional to the quality of the solution as perceived by their peers. This quality, as seen by the other fireflies, it is
called attractiveness (Fister et al., 2013). An interesting phenomenon, regarding the comparison between FA and other optimization algorithms, is that FA presents two remarkable
asymptotic behaviors, according to the selected value of parameter
c. The first one is c ! 0, since the attractiveness becomes constant
and equal to b0 throughout the search space. So, the algorithm
behaves as a particular case of PSO. The second notable asymptotic
behavior occurs when we have c ! 1, which causes the FA to
behave as a parallel case of Simulated Annealing (Fister et al.,
2013). As a conclusion, any AF based process has an intermediate
behavior between these two techniques. In the FA described in
Gao, Luo, Teng, He, and Jiang (2013), several stopping criteria are
mentioned, such as the quality of the worst solution in the swarm,
the quality of the best solution in the swarm and the number of
evaluations of the fitness function. Regarding the first and second
conditions, it noteworthy to point out that when a threshold such
as this is imposed, it is expected that there will be convergence of
the algorithm with a smaller number of iterations, and consequently with a smaller number of evaluations of the function goal.
However, this convergence comes coupled with a loss of precision,
i.e. there may be premature convergence in the presence of a local
maximum.
The proposed solution has an incremental character, therefore
in each iteration of Algorithm 4 a single scan is processed. Every
iteration begins by the acquisition of a scan by means of an invocation of the method called Acquire_ Scan. The scan thus obtained
is then stored in a list called lv that contains all the scans heretofore collected in chronological order. Then, the scan acquired at
the present instant lv ðtÞ is matched against the scan obtained in
the instant that immediately preceded it lv ðt 1Þ, hence an estimate of the motion that took place in between both measurements
is attained in the form of a relative pose DP (sequential pose difference). After that, the pose graph G is updated by the inclusion of a
new node, which is connected to the last created node by an edge
that encodes the sequential pose difference DP. Immediately following this update, the resulting graph is optimized. Once the optimization of G is concluded, the method Seek_Loop_Closure is called,
receiving as arguments G and lv , whereby new edges are added if
any pair of nodes is determined to have, in its corresponding scan,
represented the same region of the space being explored. Afterwards, the yielded graph pose G is once more optimized thus ending the processing of scan lv ðtÞ, and concluding the current
iteration. This sequence of steps is repeated until a Halt order is
issued by an external agent, in which case the global map M is generated from G and lv ðtÞ by a call to Generate_Global_Map and the
whole procedure is finished.
The first iteration constitutes a special case since no previous
scan exists upon which a reference may be based. Therefore, it is
pre-determined that the reference frame of the first pose, and its
corresponding scan, will be used as a basis for all succeeding measurements and poses. In fact, the first scan acquisition induces the
system to introduce the first node in the graph.
5. Proposed solution
5.1. The scan matcher
The proposed solution to the SLAM problem consists of four
parts: (i) a scan matcher; (ii) a pose-graph updating system; (iii)
a pose-graph optimization system; and (iv) a loop closure detection system. Algorithm 4 shows the macro-architecture of the proposed method, wherein, the scan matcher is implemented by
function Align_Scans; the pose-graph updating system by function
Update_Graph; the pose-graph optimization system by function
Optimize_Graph; and the loop closure detection system by function
Seek_Loop_Closure.
The scan matcher role, in our system, is to determine the relationship that exists between pairs of LiDAR scans. While odometric
relationships could be derived for the case of subsequent scans, no
reliable correspondence could be established between nonsequential pairs of scans without a proper scan-matching procedure. Therefore, the impact of such a fundamental part of our SLAM
solution cannot be understated, and its accuracy and overall performance is bound to deeply affect the overall system’s
performance.
11
N. Nedjah et al. / Expert Systems with Applications 159 (2020) 113547
The scan matcher, whose main steps are given in Algorithm 5, is
designed to operate on two different, and complementary, modes.
The first mode of operation is a scan-to-scan alignment based on a
point-to-point distance metric. Its operation is dependent on the
minimization of the used distance metric, and it operates on a pair
of scans by first determining a set of correspondences between
them, based on a nearest neighbor criteria. The second mode is a
scan-to-map alignment and consists of minimizing the distance
between the pairs of corresponding points. The result of the aforementioned procedure is a transformation that leads to a maximal
alignment between the input scans. So, the scan matcher works
in two modes in an attempt to reduce uncertainties of scan matching. Hence, when the scan-to-scan mode fails to achieve an acceptable alignment (i.e. alignment error is above a threshold), the scanto-map is executed, attempting to fine tune the relative position
found by the scan-to-scan alignment in order to hopefully reduce
the alignment error.
Algorithm 5 Align_Scansðv ; v r Þ
Require t, lk , rk ; 1: lc :¼ Seek_Correspondencesðv ; v r Þ
2: ½ DP k T :¼ Minimize Eq. 14
2: if k > then
4: m :¼ Renderize_Local_Map ðv r Þ
½ DP k T :¼ Minimize Eq. 16 with arguments ðv ; mÞ
k :¼ Calculate the new k using Eq. 14 with DP as its
argument.
7: end if
8: lk :¼ 1t k þ ðt 1Þlk
h
i1
r2k þ ðk lk Þ2 2
9: rk :¼ t1
t
5:
6:
10: :¼ lk þ rk
11: return DP
The scan alignment of Algorithm 5 receives two scans: the scan
under investigation v and the reference scan v r . Its operation
begins by seeking correspondences between the scans v and v r ,
which procedure is denoted Seek_Correspondences. It returns a
bijection between v and v r that associates to each point of v the
closest point in v r , following thus a nearest neighbor association
criterion. Given the list of correspondences between the points of
each scan, the scan alignment task is, thus, reduced to the determination of a minimum point via function f defined in Eq. 14, called
the objective function of the minimization matching process that
ensures the scan alignment during the point-to-point mode. The
optimization is performed by the application of the selected swarm
intelligence technique, which were previously defined: ABC, FA
and PSO. This process returns two results: a relative pose, called
DP, and the value of the aforementioned objective function,
denoted k. If k is below the error threshold , the alignment is
deemed to be satisfactory and the relative pose DP is accepted.
Otherwise, and in order to improve the so-far achieved scan alignment, we attempt to reduce the scan-matching error by doing a
local fine search around the found relative position. This will be
further explained in the sequel. In this purpose, the reference scan
v r is rendered as a local occupancy grid map m and a scan alignment is attempted in the scan-to-map mode of operation. This
alignment is also performed via a minimization process using the
selected swarm-based optimization meta-heuristic. Nevertheless,
in this case, the objective function is different from the one used
during the scan-to-scan alignment. So, during this procedure, the
objective function employed takes the form in function g, defined
by Eq. 16. The result of the optimization procedure is, as in the first
case, a pair containing DP and k. It is noteworthy to point out, however, that the value of k thus obtained is not comparable with the
previous given that it is computed using a different objective function. Hence, a new value of k is computed using Eq. 14. This new
value is used to update the dynamic error threshold .
The rejection threshold determines whether the point-topoint alignment produced a pose that is an outlier. In this positive
case, the scan-to-map mode would be employed to correct the
pose estimation, which is especially recommended in the case of
loop closures. There are several strategies for rejection threshold
determination (Pomerleau, Colas, Ferland, and Michaud, 2009).
These were first used to determine outliers when using ICP and
its variants (Druon, Aldon, and Crosnier, 2006). Among these
strategies, one can find:
Fix strategy, wherein the threshold is a fixed pre-defined value;
Zhang’s strategy, wherein the threshold is based on statistical
moments of the data distribution. It is set to either l þ 3r when
the pairing is assumed to be quite good, l þ 2r when the pairing is assumed to be still good, l þ r when the pairing is
assumed to be not so bad or the median of the distribution
when the pairing is assumed to really bad, depending on a
user-defined parameter (Zhang, 1994);
Mean strategy, wherein the threshold is set l þ r (Druon et al.,
2006).
We use the latter rejection strategy i.e. the mean strategy to
decide whether to apply the scan-to-map to correct the pose
obtained by the point-to-point based alignment.
So, threshold is defined as the sum of the mean and the standard deviation of variable k obtained along the current SLAM session. At the end of each, and every, call to Algorithm 5 there is
an update to lk and rk , followed by an update to , so that its value
is consistent with the new running mean and standard deviation of
the objective function values. Note that values of lk and rk for the
current scan alignment are computed incrementally based on their
values in the previous scan, taking into account the total number of
scans done so far, denoted by t, which is initialized as 1 and incremented at the end of each at iteration of the SLAM process, as
shown in line 5 and line 12 of Algorithm 4, respectively. Evidently,
and regardless of what mode of operation is responsible for its
computation, Algorithm 5 DP is always returned.
The objective function used for the scan-to-scan alignment,
which evaluates the quadratic distance between the points of scan
v and those of scan v r in the coordinate system centered in pose
DP, is specified by Eq. 14:
f ðDPÞ ¼ log 1 þ
XN
i¼1
DP vx
vy
i
i
v rx
v ry
2!
i
;
ð14Þ
i
where DP is a relative pose and is the operation of composition
between a pose and a point, defined in Eq. 1. This composition
allows us to express the points in v in the same coordinate system
as the points in v r centered at new pose DP. Note that the log function is computed in order to limit the wide variation of the used
comparison function f. So, we impose the logð1 þ xÞ re-scale transformation (Baeck, Fogel, and Michalewicz, 2000). Constant 1 is
added to avoid the non-definition of the logarithm function on 0.
The scan-to-map alignment allows us to correct the robot pose
estimation exploiting three facts: (i) all beams of a scan must end
at obstacles; (ii) the robot estimated position must be empty in the
current occupancy map; and (iii) robot must be able to reach that
position in the map and continue its mapping from thereon. The
objective function for this second mode of scan-to-map matching
is inspired by these two facts. The minimization of the proposed
objective function attempts to find the best matching that mini-
12
N. Nedjah et al. / Expert Systems with Applications 159 (2020) 113547
mizes any breach of these two facts. Moreover, the scan-to-map
alignment is performed in the manner previously described via a
minimization process using the swarm-based optimization metaheuristic. In this case, the objective function takes on the form
described in Eq. 15 (Oliveira, Nedjah, and Mourelle, 2017). Namely,
let v xi and v yi be the Cartesian coordinates of the obstacle detected
by the i-th beam of scan v and let DP be a relative pose. The result
of the composition operation between the point v xi ; v yi and the
relative pose DP, in the coordinates of the local occupancy grid
map, is denoted ðni ; fi Þ and defined by Eq. 15:
ni
fi
¼ DP vx
vy
i
:
ð15Þ
i
The process of scan-to-map alignment is then reduced to the
minimization of the objective function g defined in Eq. 16:
gðni ; fi Þ ¼
n
X
255 mðni ; fi Þ
255
i¼1
þ
.1 X
.1
X
mðxr þ k; yr þ ‘Þ
;
255
ð. þ 2Þ k¼1. ‘¼1.
1
ð16Þ
2
wherein n is the number of beams within the current scan, xr and yr
represent the position of the robot in the local map. Variable m represents the local occupancy grid map as a matrix of unsigned eight
bit integer numbers that codify the occupancy probabilities from 0
to 100% by mapping them linearly to the range from 255 to 0,
respectively. So, it is established that value 0 indicates an entry in
the map of 100% probability of being occupied while value 255
denotes an entry of 100% probability of being empty. Variable .
is defined in terms of the robot’s diameter d and the map cell size
c as in Eq. 17:
.¼
8
>
< dd=ce þ 1
if dd=ceiseven
>
:
otherwise:
ð17Þ
dd=ce
It is noteworthy to emphasize that the first summation term in
Eq. 16 is with respect to all n beams within the scan one wishes to
localize and thus evaluates the veracity of the first fact (i.e. scan
beams must end at obstacles), and the second summation term is
employed to penalize the presence of occupied cells in the
ð. þ 2Þ2 adjacent cells of the robot location, taking into account
the robot’s size and map cell size, and thus verifying the veracity
of the second fact (i.e. robot estimate position must be empty in
the map), augmented by the extra cells around the robot location,
thus guaranteeing the veracity of the third fact (i.e. robot must be
able to reach the position and continue its trajectory from it). By
aiming at an empty position yet with neighboring position also
empty, the robot can be away from walls and corners and any
other obstacles in a such a way that it can map the wall and other
obstacles and still maneuver to continue its journey, mapping the
so-far unknown environment. For instance, with a usual cell size of
0.05 m and a robot with a diameter of 0.15 m, Eq. 16 would penalize the 25 (i.e. 9 cells where the robot is located and 16 cells around
that location) adjacent cells, around the robot estimated position
(cell mðxr ; yr Þ) in the map.
In both modes of operation three halting conditions have been
imposed upon the optimization procedure: a limit of 1000 maximum iterations performed by each technique, a stagnation limit
of 50 iterations, wherein the difference between the best and the
worst solution within the swarm does not vary by more than
106 , and a tolerance threshold, whereby a solution whose corresponding objective function value is inferior to 10
accepted.
6
is immediately
5.2. Pose graph manipulation
Pose graphs are the backbone of our solution by providing us
with a means of attaining, and maintaining, global consistency
within the maps generated and the trajectory estimated therein.
We have adopted the methods described in detail within
(Grisetti et al., 2010) to perform the optimization of the pose
graphs generated by our proposed solution.
5.2.1. Graph update
The steps required to update the pose graph order to incorporate the effects of a restriction in a given edge labeled by the
obtained pose DP are specified in Algorithm 6. So, besides inserting
an edge between nodes i and j, procedure Insert_Edge_RestrictionðG; i; j; DPÞ also computes the restriction heij ; Xij i associated
with this edge. Every time a relative pose DP is determined by
the scan matching, it is passed to this procedure, which evaluates
whether the new pose represents a translation of more than
0:5 m or a rotation of more than 0:5 rad. In the positive case, a
new node, called j, is inserted into the pose graph. In effect, if node
j is already in the graph i.e. in case of some loop closure, a new edge
is inserted in between i and j, otherwise a new node j is inserted
and it is linked to node i by an edge corresponding to the restriction implied by DP. It should be noted that the existence of node
i in the graph set of nodes GN is always true. Recall that pose DP
is determined by the scan matcher with an associated standard
deviation of rlambda for the objective function value k, defined in
Eq. 14.
Algorithm 6 Update_Graph(G, DP, i, j)
Require X
qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi
1: if
DP 2x þ DP2y > 0:5morjDPh j > 0:5rad then
2: if j R GN then
3:
G :¼ Insert_NodeðG, jÞ
4: end if
5: G :¼ Insert_Edge_RestrictionðG, i, j, DPÞ
6: end if
7: Return G
Let eij and Xij be the error and the information matrix associated
with an newly inserted edge between node i in reference to pose pi ,
and node j in reference to pose pj , given the relative pose DP
obtained by the scan matching of Algorithm 5. Error eij is defined
as in Eq. 18 (Carlone, Aragues, Castellanos, and Bona, 2011;
Grisetti et al., 2010):
"
eij ¼
RTDP RTi ðt j t i Þ tDP
hj hi hDP
#
;
ð18Þ
wherein transformation is defined as in Eq. 2, and Ri is the rotation matrix that has as orientation angle that of pose pi and RDP is a
rotation matrix whose angle coincides with that of a rotation that
leads to the maximum overlap of observation made from pose pi
to that made from pose pj . This is simply orientation hDP of pose
DP. Matrix Ri is defined in Eq. 19:
2
6
Ri ¼ 4
cosðhi Þ
sinðhi Þ
sinðhi Þ
cosðhi Þ
3
2
7
5;
6
RD P ¼ 4
cosðhDP Þ
sinðhDP Þ
sinðhDP Þ
cosðhDP Þ
3
7
5:
ð19Þ
T
T
Vectors t i and t j are defined as v xi v yi
and v xj v yj
respectively, while vector tDP is the translational component whose value
N. Nedjah et al. / Expert Systems with Applications 159 (2020) 113547
allows the maximum overlap of the observation made from poses pi
to that made from pose pj . This is simply the translational component of pose DP defined as ½ v DP v DP .
The information matrix Xij regarding the inserted edge between
nodes i and j, given the resulting pose DP yielded by the scan
matching step with standard deviation rk is defined as in Eq. 20:
13
ðH Dx ¼ bÞ we implemented the QR factorization, using the
library available of Guennebaud et al. (2010).
T
Xij ¼
1
r2k
I;
ð20Þ
wherein I is the identity matrix.
5.2.2. Pose graph optimization
As previously stated, the method employed to optimize pose
graphs within the present work uses the exact same algorithm
for this purpose described in the graph-based SLAM Tutorial
(Grisetti et al., 2010). This algorithm allows to optimize the information matrix and the information vector for all cases. However, it
must not be ignored that while in function Optimize GraphðGÞ the
only input argument is the pose graph itself, it should be understood that from such an argument that the list of poses (nodes)
GN ¼ x¼
x1:T and the list of constraints (edges) N C can be extracted.
Moreover, the graph optimization function seeks the most likely
configuration of nodes by means of a Gauss–Newton type of optimization procedure. Furthermore, it should be self-evident that
this function alters the considered pose graph by updating the
poses to the ones deemed more likely to be the case by this
procedure.
To give a brief idea on the graph optimization process, let eij be
the value of the difference between the expected observation zij
and the actual observation ^zij . This process is termed as optimization because it minimizes the odd-logarithmic probability of a
given node configuration, i.e. given a node configuration and a
set of restrictions between them, and assuming that the node position is approximately normally distributed, there is a probability
density function of the form g expð0:5 eTij Xij eij Þ for each pair
of nodes corresponding to poses i and j between which there is a
measurement zij , wherein g is a normalization constant and Xij is
the information matrix of a given virtual measurement between
the nodes i and j. From this fact, it follows that, if it is assumed that
the positions of the nodes are independently distributed, the probability of a given configuration of nodes can be expressed as a product of all the exponential terms presented. This conclusion
follows immediately from the premise of the independence of variables and the law of total probability (KKokoska and
Zwillingerokoska and Zwillinger, 1999). The problem of maximizing the probability of a configuration is formulated in a computationally more efficient way, adopting the log-likelihood. Using
the odd-logarithmic probability, the task of maximizing the probability of a node configuration is then posed as a problem of minimizing a sum rather than maximizing a product. The explicit form
of the minimized nonlinear objective function to find the optimal
node configuration is defined by Eq. 21:
Fðx1:t Þ ¼
X
eTij Xij eij ;
ð21Þ
hi;ji2GC
wherein GC is the constraints set regarding all edges of G and x1:t
denotes the sequence of all poses collected from time 1 to time t.
It is noteworthy to point out that the linearization of Eq. 21 was
used in order to apply the Gauss–Newton method. The derivation
process of the linear form is quite usual and can be found at
TThrun and Montemerlohrun and Montemerlo (2006), Grisetti
et al. (2010), Kümmerle et al. (2011). In order to solve the system
5.2.3. Loop closure search and detection
In Algorithm 7, we describe the method whereby the proposed
solution deals with the problems of finding adequate extemporaneous constraints and loop closures. This particular subsystem is
periodically invoked through the entire duration of the SLAM task
in order to maintain the global consistency of the map and the trajectory under construction. In a nutshell, we dealt with the problem of seeking and detecting loop closures by means of a
successive, and methodical, elimination of potential candidates.
Algorithm 7 Seek_Loop_ClosureðG, lv Þ
T
:¼ Select_Candidates(G)
1: lc l0v
0
T
0
2: lc l00v
:¼ Eliminate_Outliers(G, lc , lv )
3: fori ¼ 1; ; N c do
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
14:
½ DP k T :¼ Align_Scans(v , v r )
Ebefore :¼ Calculate_Error(G)
0
G :¼ Insert_EdgeðG, t, lc , DPÞ
G :¼ Optimize_Graph(G)
Eafter :¼ Calculate_Error(G)
if Ebefore < Eafter then
0
G :¼ Remove_EdgeðG, t, lc , DPÞ
G :¼ Optimize_Graph(G)
end if
end for
return G
The process of selecting candidates suitable for pairing with the
current pose to form a loop closure edge are selected by searching
the graph for the nearest poses that have been traversed. This
search is conducted with the aid of an auxiliary KD-Tree
(Friedman et al., 1977; BBentley and Friedmanentley and
Friedman, 1979; Bentley, 1980), containing the translational components of the poses.
The Eliminate_Outliers procedure receives as its arguments a list
0
of the potential candidates lc , and the list lv that contains the scans
taken from each of the poses in lc . Then all the nodes whose corresponding poses lie outside of the covariance ellipse, that demarks
the threshold around the mean wherein 95% of the mass of the
probability distribution density function of current pose is concentrated, are removed from lc . Of course, each time a pose within the
candidate list is eliminated from lc the corresponding scan is also
0
removed from lv . Afterwards, each of the poses within the list is
tested by the alignment of the scan obtained from each pose to
the one corresponding to the current pose. Each alignment, produces a relative pose and an error value, as was specified in Algorithm 5. In order to validate a possible correspondence, we
assumed the criterion that any constraint that is valid must not
make the error of the graph optimization procedure grow, since
this would indicate that the node configuration that ensues is less
likely than it was before the introduction of the aforementioned
constraint. This final testing stage, therefore, proceeds by, one at
a time, introducing an edge between the current pose and each
of the remaining candidate poses. The restriction is generated
based on the relative pose returned by the scan matcher. Every
time a constraint is added, the graph is optimized and the resulting
error is compared to the one that was obtained before the introduction of the new constraint, if it is larger the constraint is
removed from the graph and the candidate is eliminated from
14
N. Nedjah et al. / Expert Systems with Applications 159 (2020) 113547
the candidate list. The search continues in such a fashion until no
more candidates remain in the list. It should be kept in mind that
by the end of the search the graph will contain all the valid loop
closures the system could detect, however many there may be,
and be optimized using those constraints.
Algorithm 8 computes the error incurred by the current configuration of nodes in the graph. It does so by sweeping all edges in
the graph and accumulating the value eij corresponding to the error
between every node pair (i; j) that is connected by an edge in the
graph. The value Xij is the block matrix Hij matrix, i.e. the block
of the information matrix encompassing nodes i and j
Algorithm 8: Calculate_ErrorðGÞ
1: E :¼ 0
2: fori ¼ 1, , t do
3: forj ¼ 1, , t do
4:
if an edge between nodes i and j exists then
5:
E :¼ E þ eTij Xij eij
6:
end if
7: end for
8: end for
9: return E
6. Performance results
The system thus far described has been implemented in C++ and
the general framework for graph optimization G2O (Kümmerle
et al., 2011). Moreover, we have also employed the linear algebra
library Eigen version 3:2:9 through the system, the nanoflann
library version 1:2:2 was used for implementing the nearest neighbor search with KD-Trees and the OpenCV library version 3:2:0 was
used to produce the image files corresponding to the occupancy
grid maps that were generated internally. All the tests that were
performed on public domain datasets used a PC with an Intel Core
i7 870 2:93 GHz processor, 8 GB RAM running Debian Linux 8:6.
It is noteworthy mentioning that the performance analysis performed in this section is twofold. first, in Section 6.4, we assess and
compare the performance of the three used meta-heuristics, then,
in Section 6.5, we compare the whereby achieved performance to
that achieved by some notable SLAM published works. The presented analysis is based on the most used error metrics for SLAM
methods, as defined in Section 6.1, a set of SLAM benchmarks, as
introduced in Section 6.2, and on a parameter settings for the three
exploited meta-heuristics, as set in Section 6.3.
6.1. Error metrics
We have performed tests to evaluate our system based on its
accuracy and the overall speed of execution. About the accuracy,
it should be noted that we have employed the method described
in Kümmerle et al. (2009). The accuracy evaluation, by this
method, is based on the application of Eqs. 22 and 23. The absolute
translational error and is defined by Eq. 22:
eT ¼
N
1X
jTðdi;j di;j Þj;
N i;j
ð22Þ
where N is the number of relations considered for the evaluation, di;j
is the difference between poses i and j; di;j is the reference relation
and the operator TðÞ allows the separation of the translational components of a relative pose. The absolute rotational error is analogously defined by Eq. 23:
eR ¼
N
1X
jRðdi;j di;j Þj;
N i;j
ð23Þ
wherein RðÞ is the operator that separates the angular component
(orientation) of a relative pose and is completely analogous to
TðÞ. Recall that the operation used to compute pose difference
is given in Eq. 2.
6.2. Benchmark datasets
To validate our SLAM technique, and enable its comparison to
other systems currently in existence we have selected eight public
domain datasets. The datasets were selected based on two criteria:
diversity and notoriety. By diversity we mean that the datasets
were selected with the intent of presenting various distinctive sets
of characteristics. By notoriety we simply bring attention to the
widespread usage of the aforementioned datasets within the SLAM
research community, as can be concluded by analyzing the sheer
number of works that employ them such as (Grisetti et al., 2007;
Kaess et al., 2008; Olson, 2008; Kümmerle et al., 2009; Grisetti
et al., 2010; Kümmerle et al., 2011; Hess, Kohler, Rapp, and
Andor, 2016).
All the datasets we have used can be seen in Fig. 1, and are
available at HHoward and Royoward and Roy (2003), Carlone
(2014), including reference measurement data that are used to
compute the reference relation. First on the top left corner is a reference map of the INTEL Research Lab (INTEL), followed to its right
by a reference map of the third floor of the ACES building in the
Texas University in Austin (ACES3), then to its right is the reference
map of the Freiburg building 79 (FR079), and followed at to the
right the reference map of the University of Washington in Seattle
(SEATTLE). Below these four maps, we have first the reference map
of a site in Orebro University in Sweden (OREBRO), followed to the
right by a reference map of the Computer Science Artificial Intelligence Laboratory at MIT (CSAIL) map, followed to the right by a reference map of the MIT Killian Court (KILLIAN). In the remainder of
the last row, we show a reference map of the Acapulco Convention
Center in Mexico (MEXICO). It is noteworthy to point out that the
maps were generated using the MatlabTM function buildMap and
are down scaled approximately as indicated in each map of shown
in Figure fig:compositeMaps.
6.3. Parameter settings
The parameters of the meta-heuristics were adjusted empirically based on 30 repeated alignment tests per scenario regarding
each of the used meta-heuristics. Each case was constructed based
on a scan of one of the data sets that we selected for random experimentation (a random number generator was used so that the
selection could take place without any human bias). We selected
10 scans and applied 3 transformation types to each scan: a pure
rotation, a pure translation and a roto-translation. From the resulting 30 scenarios, two variants were generated: one without noise
and one with zero mean Gaussian noise and a unit variance. The
tests that were carried out led to the choice of parameters specified
in Table 1. It is worth mentioning that the time taken to execute
the optimization process as well as the achieved accuracy were
taken into account.
6.4. Comparison of meta-heuristics’ performance
Table 2 shows the absolute translational and rotational errors
for each of the 8 used benchmarks of Fig. 1, wherein the lowest
error and time values are highlighted. It allows a fair comparison
of how each meta-heuristic performed with respect to its accuracy
15
N. Nedjah et al. / Expert Systems with Applications 159 (2020) 113547
Fig. 1. Reference benchmark maps: INTEL [1:4,200], ACES3 [1:4,500], FR079 [1:5,000] and SEATTLE [1:5,000]; OREBRO [1:1,700], CSAIL [1:5,000] and KILLIAN [1:5,000];
MEXICO [1:3,200].
Table 1
Configuration of the meta-heuristic’s parameters.
PSO
Parameter
Swarm (n)
Inertia (w)
Cognitive (c1 )
Social (c2 )
ABC
Value
50
0:7
1:49
1:49
Parameter
Colony (n)
Food source (N)
Threshold (l)
FA
Value
20
10
10
Parameter
Fireflies (n)
Randomization (a)
Attractiveness (b0 )
Absorption (c)
Value
20
0:75
0:2
1:0
16
N. Nedjah et al. / Expert Systems with Applications 159 (2020) 113547
Table 2
Absolute translational errors in meters and rotational errors in radian, regarding each of the 8 used benchmark together with the required execution time per scan matching in
milliseconds.
Benchmark
Metric
ABC
FA
PSO
INTEL
eT
eR
ACES3
eT
eR
0:0366 0:015
3:33 0:98
243:86 32:18
0:043 0:1
1:277 0:075
365:49 41:33
0:092 0:007
0:234 0:337
1; 479:90 90:33
0:113 0:036
0:619 0:039
849:69 30:27
0:255 0:032
2:28 1:904
56:48 12:78
0:481 0:006
0:381 0:068
270:95 14:99
0:083 0:005
0:617 0:353
100:69 6:81
0:358 0:003
0:128 0:012
63:77 8:78
0:086 0:029
2:05 0:321
185:66 32:69
0:021 0:012
0:253 0:059
112:97 17:78
0:085 0:020
0:985 1:039
1; 588:70 53:75
0:269 0:202
0:043 0:052
655:96 51:99
0:015
0:135
593:90
0:028
0:181
208:01
0:104
0:200
56:63
0:232
0:045
231:02
0:253
0:253
362:68
0:169
0:411
191:68
Time
Time
FR079
eT
eR
Time
SEATTLE
eT
eR
Time
OREBRO
eT
eR
Time
CSAIL
eT
eR
KILLIAN
eT
eR
MEXICO
eT
eR
Time
Time
Time
0:012
0:075
35:57
0:007
0:052
30:51
1:451 0:0170
1:68 0:109
8891:31 472:47
0:6136 0:07
0:759 0:0688
287:16 36:07
in each of the aforementioned eight datasets. The smallest errors
are highlighted. Indeed, it is easy to verify that ABC attained a
higher degree of accuracy than the other optimization procedures
in the estimation of the translational component of the robot’s trajectory in 4 of the 8 datasets, namely: INTEL, OREBRO, CSAIL and
KILLIAN. Still regarding the translational estimation, the FA algorithm achieved a higher accuracy than the other two techniques
in 3 out of the 8 cases: FR079, KILLIAN and MEXICO. We also
remark that the PSO algorithm only obtained a more accurate
result in the estimation of the translational components of the trajectory in the ACES3 dataset.
In the case of the estimation of the orientation of the robot
along its path, unlike what was observed in the estimation of the
translational components, the behavior of all meta-heuristics was
more balanced. The ABC algorithm only exhibited a performance
superior to the other two techniques in 2 of the 8 cases: ACES3
and OREBRO. The PSO algorithm showed a higher accuracy in 4
out of the 8 cases: INTEL, SEATTLE, CSAIL and MEXICO. In this case,
the FA algorithm exhibited a performance in between the ones
shown by the other two techniques. Moreover, the FA algorithm
has presented the smallest absolute rotational error in 2 of the 8
of the test cases: FR079 and KILLIAN.
In Fig. 2, the average processing time per scan matching, i.e. the
average duration of a cycle of toggling between the scan alignment
that sequentially estimates the displacement every time a new
scan is acquired and the whole process of updating the pose graph
to reflect the new information. It is noteworthy to point out that,
even though the choice of optimization technique definitely affects
the speed of the whole system, the environmental characteristics
seem to be one of the main factors that determine the time consumed processing each scan. On that regard, the fewer distinctive
characteristics the environment, and consequently the scans
obtained, has, the harder it would be to confidently determine correspondences between measurements acquired from different
poses. This leads to a cascade effect, given that low quality correspondences force the pose graph manipulation and optimization
subsystems to operate with bigger uncertainty bounds and, consequently, with potentially erroneous loop closure constraints. Bearing that in mind, we may remark that the FA algorithm was the
0:006
0:068
7:41
0:008
0:077
23:39
0:8019 0:1863
2:79 1:266
72:55 5:55
0:0517 0:01
0:614 0:279
79:30 4:45
0:001
0:059
72:98
0:008
0:007
31:55
1:3329 0:1842
1:451 0:557
341:61 39:09
0:689 0:159
0:917 0:0802
381:09 42:62
fastest of the three we tested, outperforming both the other techniques in 6 of the 8 cases, i.e. excluding FR079 and CSAIL. The PSO
algorithm was the second fastest, outperforming ABC in 7 out of
the 8 test scenarios, i.e. excluding KILLIAN. The ABC algorithm
was the slowest of the three meta-heuristics, in spite of having
been faster than PSO in the KILLIAN dataset.
6.5. Comparative analysis
We shall, henceforth, compare the results of our proposed solution to other existing SLAM systems. In particular, we shall apply
our system, with each of the swarm intelligence meta-heuristics,
to each of the datasets that were tested in Kümmerle et al.
(2009) using the results reported therein, thus allowing a direct
comparison of the results we obtained regarding the accuracy of
our approach. We remark that in the following results: the application scan matching (only) was denoted SM, the application of a
Rao-Blackwellized particle filter has been denoted RBPF, the application of what the authors of Kümmerle et al. (2009) called Graph
Mapping was denoted GM and the results denoted Cartographer
are but a direct application of the system proposed in Hess et al.
(2016). The scan matching employed in Kümmerle et al. (2009)
consists of an incremental and sequential alignment of scans as
described within LLu and Miliosu and Milios (1997), Censi,
Iocchi, and Grisetti (2005). The RBPF based solution used was
based on the system described in Grisetti et al. (2007), and used
50 particles. The Graph Mapping approach estimates a map by
means of a pose graph optimization procedure described in detail
in Grisetti et al. (2010). The Cartographer system is a recent
approach to SLAM and was described in some detail in Hess et al.
(2016). Cartographer is quite similar to our solution in many
aspects since both use scan matching as the core mechanism for
the estimation of constraints for the pose graph, both rely on pose
graph optimization to recover the map and trajectory estimates
and both operate online. Our implementation, however, employs
swarm intelligence based optimization to perform scan matching,
as opposed to Newton-like methods, and uses no pre-computed
grids, unlike Cartographer. It is worthy mentioning that all standard deviations have been omitted from the bar charts in Figs. 3
N. Nedjah et al. / Expert Systems with Applications 159 (2020) 113547
17
Fig. 2. Average processing time per scan matching in milliseconds in logarithmic scale for the proposed approach.
and 4 due to the magnitude difference between the error values of
some other systems and ours, which had a much smaller spread as
will be further demonstrated. Note that only 5 out of the 8 maps
are used in this comparison. This is simply due to the nonavailability of the performance metrics regarding the SEATTLE,
OREBRO and MEXICO datasets.
Amongst the meta-heuristics we have tested, ABC is the one
that obtained the lower absolute translational error in the majority
Fig. 3. Comparative chart of the absolute translational error in meters, regarding 5
of the 8 public domain benchmarking datasets utilized.
Fig. 4. Comparative chart of the absolute rotational error in degrees, regarding 5
out of the 8 public domain benchmarking datasets utilized.
of the cases. In that regard, when we compare the results achieved
by the proposed solution, using the ABC meta-heuristic, with Cartographer, for example, we shall find that in two of the 5 datasets,
for which we have the results of Cartographer, namely INTEL and
CSAIL datasets our system had, respectively, 22% and 44% lower
absolute translational errors. Furthermore, in the particular case
of the CSAIL dataset the absolute rotational error was 50:95% inferior to that of Cartographer. Nonetheless, in the ACES3 dataset, our
implementation obtained an absolute rotational error that was
242:36% superior to the one reported for Cartographer. A comparison to the RBPF based solution was even more encouraging, and
our approach has obtained results that are in the order of or superior to the particle filter approach in 60% of the cases analyzed.
Further still, it is also verifiable that in 40% of the considered cases
our implementation has shown results that were either within the
same bounds or superior to the ones reported for Graph Mapping.
In fact, in ACES3 dataset our approach managed to get a translational error that was 87:86% inferior to what is reported Graph
Mapping. As was expected, our approach was shown to be more
effective than the exclusive application of scan matching in 80%
of the scenarios contemplated within this particular benchmark.
The Firefly Algorithm did not achieve results as accurate as
those of ABC, even though it was the fastest of the 3 metaheuristics we have tested in 87:5% of the cases and consuming,
on average, only 23% of the time spent per scan by ABC. The FA
meta-heuristic has shown to be faster in the following datasets:
INTEL, FR079, SEATTLE, OREBRO, KILLIAN and MEXICO. It is noteworthy to observe that FA is the absolute winner in the case of
the MEXICO dataset as it achieved the lowest translational err,
the lowest rotational error and this in a record minimal execution
time. None of the other techniques has reached such a outstanding
performance.
The particle swarm optimization technique has shown an accuracy that was inferior to that achieved when applying ABC, but its
average processing time per scan was in between the one presented by the other two meta-heuristics. The poor accuracy shown
in 62:5% of the cases, however, leaves little room for its usage as a
compromise solution in the form of trade of speed for accuracy, at
least in the form known as global best PSO, which we employed.
Furthermore, we strongly think that the proposed SLAM system
would profit a great deal in terms of accuracy if it embeds the likelihood field (Thrun et al., 2005), which takes into account measurement uncertainty, into the proposed methodology. An example is
the likelihood analysis used by GMapping (Grisetti et al., 2007).
18
N. Nedjah et al. / Expert Systems with Applications 159 (2020) 113547
Because the performance of our proposed approach is highly
dependent on that of the scan matching, we have also complemented this analysis by comparing the accuracy obtained by some
existing different scan-matching algorithms, namely: standard ICP
algorithm (LLu and Miliosu and Milios, 1997), PLICP (Censi, 2008)
and PSM (DDiosi and Kleemaniosi and Kleeman, 2007). The dataset
used is the INTEL Lab and the MIT KILLIAN. These two benchmarks
are used due to the availability of performance measures in Li,
Zhong, Hu, and Ai (2016), which are computed as in the pointto-point metrics defined in Eq. 24:a
ex ¼
N
1X
jv x v xi j;
N i¼1 i
eh ¼
N
1X
jv h v hi j;
N i¼1 i
ey ¼
N
1X
jv y v yi j;
N i¼1 i
ð24Þ
wherein x and y are the ground truth regarding translation and h
is that regarding orientation, and N denotes the number of scan
pairs used.
Table 3 shows the translational and rotational errors achieved
by the compared scan-matching algorithms, wherein the lowest
error values are highlighted. In this experiment, we use 50 pairs
of scans, randomly selected from those available in the data of
the used benchmarks (HHoward and Royoward and Roy, 2003). It
is important to emphasize that the same scans are used for all compared matching techniques, and that the performance ICP, PLICP
and PSM are taken from Li et al. (2016). The comparison of these
performances is illustrated via the bar diagram of Fig. 5.
Based on the results of these two case studies, i.e. INTEL and
CSAIL, it is safe to conclude, the proposed approach achieves better
accuracy than those obtained by the ICP and PLICP both in terms of
translational and rotational movements. Furthermore, the swarmbased technique achieved better accuracy than PSM regarding
translational movements. For the four instances (i.e. either ex
and/or ey for both benchmarks), the proposed technique provides
alignments with lower errors than PSM. Nonetheless, the latter
provides somehow better, but similar performance regarding the
rotational component. For both benchmarks, PSM occasioned the
lowest rotational error.
It also safe to conclude that the proposed scan matching based
on ABC produces better alignments both in terms of translational
and rotational adjustments. However, it noticeable that the proposed scan-matching solutions based on FA and PSO introduce
higher errors regarding rotational movements, with PSO being
the worst case. It is noteworthy to emphasize that generally speaking, PSM and the proposed scan-matching solutions based on ABC,
FA and PSO all perform much better than ICP and PLICP both
regarding the translational and rotational errors, with PLICP
achieving better accuracy than standard ICP. However, it is worth
mentioning that without initial guess of ICP, i.e. the initial guess
was set to zero.
Finally, it is important to point out that, because the execution
times for public benchmarks were not provided for the compared
SLAM methods nor for the scan matching techniques, we are not
able to situate the proposed approach in terms of execution speed,
except regarding the ICP scan matching, as it was also implemented locally. For 100 iterations, it achieved in average a slightly
lower execution time. However, given the fact that the ABC, FA and
PSO are computationally intensive due to the repeated evaluation
of the objective functions during the scan-matching step, we suspect that our approach would require more time than PLICP and
PSM. So, this may limit the application of the proposed SLAM in
a system, wherein a real-time response is required. Nonetheless,
a parallel GPU-based implementation of the proposed approach
can certainly mitigate this limitation, and it might even outperform
existing approaches as the meta-heuristics are embarrassingly parallel workloads.
Table 3
Translational errors in meters and rotational errors in radian, regarding the scan matching step only for the INTEL and CSAIL datasets for different scan-matching algorithms.
Datasets
INTEL
Metrics
ex
ey
eh
ex
ey
eh
0.44
0.17
0.06
0.0298
0.2811
0.1239
0.36
0.22
0.22
0.0215
0.03593
0.0799
0.41
0.39
0.08
0.1211
0.1015
0.1399
0.58
0.53
0.28
0.0178
0.0751
0.1510
0.27
0.29
0.09
0.0185
0.0503
0.0293
0.25
0.26
0.13
0.1659
0.2012
0.2528
Algorithms
Proposed
ICP
PLICP
PSM
ABC
FA
PSO
CSAIL
Fig. 5. Comparative chart of the point-to-point translational errors in meters, and rotational errors in radian, regarding the INTEL and CSAIL datasets for different scanmatching algorithms.
N. Nedjah et al. / Expert Systems with Applications 159 (2020) 113547
7. Conclusions
In this paper, we investigated the development of a solution to
the simultaneous location and mapping problem using swarm
intelligence techniques, namely ABC, FA and PSO. It is noteworthy
to emphasize that swarm-intelligence-based optimization is only
applied to scan matching, but not to pose graph optimization.
The SLAM proposed approach is based on two pillars: the scan
alignment, through the exploitation of the swarm intelligence
techniques, based on the principle of maximum likelihood; and
the application of a pose graph optimization system as a way to
allow the system to maintain consistent estimates over extensive
mapping sessions. The proposed scan-matching step proceeds into
two main steps, aiming at further reducing the alignment error:
The scan-to-scan alignment and the scan-to-map alignment. Both
steps are achieved using the same swarm-based optimization
meta-heuristics.
The proposed solution has been tested on eight public domain
datasets and its performance was evaluated, to the extent possible,
using methodologies employed by other works in the field to allow
a meaningful comparison and analysis of the different results. The
experimentation we have done suggests that the ABC algorithm
may be better suited to the scan-matching problem, in the SLAM
context, as far as accuracy is concerned. On the other hand, the
FA algorithm has presented itself as a better compromise solution
compared to ABC due to its similar, but slightly inferior, precision
and higher average execution speed. Indeed, in our tests the FA
algorithm showed a performance in between that of ABC and
PSO, overcoming the accuracy of the former in a few cases and
being in most cases faster than the latter, and at least as accurate.
As an overall conclusion, we can safely establish that while ABC
offered accuracy and PSO offered speed, FA offered a trade-off
between both metrics.
Furthermore, we compared the performance of the proposed
approach to three existing SLAM implementations. So, considering
the used benchmarks and regarding the translational and rotational absolute errors, the proposed approach achieved similar performance in most cases (3 out of 5) and showed a better
performance for some of the evaluated benchmarks (1 out 5).
One of the meta-heuristics, namely ABC, obtained better translational and rotational absolute error than all compared existing
implementation. Nonetheless, it exhibited a lower performance
in one of the studied cases (1 out 5).
We have also observed that while the accuracy provided by the
proposed scan-matching approach is desirable, such characteristic
comes at a price. The trade-off between speed and accuracy in the
scan-matching procedure has to be carefully analyzed, since the
higher the error in this stage of the process, the more difficult it
becomes to find the optimal (most likely) configuration of nodes
in the pose graph. Indeed, in many cases where the time of a single
alignment for one of the three techniques was exceptionally lower
than the time required by the other two, the time consumed while
seeking loop closures grew up, as did the time required to optimize
the pose-graph. In fact, this increase is often more than both the
time taken seeking adequate loop closures and optimizing the
pose-graph altogether. This happens especially in scenarios that
involve high alignment errors, and higher uncertainty, not only
compensated the time required per alignment but actually outgrew it. Here is a classical scenario, where the ‘‘There is no such a
thing as free lunch” applies. So, it would be interesting to further
investigate the application of the ABC to improve its execution
time to take advantage of the achieved high accuracy. Also, the
same strategy could be explored with FA to improve the scanmatching translational and rotational accuracy without impacting
too much the overall execution time. Both improvements can be
19
viewed as a tuning the configuration of the parameters of the
meta-heuristics. One possible approach for the efficient parameter
setting is an adaptive configuration during the SLAM actual operation. Moreover, we intend to provide a parallel GPU-based implementation in order to reduce the execution time during scan
alignments. Another alternative for future work consists of extending the proposed solution to 3D SLAM. Such an extension would
only entail expanding the coordinate of the basic agent position.
Furthermore, we would like also to run the proposed implementation on a real robotic platform and apply it to solve a real-world
SLAM-based application.
Declaration of Competing Interest
The authors declare that they have no known competing financial interests or personal relationships that could have appeared to
influence the work reported in this paper.
Acknowledgement
First and foremost, we are eternally grateful to the reviewers of
this paper. The implementation of their recommendations
improved a great deal the contents of the paper. We grateful to
the Research Support Foundation of the State of Rio de Janeiro—
FAPERJ ( http://www.faperj.br) for funding this research. We also
would like to thank the National Council of Scientific and Technological Development in Brazil—CNPq ( http://www.cnpq.br) for the
continuous financial support.
References
Akai, N., Morales, Y., Takeuchi, E., Yoshihara, Y., & Ninomiya, Y. (2017). Robust
localization using 3D NDT scan matching with experimentally determined
uncertainty and road marker matching. In Proceedings of the IEEE intelligent
vehicles symposium (pp. 1356–1363). IEEE Press.
Baeck, T., Fogel, D., & Michalewicz, Z. (2000). Evolutionary computation 1: Basic
algorithms and operators (Chapter 23). Oxon, OX: Taylor & Francis.
Bentley, J. L. (1980). Multidimensional divide-and-conquer. Communications of the
ACM, 23(4), 214–229.
Bentley, J. L., & Friedman, J. H. (1979). Data structures for range searching. ACM
Computing Surveys, 11(4), 397–409.
Besl, P., & McKay, N. (1992). A method for registration of 3-d shapes. IEEE
Transactions on Pattern Analysis and Machine Intelligence, 14(2), 239–256.
Biber, P., & Strasser, W. (2003). The normal distributions transform: A new approach to
laser scan matching (3, pp. 2743–2748). IEEE Press.
Carlisle, A. & Dozier, G. (2000). Adapting particle swarm optimization to dynamic
environments. In International conference on artificial intelligence (Vol. 1, pp.
429–434). Las Vegas, Nevada, USA: IEEE Press..
Carlone, L. (2014). Robotics 2d-laser datasets. http://www.ipb.uni-bonn.
de/datasets/..
Carlone, L., Aragues, R., Castellanos, J., & Bona, B. (2011). A linear approximation for
graph-based simultaneous localization and mapping. In Robotics: Science and
Systems VII.
Censi, A. (2008). An ICP variant using a point-to-line metric. In 2008 IEEE
international conference on robotics and automation, number 4543181 (pp. 19–
25). Pasadena, USA: IEEE..
Censi, A., Iocchi, L. & Grisetti, G. (2005). Scan matching in the hough domain. In
Proceedings – IEEE international conference on robotics and automation (pp. 2739–
2744). Barcelona, Spain: IEEE..
Chatterjee, A., & Matsuno, F. (2010). A geese pso tuned fuzzy supervisor for EKF
based solutions of simultaneous localization and mapping (SLAM) problems in
mobile robots. Expert Systems with Applications, 37(8), 5542–5548.
Chatterjee, A., Ray, O., Chatterjee, A., & Rakshit, A. (2011). Development of a real-life
EKF based SLAM system for mobile robots employing vision sensing. Expert
Systems with Applications, 38(7), 8266–8274.
Chen, Y., & Medioni, G. (1992). Object modeling by registration of multiple range
images. Image and Vision Computing, 10, 145–155.
Cox, I. J. (1991). Blanche—an experimental in guidance and navigation of an
autonomous robot vehicle. IEEE Transactions on Robotics and Automation, 7(2),
193–204.
Diosi, A., & Kleeman, L. (2007). Fast laser scan matching using polar coordinates.
International Journal of Robotic Research, 26, 1125–1153.
Dorigo, M., Caro, G. D., & Gambardella, L. M. (1999). Ant algorithms for discrete
optimization. Artificial Life, 5, 137–172.
20
N. Nedjah et al. / Expert Systems with Applications 159 (2020) 113547
Druon, S., Aldon, M.-J., & Crosnier, A. (2006). Color constrained ICP for registration of
large unstructured 3d/color data sets. IEEE International Conference on
Information Acquisition, 62, 249–255.
Engelbrecht, A. P. (2006). Fundamentals of computational swarm intelligence
(Chapters 12 and 13). John Wiley & Sons, Chichester, West Sussex, England, 1
edition..
Fister, I., Yang, X. S., & Brest, J. (2013). A comprehensive review of firefly algorithms.
Swarm and Evolutionary Computation, 13, 34–46.
Friedman, J. H., Bentley, J. L., & Finkel, R. A. (1977). An algorithm for finding best
matches in logarithmic expected time. ACM Transactions on Mathematical
Software, 3(3), 209–226.
Gao, M.-L., Luo, D.-S., Teng, Q.-Z., He, X.-H., & Jiang, J. (2013). Object tracking using
firefly algorithm. IET Computer Vision, 7(4), 227–237.
Grisetti, G., Kummerle, R., Stachniss, C., & Burgard, W. (2010). A tutorial on graphbased SLAM. IEEE Intelligent Transportation Systems Magazine, 2(4), 31–43.
Grisetti, G., Stachniss, C., & Burgard, W. (2007). Improved techniques for grid
mapping with rao-blackwellized particle filters. IEEE Transactions on Robotics,
23, 34–46.
Guennebaud, G., Jacob, B. & et al. (2010). Eigen v3. http://eigen.tuxfamily.org..
Gutmann, S., & Schlegel, C. (1996). Amos: Comparison of scan matching approaches
for self-localizationin indoor environments. In Proceedings of the First Euromicro
Workshop on Advanced Mobile Robots (pp. 61–67). IEEE Press.
Gutmann, S., Weigel, T., & Nebel, B. (1999). Fast, accurate, and robust self-localization
in polygonal environments. Robocup workshop at IJCAI-99 (Vol. 3. IEEE Press.
Hertzberg, C., Wagner, R., Frese, U., & Schroder, L. (2013). Integrating generic sensor
fusion algorithms with sound state representations through encapsulation of
manifolds. Information Fusion, 14, 57–77.
Hess, W., Kohler, D., Rapp, H., & Andor, D. (2016). Real-time loop closure in 2d lidar
SLAM. 2016 IEEE international conference on robotics and automation (ICRA)
(pp. 1271–1278). Stockholm, Sweden: IEEE. number 7487258.
Howard, A. & Roy, N. (2003). The robotics data set repository (radish). http://
radish.sourceforge.net/..
Jeong, J., Yoon, T. S., & Park, J. B. (2018). Multimodal sensor-based semantic 3d
mapping for a large-scale environment. Expert Systems with Applications, 105,
1–10.
Jiang, X., Li, T., & Yu, Y. (2016). A novel SLAM algorithm with adaptive kalman filter.
In 2016 International conference on advanced robotics and mechatronics (ICARM)
(pp. 107–111). Macau, China: IEEE.
Kaess, M., Ranganathan, A. & Dellaert, F. (2008). iSAM: Incremental smoothing and
mapping. 24:1365–1378..
Karaboga, D. (2005). An idea based on honey bee swarm for numerical optimization.
Kayseri, Turkey: Technical report.
Kennedy, J. (1997). The particle swarm: social adaptation of knowledge. In IEEE
international conference on evolutionary computation (pp. 303–308).
Indianapolis, USA: IEEE..
Kennedy, J. (2006). Swarm intelligence. In Handbook of nature-inspired and
innovative computing (pp. 187–219). New York, USA: Springer.
Kennedy, J. & Eberhart, R. C. (1995). Particle swarm optimization. In Proceedings of
the 1995 IEEE international conference on neural networks (pp. 1942–1948).
Perth, Australia, IEEE Service Center, Piscataway, NJ: IEEE..
Kokoska, S., & Zwillinger, D. (1999). CRC standard probability and statistics tables and
formulae (Chapter 3). Boca Raton, Florida, EUA: CRC Press.
Kümmerle, R., Grisetti, G., Strasdat, H., Konolige, K., & Burgard, W. (2011). G2O: A
general framework for graph optimization. In Proceedings – IEEE international
conference on robotics and automation (pp. 3607–3613). Shanghai, China: IEEE.
Kümmerle, R., Steder, B., Dornhege, C., Ruhnke, M., Grisetti, G., Stachniss, C., &
Kleiner, A. (2009). On measuring the accuracy of SLAM algorithms. Autonomous
Robots, 27(4), 387.
Li, J., Zhong, R., Hu, Q., & Ai, M. (2016). Feature-based laser scan matching and its
application for indoor mapping. Sensors, 16(1265).
Lu, F., & Milios, E. (1997). Globally consistent range scan alignment for environment
mapping. Autonomous Robots, 4(4), 333–349.
Lu, F., & Milios, E. (1997). Robot pose estimation in unknown environments by
matching 2d range scans. Journal of Intelligent and Robotic Systems, 18(3),
249–275.
Luo, F.-Y. (2017). Accelerated ICP based on linear extrapolation.
Mingas, G., Tsardoulias, E., & Petrou, L. (2012). An FPGA implementation of the
SMG-SLAM algorithm. Microprocessor and Microsystems, 36(3), 190–204.
Montemerlo, M., Thrun, S., Koller, D. & Wegbreit, B. (2002). Fastslam: A factored
solution to the simultaneous localization and mapping problem. In Proceedings
of 8th National Conference on artificial intelligence/14th conference on innovative
applications of artificial intelligence (68(2), pp. 593–598)..
Mutz, F., Veronese, L. P., Oliveira-Santos, T., de Aguiar, E., Cheein, F. A. A., & Souza, A.
F. D. (2016). Large-scale mapping in complex field scenarios using an
autonomous car. Expert Systems with Applications, 46, 439–462.
Nieto, J., Bailey, T., & Nebot, E. (2007). Recursive scan-matching SLAM. Robotics and
Autonomous Systems, 55(1), 39–49.
Oliveira, P. J. A., Nedjah, N. & Mourelle, L. M. (2017). SLAM baseado em scanmatching com otimização por enxame de partículas. In Proceedings of the XIII
Brazilian Congress on Computational Intelligence, Curitiba, Brazil, DOI: 10.21528/
CBIC2017-96..
Olson, E. (2009). Real-time correlative scan matching. In IEEE international
conference on robotics and automation (ICRA’09) (pp. 4387–4393). IEEE Press..
Olson, E. B. (2008). Robust and efficient robotic mapping. PhD thesis, Cambridge,
MA, USA. AAI0821013..
Pomerleau, F., Colas, F., Ferland, F. & Michaud, F. (2009). Relative motion threshold
for rejection in ICP registration. In Results of the 7th international conference on
field and service robotics (Vol. 62, pp. 229–238)..
Segal, A., Haehnel, D., & Thrun, S. (2009). Generalized-ICP. Robotics: Science and
systems V (volume 5, pp. 1–8). Cambridge, Massachusetts: MIT Press.
Smith, R., Self, M., & Cheeseman, P. (1990). Estimating uncertain spatial
relationships in robotics. Autonomous Robot Vehicles, 4(April), 167–193.
Smith, R. C., & Cheeseman, P. (1986). On the representation and estimation of
spatial uncertainty. The International Journal of Robotics Research, 5(4), 56–68.
Thrun, S., Burgard, W., & Fox, D. (2005). Probabilistic robotics (Chapters 6 and 10) (1,
p. edition.). Cambridge, Mass, USA: The MIT Press.
Thrun, S., & Montemerlo, M. (2006). The GraphSLAM algorithm with applications to
large-scale mapping of urban structures. International Journal on Robotics
Research, 25(5), 403–430.
Walter, M. R., Eustice, R. M., & Leonard, J. J. (2007). Exactly sparse extended
information filters for feature-based SLAM. The International Journal of Robotics
Research, 26(4), 335–359.
Woods, F. S. (2005). An introduction to advanced methods in analytic geometry
(Chapter 14). New York, USA: Ginn and Company.
Zhang, Q.-B., Wang, P., & hai Chen, Z. (2019). An improved particle filter for mobile
robot localization based on particle swarm optimization. Expert Systems with
Applications, 135, 181–193.
Zhang, Z. (1994). Iterative point matching for registration of free-form curves.
International Journal of Computer Vision, 13(2), 119–152.