問題設定

平面あるいは空間を格子状に分割し、各格子 $m_{i}$ に占有確率を付与する。すなわち地図とは格子の集合であり、 $m=(m_{1}, m_{2}, \dots)$ と表記する。Ground truth位置 $x_{t}$ と観測結果 $z_{t}$ から地図を推定するので、 $P(m | x_{1:t}, z_{1:t})$ を算出する問題と言える。原理的には、各地図 $n$ に対して

$\begin{eqnarray} P(n | x_{1:t}, z_{1:t}) &=& \displaystyle{\frac{P(x_{1:t}, z_{1:t}, n)}{P( x_{1:t}, z_{1:t})}} \\\ &=& \displaystyle{\frac{P(x_{1:t}, z_{1:t} | n)P(n)}{P( x_{1:t}, z_{1:t})}} \\\ &\sim& P(x_{1:t}, z_{1:t} | n)P(n) \end{eqnarray}$

となり、各地図に対するありうる確率を算出することができるが、地図候補サイズが格子数の指数で増えていくため全ての地図候補を算出することは実用上不可能である。

問題を簡単にするため、地図を各格子に対する部分問題に分割する。すなわち

$\begin{equation} P(m | x_{1:t}, z_{1:t}) \sim \displaystyle{\prod_{i} P(m_{i} | x_{1:t}, z_{1:t})} \end{equation}$

とする。ただし、この格子は各格子が互いに独立であることを前提にしており、以下の議論ではこの仮定をおく。ただしセンサー分解能より格子が細かい状況等ではこの仮定は不適切である。

地図作成アルゴリズム

逆観測モデルを使うやりかた

逆観測モデル $P(m | x_{t}, z_{t})$ を使って地図を作成するやり方を説明する。「逆観測」と表現しているのは、通常は地図が与えられて観測結果を表現するという因果関係に沿ったモデルを考えるが、その逆変換のモデルであるからである。

占有確率を求める際にオッズを使うと便利である。オッズとは

$\begin{eqnarray} L &=& \log \displaystyle \frac{P}{\overline{P}} \\\ &=& \log \displaystyle\frac{P}{1-P} \end{eqnarray}$

で確率 $P$ をオッズ $L$ に変換して扱うことである。これにより0や1付近の確率がオッズ座標では引き延ばされるので誤差などが蓄積しづらくなるメリットがある。

ここで

$\begin{eqnarray} P(m | x_{1:t}, z_{1:t}) &=& \displaystyle{\frac{P(x_{t}, z_{t} | m, x_{1:t-1}, z_{1:t-1}) P(m | x_{1:t-1}, z_{1:t-1})}{P(x_{t}, z_{t} | x_{1:t-1}, z_{1:t-1})}} \ \ \ \ \ (ベイズの定理(1) )\\\ &=& \displaystyle{\frac{P(m,x_{1:t}, z_{1:t})}{P(m, x_{1:t-1}, z_{1:t-1})}} \times \displaystyle{\frac{P(m | x_{1:t-1}, z_{1:t-1})}{P(x_{t}, z_{t} | x_{1:t-1}, z_{1:t-1} )}} \\\ &=& \displaystyle{\frac{P(x_{1:t-1}, z_{1:t-1}) P(m|x_{t}, z_{t})P(x_{t}, z_{t})}{P(m, x_{1:t-1}, z_{1:t-1})}} \times \displaystyle{\frac{P(m | x_{1:t-1}, z_{1:t-1})}{P(x_{t}, z_{t} | x_{1:t-1}, z_{1:t-1} )}} \\\ &=& \displaystyle{\frac{P(m|x_{t}, z_{t})P(x_{t}, z_{t})}{P(m)}} \times \displaystyle{\frac{P(m | x_{1:t-1}, z_{1:t-1})}{P(x_{t}, z_{t} | x_{1:t-1}, z_{1:t-1} )}} \\\ \end{eqnarray}$

（途中のベイズの定理についてはSLAM定式化を参照）

となるが、 $m=0, 1$ を明示的に書き、後でオッズをとるための形にすると

$\begin{eqnarray} \frac{P(m=1 | x_{1:t}, z_{1:t})}{P(m=0 | x_{1:t}, z_{1:t})} &=& &=& \displaystyle{\frac{P(m=0)}{P(m=1)} \times \frac{P(m=1 | x_{t}, z_{t})}{P(m=0 | x_{t}, z_{t})} \times \frac{P(m=1 | x_{1:t-1}, z_{1:t-1})}{P(m=0 | x_{1:t-1}, z_{1:t-1})}} \end{eqnarray}$

以上の考察から、時刻 $t$ までのオッズ $L(t)$ に関する漸化式を作ると

$\begin{eqnarray} L(t) &\equiv& \log \displaystyle \frac{P(m=1 | x_{1:t}, z_{1:t})}{P(m=0 | x_{1:t}, z_{1:t})} \\\ &=& \log \displaystyle{\frac{P(m=1 | x_{1:t-1}, z_{1:t-1})}{P(m=0 | x_{1:t-1}, z_{1:t-1})}} + \displaystyle{\frac{P(m=1 | x_{t}, z_{t})}{P(m=0 | x_{t}, z_{t})}} - \log \displaystyle{\frac{P(m=1)}{P(m=0)}} \\\ &=& L(t-1) + \displaystyle{\frac{P(m=1 | x_{t}, z_{t})}{P(m=0 | x_{t}, z_{t})}} - L(0) \end{eqnarray}$

したがって、時刻 $t$ の位置 $x_{t}$ と観測結果 $z_{t}$ が得られた時に、センサー観測範囲内の格子 $m_{i}$ について、逆観測モデル $P(m | x_{t}, z_{t})$ を使って漸化式でオッズを更新していけば良い。センサー観測範囲外の格子については当然更新しないことに注意。

直接探索するやりかた

尤度が最も高い地図を探索する方法である。すなわち $P(m | x_{1:t}, z_{1:t})$ を最大化する $m$ を探索する。

$\begin{eqnarray} P(m | x_{1:t}, z_{1:t}) &=& \displaystyle{\frac{P(z_{1:t} | m, x_{1:t}, ) P(m, x_{1:t}) }{P(x_{1:t}, z_{1:t})}} \\\ &\sim & P(z_{1:t} | m, x_{1:t}) P(m | x_{1:t}) \\\ &\sim & P(z_{1:t} | m, x_{1:t}) P(m) \\\ &=& P(m) \prod_{t} P(z_{t} | m, x_{t}) \ \ \ \ \ (観測結果に影響を与えるのは地図とその時の自己位置だけ) \end{eqnarray}$