ロボット情報工学特論 no.14 - 長井研究室

http://apple.ee.uec.ac.jp/isyslab
ロボット情報工学特論 no.14
Advanced Information Engineering for Robotics no.14
第１４回：知能ロボティクス２～確率ロボティクス応用編～
電気通信大学大学院
情報理工学研究科
知能機械工学専攻
長井隆行
http://apple.ee.uec.ac.jp/isyslab
Outline

自己位置推定



環境地図



占有格子地図
地図の生成
SLAM



ベイズフィルタの応用
モンテカルロ位置推定（MCL）
カルマンフィルタ
パーティクルフィルタ（FastSLAM）
パスプランニング


RRT
確率的ロードマップ
2
http://apple.ee.uec.ac.jp/isyslab
移動ロボットの問題

ロボットが環境の中を動き回る


リアクティブに動くという考え方（ローカルなプランニング）


どのように目的地まで行くか？
その場で判断する⇒地図や経路は必要ない
グローバルなプランニング

自己位置推定


今どこにいるかをどのように推定するか？
経路計画


どのようなデータ構造でどのように作るか？
冷蔵庫
テテレビ
ー
ブ
ル
椅
子
環境地図


ソ
フ
ァ
長
机
椅子
長机
キッチン
テーブル
椅子
棚
本棚
棚
本棚
本棚
キッチン
テーブル
トイレ
棚
長
テーブル机
ベッド
ソ
フ
ァ
テレビ
椅子
地図を使ってどのように経路を計画するか
確率ロボティクスの主要問題
3
http://apple.ee.uec.ac.jp/isyslab
位置推定問題



ロボットの姿勢 : (x,y,q) ⇒直接計測できない
センサ計測値から(x,y,q)を推定
位置推定問題の分類
 位置追従：初期のロボット姿勢が既知
 姿勢の誤差が小さい⇒正規分布などで近似できる
 局所的な問題
 大域的位置推定：初期姿勢が未知
 正規分布などで信念を近似することができない
 大域的な推定は位置追従より難しい
 誘拐ロボット問題：ロボットが他の位置にテレポーテーションする
 ロボットが誤った信念をもってしまう⇒誤りからの回復能力が必要
 大域的位置推定よりも難しい
4
http://apple.ee.uec.ac.jp/isyslab
位置推定の確率モデル



マルコフ性⇒ 条件付き独立性
p( xt | xt 1 , ut , m)
状態遷移確率
p( zt | xt , m)
計測確率
xt 1
ut 1
zt 1
xt 1
xt
ut
zt
m ：地図
x ：状態（位置）
u ：制御
z ：計測
ut 1
zt 1
m
5
http://apple.ee.uec.ac.jp/isyslab
マルコフ位置推定

マルコフ位置推定アルゴリズム（ bel ( xt 1 ), ut , zt , m ）
for all xt do
bel ( xt )   p( xt | ut , xt 1 , m)bel ( xt 1 )dxt 1
bel ( xt )  p( zt | xt , m)bel ( xt )
end for
return bel ( xt )
•ベイズフィルタと同じ
•地図mが入っているだけ
•x0の与え方で各種位置推定問題に対応する
•実装の問題
⇒カルマンフィルタが使える
6
http://apple.ee.uec.ac.jp/isyslab
ロボットの動作モデル

ロボットの動作モデル ⇒ 状態方程式
p( xt | ut , xt 1 ) が必要

２つの動作モデル

オドメトリベース

速度ベース (デッドレコニング)

エンコーダの情報が使える場合はオドメトリを使うのが一般的

エンコーダの情報が使えない場合は速度ベース

速度と時間で姿勢を計算するモデル
7
http://apple.ee.uec.ac.jp/isyslab
動作モデルが必要な理由
車輪の径が異なる
理想的な場合
バンプ
カーペット（すべり）
など様々・・・
8
http://apple.ee.uec.ac.jp/isyslab
オドメトリモデル
 ロボットが x , y,q
から
x ' , y ' ,q '
へ移動
 オドメトリ情報 u   rot1 ,  rot2 ,  trans
 trans  ( x ' x ) 2  ( y ' y ) 2
 rot1  atan2( y ' y, x ' x )  q
 rot2  q 'q   rot1
x , y,q
 rot1
 rot2
 trans
x ' , y ' ,q '
9
http://apple.ee.uec.ac.jp/isyslab
オドメトリの雑音モデル
 計測されるオドメトリは理想的なオドメトリに
