-
Roboticahttp://journals.cambridge.org/ROB
Additional services for Robotica:
Email alerts: Click hereSubscriptions: Click hereCommercial
reprints: Click hereTerms of use : Click here
Towards features updating selection based on the covariance
matrix ofthe SLAM system state
Fernando A. Auat Cheein, Fernando di Sciascio, Gustavo Scaglia
and Ricardo Carelli
Robotica / Volume 29 / Issue 02 / March 2011, pp 271 - 282DOI:
10.1017/S0263574710000111, Published online: 31 March 2010
Link to this article:
http://journals.cambridge.org/abstract_S0263574710000111
How to cite this article:Fernando A. Auat Cheein, Fernando di
Sciascio, Gustavo Scaglia and Ricardo Carelli (2011). Towards
features updatingselection based on the covariance matrix of the
SLAM system state. Robotica, 29, pp 271-282
doi:10.1017/S0263574710000111
Request Permissions : Click here
Downloaded from http://journals.cambridge.org/ROB, IP address:
200.83.129.91 on 24 Nov 2013
-
http://journals.cambridge.org Downloaded: 24 Nov 2013 IP
address: 200.83.129.91
Robotica (2011) volume 29, pp. 271282. Cambridge University
Press 2010doi:10.1017/S0263574710000111
Towards features updating selection based on the
covariancematrix of the SLAM system stateFernando A. Auat Cheein,
Fernando di Sciascio, Gustavo Scagliaand Ricardo CarelliInstituto
de Automatica, National University of San Juan, San Juan,
Argentina(Received in Final Form: March 4, 2010. First published
online: March 31, 2010)
SUMMARYThis paper addresses the problem of a features
selectioncriterion for a simultaneous localization and
mapping(SLAM) algorithm implemented on a mobile robot. ThisSLAM
algorithm is a sequential extended Kalman filter(EKF)
implementation that extracts corners and lines fromthe environment.
The selection procedure is made accordingto the convergence theorem
of the EKF-based SLAM.Thus, only those features that contribute the
most to thedecreasing of the uncertainty ellipsoid volume of the
SLAMsystem state will be chosen for the correction stage ofthe
algorithm. The proposed features selection procedurerestricts the
number of features to be updated during theSLAM process, thus
allowing real time implementationswith non-reactive mobile robot
navigation controllers. Inaddition, a Monte Carlo experiment is
carried out in orderto show the map reconstruction precision
according to theKullbackLeibler divergence curves. Consistency
analysisof the proposed SLAM algorithm and experimental resultsin
real environments are also shown in this work.
KEYWORDS: SLAM; Feature selection; Mobile robot.
1. IntroductionThis article concerns the problem of the
feature-basedsimultaneous localization and mapping (SLAM)
algorithmapplied to a mobile robot. The correction stage of the
SLAMis restricted to the features of the environment that aremost
significant for the consistency and convergence of thealgorithm.
This restriction decreases the computational costof the algorithm,
making it more suitable for non-reactivecontrol applications. It is
also presented a map divergenceanalysis when compared the obtained
SLAMs map withrespect to other maps of the same environment.
The mobile robotic field is composed by three main
areas:navigation, localization and mapping. The knowledge of
therobot about its position inside an environment and the way
thevehicle interacts with it defines the autonomy of the robot.A
mobile robot is said autonomous if there is no externalsupervision
in order to accomplish, efficiently, the robotictask. To reach full
autonomy, the robot has to know itsposition within the environment
(localization), to model the
* Corresponding author. E-mail: [email protected]
environment (mapping) and to navigate through it.1,2 TheSLAM
algorithm comes up as a solution to the problem oflocalization and
mapping within different environments.3
The SLAM algorithm recursively corrects the pose position and
orientation of the vehicle using environmentalinformation while the
environment is modeled using theestimated pose information4,5 at
the same time. The SLAMconsiders the pose of the robot and the
parameters that modelthe environment as part of the same system
state, therefore,increasing its size as the knowledge about the
environmentgrows. Several solutions to the SLAM problem have
beenproposed in the literature over the last 20 years,
mainlydepending on the robotic task. The extended Kalman
filter(EKF) is one of the most used filters as a SLAM solution.6
Itmodels the environment and the vehicle as Gaussian
randomvariables. Due to the fact that the EKF works with
linearizedmodels of the environment and the mobile robot motion,the
errors of the models are not strictly minimized.3 TheEKFSLAM
processing time is bigger than other approachessuch as the
information filter (IF) or the unscented Kalmanfilter6 (UFK). In
order to deal with non-Gaussian models,the particle filter (PF) has
shown a better performancewhen dealing with bigger amount of data
and with non-Gaussian environments. Despite of this, the EKFSLAM
iswidely used in robotics because of its simplicity and its
easyprogramming for practical robotics issues.79
The integration of the SLAM with control strategiespresents an
incipient field of research.3,10,11 Althoughreactive-based control
is implemented in most of the SLAM-control applications, plan-based
control integrating the mapderived by the SLAM remains under study.
One of the keyproblems in the fusion of SLAM with plan-based
controlstrategies in real time implementations is that the SLAM
isnot a constant time algorithm. If the environment is modeledby
features, e.g. lines, corners, corridors, trees, furrows, theSLAM
system state increases its size as the robot acquiresmore features
from the environment.
The increment of the size of the SLAM system statesrepresents an
increment of the processing time needed toexecute the SLAM
algorithm. Furthermore, the processingtime of the EKFSLAM increases
with the number offeatures with appropriate association that will
be usedduring the correction stage of the algorithm. Several
attemptsto restrict the processing time of the correction stage
ofthe SLAM algorithm can be found in the literature. For
-
http://journals.cambridge.org Downloaded: 24 Nov 2013 IP
address: 200.83.129.91
272 Towards features updating selection based on the covariance
matrix of the SLAM system stateexample, in ref. [12], a
modification of the SLAM correctionstage is proposed, decreasing
the computational time of thealgorithm.
The feature extraction procedure must be accurate, robustand
reliable. Many works found in the literature use
thesecharacteristics of the feature extraction procedure to
selectthe features to be used by the SLAM algorithm. Thus,
thefeature selection criterion depends on the feature
extractionprocedure which in turn depends on the type of sensorused
by the robot. In refs. [4, 5] the authors present afeature-based
SLAM that extracts trees from the environmentusing a Laser range
sensor and a clustering algorithm. Theydiscard the elements of the
environment that the cluster doesnot give a reliable detection. In
refs. [13, 14] the authorspresent a visual-based SLAM that uses
principal componentanalysis (PCA) to extract features from the
environment andto discard redundant information. For indoor
applications,the most widely used features are geometric primitives
thatcan be associated with elements of the environments suchas
walls associated with lines and corners. In ref. [8]the authors
present a clustering algorithm to extract linesfrom range
measurements. Thus, if the quality of a line, e.g.the length of the
associated wall, or the number of pointsused by the cluster does
not exceed a certain threshold,that line is discarded and it will
not be used during theSLAM algorithm. Other algorithms have been
proposed inthe scientific literature to extract lines. The work
of15 uses theHough transformation to obtain lines from the
environmentwith the corresponding computational cost. The fusion of
theinformation provided by two or more exteroceptive sensorsin
order to obtain a robust feature extraction procedure wasalso
implemented. In refs. [16, 17] the authors present thefusion of a
monocular video with a range laser in order toextract lines,
corners and planes from the environment. Theyuse fuzzy algorithms
to determine the features and thosefeatures with the lowest
covariance are then selected.
In the works presented before, the features selectingprocedure
is performed during the feature extraction ordetection process.18
Once the features are selected, they areused by the SLAM algorithm.
On the other hand, other worksattempted to restrict the number of
features to be used duringthe correction stage of the SLAM19,20
independently of thefeature extraction procedure. In ref. [21], a
threshold of theentropy of the feature calculated from the
covariance matrixof the SLAM system state is taken into account for
choosingthe features to be used in the SLAM; in ref. [9], the
SLAMuses only the two last features for the correction. All
theseworks do not evaluate before the updating stage how thefeature
will affect a priori the SLAM process.
In this work we propose an EKFSLAM with featureupdating
selection criterion for choosing whether a featureshould be used in
the correction stage of the SLAM.The selection criterion is based
on the evaluation of theeigenvalues of the covariance sensibility
ratio of the SLAMsystem state. Thus, we restrict the correction
stage to thosefeatures with correct association that presents the
smallestsum of eigenvalues of the covariance ratio affected by
theobserved feature. In that way, the correction is made inall
directions of the estimation process. Furthermore, theEKFSLAM
configuration used in this paper corresponds
to a sequential algorithm.6 The Jacobian matrices of
theobservation models in the sequential EKFSLAM, are sparsematrices
which reduces the computational cost of the SLAM.Real time
experimental results along with consistency testsand computational
costs analysis are shown in this work.The experimental results
shown were carried out for boththe classical sequential EKFSLAM and
the EKFSLAMwith feature updating selection proposed in this work.
Inaddition, a KullbackLeibler divergence test is carried outin
order to show that the map built by the EKFSLAMwith feature
updating selection presents a smaller divergencevalue with respect
to the classical sequential EKFSLAMwhen compared with a digitalized
architectonic draw of thereal environment used for the
experiments.
This paper is organized as follows: Section 2 presentsboth the
classical sequential EKFSLAM and the EKFSLAM with features updating
selection proposed in thiswork. Section 3 shows the consistency
tests of the SLAMalgorithm and the experimental results carried out
at thefacilities of the Instituto de Automatica. Section 4 shows
theKullbackLeibler divergence curves of the different mapsbuilt by
the SLAM algorithms and Section 5 shows theconclusions of this
work.
2. MethodsIn this section, the classic sequential EKFSLAM
ispresented along with the EKFSLAM algorithm withrestriction over
the number of features to be updated proposedin this paper.
2.1. EKFSLAM algorithmThe EKF equations for the SLAM problem
solution arestated in Eqs. (1)(5). By hypothesis, all variables
involvedin the estimation process are considered as Gaussian
randomvariables. Thus, the SLAM system state and the featuresmodel
have Gaussian distributions.
Prediction stage
t = f ( t1, ut ), (1)Pt = AtPt1ATt + WtQt1WTt . (2)
Correction stage
Kt = Pt H Tt(HtP
t H
Tt + Rt
)1, (3)
t = t + Kt (zt h( t )), (4)Pt = (I KtHt )Pt . (5)
In Eqs. (1) and (2) the prediction stage of the EKF is
stated,where t is the predicted system state at time t; ut is the
inputcontrol commands and t is the corrected system state at timet;
f describes the evolution of the elements of . Pt and Pt arethe
predicted and corrected covariance matrices respectively,at time t;
At is the Jacobbian matrix of f with respect to theSLAM system
state ( ) and Qt is the covariance matrix of thenoise associated
with the process, whereasWt is its Jacobbianmatrix. Equations
(3)(5) represent the correction stage ofthe EKFSLAM. Kt is the
Kalman gain of the algorithm
-
http://journals.cambridge.org Downloaded: 24 Nov 2013 IP
address: 200.83.129.91
Towards features updating selection based on the covariance
matrix of the SLAM system state 273at time t; Ht is the Jacobian
matrix of the measurementmodel (h in Eq. (4)) and Rt is the
covariance matrix of theactual measurement zt . In Eq. (4), (zt h(
t )) is called theinnovation process6 and takes place when the data
associationprocedure has reached an appropriate matching between
theobserved feature and the predicted one (h( t )). Both,
theprocess model (f in Eq. (1)) and the observation model
arenon-linear expressions. In this work, the features extractedfrom
the environment correspond to lines and corners, eitherconcave or
convex.
The SLAM system state is composed by both the vehiclesdegrees of
freedom and the map, represented by theparameters that define the
features. The map is consideredas stationary. Thus, the evolution
computed in Eq. (1)corresponds to the robots movements. More
informationabout this topic can be found in ref. [4, 5]. The mobile
robotused in this work is a non-holonomic unicycle type Pioneer3DX,
built by ActivMedia R . Figure 1 shows a scheme ofthe vehicle and
Eq. (6) represents their kinematic equations.The robot has a range
sensor laser -built by SICK R whichacquire 181 measurements, from 0
to 180, within a 30m range. The models of the features are shown in
Eqs. (7)and (8), and Fig. 2 shows the geometrical representation
ofeach variable used in (7) and (8). More information aboutfeatures
extraction and the SLAM algorithm implemented inthis work can be
found in refs. [3, 8]:
xtyt
t
=
xt1yt1
t1
+ t
cos(t1) a sin(t1)sin(t1) a cos(t1)
0 1
(
utt
)+ t, (6)
zcorner,t = hi[Poserobot,t , pi,t , wt ] =[zRz
]
=
(xt xe)2 + (yt ye)2
arctan(yt yext xe
) t
+
[wRw
], (7)
zline,t = hi[Poserobot,t , pi,t , wt ] =[zz
]
Fig. 1. Schematic of the mobile robot used.
Fig. 2. Feature extraction.
=[r xt cos() yt sin()
t]
+[ww
]. (8)
In Eq. (6), xt , yt and t are the coordinates of h the pointof
control in Fig. 1; of t is the discrete time Gaussianprocess noise
associated with the robots model, ut is thelinear velocity command
applied to the vehicle and t is theangular velocity command. In
Eqs. (7) and (8), w signals wR,w,w,w represent the Gaussian
measurementnoises associated with the observation models (zline,t
andzcorner,t ).
In this work, a sequential EKF algorithm was implementedas in
ref. [6]. In this algorithm, the correction iterationis executed
with one innovation at a time. This algorithmimplies that the
Jacobian matrix of the observation model,H, be a sparse matrix as
shown in ref. [6]. Along with themap generated by the SLAM process,
a secondary mapwas also implemented. This secondary map, storages
thebeginning and ending points of the segments associated withlines
in the environment. It is also maintained and updatedaccording to
the evolution of the SLAM system state. Thesecondary map allows the
environment reconstruction forfeasible navigation purposes. More
details about this mapcan be found in ref. [23].
2.2. Classical sequential EKFSLAMThe classical sequential
EKFSLAM was previouslypresented in ref. [6]. It has the property of
sequentiallyupdating the SLAM system state one feature at a time.
Thus,the Jacobbian matrices of the observation model are
sparsewhich reduce the processing time of the algorithm. This isdue
to the fact that the entries of the Jacobbian matrix of
theobservation model that do not correspond to the observedfeature
will be considered as zero. Algorithm 1 summarizesthe classical
sequential EKFSLAM.Let Nt be the set of observed features at time t
(i)Let Mt Nt be the set of features with correct
matching (ii)for i = 1 : #Mt (iii)
Kt = Pt H Tt(HtP
t H
Tt + Rt
)1 (iv)t = t + Kt (zt h( t )) (v)
-
http://journals.cambridge.org Downloaded: 24 Nov 2013 IP
address: 200.83.129.91
274 Towards features updating selection based on the covariance
matrix of the SLAM system statePt = (I KtHt )Pt (vi)Mt = Mt {zi}
(vii)Pt := Pt, t := t (viii)
end for (ix)Algorithm 1 Classical sequential EKFSLAM
Algorithm 1 represents the correction stage of the
classicalsequential EKF-SLAM. Thus, the prediction stage of theEKF
along with the computing of the robots movements isnot shown. After
observing the environment, those featuresthat present a correct
data association with respect to thepredicted ones, will be used
for the EKF correction sentence(i) and (ii). In this paper, the
data association method usedis based on the Mahalanobis
distance.2,24 The for loop insentences (iii)(ix) shows the
sequentially part of the EKF-SLAM. For a given feature zt , the
Kalman gain, the SLAMsystem state updating and the posterior
covariance of theSLAM system state are carried out in sentences
(iv)(vi).Sentences (vii) implies that the zt feature is extracted
fromthe set of features previously found in (ii) and will no
longerbe used during the correction stage. The SLAM systemstate
updated in (v) and the posterior covariance in (vi) willbecome,
respectively, the predicted SLAM system state andthe prior
covariance matrix of the SLAM in the next iterationof the algorithm
sentence (ix). The for loop repeats untilall features with correct
association were used.
2.3. Sequential EKFSLAM with featuresupdating selectionThe
features updating selection procedure proposed in thispaper does
not alter the sequential execution of the EKFSLAM presented in
Section 2.2. On the contrary, the featuresselection procedure
bounds the correction stage to thosefeatures with correct data
association that will causeonly a meaningful correction of the SLAM
system state.
In order to determine which features will be more relevantfor
the SLAM system state, let us considerer the twoconvergence
theorems presented in ref. [25].
Theorem 1. Let Pt and Pt be the prior and posteriorSLAM system
state covariance respectively. Then,
|Pt | |Pt |.
Theorem 2. Let Pt,M be the covariance matrix of the featuresof
the SLAM system state. Then,
limt |Pt,M | = 0.
Taking into account Theorem 1 above and Eq. (5), thefollowing
holds,{|Pt | = |I KtHt ||Pt ||Pt | |Pt |
|Pt ||Pt |= |I KtHt | 1.
(9)Equation (9) represents the covariance sensibility ratio
of
the SLAM system state. Thus, according to Eq. (9), thesmaller |I
KtHt |, the higher the correction. This is so,because the
determinant of the covariance matrix of theSLAM system state (|Pt
|) can be seen as a measure of the
volume of the covariance ellipse associated to the systemstate.
Considering also that |Pt | does not depend on theobserved feature,
according to Eq. (9) can be stated that
zi Mt, zopt : argz min(|Pt |) argz min(|I KtHt |).(10)
In Eq. (10), Mt is the set of features with
appropriateassociation as was previously shown in Algorithm
1.Equation (10) also provides the tool for choosing the
features(zopt ) that are more significant for the SLAM. Thus,
thosefeatures with the smallest values of |I KtHt | will beused in
the correction stage. Instead of processing theentire determinant
of (I KtHt ), we will calculate theireigenvalues. Due to the fact
that (I KtHt ) is a matrixwhich dimensions vary according to the
number of features ofthe map, the calculation of their eigenvalues
could demandhigher processing time than the classical sequential
EKFSLAM. To avoid this situation, lets considerer that Ht is
asparse matrix of the form:
Ht = [Hv,t 1 Hz,t 2] (11)
as was stated in Section 2.1. In Eq. (11), the Jacobbian of
theobservation model (Ht ) is composed by the Jacobbian of
theobservation with respect to the vehicles degrees of freedom(Hv,t
) and the Jacobbian of the observation with respect tothe features
parameters on the position of the feature (Hz,t );
1 and 2 are null block matrices.
The Kalman gain, Kt , can also be calculated in the formof Eq.
(11). Thus,
Kt =[KTv,t K
T
1 K
Tz,t K
T
2]. (12)
Using Eqs. (11) and (12), the expression (I KtHt ) can
becalculated as
(I KtHt ) =
Iv
I1Iz
I2
Kv,t
K1Kz,t
K2
[Hv,t 1 Hz,t 2
],
=
Iv Kv,tHv,t Kv,tHz,t
K1Hv,t I1 K1Hz,t
Kz,tHv,t Iz Hz,t
K2Hv,t K2Hz,t I2
.
(13)
In Eq. (13), I is the identity matrix; Iv , I1, Iz, I2 are
identityblock matrices with the dimensions of Kv,tHv,t ,
K11,Kz,tHz,t and K22 respectively, and is a general nullblock
matrix.
-
http://journals.cambridge.org Downloaded: 24 Nov 2013 IP
address: 200.83.129.91
Towards features updating selection based on the covariance
matrix of the SLAM system state 275Up to this point, it can be seen
that the determinant
|I KtHt | can be calculated directly from Eq. (13). Thus,
|I KtHt | =
Iv Kv,tHv,t Kv,tHz,t
K1Hv,t I1 K1Hz,t
Kz,tHv,t Iz Hz,t
K2Hv,t K2Hz,t I2
,
=Iv Kv,tHv,t Kv,tHz,tKz,tHv,t Iz Kz,tHz,t
. (14)Eq. (14) shows the reduced form of |I KtHt |. If one of
theeigenvalues of Eq. (14), tends to zero faster than the
others,then the entire determinant will tend to zero. This could
causethat the posterior covariance matrix of the SLAM systemstate,
Pt , be corrected faster in the direction associated withthat
eigenvalue than in the rest of the directions. Insteadof using Eq.
(14) for choosing the significant features asstated previously in
this paper we propose to observe theeigenvalues evolution of (I
KtHt ). Nevertheless, the lastdoes not contradict Eq. (14),
instead, the SLAM algorithmwill be able to choose the direction of
the correction basedon the eigenvalues information. Thus, the
determinant of Eq.(14) provides lesser information than the
observation of theeigenvalues.
In order to calculate the eigenvalues of Eq. (13), thefollowing
theorem is necessary.
Theorem 3. Let S nn be a non-singular matrix of theform
S = A L1 B1 Ir 2
C L2 D
,
where Ir is an identity block matrix with dimension r <
n;A,B,C,D,L1, L2 are non-empty block matrices; 1 and
2 are null block matrices. Then, the following
statementholds,
Eig([
A B
C D
]) Eig(S).
Thus, the set of eigenvalues of the sub-matrix [AB
C
D] is
included in the set of eigenvalues of S.Proof. Applying the
definition of eigenvalue, we have that
S I =A IA L1 B1 Ir Ir 2
C L2 D ID
with IA and ID in the dimensions of A and D
respectively.Then,
|S I | = (S I )rA IA BC D ID
= (S I )r
[A B
C D
]
[IA
ID
] ,
Eig
([A B
C D
]) Eig(S).
According to this, the eigenvalues of S are: Eig(S) =Eig([A
B
C
D]) Ones, where Ones is a set of r eigenvalues
equal to the unity.
Corollary. Identical demonstration is performed if S wereof the
form
S =A 1 BL1 Ir L2
C 2 D
.
Thus, according to Eq. (13), let us suppose that S = I KtHt,
then because of Theorem 3 we have that
Eig([
Iv Kv,tHv,t Kv,tHz,tKz,tHv,t Iz Kz,tHz,t
]) Eig(I KtHt ).
(15)
Considering that Iv is a 3 3 matrix (the robot has threedegrees
of freedom) and Iz is a 2 2 matrix (every featureis defined by two
parameters, see Eqs. (7) and (8)), the finalcalculation in (15) are
the eigenvalues of a 5 5 matrix.Furthermore, by inspection of the
proof of Theorem 3, it canbe concluded that the eigenvalues of [
IvKv,tHv,tKv,tHz,t
Kz,tHv,tIzKz,tHz,t ]
are the only eigenvalues of (I KtHt ) affected by theobservation
zi in Algorithm 1, because the rest of theeigenvalues are equal to
the unity.
Equation (15) also provides the feature selection
criterionmethod used in this work. Considering that the aim of
thispaper is to update the correction stage of the EKFSLAMwith the
most significant features, this approach is based onobserving the
sum of eigenvalues of Eq. (15). Thus, froma set of Mt features with
correct association, those featureswith the lowest sum of
eigenvalues will be chosen for thecorrection stage.
zi Mt, zopt : argz min(|Pt |) argz min
(
Eig
([Iv Kv,tHv,t Kv,tHz,tKz,tHv,t Iz Kz,tHz,t
])). (16)
The main difference of taking the sum of eigenvalues in(15)
instead of its determinant is that, if an eigenvalue tendsto zero,
also does the determinant and the correction is notguaranteed to
happen in all directions.
The implementation algorithm with the approachpresented in this
work is summarized in Algorithm 2.
Let Nt be the set of observed features at time t (i)Let Mt Nt be
the set of features with correct
matching (ii)Let MAX be the maximum number of features to be
used (iii)for i = 1 : min(#Mt,MAX) (iv)
find zopt : argz min(|Pt |) argz min(Eig
([Iv Kv,tHv,t Kv,tHz,tKz,tHv,t Iz Kz,tHz,t
]))(v)
Kt = Pt H Tt(HtP
t H
Tt + Rt
)1 (vi)t = t + Kt (zt h( t )) (vii)
-
http://journals.cambridge.org Downloaded: 24 Nov 2013 IP
address: 200.83.129.91
276 Towards features updating selection based on the covariance
matrix of the SLAM system statePt = (I KtHt )Pt (viii)Mt = Mt
{zopt} (ix)Pt := Pt, t := t (x)
end for (xi)Algorithm 2 Sequential EKFSLAM with
FeatureSelection
Algorithm 2 shows the correction stage of the EKFSLAM. Once
extracted the features of the environment anddetermined those
features with appropriate association, thefor loop of the
correction stage begins. The maximum numberof features to be
updated (MAX) is stated in sentence (iii). Inthis work, MAX = 2. In
this way, the two most significantfeatures will be used for the
correction stage. Thus, thelimit of the for loop is governed by MAX
or the number ofobserved features with correct association (for the
case when#Mt < MAX). Sentence (v) shows the feature
selectioncriterion. The most significant feature is updated first.
Inorder to do so, zopt must be found from the set Mt . The restof
the algorithm performs identically than the Algorithm 1shown
before.
The fact of updating only the most significant features inthe
eigenvalues sense ensures consistency of the SLAM see Theorem 1 and
saves processing time as will be shownin the following
sections.
3. Experimental ResultsIn this section is shown the consistency
test of the algorithmproposed in this work in addition with the
real timeexperimental results carried out at the facilities of the
Institutode Automatica of the National University of San Juan.
3.1. Consistency analysisThe consistency test was carried out
within a simulatedenvironment considering that the true position of
the mobilerobot is required. The environment was simulated usingthe
MobileSim R software provided by ActivMedia R . Theconsistency
results are shown in Fig. 3.
Figure 3(a) shows the simulated environment which iscomposed by
corners and lines. The mobile robot starts ina known initial
position and orientation inside the map, thatis, the SLAM system
state, t , is previously initialized. Inthe simulation, T0 = [ x0
y0 0 ] = [ 30 30 0.1 ]. In dashedgrey is the desired path to be
travelled by the robot throughthe environment. Figure 3(b) shows
the map reconstructionbased on the SLAM system state parameters and
thesecondary map maintained during the process.
The navigation of the mobile robot is governed by
thedetermination of local frontier points as it is shown inref.
[10]. The local frontier points strategy finds non-occupied points
of the navigable space and directs therobots movements to that
point. The last is accomplishedby generating a kinematically
plausible path from the robotsposition to the frontier point. This
exploration strategy causesthe actual travelled path by the mobile
robot differs from thedesired one. Figures 3(ce) show the error of
the estimatedpose of the mobile robot with respect to the real pose
obtained by simulation. In the three cases, the error isbounded by
two times the corresponding standard deviation.
Thus, the sequential EKFSLAM with features updatingselection
algorithm is consistent. This simulation also showshow the map is
built after closing the loop by the mobilerobot.
3.2. Real time experimental resultsSeveral experiments were
carried out at the facilities of theInstituto de Automatica of the
National University of SanJuan. In order to check the performance
of the proposedalgorithm, both the classical sequential EKFSLAM
andthe EKFSLAM with features updating selection criterionwere
implemented in parallel in the mobile robot. Thus, bothalgorithms
worked at the same time in order to share thesame mobile robots
path for both SLAM strategies. Theexploration strategy is made by
the determination of localfrontier points as was stated in Section
3.1. In addition,all experiments were real time realizations. The
odometricinformation of the mobile robot was used for the
explorationstrategy, although it was not used in the
estimationprocess.
Figure 4 shows an example of the map reconstructionwhen applying
both EKFSLAM algorithms. The initialSLAM system state was set to T0
= [ 11 8 /2 ]. Figure 4(a)shows the map reconstruction based on the
classic sequentialEKFSLAM system state whereas Fig. 4(b) shows
themap reconstruction based on the EKFSLAM with featureupdating
selection.
Since the correction stage of the SLAM with featureselection
approach is bounded by MAX the maximumnumber of features to be
updated, see Algorithm 2 the robot performs a higher number of
measurements ofthe environment, increasing the number of features
to beacquired and the possibility of revisiting past features. Asit
can be seen, both maps of Figs. 4(a) and 4(b) areconsistent. Figure
4(c) shows the processing time demandedby both algorithms. The
classical sequential EKFSLAMshows a quadratic evolution of its
accumulated processingtime constrained to the unbounded number of
featuresto be updated; the EKFSLAM with features updatingselection
also shows a quadratic evolution of the accumulatedprocessing time
but with a slower growing with respect tothe classical sequential
EKFSLAM processing time. Thisis so because, as was stated before,
the number of featuresto be updated remains unchanged. Although
Fig. 4(c) is aconsequence of the correction restriction and could
apply toany restriction policy, the fact of choosing the most
significantfeatures prevents the algorithm to become inconsistent.
Inaddition, Fig. 4(d) shows the accumulated time that the
robotremains in an open loop situation. The sampled time of
themobile robot used Pioneer 3DX must be a multiple of0.1 s. Thus,
if all the process demands, e.g., 0.15 s, thenthe sampled time of
the robot will be of 0.2 s. These 0.2 swill be the time that the
robot remains in the open loop(the robot cannot change their
driving commands). Thus, inFig. 4(d), the classical sequential
EKFSLAM has reacheda maximum sampled time of 0.7 s whereas the
EKFSLAM with the features selection procedure proposed inthis work
has remained constant to 0.2 s. The open looprepresents a
compromise in the control strategy adopted
-
http://journals.cambridge.org Downloaded: 24 Nov 2013 IP
address: 200.83.129.91
Towards features updating selection based on the covariance
matrix of the SLAM system state 277
Fig. 3. Consistency test of the sequential EKFSLAM with the
feature selection approach proposed in this paper. (a) Simulated
environment(solid black lines) with the desired path (dashed line);
(b) map reconstruction of the environment. The grey points
correspond to raw laserdata; solid black segments represent walls
of the environment; light grey circles correspond to corners; black
squares are the beginning andending points associated with the
segments and the solid grey line is the path travelled by the
mobile robot; (c) The error in the x-coordinateof the mobile robot
is bounded by two times its standard deviation; (d) The error for
the y-coordinate; (e) The error for the orientation ofthe mobile
robot.
for the mobile robot driving because collisions must
beavoided.
Considering that the classical sequential EKFSLAMcorrects the
system state with all the features with correctassociation see
Algorithm 1, as the number of featureswith correct association
grows, also grows the processingtime of the SLAM algorithm; then
the robot will havea variable sampling time. On the other hand,
with thefeature selection approach, the computational cost
remains
bounded and thus, the SLAM is more suitable for
controlimplementations.
4. Maps DivergenceThe maps shown in Figs. 4(ab) represent the
environmentbuilt by the two SLAM algorithms implemented in
thiswork. Although in both cases the mobile robot navigatesthe same
environment, the number of features acquired by
-
http://journals.cambridge.org Downloaded: 24 Nov 2013 IP
address: 200.83.129.91
278 Towards features updating selection based on the covariance
matrix of the SLAM system state
Fig. 4. Performance of the SLAM with the feature selection
approach with respect to the classical sequential EKFSLAM. (a) Map
builtusing the classical sequential EKFSLAM system state. The path
travelled by the robot is represented by a solid line; grey circles
arecorners of the environment; solid black segments represent walls
and black squares are the beginning and ending points of the
segments;(b) map built using the EFKSLAM feature updating selection
system state. The path travelled by the robot is the same than the
one shownin (a); (c) accumulated processing time. The processing
time of the feature selection approach remains quadratic but
growing slower thanthe classical sequential EKFSLAM. The SLAM with
the feature selection approach consumes lesser time the classical
EKF-SLAM; (d)Accumulated open loop time. The robot remains more
time under an open loop situation with the classical EKF-SLAM than
with thefeature selection approach proposed in this paper.
each algorithm varies. Then, also varies the precision of
themaps geometrical information, e.g. number of walls and
theirlengths. In order to get a measure of the discrepancy
betweenthe obtained maps, in this work we have implemented
theKullbackLeibler divergence criterion.
The KullbackLeibler divergence26 is a pseudo-metricthat gives
the divergence between two probability densityfunctions and it is
presented in Eq. (17) for the continuumcase and in (18) for the
discrete case. The reference functionsin Eqs. (17) and (18) are
g(.), density function, and g(.), massfunction, respectively:
T0 = [x0 y0 0] = [ 30 30 0.1 ], (17)
Dp||q =
p(x) log
(p(x)q(x)
). (18)
In order to apply the KullbackLeibler divergence to themaps
built by the SLAM algorithms, it is first necessary to
calculate the probability associated to every point within
themap. That is, every point in the space travelled and mappedby
the robot should have a probability value associated withit. The
most widely used solution in the literature is togrid the map.6
Nevertheless, the gridding process demandshigh computational cost,
directly related to the size of thegrid and the map, making it
unsuitable for possible realtime applications. In addition, a
probability value must becalculated for each cell in the gridded
map.
In this work we propose a Monte Carlo experiment forgenerating a
set of significant points covering the map andthen, calculate the
probability value associated with eachpoint using the concept of
sum of Gaussians.21 The followingsection will explain the last in
detail.
4.1. Monte Carlo experimentThe Monte Carlo experiment consists
of generatingEuniformly distributed points within the map built by
theSLAM. In order to do so, the limits of the map must be
-
http://journals.cambridge.org Downloaded: 24 Nov 2013 IP
address: 200.83.129.91
Towards features updating selection based on the covariance
matrix of the SLAM system state 279previously defined. For
comparison purposes, the same setE should cover all the maps that
are going to be used in theKullbackLeibler divergence. Thus, the
limits of the mapare calculated according to the maximum and
minimumvalue occupied by a feature in the Cartesian
coordinatesystem in both maps. For example, if we use Figs. 4(a)and
4(b), the minimum common x-coordinate (Xmax) limitbetween both maps
is near 9 m; the maximum common x-coordinate (Xmin) limit is near
19 m; the minimum commony-coordinate (Ymax) limit is near 6 m and
the maximumcommon y-coordinate (Ymin) limit is near 27 m. Thus, the
setof points E should be uniformly generated within those
limits.The Monte Carlo experiment is then executed as shown inEqs.
(19)(22):
i U (0, 1), i = 1, . . . #E, (19)mi = Xmin + (Xmax Xmin) i,
(20)
i U (0, 1), i = 1, . . . #E, (21)ni = Ymin + (Ymax Ymin) i .
(22)
Equations (19) and (21) represent the independent
uniformlygeneration of points, where # E is the number of
pointsthat will be used; the pair (mi ni )T E represents a pointof
the map bounded by the limits Xmin, Xmax, Ymin andYmax. Figure 5
shows how a partiality of the map shown inFig. 4(a) is affected by
the Monte Carlo experiment. For thisrealization were used 200
points (#E = 200). In addition,the map in Fig. 4(b) has the same
Monte Carlo pointsdistribution.
4.2. Sum of GaussiansTo calculate the probability of a point
inside the built mapit is necessary to determine how a feature
influences theprobability of that point. By considering that all
features
Fig. 5. Monte Carlo points generation. A partiality of the
mapshown in Fig. 4(a) is covered by the Monte Carlo points (solid
greycircles); the solid black lines are the segments associated to
wallsof the environment; light grey circles are corners and black
squaresare the beginning and ending points of the segments.
included in the SLAM system state are Gaussian randomvariables,
the probability of a point given all these featurescan be estimated
by a sum of Gaussians.27 Thus, if L is thenumber of features
contained in the SLAM system state, theprobability of point the pi
= [mi ni ]T is calculated as it isshown in (23).
Probability of pi
=L
k=1k
1(2)2 |Pk|
e 12
([mini
][
kxky
])TP1k
([mini
][
kxky
]). (23)
In Eq. (23), Pk is the covariance matrix of the kth feature
ofthe SLAM system state, determined as shown in Eq. (24); kxand ky
are the estimated mean values of the feature; miandni are the
Cartesian coordinates of the point and k is theweight factor
associated with the Gaussian distributions toaccomplish the
probability distribution fusion:
t =
v1.
.
.
k.
.
.
Pt =
Pvv,t PTkv,t .
.
.
.
.
.
.
.
.
.
.
.
Pkv,t Pkk,t . . .
. (24)
In Eq. (24), v is the estimate of the robots pose; i, i =1 . . .
n is the estimate of the features parameters; Pvv,t is therobots
covariance; Pkv,t is the cross-correlation between therobots
covariance and the kth features covariance and Pkk,tis the
covariance matrix of the kth feature.
In this work, the environment is represented by lines
andcorners, where corners are defined in the Cartesian space
andlines in the polar space see Eqs. (7) and (8). In order touse
Eq. (23) as an estimate of the probability of a point, it
isnecessary to refer all distribution functions to a same space.The
appropriate transformations carried out can being seen inref. [23].
Thus, once the probabilities of all points of E withrespect to all
features of the map are obtained, they must becollapsed into one
representative value for each point. To doso, a weighted sum of
Gaussians is proposed as was statedin Eq. (23). The k factors in
(23) are obtained according toref. [27]. The final result is always
less than 1 and the sumof all probability values is also less than
1.
Using the sum of Gaussians concept we have aprobabilistic
representation of all the points of Ecoveringthe map raised by the
SLAM.
4.3. KullbackLeibler divergenceApplying the Monte Carlos points
generation and thecalculation of the probability value associated
with eachof these points by means of the sum of Gaussians method,we
can convert the map obtained by the SLAM algorithmin a probability
mass function. This probability massfunction has the Monte Carlos
set of points (E) as itsdomain. Then, converting the maps obtained
by the SLAMalgorithms (see Figs. 4(a) and 4(b)) it is possible to
calculate
-
http://journals.cambridge.org Downloaded: 24 Nov 2013 IP
address: 200.83.129.91
280 Towards features updating selection based on the covariance
matrix of the SLAM system state
Fig. 6. Mean and Variance of the divergence after several
MonteCarlo realizations with different number of points generation.
(a)Divergence between the map built by the classical sequential
EKF-SLAM (considered as p(.) in Eq. (18)) and the map built by
theEKF-SLAM with feature updating selection approach (consideredas
q(.) in Eq. (18)); (b) In this case, the map built by the EKFSLAM
with feature updating selection is considered as p(.) in Eq.(18)
whereas q(.) is the map built by the classical sequential
EKFSLAM.
the divergence between their associated probability
massfunctions.
Figure 6 shows the divergence between the two mapsshown in Fig.
4. For this result, several Monte Carlorealizations were carried
out. For each number of points,a hundred experiments were executed,
obtaining in that way,the mean and variance associated with the
Monte Carlorealization. Figure 6(a) shows the divergence of the map
builtby the classical sequential EKFSLAM with respect to themap
built by the EKFSLAM with feature updating selection,and Fig. 6(b)
shows the divergence between the map built bythe feature selection
criterion and the classical EKFSLAMone (remember that we are using
a pseudo-metric measurefor the divergence of maps thus being
sensitive to the orderthe comparison is performed).
By inspection in Fig. 6 is possible to see that, in bothcases,
after the experiment of 2 104 points, the meanand variance of the
experiment remain approximately inthe same value. That means, a set
of #E = 2 104 pointsis needed to obtain a divergence value between
the mapsshown in Fig. 4. Furthermore, Fig. 7 shows the divergenceof
the maps shown in Fig. 4 with respect to a digitalizedversion of
the facilities of the Instituto de Automatica. Inorder to compute
this divergence value, the features of thereal map (the digitalized
architectonic draw of the Institutode Automatica) were considered
as degenerated Gaussiansfeatures.
Figure 7 also shows that the divergence of the EKFSLAM with
feature updating selection map with respectto the real map remains
smaller than the divergence of theclassical sequential EKFSLAM map
with respect to thereal map, for all the Monte Carlo realizations.
In both casesin Fig. 6, from #E = 1 104 we can have an idea of
thedivergence. The fact that the SLAM with feature
selectionapproach shown in this work presents a smaller value
ofdivergence with respect to the real map than the
classicalsequential EKFSLAM can be interpreted as the mapobtained
by the proposal of this work, acquires more accurategeometrical
information of the environment than the
classicalimplementation.
5. ConclusionsIn this paper, an EKFSLAM algorithm based on
therestriction in the number of features to be corrected
waspresented. The correction restriction was based on selectingthe
most significant features in the sense of the covar-iance
sensibility ratio of the SLAM system state. Thus,those features
with the smallest sum of eigenvalues in thesensibility expression
of the SLAM covariance matrix (seeEq. (9)) were chosen for the
update stage of the EKF. Thelast ensured that the estimation
process will not turn intoinconsistency.
The restriction also implied that the computational cost ofSLAM
process was bounded (when bounded the numberof features to be
corrected, as was implemented in thiswork) which reduced the open
loop time of the mobilerobot. Thus, the smaller the open loop time
the higher thenumber of control actions to drive the robot and to
map theenvironment. The fact of implementing a SLAM algorithmwith
bounded correction stage makes the algorithm moresuitable for
control strategies implementations.
In order to define the precision of the map, the KullbackLeibler
divergence was carried out between the mapsobtained by both SLAM
algorithms implemented in thiswork and a digitalized architectonic
draw of the environment.For several Monte Carlo experiments, the
map built bythe EKFSLAM with feature updating selection turnedto be
less divergent than the map built by classicalsequential EKFSLAM
when compared to the real mapof the environment. Thus, the map
built by the SLAMwith updating restrictions presented in this paper
is moregeometrically similar to the real map than the one built
bythe classical sequential EKFSLAM. The last result ensures
-
http://journals.cambridge.org Downloaded: 24 Nov 2013 IP
address: 200.83.129.91
Towards features updating selection based on the covariance
matrix of the SLAM system state 281
Fig. 7. Divergence between the maps obtained by the SLAM
algorithms and a digitalized architectonic draw of the Instituto of
Automatica(real map of the environment).
more reliable information for mobile robot maps
planningpurposes.
AcknowledgmentsThe authors gratefully acknowledge Universidad
Nacional deSan Juan of Argentina, CONICET (Argentina) and ANPCyTfor
funding this research.
References1. R. C. Arkin, Behavior-Based Robotics (MIT Press,
Cambridge,
1998).2. R. Siegwart and I. R. Nourbakhsh, Introduction to
Autonomous
Mobile Robots (MIT Press, Cambridge, 2004).3. J. Andrade-Cetto
and A. Sanfeliu, Environment Learning for
Indoor Mobile Robots (Spinger Tracks in Advanced
Robotics,Springer, 2006).
4. H. Durrant-Whyte and T. Bailey, Simultaneous localizationand
mapping (SLAM): Part I essential algorithms, IEEERobot. Autom. Mag.
13, 99108 (2006).
5. H. Durrant-Whyte and T. Bailey, Simultaneous localizationand
mapping (SLAM): Part II state of the art, IEEE Robot.Autom. Mag.
13, 108117 (2006).
6. S. Thrun, W. Burgard and D. Fox, Probabilistic Robotics
(MITPress, Cambridge, 2005).
7. J. E. Guivant and E. M. Nebot, Optimization of
thesimultaneous localization and map-building algorithm for
real-time implementation, IEEE Trans. Robot. Autom. 17,
242257(2001).
8. A. Garulli, A. Giannitrapani, A. Rosi and A. Vicino,
Mobilerobot SLAM for Line-Based Environment
Representation,Proceedings of the 44th IEEE Conference on Decision
andControl, pp. 20412046, Seville, Spain (2005).
9. J. Castellanos, R. Martinez-Cantin, J. Tardos and J.
Neira,Robocentric map joining: Improving the consistency of
EKF-SLAM, Robot. Auton. Sys. 55. 2129 (2007).
10. T. Tao, Y. Huang, F. Sun and T. Wang, Motion Planning
forSLAM Based on Frontier Exploration, Proceedings of theIEEE
International Conference on Mechatronics and Auto-mation, pp.
21202125, Takamatsu, Kagawa, Japan (2007).
11. B. Xi, R. Guo, F. Sun and Y. Huang, Simulation Researchfor
Active Simultaneous Localization and Mapping Basedon Extended
Kalman Filter, Proceedings of the IEEEInternational Conference on
Automation and Logistics,pp. 24432448, Qingdao, China (2008).
12. J. Knight, A. Davison and I. Reid, Towards Constant TimeSlam
Using Postponement, Proceedings IEEE/RSJ Int. Conf.on Intelligent
Robots and Systems, pp. 405413, Maui, Hawaii,USA (2001).
13. N. Vlassis, R. Bunschoten and B. Krose, Learning
Task-Relevant Features From Robot Data, Proceedings of theIEEE
International Conference on Robotics and Automation,pp. 499504,
Seoul, Korea (2001).
14. E. Brunskill and N. Roy, SLAM using Incremental
Probabil-istic PCA and Dimensionality Reduction, Proceedings of
theIEEE International Conference on Robotics and Automation,pp.
342347, Barcelona, Spain (2005).
15. W. Z. Yu, H. X. Han, L. Xin, W. Min and Y. H. Cheng,
ASimultaneous Localization and Mapping Method Based onFast-Hough
Transform, Inf. Technol. J. 7, 190194 (2008).
16. G. A. Borges, M. J. Aldon, V. H. Alcalde and L. R.
Suarez,Local Map Building for Mobile Robots by Fusing LaserRange
Finder and Monocular Video Images, IEEE LatinAmerican Robotic
Symposium, pp. 16, Sao Luis-MA, Brazil(2005).
17. S. Fu, H. Liu, L. Gao and Y. Gai, SLAM for MobileRobots
using Laser Range Finder and Monocular Vision,International
Conference on Mechatronics and Machine Visionin Practice, Xiamen,
China (2008) pp. 9196.
18. S. Theodoridis and K. Koutroumbas, Pattern
Recognition,(Elsevier, San Diego, USA, 2006).
19. E. Eade and T. W. Drummond, Edge Landmarks in MonocularSLAM,
The 17th British Machine Vision Conference(BMVC), pp. 588596,
Edinburgh, UK (2006).
20. Y. J. Lee and J. B. Song, Autonomous Selection,
Registrationand Recognition of Objects for Visual SLAM in
Indoor
-
http://journals.cambridge.org Downloaded: 24 Nov 2013 IP
address: 200.83.129.91
282 Towards features updating selection based on the covariance
matrix of the SLAM system stateEnvironments, The International
Conference on Control,Automation and Systems, pp. 668673, Seul,
Korea (2007).
21. D. Zhang, L. Xie and M. D. Adams, Entropy based
FeatureSelection Scheme for Real Time Simultaneous Localizationand
Map Building IEEE Conference on Intelligent Robotsand Systems, pp.
649654, Edmonton, Canada (2005).
22. S. Fintrop, P. Jensfelt and H. I. Christensen,
AttentionalLandmark Selection for Visual SLAM, Proceedings of
theInternational Conference on Intelligent Robots and Systems,pp.
25822587, Biejing, China (2006).
23. F. A. A. Cheein, Simultaneous Localization and Mapping of
aMobile Robot based on Uncertainty Zones Navigation Ph.D.Thesis
(San Juan, Argentina: National University of San Juan,2009).
24. M. Dissanayake, P. Newman, S. Clark, H. Durrant-Whyte andM.
Csorba, A solution to the simultaneous localization andmap building
(SLAM) problem, IEEE trans. Robot. Autom.17, 229241 (2001).
25. S. J. Julier and J. K. Uhlmann, A counter Example to
theTheory of Simultaneous Localization and Map Building,IEEE
International Conference on Robotics and Automation,pp. 42384243,
Seul, Korea (2001).
26. T. M. Cover and J. A. Thomas, Elements of InformationTheory,
2nd ed. (Wiley-Interscience, New York, USA,2006).
27. A. S. Miralles and M. A. S. Bobi, Global Path Planning
inGaussian Probabilistic Maps, J. Intell. Robot. Syst. 40,
89102(2004).