Probabilistic Robotics - Chap.8 Mobile Robot Localization: Grid And Monte Carlo

8.1 Introduction

　８章では下記２つのローカライゼーションアルゴリズムについて述べる。

グリッドローカライゼーション

　beliefの計算にヒストグラムフィルタを使う。グリッドローカライゼーションは、グリッドを細かくすると、計算負荷が高くなり処理が遅くなるという問題がある。

モンテカルロローカライゼーション（MCL:Monte Carlo Localization）

　パーティクルフィルタを使ってローカライゼーションを行う、有名なアルゴリズム。欠点は色々とあるものの、誘拐ロボット問題や動的環境下への対応が可能。

これら２つのアルゴリズムは、下記の共通する特徴がある。

センサ生値を処理するため、特徴量の抽出といった加工処理が不要
ノンパラメトリック
グローバルローカライゼーションのための手法であり、誘拐ロボット問題に対応可能

　以降では、上記２つのアルゴリズムの詳細を述べる。

8.2 Grid Localization

8.2.1 Basic Algorithm

　グリッドローカライゼーションは、ヒストグラムフィルタを利用してグリッドマップ上でのロボットの事後beliefを近似する。つまり事後beliefは、下記の様に各グリッド ${\mathrm{x}_k}$ での存在確率 ${p_{k, t}}$ の集合
${ bel(x_t) = \{ p_{k, t} \} \tag{1} }$
として表せる。また各グリッド ${\mathrm{x_k}}$ の集合であるグリッドマップ ${X_t}$ は、
${ domain(X_t) \ = \ \mathrm{x}_{1, t} \ \cup \ \mathrm{x}_{2, t} \ \cup \ ... \ \cup \mathrm{x}_{k, t} \tag{2} }$
とできる。
　グリッドローカライゼーションの擬似コードを下記に示す。グリッドローカライゼーションは、入力として各グリッドでの存在確率 ${\{ p_{k, t} \}}$ と最新の観測結果、制御量、マップ情報を必要とする。そして処理結果として、最新の事後beliefを出力する。疑似コード中の ${\boldsymbol{motion\_model}}$ と ${\boldsymbol{measurement\_model}}$ は、Chapter5、6で紹介したどの方法を適用してもよい。

${ 1: \ \ \ \boldsymbol{Algorithm Grid\_localization}(\{p_{k, t}\}, u_t, z_t, m): \\ 2: \ \ \ \ \ for \ all \ k \ do \\ 3: \ \ \ \ \ \ \ \overline{p}_{k, t} = \sum_i p_{i, t-1} \boldsymbol{motion\_model}(mean(\mathrm{x}_k), u_t, mean(\mathrm{x}_i)) \\ 4: \ \ \ \ \ \ \ p_{k, t} = \eta \ \overline{p}_{k, t} \boldsymbol{measurement\_model}(z_t, mean(\mathrm{x}_k), m) \\ 5: \ \ \ \ \ endfor \\ 6: \ \ \ \ \ return \ \{p_{k, t}\} }$

8.2.2 Grid Resolution

　グリッドローカライゼーションにおける重要な変数は、グリッドの解像度である。グリッドの決め方には、トポロジカルな方法とメトリックな方法がある。トポロジカルな方法では、環境を特徴的な領域に分割する。ここで言う特徴的な領域とは、ドアや窓といったランドマークが存在する領域を指す。メトリックな方法では、環境の特性を考慮せずに均一なグリッドに分割する。分割数は、特徴的な領域は少ない場合が多いので、メトリックな方法の方が多い。
　分割数を多くすると計算負荷が高まるため、解像度は粗くなりがちである。粗さへの対応は、センサモデルと運動モデルの工夫が必要である。例えばメトリックな方法でグリッドを切った場合、グリッドが ${1m \times 1m}$ で、ロボットの速度が ${1m/s}$ とすると、運動モデルが頻繁に更新してもグリッドの遷移を検出できない。そのため、運動モデルの更新頻度を下げるといった対応が必要である。

