Probabilistic Robotics
Download
Report
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:
Pmt[ij] | zt , xt
log
1 Pmt[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