Recommended
More Related Content
What's hot
What's hot (20)
Similar to 確率的自己位置推定
Similar to 確率的自己位置推定 (20)
確率的自己位置推定
- 2. ロボットとは センサ マイコン アクチュ エータ PID制御 / ⾜足回り ⾃自⼰己位置推定
- 3. ⽬目次 1. 確率率率分布 2. ベイズの定理理 3. 確率率率的⾃自⼰己位置推定 4. カルマンフィルタ 〜～実践編〜～ 5. パーティクルフィルタ 6. 応⽤用 7. まとめ
- 4. 本⽇日のまとめ
- 8. 確率率率分布 おさらい
- 9. 確率率率分布 • 正規分布 1000999998 1001 1002 1000999998 1001 1002
- 12. ベイズの定理理 • なにが嬉しいか 1. 結果から原因を推定できる 2. 情報を得ることによって変化する確率率率を扱える
- 13. ベイズの定理理 • Θ (パラメータ)： 隠された真実（状態／⾃自⼰己位置） • D (データ） ： 観測結果
- 14. 条件付き確率率率 • P(A|B) ：Bという条件の下での、Aの確率率率 • Bというパラメータでの、Aの確率率率 • Bという情報を知ったあとでの、Aの確率率率
- 15. 尤度度 • Θというパラメータでの、Dの確率率率 – どんな値が観測されやすいか？ – 現在の状態によって変わる – 既にわかってる 観測値 確率率率 Θ=2のとき Θ=1のとき
- 16. 事後確率率率 • Dという情報を知ったあとでの、Θの確率率率 – どんな状態っぽいか？ – 観測値によって変わる – 分からない。推定したい！ 状態 確率率率 D=3のとき D=10のとき
- 17. 因果を遡れる！ • データの⽣生成過程が分かれば、 • ⽣生成される時に使われたパラメータを推定できる。 原因 結果 観測結果 隠された真実 推定 尤度度 P(D|Θ) 事後確率率率 P(Θ|D) cf. 機械学習 Θ D
- 18. 例例 • この場所でこの⽅方向を向いていると、 • こんな景⾊色が⾒見見える • こんな景⾊色が⾒見見えるなら、 • この場所でこの⽅方向を向いてそう – この場所+この⽅方向 ：Θ パラメータ／原因 – こんな景⾊色 ：D データ／結果 尤度度 P(D|Θ) 事後確率率率 P(Θ|D)
- 19. モンティ・ホール問題 A B C A? A ヤギ C A or C?
- 20. ベイジアンの解答 ⾃自分が選んだカーテン: A 司会者の開けたカーテン: X • P(Aが正解|X=B) = P(X=B|Aが正解) P(Aが正解) / P(X=B) = 1/2 1/3 / P(X=B) = 1/3 • P(Cが正解|X=B) = P(X=B|Cが正解) P(Cが正解) / P(X=B) = 1 1/3 / P(X=B) = 2/3 情報 事前分布 事後分布
- 21. 確率率率が変化する！ • 情報を得ることによって主観確率率率が変わる 事前確率率率： Dを知る前のΘの確率率率 事前確率率率事後確率率率 ⼀一度度ベイズ推定して得られた事後分布を、 次のベイズ推定の事前分布として使う！ ベイズ推定を繰り返して主観確率率率を更更新していく ベイズフィルタ
- 22. フィルタって？ • Predic)on 予測 時間 cf. ローパスフィルタ • Filtering 瀘波 • Smoothing 平滑滑化 ×スパムメールフィルタ ところで 現在 → 未来 現在 → 現在 過去 ← 現在
- 23. ベイズの定理理 • なにが嬉しいか 1. 結果から原因を推定できる 2. 情報を得ることによって変化する確率率率を扱える （フィルタリング）
- 25. ⾃自⼰己位置推定 • ベイズフィルタ ＋ ⾃自⼰己位置推定 ＝ 確率率率的⾃自⼰己位置推定 • ベイズの定理理があっても、近似なしでは無理理！ 1. 確率率率分布をどう表現するか 2. 確率率率分布をどう更更新していくか 3. 計算量量をどう抑えるか • 考えられた２つの近似⽅方法 1. 単純化して綺麗麗にとく ＝ カルマンフィルタ 2. 真実を追究するために⼒力力技で ＝ パーティクルフィルタ
- 26. ⾃自⼰己位置推定 ロボットの動作をモデル化 1. 状態遷移 2. 観測 アルゴリズム 1. 予測（時間を進める） 2. 更更新（ベイズ推定） 観測(t+1) 状態(t) 観測 状態(t+1)状態(t-‐1) 観測(t+1)観測(t+1) 状態遷移 ベイズ推定
- 27. カルマンフィルタ • カルマンフィルタだと、 – すべての分布を正規分布で考える – 状態モデルと観測モデルは線形 ⼀一番簡単な、 • 状態空間と観測空間が同じで１次元の場合 例例えば、1⽅方向にだけ平⾏行行移動+ポテンショ 確率率率考えないなら、 「 ⾃自⼰己位置 ＝ ポテンショの値 」 確率率率を考えると ・・？
- 28. モデル化 1. 最初は x = 0m の地点にいる 2. 右に5m進むぐらいモータ回した – ⼊入⼒力力 u1 = 5 3. ポテンショはx = 2mだと⾔言っている – 観測値 z1 = 2 ノイズ (正規分布) ⼊入⼒力力 状態遷移モデル 観測モデル x0 = 0 x1 = 0 + 5 + (-‐‑‒2) = 3 こうなってたかも？： z1 = 3 + (-‐‑‒1) = 2
- 29. t-‐1での推定値 平均 0 分散 0 tでの推定値 平均 5 分散 2⼊入⼒力力 平均 5 分散 2 予測ステップ • 状態遷移モデルから導く – 分散が必ず増⼤大する 平均 分散
- 30. • 観測モデルを尤度度として、ベイズ推定 – 平均は内分点 – 分散は必ず減少 1 -‐ k : k ただし、 予測値 平均 5 分散 2 観測値 平均 2 分散 1 推定値 平均 3 分散 2/3 更更新ステップ (k = 2/3 )
- 31. ⼀一般の場合 1. 状態遷移に係数がつく場合 – 前の状態と⼊入⼒力力でどう次が決まるか 2. 状態空間と観測空間が異異なる場合 – ラインセンサで直接位置を⾒見見れるわけではない 3. 多次元の場合 – 直線でなく平⾯面を動く 4. (⾮非線形の場合)
- 32. カルマンフィルタ やってみよう
- 33. ⼿手順 1. 状態変数を決める 2. 状態遷移モデルを⽴立立てる 3. 観測モデルを⽴立立てる 4. ⾮非線形ならヤコビ⾏行行列列を求める 5. 測定誤差の分散を決める 6. 式に代⼊入してプログラム書く 7. 実際に動かす・可視化する
- 34. 制御対象 • NHK2016のエコロボット – 対向２輪輪 – センサ • ラインセンサ • 左右エンコーダ – アクチュエータ • 前輪輪の向きを変えるサーボモータ • ⽬目的 フィールドの情報は予め知っているとして、 ２つのセンサで、⾃自⼰己位置推定する →⾃自⼰己位置に基づいて進⾏行行⽅方向を変える
- 35. 1. 状態変数を決める • 普通は の３次元ベクトル • 状態モデルを⼒力力学的に考えるなら の6次元ベクトル
- 36. 2. 状態遷移モデル １つ前の状態から次の状態求める式 (動作モデル) エンコーダを観測に含めると、状態変数の次元が増えちゃう → エンコーダの観測値を⼊入⼒力力として扱う
- 38. 4. ⾮非線形ならヤコビ⾏行行列列を求める 線形システムにしか扱えないカルマンフィルタ ＋ 線形近似 ↓ 拡張カルマンフィルタ ⾮非線形
- 39. 線形近似 テイラー展開して1次までだけ採⽤用 微分係数：接線の傾き ヤコビ⾏行行列列：接平⾯面の⽅方向 微分するだけ・・・
- 40. 4. ⾮非線形ならヤコビ⾏行行列列を求める 線形システムにしか扱えないカルマンフィルタ ＋ 線形近似 ↓ 拡張カルマンフィルタ ⾮非線形 線形
- 41. 5. 誤差のモデル化 • 誤差の正規分布の平均は0、分散は? – センサの値のばらつき具合 • どのセンサを、どのくらい信頼するか • 適当でも案外うまくいく – 実際に測ったら、より正確なモデルになる
- 42. 6. 式に代⼊入してプログラムを書く カルマンフィルタの式に計算した⾏行行列列を代⼊入 C++ならMatrixクラスなどで⾏行行列列を扱える Matrix A(3,3); A << 1 << 0 << 0 << 0 << 1 << 0 << 0 << 0 << 1; A.transpose(); //転置 A.inverse(); //逆⾏行行列列 F*X*F.transpose() (標準では⼊入ってない)
- 43. 7. 可視化 • ちゃんと⾃自⼰己位置推定するなら可視化は必須 • ROSで簡単にできるっぽい
- 44. 誤差楕円 • 収束具合も確認したい →誤差楕円 「99%の確率率率でこの楕円の中にいる」 • 2次元正規分布(θは無視)の分散共分散⾏行行列列 • 固有値２つ – ⼤大きい⽅方が⻑⾧長半径、⼩小さいほうが短半径 • 固有ベクトル２つ – ⻑⾧長軸と短軸⽅方向 可視化のポイント
- 46. カルマンフィルタの問題点 1. ガウス分布でないといけない – 実際そんなに単純化していいのか？ • 例例：カメラ, LRF 2. 線形システムしか扱えない – ヤコビ⾏行行列列で線形化したらできたのでは？ – ⾮非線形性が強いと誤差が⼤大きくなってしまう
- 47. 確率率率分布の表し⽅方 1. カルマンフィルタでは – 正規分布で近似 – 正規分布は2つのパラメータで完全に決まる 2. パーティクルフィルタでは – ⽬目的の分布からのたくさんのサンプルで近似 – パラメータはサンプルの数だけ必要 • 確率率率の⾼高いところにサンプルが多い • 確率率率の低いところはサンプル少ない
- 48. モンテカルロ法 • 乱数を使うアルゴリズムのこと • 例例：積分
- 49. パーティクルフィルタ • モンテカルロ位置推定 • 粒粒⼦子（パーティクル）の集合で確率率率分布を近似 – 状態空間上の点⼀一つ⼀一つが候補を表す 1. 予測 – 状態遷移モデル 2. 重み計算 – 観測モデルで尤度度計算 3. リサンプリング (世代交代) – 重み付きサンプリング
- 50. モデル化 1. 最初は x = 0m の地点にいる 2. 右に5m進むぐらいモータ回した – ⼊入⼒力力 u1 = 5 3. ポテンショはx = 2mだと⾔言っている – 観測値 z1 = 2 ノイズ (正規分布) ⼊入⼒力力 状態遷移モデル 観測モデル x0 = 0 x1 = 0 + 5 + (-‐‑‒2) = 3 こうなってたかも？： z1 = 3 + (-‐‑‒1) = 2
- 51. 予測 • パーティクルごとにノイズものせてシミュレー ション – もとの分布より広がる
- 52. 尤度度計算 • 各パーティクルの尤度度を求める – このパーティクルの重み ／重要度度 – パーティクルの⽴立立場から、実際の観測値が得られる確率率率
- 53. • 重み付きサンプリング – 各パーティクルの重みに⽐比例例してサンプリング – 重みの⼤大きいパーティクルは何回も選ばれる – 重みの⼩小さいパーティクルは滅多に選ばれない • 新しいパーティクルで前のパーティクルを置き換える リサンプリング 本当の分布からサンプリングしたいけど サンプルからさらにサンプリング
- 54. 推定値の計算 • カルマンフィルタでは、平均は必ず計算されている • パーティクルフィルタでは、パーティクルから改めて計 算する必要がある • 確率率率分布の平均は積分 – モンテカルロ積分
- 55. 特徴 • どんな分布でも表せる • どんな複雑なモデルでも順⽅方向だけ簡単にできれば良良い – 状態遷移モデルでサンプリング – 観測モデルで尤度度計算 • ベイズの定理理との対応が分かりやすい • ヤコビ⾏行行列列や分散を考えなくていい • パーティクル数が多く必要 (1000とか)
- 56. V.S. 拡張カルマンフィルタ • ガウス分布のみ… • 計算速い！ • 実装が⾯面倒 • (理理解しづらい) パーティクルフィルタ • 任意の分布！ • 計算遅い… • 実装が簡単 • (理理解しやすい) Ensemble カルマンフィルタ Unscented カルマンフィルタ ⼀一⼈人の気難しい天才 みんなで ⼒力力をあわせてがんばろー！
- 57. Ensembleカルマンフィルタ • カルマンフィルタとパーティクルフィルタの折衷案 • パーティクルフィルタほど精度度⾼高くないが、 • 少ないパーティクル数(〜～100)で済むため、計算速い • 次元数がかなり⾼高いとき真価を発揮 • 次元数低かったらパーティクルフィルタでも⼗十分速い • らしい。
- 58. 発展編 こんなこともできるよ！
- 59. SLAM • Simultaneous Localiza)on and Mapping • 同時 ⾃自⼰己位置推定&地図⽣生成
- 60. Visual SLAM • 景⾊色⾒見見ながら歩き回って、地図できてた • hhps://youtu.be/oJt3Ln8H03s • hhps://youtu.be/8DISRmsO2YQ • ⼈人間みたい・・? いやいや、
- 61. Bayesian Brains • 実は脳(⼤大脳新⽪皮質)もベイズ推定してるかもしれない – 不不確実な世界に対処するために、予測とその誤差による修正は 必須 – Predic)ve Coding仮説 ただし、 • 観測モデル・状態モデルも更更新していく (学習) – 世界に対する認識識を改めていく
- 62. 本⽇日のまとめ
- 63. まとめ • 因果を遡り、 • 主観確率率率の変化を計算するベイズ推定 • それを繰り返すベイズフィルタ • 線形近似・ガウス近似して厳密に解くカルマンフィルタ • たくさんシミュレーションして数値的に解くパーティク ルフィルタ • それらの⾃自⼰己位置推定などへの応⽤用
- 67. ロボットとは センサ マイコン アクチュ エータ PID制御 / ⾜足回り ⾃自⼰己位置推定
- 69. 参考 • ベイズ論論vs頻度度論論 hhp://qiita.com/MoriKen/items/09da26466c00500bcd68 • 1次元KF図解 hhp://qiita.com/MoriKen/items/0c80ef75749977767b43 • EnKFについて hhp://qiita.com/g_rej55/items/618778dbb4bf286a985b • ベイズ推定とパーティクルフィルタ hhp://www.ic.is.tohoku.ac.jp/~swk/lecture/ic2010/ kagami_ic20100803.pdf • 上⽥田隆⼀一(訳)「確率率率ロボティクス」毎⽇日コミュニケーション ズ • ⽚片⼭山徹「応⽤用カルマンフィルタ」朝倉書店 • 有本卓「カルマンフィルター」産業図書