パーティクルフィルタの癖から知るロボットへの確率的手法の正しい適用方法

上田隆一

産業技術大学院大学

2014年 5月 21日第 58回システム制御情報学会研究発表講演会

2014 年 5 月 21 日 1 / 35

自己紹介

上田隆一（36）産業技術大学院大学産業技術研究科情報アーキテクチャ専攻助教USP研究所アドバイザリーフェロー

研究歴・職歴2000年～2008年: ロボカップ 4足ロボットリーグ参戦

パーティクルフィルタを 2001年に実装し実践投入

2007年に「Probabilistic ROBOTICS」を翻訳

2009年～2013年 7月: 研究を中断しプログラマーとして働くUSP研究所にてシェルスクリプトで企業システムを構築する仕事業務の傍ら雑誌記事執筆やコミュニティー活動

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 2 / 35

最近の活動

本年度中に 2冊書籍を出版予定シェルスクリプトでウェブサイトを作る本（仮）シェルプログラミング本（仮）

6月に大阪で日本 UNIXユーザ会と共催で勉強会講師 .. Link

ロボット研究パーティクルフィルタ関連の研究から再開

Ryuichi UEDA: Generation of Search Behavior by aModification of Q-MDP Value Method, IAS-13, 2014.(to appear)

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 3 / 35

予稿の訂正

予稿で「測度」と表現されている部分2ページ目: ルベーグ積分で言うところの「測度」 → 「測度を計算するための値」他「測度」となっているものは「値」

あろうことか基礎用語を誤用してしまいました。申し訳ございません。

ベイズの公式の ηを説明する ηがパーティクルフィルタではη−1になっている

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 4 / 35

本日の内容

パーティクルフィルタを実装する上で、普通の研究発表では取り上げられないことについて説明

知らないとチューニング地獄に陥ったり、自己流の安易な工夫をしたりということに

パーティクルフィルタの基礎パーティクルフィルタとは（1章）ベイズフィルタ（2章）パーティクルフィルタ（3章）

パーティクルフィルタの実装上、知っておくべき「癖」（4章）

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 5 / 35

1. パーティクルフィルタとは

ベイズフィルタを近似実装したもの確率分布をモンテカルロ法で近似逐次モンテカルロ法 [Doucet2001]ともロボット、画像処理、金融 · · ·

自己位置推定の用途ではMonte Carlo localization（MCL）とも呼ばれる [Fox2000]

自己位置推定の例: .. Link

ロボットが動いたらパーティクルも動くセンサ等からの情報で、ロボットがいないと考えられる位置からパーティクルが除去されていくパーティクルとロボットの位置の乖離が疑われるとリセット

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 6 / 35

パーティクルフィルタの使用例

RoboCup .. Link

雑音が極めて大きい・得られる情報が限定的携帯電話のカメラ程度の視野・画素数カメラの揺れによる撮影位置のズレ・画像のブレロボット同士の衝突（向きが 90[deg]変わることも）

つくばチャレンジ .. Link

屋外位置推定に利用できる多種多様な情報

GPS、距離センサ、画像、地磁気

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 7 / 35

2. ベイズフィルタによる自己位置推定

ロボットは自身の位置を知らないその代わり、確率密度関数 bを通して知覚

b: belief（信念）ロボットが自身の位置について信じている情報POMDPs（partially observable Markov decision processes）では制御対象の状態とみなされる（信念状態）

図: 一軸上を移動するロボットの推定位置(b)のような状態にすることが推定の目的

x

x

val

ue

of b(x

)

(a) representation of belief

b

(c) convinced and wrong

x

x

(b) convinced and right

(d) unknown

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 8 / 35

信念の更新（motion update）

ロボットが動いたら bも動かさないとズレが発生bの分布をロボットの動きに合わせて動かす

下式の左辺の bを新しい bの分布とする

b(x′) =

∫x∈X

paxx′b(x)dx

paxx′ : 状態遷移モデル「xで行動 aをとると x′ に到達する」という事象に対する確率密度関数状態方程式に相当

X

Y

X

Y

(a) actual motion of a robot (b) what the robot thinks

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 9 / 35

状態遷移モデルの作り方

動作指令（前ページの a）でどれだけロボットが変位するか実験で調査

X

Y“go foward, 1m”

pgo forward, 1m

(0,0) x’

留意点指令の意図通りにはロボットは動かず、変位にバイアスが発生動き出しと走行中で移動距離に相違

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 10 / 35

信念の更新（sensor update）

ロボットが情報（本発表では記号 dで表現）を得るとベイズの公式で bを更新可能

「情報 dを知ったら信念 bがどう変化するか？」を計算手続き的には下記の簡素な式で表現可能

b(x)←− q(d|x)b(x)/η尤度関数 q(d|x)

「xにいると仮定したときに dが得られる確率」に比例する関数「情報 d」という抽象的なものを数値化

(b) what the robot thinksX

Y

