Probabilistic Robotics - Chap.7 Mobile Robot Localization: Markov and Gaussian

7

移動ロボットのローカライゼーションとは、環境地図中でロボットの座標を決める問題である
一方で、座標変換の問題とも捉えることができる
ローカライゼーションとは、地図座標系とロボットのローカル座標系のマッチングをとるプロセスである
ただし残念なことに、ロボットの座標を直接センシングする方法はない。
ノイズの無いセンサなどない。
センサ１つでは座標を求めるには情報が足りない

7.1

ローカライゼーションの問題点を分類してみる

ローカル VS グローバル

ローカライゼーション問題は起動時もしくは実行中に利用できる知識で特徴づけられる
難しさに応じて３つに分類する

位置追跡

この時ロボットの初期座標が既知だとする
計測した位置の含まれる誤差は小さいと仮定
位置追跡は、ロボットの真の座標周辺で起こるローカルな問題

グローバルローカライゼーション

ロボットの初期座標は不明

誘拐ロボット問題(kidnapped robot problem)

グローバルローカライゼーションの一種だが、一層難しい問題
動作中にロボットがどこかに連れ去られたり、テレポートした時の位置特定の問題

静的 vs 動的

環境が静的か動的かということ
静的な環境とは、変数がロボットの座標のみ
動的な環境とは、環境中にロボット以外にも、位置や設定が可変なオブジェクトがいる。人や日光など。

動的な環境に対応するには２つの方法がある

動的な物体は状態ベクトルに含める

マルコフ仮定で正当化できるが、計算量が増える等の副作用がある

ある状態下ではモデル化されていない動作を排除する

パッシブ　vs　アクティブアプローチ

ローカライゼーションアルゴリズムがロボットの動作を制御できるかどうか

パッシブローカライゼーション

ロボットの動作のみ観測する
ロボットはローカライゼーションとは別の方法で制御される
例えばランダムに動いたり、日常業務をこなす

アクティブローカライゼーション

ローカライゼーションアルゴリズムが観測だけでなく、ロボットの制御もする
制御はローカライズの誤差や危険地帯にローカライズの精度が悪いロボットを動かすためのコストを最小化する

アクティブローカライゼーションの重要な制約事項は、アクティブローカライゼーションがロボット全身を制御することが必要な点である。
実際の面では、ローカライゼーション以外のタスクも実行している時にローカライゼーションが出来ることが必要。

ロボット単体 vs ロボット複数台

ロボットの数で分類

単体ロボットのローカライゼーション

ロボット間の通信問題がなく、また全データを単体のロボットへ使えるという便利さ

複数台ロボットのローカライゼーション

結局は単体ロボットそれぞれのローカライゼーション問題

7.2 マルコフローカライゼーション

確率的ローカライゼーションのアルゴリズムは、ベイズフィルタの派生
その単純な利用例としてマルコフローカライゼーションがある。
マルコフローカライゼーションのアルゴリズムは下記の通り
${ Algorithm \ Markov_localization \left( bel \left( x_{t-1} \right), u_t, z_t, m \right):\\ \ \ \ for \ all \ x_t \ do\\ \ \ \ \ \ \ \overline{bel} \left( x_t \right) = \int p \left( x_t | u_t, x_{t-1}, m \right) bel \left( x_{t-1} \right) dx_{t-1}\\ \ \ \ \ \ \ bel \left( x_t \right) = \int \eta \ p \left( z_t | x_t, m \right) \overline{bel} \left( x_t \right)\\ \ \ \ endfor\\ \ \ \ return \ bel \left( x_t \right) }$
マルコフローカライゼーションが対応するのは、静的環境下での

位置追跡問題
グローバルローカライゼーション問題
誘拐ロボット問題

初期信念 ${bel \left( x_0 \right)}$ は、ロボットの初期姿勢を反映したもので、ローカライゼーション問題毎に別の値をセットする。

位置追跡問題

