2022年度ロボットフロンティア第2回

ベイズの定理，ベイズフィルタ，カルマンフィルタ，パーティクルフィルタ

Transcript

1. 確率ロボティクス入門 第2回 千葉工業大学 上田隆一 2022年5月18日 @中部大学
2. 前回のおさらい • ロボットの自己位置推定: 尤度𝐿𝐿(𝒙𝒙|𝐳𝐳)をうまく設計すると 実世界で機能 • センサ遮りに大きく影響されないように • 最悪の状況でも「ここにはいないだろう」という場所の尤度を低く • まだ残る問題: ひとつの尤度だけでは自己位置が求まらない 2022 5 18 年 月 日 @ ロボットフロンティア 中部大学 2 尤度場 X 壁 L 5m 尤度L レーザーの遮りを考慮した尤度
3. 尤度からの自己位置推定 • どうするか？: 下図のような操作を繰り返す 2022 5 18 年 月 日 @ ロボットフロンティア 中部大学 3 X Y このへん にいそう 動くと位置が 不明確に このへん が高尤度 ロボット が移動 センシング 情報の統合 戻る  この操作: 確率の計算
4. 確率分布 • 将来のことで/隠れていて分からないことについて， 「何が起こりそうか/起こっていそうか」を表す関数やグラフ • 例: 次に出るさいころの目の予想 • 次に出る目を𝑥𝑥と表しましょう • イカサマがなければ「次」を仮に 何回もやると6回に1回出る →𝑃𝑃 𝑥𝑥 = 1/6と表現 • 分布はさいころに対する我々の知識 • 次の目は分からないけど，傾向は分かる 2022 5 18 年 月 日 @ ロボットフロンティア 中部大学 4 1 2 3 4 5 6 𝑥𝑥 𝑃𝑃 1/6 妥当であろうと思われる確率分布
5. 「分布は知識」 • 確率分布は客観的なものだけではない • 例: 賭場のサイコロに細工をして1を出やすくした場合 • 胴元は右上の分布を頭に描く • 客は怪しまない限り右下の分布を頭に描く • 客の考えは情報がないだけで誤りではない • でも賭けに負ける可能性が高い（期待値の計算を誤る） • 例からわかること • 同じ物事に対しても各個人は別々の分布を頭に描く • どっちが正しいということはない • 情報を取り入れることで，より判断に有効な分布に 2022 5 18 年 月 日 @ ロボットフロンティア 中部大学 5 1 2 3 4 5 6 𝑥𝑥 𝑃𝑃 1 2 3 4 5 6 𝑥𝑥 𝑃𝑃 1/6 客の頭の中 胴元の頭の中
6. ベイズの定理 • 知識をアップデートするための定理 • 何かに対して𝑃𝑃 𝑋𝑋 と考えていたら情報𝑌𝑌が得られて𝑃𝑃 𝑋𝑋|𝑌𝑌 に • 確率の乗法定理(確率ロボティクスでは超重要)から導出 • 𝑃𝑃 𝑋𝑋, 𝑌𝑌 = 𝑃𝑃 𝑋𝑋 𝑌𝑌 𝑃𝑃(𝑌𝑌) • 𝑃𝑃 𝑋𝑋, 𝑌𝑌 = 𝑃𝑃 𝑌𝑌 𝑋𝑋 𝑃𝑃(𝑋𝑋) • 2式の右辺を=で結んで整理: 𝑃𝑃 𝑋𝑋 𝑌𝑌 = 𝑃𝑃(𝑌𝑌|𝑋𝑋)𝑃𝑃(𝑋𝑋) 𝑃𝑃 𝑌𝑌 = 𝜂𝜂𝑃𝑃 𝑌𝑌 𝑋𝑋 𝑃𝑃 𝑋𝑋 • 𝜂𝜂 = 𝑃𝑃(𝑌𝑌)−1は𝑌𝑌が確定しているので定数 • 𝑃𝑃(𝑌𝑌|𝑋𝑋): 尤度関数𝐿𝐿(𝑋𝑋|𝑌𝑌)とみなせる • 情報𝑌𝑌に対して尤もらしい𝑋𝑋の確率が𝑃𝑃(𝑋𝑋)から𝑃𝑃 𝑋𝑋 𝑌𝑌 に増加 • 妥当な尤度関数があれば知識をアップデートできる 2022 5 18 年 月 日 @ ロボットフロンティア 中部大学 6
7. ベイズの定理による サイコロの知識のアップデート • 例 • 各出目は𝑃𝑃 𝑝𝑝1 , 𝑝𝑝2 , … , 𝑝𝑝6 で出ると思っている • サイコロを振ったら𝑥𝑥new が出た • ベイズの定理で知識を更新 • 𝑃𝑃 𝑝𝑝1 , 𝑝𝑝2 , … , 𝑝𝑝6 |𝑥𝑥new = 𝜂𝜂𝑃𝑃 𝑥𝑥new |𝑝𝑝1 , 𝑝𝑝2 , … , 𝑝𝑝6 𝑃𝑃 𝑝𝑝1 , 𝑝𝑝2 , … , 𝑝𝑝6 = 𝐿𝐿 𝑝𝑝1 , 𝑝𝑝2 , … , 𝑝𝑝6 |𝑥𝑥new 𝑃𝑃 𝑝𝑝1 , 𝑝𝑝2 , … , 𝑝𝑝6 • 𝑥𝑥new として1が高頻度で出る → 𝑝𝑝1 の確率分布の中心値が大きくなっていく →グラサイだという疑惑 2022 5 18 年 月 日 @ ロボットフロンティア 中部大学 7 1 2 3 4 5 6 𝑥𝑥 𝑃𝑃 1/6 分布に不確かさ 疑り深い人ほど幅が大きい 𝑃𝑃 𝑝𝑝1 , 𝑝𝑝2 , … , 𝑝𝑝6 1 2 3 4 5 6 𝑥𝑥 𝑃𝑃 だんだん形状が 近づいていく
8. 自己位置に関する確率分布 • さいころの分布と違うところ • 多次元 • 平面で考える場合 𝒙𝒙 = (𝑥𝑥, 𝑦𝑦, θ) （頭で考えるときはとりあえずθを無視） • 連続値 • 足し算で1ではなくて積分で1 • 等高線のような形状 • 様々な出来事で変化 • 同じところ • 「知識」の表現であること • 「信念分布」と呼ばれる • ロボットは自身の位置を知らないけど，傾向について見解（信念）を持つ 2022 5 18 年 月 日 @ ロボットフロンティア 中部大学 8 自著「詳解確率ロボティクス」より転載
9. 自己位置に対する知識の推移1 • 「最初」の自己位置: 𝑝𝑝 𝒙𝒙 • 一様分布に • なにも知らない状態 • 多くの場合，ロボットは初期位置𝒙𝒙0 を教えられる • 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 • ちなみに「 |」のある確率（分布） を条件付き確率と言う • このあと，動いたりセンシング したりで「 |」の右側に情報が 増えていく • 次ページ 2022 5 18 年 月 日 @ ロボットフロンティア 中部大学 9 X X p 分布𝑝𝑝 𝒙𝒙 |𝒙𝒙0 X p 分布𝑝𝑝 𝒙𝒙 実世界
10. 自己位置に対する知識の推移2 • 次の時刻にロボットが動く • ロボットが得る情報: 自分の出したモータの出力 𝒖𝒖1 • →分布: 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 , 𝒖𝒖1 • 移動後，センシング • ロボットが得る情報:センサ情報 𝐳𝐳1 • →分布: 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 , 𝒖𝒖1 , 𝐳𝐳1 • 以後，繰り返し • 分布: 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 , 𝒖𝒖1 , 𝐳𝐳1 , 𝒖𝒖2 , 𝐳𝐳2 , 𝒖𝒖3 , 𝐳𝐳3 , … • 略記: 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡 • 𝑏𝑏という記号を与えましょう • 𝑏𝑏0 𝒙𝒙 = 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 • � 𝑏𝑏𝑡𝑡 𝒙𝒙 = 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡−1 • 𝑏𝑏𝑡𝑡 𝒙𝒙 = 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡 • 𝑏𝑏: belief（信念）の頭文字 2022 5 18 年 月 日 @ ロボットフロンティア 中部大学 10 注意: 単にどう表記するか という話しかしていない
11. 移動前の信念分布 移動後の信念分布 移動に関する計算（の前に図解） • ロボットが移動すると分布も移動 • 下: 1次元の例 • 右: 2次元平面の例 2022 5 18 年 月 日 @ ロボットフロンティア 中部大学 11 「詳解確率ロボティクス」[上田2019]より転載，改変 （細かい説明はここでは無視しましょう） X X p 移動 移動 想定される動きの雑音 にあわせて分布を拡大
12. 移動に関する計算 • 𝑏𝑏𝑡𝑡−1 𝒙𝒙 から� 𝑏𝑏𝑡𝑡 𝒙𝒙 を導出 • 𝑏𝑏𝑡𝑡−1 𝒙𝒙 = 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡−1 , 𝐳𝐳1:𝑡𝑡−1 • � 𝑏𝑏𝑡𝑡 𝒙𝒙 = 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡−1 • 次のように変形 • � 𝑏𝑏𝑡𝑡 𝒙𝒙 = 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡−1 = 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡−1 , 𝐳𝐳1:𝑡𝑡−1 , 𝒖𝒖1:𝑡𝑡 = 𝑝𝑝 𝒙𝒙 |𝑏𝑏𝑡𝑡−1 , 𝒖𝒖𝑡𝑡 = 𝑝𝑝 𝒙𝒙 |𝒙𝒙′, 𝒖𝒖𝑡𝑡 𝑏𝑏𝑡𝑡−1(𝒙𝒙′) = ∫ 𝑝𝑝 𝒙𝒙 |𝒙𝒙′, 𝒖𝒖𝑡𝑡 𝑏𝑏𝑡𝑡−1 𝒙𝒙′ 𝑑𝑑𝒙𝒙′ • は期待値 • ここらへんはちゃんと説明してなくてスミマセン • 計算に必要な情報: 状態遷移モデル 𝑝𝑝 𝒙𝒙 |𝒙𝒙′, 𝒖𝒖 • 𝒙𝒙′でモータの出力が𝒖𝒖だったら次の時刻にどこにいくかを示す確率分布 • 実験や経験から求めておく 2022 5 18 年 月 日 @ ロボットフロンティア 中部大学 12 𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡−1 , 𝐳𝐳1:𝑡𝑡−1 の情報は 𝑏𝑏𝑡𝑡−1 に集約されている
13. センサ情報（以後「観測」）に関する計算 • 計算: � 𝑏𝑏𝑡𝑡 𝒙𝒙 から𝑏𝑏𝑡𝑡 𝒙𝒙 を導出 • � 𝑏𝑏𝑡𝑡 𝒙𝒙 = 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡−1 • 𝑏𝑏𝑡𝑡 𝒙𝒙 = 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡 • 増えた情報𝐳𝐳𝑡𝑡 を利用して ベイズの定理を適用 • 𝑏𝑏𝑡𝑡 𝒙𝒙 = � 𝑏𝑏𝑡𝑡 𝒙𝒙|𝐳𝐳𝑡𝑡 = η𝑝𝑝 𝐳𝐳𝑡𝑡 |𝒙𝒙 � 𝑏𝑏𝑡𝑡 𝒙𝒙 = η𝐿𝐿 𝒙𝒙|𝐳𝐳𝑡𝑡 � 𝑏𝑏𝑡𝑡 𝒙𝒙 2022 5 18 年 月 日 @ ロボットフロンティア 中部大学 13 [上田2019]より転載 （細かい説明はここでは無視しましょう）
14. これまでのまとめ • 導出は難しいが，以下を知っていると 自己位置推定の計算が可能 • 初期値𝑏𝑏0 𝒙𝒙 = 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 を準備 • 𝒙𝒙0 がない場合もある • 以下，繰り返し • ロボットが動く: � 𝑏𝑏𝑡𝑡 𝒙𝒙 = 𝑝𝑝 𝒙𝒙 |𝒙𝒙′, 𝒖𝒖𝑡𝑡 𝑏𝑏𝑡𝑡−1(𝒙𝒙′) • ロボットがセンシング: 𝑏𝑏𝑡𝑡 𝒙𝒙 = η𝐿𝐿(𝒙𝒙|𝐳𝐳𝑡𝑡 )� 𝑏𝑏𝑡𝑡 𝒙𝒙 2022 5 18 年 月 日 @ ロボットフロンティア 中部大学 14 どうやってプログラムにするのか？ 「ベイズフィルタ」 と呼ばれる手続き
15. 2次元のガウス分布 by Piotrg~commonswiki https://commons.wikimedia.org/w iki/File:Multivariate_Gaussian.png CC BY-SA 3.0 カルマンフィルタによる自己位置推定 • 多変量「ガウス分布」に確率分布を制限して ベイズフィルタを実装 • ガウス分布: 正規分布とも呼ばれる • 信念の表現 • 𝑏𝑏𝑡𝑡 𝒙𝒙 = exp[− 1 2 𝒙𝒙𝑡𝑡 − 𝝁𝝁𝑡𝑡 ⊤Σ𝑡𝑡 −1 𝒙𝒙𝑡𝑡 − 𝝁𝝁𝑡𝑡 ] • 適切な近似で指数部の計算だけに • 移動時: � Σ𝑡𝑡 = 𝐹𝐹𝑡𝑡 Σ𝑡𝑡−1 𝐹𝐹𝑡𝑡 ⊤ + 𝐴𝐴𝑡𝑡 𝑀𝑀𝑡𝑡 𝐴𝐴𝑡𝑡 ⊤ • 観測時: • Σ𝑡𝑡 = (𝐼𝐼 − 𝐾𝐾𝑡𝑡 𝐻𝐻𝑡𝑡 )� Σ𝑡𝑡 • 𝝁𝝁𝑡𝑡 = 𝐾𝐾𝑡𝑡 𝒛𝒛𝑡𝑡 − 𝒉𝒉 � 𝝁𝝁𝑡𝑡 + � 𝝁𝝁𝑡𝑡 • ここで𝐾𝐾𝑡𝑡 = � Σ𝑡𝑡 𝐻𝐻𝑡𝑡 ⊤(𝐻𝐻𝑡𝑡 � Σ𝑡𝑡 𝐻𝐻𝑡𝑡 ⊤ + 𝑄𝑄𝑡𝑡 )−1 2022 5 18 年 月 日 @ ロボットフロンティア 中部大学 15
16. カルマンフィルタによる自己位置推定の例 • 左: 観測なし • 移動によって位置・向きの不確かさが増加していく • 右: ★の向きと距離を計測して信念分布に反映 2022 5 18 年 月 日 @ ロボットフロンティア 中部大学 16 https://www.youtube.com/watch?v=aNzGCMRnvXQ
17. カルマンフィルタの限界 • うまく実装すれば機能するが・・・ • 限界が存在 • 向きを持つロボットが移動する際には， 信念分布の形状はガウス分布にならない • 尤度関数もガウス分布と 同じような形状にならない • 「複数の候補」が表現できない • ロボットのいる環境は空中と違って複雑 2022 5 18 年 月 日 @ ロボットフロンティア 中部大学 17 [上田2019]より [上田2019]より
18. 分身を使った自己位置推定 • 右のようなシミュレーションでできる図を そのまま利用できないか？ • いくつもロボットの分身を動かす • 分身の分布（偏在具合）を 信念分布の近似とみなす • 移動に関する具体的な操作 • 初期の信念分布（位置・向きを教えてもらえる場合）: • 分身を教えてもらった位置・向きに配置 • 移動: 状態遷移モデル 𝑝𝑝 𝒙𝒙 |𝒙𝒙′, 𝒖𝒖𝑡𝑡 にしたがって動かす 2022 5 18 年 月 日 @ ロボットフロンティア 中部大学 18
19. 数学的な根拠 • 「ドローイング」 • 確率分布にしたがってひとつ値を取り出す行為 • 𝑥𝑥~𝑃𝑃(𝑥𝑥) • ドローイングを繰り返して集めたデータ（標本，サンプル）は， 元の分布の近似 • 例: さいころを何度も振って出目の統計をとると， 頻度の割合が1/6に近づく サンプルで元の分布を近似可能 2022 5 18 年 月 日 @ ロボットフロンティア 中部大学 19 1 2 3 4 5 6 𝑥𝑥 𝑃𝑃 1/6 つまり𝑥𝑥~𝑃𝑃(𝑥𝑥)の繰り返し 何度（ 𝑁𝑁 回としましょう） もさいころを振って各目の 出た回数を記録 1 2 3 4 5 6 𝑥𝑥 頻度 𝑁𝑁/6
20. 移動における信念分布の近似 • p.18下の手続きは近似になっているだろうか？ • 近似対象: • 初期値 𝑏𝑏0 𝒙𝒙 = 𝑝𝑝 𝒙𝒙 |𝒙𝒙0 • 移動による分布の変化 � 𝑏𝑏𝑡𝑡 𝒙𝒙 = 𝑝𝑝 𝒙𝒙 |𝒙𝒙′, 𝒖𝒖𝑡𝑡 𝑏𝑏𝑡𝑡−1(𝒙𝒙′) • 初期値の分布 • 𝒙𝒙0 に分身を固めて置く手続き: 𝑏𝑏0 からのドローイングと考えると妥当 • 移動 • やっていることは𝒙𝒙 ~𝑝𝑝 𝒙𝒙 |𝒙𝒙′, 𝒖𝒖𝑡𝑡 の操作 • 𝒙𝒙′: 𝑏𝑏𝑡𝑡−1 を近似する各分身（=サンプル）の位置・向き • 𝑏𝑏𝑡𝑡−1 で𝒙𝒙′が分布しているときの𝑝𝑝 𝒙𝒙 |𝒙𝒙′, 𝒖𝒖𝑡𝑡 の分布（つまり� 𝑏𝑏𝑡𝑡 𝒙𝒙 ）を近似 2022 5 18 年 月 日 @ ロボットフロンティア 中部大学 20
21. 尤度関数の反映 • 各分身に尤度の変数を持たせる • この場合の尤度: 各分身の 位置・向きの尤もらしさ • そのまま尤度関数の考え方が適用可能 • 演算 • ある分身の尤度を𝑤𝑤とする • 尤度ではあるが「重み」と表現 • センサ情報が入ると，次のように更新 • 𝑤𝑤 ⟵ 𝑤𝑤𝑤𝑤(𝒙𝒙|𝐳𝐳) 2022 5 18 年 月 日 @ ロボットフロンティア 中部大学 21 分身1 14点（黒: 6，灰: 2） 分身2 7点（黒: 1，灰: 5） 分身1のほうが，分身2より 2倍「尤もらしい」と考える． （前回の図とほぼ同じ）
22. 観測を重ねることによる重みの変化 • 尤もらしい分身は，次の観測でも高い尤度になりやすい →ほとんどの分身の重みが相対的に0に 2022 5 18 年 月 日 @ ロボットフロンティア 中部大学 22 例: 矢印の長さが重み →1つの分身の重みだけ 大きくなってあとはゼロ
23. リサンプリング • 重みの大きい分身を増やす • 方法 • パーティクルの重みを積み上げたリストを作成 • 1/𝑁𝑁ずつリストを進んで分身を𝑁𝑁個コピー • コピーで作った分身の重みを1/𝑁𝑁にして新たな分身の集団を作成 • 元の分身の集団は破棄 2022 5 18 年 月 日 @ ロボットフロンティア 中部大学 23 [上田2019]より 公平になるように，乱数で 開始位置を少しずらす
24. 分身を使った自己位置推定の完成 • 別名: • パーティクルフィルタ，粒子フィルタ（粒子: 分身のこと） • MCL（Monte Carlo localization） 2022 5 18 年 月 日 @ ロボットフロンティア 中部大学 24 https://www.youtube.com/watch?v=mGkNZUbasXo より．https://github.com/ryuichiueda/emclを使用
25. 分身を使った自己位置推定 →パーティクルフィルタ • 𝑁𝑁個あるうちの𝑖𝑖番目のパーティクルを次のように表現 • 𝜉𝜉𝑡𝑡 (𝑖𝑖) = (𝒙𝒙𝑡𝑡 𝑖𝑖 , 𝑤𝑤𝑡𝑡 (𝑖𝑖)) （𝑖𝑖 = 0, 1, 2, … , 𝑁𝑁 − 1） • 手続き • 移動時: 𝒙𝒙𝑡𝑡 𝑖𝑖 ~𝑝𝑝 𝒙𝒙 |𝒙𝒙𝑡𝑡−1 𝑖𝑖 , 𝒖𝒖𝑡𝑡 • 観測時: 𝑤𝑤𝑡𝑡 (𝑖𝑖) ⟵ 𝑤𝑤𝑡𝑡 (𝑖𝑖)𝐿𝐿(𝒙𝒙𝑡𝑡 𝑖𝑖 |𝐳𝐳𝑡𝑡 ) • その後，移動直前にリサンプリング 2022 5 18 年 月 日 @ ロボットフロンティア 中部大学 25
26. まとめ • 確率分布を各自の知識と考える • 人工知能（つまり主観を持つ何か）を考えるうえで必須と考えられる • ベイズの定理 • 知識のアップデート方法 • 尤度関数を使った自己位置推定（ベイズフィルタ） • 2種類の実装を紹介（いずれも近似計算） • カルマンフィルタ • パーティクルフィルタ 2022 5 18 年 月 日 @ ロボットフロンティア 中部大学 26