8.2.3 Comuputational Consideration

　ローカライゼーション時の計算負荷を下げる方法は、沢山検討されている。

Model pre-caching

　予め特定の計測結果を保存しておき、現在の値とその値を比較することで、瞬時に状態を特定可能にする。

Sensor Subsampling

　センサデータを全て処理するのではなく、重要な一部のデータのみを処理する。

Delayed Motion Update

　制御や観測を行うよりも低い頻度でbeliefの更新を行う。

Selective Updating

　事後確率に対する閾値を設定しておき、閾値を超えているグリッドのみ更新を行う。

8.3 Monte Carlo Localization(MCL)

8.3.2 The MCL Algorithm

　ベーシックなMCLアルゴリズムでは、beliefはパーティクルのセットで表現される。beliefの更新方法は下記の擬似コードの通り。

${ \ 1: \ Algorighm \ MCL(\chi_{t-1}, u_t, z_t, m): \\ \ 2: \ \quad \overline{chi}_t = \chi_t = \varnothing \\ \ 3: \ \quad for \ m = 1 \ to \ M \ do \\ \ 4: \ \qquad x^{[m]}_t = \boldsymbol{sample\_motion\_model}(u_t, x^{[m]}_{t-1}) \\ \ 5: \ \qquad w^{[m]}_t = \boldsymbol{measurement\_model}(z_t, x^{[m]}_t, m) \\ \ 6: \ \qquad \overline{\chi}_t = \overline{\chi}_t + \langle x^{[m]}_t, w^{[m]}_t \rangle \\ \ 7: \ \quad endfor \\ \ 8: \ \quad for \ m=1 \ to \ M \ do \\ \ 9: \ \qquad draw \ i \ with \ probability \ \propto \ w^{[i]}_t \\ 10: \ \qquad add \ x^{[i]}_t \ to \ \chi_t \\ 11: \ \quad endfor \\ 12: \ \quad return \ \chi_t }$

初期beliefは、事前分布 ${p(x_0)}$ を元にランダムに生成した ${M}$ 個のパーティクルとして得る。この時各パーティクルにはimportance factor \ ${M^{-1}}$ を割り当てる。なお上記擬似コード中の ${\boldsymbol{sample\_motion\_model}}$ と ${\boldsymbol{measurement\_model}}$ は、グリッドローカライゼーションと同様に、５章、６章で紹介された方法を摘要できる。

8.3.4 Properties of MCL

　MCLは殆ど全ての実用上重要な分布を近似できる。一般的なパーティクル数 ${M}$ をセットするための戦略は、 ${u_t}$ と ${z_t}$ が更新されるまでリサンプリングしないことである。またMCLの利点として、ノンパラメトリックな近似ができる点がある。

8.3.5 Random Particle MCL: Recovery form Failures

　今までに述べたMCLのアルゴリズムでは、グローバルローカライゼーションは可能だが失敗時に復帰できない、誘拐ロボット問題には対処できない、といった問題がある。それは、パーティクルが一度現在の姿勢と異なる場所に集まると、正しい姿勢へ回復できないためである。
　これら問題へのシンプルな対応策として、ランダムパーティクルの注入する方法がある。これにより、運動モデルの中にランダムな部分空間が出来る。ではどうやってランダムにパーティクルを注入するべきか。ある種の推論を使って行うのがよい。
　１つの方法として、観測モデルを監視し、モデルと平均確率を関連付ける方法がある。（ToDo:よくわからん）平均値は、観測モデルを近似できる。通常数ステップにわたった平均を求めるのがよい。
　観測確率が低い理由は、ローカライゼーションの失敗以外にも、センサノイズが異常に高い、パーティクルが空間上に分散しすぎている（集まっていない）、などがある。
　パーティクスル数を決める時、数ステップの観測尤度の平均を維持し、より多くのステップの平均に関連付けるとよい。

