09d ekf localization

8/10/2019 09d Ekf Localization

1/38

Wolfram Burgard, Cyrill Stachniss,

Maren Bennewitz, Kai Arras

EKF Localization

Introduction toMobile Robotics

Slides by Kai Arras and Wolfram Burgard

Last update: June 2010


2/38

Localization

Given

Map of the environment.

Sequence of sensor measurements.

Wanted Estimate of the robots position.

Problem classes

Position tracking

Global localization

Kidnapped robot problem (recovery)

Using sensory information to locate the robotin its environment is the most fundamentalproblem to providing a mobile robot withautonomous capabilities. [Cox 91]


3/38

Landmark-based Localization

EKF Localization:Basic Cycle

3


4/38



4


5/38



5

raw sensory data

landmarks

innovation from

matched landmarks

predictedmeasurements in

sensor coordinates

landmarks in globalcoordinates

encoder measurements

predicted

state

posteriorstate


6/38

State Prediction (Odometry)


6

Control uk: wheel displacementssl, sr

Error model: linear growth

Nonlinear process model f:


7/38

State Prediction (Odometry)


7

Control uk: wheel displacementssl, sr

Error model: linear growth

Nonlinear process model f:


8/38


Landmark Extraction (Observation)

8

Extractedlines

Hessian line model

Extracted linesin model space

Raw laserrange data


9/38


Measurement Prediction

...is a coordinate frame transform world-to-sensor

Given the predicted state (robot pose),predicts the location and location

uncertainty of expected

observations in sensor coordinates

9

model space


10/38

Data Association (Matching) Associates predicted measurements

with observations

Innovationand innovation

covariance

Matching onsignificance

level alpha


10

model space

Green: observation

Magenta: measurement prediction


11/38


Update

Kalman gain

State update (robot pose)

State covariance update

11

Red: posterior estimate


12/38

EKF Localization with Point Features



13/38

1. EKF_localization ( t-1,!t-1, ut, zt,m):

Prediction:

2.

3.

4.

5.

6. "t =Gt"t#1GtT+ B

tQ

tB

t

T

Bt ="g(ut,t#1)

"ut=

"x'

"v t

"x'

"$t"y'

"v t

"y'

"$t"%'

"v t

"%'

"$t

&

'

((((((

)

*

++++++

Qt=

"1| v

t |+"

2 |#

t |( )

2

0

0 "3 | vt |+"4 |#t |( )

2

$

%&

&

'

()

)Motion noise

Jacobian of gw.r.t location

Predicted mean

Predicted covariance

Jacobian of gw.r.t control


14/38

1. EKF_localization ( t-1,!t-1, ut, zt,m):

Correction:

2.

3.

4.

5.

6.

7.

8.

St= H

t"

tH

t

T+ R

t

Rt=

"r

20

0 "r

2

#

$%

&

'(

Predicted measurement mean

Innovation covariance

Kalman gain

Updated mean

Updated covariance

Jacobian of hw.r.t location


15/38

EKF Prediction Step


16/38

EKF Observation Prediction Step


17/38

EKF Correction Step


18/38

Estimation Sequence (1)


19/38

Estimation Sequence (2)


20/38

Comparison to GroundTruth


21/38

[Arras et al. 98]:

Laser range-finder and vision

High precision (


22/38

EKF Localization Example

Line and point landmarks


23/38


Line and point landmarks


24/38


Expo.02: Swiss National Exhibition 2002

Pavilion "Robotics"

11 fully autonomous robots tour guides, entertainer, photographer 12 hours per day 7 days per week

5 months

3,316km travel distance almost 700,000 visitors 400 visitors per hour

Localization method: Line-Based EKF


25/38



26/38

26

Global EKF Localization

Interpretation tree


27/38

27


Env. Dynamics


28/38

28


Geometric constraints we can exploit

Location independent constraints

Unary constraint:intrinsic property of feature

e.g. type, color, size

Binary constraint:

relative measure between featurese.g. relative position, angle

All decisions on a significance level "


29/38

29


Interpretation Tree[Grimson 1987], [Drumheller 1987],

[Castellanos 1996], [Lim 2000]

Algorithm

backtracking

depth-first

recursive

uses geometric constraints

worst-case exponentialcomplexity


30/38

30


Pygmalion

"= 0.95 , p= 2


31/38

31


"= 0.95 , p= 3

Pygmalion


32/38

32


"= 0.95 , p= 4texe:633 msPowerPC at 300

MHz

Pygmalion


33/38

33


"= 0.95 , p= 5

texe:633 ms(PowerPC at 300 MHz)

Pygmalion


34/38

05.07.02, 17.23 h


"= 0.999

At Expo.02

[Arras et al. 03]


35/38

texe= 105 ms

05.07.02, 17.23 h


"= 0.999

At Expo.02

[Arras et al. 03]


36/38

05.07.02, 17.32 h


"= 0.999

At Expo.02

[Arras et al. 03]


37/38

05.07.02, 17.32 h


"= 0.999 texe= 446 ms

At Expo.02

[Arras et al. 03]


38/38

EKF Localization Summary

EKF localization implements pose tracking Very efficient and accurate

(positioning error down to subcentimeter)

Filter divergence can cause lost situations from

which the EKF cannot recover Industrial applications

Global EKF localization can be achieved using

interpretation tree-based data association Worst-case complexity is exponential

Fast in practice for small maps

09d ekf localization

Documents