(a) observation in the real world

a landmark

measurement of distance(with some noise)

X

Y

high likelyhoodincrease of b(x)

b

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 11 / 35

尤度関数の一般的な作成方法

例: カメラでマーカまでの距離を計測

ロボットを様々な地点に置き、距離の計測値と実際の距離をプロット「ある計測値に対して、実際の距離がどのように分布するか？」を数式化すると尤度関数に

X

Y many measurements from various points

actual distance

mea

sure

ddis

tance

q(measured | actual)

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 12 / 35

ベイズフィルタによる自己位置推定まとめ

事前の作業状態遷移モデル paxx′ と尤度関数 q(d|x)をロボットに実装

オンライン時ロボットの最初の信念 bを準備

スタート位置が分かっているなら真の位置に収束したもの分からないなら一様分布

bを逐次更新ロボットが動いたら paxx′ で bを更新ロボットが何か知ったら q(d|x)で bを更新

今のところ bをあるがまま表現できる計算機は存在せず

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 13 / 35

3. パーティクルフィルタ

ベイズフィルタをノンパラメトリックに近似パラメトリック: カルマンフィルタ

信念 bの確率分布を多次元ガウス分布（のパラメータ）で表現

パーティクルフィルタは bを特定の数式でモデル化しない

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 14 / 35

信念状態の近似

状態空間にひろがる確率分布を、重み付きの点（パーティクル）を散布して近似

X

Y

θ

X

Y

θ

(a) a probability distribution (b) approximation

ξ(i) = (x

(i),w(i))

※図中の x (i) は、あるパーティクルの位置 (x, y, θ) を表す

厳密には何を近似しているのか？ある領域にロボットが存在する確率パーティクルの数が多いほど領域を狭くできる期待

P (A) =

N∑i=1

wiδ[x(i) ∈ A

]X

Y

θA

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 15 / 35

motion update

ロボットが移動する前のパーティクルから新しいパーティクルを生成（リサンプリング）リサンプリングの手順（N 回繰り返す）

...1 重みに比例する確率で一つパーティクルを選択

...2 パーティクルの位置から paxx′ に従って移動後の位置を選択

...3 移動後の位置に重み 1/N のパーティクルを作成

古いパーティクルは破棄

X

Y

X

Y

(a) actual motion of a robot (b) what the robot thinks

例: .. Link

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 16 / 35

リサンプリングでのパーティクルの選択

O(N)で済ますこと「等間隔サンプリング（系統サンプリング）」が一般的

パーティクルの持つ重みを累積した棒グラフを作る[0, 1)からランダムに ϵを選択矢印の先にあるパーティクルを選択

...w(1) w(2) w(3) w(N-1) w(N)

1

N

1

N

1

N

1

N

1

N

1

N...ε

N

1−ε

N

0 1p

ξ(1)

ξ(2)

ξ(3)

ξ(Ν−1)

ξΝ

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 17 / 35

sensor update

重みに尤度関数の値をかけて、重みの合計が 1になるように正規化

...1 w(i) ←− w(i)q(d|x(i))

...2 η ←−∑N

i=1 w(i)

...3 w(i) ←− w(i)/η

(b) what the robot thinksX

Y

(a) observation in the real world

a landmark

measurement of distance(with some noise)

X

Y

inclease of weights

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 18 / 35

4. パーティクルフィルタの「癖」

最大の問題: パーティクルの分布と自己位置が乖離頻繁に起こる

ここでは「◦◦手法を紹介」ではなく、以下を考察なぜ乖離するか乖離したらどうすべきか乖離が高頻度なときにどうするか

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 19 / 35

乖離する原因

パーティクルの数の不足

入ってくる情報が互いに独立していない

想定外の事象

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 20 / 35

パーティクルの数の不足

何に対して不足するか？→ 尤度の反映時尤度関数が鋭すぎると下図のようにロボットから遠いパーティクルに重みが偏ることがある

inclease of weights

(a) before (b) after

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 21 / 35

シミュレーションの例(b): (a)より使用しているセンサの精度が高い

one observationone step

one observationone step

(a)

(b)

all particles here

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 22 / 35

不足の対策

実装の工夫による計算量の抑制→パーティクルを増やす重み、その他変数を整数型で持って整数型で演算三角関数や乱数などを配列に書き出しておく高級言語を使わない

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 23 / 35

センサの精度より誤差を多く見積もった尤度関数を利用下図: あるシミュレーションで、センサの精度・尤度関数を変化させて自己位置推定の誤差を求めたもの（予稿から）

(a): X軸左に行くほどセンサの精度が高いが結果が悪化(b): 誤差を多めに見積もった尤度関数で結果の悪化を抑制

(a) use of likelyhood functions based on the preciseness of measurements

small

larg

e

(b) use of a relaxed likelyhood functionpreciseness of measurements

the likelyhood functionfor this condition is usedfor the others.

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 24 / 35

入ってくる情報の独立性

