確率ロボティクス2016第13回
Fri Jan 13 21:43:15 JST 2017 (modified: Fri Nov 29 17:18:15 JST 2019)
views: 1101, keywords:
Tweet
確率ロボティクス
第13回
上田 隆一
2016年1月18日@千葉工業大学
本日の内容
- graph based SLAM
graph based SLAM
オフラインSLAM
- Full SLAM（完全SLAM）とも
- 全ての情報が出揃ったところで、全ての情報に対して最も説明がつく地図を求める
- 問題: [latex]\\text{argmax}_{m,x_{1:T}} P(m,x_{1:T} | u_{1:T}, z_{1:T},x_0)[/latex]
- [latex]x_{0:T}[/latex]: ロボットの姿勢のシーケンス（経路）
- 一般的なアルゴリズム
- 経路を求めてから地図を求めるという手順
ロボットの経路の算出
- 例えばオドメトリの情報だけで[latex]x_{0:T}[/latex]を初期化
- これだけだと実際とかなりずれる
- センサの値で少しずつ補正をかけていく
- 評価関数を定義してニュートン法等で
最適化問題への落とし込み
- 二つの姿勢[latex]x_i,x_j[/latex]について、以下の差を考える
- [latex]\\hat{z}_{ij}(x_i,x_j)[/latex]: ある時点での推定値で作るベクトル
- [latex]z_{ij}[/latex]: [latex]x_i,x_j[/latex]それぞれの地点で得たセンサデータから計算される相対姿勢
- この両者の差が全体的に少ない経路を探す・・・のだけど
情報の多寡の考慮
- 差を小さくするときに重みをつける
- センサで正確に相対姿勢が分かっているところを重視
- 情報行列[latex]\\Omega_{ij}[/latex]を基準に
- 誤差の少なそうな [latex]\\approx[/latex] 情報が多い
- 共分散行列の逆行列
- 計測ごとに足し算していける（前回の講義）
尤度の計算
- 1対の姿勢間の推定値[latex]\\hat{z}_{ij}(x_i,x_j)[/latex]とセンサデータの示す推定姿勢[latex]\\hat{z}_{ij}(x_i,x_j)[/latex]の差:
- [latex]\\ell_{ij} \\propto \\{ z_{ij} - \\hat{z}_{ij}(x_i,x_j) \\}^T \\Omega_{ij} \\{ z_{ij} - \\hat{z}_{ij}(x_i,x_j) \\}[/latex]
- [latex]e_{ij} = z_{ij} - \\hat{z}_{ij}(x_i,x_j)[/latex]とすると[latex]\\ell_{ij} \\propto e_{ij}^T \\Omega_{ij} e_{ij}[/latex]
- [latex]F_{ij} = e_{ij}^T \\Omega_{ij} e_{ij}[/latex]としておきます
- 対数尤度として扱われる
- マハラノビス距離の2乗でもある
- この値の和[latex]F(\\boldsymbol{x})[/latex]を最小にする
- [latex]F(\\boldsymbol{x}) = \\sum_{<i,j> \\in C} F_{ij}[/latex]
- [latex]\\boldsymbol{x}[/latex]: 姿勢の変数を並べたベクトル
和を減少させる差分の導出
- 今の経路の推定値[latex]\\boldsymbol{x}[/latex]（チュートリアルでは[latex]\\breve{\\boldsymbol{x}}[/latex]）周りで、どっちに推定値を動かせば前ページの値が最も減るかを求める
- 導出手順
- [latex]F(\\boldsymbol{x})[/latex]を[latex]\\Delta{\\boldsymbol{x}}[/latex]だけ動かして値が動かないところを探す
- 線形化のときに出てきた方法
- [latex]F(\\boldsymbol{x})[/latex]を[latex]\\Delta{\\boldsymbol{x}}[/latex]だけ動かして値が動かないところを探す
手順
- 推定値を動かした時の誤差の増分を線形化しておく
- [latex]e_{ij}(x_i + \\Delta x_i , x_j + \\Delta x_j) = e_{ij} (\\boldsymbol{x} + \\Delta \\boldsymbol{x}) \\approx e_{ij} + J_{ij}\\Delta \\boldsymbol{x} [/latex]
- [latex]F(\\boldsymbol{x})[/latex]の変化量を求める
- [latex]F(\\boldsymbol{x} + \\Delta\\boldsymbol{x}) = \\sum_{<i,j> \\in C} F_{ij}( \\boldsymbol{x} + \\Delta\\boldsymbol{x} ) \\\\ = \\sum_{<i,j> \\in C} \\{ (e_{ij} + J_{ij} \\Delta\\boldsymbol{x})^T \\Omega_{ij}(e_{ij} + J_{ij} \\Delta\\boldsymbol{x}) \\}\\\\ \\approx \\sum_{<i,j> \\in C} \\{ \\text{定数} + 2e_{ij}^T \\Omega_{ij} J_{ij}\\Delta{\\boldsymbol{x}} +\\Delta{\\boldsymbol{x}}^T J_{ij}^T \\Omega_{ij} J_{ij}\\Delta{\\boldsymbol{x}} \\} [/latex]
- 記号の整理
- [latex]e_{ij}^T \\Omega_{ij} J_{ij}[/latex]を[latex]\\boldsymbol{b}_{ij}[/latex]に
- [latex]J_{ij}^T \\Omega_{ij} J_{ij}[/latex]を[latex]H_{ij}[/latex]に
- [latex]F(\\boldsymbol{x} + \\Delta\\boldsymbol{x}) \\approx \\sum_{<i,j> \\in C} \\{ \\text{定数} + 2\\boldsymbol{b}_{ij}\\Delta{\\boldsymbol{x}} +\\Delta{\\boldsymbol{x}}^T H_{ij}\\Delta{\\boldsymbol{x}} \\}[/latex]
- 和の部分を巨大なベクトル、行列に
- [latex]F(\\boldsymbol{x} + \\Delta\\boldsymbol{x}) \\approx \\text{定数} + 2\\boldsymbol{b}^T \\Delta{\\boldsymbol{x}} +\\Delta{\\boldsymbol{x}}^T H \\Delta{\\boldsymbol{x}} [/latex]
- [latex]H[/latex]: 経路に関する情報行列
- [latex]\\boldsymbol{b}[/latex]: 係数ベクトルと呼ぶ
- 変化量がゼロになる[latex]\\Delta\\boldsymbol{x}[/latex]を求める
- 上の式の右辺を[latex]\\Delta{\\boldsymbol{x}}[/latex]で微分した量がゼロになる[latex]\\Delta\\boldsymbol{x}[/latex]
- [latex]H \\Delta\\boldsymbol{x} = - \\boldsymbol{b}[/latex]
情報行列[latex]H[/latex]の性質
- [latex]H[/latex]: 関係性が得られている姿勢に関係する要素だけが非ゼロのスパース行列
- このような形の行列[latex]H_{ij}[/latex]が足し合わさった形になる
- [latex]H_{ij} = \\begin{bmatrix} \\ddots \\\\ &A_{ij}^T\\Omega_{ij}A_{ij} & \\dots & A_{ij}^T\\Omega_{ij}B_{ij} \\\\ & \\vdots & \\ddots & \\vdots \\\\ &B_{ij}^T\\Omega_{ij}A_{ij} & \\dots & B_{ij}^T\\Omega_{ij}B_{ij} \\\\ & & & & \\ddots \\end{bmatrix} [/latex]
ベクトル[latex]b[/latex]の性質
- なんの値かはよくわからない（ごめんなさい）が次のような形のベクトル[latex]\\boldsymbol{b}_{ij}[/latex]が足し合わさった形になる
- [latex]\\boldsymbol{b}_{ij} = \\begin{bmatrix} \\vdots \\\\ A_{ij}^T\\Omega_{ij} e_{ij} \\\\ \\vdots \\\\ B_{ij}^T\\Omega_{ij} e_{ij} \\\\ \\vdots \\\\ \\end{bmatrix} [/latex]
アルゴリズム
- 以下を収束するまで繰り返し
- オドメトリ等のデータから初期の[latex]x_{0:T}[/latex]を初期化
- センサデータから得られる各相対姿勢に関して
- [latex]H_{ij}[/latex]を[latex]H[/latex]に足す
- [latex]\\boldsymbol{b}_{ij}[/latex]を[latex]\\boldsymbol{b}[/latex]に足す
- [latex]H[/latex]の[latex]H_{01}[/latex]の部分の左上
（姿勢[latex]x_0[/latex]の部分）に単位行列を足す
- 姿勢[latex]x_0[/latex]は絶対的に正しいので値をかさ上げ
- [latex]H\\Delta \\boldsymbol{x} = - \\boldsymbol{b}[/latex]から[latex]\\Delta \\boldsymbol{x}[/latex]を導出
- [latex]x_{0:T}[/latex]を[latex]\\Delta \\boldsymbol{x}[/latex]で更新
数式上の問題
- 特異点が出る（らしい）
- クォータニオン等、適切な表現を用いる
課題
- 講義で扱った手法について簡単な例題を作り、コードを書いて解く
- コードと問題の定義から解答までまとめたドキュメントを提出
- 提出方法
- コードをGitHubか何かにアップしてREADMEに説明と動いた時の様子を書く
- GitHubがわからん人はコードとドキュメントをメールで添付してください
- Travis CIとかで実行してるときの様子がわかるようにしてくださると大変ありがたく（点数おまけします）
- 30点満点にします（30点を超えることもあり）
- 締め切り: 2/8