2017-06-27

論文紹介：Information-based Active SLAM via Topological Feature Graphs

どんなもの

　Topological Feature Graph(TFG)を使ったActive SLAMの提案。占有格子地図(Occupancy Grid Map)に比べ、地図のスケールの精度や長時間実行時のドリフトが少ない。

先行研究と比べてどこがすごい？

　特徴量ベースなトポロジカルマップでActive SLAMを可能にしたこと。

Active SLAM

　未知領域に対し、ロボット自身がパスプランニングして行うSLAM

技術の手法やキモはどこ？

どうやって有効と検証？

　シミュレーション上で、変数の数やCPUのアイドル時間、位置・姿勢の誤差を比較した。姿勢の誤差を除き、全て占有格子地図を使ったActive SLAM(nearest-frontier exploration algorithm)よりもよい性能を示した。

議論はある？

　比較対象が、占有格子地図（メトリックマップタイプの地図）のSLAMであるため、TFGと同じトポロジカルマップのActive SLAMと比べる必要がある。

次に読むべき論文？

　今は思いつかず

参考文献

http://people.csail.mit.edu/lpaull/publications/Mu_CDC_2016.pdf

2017-05-21

Probabilistic Robotics - Chap.9 Occupancy Grid Mapping

ロボット確率ロボティクス

9.1 Introduction

　今まではロボットが動作を開始する前に、環境地図が既知であることを仮定していた。ただこれは現実世界では稀なケースである。そこでロボットが自律的に環境地図を知る必要がある。ただし、環境地図の獲得には、下記２つの理由から困難な課題の１つである。

地図の候補空間は巨大である
地図を得ることは"鶏と卵"の問題がある

困難度合いを決める要因には、下記４つがある。

地図のサイズ（地図を作る上で対象となる空間の大きさ）
認識・動作におけるノイズの程度
認識の曖昧さ（特徴量が少ない、似たような光景が広がっているなど）
周回路（ループクロージャの問題）

　本章では、ロボットの姿勢は既知と仮定（ ${mappig \ with \ known \ pose}$ 問題）。Grid Mappingは、ロボットの姿勢が既知という仮定の元、ノイズを含む不確かなセンサデータから環境地図を生成する技術である。地図中の各Gridは、 ${occupied}$ か ${free}$ のどちらかの値を取る。どちらの値を取るかは、近似的に実装する事後確率で表現する。
Occupancy Grid Mapは、SLAMやパスプランニングに使うことは少ない。

9.2 The Occupancy Grid Mapping Algorithm

環境地図に関する事後確率は
${ p(m | z_{1:t}, x_{1:t}) \tag{9.1} }$
で表される。ここで ${x_{1:t}}$ は、計測開始後から現在までのロボットの経路（各時刻でのロボットの姿勢）を、 ${z_{1:t}}$ は計測開始後から現在までのセンサデータを、それぞれ表す。
なおOccupancy Grid Mapは一般的に２次元の地図を表すが、何層にも重ねることで３次元の地図を形成することもできる。ただし計算コストが膨大なため、通常Occupancy Grid Mapで３次元地図は構成しない。

環境地図 ${m}$ は、沢山のセルで構成されることから、
${ m \ = \ \{m_i\} }$
とできる。このとき ${m_i \in \{0, \ 1\}}$ 、 ${p(m_i \ = \ 1) \ = \ p(m_i)}$ とする。

式(9.1)の問題は、次元数である。もしも地図が10,000個のセルで構成される場合、地図の次元は ${2^{10000}}$ となる。そのため、地図がセルの集合体だと考えると、計算量が膨大となる。

そこで標準的なOccupancy Grid Mapは、各セルに注目して事後確率を計算する。つまり ${p(m_i | z_{1:t}, x_{1:t})}$ を計算し、
${ \displaystyle p(m | z_{1:t}, x_{1:t}) = \prod_i p(m_i | z_{1:t}, x_{1:t}) }$
のように統合することで、環境地図の事後確率を求める。

