27
Simultaneous Localization and Mapping Presented by Lihan He Apr. 21, 2006

Simultaneous Localization and Mapping

  • Upload
    perrin

  • View
    92

  • Download
    0

Embed Size (px)

DESCRIPTION

Simultaneous Localization and Mapping. Presented by Lihan He Apr. 21, 2006. Outline. Introduction SLAM using Kalman filter SLAM using particle filter Particle filter SLAM by particle filter My work : searching problem. Introduction: SLAM. SLAM: Simultaneous Localization and Mapping. - PowerPoint PPT Presentation

Citation preview

Page 1: Simultaneous Localization and Mapping

Simultaneous Localization and Mapping

Presented by Lihan HeApr. 21, 2006

Page 2: Simultaneous Localization and Mapping

Outline

Introduction

SLAM using Kalman filter

SLAM using particle filter

Particle filter

SLAM by particle filter

My work : searching problem

Page 3: Simultaneous Localization and Mapping

Introduction: SLAM

SLAM: Simultaneous Localization and Mapping

A robot is exploring an unknown, static environment.

Given: The robot’s controls Observations of nearby features

Estimate: Location of the robot -- localization

where I am ? Detail map of the environment – mapping

What does the world look like?

The controls and observations are both noisy.

Page 4: Simultaneous Localization and Mapping

Introduction: SLAM

Markov assumption

x0 x1 x2 xt-1 xt…

o1 ot-1…

a1 at-1…

o2 ot

a2 at

m

State transition:

Observation function:

),|( 1 ttt axxp

)|( tt xop

Page 5: Simultaneous Localization and Mapping

Introduction: SLAM

Method: Sequentially estimate the probability distribution p(xt) and update the map.

Prior: p(x0)

p(x1)

p(m1) or m1

… p(xt-1)

p(mt-1) or mt-1

p(xt)

p(mt) or mt

111 )(),|()|(),|( tttttttttt dxxpaxxpxopaoxp

Prior distribution on xt after taking action at

)|( tt axp

Page 6: Simultaneous Localization and Mapping

Introduction: SLAM

Map representations

1. Landmark-based map representation

2. Grid-based map representation

Track the positions of a fixed number of predetermined sparse landmarks.

Observation: estimated distance from each landmark.

Map is represented by a fine spatial grid, each grid square is either occupied or empty.

Observation: estimated distance from an obstacle using a laser range finder.

Page 7: Simultaneous Localization and Mapping

Introduction: SLAM

Methods:

1. Parametric method – Kalman filter

2. Sample-based method – particle filter

The robot’s trajectory estimate is a tracking problem

Represent the distribution of robot location xt (and map mt ) by a Normal distribution ),( ttN

Sequentially update μt and Σt

Represent the distribution of robot location xt (and map mt) by a large amount of simulated samples.

Resample xt (and mt) at each time step

Page 8: Simultaneous Localization and Mapping

Introduction: SLAM

Why is SLAM a hard problem?

Robot location and map are both unknown.

Location error

Map error

The small error will quickly accumulated over time steps.

The errors come from inaccurate measurement of actual robot motion (noisy action) and the distance from obstacle/landmark (noisy observation).

When the robot closes a physical loop in the environment, serious misalignment errors could happen.

Page 9: Simultaneous Localization and Mapping

SLAM: Kalman filter

111 )(),|()|(),|( tttttttttt dxxpaxxpxopaoxp

Update equation:

Assume:Prior p(x0) is a normal distribution

Observation function p(o|x) is a normal distribution

Then:Posterior p(x1), …, p(xt) are all normally distributed.

Mean μt and covariance matrix Σt can be derived analytically.

Sequentially update μt and Σt for each time step t

),(~),|( ttttt Naoxp

Page 10: Simultaneous Localization and Mapping

SLAM: Kalman filter

1111 )(),|()|(),|( tttttttttt dxxpaxxpxopaoxp

Assume:

),0(~ ,1 tat

atttttt RNaGxFx

),0(~ , tot

otttt QNxHo

),(),|( 11 tttttttt RaGxFNaxxp State transition

Observation function ),()|( ttttt QxHNxop

Kalman filter:

Propagation (motion model): ),()|( tttt Naxp

,1 ttttt aGF tTtttt RFF 1

Update (sensor model): ),(),|( ttttt Naoxp 1)( t

Tttt

Tttt QHHHK

),( tttttt HoK tttt HKI )(

),,,(),( 11 tttttt oaKalman

Page 11: Simultaneous Localization and Mapping

SLAM: Kalman filter

The hidden state for landmark-based SLAM:

TNyNxyxtrtrtrt llllyxx ],,...,,,,,[ 11,,,

localization mapping

Map with N landmarks: (3+2N)-dimentional Gaussian

State vector xt can be grown as new landmarks are discovered.

NyNyNyNxNyyNyxNyrNyrNyr

NyNxNxNxNxyNxxNxrNxrNxr

NyyNxyyyyxyryryr

NyxNxxyxxxxrxrxr

NyrNxryrxrrrrrrr

NyrNxryrxrrrrrrr

NyrNxryrxrrrrrrr

Ny

Nx

y

x

r

r

r

llllllllllylx

llllllllllylx

llllllllllylx

llllllllllylx

llllyx

lylylylyyyyyx

lxlxlxlxxyxxx

l

l

l

l

y

x

t Nx

11

11

111111111

111111111

11

11

11

1

1 , ~

Page 12: Simultaneous Localization and Mapping

SLAM: particle filter

111 )(),|()|(),|( tttttttttt dxxpaxxpxopaoxp

Update equation:

Idea: Normal distribution assumption in Kalman filter is not necessary

A set of samples approximates the posterior distribution

and will be used at next iteration.

Each sample maintains its own map; or all samples maintain a single

map.

The map(s) is updated upon observation, assuming that the robot

location is given correctly.

),|( ttt aoxp

Page 13: Simultaneous Localization and Mapping

SLAM: particle filter

Assume it is difficult to sample directly from ),,|( 1 tttt aoxxp

But get samples from another distribution is easy.),,|( 1 tttt aoxxq

We sample from , with normalized weight for each xi

t as),,|( 1 tttt aoxxq

),,|(),,|(

1

1

ttit

it

ttit

iti

t aoxxqaoxxpw

The set of (particles) is an approximation of sNi

it

it wx 1},{

),,|( 1 tttt aoxxp

sN

i

itt

ittttt xxwaoxxp

11 )(),,|(

Particle filter:

Resample xt from ,with replacement, to get a sample set

with uniform weights

sN

i

itt

it xxw

1)(

sit Nw /1

sNi

itx 1}{

sNi

itx 1}{

Page 14: Simultaneous Localization and Mapping

SLAM: particle filter

Particle filter (cont’d):

),,|( 1 tttt aoxxp ),,|( 1 tttt aoxxq

Ns

i

itt

it xxw

1)(

0.2

0.3

0.4

0.1

7tx

10tx

1tx

4tx

5tx

6tx

},,,,,,,,,{ 7666655544tttttttttt xxxxxxxxxx

1.0itw

Page 15: Simultaneous Localization and Mapping

SLAM: particle filter

Choose appropriate

Choose ),|(),,|( 11 ttttttt axxpaoxxq

Then )|(),|(

),|()|(

1

1 itt

tit

it

tit

it

itti

t xopaxxp

axxpxopw

Transition probability

Observation function

),,|( 1 tttt aoxxq

Page 16: Simultaneous Localization and Mapping

SLAM: particle filter

Algorithm:

Let state xt represent the robot’s location, ),,( ,,, trtrtrt yxx

1. Propagate each state through the state transition probability itx 1

),|( 1 ttt axxp . This samples a new state given the previous state.itx

2. Weight each new state according to the observation function itx )|( i

tt xop

3. Normalize the weights, get .

4. Resampling: sample Ns new states from sNi

itx 1}{

sNi

itw 1}{

sN

i

itt

it xxw

1)(

sNi

itx 1}{ are the updated robot location from ),,|( 1 tttt aoxxp

Page 17: Simultaneous Localization and Mapping

SLAM: particle filter

State transition probability:

),0(1,, xxaxtrtr Nbxcxxt

),0(1,, yyaytrtr Nbycyyt

),0(1,, Nbctatrtr

are the expected robot moving distance (angle) by taking action at.),,(ttt aaa yx

Observation probability:

k

tkttt mxdpmxop ),|(),|(

),(),|( okmapt

kt dNmxdp

ktd Measured distance (observation) for sensor kkmapd Map distance from location xt to the obstacle

Page 18: Simultaneous Localization and Mapping

SLAM: particle filter

Lots of work on SLAM using particle filter are focused on: Reducing the cumulative error Fast SLAM (online) Way to organize the data structure (saving robot path and map).

Maintain single map: cumulative errorMultiple maps: memory and computation time

In Parr’s paper: Use ancestry tree to record particle history Each particle has its own map (multiple maps) Use observation tree for each grid square (cell) to record the map

corresponding to each particle. Update ancestry tree and observation tree at each iteration. Cell occupancy is represented by a probabilistic approach.

Page 19: Simultaneous Localization and Mapping

SLAM: particle filter

Page 20: Simultaneous Localization and Mapping

Searching problem

The agent doesn’t have map, doesn’t know the underlying model, doesn’t know where the target is.

Agent has 2 sensors:

Camera: tell agent “occupied” or “empty” cells in 4 orientations, noisy sensor.

Acoustic sensor: find the orientation of the target, effective only within certain distance.

Noisy observation, noisy action.

Assumption:

Page 21: Simultaneous Localization and Mapping

Searching problem

Differences from SLAM Rough map is enough; an accurate map is not necessary. Objective is to find the target. Robot need to actively select actions to

find the target as soon as possible.

Similar to SLAM To find the target, agent need build map and estimate its location.

Page 22: Simultaneous Localization and Mapping

Searching problem

Model:

Environment is represented by a rough grid; Each grid square (state) is either occupied or empty. The agent moves between the empty grid squares. Actions: walk to any one of the 4 directions, or “stay”. Could fail in

walking with certain probability. Observations: observe 4 orientations of its neighbor grid squares:

“occupied” or “empty”. Could make a wrong observation with certain probability.

State, action and observation are all discrete.

Page 23: Simultaneous Localization and Mapping

Searching problem

In each step, the agent updates its location and map:

Belief state: the agent believes which state it is currently in. It is a distribution over all the states in the current map.

The map: The agent thinks what the environment is .

For each state (grid square), a 2-dimentional Dirichlet distribution is used to represent the probability of “empty” and “occupied”.

The hyperparameters of Dirichlet distribution are updated based on current observation and belief state.

Page 24: Simultaneous Localization and Mapping

Searching problem

Belief state update:

)|()|(),|(),|()()(

aopasqpasqopoasqpsb tt

tt

),|( 1 asqsqp jtt

)|(

)(),|(),|()(

11

aop

sqpasqsqpasqopsNs

jtjtttj

)()1( sb t

where

)"e"(),,"e"|( 1 mptyspasqmptyssqp jtt

Probability of successful moving from sj to s when taking action a

From map representation

ns)orientatio 4(

),|(),|(j

tjt asqopasqopand

with ),|( asqop tj

)"o"()""|()"e"()""|( ccupiedspoccupiedsopmptyspemptysop jjjjjj

Neighbor of s in orientation j

the set of neighbor states of s

Page 25: Simultaneous Localization and Mapping

Searching problem

Map update (Dirichlet distribution):

Assume at step t-1, the hyperparameter for state S is )]( , (s)[)( )1()1()1( suus toccupied

tempty

t u

At step t, the hyperparameter for state S is updated as

)(

)()1( )()|"e"()()(sNs

jt

jttemptyempty

j

sbso,qmptyspsusu

)(

)()1()( )()|""()()(sNs

jt

jttoccupied

toccupied

j

sbso,qoccupiedspsusu

)|"e"( jt so,qmptysp )|"o"( jt so,qccupiedsp and are the posterior after observing o given that the agent is in the neighbor of state s. If the

probability of wrong observation for any orientation is p0, then

)"e"(),"e"|( )|"e"( mptyspsqmptysopso,qmptysp jtjt

priorp0 if o is “occupied”1-p0 if o is “empty”

)|"o"( jt so,qccupiedsp can be computed similarly.

Page 26: Simultaneous Localization and Mapping

Searching problem

Belief state update:

0

0 1 0

0

0.950.05

0.050.95

10

0.950.05

0.050.95

Map representation update:

0

0 0.001 0

0 0.001 0.079 0.918 0

0 0.001 0

0

0.0010.000

0.0020.000

1.0260.054

0.8730.046

0.0010.000

0.1260.954

1.9530.047

1.9430.054

0.8720.046

0.0010.001

0.0551.025

0.0460.872

0.0000.001

a=“right” a=“up”

0

0 0.001 0

0 0.000 0.211 0.745 0

0 0.000 0.001 0.002 0.033 0.009 0

0 0.000 0.000 0.000 0

0 0.000 0

0

0.0000.000

0.0010.000

0.2020.011

0.7080.037

0.0000.000

0.2030.011

1.9470.091

1.8490.058

0.7160.038

0.0000.000

0.0020.000

0.1280.954

2.1860.059

2.6930.092

0.9120.048

0.0090.001

0.0000.000

0.0020.001

0.0571.025

0.0780.874

0.0090.001

0.0000.000

0.0000.001

0.0000.000

0.0000.000

a=“right”a=“up”

occupied

empty

uu

Page 27: Simultaneous Localization and Mapping

Searching problem

Choose actions:

Each state is assigned a reward R(s) according to following rules:

t

t sbsR

)(1)( )(

)"e"()( mptyspsR

)()()( )( swindowsbsR t

Less explored grids have higher reward.

Try to walk to the “empty” grid square.

Consider neighbor of s with priority.

x x