Upload
ely
View
50
Download
1
Embed Size (px)
DESCRIPTION
Graphical Models for Mobile Robot Localization. Shuang Wu. Global Localization. In an occupancy map, estimate the pose of the robot X = . (0,0). occupied. free. θ. The inputs. Laser readings z t = {r 1 ,r 2 … r 180 } Odometer reading a t = {d,theta} - PowerPoint PPT Presentation
Citation preview
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