Occupancy Grid Mappingでは、各グリッドの状態を表すのに下記式(9.5)で表される対数オッズを使う。
${ \displaystyle l_{t, i} \ = \ log \frac{p(m_i | z_{1:t}, x_{1:t})}{1 \ - \ p(m_i | z_{1:t}, x_{1:t})} \tag{9.5} }$
対数オッズを使うメリットは、事後確率が計算結果が不安定になる０、１の近傍を取らなくなることである。

${ 1: \ Algorithm \ occupancy\_grid\_mapping(\{l_{t-1, i}\}, x_t, z_t): \\ 2: \ \quad for\ all \ cells \ m_i \ do \\ 3: \ \qquad if \ \ m_i \ \ in \ perceptual \ field \ of \ z_t \ then \\ 4: \ \qquad \quad l_{t, i} = l_{t-1, i} + inverse\_sensor\_model(m_i, x_t, z_t) - l_0 \\ 5: \ \qquad else \\ 6: \ \qquad \quad l_{t,i} = l_{t-1, i}\\ 7: \ \qquad endif \\ 8: \ \quad endfor \\ 9: \ \quad return \{l_{t,i}\} }$

なお式中の ${l_0}$ は、対数オッズで表現されるOccupancy Grid Mapの事前分布で、
${ \displaystyle l_0 = log \frac{p(m_i \ = \ 1)}{p(m_i \ = \ 0)} \ = \ log \frac{p(m_i)}{1 \ - \ p(m_i)} }$
とする。ここで、 ${p(m_i) \ = \ p(m_i \ = \ 1)}$ であり、
${ \displaystyle inverse\_sensor\_model(m_i, x_t, z_t) \ = \ log \frac{p(m_i | z_t, x_t)}{1 - p(m_i | z_t, x_t)} }$
である。

なお、Occupancy Grid Mapを作成する時は、ロボット自身が専有する領域（ロボットの身体）を考慮することは大切である。特に通行人が多いとき等には有用である。

9.3 Learning Inverse Measurement Models

9.3.1 Inverting the Measurement Model

Occupancy Grid Mapは、各セルの逆観測モデル ${p(m_i | x, z)}$ が必要である。
Table9.2ではとりあえずの逆観測モデルを定義した。
ここでいう逆観測モデルは、複数のマップを用意し、それぞれのマップの ${m^k_i \ = \ m_i}$ を満たすマップを使って逆観測モデルを近似する。つまり、
${ \displaystyle p(m_i | x, z) \ = \ \eta \sum_{m:m(i)=m_i} p(z | x, m) p(m) }$
となる。上記の式は計算不可能のため、ロジスティクス回帰やニューラルネットワーク等の教師付き学習アルゴリズムを使って近似的に求める。

2017-05-07

Probabilistic Robotics - Chap.6 Robot Perception

1.Introduction
６章に記載されているのはEnvironment Measurement Model（環境計測モデル）
確率ロボティクスではセンサデータの計測に伴うノイズもモデル化する

${ \displaystyle p(z_t | x_t, m) \\ z_t：時刻tでのセンサデータ\\ x_t：時刻tでのロボットの位置\\ m：環境地図 }$

確率ロボティクスでは、センサモデルの不正確さに対し、統計的手法を用いて対応する

${ \displaystyle z_t = \{z^1_t, ..., z^K_t\} z^k_t:センサAが時刻tにおいて計測した個々のデータ（LRFならば角度\thetaにおける距離） }$

${ \displaystyle p(z_t | x_t, m) = \prod_{ k = 0 }^K p(z^k_t | x_t, m) }$
この式は、 ${p^k_t}$ がそれぞれ独立していることを仮定しているが、理想状態でしかそれはありえない。

6.2 Maps

地図 ${m}$ はオブジェクトのリストを表す
${\displaystyle m = \{m_1, ... , m_N\} }$

地図には２種類ある

