slam u sing s ingle l aser r ange f inder aliakbar aghamohammadi, amir h. tamjidi, hamid d. taghirad...

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