Upgrade to Pro — share decks privately, control downloads, hide ads and more …

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

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

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

114d1b33c50cdc86c4c641682190407f?s=128

Ryuichi Ueda

May 18, 2022
Tweet

More Decks by Ryuichi Ueda

Other Decks in Technology

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