Магистерский проект
Simultaneous localization
and mapping
Магистрант:
Матрунич
Сергей Анатольевич
Научный руководитель:
Зимянин Л.Ф.
2008
Краткое содержание
 Введение
 SLAM используя фильтр Калмана
 SLAM используя фильтр для частиц
 Исследование : проблема поиска
Введение: SLAM
SLAM: Simultaneous Localization and Mapping
Робот изучает незнакомое, статическое окружение.
Дано:
 Система управления роботом
 Визуальные датчики
Оба источника данных зашумлены.
Оценка:
 Положения робота -- localization
где Я ?
 Детализация окружающего пространства – mapping
На что похоже то что вокруг меня?
Введение: SLAM
Допущение Маркова
a1
x0
…
a2
at
at-1
x1
x2
…
xt-1
xt
o1
o2
…
ot-1
ot
m
State transition:
p( xt | xt 1 , at )
Observation function:
p(ot | xt )
Введение: 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(xt)
p(mt-1) or mt-1
p(mt) or mt
p( xt | ot , at )  p(ot |xt ) p( xt | xt 1 , at ) p( xt 1 )dxt 1
p( xt | at )
Prior distribution on xt after taking action at
Введение: SLAM
Методы:
The robot’s trajectory estimate is a tracking problem
1. Parametric method – Kalman filter
Represent the distribution of robot location xt (and map mt ) by a Normal
distribution N (t , t )
Sequentially update μt and Σt
2. Sample-based method – particle filter
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
Введение: SLAM
Почему SLAM трудная задача?
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: фильтр Калмана
Корректирующее уровнение:
p( xt | ot , at )  p(ot |xt ) p( xt | xt 1 , at ) p( xt 1 )dxt 1
Предположение:
Prior p(x0) is a normal distribution
Observation function p(o|x) is a normal distribution
Тогда:
Posterior p(x1), …, p(xt) are all normally distributed.
p( xt | ot , at ) ~ N (t , t )
Mean μt and covariance matrix Σt can be derived analytically.
Sequentially update μt and Σt for each time step t
SLAM: фильтр Калмана
Assume:
xt  Ft xt 1  Gt at   ta ,  ta ~ N (0, Rt )
State transition
p( xt | xt 1 , at )  N ( Ft xt 1  Gt at , Rt )
ot  H t xt   to ,  to ~ N (0, Qt )
Observation function
p(ot | xt )  N ( H t xt , Qt )
Kalman filter: ( t , t )  Kalman( t 1 , t 1 , at , ot )
Propagation (motion model):
t  Ft t 1  Gt at ,
p( xt | at )  N (  t , t )
t  Ft t 1Ft  Rt
Update (sensor model):
T
p( xt | ot , at )  N (t , t )
Kt  t Ht ( Ht t Ht  Qt )1
T
T
t  t  Kt (ot  H t t ),
 t  ( I  K t H t ) t
p( xt | ot , at )  p(ot |xt ) p( xt | xt 1 , at 1 ) p( xt 1 )dxt 1
SLAM: фильтр Калмана
The hidden state for landmark-based SLAM:
xt  [ xr ,t , yr ,t , r ,t , l1x , l1y ,..., lNx , lNy ]T
localization
   xr 
  xr xr



   yr 
  xr y r
  

  r 
 xr r
  l 
 x l
xt ~ N   1 x  ,  r 1 x
   l1 y 
  xr l1 y
  
 



   l Nx 
  xr l Nx
  
 x l
 r Ny
  l Ny 
mapping
x y
y y
y
yl
yl
x
y
 
 l
 l


yl
yl
 l
 l
r r
r r
r r
r 1x
r 1y
r Nx
r Ny
r r
r r
r r
r 1x
r 1y
r Nx
r Ny
x l
yl
 l
l l
l l
x l
yl
 l
l l
l l
1y 1y
  l1 y l Nx



1 y l Nx
  l Nxl Nx
r 1x
r 1x
r 1x
1x 1x
1x 1 y
l
l
1 x l Nx
1 x l Ny
r 1y
r 1y
r 1y
1x 1 y
l
l
1 y l Ny
  xr l Nx
  yr l Nx
   r l Nx
  l1 xl Nx

  l Nxl Ny
x l
yl
 l
l l
l l



r Ny

r Ny  

1 x Ny

1 y Ny  
 

 l Nxl Ny  
 l Nyl Ny  

r Ny
Map with N landmarks: (3+2N)-dimentional Gaussian
State vector xt can be grown as new landmarks are discovered.
SLAM: фильтр для частиц
Update equation:
p( xt | ot , at )  p(ot |xt ) p( xt | xt 1 , at ) p( xt 1 )dxt 1
Основная идея:

Normal distribution assumption in Kalman filter is not necessary

A set of samples approximates the posterior distribution p( xt | ot , at )
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.
SLAM: фильтр для частиц
Particle filter:
i Ns
Assume it is difficult to sample { xt }i 1 directly from
But get samples from another distribution
i N
We sample { xt }i s1 from q( xt
for each xit as
p( xt | xt 1 , ot , at )
q( xt | xt 1 , ot , at )
is easy.
| xt 1 , ot , at ) , with normalized weight
p( xti | xti1 , ot , at )
w 
q( xti | xti1 , ot , at )
i
t
The set of {xti , wti }iNs1 (particles) is an approximation of
p( xt | xt 1 , ot , at )
Ns
p( xt | xt 1 , ot , at )   wti ( xt  xti )
i 1
Resample xt from
Ns
i
i
 wt  ( xt  xt )
