Download
1 / 19
200 likes | 336 Views
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 z t = {r 1 ,r 2 … r 180 } Odometer reading a t = {d,theta}
E N D
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
prediction update Could use Bayes Filter
Recursive Bayes Filter normalizing constant action model sensor model
<x,y,θ>t <x’,y’,θ’>t+1 odometer reading New Positions:Generate X’ according to P(X’|X,a) p(X’|X,a) ~ X’ + N(0,Σd) + N(0, Σθ) at = (1, 45˚)
p(z|X’) = Sensor model • weight w’ by p(z|X’) 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 zt = (1, 1.41,…,3)
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 proposal target Sampling Key: represent belief states by set of weighted samples 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.
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 Prediction: • Now the particle set is represented by Update: Proposal distribution: Importance weights
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.
