マルチモーダルカテゴリゼーションに基づく概念の獲得

(1)

ロボット情報工学特論

Advanced Information Engineering for Robotics

第９回：知能ロボティクス２

(2)

Outline



自己位置推定



ベイズフィルタの応用



モンテカルロ位置推定（MCL）



環境地図



占有格子地図



地図の生成



SLAM



カルマンフィルタ



パーティクルフィルタ（FastSLAM）



パスプランニング



RRT



確率的ロードマップ

(3)

ベッド _ソファ椅子長机トイレキッチンテーブルキッチンテーブル本棚本棚本棚長机長机椅子椅子ソファ椅子テーブルテレビテレビテーブル棚棚棚冷蔵庫

移動ロボットの問題



ロボットが環境の中を動き回る



どのように目的地まで行くか？



リアクティブに動くという考え方（ローカルなプランニング）



その場で判断する⇒地図や経路は必要ない



グローバルなプランニング



環境地図



どのようなデータ構造でどのように作るか？



自己位置推定



今どこにいるかをどのように推定するか？



経路計画



地図を使ってどのように経路を計画するか



確率ロボティクスの主要問題

(4)

位置推定問題



ロボットの姿勢 : ( x, y,

θ

₎

⇒直接計測できない



センサ計測値から( x, y,

θ

)を推定



位置推定問題の分類



位置追従

：初期のロボット姿勢が既知



姿勢の誤差が小さい⇒正規分布などで近似できる



局所的な問題



大域的位置推定

：初期姿勢が未知



正規分布などで信念を近似することができない



大域的な推定は位置追従より難しい



誘拐ロボット問題

：ロボットが他の位置にテレポーテー

ションする



ロボットが誤った信念をもってしまう⇒誤りからの回復能力が必要



大域的位置推定よりも難しい

(5)

位置推定の確率モデル



マルコフ性⇒

条件付き独立性



状態遷移確率



計測確率

)

,

|

(

x

₁

u

m

p

_t

₋

_t

)

,

|

(

z

x

m

p

_t

1 −

t

x

_t

x

_t

₊

₁

1 −

t

u

_t

u

_t

₊

₁

1 −

t

z

_t

z

_t

₊

₁

m

：地図

m

x

：状態（位置）

u

：制御

z

：計測

(6)

マルコフ位置推定



マルコフ位置推定アルゴリズム（

）

for all do

end for

return

1

1 ,

)

(

)

,

|

(

)

(

x

_t

=

∫

p

x

_t

u

_t

x

_t

₋

m

bel

x

_t

₋

dx

_t

₋

bel

)

(

)

,

|

(

)

(

x

_t

p

z

_t

x

_t

m

bel

x

_t

bel

=

η

m

z

u

x

bel

(

_t

₋

₁

),

_t

,

_t

,

t

x

)

(

x

_t

bel

_{•ベイズフィルタと同じ}

•地図mが入っているだけ

•x0の与え方で各種位置推定問題に対応する

•実装の問題

⇒カルマンフィルタが使える

(7)

ロボットの動作モデル



ロボットの動作モデル ⇒ 状態方程式

が必要



２つの動作モデル



オドメトリベース



速度ベース

(

デッドレコニング

)



エンコーダの情報が使える場合はオドメトリを使うのが一般的



エンコーダの情報が使えない場合は速度ベース



速度と時間で姿勢を計算するモデル

)

,

|

(

x

_t

u

_t

x

_t

₋

₁

p

(8)

動作モデルが必要な理由

バンプ

理想的な場合

車輪の径が異なる

カーペット（すべり）

など様々・・・

(9)



ロボットが

から

へ移動



オドメトリ情報

オドメトリモデル

2

2 )

'

(

)

'

(

x

y

trans

=

−

+

−

δ

θ

δ

_rot

₁

=

atan2

(

y

'

−

y

,

x

'

−

x

)

−

1

2 '

rot

θ

δ

=

−

θ

,

, y

x

_x

_'

_,

_y

_'

_,

θ

_'

trans

rot

u

=

δ

₁

,

δ

₂

,

δ

trans

δ

1 rot

δ

2 rot

δ

θ

,

, y

x

'

,

'

,

'

y

θ

x

(10)

オドメトリの雑音モデル



計測されるオドメトリは理想的なオドメ

トリに雑音をのせることで表現する

|

1

₁ ₁ ₂

ˆ

trans rot

rot

δ

ε

α

δ

α

δ

=

+

₊

|

2

₁ ₂ ₂

ˆ

trans rot

rot

δ

ε

α

δ

α

δ

=

+

₊

|

₄ ₁ ₂ 3

ˆ

rot rot trans

trans

δ

ε

α

δ

α

δ

=

+

₊

(11)

確率の計算（ゼロ平均）

1. prob_normal_distribution

(a,b):

2. return

2 2 2 2 1 2

2

1 )

