11/5/15
Robot Localization:
State Estimation and Particle Filters
Manuela M. Veloso
15-381 Fall 2015
CoBot(s) Collaborative Robot(s)
[Link]/~coral/cobot
Joydeep Biswas, Brian Coltin, Stephanie Rosenthal, Mehdi Samadi, Thomas
Kollar, Cetin Mericli, Juan Pablo Mendoza, Yichao Sun, Vittorio Perera
Tasks
Go
to
a
loca-on
Deliver
a
message
Escort
a
visitor
Transport
object
between
loca-ons
Semi-autonomous
telepresence
Visitor
companion
Robots designed and built
by Michael Licitra
11/5/15
Go-To-Room Task Example
Robot Localization
Determine the robots pose (x, y, and theta) in an
environment from sensing
Challenges:
Noisy or incorrect sensing
Odometry model
Common solution: particle filters
Maintain multiple hypotheses (particles) with
likelihoods (weights)
Must limit number of hypotheses based on
computational requirements, cannot cover entire
configuration space
4
11/5/15
Robots Position Localization
- Apriori: motion model, map
- Given: actual motion, sensing
- Compute: probabilistic
distribution of position belief
- Method: Bayesian update
Standard Monte Carlo Localization
Maintain robot poses pi and weights wi
Three steps (Dellaert et al. 1999):
Predict Step. Move the particle poses based on the
motion model of the robot, with motion noise.
Update Step. Multiply particle weights wi by p(s|li), or the
likelihood of making the observed sensor readings given
that the robot is in pose pi. p(s|l) is referred to as the
sensor model.
Resample Step. Choose new particles probabilistically by
sampling from old particle weights.
Inexpensively model multi-modal distributions, but
cannot cover entire space
6
11/5/15
Sequential State Estimation
Sensor model p (s|x)
Dynamics model p(xt+1 | xt, ut)
Compute p(xt+1 | xt, ut, st+1)
Dynamic Bayes Network
Particle filters
See Russell & Norvig, chapter 15
Example Particle Filter
Two states: Raining
and Not Raining
For large N, the
number of samples
occupying a state xt
after observations
e1:t is
11/5/15
Example Particle Filter
R0
P(R1)
0.7
0.2
Use Model
Example Particle Filter
R0
P(R1)
0.7
0.2
Use Model
11/5/15
Example Particle Filter
Observe
umbrella
We weight each sample by the likelihood for the
observation at time t+1.
Example Particle Filter
Resample: replicate each sample with probability
proportional to its weight.
11/5/15
Example Particle Filter
The number of samples after resampling is
proportional to the total weight in xt+1 before
resampling.
The Autonomous Localization Problem
Prior: Map
Given: History of observations, robot actions
Estimate: Pose (x,y,) of robot
Autonomous robot: pose belief run-time update
Predict, based on robot odometry
Update, based on perceived sensed signals
Constrain, based on map properties
11/5/15
The Multi-Floor Building Map
Vector-Based Map Representations
11/5/15
Effective Autonomous Localization
Kinect RGB (not used) + Depth Sensing
Sensing and Map
Need to match sensing to map
11/5/15
Fast Sampling Plane Filtering (FSPF)
Biswas & Veloso, ICRA2011
Based on RANSAC
Output:
A set of plane filtered
points
A set of associated
normals
A set of rejected
(outlier) points
Fast Sampling Plane Filtering (FSPF)
J. Biswas & [Link], ICRA2011
10
11/5/15
Fast Sampling Plane Filtering (FSPF)
J. Biswas & [Link], ICRA2011
Fast Sampling Plane Filtering (FSPF)
J. Biswas & [Link], ICRA2011
11
11/5/15
Fast Sampling Plane Filtering (FSPF)
J. Biswas & [Link], ICRA2011
Fast Sampling Plane Filtering (FSPF)
J. Biswas & [Link], ICRA2011
12
11/5/15
Fast Sampling Plane Filtering (FSPF)
J. Biswas & [Link], ICRA2011
Fast Sampling Plane Filtering (FSPF)
J. Biswas & [Link], ICRA2011
13
11/5/15
Raw 3D point cloud, color coded by
depth from camera
FSPF generated plane filtered point
cloud, color coded by depth from
camera
Green arrows show plane normals
Depth-Based Localization
Blue lines: 2D vector map
Orange circles: particles (robot pose
estimates)
Red dots: plane filtered points
Good localization => good overlap between
plane filtered points and map
White points: raw 3D
points
Colored points: plane
filtered points
Green arrows: normals
14
11/5/15
Summary
Particle filters for sequential state estimation
Depth-camera sensing
Russell and Norvig chapter 15
J. Biswas & M. Veloso FSPF; Episodic NonMarkov Localization
[Link] et al. CoBots, IJCAI 2015
Next class Symbiotic autonomy and Multi-robot
systems
15