特徴ベース：オブジェクトの位置は地図中に含まれている。形状情報のみ特徴として持つ。そのため、ロボット用のマップとしてはこちらの形式がよく使われる。
位置ベース： ${m}$ の要素は環境中に存在するオブジェクトの位置を表す。各オブジェクトの位置をデータとして保つ必要があるので、データサイズが大きい。空きスペースに関しての情報も保つ必要がある、つまり全セルの情報を保つ必要がある。

6.3 Beam Models of Range Finders

Range Finderは最もよく使われるセンサ

6.3.1 The Basic Measurement Algorithm

ここで紹介するモデルは４つのエラーを考慮する

分解能によって生じる誤差

正しい距離は計測できているものの、分解能の制約により誤差が乗る場合がある。
正しい距離を ${z^{k*}_t}$ とする
本計測誤差は、平均 ${z^{k*}_t}$ 、分散 ${\sigma_{hit}}$ で表される狭いガウス分布でモデル化できる。
${\sigma_{hit}}$ は本ノイズモデル固有のパラメータ

f:id:graziegrazie:20170405202747j:plain

予想外の物体

環境地図に元々含まれていない物体は、元々の地図に記載していたよりも短い距離で見つかることある
尤度は物体との距離に応じて減少していくことから、指数分布で表される。
この指数分布はモデル固有のパラメータ ${\lambda_{short}}$ を使って表せる。

${ \begin{eqnarray} p_{short}(z^k_t|x_t,m) = \begin{cases} \eta \lambda_{short} e^{-\lambda_{short} z^k_t} if 0 \gt z^k_t \gt z^{k*}_t \\ 0 otherwise \end{cases} \end{eqnarray} }$

指数分布は自らの位置から本来の対象の位置までの尤度を表すため、累積確率は

計測失敗

計測可能レンジの超過
つまり計測値は最大値を示す

ランダム計測

ごくたまに起きる説明の使いない計測結果

それぞれのエラーの密度関数は、それぞれ重みを掛けて合体することで、本モデルの確率密度関数を表現できる

Limitation of the Beam Model

Beamモデルには２つの主な欠点がある
滑らかさがない
　椅子を検知したら椅子の足それぞれマッピングするかもしれない
　ロボットの位置が変化したら、beamの座標変換の計算量が大変なことになる
滑らかさの欠如は２つの問題を招く
　beliefの近似が正しい状態を見失いやすい
　山登り法は局所最大値に陥りやすい
計算量が膨大

4 Liklihood Fields for Range Finders

6.3の制限を解決する尤度フィールドモデルについて述べる。
確率計算などはしないが、実際に使ってみると良い結果が求まる、アドホックなアルゴリズム。

キーとなるアイデアは、 ${z_t}$ の端点をグローバルマップ上に投影すること。
これにより、グローバルマップとロボットのローカルマップの位置関係がわかる。
投影される端点の座標は、
${ \left( \begin{array}{ccc} x_{z^k_t} \\ y_{z^k_t} \end{array} \right) = \left( \begin{array}{ccc} x \\ y \end{array} \right) + \left( \begin{array}{ccc} cos \theta & -sin \theta \\ sin \theta & cos \theta \end{array} \right) \left( \begin{array}{ccc} x_{k,sens} \\ y_{k,sens} \end{array} \right) + z^k_t \left( \begin{array}{ccc} cos \left(\theta + {\theta}_{k,sens} \right) \\ sin \left(\theta + {\theta}_{k,sens} \right) \end{array} \right) \tag{6.32} }$
として表される。
このとき、 ${ x_t = \left( \begin{array}{ccc} x & y & \theta \end{array} \right)^T }$ はロボットの座標、
${ \left( \begin{array}{ccc} x_{k,sens} & y_{k,sens} \end{array} \right)^T }$
はセンサ検出データの端点
をそれぞれ示す。

Beam Modelと同様に、こちらは３種類のノイズを定義

計測ノイズ

　計測過程で起こる、ガウス分布でモデル化されるノイズ

計測失敗

　センサの最大計測可能距離の時を指す。その時、値が正しいかは不明。

