Graphical Models for Mobile Robot Localization

Shuang Wu

Global Localization In an occupancy map, estimate the

pose of the robot X = <x,y,θ>

(0,0)

θ

occupied

free

The inputs Laser readings zt = {r1,r2…r180} Odometer reading at = {d,theta} Prior sample set, Bel(X0) = <Xi,wi>, n

possible pose

Could use Bayes Filter

)|()( ...0 ttt dXPXBel

1 1 0 1 0

1 0

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

( | ,... )t t t t t t t

tt t

P z X a z z P X a z zBel X

P z a z

0.. 1 0( , ,..., )t t td a z z

1 0( ) ( | ) ( | , ,..., )t t t t t tBel X P z X P X a z z predictionupdate

1 1 1 1 0 1( ) ( | ) ( | , ) ( | , ... )t t t t t t t t t tBel X p z X p X X a p X a z z dX 1 1 1 1( ) ( | ) ( | , ) ( )t t t t t t t tBel X p z X p X X a Bel X dX

Recursive Bayes Filter

sensor model

action modelnormalizing constant

)(),|()|()( '11

'1

ntt

ntttt

nt XBelaXXPXzPCXBel

New Positions:Generate X’ according to P(X’|X,a)

<x,y,θ>todometer reading <x’,y’,θ’>t

+1

at = (1, 45˚) p(X’|X,a) ~ X’ + N(0,Σd) + N(0, Σθ)

)cos(*a x x’ )sin(*a y y’

Sensor model weight w’ by p(z|X’)

zt = (1, 1.41,…,3)

w,<x’,y’,θ’>t+1 p(z|X’)*w, <x’,y’,θ’>t+1

p(z|X’) = N(d,Σs), where Σs is the measure of noise in the laser readings d is the distance to the closest obstacle

p(z|X’) =

180

1

)'|(r

r Xzp

Sampling

Likelihood weighted sampling

Samples are drawn from the target distribution

Importance Sampling

Samples are drawn from a proposal distribution(g)

Re-weight samples to account for the difference between the proposal and target distribution

proposaltarget

Key: represent belief states by set of weighted samples

( )( )

( )

f xp x

g x

g 0, f 0

Resampling

Reason: Waste of CPU time if we keep propagating particles that have 0 weight.

Goal: Minimize variance of the importance weights

Rao-Blackwell Theorem Particle Filters approximate any distribution

independent of size of state space. The complexity of the standard PF algorithm is O(N).

Motivation: In very high-dimensional spaces, a large number of particles is needed to represent the posterior Solution: Reduce size of state space by marginalizing out some

of the variables.

]]|[[]|[[)( LRVarLRVarRVar ]]|[[)( LRVarRVar

Rao-Blackwellised

Step1. Divide the set of variables into sets R and L where R = set of sampled variables and L = remaining variables in the DBN at time t. Choice of RB variables: nodes that have no parents in the current

time slice or have parents that are already in R. Keep growing R until the remaining variables can be updated exactly.

Step2. Sample the set of variables Rt from Rt-1 using standard PF Step3. Compute the remain variables analytically.

RBPF Now the particle set is represented by

) , | ( ) , | ( ) , | (1 :1 1 :1 1 1 1 :1

1

: 1

it t t t

it t

X

it tz X P X X P z X P

t

t

),|(),|(),|( 1:11:1111:1

1

:1

itttt

itt

X

itt zYPYYPzYP

t

t

Prediction:

Update:

),|()|(),|( :11:1:1:1itttt

Yt

ittt zYPYzPzYP

),|()|(),|( :11:1:1:1itttt

Xt

ittt zXPXzPzXP

Proposal distribution:)|()|()|( 1:11:11:1:1 t

itttt

it zPPzq

),|()|(),|()|()|(),|( 1:11:11:11:1:11:1 ttttY

Yttttt

X

Xttttttt zYPYzPzXPXzPzPzzPw

tt

Importance weights

),|(),,|(, :1:1:1:1:1ittt

ittt

it zYPzXP

Implementation

Implementation Summary: • Used Rao-Blackwellised particle filtering to solve localization problem. • Able to localize within about 4cm (using a 20 x 20 x 10 resolution grid). • Implementation is written in Java. • Takes 16 seconds on my 2.4 GHz 512MB Windows XP machine for data set 1 (not counting loading data and ray tracing pre-computations). • Ray tracing takes 8 seconds, plus 14 seconds for pre-computing grid ranges.

Results(Implemented in Java)

Step 110

Step 565

Step 580

End of Forward Run

The End