Transcript Probabilistic Robotics

Mapping with Known Poses*
CS5391 – AI Robotics
Based on the slides from the
book's website
SA-1
1
Agenda…
• Introduction to mapping:
• Motivate SLAM.
• Mapping with known poses.
• Some examples.
• Simple counting-based mapping.
• MAP estimation.
2
Why Mapping?
• Maps are a fundamental requirement:
• Provides a frame of reference to humans and robots!
• Maps used for localization, path-planning, activityplanning, active-sensing…
• Autonomous behavior requires Simultaneous
Localization And Mapping (SLAM).
3
The General Problem of Mapping
What does the
environment look like?
4
Mathematical Formulation
• Formally, given the sensor data:
d  {u1:t , z1:t }
mapping involves finding the most likely map
(mode):
*
m  arg max p(m | z1:t , u1:t )
m
• Finding full posterior is easier with independence
assumption:
Bel(m)  p(m | z1:t , u1:t )
5
Mapping is a Chicken and Egg Problem
• Localization: estimate robot pose given sensor
data and map.
• Goal: Autonomous mobile robots that require
little/no human supervision.
• Challenge: simultaneously estimate robot pose
and the map (SLAM).
• Bootstrap off of localization and map-building with known
pose.
6
Problems in Mapping
• Noise in sensing and actuation:
• Extract information from noisy sensory data?
• Model and account for motion error accumulation?
• Ambiguity in perception:
• Establish correspondence between sensor readings?
• Data association:
• Identify that robot is at a previously visited place?
• Close the loop?
• Large continuous search space:
• Binary map with N grid-cells represents 2N maps!!
7
Types of SLAM-Problems
• Grid maps or scans:
[Lu & Milios, 97; Gutmann, 98: Thrun 98; Burgard, 99; Konolige & Gutmann, 00; Thrun, 00; Arras,
99; Haehnel, 01;…]
• Landmark-based:
[Leonard et al., 98; Castelanos et al., 99: Dissanayake et al., 2001; Montemerlo et al., 2002;…
8
Occupancy Grid Maps
(Moravec and Elfes, 1985)
• Environment a collection of grid cells.
• Estimate the probability that a cell is occupied.
• Key assumptions:
• Occupancy of individual cells (m[ij]) is independent!
Bel(mt )  p (mt | z1:t , u1:t )
  Bel(mt[ ij ] )
i, j
• Robot poses are known!
• Post-processing tool…
Bel(m )  p(m
[ij ]
t
[ij ]
t
| z1:t , x1:t )
9
Updating Occupancy Grid Maps
• Idea: Update each individual cell within field-ofview using a binary Bayes filter.
• Additional assumption: Map is static i.e. control
commands can be neglected!!
Bel(mt[ij] )  p(mt[ij] | z1:t , x1:t )
10
Binary Bayes Filter (revisited)…
• Elegant, avoids truncation errors…
• Chapter 4: Table 4.2 in textbook.
p ( x | zt )
p ( x)
lt  lt 1  log
 log
1  p ( x | zt )
1  p( x)
p( x)
1
l0 ( x)  log
 log odds( x), Belt ( x)  1 
1  p ( x)
1  exp(lt )
p(mi | z1:t , x1:t )
p(mi )
lt ,i  log
, l0  log
1  p(mi | z1:t , x1:t )
1  p(mi )
1
p(mi | z1:t , x1:t )  1 
1  exp(lt ,i )
11
Updating Occupancy Grid Maps
• Use the log-odds representation.
 
B mt[ij ]  B (mt[ij1] )  inv_sensor_model(mt[ij ] , zt , xt )  B (m0[ij ] )




 
 log odds mt[ij1] | z1:t 1 , x1:t 1  log odds mt[ij ] | zt , xt  log odds m0[ij ]
 
B mt[ij] : logodds(mt[ij] )
• Cross-check with equation 4.19, Chapter 4…




P m | z ,x
1 P m
Bel mt[ ij ]  1  1 

Pm
 1 P m | z ,x



[ ij ]
t
t
t
[ ij ]
t
t
t


[ ij ]
t
[ ij ]
t



Bel m
1  Bel m


[ ij ]
t 1
[ ij ]
t 1




1
12
Updating Occupancy Grid Maps
• Update the map cells using the inverse sensor
model:


P mt[ij] | zt , xt
log
1  P mt[ij] | zt , xt


• Log-odds of occupancy of grid cell, given the
current measurement and known pose.
• Information about the world conditioned on
measurements caused by the world.
• Table 9.2 for an ad-hoc example (typo!).
• Task: Section 9.3 for principled functionapproximation.
13
Adhoc Inverse Sensor Model
• Incorporate factors involved in the computation of:
Pmt[ij] | zt , xt 
log
1  Pmt[ij] | zt , xt 
• Factors:
• Relative (d , ) of robot from cell’s center-of-mass.
• Width of obstacle and sensor beams.
• Cells outside field-of-view:
l0
• Cells within suitable range: locc
• Other cells: l free
l free  l0  locc
14
Function Approx. Inv. Sensor Model
• Sampling-based approach.
• Approach:
• Sample a map from feasible maps:
• Sample a robot pose:
m[ k ] ~ p(m)
x[k ]
• Sample a measurement:
z [k ]
• Get ground-truth occupancy value from map: occ(m
[ij ][ k ]
)
• Learn predictor that minimizes error over data samples.
 x[ k ] , z[ k ]   occ(m[ij][ k ] ), k  [1, N ]
• Use relative pose, model sensor characteristics.
15
Incremental Updating of Occupancy
Grids (Example)
16
Recap…
• Goal: simultaneously localize and mapping (SLAM).
m*  arg max p(m | z1:t , u1:t )
m
• Occupancy Grid mapping: assume known pose and
independence:
Bel(mt )  p (mt | z1:t , u1:t )
Bel(mt )   Bel(mt[ij ] )   p (mt[ ij ] | z1:t , x1:t )
i, j
i, j
• Binary Bayes filter and inverse sensor models for updating
individual cells.
• Adhoc and Sampling-based approaches for the inverse
sensor model.
[ ij ]
log


P mt | zt , xt
1  P mt[ij] | zt , xt


17
Resulting Map Obtained with Ultrasound
Sensors
18
Resulting Occupancy and Maximum
Likelihood Map
The maximum likelihood map is obtained by clipping the
occupancy grid map at a threshold of 0.5
19
Occupancy Grids: From scans to maps
20
Tech Museum, San Jose
CAD map
occupancy grid map
21
Agenda…
• Introduction to mapping:
• Motivate SLAM.
• Mapping with known poses.
• Some examples.
• Simple counting-based mapping.
• MAP estimation.
22
Alternative: Simple Counting
• For every cell count:
• hits(i, j): no. of times a beam ended at <i, j>.
• misses(i, j): no. of times a beam passed through <i, j>
hits(i, j )
Bel(m ) 
hits(i, j )  misses(i, j )
[ ij ]
• Value of interest: P(reflects(i, j))
• Count how often a cell has reflected a beam.
23
The Measurement Model
1. Pose at time t:
xt
2. Beam n of scan t:
zt , n
3. Beam not reflected:
4. Beam reflected by an object:
 t ,n  1
 t ,n  0
0 1

if  t ,n  1
  (1  m f ( xt ,n,k ) )

p( zt ,n | xt , m)   k 0
zt ,n 1
m
if  t ,n  0
f ( xt , n , zt ,n )  (1  m f ( xt , n , k ) )

k 0

zt ,n 1
n
m f ( xt ,n, zt ,n )
24
Maximum A Posteriori (MAP) Mapping
• Relax independence assumption:
Bel(mt )  p (mt | z1:t , u1:t )
  Bel(mt[ ij ] )
i, j
• Output mode of posterior instead of full posterior:
m  arg max log p(m | z1:t , x1:t )
*
m
• Use measurement models instead of inverse
models.
25
Computing the Most Likely Map
• Compute value for m that maximizes:
*
m  arg max log p(m | z1:t , x1:t )
m
• Assuming a uniform prior probability for p(m), this
is equivalent to maximizing (Section 9.4.2):
m*  arg max log p( z1:t | m, x1:t )  log p(m)
m
 arg max log p( zt | m, xt )  l0  mi
m
t
i
• Algorithm in Table 9.3 – one more task!
26
Difference between Occupancy Grid
Maps and Counting
• The counting model (with MAP) determines how
often a cell reflects a beam:
• No inverse sensor model 
• Store all data i.e. incremental updates not possible 
• The occupancy model represents whether or not a
cell is occupied by an object.
• Incremental updates possible 
• Inverse sensor models, independence assumption 
• Although a cell might be occupied by an object, it
says nothing about the reflection probability.
27
Example Occupancy Map
28
Example Reflection Map
glass panes
29
Summary
• Occupancy grid maps are a popular approach to represent
the environment of a mobile robot given known poses.
• It stores the posterior probability that the corresponding area
in the environment is occupied.
• Occupancy grid maps can be learned efficiently considering
each cell independently from all others.
• Reflection maps are an alternative representation, each cell
stores the probability that a beam is reflected by this cell.
• Reflection maps are more optimal.
• MAP approach relaxes independence constraint.
• Sensor model for computing the likelihood of measurements.
30