Introduction to SLAM Simultaneous Localization and Mapping_ Paul Robertson

8/13/2019 Introduction to SLAM Simultaneous Localization and Mapping_ Paul Robertson

1/37

Introduction to SLAM

Simultaneous LocalizationAnd Mapping

Paul Robertson

Cognitive RoboticsWed Feb 9th, 2005


2/37

Outline

Introduction Localization

SLAM Kalman Filter

Example

Large SLAM Scaling to large maps

2


3/37

Introduction

3

(Localization) Robot needs to estimate itslocation with respects to objects in its

environment (Map provided). (Mapping) Robot need to map the positions

of objects that it encounters in itsenvironment (Robot position known)

(SLAM) Robot simultaneously mapsobjects that it encounters and determines itsposition (as well as the position of the

objects) using noisy sensors.


4/37

Show Movie

4


5/37

Localization

Tracking

Bounded uncertainty

Can flip into kidnapping problem

Global Localization

Initially huge uncertainties

Degenerates to tracking

Kidnapping Problem Unexpected global localization

Tracking

GlobalLocalization

Kidnapped

5


6/37

Representing Robots

Position of robot isrepresented as a triple

consisting of its xt, ytcomponents and itsheading t:

Xt=(xt, yt, t)T

Xt+1=Xt + (utt cos t, utt sin t, utt)T

xt

xt+1t

xi, yi

utt

6


7/37

Kalman Filter Localization

Robot

Landmark

7


8/37


Robot

Landmark

8


9/37


Robot

Landmark

9


10/37


Robot

Landmark

10


11/37

Basic SLAM

Localize using a Kalman Filter (EKF)

Consider all landmarks as well as the robotposition as part of the posterior.

Use a single state vector to store estimates

of robot position and feature positions. Closing the loop allows estimates to be

improved by correctly propagating all thecoupling between estimates which arise inmap building.

11


12/37

Initialize Feature A

CB

A

12


13/37

Drive Forward

CB

A

13


14/37

Initialize C

14

CB

A


15/37

Initialize B

15

A

CB


16/37

Drive Back

16

A

CB


17/37

Re-measure A

17

A

CB


18/37

Re-measure B

18

A

CB


19/37

Basic EKF framework for SLAM

x =

xvy

1y2..

.

System State Vector

, P=

Pxx Pxy1 Pxy2 Py1x Py1y1 Py1y2

Py2x Py2y1 Py2y2 . . .. . .

. . .

Covariance Matrix (Square, Symmetric)

x and P grow as features are added to the map!

19


20/37

SLAM steps

1. Define robot initial position as the root of theworld coordinate space or start with some pre-

existing features in the map with highuncertainty of the robot position.

2. Prediction: When the robot moves, motion

model provides new estimates of its newposition and also the uncertainty of its location positional uncertainty always increases.

3. Measurement: (a) Add new features to map (b)re-measure previously added features.

4. Repeat steps 2 and 3 as appropriate.

20

Th K l Filt


21/37

The Kalman Filter

Features:

1. Gaussian Noise2. Gaussian State Model

3. Continuous States4. Not all state variables need to be observable.

5. Only requires memory of the previous estimate.6. Update is quadratic on the number of state

variables.

21

U d i i i h


22/37

Updating state estimate with a

new observation

22

1. Predict current state from the previous

state estimate and the time step.

2. Estimate the observation from the

prediction.3. Computer the difference between the

predicted observation and the actualobservation.

4. Update the estimate of the current

state


23/37

Vector Kalman Filtergain correction

G(k)

C T A

Y(k)+

-

+

+

X(k)

estimate

delay

AX(k-1)measurement parameter

ACX(k-1)

Y(k)

system parameter

current prediction

23

G(k) = AP(k|k-1)CT[CP(k|k-1)CT+R(k)]-1 (Predictor gain)

P(k+1|k) = [A-G(k)C]P(k|k-1)AT+Q(k) (Prediction mean square error)


24/37

Simple Example


25/37

Simple Example

Robot with estimated position (Px, Py) andestimated velocity (Vx, Vy). Four state variables( Px, Py, Vx, Vy )

T

Relationships between the state variables:P

x+= tV

xPy += tVy

Observations: (Px

, Py

)

Position variance (p) = 100.0

Velocity variance (

v) = 0.125


26/37

System Equation

26

Px (k)

Py (k)

Vx (k)Vy (k)

Px (k+1)

Py (k +1)

Vx (k +1)Vy (k +1)

=

1 0 T 0

0 1 0 T

0 0 1 00 0 0 1

0

0

v

v

+

X(k+1) A X(k) w(k)


27/37

S t N i


28/37

System Noise

0 0 0 0

0 0 0 0

0 0 v 00 0 0 v

System Noise Covariance Matrix Q(k) =

28


29/37

Prediction Matrices

p

0 p

0

0 p 0 p0 p pv 0

0 p 0 pv

Initial Prediction Covariance Matrix (P) =

State Equations (A) =

1 0 T 0

0 1 0 T

0 0 1 0

0 0 0 1

29


30/37

30


31/37

Run Kalman Filter Demo Program

31


32/37

Problems with the Kalman Filter

1. Uni-modal distribution (Gaussian) oftenproblematic.

2. Can be expensive with large number ofstate variables.

3. Assumes linear transition model system equations must be specifiable as a

multiplication of the state equation. TheExtended Kalman Filter (EKF) attempts toovercome this problem.

32

Additional Reading List for


33/37

Additional Reading List for

Kalman Filter

Russell&Norvig Artificial Intelligence amodern approach second edition 15.4(pp551-559).

33

L SLAM


34/37

Large SLAM

Basic SLAM is quadratic on the number of featuresand the number of features can be very large.Intuitively we want the cost of an additional pieceof information to be constant.

Lets look at one approach that addresses this issue bydividing the map up into overlapping sub maps.

Leonard&Newman Consistent, Convergent, andConstant-Time SLAM IJCAI03

34


35/37

35

L ti V t


36/37

Location Vector

The mapped space is divided up intooverlapping sub maps with shared features in

the overlapping sub maps.

T(i, j) = (x, y, )T (Location vector)

An entity is a location + a unique ID.

A Map is a collection of entities described with

respect to a local coordinate frameEach map has a root entity i and a map location

vector T(G,m) 36


37/37

37

Documents

Introduction to SLAM Simultaneous Localization and Mapping_ Paul Robertson