説明の付かない計測結果

　ランダムに発生するノイズ

5 Correlation-Based Measurement Models

文献でよく見かけるmap matchingを使うモデル。
map matchingは、連続するスキャンデータをローカルマップへ落とし込む
つまりmap matchingは、グローバルマップを ${m}$ 、ローカルマップを ${m_{local}}$ とした時、尤度 ${p\left(m_{local} | x_t, m \right)}$ が高い場所を探すことを指す

尤度は、
${ p\left(m_{local} | x_t , m \right) = max \left\{ {\rho}_{m , m_{local} , x_t} , 0 \right\} }$
で表され、グローバルマップとローカルマップの相関係数 ${{\rho}_{m , m_{local} , x_t}}$ と平均地図 ${\overline{m}}$ はそれぞれ、
${ \rho_{m , m_{local} , x_t} = \frac { \sum_{x, y} \left(m_{x, y} - \overline{m} \right) \left( m_{x, y, local} \left( x_t \right) - \overline{m} \right) } { \sqrt {\sum_{x, y} \left(m_{x, y} - \overline{m} \right)^2 \sum_{x, y} \left( m_{x, y, local} \left( x_t \right) - \overline{m} \right)^2 } } }$
${ \overline{m} = \frac{1}{2N} \sum_{x, y} \left( m_{x, y} + m_{x, y, local} \right) }$
尤度フィールドに対するmap matchingの利点は、相関係数に応じてフリースペースを決めること。これにより、センサの計測範囲外のデータも考慮してマップを作ることができる
一方map matchingの欠点は、物理的に明確な説明ができないこと

Feature-Based Measurement Model

今まではセンサの生値をベースとした話
これは一歩進んだ、センサデータから求めた特徴量をベースとした話
Featureベースの利点は、特徴量を低次元化することで計算量の削減が可能な点

ロボットの世界では、環境中の特徴量はランドマークと呼ぶことが一般的
特徴量を求める時に一般的なのは、センサがセンサを基準としたランドマークとの距離と方角を計測できるとする仮定
加えて、特徴量抽出器はサイン（本書では数値で表されるとする）を出力する可能性がある

特徴量ベクトル ${f}$ は、距離 ${r}$ と方角 ${\phi}$ 、サイン ${s}$ それぞれを使って
${ f \left( z_t \right) = \left\{ f^1_t, f^2_t, ... \right\} = \left\{ \left( \begin{array}{ccc} r^1_t \\ \phi^1_t \\ s^1_t \end{array} \right) , \left( \begin{array}{ccc} r^2_t \\ \phi^2_t \\ s^2_t \end{array} \right) ,... \right\} }$

確率ロボティクスのアルゴリズムでは下記の条件付き確率を仮定する
${ \displaystyle p \left( f \left( z_t \right) | x_t, m\right) = \prod_i p(r^i_t, \phi^i_t, s^i_t | x_t, m) }$

特徴量ベース地図に向けたセンサモデルの改定
特徴量の地図上での位置を ${m_i}$ とし、ロボットの座標を ${ \left( \begin{array}{ccc} x \ \ y \ \ \theta \end{array}\right)^T }$ とすると、特徴量の位置は
${ \left( \begin{array}{ccc} r^i_t \\ \phi^i_t \\ s^i_t \end{array} \right) = \left( \begin{array}{ccc} \sqrt{ \left( m_{j, x} - x \right)^2 + \left(m_{j, y} - y \right)^2 } \\ atan2 \left( m_{j, y} - y, m_{j, x} - x \right) - \theta \\ s_j \end{array} \right) + \left( \begin{array}{ccc} \epsilon_{\sigma^2_r} \\ \epsilon_{\sigma^2_\phi} \\ \epsilon_{\sigma^2_s} \end{array} \right) }$
ここで ${\epsilon_{\sigma^2_r}と\epsilon_{\sigma^2_r}, \epsilon_{\sigma^2_r}}$ は、平均０で、分散がそれぞれ ${\sigma_r, \sigma_\phi, \sigma_s}$ のガウシアン誤差変数

