Advanced Mobile Robotics
Dr. Jizhong Xiao
Department of Electrical Engineering
CUNY City College
jxiao@ccny.cuny.edu
City College of New York
1
Probabilistic Robotics
Probabilistic Sensor Models
Beam-based Model
Likelihood Fields Model
Feature-based Model
City College of New York
Bayes Filter Reminder
• Prediction (Action)
bel ( xt )   p ( xt | ut , xt 1 ) bel ( xt 1 ) dxt 1
• Correction (Measurement)
bel( xt )   p( zt | xt ) bel( xt )
City College of New York
Sensors for Mobile Robots
• Contact sensors: Bumpers
• Internal sensors
– Accelerometers (spring-mounted masses)
– Gyroscopes (spinning mass, laser light)
– Compasses, inclinometers (earth magnetic field, gravity)
• Proximity sensors
–
–
–
–
Sonar (time of flight)
Radar (phase and frequency)
Laser range-finders (triangulation, tof, phase)
Infrared (intensity)
• Visual sensors: Cameras
• Satellite-based sensors: GPS
City College of New York
4
Proximity Sensors
• The central task is to determine P(z|x), i.e., the
probability of a measurement z given that the robot is at
position x.
• Question: Where do the probabilities come from?
• Approach: Let’s try to explain a measurement.
City College of New York
5
Noise Issues
Sensors are imperfect!
Specular Reflection
City College of New York
6
Beam-based Sensor Model
• Scan z consists of K measurements.
z  {z1 , z2 ,...,zK }
A cyclic array of k ultrasound sensors
• Individual measurements are independent
given the robot position and the map of
environment.
K
P( z | x, m)   P( zk | x, m)
k 1
City College of New York
7
Beam-based Sensor Model
K
P( z | x, m)   P( zk | x, m)
k 1
City College of New York
8
Typical Measurement Errors of an Range Measurements
1. Beams reflected by
obstacles
2. Beams reflected by
persons / caused by
crosstalk
3. Random measurements
4. Maximum range
measurements
City College of New York
9
Proximity Measurement
• Measurement can be caused by …
–
–
–
–
a known obstacle.
cross-talk.
an unexpected obstacle (people, furniture, …).
missing all obstacles (total reflection, glass, …).
• Noise is due to uncertainty …
–
–
–
–
in measuring distance to known obstacle.
in position of known obstacles.
in position of additional obstacles.
whether obstacle is missed.
City College of New York
10
Beam-based Proximity Model
Measurement noise
Unexpected obstacles
Likelihood of sensing unexpected
objects decreased with range
zexp
0
Phit ( z | x, m)   hit
zmax
1
2
2
e
1 ( z  zexp )

2
2
0
2
zexp
  e  z
Punexp ( z | x, m)  
 0
Gaussian Distribution
zmax
z  z exp 

otherwise
Exponential Distribution
City College of New York
11
Beam-based Proximity Model
Random measurement
Max range/Failures
If fail to detect obstacle, report
maximum range
0
zexp
Prand ( z | x, m) 
zmax
1
z max
0
zexp
zmax
 1 if z  z max
Pmax ( z | x, m)  
0 otherwise
Uniform distribution
Point-mass distribution
City College of New York
12
Resulting Mixture Density
  hit 


 unexp 
P ( z | x, m)  
 max 


 
 rand 
T
 Phit ( z | x, m) 


 Punexp ( z | x, m) 

Pmax ( z | x, m) 


 P ( z | x, m) 
 rand

Weighted average, and  hit   un exp   max   rand  1
How can we determine the model parameters?
  { hit ,  un exp ,  max ,  rand ,  , }
System identification method: maximum likelihood estimator (ML estimator)
City College of New York
13
Raw Sensor Data
Measured distances for expected distance of 300 cm.
Sonar
Laser
City College of New York
14
Approximation
• The likelihood of the data Z is given by
P(z | X, m, Q)
  { hit ,  un exp ,  max ,  rand ,  , }
• Our goal is to identify intrinsic parameters Qthat
maximize this log likelihood
– Mathematic derivation can be found pp163~167
– Psudo-code can be found in pp160
• An iterative procedure for estimating parameters
• Deterministically compute the n-th parameter to
satisfy normalization constraint.
City College of New York
15
Approximation Results
Laser is more accurate than
sonar, narrower Gaussians
Laser
The smaller range, the more
accurate the measurement
Sonar
400cm
300cm
City College of New York
16
Example
z
P(z|x,m)
Laser scan, projected into a previously
acquired map
The likelihood, evaluated for all positions
and projected into the map. The darker a
position, the larger P(z|x,m)
City College of New York
17
Summary Beam-based Model
• Assumes independence between beams.
– Justification?
– Overconfident!
 Suboptimal result