i 1
with uniform weights wi  1 / N
t
s
,with replacement, to get a sample set
SLAM: фильтр для частиц
Particle filter (cont’d):
0.4
0.3
0.2
0.1
p( xt | xt 1 , ot , at )
q( xt | xt 1 , ot , at )
xt1
xt4 xt5 xt6 xt7
Ns
i
i
 wt  ( xt  xt )
i 1
{xt4 , xt4 , xt5 , xt5 , xt5 , xt6 , xt6 , xt6 , xt6 , xt7 }
wti  0.1
xt10
SLAM: фильтр для частиц
Choose appropriate
q( xt | xt 1 , ot , at )
q( xt | xt 1 , ot , at )  p( xt | xt 1 , at )
Choose
p(ot | xti ) p( xti | xti1 , at )
i
Then w 

p
(
o
|
x
t
t)
i
i
p( xt | xt 1 , at )
i
t
Transition probability
Observation function
SLAM: фильтр для частиц
Алгоритм:
Let state xt represent the robot’s location, xt  ( xr ,t , yr ,t , r ,t )
i
1. Propagate each state xt 1 through the state transition probability
p( xt | xt 1 , at ) . This samples a new state xti
given the previous state.
2. Weight each new state xti according to the observation function
p(ot | xti )
3. Normalize the weights, get {wti }iNs1 .
i Ns
t i 1
4. Resampling: sample Ns new states { x }
{ xti }iNs1
Ns
from
are the updated robot location from
i
i
 wt  ( xt  xt )
i 1
p( xt | xt 1 , ot , at )
SLAM: фильтр для частиц
State transition probability:
xr ,t  xr ,t 1  cx xat  bx  N (0,  x )
yr ,t  yr ,t 1  c y yat  by  N (0,  y )
 r ,t   r ,t 1  c a  b  N (0,   )
t
( xat , yat , at ) are the expected robot moving distance (angle) by taking action at.
Observation probability:
p (ot | xt , m)   p (d tk | xt , m)
k
k
p(dtk | xt , m)  N (dmap
, o )
d tk Measured distance (observation) for sensor k
k
d map
Map distance from location xt to the obstacle
SLAM: фильтр для частиц
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 error
Multiple 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: фильтр для частиц
Проблема поиска
Assumption:
 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.
Проблема поиска
Similar to SLAM

To find the target, agent need build map and estimate its location.
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.
Проблема поиска
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.
Проблема поиска
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.
Проблема поиска
Belief state update:
p(o | qt  s, a) p(qt  s | a)
b ( s)  p(qt  s | a, o) 
p (o | a )
(t )

where
b(t1) (s)
p(o | qt  s, a)  p(qt  s | qt 1  s j , a) p(qt 1  s j )
s j N ( s )
p (o | a )
p(qt  s | qt 1  s j , a)
the set of neighbor states of s
 p(qt  s | s " empty" , qt 1  s j , a) p( s " empty" )
Probability of successful moving
from sj to s when taking action a
and
with
p(o | qt  s, a) 

j  ( 4 orientations)
From map
representation
p(o j | qt  s, a)
p (o j | qt  s, a )
 p(o j | s j " empty" ) p( s j " empty" )  p(o j | s j " occupied " ) p( s j " occupied " )
Neighbor of s in orientation j
Проблема поиска
Обновление карты (распределение Дирихле):
Предпологаем на шаге t-1, гиперпараметр для S
( t 1)
( t 1)
u(t 1) (s)  [uempty
(s) , uoccupied
(s)]
На шаге t, гиперпараметр для S обновляется до
( t 1)
uempty ( s)  uempty
( s)   p( s " empty"| o,qt  s j )b (t ) ( s j )
s j N ( s )
(t )
( t 1)
uoccupied
( s)  uoccupied
( s)   p( s " occupied "| o,qt  s j )b (t ) ( s j )
s j N ( s )
p( s " empty"| o,qt  s j ) and p( s " occupied "| o,qt  s j ) 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
p( s " empty"| o,qt  s j )  p(o | s " empty" , qt  s j ) p( s " empty" )
p0 if o is “occupied”
1-p0 if o is “empty”
prior
p( s " occupied "| o,qt  s j ) can be computed similarly.
Проблема поиска
Предпологаемое изменение:
0
0
0
0
0
0
a=“right”
1
0
0
0.001
0
a=“up”
0
0.001 0.079 0.918
0
0
0.001
0
0
0.000 0.211 0.745
0.000 0.000 0.000
0
0
0
0
0.000 0.001 0.002 0.033 0.009
0
0
0.001
0.000
0
0
0
0
 uempty 

u

 occupied 
Map representation update: 
0.05
0.95
0.95
0.05
1
0
0.05
0.95
a=“right”
0.95
0.05
0.001
0.000
0.002 1.026 0.873
0.000 0.054 0.046
0.001 0.126 1.953 1.943 0.872
0.000 0.954 0.047 0.054 0.046
0.001 0.055 0.046
0.001 1.025 0.872
0.000
0.001
a=“up”
0.000
0.000
0.001 0.202 0.708
0.000 0.011 0.037
0.000 0.203 1.947 1.849
0.000 0.011 0.091 0.058
0.000 0.002 0.128 2.186 2.693
0.000 0.000 0.954 0.059 0.092
0.000 0.002 0.057 0.078
0.000 0.001 1.025 0.874
0.000 0.000 0.000
0.000 0.001 0.000
0.000
0.000
0.716
0.038
0.912 0.009
0.048 0.001
0.009
0.001
Проблема поиска
Выберите действие:
Each state is assigned a reward R(s) according to following rules:
R(s) 
1
(t )
 b (s)
Less explored grids have higher reward.
t
R( s)  p( s " empty" )
R(s)  b(t ) ( s)  window(s)
x
Try to walk to the “empty” grid square.
Consider neighbor of s with priority.
x
Cпасибо за внимание