距離・角度センサの主要な問題点は、データの関連付けである。
これを解決するために ${f^i_t}$ と ${m_j}$ の間の一致変数 ${c}$ を導入する
${c_i = j \leq N}$ の時、 ${i}$ 番目の特徴量は ${j}$ 番目のランドマークと一致する
${N}$ はランドマークの総量

Sampling Poses

今までは特徴量の確からしさについて議論してきた
一方で、特徴量と地図情報からロボットの座標を推定したい時もある
そのためには、事前確率 ${p \left( x_t | c^i_t, m \right)}$ が必要である。
あり得ないが事前確率を均一分布とするとロボットの座標の尤度は
${ p \left( x_t | f^i_t, c^i_t, m \right) = \eta \ p \left( f^i_t | c^i_t, x_t, m \right) p \left( x_t | c^i_t, m \right) \\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ = \eta \ p \left( f^i_t | c^i_t, x_t, m \right) }$

2017-05-07

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を参照のこと

2017-05-02

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は、一般的には良い考え方である。測距計以外のセンサでも同様の考え方を使って観測モデルを更新できる。

2017-05-01

Probabilistic Robotics - Chap.5 Robot Motion

確率ロボティクスロボット

5.1 Introduction

　モーションモデルはベイズフィルタの予測ステップにおいて肝となる状態遷移確率を構成する。本章では、ロボットの運動はある平面上での移動のみを取り上げる。

5.2 Preliminaries

5.2.1 Kinematic Configuration

　ロボットの姿勢は下記の式で表せるものとする。
${ \begin{pmatrix} x \\ y \\ \theta \end{pmatrix} }$
ロボットの向きはよく、 ${bearing}$ や ${heading direction}$ と呼ばれる。また姿勢から向きを取ったものは ${位置}$ と呼ぶ。

5.2.2 Probabilistic Kinematics