(

σ

_πσ

ε

x

e

x −

=

正規分布



一般的には正規分布が用いられる













−

₂2 2

₂

1 exp

2

1 b

a

b

π

(12)

1. Algorithm motion_model_odometry

(x,x’,u)

2.

3.

4.

5.

6.

7.

8.

9.

10. 11. return

p

₁

· p

₂

· p

₃

動作モデル（事後確率）の計算

2

2 )

'

(

)

'

(

x

y

trans

=

−

+

−

δ

θ

δ

_rot

₁

=

atan2

(

y

'

−

y

,

x

'

−

x

)

−

1

2 '

rot

θ

δ

=

−

2

2 )

'

(

)

'

(

ˆ

_x

_y

trans

=

−

+

−

δ

θ

δ

ˆ

_rot

₁

=

atan2

(

y

'

−

y

,

x

'

−

x

)

−

1

2 '

ˆ

rot

θ

δ

=

−

)

ˆ

|

ˆ

|

,

ˆ

(

prob

_rot

₁

_rot

₁

_rot

₁

₂

_trans

1 =

δ

−

δ

α

δ

+

α

δ

p

|))

ˆ

|

ˆ

(|

ˆ

,

ˆ

(

prob

_trans

₃

_trans

₄

_rot1

_rot2

2 =

δ

−

δ

α

δ

+

α

δ

+

δ

p

)

ˆ

|

ˆ

|

,

ˆ

(

prob

_rot

₂

_rot

₂

₁

_rot

₂

_trans

3 =

δ

−

δ

α

δ

+

α

δ

p

odometry values

(u)

(13)

動作モデルの例

x’

u

p(x|u,x’)

u

x’

(14)

オドメトリ動作モデルのサンプリング

1. Algorithm sample_motion_model

(u, x)

:

1.

2.

3.

4.

5.

6.

7. Return

)

|

sample(

ˆ

2 1 1 1

1 rot rot trans rot

δ

α

δ

α

δ

=

+

|))

|