ベイズの定理を利用する前提条件時々刻々と得られる情報 d1, d2, d3, . . . が互いに独立例えば d1 と d2 の雑音に q(d|x)以外の関連性がないこと

ロボットの用途では多くの場合、独立性が期待できない距離センサで壁との距離を計測したら実際より短い測定結果→次の計測値も短い可能性ホワイトノイズとして扱っているものの背後に理由

影響極端に言えば、同じ情報に対して何度もベイズの定理を適用したことになって信念の妥当性が得られない

b(x)←− q(d|x)nb(x)/η

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 25 / 35

尤度関数で想定しない事象

尤度関数の作成時に考慮しなかった/できなかったことが発生例: ロボカップで起こった数々の「想定外」

照明の当てかたで画像処理結果に偏りランドマークの位置がルールブックとわずかに相違ランドマークと同色の服を着ている人の存在

影響慢性的な信念 bと真値とのずれ・間違い

考えられる対策「想定外」の除去（入念なチューニングと事前検証、露払い）限界がある

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 26 / 35

「正しい尤度関数」とは何だろうか？

想定されるものを全て考慮できるか？通行人、距離センサの計測対象の移動、照明条件、能動的な妨害、天変地異、大事故、· · ·

尤度関数を得るための実験でどうやって状況を再現する？頻度の極端に少ないものをどう考慮する？照明条件等、ゆるやかに変動する雑音の原因をどう扱う？

「正しい/正しくない」: 二つの統計学を混ぜてないか？伝統的な統計学（頻度主義・客観主義）ベイズ統計学（ベイズ主義・主観主義）

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 27 / 35

頻度主義とベイズ主義の違い

頻度主義: 無限回試行を行った場合の極限値が確率尤度関数のもととなる確率分布も実験の極限値という考え方

ベイズ主義: 「情報がない」ということに対して確率を定義可能

イカサマコインの例

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 28 / 35

尤度関数というものをどう扱うか

ベイズ主義で考える尤度も信念もロボット（とプログラマ）の主観

たとえ実験で尤度関数を求めたとしても、それは何らかの前提に基づいている

「正しい唯一無二の尤度関数」を追求してはいけない理由があれば改変してよい

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 29 / 35

実験で求められない事象を尤度関数に反映[Thrun2005]で言及下図: 距離センサの計測に通行人の存在を考慮

通行人の存在頻度は一定しない壁から遠い位置の尤度を適当に水増しする

x

q(d | x)

wall d

x

wall d

x

val

ue

of

q

q(d | x)

x

val

ue

of

q

(a) measurement of a wall (b) possibility of existence

of pedestrians

(c) likelyhood obtained

by experiments

(d) likelyhood with consideration

of pedestrians

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 30 / 35

一様分布の利用 [Ueda2002]

予測できない事象があってもおおかた成立する事柄から尤度関数を設計

例: 距離センサは（通行人を考慮すると）壁の距離以下の計測値を返すことがある

x

q(d | x)

wall d

x

wall d

x

val

ue

of

q

(a) measurement of a wall(b) possibility of existence

of pedestrians

(d) likelyhood obtained

by a thought experiment

x

wall d

(c) no possibility of a penetration

(caution: depending on the sensor)

情報の独立性を気にしなくてもよい何度同じ情報を入力しても信念 bの分布は不変（冪等性）

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 31 / 35

他の検討事項

適切に情報を間引く独立性を十分に確保できるが明確な基準が必要

信念が真値と乖離していると分かったら、信念の全部あるいは一部を棄却

信念はあくまで主観であって絶対的に正しいものではないリセット

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 32 / 35

リセット

信念と真値の乖離→もはやベイズの定理が有効でないベイズの定理以外の枠組みで対応

「リセット」の必要性リセット: なんらかの情報に基づいて信念を作り直すこと

no particle

乖離の認識ロボットからは乖離しているかどうかは分からないη =

∑Ni=1 w

(i) を求めたときに小さすぎる場合に乖離を疑う

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 33 / 35

主なリセット手法

名称等 具体的な方法 考え方推定やり直し η が閾値を下回ったら、

ランダムにパーティクルを再配置

情報をすべて捨てる

センサリセット[Lenser2000]

η が閾値を下回ったら、尤度関数 q(d|x)にしたがって再配置

直前の情報 d重視

膨張リセット[上田 2005]

η が閾値を下回ったら、bの分布を拡大

もとの信念 b重視

乖離が発生したときに何を使い回すと間違いが少ないか考慮して選択

例: .. Link

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 34 / 35

5. まとめ

パーティクルフィルタ: ベイズフィルタの実装

パーティクルフィルタが常に正確なことは期待しない情報の独立性を疑う大量に入力した情報には想定外の事象ベイズの定理以外に何か根拠があるのなら、積極的にリセットを行う

第 58 回システム制御情報学会研究発表講演会 2014 年 5 月 21 日 35 / 35