初期姿勢が既知であれば、 ${bel \left( x_0 \right)}$ は質点分布として初期化される
初期姿勢を ${\overline{bel} \left( x_0 \right)}$ とすると、
${ bel \left( x_0 \right) = \left\{ \begin{array}{} 1 & if \ x_0 \ = \ \overline{x_0}\\ 0 & otherwise \end{array} \right. }$

信念 ${bel \left( x_0 \right)}$ は大抵 ${x_0}$ を中心とした、分散の小さなガウス分布で初期化する

・グローバルローカライゼーション
初期座標が未知の場合、 ${bel \left( x_0 \right)}$ は、環境地図を対象とした一様分布で初期化する
${bel \left( x_0 \right) = \frac{1}{|X|}}$
ここで ${|X|}$ は、地図内の全姿勢空間の体積（ルベーグ測度*1）である。

7.3 EKF（Extended Kalman Filter）ローカライゼーション

EKFローカライゼーションはマルコフローカライゼーションの特殊ケース
本節で紹介するEKFローカライゼーションは、特徴量の集合でマップが表されるものと仮定
特徴量のIDは、一致変数で表現する
一致変数の値は既知とする

7.4.1

EKFでの単峰な信念を順応させるために、２つのことを仮定する

求めた特徴量がどのランドマークと一致するかは既知
ロボットの初期姿勢は比較的よくわかる（直訳だとこうなんだが日本語としては変だよねぇ。。）

更新後の信念の分散は、更新前の信念や観測密度（p(z|x)だから、観測モデルとしても良い気が。。）よりも小さくなる
　→　独立する推定値を纏めることで、不確かさを小さくすることができるため

7.4.2

本小節では、EFKローカライゼーションの具体例について述べる
特徴量ベースの地図で、速度ベースの運動モデルを使用する
下記はランドマークと特徴量の一致が既知で、回転運動を含む場合（直進のみ、 ${\omega_t = 0}$ の場合は除く）とする

${ Algorithm \ EKF\_Localization\_known\_correspondences \left( \mu_{t-1}, \sigma_{t-1}, u_t, z_t, c_t, m \right):\\ \theta = \mu_{t-1, \theta}\\ G_t = \begin{pmatrix} 1 & 0 & -\frac{v_t}{\omega_t}cos \theta + \frac{v_t}{\omega_t} cos \left( \theta + \omega_t \Delta t \right) \\ 0 & 1 & -\frac{v_t}{\omega_t}sin \theta + \frac{v_t}{\omega_t} sin \left( \theta + \omega_t \Delta t \right) \\ 0 & 0 & 1 \end{pmatrix}\\ V_t = \begin{pmatrix} \frac{-sin \theta + sin \left( \theta + \omega_t \Delta t \right)}{\omega_t} & \frac{v_t sin \theta - sin \left( \theta + \omega_t \Delta t \right)}{\omega^2_t} + \frac{v_t cos \left( \theta + \omega_t \Delta t \right) \Delta t}{\omega_t} \\ \frac{cos \theta - cos \left( \theta + \omega_t \Delta t \right)}{\omega_t} & - \frac{v_t cos \theta - cos \left( \theta + \omega_t \Delta t \right)}{\omega^2_t} + \frac{v_t sin \left( \theta + \omega_t \Delta t \right) \Delta t}{\omega_t} \\ 0 & \Delta_t \end{pmatrix}\\ M_t = \begin{pmatrix} \alpha_1 v^2_t + \alpha_2 \omega^2_t & 0 \\ 0 & \alpha_3 v^2_t + \alpha_4 \omega^2_t \end{pmatrix}\\ \overline{\mu_t} = \mu_{t-1} + \begin{pmatrix} - \frac{v_t}{\omega_t}sin \theta + \frac{v_t}{\omega_t} sin \left( \theta + \omega_t \Delta t \right) \\ \frac{v_t}{\omega_t}cos \theta - \frac{v_t}{\omega_t} cos \left( \theta + \omega_t \Delta t \right) \\ \omega_t \Delta t \end{pmatrix}\\ \overline{\Sigma_t} = G_t \Sigma_{t-1} G^T_t \ + \ V_t M_t V^T_t \\ Q_t = \begin{pmatrix} \sigma^2_r & 0 & 0 \\ 0 & \sigma^2_{\phi} & 0 \\ 0 & 0 & \sigma^2_{s} \end{pmatrix}\\ for \ all \ observed \ features \ z^i_t \ = \begin{pmatrix} r^i_t & \phi^i_t & s^i_t \end{pmatrix}^T \ do\\ \ \ \ j \ = \ c^i_t \\ \ \ \ q \ = \ \left( m_{j, x} - \overline{\mu}_{t, x} \right)^2 + \left( m_{j, y} - \overline{\mu}_{t, y} \right)^2 \\ \ \ \ \hat{z}^i_t = \begin{pmatrix} \sqrt{q} \\ atan2 \left( m_{j, y} - \overline{\mu}_{t, y}, m_{j, x} - \overline{\mu}_{t, x} \right) - \overline{\mu}_{t, \theta} \\ m_{j, s} \end{pmatrix}\\ \ \ \ H^i_t = \begin{pmatrix} - \frac{m_{j, x} - \overline{\mu}_{t, x}}{\sqrt{q}} & - \frac{m_{j, y} - \overline{\mu}_{t, y}}{\sqrt{q}} & 0 \\ \frac{m_{j, y} - \overline{\mu}_{t, y}}{q} & \frac{m_{j, x} - \overline{\mu}_{t, x}}{q} & -1 \\ 0 & 0 & 0 \end{pmatrix}\\ \ \ \ S^i_t = H^i_t \overline{\Sigma}_t [ H^i_t ]^T + Q_t \\ \ \ \ K^i_t = \overline{\Sigma}_t [ H^i_t ]^T [ S^i_t ]^{-1} \\ \ \ \ \overline{\mu}_t = \overline{\mu}_t + K^i_t \left( z^i_t - \hat{z}^i_t \right) \\ \ \ \ \overline{\Sigma}_t = \left( I - K^i_t H^i_t \right) \overline{\Sigma}_t \\ endfor \\ \mu_t = \overline{\mu}_t \\ \Sigma_t = \overline{\Sigma}_t \\ p_{z_t} = \prod_i det \left(2 \pi S^i_t \right)^{- \frac{1}{2}} exp \left\{ - \frac{1}{2} \left( z^i_t - \hat{z}^i_t \right)^T [S^i_t ]^{-1} \left( z^i_t - \hat{z}^i_t \right) \right\}\\ return \ \mu_t, \Sigma_t, p_{z_t} }$

7.5 Estimating Correspondences

7.5.1 EKF Localization with Unknown Correspondences

今までのEKFローカライゼーションは、ランドマークの一致が不確実性無しに決められる時のみ使えた。でもそんなの極めてまれ。
ローカライゼーションの過程でランドマークの一致を計算する方法としては、maximum likelihood correspondenceがある
ただ不安定さがあるため、２つのテクニックを使う

互いを混乱させるような十分にユニークで十分に離れているランドマークを選択することは考えにくい
ロボットの姿勢の不確かさが小さいことを確認

今までのEKFローカライゼーションと今回のものの主な違いは、measurement updateである。

7.6 Multi-Hypothesis Tracking

データの関連付けが困難な時に使う手法として、Multi-Hypothesis Tracking(MHT)がある
MHTはbeliefを複数のガウス分布で表せる
MHTアルゴリズムは混合成分の数を小さく保てる
本書ではMHT自体の詳細は紹介せず、MHTを使ったアルゴリズムを紹介する

7.7 UKF Localization

UKFローカライゼーションは、unscented Kalman Filterを使った特徴量ベースのアルゴリズム。アルゴリズムの詳細は下記の通り。なお下記では ${z_t}$ で検出されるランドマークは１つであり、そのランドマークのIDは既知と仮定している。

${ 1: \ \ \ Algorithm \ UKF\_Localization(\mu_{t-1}, \Sigma_{t-1}, u_t, z_t, m): \\ \ \ Generate \ augmented \ mean \ and \ covariance \\ 2: \ \ \ \ \ M_t = \begin{pmatrix} \alpha_1 v^2_t + \alpha_2 \omega-2_t && 0 \\ 0 && \alpha_3 v^2_t + \alpha_4 \omega-2_t \end{pmatrix} \\ 3: \ \ \ \ \ Q_t = \begin{pmatrix} \sigma^2_r && 0 \\ 0 && \sigma^2_{\phi} \end{pmatrix} \\ 4: \ \ \ \ \ \mu^{\alpha}_{t-1} = \begin{pmatrix} \mu^T_{t-1} && (0 \ \ 0)^T && (0 \ \ 0)^T)^T \end{pmatrix} \\ 5: \ \ \ \ \ \Sigma^{\alpha}_{t-1} = \begin{pmatrix} \Sigma_{t-1} && 0 && 0 \\ 0 && M_t && 0 \\ 0 && 0 && Q_t \end{pmatrix} \\ \ \ Generate \ Sigma \ Points \\ 6: \ \ \ \ \ \chi^{\alpha}_{t-1} = \begin{pmatrix} \mu^{\alpha}_{t-1} && \mu^{\alpha}_{t-1} + \gamma \sqrt{\Sigma^{\alpha}_{t-1}} && \mu^{\alpha}_{t-1} - \gamma \sqrt{\Sigma^{\alpha}_{t-1}} \end{pmatrix} \\ \ \ Pass \ Sigma \ Points \ through \ motion \ model \ and \ compute \ Gaussian \ statistics \\ 7: \ \ \ \ \ \chi^x_t = g(u_t + \chi^u_t , \chi^{x}_{t-1}) \\ 8: \ \ \ \ \ \overline{u}_t = \sum^{2L}_{i=0} w^{(m)}_i \overline{\chi}^x_{i,t} \\ 9: \ \ \ \ \ \overline{\Sigma}_t = \sum^{2L}_{i=0} w^{(c)}_i (\overline{\chi}^x_{i,t} - \overline{u}_t)(\overline{\chi}^x_{i,t} - \overline{u}_t)^T \\ \ \ Predict \ observations \ at \ sigma \ points \ and \ compute \ Gaussian \ statistics \\ 10: \ \ \ \ \ \overline{Z}_t = h(\overline{\chi}^x_t) + \chi^z_t \\ 11: \ \ \ \ \ \hat{z} = \sum^{2L}_{i=0} w^{(m)}_i \overline{Z}_{i, t} \\ 12: \ \ \ \ \ S_t = \sum^{2L}_{i=0} w^{(c)}_i (\overline{Z}_{i, t} - \hat{z}_t) (\overline{Z}_{i, t} - \hat{z}_t)^T \\ 13: \ \ \ \ \ \Sigma^{x, z}_t= \sum^{2L}_{i=0} w^{(c)}_i (\overline{\chi}^x_{i, t} - \hat{z}_t) (\overline{Z}_{i, t} - \hat{z}_t)^T \\ \ \ Update \ mean \ and \ covariance \\ 14: \ \ \ \ \ K_t = \Sigma^{x, z}_t S^{-1}_t \\ 15: \ \ \ \ \ \mu_t = \overline{\mu}_t + K_t (z_t - \hat{z}_t) \\ 16: \ \ \ \ \ \Sigma_t = \overline{\Sigma}_t - K_t S_t K^T_t \\ 17: \ \ \ \ \ p_{z_t} = det(2 \pi S_t)^{- \frac{1}{2}} exp \{ -\frac{1}{2} (z_t - \hat{z}_t)^T S^{-1}_t (z_t - \hat{z}_t) \} \\ 18: \ \ \ \ \ return \ \mu_t, \Sigma_t, p_{z_t} }$

7.11 Exercise

　自分なりの回答を記載してみます。もしもご覧になって、「正解！」「間違っている。。」などのコメントもらえますと幸いです。

1.
(a)１つのランドマークの位置しか同定できていないと、ロボットはランドマークから距離Dだけ離れた円周上に存在することはわかるが、円周上のどこにいるかは特定できない。そのため、確度の低い事後分布が求まる。もしも２つ以上のランドマークの位置を同定すると、２つの円の交点上に存在することになり、位置を高精度に特定できる。そのため、確度の高い事後分布が求まる。

*1:http://www.f-denshi.com/000TokiwaJPN/16lebeg/030lbg.htmlを参照のこと