SLAM USING SINGLE LASER RANGE FINDER
AliAkbar Aghamohammadi, Amir H. Tamjidi, Hamid D. Taghirad
Advance Robotic and Automation Systems Lab (ARAS),Electrical and Computer Engineering DepartmentK. N. Toosi University of Technology, Iran
OUTLINE1-Motivation & Contributions2-Probabilistic Framework3-Feature Extraction4-Error Modeling For Individual Features5-Motion Prediction6-Data Association7-Adding new features 8-Filtering (IEKF)9-Results10-Conclusion11-Refrences
-200 -150 -100 -50 0 50 100 1500
50
100
150
200
250
300
350
400
μ
β β
αi+1
qd
ri+1
ri
fk (real feature in the environment)
Pi (selected edge feature)
xk
extracted features from k'th scan
covariances associated with features
Matching
optimization process
implicit function theorem
xk+1
cov(xk)
cov(xk+1
)
Prediction Box
extracted features from (k+1)'th scan
2
MOTIVATION
traditional encoder-base dynamic modeling are sensitive to:a) slippageb) surface type changingc) imprecision in the parameters of robot's
hardware.
3
MAIN CONTRIBUTIONS
The key contributions of LSLAM include:
4
PROBABILISTIC FRAMEWORK
State Vector of the system comprises of robot pose and spatial features, represented in world coordinates
At system start-up, feature-based map is initialized; this map is updated dynamically by the Extended Kalman Filter until operation ends. The probabilistic state estimates of the robot and features are updated during robot motion and feature observation. When new features are observed the map is enlarged with new states.
5
1 2
1 1 1 1 1 2
2 1 2 1 2 2
1
2
ˆ
ˆˆ ,
ˆ
r r r rr
x x x f x f
f x f f f f
f x f f f f
x P P P
f P P Px P
P P Pf
1 2,k
kk
r tk f n
f
xx x f f f
x
Robot Pose
features
OUTLINE
-200 -150 -100 -50 0 50 100 1500
50
100
150
200
250
300
350
400
μ
β β
αi+1
qd
ri+1
ri
fk (real feature in the environment)
Pi (selected edge feature)
xk
extracted features from k'th scan
covariances associated with features
Matching
optimization process
implicit function theorem
xk+1
cov(xk)
cov(xk+1
)
Prediction Box
extracted features from (k+1)'th scan
6
1-Motivation & Contributions2-Probabilistic Framework3-Feature Extraction4-Reliability Measure Calculation
5-Motion Prediction6-Data Association7-Adding new features 8-Filtering (IEKF)9-Results10-Conclusion11-Refrences
FEATURE EXTRACTION
k
k
r
kf
xx
x
Point Features
Line Features
More Informative Features
7
1
2
ˆ
ˆˆf
f
x f
FEATURE EXTRACTION
8
Steps
features
OMITTING VARIANT FEATURES
There exist two kind of variant features:1) Those, appear due to occlusion2) Those, appear due to low incidence angle
9
FEATURE EXTRACTION RESULTS
10-200 -150 -100 -50 0 50 100 1500
50
100
150
200
250
300
350
400
(cm)
(cm)
Variant feature
Variant feature
Invariant features
Jump edge
Jump edge
Occlusion
Occlusion
High Curvature
High Curvature
Low incidence
angleLow incidence
angle
Extracted Features
OUTLINE
-200 -150 -100 -50 0 50 100 1500
50
100
150
200
250
300
350
400
μ
β β
αi+1
qd
ri+1
ri
fk (real feature in the environment)
Pi (selected edge feature)
xk
extracted features from k'th scan
covariances associated with features
Matching
optimization process
implicit function theorem
xk+1
cov(xk)
cov(xk+1
)
Prediction Box
extracted features from (k+1)'th scan
11
1-Motivation & Contributions2-Probabilistic Framework3-Feature Extraction4-Reliability Measure Calculation
5-Motion Prediction6-Data Association7-Adding new features 8-Filtering (IEKF)9-Results10-Conclusion11-Refrences
RELIABILITY MEASURE CALCULATIONFOR INDIVIDUAL FEATURES
Feature uncertainty Observation noise Uncertainty due to quantization
, ,i i ob i q if p e e
μ
β β
αi+1
qd
ri+1
ri
fk (real feature in the environment)
Pi (selected edge feature)
Fig. 5
12 eө
er
pi
i
ri
MEASUREMENT NOISE
sin( ) cos( )( )
cos( ) sin( )i i
ob i ii i
e re ar b
13
nob i ie p p
cos( ) cos( ),
sin( ) sin( )n i
i ii i r i i
i i
ep r e p r
e
eө
er
ir ie ar b
pi
i
ri
QUANTIZATION ERROR
14
This issue causes that the point pi, considered as a feature point, not necessarily be the same physical feature in the environment.
ββ
μ
αi+1
qd
fk (real feature in the environment)
Pi (selected edge feature)
qe t
ri+1 ri ri-1
FEATURE COVARIANCE
Measurement and quantization errors are independent from each other
ˆ ˆ( ) ( ) ( ) ( ) ( )i
Tk k k k k ob iCov f E f f f f Cov e Cov eq
2 2
2
cos ( ) cos( )sin( )
3 cos( )sin( ) sin ( )id i i i
i i i
qCov e
qi
2 2
2sin ( ) -sin( ) cos( )2( )
2-sin( ) cos( ) cos ( )
2cos ( ) sin( ) cos( )2
2sin( ) cos( ) cos ( )ba
i i iCov e rob i
ii i i
i i iri
i i i
15
16
-200 -150 -100 -50 0 50 100 1500
50
100
150
200
250
300
350
400
OUE
WUE
-70 -65 -60 -55 -50 -45 -40
285
290
295
300
305
310
(cm)
(cm)
OUE
WUE
OUTLINE
-200 -150 -100 -50 0 50 100 1500
50
100
150
200
250
300
350
400
μ
β β
αi+1
qd
ri+1
ri
fk (real feature in the environment)
Pi (selected edge feature)
xk
extracted features from k'th scan
covariances associated with features
Matching
optimization process
implicit function theorem
xk+1
cov(xk)
cov(xk+1
)
Prediction Box
extracted features from (k+1)'th scan
17
1-Motivation & Contributions2-Probabilistic Framework3-Feature Extraction4-Reliability Measure Calculation
5-Motion Prediction6-Data Association7-Adding new features 8-Filtering (IEKF)9-Results10-Conclusion11-Refrences
MOTION PREDICTION
Traditional models, based on encoders' data, suffer from some problems in motion modeling such as wheel slippage, unequal wheel diameters, unequal encoder scale factors, inaccuracy about the effective size of wheel base, surface irregularities, and other predominantly environmental effects
xk
uk
wk , cov(w
k)
xk+1
=f(xk,u
k,w
k)
cov(xk+1
)=Fxcov(x
k)F
xT+F
wcov(w
k)F
wT
xk+1
cov(xk)
cov(xk+1
)
Prediction Box
Traditional Method
18
MOTION PREDICTION
we use a prediction model, which does not merely rely on robot, but it uses environmental information too. Thus, method is robust with respect to wheel slippage, surface changing and other unsystematic effects and inaccurate information about robot's hardware. 19
xk
extracted features from k'th scan
covariances associated with features
Matching
optimization process
implicit function theorem
xk+1
cov(xk)
cov(xk+1
)
Prediction Box
extracted features from (k+1)'th scan
LSLAMMethod
Matching:
Pose Shift Calculation ( Cost function based on weighted feature-
based Range scan matching )
20
-300 -200 -100 0 100 200-300
-250
-200
-150
-100
-50
0
50
100
150
200
(cm)
(cm)
connecting line
robot
MOTION PREDICTION
1
1
ˆ ˆ ˆ ˆ( ( )) ( ( ))m
pre new t pre new
j j j j j
j
E f Rf T C f Rf T
If there was an explicit relationship between features and pose shift:
Indeed, Since T* and R* have to minimize the cost function E, we have an implicit relationship derived from:
X contains the parameters
of T and R.
Thus there is an implicit relationship between features and pose shift.
But there is not !!!But there is not !!!
cov( ) cov( ) TX J F J*
* 2ˆ ˆ ˆ( ) ( ) ( )X
X g F F F O F FF
21
MOTION PREDICTION – Uncertainty Calculation
* ( )X g F
*
*
( , ) 0X X
X FE
X
The implicit function theory can provide the desired Jacobian via below equation:
22
MOTION PREDICTION – Uncertainty Calculation
1
*
12 2
*
2
JX F
E EJ at X X
X F X
complicated but a tractable matter of differentiation
*cov( ) cov( ) T
X XX J F J
*XJ
F
OUTLINE
-200 -150 -100 -50 0 50 100 1500
50
100
150
200
250
300
350
400
μ
β β
αi+1
qd
ri+1
ri
fk (real feature in the environment)
Pi (selected edge feature)
xk
extracted features from k'th scan
covariances associated with features
Matching
optimization process
implicit function theorem
xk+1
cov(xk)
cov(xk+1
)
Prediction Box
extracted features from (k+1)'th scan
23
1-Motivation & Contributions2-Probabilistic Framework3-Feature Extraction4-Reliability Measure Calculation
5-Motion Prediction6-Data Association7-Adding new features 8-Filtering (IEKF)9-Results10-Conclusion11-Refrences
DATA ASSOCIATION
Batch data association methods greatly reduce the ambiguity in data association process. Thus, here JCBB method is adopted for data association.
After data association process, extracted features from new scan fall into two categories: New features, which are not matched with any
existent feature in the map Existing features, (matched ones)
24
FILTERING AND ADDING NEW FEATURES
Existing features, (matched ones) are used to update the system state vector
Each newly seen feature is first transformed to the map reference coordinate and then the transformed feature is augmented with the system state vector.
1
1
ˆˆ ˆ
kaugmented
wknewlyseen
xx
f
25
( ) ( ) 1
1, 1 1 1cov( ) [ cov( ) cov( )]T T
k i k x x k x
r
kK x H H x H F
( ) ( ) ( ) ( ) ( )
1, 1 1 1, 1, 1 1,1ˆ ˆ ˆ ˆ ˆ[ ( ( ) ]( ))
k i k k i k i k k i
r
k xx x K F h x H x x
( ) ( ) ( )
1, 1 1 1, 1cov( ) cov( ) cov( )
k i k k i x kx x K H x
1-calculate kalman gain
3-calculate covariance update
2-calculate state vector update
OUTLINE
-200 -150 -100 -50 0 50 100 1500
50
100
150
200
250
300
350
400
μ
β β
αi+1
qd
ri+1
ri
fk (real feature in the environment)
Pi (selected edge feature)
xk
extracted features from k'th scan
covariances associated with features
Matching
optimization process
implicit function theorem
xk+1
cov(xk)
cov(xk+1
)
Prediction Box
extracted features from (k+1)'th scan
26
1-Motivation & Contributions2-Probabilistic Framework3-Feature Extraction4-Reliability Measure Calculation
5-Motion Prediction6-Data Association7-Adding new features 8-Filtering (IEKF)9-Results10-Conclusion11-Refrences
RESULTS
27
Melon: a tracked mobile robot equipped with two low range Hokuyo URG_X002 laser range scanners(High Slippage)
An Structured Environment
PURE LOCALIZATION
28
ICP Method
RESULTS(PURE LOCALIZATION)
29
HAYAI Method
PURE LOCALIZATION
30
Proposed motion model
LSLAM
The environment consists of many features. Ground truth is available Loop closing effects can be investigated in a large loop
31
-800 -700 -600 -500 -400 -300 -200 -100 0 100 200-200
-100
0
100
200
300
400
500
600
700
800
Start of pathEnd of path(after a lap)
Simulation Results
LSLAM - SIMULATION
0 20 40 60 80 100 120 140-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
2.5
step number
erro
r in
X d
irect
ion
0 20 40 60 80 100 120 140-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
step number
err
or
in Y
dire
ctio
n
0 20 40 60 80 100 120 140-0.4
-0.3
-0.2
-0.1
0
0.1
0.2
0.3
step number
err
or in
he
ad
ing
an
gle
32
Estimated errors (blue curves) and estimated variances (red curves) in x, y and theta (robot heading)
Error in y
Error in x
Error in θ
LSLAM (REAL SCAN DATA)
33
-300 -200 -100 0 100-100
-50
0
50
100
150
200
250
300
x (cm)
y (c
m)
LSLAM
Feature-based map resulted from LSLAM
0 50 100 150-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
step number
err
or
in x
dire
ctio
n
0 50 100 150-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
step number
err
or
in y
dire
ctio
n
0 50 100 150-4
-3
-2
-1
0
1
2
3
4
step number
erro
r in
head
ing
angl
e
Error in x Error in θError in y
0 50 100 150-400
-300
-200
-100
0
100
200
300
400
step number
err
or
in x
dire
ctio
n
0 50 100 150-250
-200
-150
-100
-50
0
50
100
150
200
250
step number
erro
r in
y di
rect
ion
0 50 100 150-150
-100
-50
0
50
100
150
step number
erro
r in
head
ing
angl
e
Pure Localization
8-CONCLUSION
introducing robust motion model with respect to robot slippage and inaccuracy in hardware-related measures
calculating reliability measure for robot’s displacement derived through the feature-based laser scan matching
Extract features in different scales construct an IEKF framework merely based
on laser range finder information
34
9-REFERENCES
[1] Robot pose estimation in unknown environments by matching 2D range scans. Lu, F. and Milios, E. 1997. 1997, Journal of Intelligent and Robotic Systems, Vol. 18, pp. 249-275.
[2] Metric-based scan matching algorithms for mobile robot displacement estimation. Minguez, J., Lamiraux, F. and Montesano, L. 2005. Barcelona, Spain. : s.n., 2005. Proceedings of the IEEE International Conference on Robotics and Automation (ICRA).
[3] Scan alignment with probabilistic distance metric. AJensen, B. and Siegwart, R. 2004. 2004. Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems.
[4] Weighted range sensor matching algorithms for mobile robot displacement estimation. Pfister, S., et al. 2002. s.l. : Proceedings of the IEEE International Conference on Robotics and Automation (ICRA’02), 2002. pp. 1667-1674.
[5] Feature-Based Laser Scan Matching For Accurate and High Speed Mobile Robot Localization. Aghamohammadi, A.A., et al. 2007. s.l. : European Conference on Mobile Robots (ECMR’07), 2007.
[6] High-speed laser localization for mobile robots. Lingemann, K., et al. 2005. 4, s.l. : Journal of Robotics and Autonomous Systems, 2005, Vol. 51, pp. 275–296.
[7] Natural landmark-based autonomous vehicle navigation. Madhavan, R. and Durrant-Whyte, H. F. 2004. s.l. : Robotics and Autonomous Systems, 2004, Vol. 46, pp. 79-95.
[8] Mobile robot positioning with natural landmark. Santiso, E., et al. 2003. Coimbra, Portugal : s.n., 2003. Proceedings of the 11th IEEE International Conference on Advanced Robotics (ICAR’03). pp. 47-52.
[9] Recursive Scan-Matching SLAM. Nieto, J., Bailey, T. and Nebot, E. 2007. 1, s.l. : Journal of Robotics and Autonomous Systems, January 2007, Vol. 55, pp. 39-49.
[10] Nieto, J. 2005. Detailed environment representation for the slam problem. Ph.D. Thesis. s.l. : University of Sydney, Australian Centre for Field Robotics, 2005.
[11] Globally consistent range scan alignment for environment mapping. Lu, F. and Milios, E. 1997. : Autonomous Robots, 1997, Vol. 4, pp. 333–349.
[12] An Interior, Trust Region Approach for Nonlinear. Coleman, T.F. and Y., Li. 1996. s.l. : SIAM Journal on Optimization, 1996, Vol. 6, pp. 418-445.
[13] Data association in stochastic mapping using the joint compatibility test. Neira, J. and Tardos, J.D. 2001. 6, s.l. : IEEE Transactions on Robotics and Automation, 2001, Vol. 17, pp. 890–897.
[14] Gelb, A. 1984. Applied Optimal Estimation. s.l. : M.I.T. Press, 1984. [15] A real-time algorithm for mobile robot mapping with applications to multi-robot and 3D mapping. Thrun,
S., Bugard, W. and Fox, D. 2000. s.l. : International Conference on Robotics and Automation, 2000. pp. 321–328.
35