雑音をのせることで表現する
ˆrot1   rot1  
1 | rot1 | 2
ˆtrans   trans  
ˆrot2   rot2  
3
| trans|
| trans| 4 | rot1  rot 2 |
1 | rot 2 | 2
| trans|
10
http://apple.ee.uec.ac.jp/isyslab
確率の計算（ゼロ平均）
正規分布

1.
2.
一般的には正規分布が用いられる
prob_normal_distribution(a,b):
return
 1 a2 
exp 
2
2
2b
 2b 
1
  ( x) 
2

1
2
2
e
1 x2
2 2
11
http://apple.ee.uec.ac.jp/isyslab
動作モデル（事後確率）の計算
1.
Algorithm motion_model_odometry (x,x’,u)
2.
 trans  ( x ' x ) 2  ( y ' y ) 2
 rot1  atan2( y ' y, x ' x )  q
 rot2  q 'q   rot1
3.
4.
odometry values (u)
2
2
ˆ


(
x
'

x
)

(
y
'

y
)
5.
trans
values of interest (x,x’)
ˆrot1  atan2( y' y, x' x)  q
6.
ˆrot2  q 'q  ˆrot1
7.
ˆ ,  | ˆ |  ˆ )
p

prob
(



1
rot1
rot1
1
rot1
2 trans
8.
ˆ ,  ˆ   (| ˆ |  | ˆ |))
p

prob
(



2
trans
trans
3 trans
4
rot1
rot2
9.
ˆ ,  | ˆ |  ˆ )
p

