Upload
perrin
View
92
Download
0
Tags:
Embed Size (px)
DESCRIPTION
Simultaneous Localization and Mapping. Presented by Lihan He Apr. 21, 2006. Outline. Introduction SLAM using Kalman filter SLAM using particle filter Particle filter SLAM by particle filter My work : searching problem. Introduction: SLAM. SLAM: Simultaneous Localization and Mapping. - PowerPoint PPT Presentation
Citation preview
Simultaneous Localization and Mapping
Presented by Lihan HeApr. 21, 2006
Outline
Introduction
SLAM using Kalman filter
SLAM using particle filter
Particle filter
SLAM by particle filter
My work : searching problem
Introduction: SLAM
SLAM: Simultaneous Localization and Mapping
A robot is exploring an unknown, static environment.
Given: The robot’s controls Observations of nearby features
Estimate: Location of the robot -- localization
where I am ? Detail map of the environment – mapping
What does the world look like?
The controls and observations are both noisy.
Introduction: SLAM
Markov assumption
x0 x1 x2 xt-1 xt…
o1 ot-1…
a1 at-1…
o2 ot
a2 at
m
State transition:
Observation function:
),|( 1 ttt axxp
)|( tt xop
Introduction: SLAM
Method: Sequentially estimate the probability distribution p(xt) and update the map.
Prior: p(x0)
p(x1)
p(m1) or m1
… p(xt-1)
p(mt-1) or mt-1
p(xt)
p(mt) or mt
111 )(),|()|(),|( tttttttttt dxxpaxxpxopaoxp
Prior distribution on xt after taking action at
)|( tt axp
Introduction: SLAM
Map representations
1. Landmark-based map representation
2. Grid-based map representation
Track the positions of a fixed number of predetermined sparse landmarks.
Observation: estimated distance from each landmark.
Map is represented by a fine spatial grid, each grid square is either occupied or empty.
Observation: estimated distance from an obstacle using a laser range finder.
Introduction: SLAM
Methods:
1. Parametric method – Kalman filter
2. Sample-based method – particle filter
The robot’s trajectory estimate is a tracking problem
Represent the distribution of robot location xt (and map mt ) by a Normal distribution ),( ttN
Sequentially update μt and Σt
Represent the distribution of robot location xt (and map mt) by a large amount of simulated samples.
Resample xt (and mt) at each time step
Introduction: SLAM
Why is SLAM a hard problem?
Robot location and map are both unknown.
Location error
Map error
The small error will quickly accumulated over time steps.
The errors come from inaccurate measurement of actual robot motion (noisy action) and the distance from obstacle/landmark (noisy observation).
When the robot closes a physical loop in the environment, serious misalignment errors could happen.
SLAM: Kalman filter
111 )(),|()|(),|( tttttttttt dxxpaxxpxopaoxp
Update equation:
Assume:Prior p(x0) is a normal distribution
Observation function p(o|x) is a normal distribution
Then:Posterior p(x1), …, p(xt) are all normally distributed.
Mean μt and covariance matrix Σt can be derived analytically.
Sequentially update μt and Σt for each time step t
),(~),|( ttttt Naoxp
SLAM: Kalman filter
1111 )(),|()|(),|( tttttttttt dxxpaxxpxopaoxp
Assume:
),0(~ ,1 tat
atttttt RNaGxFx
),0(~ , tot
otttt QNxHo
),(),|( 11 tttttttt RaGxFNaxxp State transition
Observation function ),()|( ttttt QxHNxop
Kalman filter:
Propagation (motion model): ),()|( tttt Naxp
,1 ttttt aGF tTtttt RFF 1
Update (sensor model): ),(),|( ttttt Naoxp 1)( t
Tttt
Tttt QHHHK
),( tttttt HoK tttt HKI )(
),,,(),( 11 tttttt oaKalman
SLAM: Kalman filter
The hidden state for landmark-based SLAM:
TNyNxyxtrtrtrt llllyxx ],,...,,,,,[ 11,,,
localization mapping
Map with N landmarks: (3+2N)-dimentional Gaussian
State vector xt can be grown as new landmarks are discovered.
NyNyNyNxNyyNyxNyrNyrNyr
NyNxNxNxNxyNxxNxrNxrNxr
NyyNxyyyyxyryryr
NyxNxxyxxxxrxrxr
NyrNxryrxrrrrrrr
NyrNxryrxrrrrrrr
NyrNxryrxrrrrrrr
Ny
Nx
y
x
r
r
r
llllllllllylx
llllllllllylx
llllllllllylx
llllllllllylx
llllyx
lylylylyyyyyx
lxlxlxlxxyxxx
l
l
l
l
y
x
t Nx
11
11
111111111
111111111
11
11
11
1
1 , ~
SLAM: particle filter
111 )(),|()|(),|( tttttttttt dxxpaxxpxopaoxp
Update equation:
Idea: Normal distribution assumption in Kalman filter is not necessary
A set of samples approximates the posterior distribution
and will be used at next iteration.
Each sample maintains its own map; or all samples maintain a single
map.
The map(s) is updated upon observation, assuming that the robot
location is given correctly.
),|( ttt aoxp
SLAM: particle filter
Assume it is difficult to sample directly from ),,|( 1 tttt aoxxp
But get samples from another distribution is easy.),,|( 1 tttt aoxxq
We sample from , with normalized weight for each xi
t as),,|( 1 tttt aoxxq
),,|(),,|(
1
1
ttit
it
ttit
iti
t aoxxqaoxxpw
The set of (particles) is an approximation of sNi
it
it wx 1},{
),,|( 1 tttt aoxxp
sN
i
itt
ittttt xxwaoxxp
11 )(),,|(
Particle filter:
Resample xt from ,with replacement, to get a sample set
with uniform weights
sN
i
itt
it xxw
1)(
sit Nw /1
sNi
itx 1}{
sNi
itx 1}{
SLAM: particle filter
Particle filter (cont’d):
),,|( 1 tttt aoxxp ),,|( 1 tttt aoxxq
Ns
i
itt
it xxw
1)(
0.2
0.3
0.4
0.1
7tx
10tx
1tx
4tx
5tx
6tx
},,,,,,,,,{ 7666655544tttttttttt xxxxxxxxxx
1.0itw
SLAM: particle filter
Choose appropriate
Choose ),|(),,|( 11 ttttttt axxpaoxxq
Then )|(),|(
),|()|(
1
1 itt
tit
it
tit
it
itti
t xopaxxp
axxpxopw
Transition probability
Observation function
),,|( 1 tttt aoxxq
SLAM: particle filter
Algorithm:
Let state xt represent the robot’s location, ),,( ,,, trtrtrt yxx
1. Propagate each state through the state transition probability itx 1
),|( 1 ttt axxp . This samples a new state given the previous state.itx
2. Weight each new state according to the observation function itx )|( i
tt xop
3. Normalize the weights, get .
4. Resampling: sample Ns new states from sNi
itx 1}{
sNi
itw 1}{
sN
i
itt
it xxw
1)(
sNi
itx 1}{ are the updated robot location from ),,|( 1 tttt aoxxp
SLAM: particle filter
State transition probability:
),0(1,, xxaxtrtr Nbxcxxt
),0(1,, yyaytrtr Nbycyyt
),0(1,, Nbctatrtr
are the expected robot moving distance (angle) by taking action at.),,(ttt aaa yx
Observation probability:
k
tkttt mxdpmxop ),|(),|(
),(),|( okmapt
kt dNmxdp
ktd Measured distance (observation) for sensor kkmapd Map distance from location xt to the obstacle
SLAM: particle filter
Lots of work on SLAM using particle filter are focused on: Reducing the cumulative error Fast SLAM (online) Way to organize the data structure (saving robot path and map).
Maintain single map: cumulative errorMultiple maps: memory and computation time
In Parr’s paper: Use ancestry tree to record particle history Each particle has its own map (multiple maps) Use observation tree for each grid square (cell) to record the map
corresponding to each particle. Update ancestry tree and observation tree at each iteration. Cell occupancy is represented by a probabilistic approach.
SLAM: particle filter
Searching problem
The agent doesn’t have map, doesn’t know the underlying model, doesn’t know where the target is.
Agent has 2 sensors:
Camera: tell agent “occupied” or “empty” cells in 4 orientations, noisy sensor.
Acoustic sensor: find the orientation of the target, effective only within certain distance.
Noisy observation, noisy action.
Assumption:
Searching problem
Differences from SLAM Rough map is enough; an accurate map is not necessary. Objective is to find the target. Robot need to actively select actions to
find the target as soon as possible.
Similar to SLAM To find the target, agent need build map and estimate its location.
Searching problem
Model:
Environment is represented by a rough grid; Each grid square (state) is either occupied or empty. The agent moves between the empty grid squares. Actions: walk to any one of the 4 directions, or “stay”. Could fail in
walking with certain probability. Observations: observe 4 orientations of its neighbor grid squares:
“occupied” or “empty”. Could make a wrong observation with certain probability.
State, action and observation are all discrete.
Searching problem
In each step, the agent updates its location and map:
Belief state: the agent believes which state it is currently in. It is a distribution over all the states in the current map.
The map: The agent thinks what the environment is .
For each state (grid square), a 2-dimentional Dirichlet distribution is used to represent the probability of “empty” and “occupied”.
The hyperparameters of Dirichlet distribution are updated based on current observation and belief state.
Searching problem
Belief state update:
)|()|(),|(),|()()(
aopasqpasqopoasqpsb tt
tt
),|( 1 asqsqp jtt
)|(
)(),|(),|()(
11
aop
sqpasqsqpasqopsNs
jtjtttj
)()1( sb t
where
)"e"(),,"e"|( 1 mptyspasqmptyssqp jtt
Probability of successful moving from sj to s when taking action a
From map representation
ns)orientatio 4(
),|(),|(j
tjt asqopasqopand
with ),|( asqop tj
)"o"()""|()"e"()""|( ccupiedspoccupiedsopmptyspemptysop jjjjjj
Neighbor of s in orientation j
the set of neighbor states of s
Searching problem
Map update (Dirichlet distribution):
Assume at step t-1, the hyperparameter for state S is )]( , (s)[)( )1()1()1( suus toccupied
tempty
t u
At step t, the hyperparameter for state S is updated as
)(
)()1( )()|"e"()()(sNs
jt
jttemptyempty
j
sbso,qmptyspsusu
)(
)()1()( )()|""()()(sNs
jt
jttoccupied
toccupied
j
sbso,qoccupiedspsusu
)|"e"( jt so,qmptysp )|"o"( jt so,qccupiedsp and are the posterior after observing o given that the agent is in the neighbor of state s. If the
probability of wrong observation for any orientation is p0, then
)"e"(),"e"|( )|"e"( mptyspsqmptysopso,qmptysp jtjt
priorp0 if o is “occupied”1-p0 if o is “empty”
)|"o"( jt so,qccupiedsp can be computed similarly.
Searching problem
Belief state update:
0
0 1 0
0
0.950.05
0.050.95
10
0.950.05
0.050.95
Map representation update:
0
0 0.001 0
0 0.001 0.079 0.918 0
0 0.001 0
0
0.0010.000
0.0020.000
1.0260.054
0.8730.046
0.0010.000
0.1260.954
1.9530.047
1.9430.054
0.8720.046
0.0010.001
0.0551.025
0.0460.872
0.0000.001
a=“right” a=“up”
0
0 0.001 0
0 0.000 0.211 0.745 0
0 0.000 0.001 0.002 0.033 0.009 0
0 0.000 0.000 0.000 0
0 0.000 0
0
0.0000.000
0.0010.000
0.2020.011
0.7080.037
0.0000.000
0.2030.011
1.9470.091
1.8490.058
0.7160.038
0.0000.000
0.0020.000
0.1280.954
2.1860.059
2.6930.092
0.9120.048
0.0090.001
0.0000.000
0.0020.001
0.0571.025
0.0780.874
0.0090.001
0.0000.000
0.0000.001
0.0000.000
0.0000.000
a=“right”a=“up”
occupied
empty
uu
Searching problem
Choose actions:
Each state is assigned a reward R(s) according to following rules:
t
t sbsR
)(1)( )(
)"e"()( mptyspsR
)()()( )( swindowsbsR t
Less explored grids have higher reward.
Try to walk to the “empty” grid square.
Consider neighbor of s with priority.
x x