Recommended
More Related Content
What's hot
What's hot (20)
More from Ryuichi Ueda
More from Ryuichi Ueda (20)
確率ロボティクス第13回
- 2. 本日の内容 • GraphSLAM（の入り口） – カルマンフィルタ – 情報フィルタ – GraphSLAMの考え方 Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 2
- 3. カルマンフィルタ • ベイズフィルタの諸々の確率分布を 多変量ガウス分布に限定したもの – 歴史的には話が逆 – 計算が行列演算だけで済む Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 3 この行列（共分散行列）を 足したり掛けたり
- 4. カルマンフィルタのアルゴリズム • このような線形な系で状態の値xを推定 Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 4 入力 雑音 雑音
- 5. • motion update Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 5 動いた後 の推定値 動く前の 推定値 入力に よる遷移 動いた後の 共分散行列 動く前の 共分散行列 雑音εの 共分散行列 状態方程式
- 6. • sensor update Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 6 センサ値 センサ値 で推定値 を補正 「カルマン ゲイン」 を求める 推定位置から 予想された センサ値 センサ雑音δ の共分散行列
- 7. 情報フィルタ • カルマンフィルタと双対 • 情報行列と情報ベクトルを更新するフィルタ • 情報がないとΩはゼロ行列に – 情報が増えていくと中身の数が大きくなっていく – 共分散行列とは逆 Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 7
- 8. • motion update – カルマンフィルタより計算量が多くなる Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 8
- 9. • sensor update – カルマンフィルタより計算量が小さい – 定数・既知の変数との足し算 Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 9 ここが足し算になっていることがGraphSLAMにとって重要
- 10. 非線形への拡張 • 拡張カルマンフィルタ・拡張情報フィルタ – 非線形関数をある点の周りで線形化して計算 – 話がややこしくなるので続きは確率ロボティクス を参考のこと • GraphSLAMは非線形の式で定式化される Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 10
- 11. GraphSLAM • 大雑把な説明 – 情報行列Ωに移動や観測のデータをどんどん追加 – Ωからロボットの軌跡を最適化で求める • 基本的にはオフラインで Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 11 x0 x1 x2 x3 姿勢（3次元） m1 m2 移動 観測 ランドマーク（2次元と特徴量1次元）
- 12. GraphSLAMの「グラフ」 • このような巨大な情報行列と情報ベクトル Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 12 x0 x1 x2 ... m1 m2 m3 ... x0 x1 x2 m1 m2 m3 Ω0 x0 x1 x2 m1 m2 m3
- 13. 移動時のデータ追加 • xtとxt+1に情報行列を追加 Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 13 x0 x1 x2 ... m1 m2 m3 ... x0 x1 x2 m1 m2 m3 Ω0どんどん 部分的な 情報行列を 足していく どんどん 部分的な 情報ベクトルを 足していく
- 14. 追加する情報 • 情報行列に次の6x6行列を追加 • 情報ベクトルに次の6次元縦ベクトルを追加 Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 14 xt-1からxtへの移動を 線形化した時の行列移動の雑音の 共分散行列 推定値mt-1周辺での 状態方程式の計算値
- 15. 観測時のデータ追加 • 観測した姿勢と特徴（ランドマーク）を 結びつけていく Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 15 x0 x1 x2 ... m1 m2 m3 ... Ω0 x0 x1 x2 m1 m2 m3 6x6の情報行列を 分けて足す 情報ベクトルを 分けて足す
- 16. 追加する情報 Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 16 • 情報行列に次の6x6行列を追加 • 情報ベクトルに次の6次元縦ベクトルを追加 計測誤差を並べた 3x6行列 （確率ロボティクス p.315参照のこと） 計測の雑音の 共分散行列 計測値 推定姿勢で予測 される計測値
- 17. 後処理 • 情報行列自体はそのままでは何も情報を与えない • 情報行列と情報ベクトル縮小していく – 最終的に姿勢の遷移の情報が残る • 例 – X1とX2でm1を見た情報からX1とX2に情報を追加 Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 17 x0 x1 x2 ... m1 m2 m3 ... Ω0 X X A x0 x1 x2 m1 m2 m3 A' C A'' B B' B'' x0 x1 x2 ... m1 m2 m3 ... Ω0 X X D x0 x1 x2 m1 m2 m3 D' D'' D'''X X
- 18. • 使われる式 – 情報ベクトルから次のベクトルを引く • τ(mj): ランドマークmjが観測された姿勢xの集合 – 同じく情報行列から次の行列を引いていく • 全部引き終わると、姿勢に関する情報行列、 情報ベクトルができる Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 18
- 19. ロボットの軌跡と地図の算出 • 軌跡の算出 • 各ランドマークの位置の算出 • 誤差が大きい時は を初期値にして再度、 アルゴリズム全体を繰り返す Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 19 前のページで 求めた情報行列と 情報ベクトル