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

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

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

2022/5/25 @ 中部大学

SLAMについて

114d1b33c50cdc86c4c641682190407f?s=128

Ryuichi Ueda

May 25, 2022
Tweet

More Decks by Ryuichi Ueda

Other Decks in Technology

Transcript

  1. 確率ロボティクス入門 第3回 千葉工業大学 上田隆一 2022年5月25日 @中部大学

  2. 前回のおさらい • 確率分布は知識 • 信念𝑏𝑏 𝒙𝒙 • 情報を取り入れる計算: ベイズの定理 •

    𝑏𝑏 𝒙𝒙|𝐳𝐳 = η𝑝𝑝 𝐳𝐳|𝒙𝒙 𝑏𝑏 𝒙𝒙 • 𝒙𝒙が変化すると𝑏𝑏 𝒙𝒙 も変化(状態遷移) • 𝑏𝑏 𝒙𝒙|𝒖𝒖 = 𝑝𝑝 𝒙𝒙 |𝒙𝒙′, 𝒖𝒖 𝑏𝑏(𝒙𝒙′) • 上記の計算: ベイズフィルタ • 近似して実装すると自己位置推定が可能に 2022 5 25 年 月 日 @ ロボットフロンティア 中部大学 2
  3. 2つの自己位置推定アルゴリズム • カルマンフィルタ • ガウス分布で信念を表現 • パーティクルフィルタ • サンプリングで信念を表現 2022

    5 25 年 月 日 @ ロボットフロンティア 中部大学 3
  4. パーティクルフィルタの利用 2022 5 25 年 月 日 @ ロボットフロンティア 中部大学

    4 https://www.youtube.com/watch?v=Dgd2tOCEYnoより
  5. 今日の話題: SLAM • SLAM: simultaneous localization and mapping • 地図を作りながら自己位置推定する技術

    • 地図だけ作りたい場合はmap buildingと呼ぶことも • 00年代後半に実用 • 「地図を作りながら自己位置推定する」方法 • センシング→動く(動いた量を計測)→センシング→動く・・・ で得られたセンサ情報を貼り合わせ 2022 5 25 年 月 日 @ ロボットフロンティア 中部大学 5
  6. 問題:移動にもセンサ情報にも雑音 2022 5 25 年 月 日 @ ロボットフロンティア 中部大学

    6 真の位置・向き とは違う!! 移動量に 基づいて 貼り合わせ うまく 合わない どうしましょう? センサ情報に 基づいて 貼り合わせ 合わない きれいに 合うところが 見つからない どこでも 合ってしまう
  7. 確率を使って問題を整理 • 当面,SLAMの問題とは次のような問題としましょう • 「𝑝𝑝 𝒙𝒙1:𝑡𝑡 , 𝐦𝐦 |𝒙𝒙0 ,

    𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡 が最大となる𝒙𝒙1:𝑡𝑡 , 𝐦𝐦 を求める問題」 • 𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡 : 初期位置(地図を描く時の原点),移動,センサ情報 • 𝒙𝒙1:𝑡𝑡 : ロボットの動いた軌跡 • 𝐦𝐦: 地図 • 𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡 を説明する最尤な𝒙𝒙1:𝑡𝑡 , 𝐦𝐦 を求める問題と考えてもよい • 問題 • どうやって𝑝𝑝 𝒙𝒙1:𝑡𝑡 , 𝐦𝐦 |𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡 を計算するのか? • 𝒙𝒙1:𝑡𝑡 , 𝐦𝐦には無限に候補が存在 2022 5 25 年 月 日 @ ロボットフロンティア 中部大学 7
  8. アプローチ1: 再急降下法等による最適化 • graph-based SLAMなどの原理 (今日は説明しません) • 𝑝𝑝 𝒙𝒙1:𝑡𝑡 ,

    𝐦𝐦 |𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡 を 誤差の評価関数として実装 • 特に確率論に厳密である必要はない • 移動に雑音がない仮定で地図を初期化し, 評価値が減るようにずらしていく • 評価値が改善しない →正解の軌道と地図として出力 2022 5 25 年 月 日 @ ロボットフロンティア 中部大学 8 点数が高くなるよう に少しずつずらす 軌跡,センサ情報が どれだけずれているか点数化 (注意: 大雑把な説明です.)
  9. graph-based SLAMの様子 • 得られたセンサ情報を重ねては修正を繰り返している • 1周したあとに一気に歪みを減らす「ループの閉じ込み」が重要 • (特にジャイロがない場合)θ方向に歪みやすい 2022 5

    25 年 月 日 @ ロボットフロンティア 中部大学 9 https://youtu.be/jS5_a9BW2zI (cartographerを使用)
  10. アプローチ2: ベイズ推定 • 今日の本題 • 式を実装が簡単な形式に変形 • 𝑝𝑝 𝒙𝒙1:𝑡𝑡 ,

    𝐦𝐦 |𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡 = 𝑝𝑝 𝐦𝐦 |𝒙𝒙1:𝑡𝑡 , 𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡 𝑝𝑝(𝒙𝒙1:𝑡𝑡 |𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡 )(乗法定理) = 𝑝𝑝 𝐦𝐦 |𝒙𝒙0:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡 𝑝𝑝(𝒙𝒙1:𝑡𝑡 |𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡 ) ( 𝒙𝒙 と𝒖𝒖 の情報が冗長なので整理) • まだ複雑な式のように思えるが, 実はパーティクルで表現可能 2022 5 25 年 月 日 @ ロボットフロンティア 中部大学 10 FastSLAM ① ②
  11. FastSLAMのパーティクル • 軌跡の分布から𝑁𝑁個ドローしたもの • 位置でなく軌跡 • 一般的な自己位置推定(MCL)とは異なる • 𝒙𝒙1:𝑡𝑡 (𝑖𝑖)~𝑝𝑝(𝒙𝒙1:𝑡𝑡

    |𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡 ) (前ページ②) • 𝑖𝑖 = 0,1,2, … , 𝑁𝑁 − 1 • 𝒙𝒙1:𝑡𝑡 (𝑖𝑖)に対する𝑝𝑝 𝐦𝐦 |𝒙𝒙0:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡 (前ページ①) • 𝑝𝑝 𝐦𝐦 |𝒙𝒙0:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡 = 𝑝𝑝 𝐦𝐦 |𝒙𝒙0 , 𝒙𝒙1:𝑡𝑡 (𝑖𝑖), 𝐳𝐳1:𝑡𝑡 • 条件の𝒙𝒙0 , 𝒙𝒙1:𝑡𝑡 (𝑖𝑖), 𝐳𝐳1:𝑡𝑡 が全部固定されている 2022 5 25 年 月 日 @ ロボットフロンティア 中部大学 11 X Y ドローされた軌跡(全体で 𝑝𝑝(𝒙𝒙1:𝑡𝑡 |𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡 )を近似) 各軌跡に対して地図の 分布をぶら下げる X Y 軌跡の仮説を作る問題と, 仮説に対する地図の分布を作る問題に分かれる
  12. FastSLAMによる地図生成 • 表示されているのは最尤なパーティクルの地図 • 他にたくさんの軌跡と地図のペア • 「ループの閉じ込み」は基本的にない 2022 5 25

    年 月 日 @ ロボットフロンティア 中部大学 12 https://youtu.be/b2kYQ11PUSI (gmappingを使用)
  13. FastSLAMの処理 • 各パーティクルのデータ: • 軌跡𝒙𝒙1:𝑡𝑡 (𝑖𝑖), 重み𝒘𝒘𝑡𝑡 (𝑖𝑖),地図の信念分布𝑏𝑏𝑡𝑡 (𝑖𝑖)(𝐦𝐦) •

    移動に対して • MCLとほぼ同じ • 𝒙𝒙𝑡𝑡 (𝑖𝑖)~𝑝𝑝(𝒙𝒙|𝒙𝒙𝑡𝑡−1 (𝑖𝑖) , 𝒖𝒖𝑡𝑡 ) • 𝒙𝒙1:𝑡𝑡−1 (𝑖𝑖) に𝒙𝒙𝑡𝑡 (𝑖𝑖)をくっつけて 𝒙𝒙1:𝑡𝑡 (𝑖𝑖)に • 移動を繰り返していくとだんだんデータが増えていきそう • この話はあとで • センサ情報に対して • 𝒘𝒘𝑡𝑡−1 (𝑖𝑖) と𝑏𝑏𝑡𝑡−1 (𝑖𝑖) (𝐦𝐦)を両方更新 2022 5 25 年 月 日 @ ロボットフロンティア 中部大学 13 できるの?
  14. 重みの更新式の導出 • 𝑤𝑤𝑡𝑡 (𝑖𝑖) ∝ 𝑝𝑝 𝒙𝒙1:𝑡𝑡 𝑖𝑖 𝒙𝒙0 ,

    𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡 ∝ 𝑝𝑝 𝐳𝐳𝑡𝑡 𝒙𝒙1:𝑡𝑡 𝑖𝑖 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡−1 𝑝𝑝 𝒙𝒙1:𝑡𝑡 𝑖𝑖 𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡−1 • = 𝑝𝑝 𝐳𝐳𝑡𝑡 𝒙𝒙1:𝑡𝑡 𝑖𝑖 , 𝐳𝐳1:𝑡𝑡−1 𝑤𝑤𝑡𝑡−1 (𝑖𝑖) = 𝑝𝑝 𝐳𝐳𝑡𝑡 𝑏𝑏𝑡𝑡−1 (𝑖𝑖) (𝐦𝐦), 𝒙𝒙𝑡𝑡 (𝑖𝑖) 𝑤𝑤𝑡𝑡−1 (𝑖𝑖) = 𝑝𝑝 𝐳𝐳𝑡𝑡 𝐦𝐦, 𝒙𝒙𝑡𝑡 (𝑖𝑖) 𝑏𝑏𝑡𝑡−1 𝑖𝑖 (𝐦𝐦) 𝑤𝑤𝑡𝑡−1 (𝑖𝑖) 2022 5 25 年 月 日 @ ロボットフロンティア 中部大学 14 ベイズの定理 観測が入る前の軌跡に 対する重みなので𝑤𝑤𝑡𝑡−1 (𝑖𝑖) 冗長 𝑡𝑡 − 1までの軌跡・観測情報を 𝑡𝑡 − 1までの地図の分布に置き換え これまでの地図から考えて, 𝒙𝒙𝑡𝑡 (𝑖𝑖)で𝐳𝐳𝑡𝑡 が得られる度合
  15. 地図の更新式の導出 • 𝑏𝑏𝑡𝑡 (𝑖𝑖) 𝐦𝐦 = 𝑝𝑝 𝐦𝐦 𝒙𝒙0:𝑡𝑡 (𝑖𝑖)

    , 𝐳𝐳1:𝑡𝑡 = 𝜂𝜂𝑝𝑝 𝐳𝐳𝑡𝑡 𝐦𝐦, 𝒙𝒙0:𝑡𝑡 (𝑖𝑖) , 𝐳𝐳1:𝑡𝑡−1 𝑝𝑝 𝐦𝐦 𝒙𝒙0:𝑡𝑡 (𝑖𝑖) , 𝐳𝐳1:𝑡𝑡−1 (ベイズの定理) = 𝜂𝜂𝑝𝑝 𝐳𝐳𝑡𝑡 𝐦𝐦, 𝒙𝒙𝑡𝑡 (𝑖𝑖) 𝑝𝑝 𝐦𝐦 𝒙𝒙0:𝑡𝑡−1 (𝑖𝑖) , 𝐳𝐳1:𝑡𝑡−1 (余計な情報の削除) = 𝜂𝜂𝑝𝑝 𝐳𝐳𝑡𝑡 𝐦𝐦, 𝒙𝒙𝑡𝑡 (𝑖𝑖) 𝑏𝑏𝑡𝑡−1 (𝑖𝑖) 𝐦𝐦 • 左の因子: 自己位置推定で尤度関数と言っていたもの • 右の因子: センサ情報が入る前の地図の信念分布 • 前ページの期待値のカッコ内の分布と同じもの 2022 5 25 年 月 日 @ ロボットフロンティア 中部大学 15
  16. FastSLAMの更新式のまとめ • パーティクル1つのデータ: 𝒙𝒙𝑡𝑡 (𝑖𝑖), 𝒘𝒘𝑡𝑡 (𝑖𝑖), 𝑏𝑏𝑡𝑡 (𝑖𝑖)(𝐦𝐦) •

    過去の軌跡は,下の式をみるといらない(実時間実行のために重要) • 移動に対して: 𝒙𝒙𝑡𝑡 (𝑖𝑖)~𝑝𝑝 𝒙𝒙 𝒙𝒙𝑡𝑡−1 (𝑖𝑖) , 𝒖𝒖𝑡𝑡 • センサ情報に対して • 重みの更新: 𝑤𝑤𝑡𝑡 (𝑖𝑖) = 𝜂𝜂 𝑝𝑝 𝐳𝐳𝑡𝑡 𝐦𝐦, 𝒙𝒙𝑡𝑡 (𝑖𝑖) 𝑏𝑏𝑡𝑡−1 𝑖𝑖 (𝐦𝐦) 𝑤𝑤𝑡𝑡−1 (𝑖𝑖) • 地図の更新: 𝑏𝑏𝑡𝑡 (𝑖𝑖) 𝐦𝐦 = 𝜂𝜂𝑝𝑝 𝐳𝐳𝑡𝑡 𝐦𝐦, 𝒙𝒙𝑡𝑡 (𝑖𝑖) 𝑏𝑏𝑡𝑡−1 (𝑖𝑖) 𝐦𝐦 • 𝑝𝑝 𝐳𝐳𝑡𝑡 𝐦𝐦, 𝒙𝒙𝑡𝑡 (𝑖𝑖) : 地図𝐦𝐦に対する尤度関数とみなせる 2022 5 25 年 月 日 @ ロボットフロンティア 中部大学 16 注意: 𝜂𝜂の値は 上下の式で違う
  17. パーティクルに持たせる地図𝑏𝑏𝑡𝑡 (𝑖𝑖)(𝐦𝐦)の形式 • 点ランドマークの地図 • 特定の目印(ランドマーク)を カメラ等で観測するとき • ランドマークの位置の不確かさ をカルマンフィルタで計算

    • 占有格子地図 • レーザスキャナ等で環境の形状 を観測するとき • 色の濃さ=壁のある確率 • 推定が進むと白黒はっきり 2022 5 25 年 月 日 @ ロボットフロンティア 中部大学 17 分布 真の位置 (地図にはない)
  18. 点ランドマーク地図に対する例 • 実装については[上田2019]で • まだまだ作業が必要 • さらなる式展開 • 最初に見たランドマークに対する カルマンフィルタの初期化

    2022 5 25 年 月 日 @ ロボットフロンティア 中部大学 18
  19. 今回(SLAM)のまとめ • 自己位置推定と比較して推定対象の次元が大きい→難問 • SLAMに関する確率分布: 𝑝𝑝 𝒙𝒙1:𝑡𝑡 , 𝐦𝐦|𝒙𝒙0 ,

    𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡 • 自己位置推定に関する確率分布: 𝑝𝑝 𝒙𝒙𝑡𝑡 |𝒙𝒙0 , 𝒖𝒖1:𝑡𝑡 , 𝐳𝐳1:𝑡𝑡 , 𝐦𝐦 • 難問だが,可能となった • graph-based SLAM: 粗い地図を最適化 • FastSLAM: 軌跡の推定と地図の推定を分解 • Rao-Blackwellizationという確率の操作 2022 5 25 年 月 日 @ ロボットフロンティア 中部大学 19