prob
(



3
rot
2
rot 2
1
rot 2
2 trans
10.
11. return p1 · p2 · p3
12
http://apple.ee.uec.ac.jp/isyslab
動作モデルの例
p(x|u,x’)
x’
x’
u
u
13
http://apple.ee.uec.ac.jp/isyslab
オドメトリ動作モデルのサンプリング
1.
Algorithm sample_motion_model (u, x):
u   rot1 ,  rot2 ,  trans , x  x, y,q
1. ˆrot1   rot1  sample(1 |  rot1 |  2  trans)
ˆ
2.  trans   trans  sample( 3  trans   4 (|  rot1 |  |  rot2 |))
 sample( |  |   )
3. ˆ  
rot2
rot2
1
6.
x'  x  ˆtrans cos(q  ˆrot1 )
y'  y  ˆtrans sin(q  ˆrot1 )
q '  q  ˆ  ˆ
7.
Return
4.
5.
rot1
rot2
2
trans
sample_normal_distribution
rot2
x ' , y ' ,q '
14
http://apple.ee.uec.ac.jp/isyslab
動作モデルからのサンプリングの例
Start
15
http://apple.ee.uec.ac.jp/isyslab
オドメトリモデルの例
分散の与え方（パラメータ）によって異なる
16
http://apple.ee.uec.ac.jp/isyslab
ロボットの計測モデル


p( zt | xt , m) をモデル化する必要がある
ロボットの位置x（と地図）が与えられた際にzが得られる確率
移動ロボットのセンサ
接触センサ: Bumpers
 内界センサ

加速度センサ (spring-mounted masses)
 ジャイロスコープ (spinning mass, laser light)
 コンパス, 傾斜計 (earth magnetic field, gravity)


距離センサ
ソナー (time of flight)
 レーダー (phase and frequency)
 レーザーレンジファインダ (triangulation, tof, phase)
 赤外線 (intensity)



視覚センサ: カメラ
位置センサ: GPS
17
http://apple.ee.uec.ac.jp/isyslab
スキャンベースモデル
 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.
 Again,
independence between
different components is assumed.
18
http://apple.ee.uec.ac.jp/isyslab
例
尤度場
地図 m
P(z|x,m)
19
http://apple.ee.uec.ac.jp/isyslab
San Jose Tech Museum
Occupancy grid map
Likelihood field
20
http://apple.ee.uec.ac.jp/isyslab
スキャンマッチング
 スキャンから尤度場を生成しそれらを比較
することでスキャン同士のマッチングをする
21
http://apple.ee.uec.ac.jp/isyslab
相関ベースの計測モデル
 地図が占有格子地図として表現されている
 現在のスキャンを占有格子地図と同等の表
現に変換
 ロボットの想定位置において地図とスキャン
の正規化相関関数を計算
相関値 [-1, 1]
p(mlocal | xt , m)  max{ m, mlocal , xt ,0}
地図 m における位置 xt のスキャン mlocal の尤もらしさ
22
http://apple.ee.uec.ac.jp/isyslab
拡張カルマンフィルタの利用
カルマンフィルタを使ってマルコフ位置推定を実現
 動作モデル、計測モデルの線形化が必要
 拡張カルマンフィルタ（EKF）

 最適ではない
 非線形性が強すぎると発散する可能性がある
 しかしすべての前提条件を満たさない時でも実際にはか
なりよく働く
23
http://apple.ee.uec.ac.jp/isyslab
1.
EKF_localization ( mt-1, St-1, ut, zt, m):
予測：
3. Gt 
5.
6.
g (ut , mt 1 )
xt 1
 x'

 mt 1, x
 y '
 
 mt 1, x
 q '

 mt 1, x
x'
mt 1, y
y '
mt 1, y
q '
mt 1, y
x' 
mt 1,q 
y ' 
 Jacobian of g w.r.t location
mt 1,q 
q ' 

mt 1,q 
 x' x' 



v


 t
t 
 y ' y ' 
g (ut , mt 1 )
Vt 
 

ut

v


t
t



q
'

q
'


 v  
t 
 t
 1 | vt |  2 | t |2

0

M t  
2
 3 | vt |  4 | t | 
0

Jacobian of g w.r.t control
Motion noise
7.
mt  g (ut , mt 1 )
Predicted mean
8.
St  Gt St 1GtT  Vt M tVtT
Predicted covariance
24
http://apple.ee.uec.ac.jp/isyslab
1.
EKF_localization ( mt-1, St-1, ut, zt, m):
更新：
2.
zˆt
2
2


mx  mt , x   m y  mt , y  


 atan 2m  m , m  m   m 
y
t,y
x
t,x
t ,q 

h( mt , m)
xt
3.
Ht
4.
  r2 0 

Qt  
2
 0 r 

 rt

m
  t,x
 t

 mt , x
rt
mt , y
t
mt , y
Predicted measurement mean
rt
mt ,q
t
mt ,q






Jacobian of h w.r.t location
St  H t St H tT  Qt
Pred. measurement covariance
6.
Kt  St H tT St1
Kalman gain
7.
mt  mt  Kt ( zt  zˆt )
Updated mean
8.
St  I  Kt H t St
Updated covariance
5.
25
http://apple.ee.uec.ac.jp/isyslab
EKF Prediction Step
26
http://apple.ee.uec.ac.jp/isyslab
EKF Observation Prediction Step
27
http://apple.ee.uec.ac.jp/isyslab
EKF Correction Step
28
http://apple.ee.uec.ac.jp/isyslab
Estimation Sequence (1)
29
http://apple.ee.uec.ac.jp/isyslab
Estimation Sequence (2)
30
http://apple.ee.uec.ac.jp/isyslab
Comparison to GroundTruth
31
http://apple.ee.uec.ac.jp/isyslab
モンテカルロ位置推定
位置推定におけるパーティクルフィルタの利用
 MCLアルゴリズム（  t 1 , ut , zt , m ）

t  t  
for m=1
( m)
t
( m)
t
x
w
to
M
do
 sample_motion_model( ut , xt(m1) )
 measuremen t_model( zt , xt( m) , m)
サンプリング
重み評価
t  t  xt( m) , wt( m)
endfor
for m=1
to
M
do
draw i with probability  wt( m)
リサンプリング
add xt(i ) to  t
endfor
return
t
32
http://apple.ee.uec.ac.jp/isyslab
ＭＣＬ位置推定の例
グローバル位置推定
誘拐ロボット問題@スミソニアン博物館
http://robots.stanford.edu/videos.html
33
http://apple.ee.uec.ac.jp/isyslab
地図生成の問題
ロボットの位置を知るためには環境地図が必要
 地図生成の問題とは、与えられた観測データ

d  {u1 , z1 , u2 , z2 ,, un , zn }
に対して最も尤もらしい地図
m  arg max P(m | d )
*
m
を計算することである
 まずはロボットの位置が既知の場合を考える
 実際には地図もロボットの位置も未知（ＳＬＡＭ問題）
34
http://apple.ee.uec.ac.jp/isyslab
占有格子地図
 位置ベースと特徴ベース
 特徴ベースの地図
 ランドマークと位置
 書き換えなどが容易
 位置ベースの地図⇒占有格子地図
 各位置（グリッド）が物体で占有されているか
どうか⇒各座標xyに割り当てる
 移動ロボットのナビゲーションに適している
 経路の探索が容易
35
http://apple.ee.uec.ac.jp/isyslab
占有格子の逐次更新の例
36
http://apple.ee.uec.ac.jp/isyslab
Resulting Map Obtained with
Ultrasound Sensors
37
http://apple.ee.uec.ac.jp/isyslab
Resulting Occupancy and
Maximum Likelihood Map
The maximum likelihood map is obtained by
clipping the occupancy grid map at a
threshold of 0.5
38
http://apple.ee.uec.ac.jp/isyslab
占有格子地図の例
スキャンデータ
占有格子地図
39
http://apple.ee.uec.ac.jp/isyslab
Tech Museum, San Jose
CADの地図
占有格子地図
40
http://apple.ee.uec.ac.jp/isyslab
ＳＬＡＭの問題
ロボット
が未知の
（静的）環
境で
行動する
場合
Given:
 ロボットの制御
 センサによる観測
Estimate:
 環境地図
 ロボットの経路
41
http://apple.ee.uec.ac.jp/isyslab
ＳＬＡＭはなぜ難しいのか？
SLAM: ロボットの経路と環境地図が共に未知



鶏と卵の問題
ロボットの姿勢には曖昧性があり得る
経路の推定エラーが地図の生成に影響を与える
42
http://apple.ee.uec.ac.jp/isyslab
ＳＬＡＭ: Simultaneous Localization and Mapping
 完全SLAM:
完全な経路と地図を推定
p( x1:t , m | z1:t , u1:t )
 オンラインSLAM: 現在の姿勢と地図を推定
p( xt , m | z1:t , u1:t )      p( x1:t , m | z1:t , u1:t ) dx1dx2 ...dxt 1
今までの情報を統合してその時ごとに推定
43
http://apple.ee.uec.ac.jp/isyslab
オンラインＳＬＡＭのグラフィカルモデル
p( xt , m | z1:t , u1:t )      p( x1:t , m | z1:t , u1:t ) dx1 dx2 ...dxt 1
44
http://apple.ee.uec.ac.jp/isyslab
完全ＳＬＡＭのグラフィカルモデル
p( x1:t , m | z1:t , u1:t )
45
http://apple.ee.uec.ac.jp/isyslab
ＳＬＡＭの手法
 スキャンマッチング
 ICP（Iterative
 EKF
Closest Points）
SLAM
 拡張カルマンフィルタ
 Fast-SLAM
 パーティクルフィルタ
 Graph-SLAM,
SEIFs
46
http://apple.ee.uec.ac.jp/isyslab
スキャンマッチング
時刻tの姿勢と地図の尤度を(t-1)の姿勢と地図
に関して最大化する

[ t 1]
xˆt  arg max p( zt | xt , mˆ

)  p( xt | ut 1 , xˆt 1 )
xt
現在の計測情報
ロボットの動き
t-1までに推定した地図
推定した姿勢とその時の計測情報に基づいて地
図情報 mˆ [t ] を更新する
47
http://apple.ee.uec.ac.jp/isyslab
スキャンマッチングの例
48
http://apple.ee.uec.ac.jp/isyslab
カルマンフィルタアルゴリズム（再掲）
 カルマンフィルタアルゴリズム（ mt 1 , St 1 , ut , zt
予測
m t  At mt 1  Bt ut
S t  At S t 1 AtT  Rt
）
計測を新たな状態推定
にどれくらい反映させるか
K t  S t CtT (Ct S t CtT  Qt ) 1 ：カルマンゲイン
更新
m t  m t  K t ( z t  Ct m t )
S t  ( I  K t Ct ) S t
return
イノベーション：
計測値と期待される
計測値との差
mt , S t
1
St  (CtT Qt1Ct  St ) 1 から逆行列の補題を利用して導出
49
http://apple.ee.uec.ac.jp/isyslab
EKF-SLAM
ランドマークを使った環境地図
 N個のランドマークを用いた地図

⇒(3+2N)次元のガウス分布
Bel ( xt , mt ) 

x
 
 y
q 
 
 l1  ,
l 
 2

 
 lN 
  x2

  xy

  xq
  xl1

  xl2
 

 xl
 N
 xy
 y2
 yq
 yl1
 yl2

 yl N
 xq
 yq
 q2
 ql1
 ql2

 qlN
 xl1
 yl1
 ql1
 l21
 l1l2

 l1lN
 xl2
 yl2
 ql2
 l1l2
 l22

 l2l N
  xl N 

  yl N 

  qlN 
  l1lN 

  l2l N 
  

2 
  lN 
数百の次元を扱うことが可能
50
http://apple.ee.uec.ac.jp/isyslab
EKF-SLAM
Map
Correlation matrix
51
http://apple.ee.uec.ac.jp/isyslab
EKF-SLAM
Map
Correlation matrix
52
http://apple.ee.uec.ac.jp/isyslab
EKF-SLAM
Map
Correlation matrix
53
http://apple.ee.uec.ac.jp/isyslab
Victoria Park データセット
[courtesy by E. Nebot]
54
http://apple.ee.uec.ac.jp/isyslab
推定経路
[courtesy by E. Nebot]
55
http://apple.ee.uec.ac.jp/isyslab
EKF SLAM実例
[courtesy by John Leonard]
56
http://apple.ee.uec.ac.jp/isyslab
EKF SLAM実例
オドメトリ
推定された軌跡
[courtesy by John Leonard]
57
http://apple.ee.uec.ac.jp/isyslab
グリッドベース FastSLAM

Can we solve the SLAM problem if no pre-defined
landmarks are available?

Can we use the ideas of FastSLAM to build grid
maps?

As with landmarks, the map depends on the poses
of the robot during data acquisition

If the poses are known, grid-based mapping is easy
(“mapping with known poses”)
58
http://apple.ee.uec.ac.jp/isyslab
オドメトリ情報を使った地図生成
59
http://apple.ee.uec.ac.jp/isyslab
Rao-Blackwellization
姿勢
地図計測
制御
SLAM事後分布
ロボット経路事後分布
位置推定：MCLを利用
既知姿勢の地図生成
MCLによる位置推定の
結果を位置を既知とした場合
の地図生成に適用する
Factorization first introduced by Murphy in 1999
60
http://apple.ee.uec.ac.jp/isyslab
Rao-Blackwellized地図生成の
グラフィカルモデル
u0
x0
u1
ut-1
x1
x2
z1
z2
...
xt
m
zt
61
http://apple.ee.uec.ac.jp/isyslab
Rao-Blackwellized地図生成
 それぞれのパーティクルがロボットのあり得る経路を表現している
 それぞれのパーティクルは
 それぞれの地図を保持している
 地図は推定された経路情報を利用してアップデートされる
 それぞれのパーティクルは観測情報とそのパーティクルが保持す
る地図によって計算される尤度に比例する確率で残される
62
http://apple.ee.uec.ac.jp/isyslab
パーティクルフィルタの例
3 particles
map of particle 1
map of particle 2
map of particle 3
63
http://apple.ee.uec.ac.jp/isyslab
さらなる問題
 格子地図の情報量が大きい
 各パーティクルがそれぞれの地図を保持するため
パーティクル数をなるべく少なくしたい
 解決策：
 提案分布の改善
 具体的な手法
 パーティクルフィルタを適用する前の姿勢推定の精度を
改善する
64
http://apple.ee.uec.ac.jp/isyslab
スキャンマッチングによる姿勢の補正
i-1番目の姿勢と地図に対してi番目の姿勢と地
図の尤度を最大化する
xˆt  arg max p( zt | xt , mˆ t 1 )  p( xt | ut 1 , xˆt 1 )
xt
現在の計測値
制御
t-1までの地図
65
http://apple.ee.uec.ac.jp/isyslab
スキャンマッチングによる動きモデル
Raw Odometry
Scan Matching
66
http://apple.ee.uec.ac.jp/isyslab
改良したオドメトリによるFastSLAM
 スキャンマッチングにより局所的に精度の高い姿勢の
推定を行う
 スキャンマッチングによる局所的（短い区間）で精度の
よいオドメトリ列をFastSLAMの入力とする
 入力自体のエラーが小さいためパーティクル数を少な
くできる
[Haehnel et al., 2003]
67
http://apple.ee.uec.ac.jp/isyslab
改良オドメトリによる地図生成の
グラフィカルモデル
u0 ... uk-1
z1 ... z k-1
x0
uk ... u2k-1
zk+1...z2k-1
...
un·k ... u(n+1)·k-1
z n·k+1... z(n+1)·k-1
u'1
u'2
...
u'n
xk
x2k
...
x n·k
zk
z2k
...
zn·k
m
68
http://apple.ee.uec.ac.jp/isyslab
Intel Labの例
 パーティクル数 15
 実時間の４倍の速度
(P4, 2.8GHz)
 スキャンマッチング時
の解像度は5cm
 地図の解像度は1cm
69
http://apple.ee.uec.ac.jp/isyslab
Intel Labの例続き
 パーティクル数 15
FastSLAM with Scan-Matching
70

Download Report