　状態遷移確率は ${p(x_t | u_t, x_{t-1}}$ として表される。 ${x_t, x_{t-1}}$ はそれぞれ、時刻 ${t, t-1}$ でのロボット状態を、 ${u_t}$ は時刻 ${t}$ でのモーションコマンドを、それぞれ表すものとする。モーションモデルにおいて、 ${u_t}$ はオドメトリを指す場合もある。
　本章では、下記２種類の確率的モーションモデルを取り上げる。

速度モーションモデル

　 ${u_t = Velocity}$ なモデル。ただし速度を指令値通り制御することは、ノイズ等の都合上難しいことため、精度は良くないことが多い。

オドメトリモーションモデル

　 ${u_t = odometry}$ なモデル。速度モデルよりも精度が高いことが多い。
ただしオドメトリは動いた結果わかる情報である。そのため、動作の予測には使えない。そのため、速度モデルはモーションプランニングに、オドメトリモデルは状態推定に、それぞれ使うことが一般的である。以降ではそれぞれについて詳細を述べる。

5.3 Velocity Motion Model

　本モデルは、ロボットの速度 ${u_t}$ は並行運動 ${v_t}$ と回転運動 ${\omega_t}$ の２種類で構成されると過程する。なお ${v_t}$ と ${\omega_t}$ は、それぞれ１次元のスカラ値である。

5.3.1 Closed Form Calculation

　速度モデルは、下記のアルゴリズムで表現される。
${ 1: \ \ Algorithm motion\_model\_velocity(x_t, u_t, x_{t-1}):\\ 2: \ \ \ \ \ \mu = \frac{1}{2} \frac{(x - x') cos\theta + (y - y') sin \theta}{(y - y') cos\theta - (x - x') sin \theta}\\ 3: \ \ \ \ \ x^{*} = \frac{x + x'}{2} + \mu (y - y') \\ 4: \ \ \ \ \ y^{*} = \frac{y + y'}{2} + \mu (x' - x) \\ 5: \ \ \ \ \ r^{*} = \sqrt{(x - x^{*})^2 + (y - y^{*})^2} \\ 6: \ \ \ \ \ \Delta \theta = atan2(y' - y^{*}, x' - x^{*}) - atan2(y - y^{*}, x - x^{*}) \\ 7: \ \ \ \ \ \hat{v} = \frac{\Delta \theta}{\Delta t} r^{*} \\ 8: \ \ \ \ \ \hat{\omega} = \frac{\Delta \theta}{\Delta t} \\ 9: \ \ \ \ \ \hat{\gamma} = \frac{\theta' - \theta}{\Delta t} - \hat{\omega} \\ 10: \ \ \ \ return \ prob(v - \hat{v}, \alpha_1 v^2 + \alpha_2 \omega^2) \cdot prob(\omega - \hat{\omega}, \alpha_3 v^2 + \alpha_4 \omega^2) \cdot prob(\hat{\gamma}, \alpha_5 v^2 + \alpha_6 \omega^2) }$

ここで ${\alpha_1}$ から ${\alpha_6}$ は、ロボット固有の誤差パラメータでる。また ${prob(x, b^2)}$ は、モーションエラーをモデル化した関数を表す。この固有パラメータの決定方法について、特に本書には記載がないです。恐らく指令値と実測値の組み合わせを記録していき、その結果が良い感じに ${prob(x, b^2)}$ の結果とフィットするようにパラメータチューンが必要と思われます。

5.3.2 Sampling Algorithm

　5.3.1では計算で求めていましたが、本節ではパーティクルフィルタの様にサンプリングを介して解析に状態遷移確率を求める手法を紹介します。サンプル点から求める際の下記の手法を使う。
${ 1: \ \ Algorithm sample\_motion\_model\_velocity(u_t, x_{t-1}):\\ 2: \ \ \ \ \ \hat{v} = v + \boldsymbol{sample}(\alpha_1 v^2 + \alpha_2 \omega^2) \\ 3: \ \ \ \ \ \hat{\omega} = \omega + \boldsymbol{sample}(\alpha_3 v^2 + \alpha_4 \omega^2) \\ 4: \ \ \ \ \ \hat{\gamma} = \boldsymbol{sample}(\alpha_5 v^2 + \alpha_6 \omega^2) \\ 5: \ \ \ \ \ x^{'} = x - \frac{\hat{v}}{\omega} sin \theta + \frac{\hat{v}}{\omega} sin (\theta + \hat{\omega} \Delta t \\ 6: \ \ \ \ \ y^{'} = y + \frac{\hat{v}}{\omega} cos \theta - \frac{\hat{v}}{\omega} cos (\theta + \hat{\omega} \Delta t \\ 7: \ \ \ \ \ \theta^{'} = \theta + \hat{\omega} \Delta t + \hat{\gamma} \Delta t \\ 8: \ \ \ \ \ return \ x_t = (x^{'}, y^{'}, \theta^{'})^T\\ }$
２～４行目は、速度パラメータにノイズを加えます。その結果を使い、５～７行目で姿勢を計算しています。サンプリング手法を使う場合、運動方程式等の物理モデルを考慮せず、サンプル点の位置からのみ姿勢が求まるという利点がある。そのため実装の観点からすると、サンプリング手法を利用ことが楽である。

5.4 Odometry Motion Model

　オドメトリは、ドリフトやスリップによってエラーは生じるものの、速度に比べて計測誤差が小さい。また実際に動いた後でないと計測できないため、モーションプランニングには使えない。

5.4.1 Closed Form Calculation

　オドメトリをベースとしたモーションモデルは、今日の確率ロボティクスベースのシステムで中心的な役割を果たしている。
オドメトリは、ロボット内部の情報量である。そのため、実世界とリンクするためには座標変換が必要である。ただしドリフトやスリップが生じるため、変換行列は一定ではない。なおこの変換行列を求めることは、自己位置推定を行うことと等価である。
　オドメトリモデルは相対動作情報を使用する。相対動作情報とは、時刻 ${t-1}$ から ${t}$ の間で、どの程度動いたかを表す情報である。ロボットの時刻 ${t-1}$ と ${t}$ の時の姿勢のペアを ${u_t}$ とすると、
${ u_t = \begin{pmatrix} \overline{x}_{t-1} \\ \overline{x}_t \end{pmatrix} }$
となる。
　この動作情報から相対動作情報を取り出すと、回転・直進・回転の３ステップに分割できる。つまり ${( \begin{pmatrix} \delta_{rot1} \delta_{trans} \delta_{rot2} \end{pmatrix} }$
と表せる。この情報が分かれば、ある時刻 ${t-1}$ から ${t}$ の間の移動を再現できる。
確率的モーションモデルでは、上記３パラメータはノイズが加わっているものと仮定する。

以上を踏まえた、オドメトリモデルでの状態遷移確率を求めるアルゴリズムは下記の通りである。
${ 1: \ \ \ Algorithm \ motion\_model\_odometry(x_t, u_t, x_{t-1}):\\ 2: \ \ \ \ \ \delta_{rot1} = atan2(\overline{y}^{'} - \overline{y}, \overline{x}^{'} - \overline{x}) - \overline{\theta} \\ 3: \ \ \ \ \ \delta_{trans} = \sqrt{(\overline{x}^{'} - \overline{x})^2 + (\overline{y}^{'} - \overline{y})^2} \\ 4: \ \ \ \ \ \delta_{rot2} = \overline{\theta}^{'} - \overline{\theta} - \delta_{rot1} \\ 5: \ \ \ \ \ \hat{\delta}_{rot1} = atan2(y^{'} - y, x^{'} - x) - {\theta} \\ 6: \ \ \ \ \ \hat{\delta}_{trans} = \sqrt{(x^{'} - x)^2 + (y^{'} - y)^2} \\ 7: \ \ \ \ \ \hat{\delta}_{rot2} = \theta^{'} - \theta - \delta_{rot1} \\ 8: \ \ \ \ \ p_1 = \boldsymbol{prob}(\delta_{rot1} - \hat{\delta}_{rot1}, \alpha_1 \hat{\delta}^2_{rot1} + \alpha_2 \hat{\delta}^2_{trans}) \\ 9: \ \ \ \ \ p_2 = \boldsymbol{prob}(\delta_{trans} - \hat{\delta}_{trans}, \alpha_3 \hat{\delta}^2_{trans} + \alpha_4 \hat{\delta}^2_{rot1} + + \alpha_4 \hat{\delta}^2_{rot2}) \\ 10: \ \ \ \ \ p_3 = \boldsymbol{prob}(\delta_{rot2} - \hat{\delta}_{rot2}, \alpha_1 \hat{\delta}^2_{rot2} + \alpha_2 \hat{\delta}^2_{trans}) \\ 11: \ \ \ \ \ return \ p_1 \cdot p_2 \cdot p_3 }$
なお ${\boldsymbol{prob}(a, b^2)}$ は、平均 ${0}$ で分散 ${b^2}$ の誤差分布における ${a}$ の発生確率を返す関数とする。また、 ${\alpha_1}$ から ${\alpha_4}$ は、ロボットの動作を特定するロボット固有のパラメータである。

上記アルゴリズムにおいて、各行が行っている内容は下記の通り

２～４行目：オドメトリ情報から相対動作情報 ${\begin{pmatrix} \delta_{rot1} \delta_{trans} \delta_{rot2} \end{pmatrix} }$ を抽出。
５～７行目：入力した姿勢 ${x_{t-1}}$ と ${x_t}$ から理想とする相対動作情報を算出。
８～１０行目：相対動作情報の各要素に対するエラーを確率的に計算
１１行目：相対動作情報の各要素に対するエラー発生確率をかけ合わせ、目的姿勢である ${x_t}$ からのずれを返す。

5.4.2 Sampling Algorithm

　サンプルベースでオドメトリモデルを求めるアルゴリズムは下記の通りである。 ${\boldsymbol{sample}(a,b^2}$ は、 ${a}$ の発生確率を返す、平均 ${0}$ で分散 ${b^2}$ な確率分布関数である。

${ 1: \ \ \ Algorithm \ motion\_model\_odometry(x_t, u_t, x_{t-1}):\\ 2: \ \ \ \ \ \delta_{rot1} = atan2(\overline{y}^{'} - \overline{y}, \overline{x}^{'} - \overline{x}) - \overline{\theta} \\ 3: \ \ \ \ \ \delta_{trans} = \sqrt{(\overline{x}^{'} - \overline{x})^2 + (\overline{y}^{'} - \overline{y})^2} \\ 4: \ \ \ \ \ \delta_{rot2} = \overline{\theta}^{'} - \overline{\theta} - \delta_{rot1} \\ 5: \ \ \ \ \ \hat{\delta}_{rot1} = \delta_{rot1} - \boldsymbol{sample}(\alpha_1 \delta^2_{rot1} + \alpha_2 \delta^2_{trans}) \\ 6: \ \ \ \ \ \hat{\delta}_{trans} = \delta_{trans} - \boldsymbol{sample}(\alpha_3 \delta^2_{trans} + \alpha_4 \delta^2_{rot1} + \alpha_4 \delta^2_{rot2}) \\ 7: \ \ \ \ \ \hat{\delta}_{rot2} = \delta_{rot2} - \boldsymbol{sample}(\alpha_1 \delta^2_{rot2} + \alpha_2 \delta^2_{trans}) \\ 8: \ \ \ \ \ x^{'} = x + \hat{\delta}_{trans} cos(\theta + \hat{delta}_{rot1}) \\ 9: \ \ \ \ \ y^{'} = y + \hat{\delta}_{trans} sin(\theta + \hat{delta}_{rot1}) \\ 10: \ \ \ \ \ \theta^{'} = \theta + \hat{\delta}_{rot1} + \hat{\delta}_{rot2} \\ 11: \ \ \ \ \ return \ x_t = (x^{'}, y^{'}, \theta^{'})^T }$

5.5 Motion And Maps

　今まで扱っていた状態遷移確率では、周囲環境を考慮していなかった。そこで状態遷移確率のパラメータとして、新たに地図 ${m}$ を導入する。地図を知ることで、壁などの到達不可能な場所にいるのか、そうでない到達可能な場所にいるのかが判別できる。これにより、ロボットの姿勢をより精度良く求める事が可能となる。
　地図の導入により、状態遷移確率は ${p(x_t | u_t, x_{t-1}, m)}$ となる。地図を知ることによって、今まで議論してきた状態遷移確率では「遷移可能」としてきた場所に遷移出来なくなるので、
${ p(x_t | u_t, x_{t-1}) \neq p(x_t | u_t, x_{t-1}, m) }$
となる。よって新規なモデルとなることから、マップベースモーションモデルと呼ぶことにする。
　マープベースモデルを閉形式で計算することは難しい。一方で、都合の良いことに計算効率のよい近似方法がある。近似式は下記の通り。
${ p(x_t | u_t, x_{t-1}, m) = \eta \frac{p(x_t | u_t, x_{t-1}) p(x_t | m)}{p(x_t)} }$
ここで ${\eta}$ は正規化係数である。通常 ${p(x_t)}$ は一様分布で表される。なお周囲環境は動的に変化しないことから、 ${m}$ は定数として考える。もしも ${p(x_t | m) = 0}$ の時は、ロボットは地図上に存在する障害物上に存在することを意味する。
　なお上記近似には問題がある。モデルでは地図を考慮しているため、ロボットが存在できる姿勢は全て遷移対象となる。しかし、例えば壁の向こう側に一瞬で行くのは難しいが、そうした物理的に遷移が可能かは考慮していない。そのため、遷移先の姿勢への移動コスト加味するなどの工夫が必要である。