(|

sample(

ˆ

2 1 4

3 trans rot rot trans trans

δ

α

δ

α

δ

=

+

)

|

sample(

ˆ

2 2 1 2

2 rot rot trans rot

δ

α

δ

α

δ

=

+

)

ˆ

cos(

ˆ

'

x

_trans _rot₁

x

=

+

δ

θ

+

δ

)

ˆ

sin(

ˆ

'

y

_trans _rot₁

y

=

+

δ

θ

+

δ

2 1

ˆ

'

θ

δ

_rot

δ

_rot

θ

=

+

'

,

'

,

'

y

θ

x

θ

δ

₁

,

₂

,

x

,

y

,

u

=

_rot _rot _trans

=

(15)

動作モデルからのサンプリングの例

(16)

オドメトリモデルの例

(17)

ロボットの計測モデル



をモデル化する必要がある



ロボットの位置x（と地図）が与えられた際にzが得られ

る確率

)

,

|

(

z

x

m

p

_t

移動ロボットのセンサ



接触センサ

:

Bumpers



内界センサ



加速度センサ

(spring-mounted masses)



ジャイロスコープ

(spinning mass, laser light)



コンパス

, 傾斜計 (earth magnetic field, gravity)



距離センサ



ソナー

(time of flight)



レーダー

(phase and frequency)



レーザーレンジファインダ

(triangulation, tof, phase)



赤外線

(intensity)

(18)

スキャンベースモデル



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

(19)

例

P(z|x,m)

地図 m

(20)

San Jose Tech Museum

(21)

スキャンマッチング



スキャンから尤度場を生成しそれらを比

較することでスキャン同士のマッチング

をする

(22)

相関ベースの計測モデル



地図が占有格子地図として表現されてい

る



現在のスキャンを占有格子地図と同等の

表現に変換



ロボットの想定位置において地図とス

キャンの正規化相関関数を計算

}

0 ,

max{

)

,

|

(

_,

t local

x

m

t

local

x

m

p

=

ρ

地図

における位置

のスキャン

の尤もらしさ

相関値 [-1, 1]

t

x

m

_local

(23)

拡張カルマンフィルタの利用



カルマンフィルタを使ってマルコフ位置推定を

実現



動作モデル、計測モデルの線形化が必要



拡張カルマンフィルタ（EKF）



最適ではない



非線形性が強すぎると発散する可能性がある



しかしすべての前提条件を満たさない時でも実際に

はかなりよく働く

(24)

予測：

3.

5.

6.

7. µ

_t

=

_g

₍

_u

_t

_,

µ

_t₋₁

₎

















∂

=

∂

=

− − − − − − − − − − − θ θ θ

µ

θ

µ

θ

µ

θ

µ

, 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 , 1 1 1

'

)

,

(

t y t x t t y t x t t y t x t t t t t

y

x

u

g

G













∂

=

∂

=

− t t t t t t t t t t

v

y

v

y

x

v

x

u

g

V

ω

θ

ω

µ

'

)

,

(

₁

(

)

(

)



_









+

=

₂ 4 3 2 2 1

|

0

0 |

|

t t t t t

v

M

ω

α

ω

α

_{Motion noise}

Jacobian of g w.r.t location

Predicted mean

Predicted covariance

Jacobian of g w.r.t control

(25)

更新：

2.

3.

4.

5.

6.

7.

8. )

ˆ

(

_t _t t t t

=

µ

+

K

z

−

z

µ

(

−

)

Σ

=

Σ













∂

=

∂

=

θ θ

µ

ϕ

µ

ϕ

µ

ϕ

µ

, , , , , ,

)

,

(

t t t t y t t y t t x t t x t t t t t

r

x

m

h

H

(

)

(

)

(

)













−

+

−

=

θ

µ

, , , 2 , 2 ,

,

2 atan

ˆ

t x t x y t y y t y x t x t

m

z

t

T

t

H

Q

S

=

Σ

+

1 −

Σ

=

T _t t t t

H

S

K













=

2 ₂

0

r r t

Q

σ

Predicted measurement mean

Pred. measurement covariance

Kalman gain

Updated mean

Updated covariance

(26)

(27)

(28)

(29)

(30)

(31)

(32)

モンテカルロ位置推定



位置推定におけるパーティクルフィルタの利用



MCLアルゴリズム（

χ

t

−

1 ,

u

t

,

z

t

,

m

）

for m=1 to M

do

endfor

for m=1 to M

do

endfor

∅

=

_t

t

χ

)

,

ion_model(

sample_mot

(

₁

)

(

m

t

m

t

u

x

=

₋

m)

,

t_model(

measuremen

(

)

(

m

t

m

t

z

x

w

=

) ( ) (

,

_tm m t t t

=

χ

+

x

w

χ

) (

y

probabilit

with

draw

i

∝

w

_tm t ) (

to

add

x

_ti

χ

サンプリング

重み評価

リサンプリング

(33)

ＭＣＬ位置推定の例

グローバル位置推定

(34)

地図生成の問題



ロボットの位置を知るためには環境地図が必要



地図生成の問題とは、与えられた観測データ

に対して最も尤もらしい地図

を計算することである



まずはロボットの位置が既知の場合を考える



実際には地図もロボットの位置も未知（ＳＬＡＭ問

}

,

{

u

₁

z

₁

u

₂

z

₂

u

_n

z

_n

d

=



)

|

(

max

arg

*

d

m

P

m

=

(35)

占有格子地図



位置ベースと特徴ベース



特徴ベースの地図



ランドマークと位置



書き換えなどが容易



位置ベースの地図⇒占有格子地図



各位置（グリッド）が物体で占有されてい

るかどうか⇒各座標xyに割り当てる



移動ロボットのナビゲーションに適してい

る



経路の探索が容易

(36)

(37)

(38)

Resulting Occupancy and

Maximum Likelihood Map

The maximum likelihood map is obtained by

clipping the occupancy grid map at a

(39)

(40)

Tech Museum, San Jose

(41)

Given:



ロボットの制御



センサによる観測

Estimate:



環境地図



ロボットの経路

ＳＬＡＭの問題

ロボット

が未知の

（静的）環

境で

行動する

場合

(42)

ＳＬＡＭはなぜ難しいのか？

SLAM: ロボットの経路と環境地図が共に未知



鶏と卵の問題



ロボットの姿勢には曖昧性があり得る

(43)

M

apping



完全SLAM:



オンラインSLAM:

今までの情報を統合してその時ごとに推定

)

,

|

,

(

x

₁

_:

_t

m

z

₁

_:

_t

u

₁

_:

_t

p

1

2

1 :

1 ,

)

(

,

|

,

)

...

|

,

(

x

_t

m

z

_t

u

_t

=

∫∫ ∫

p

x

_t

m

z

_t

u

_t

dx

_t

₋

p



現在の姿勢と地図を推定

完全な経路と地図を推定

(44)

モデル

1

2

1 :

1 ,

)

(

,

|

,

)

...

|

,

(

x

_t

m

z

_t

u

_t

=

∫∫ ∫

p

x

_t

m

z

_t

u

_t

dx

_t

₋

p



(45)

完全ＳＬＡＭのグラフィカルモデル

)

,

|

,

(

x

₁

_:

_t

m

z

₁

_:

_t

u

₁

_:

_t

p

(46)

ＳＬＡＭの手法



スキャンマッチング



ICP（Iterative Closest Points）



EKF SLAM



拡張カルマンフィルタ



Fast-SLAM



パーティクルフィルタ

(47)

スキャンマッチング

時刻tの姿勢と地図の尤度を(t-1)の姿勢と地図

に関して最大化する

推定した姿勢とその時の計測情報に基づいて

地図情報

を更新する

{

(

|

,

ˆ

)

(

|

,

ˆ

)

}

max

arg

ˆ

=

_t

[

t

−

1 ]

⋅

_t

₋

₁

_t

₋

₁

x

t

p

z

x

m

p

x

u

x

t

ロボットの動き

現在の計測情報

t-1までに推定した地図

]

[

ˆ

t

m

(48)

(49)

（再掲）



カルマンフィルタアルゴリズム

（

）

t t t t−1

,

Σ

−1

,

u ,

z

µ

t t t t t t t t t t t t t T t t t T t t t t T t t t t t t t t t

return

C

K

I

C

z

K

Q

C

K

R

A

u

B

A

Σ

−

=

Σ

−

+

=

+

Σ

=

+

Σ

=

Σ

+

=

− − −

,

)

(

)

(

)

(

1 1 1

µ

：カルマンゲイン

予測

更新

計測を新たな状態推定

にどれくらい反映させるか

イノベーション：

計測値と期待される

計測値との差

1 ₋ − −

₊

_Σ

=

Σ

(50)





























=

2 2 2 2 2 2 2 1 2 1 2 2 2 1 2 2 2 1 2 1 1 1 1 1 2 1 2 1 2 1

,

)

,

(

N N N N N N N N N N N l l l l l l yl xl l l l l l l yl xl l l l l l l yl xl l l l y x yl yl yl y y xy xl xl xl x xy x N t t

l

y

x

m

x

Bel

σ

θ















ランドマークを使った環境地図



N個のランドマークを用いた地図

⇒(3+2N)次元のガウス分布



数百の次元を扱うことが可能

EKF-SLAM

(51)

EKF-SLAM

(52)

EKF-SLAM

(53)

EKF-SLAM

(54)

Victoria Park データセット

(55)

(56)

EKF SLAM実例

(57)

EKF SLAM実例

(58)



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”)

(59)

(60)

Rao-Blackwellization

SLAM事後分布

ロボット経路事後分布

既知姿勢の地図生成

姿勢

地図

計測

制御

位置推定：MCLを利用

MCLによる位置推定の

結果を位置を既知とした場合

の地図生成に適用する

(61)

グラフィカルモデル

m

x

z

u

x

z

u

2

2 x

z

u

...

t

x

1

0

1

0 t-1

(62)

Rao-Blackwellized地図生成



それぞれのパーティクルがロボットのあり得る経路を表現している



それぞれのパーティクルは



それぞれの地図を保持している



地図は推定された経路情報を利用してアップデートされる



それぞれのパーティクルは観測情報とそのパーティクルが保持す