　どの分布を使ってサンプリングするかについての２つ目の問題は、２つの方法で言える。姿勢空間全体を対象とした一様分布を元にパーティクスルをばらまき、現在の観測値を使ってそれらを重み付けする。

　例えばランドマーク検出モデルなどのセンサモデルでは、観測モデルの分布に沿ってパーティクルをばらまくことができる。この場合は尤度の沿ってパーティクルをばらまく。

　下記にランダムランプルのばらまきを考慮したMCLのアルゴリズムを述べる。
${ \ 1: \ Algorighm \ MCL(\chi_{t-1}, u_t, z_t, m): \\ \ 2: \ \quad static \ w_{slow}, w_{fast} \\ \ 3: \ \quad \overline{chi}_t = \chi_t = \varnothing \\ \ 4: \ \quad for \ m = 1 \ to \ M \ do \\ \ 5: \ \qquad x^{[m]}_t = \boldsymbol{sample\_motion\_model}(u_t, x^{[m]}_{t-1}) \\ \ 6: \ \qquad w^{[m]}_t = \boldsymbol{measurement\_model}(z_t, x^{[m]}_t, m) \\ \ 7: \ \qquad \overline{\chi}_t = \overline{\chi}_t + \langle x^{[m]}_t, w^{[m]}_t \rangle \\ \ 8: \ \qquad w_{avg} \ = \ w_{avg} + \frac{1}{M} w^{[m]}_t \\ \ 9: \ \quad endfor \\ 10: \ \quad w_{slow} = w_{slow} + \alpha_{slow}(w_{avg} - w_{slow}) \\ 11: \ \quad w_{fast} = w_{fast} + \alpha_{fast}(w_{avg} - w_{fast}) \\ 12: \ \quad for \ m=1 \ to \ M \ do \\ 13: \ \qquad with \ probability \ max\{0.0, \ 1.0 - w_{fast}/w_{slow} \} \ do \\ 14: \ \qquad \quad add \ random \ pose \ to \ \chi_t \\ 15: \ \qquad else \\ 16: \ \qquad \quad draw \ i \ \in \ \{1,...,N\} \ with \ probability \ \propto \ w^{[i]}_t \\ 17: \ \qquad \quad add \ x^{[i]}_t \ to \ \chi_t \\ 18: \ \qquad endwith \\ 19: \ \quad endfor \\ 20: \ \quad return \ \chi_t }$
このアルゴリズムは、短期・長期それぞれの平均尤度 ${p(z_t | z_{1:t-1}}, u_{1:t}, m$ をトラッキングする点で適応的である。短期・長期の平均尤度は１０、１１行目で求めている。なお上記 ${\alpha_{fast}}$ 、 ${\alpha_{slow}}$ は、短期・長期の平均を求める指数フィルタの減衰率であり、 ${0 \leq \alpha_{slow} \ll \alpha_{fast}}$ を満たすものとする。
　このアルゴリズムの肝であるランダムサンプルの追加は、１３行目で行っている。１３行目に示すように、短期の平均が長期の平均を下回る、つまり[tex:{1.0 - w_{fast}/w_{slow} \rt 0}}の時、ランダムサンプルが追加される。これにより、観測尤度が急に減少すると、その文ランダムサンプルの沢山追加される。

8.3.6 Modifying the Proposal Distribution

　MCLの提案分布は、MCLを非効率なものにする原因の１つである。もしも提案分布と目標分布の差が大きくなれば、精度向上のためにサンプル数を増やす必要がある。提案分布が非効率なことで、ノイズのないセンサの観測結果からロボットの姿勢が寸分の狂いもなくわかるとすると、MCLが失敗することになる。そのためMCLを使う場合、精度の良いセンサよりも悪いセンサを使うことが望ましい。別の方法として、観測モデルに人工的なノイズを乗せるようにてもよい。
　その他の方法として、サンプリングプロセスを改良も考えられる。例えば動作モデルと観測モデルの役割を入れ替える、混合モデルが考えられる。混合モデルでは、観測モデルがパーティクルの生成、重みの更新を行う。混合モデルは、誤差の低減や誘拐ロボット問題へ対応が可能だが、実装には困難が伴うという問題もある。

8.4 Localization in Dynamic Environment

　これまでに述べたローカライゼーション手法の重要な制約は、静的な環境を仮定している。そのため、人などの動体を考慮していない。ただし確率的なアプローチでは、ノイズを考慮しているため、結果としてある程度動体を考慮している。一方で、センサノイズは各ステップ毎に独立していると仮定しているが、実際には数ステップにまたがって影響している。
　こうした動的な環境に対応する方法として、状態拡張（state augmentation）と外れ値の除外（outlier rejection）という２つの基本技術がある。前者を適用することで、ロボットの姿勢だけでなく人の特性（位置、速度など）を推定できるフィルタを定義できる。ただし計算が複雑になると言った問題がある。後者は、センサデータがなぜこの値になったかの原因追求と、モデル化していない動体の影響を受けたと思われる高尤度なデータを排除する。
　この方法は、本質的にはEMアルゴリズムと同じことを行う。ここで、観測モデルとして6.3節の式(6.12)を考え、新たに変数 ${\overline{c}^k_t} \in \{ hit, short, max, rand \}$ を導入する。

${ p(z^k_t | x_t, m) = \begin{pmatrix} z_{hit} \\ z_{short} \\ z_{max} \\ z_{rand} \end{pmatrix}^T \cdot \begin{pmatrix} p_{hit} (z^k_t | x_t, m) \\ p_{short} (z^k_t | x_t, m) \\ p_{max} (z^k_t | x_t, m) \\ p_{rand} (z^k_t | x_t, m) \end{pmatrix} \tag{6.12) = (8.8} }$

ここで ${p_{short}}$ は、 ${ p(\overline{c}^k_t \ = \ short \ | \ z^k_t, z_{1:t-1}, u_{1:t}, m)}$ を示すものとする。

${ \ 1: \ Algorithm \ test\_range\_measurement(z^k_t, \hat{\chi}_t, m): \\ \ 2: \ \quad p \ = \ q \ = \ 0 \\ \ 3: \ \quad for \ m \ = \ 1 \ to \ M \ do \\ \ 4: \ \qquad p \ = \ p + z_{short} \cdot p_{short}(z^k_t | x^{[m]}_t, m) \\ \ 5: \ \qquad q \ = \ q + z_{hit} \cdot p_{hit}(z^k_t | x^{[m]}_t, m) + z_{short} \cdot p_{short}(z^k_t | x^{[m]}_t, m) + z_{max} \cdot p_{max}(z^k_t | x^{[m]}_t, m) + z_{rand} \cdot p_{rand}(z^k_t | x^{[m]}_t, m) \\ \ 6: \ endfor \\ \ 7: \ if \ p/q \ \leq \ \chi \ then \\ \ 8: \ \qquad return \ accept \\ \ 9: \ else \\ 10: \ \qquad return \ reject \\ 11: \ endif }$

　アルゴリズムは上記の通り。入力はbelief ${\overline{bel}(x_t)}$ の代替であるパーティクルの集合 ${\hat{\chi}_t}$ とレンジファインダの観測データ ${z_t}$ 、地図 ${m}$ である。出力は入力した ${\hat{\chi}_t}$ の"reject"か"accept"のどちらか一方である。
　reject/acceptの判断は、観測結果が想定よりも驚くほど短いかどうかで行う。地図上に人などの動体が存在する場合、壁よりも近い場所に何かを検出することになる。そのため、観測結果が想定よりも顕著に短い場合はrejectする。
　外れ値のrejectは、一般的には良い考え方である。測距計以外のセンサでも同様の考え方を使って観測モデルを更新できる。