• Models physical causes for measurements.
– Mixture of densities for these causes.
– Assumes independence between causes. Problem?
• Implementation
– Learn parameters based on real data.
– Different models should be learned for different angles at which
the sensor beam hits the obstacle.
– Determine expected distances by ray-tracing.
– Expected distances can be pre-processed.
City College of New York
18
Likelihood Fields Model
• Beam-based model is …
– not smooth for small obstacles and at edges
(small changes of a robot’s pose can have a
tremendous impact on the correct range of
sensor beam, heading direction is particularly
affected).
– not very efficient (computationally expensive
in ray casting)
• Idea: Instead of following along the beam,
just check the end point.
City College of New York
19
Likelihood Fields Model
• Project the end points of a sensor scan Zt into the
global coordinate space of the map
 x ztk

 y zk
 t
  x   cos
  
  y   sin 

cos(   ksens ) 
 sin   x ksens 
k
  z t 


cos  y ksens 
 sin(   ksens ) 
• Probability is a mixture of …
– a Gaussian distribution with mean at distance to closest
obstacle,
– a uniform distribution for random measurements, and
– a small uniform distribution for max range measurements.
p( ztk xt , m)  hit  phit   max  pmax   rand  prand
• Again, independence between different components is
20
assumed.
City College of New York
Likelihood Fields Model
 x ztk

 y zk
 t
  x   cos
  
  y   sin 

cos(   ksens ) 
 sin   x ksens 
k
  z t 


cos  y ksens 
 sin(   ksens ) 
Distance to the nearest obstacles. Max range reading ignored
City College of New York
21
Example
Example environment
P(z|x,m)
Likelihood field
The darker a location, the less likely
it is to perceive an obstacle
Sensor
probability
O1 O2
O3
Zmax
City College of New York
Oi : Nearest point to
obstacles
22
San Jose Tech Museum
Occupancy grid map
City College of New York
Likelihood field
23
Scan Matching
• Extract likelihood field from scan and use it
to match different scan.
Sensor scan, 180 dots
The darker a region, the smaller likelihood for sensing an object there
City College of New York
24
Properties of Likelihood Fields Model
• Highly efficient, uses 2D tables only.
• Smooth w.r.t. to small changes in robot position.
• Allows gradient descent, scan matching.
• Ignores physical properties of beams.
• Will it work for ultrasound sensors?
City College of New York
25
Additional Models of Proximity Sensors
• Map matching (sonar,laser): generate small,
local maps from sensor data and match local
maps against global model. (e.g., transform
scans into occupancy maps)
• Scan matching (laser): map is represented by
scan endpoints, match scan into this map. (e.g.
ICP algorithm to stitch scans together)
• Features (sonar, laser, vision): Extract features
such as doors, hallways from sensor data.
City College of New York
26
Features/Landmarks
• Active beacons (e.g., radio, GPS)
• Passive (e.g., visual, retro-reflective)
• Standard approach is triangulation
• Sensor provides
– distance, or
– bearing, or
– distance and bearing.
City College of New York
27
Distance and Bearing
City College of New York
28
Feature-Based Measurement Model
•
•
•
 rt1   rt 2 
 1  2
1
2
f ( zt )  { f t
f t }  { t  ,  t ,}
 s1   s 2 
 t  t 
Sensors that measure range, bearing, & a signature (a numerical value,
e.g., an average color)
Conditional independence between features
Feature vector is abstracted from the measurement:
p( f ( zt ) xt , m)   p(rti , ti , sti xt , m)
i
• Feature-Based map: m  {m1 , m2 , } with m j  {m j , x , m j , y , s j }
i.e., a location coordinate in global coordinates & a signature
• Robot pose:
i
 (m  x) 2  (m  y ) 2
 2 


r
t
j,x
j, y

 r 
T
 i
xt  {x y  }
 t    a tan 2(m j , y  y, m j , x  x)   )     2 


• Measurement model:
 s i  


s

j
 t 
  s2 
T

2
r
Zero-mean Gaussian error variables with standard deviations
City College of New York
r
29
Sensor Model with Known Correspondence
•
A key problem for range/bearing sensors: data association, (arise
when the landmarks cannot be uniquely identified)
• Introduce a correspondence variable cti {1,, N  1}, between feature
f t i and landmark m j . If cti  j  N , then the i-th feature observed at time t
corresponds to the j-th landmark in the map
Algorithm for calculating the probability of a landmark measurement:
i
inputs: feature f t i , with know correspondence ct  N , the robot pose
i i
and the map, Its output is the numerical probability p( f t ct , xt , m)
City College of New York
30
Summary of Sensor Models
•
•
Explicitly modeling uncertainty in sensing is key to robustness.
In many cases, good models can be found by the following approach:
1. Determine parametric model of noise free measurement.
2. Analyze sources of noise.
3. Add adequate noise to parameters (eventually mix in densities for
noise).
4. Learn (and verify) parameters by fitting model to data.
5. Likelihood of measurement is given by “probabilistically comparing” the
actual with the expected measurement.
•
•
This holds for motion models as well.
It is extremely important to be aware of the underlying assumptions!
City College of New York
31
Thanks
Homework 4, 5 posted
City College of New York
32