本篇文章主要內容是簡介 Probabilistic Robotics 書中的 9.2 章：Occupancy Grid Mapping。本文延續了前文狀態估計的內容，所用的數學記號跟前文一樣。
如何表示場景中的地圖？
Occupancy Grid Mapping 的模型是將整個場景分成許多的小網格 \(m_i\)，並且用 \(m\) 表示整張地圖。每個小網格都有兩個值 0 或 1，代表此網格是否被占用，被占用的小網格上面可能就是有牆壁或是其他障礙物等等。在計算地圖時我們想要算的是以下式子：
\[
p(m \mid z_{1:t},x_{1:t})
\]
意思是利用到目前為止所有的狀態 \(x_{1:t}\) 以及觀測資料 \(z_{1:t}\) 給定的情況之下來算出整個地圖的機率。
上式的問題在於 m 的維度，比如說如果整個場景中有 10000 個小網格，整個場景地圖的所有可能性為 \(2^{10000}\)，因此為了方便計算我們先假設每個小網格都是獨立的，也就是：
\[
p(m \mid z_{1:t},x_{1:t}) = \prod_{i}p(m_i \mid z_{1:t},x_{1:t})
\]
接下來的部分就是要從上式推導出一個實務上可用的演算法。
推導 Occupancy Grid Mapping 式子
我們從上式開始推導。推導的過程中會用到一些數學假設，在前文中有比較詳細的介紹：
\[
p(m_i \mid z_{1:t},x_{1:t})=\frac{p(z_t \mid m_i, z_{1:t-1}, x_{1:t})p(m_i \mid z_{1:t-1}, x_{1:t})}{p(z_t \mid z_{1:t-1}, x_{1:t})}
\]
上式用了貝氏定理。接下來我們利用前文提到的馬可夫假設可以把 \(p(z_t \mid m_i, z_{1:t-1}, x_{1:t})\) 代換成 \(p(z_t \mid m_i, x_{t})\)，再用貝氏定理把 \(p(z_t \mid m_i, x_{t})\) 代換一次：
\[
p(z_t \mid m_i, x_{t}) = \frac{p(m_i \mid z_t, x_t)p(z_t \mid x_t)}{p(m_i \mid x_t)}
\]
另外同樣用馬可夫假設把 \(p(m_i \mid z_{1:t-1}, x_{1:t})\) 代換成 \(p(m_i \mid z_{1:t-1}, x_{1:t-1})\)，因為少一個 \(x_t\) 並不會影響計算地圖網格 \(m_i\) 的結果。
將上述兩者代回原式：
\[
p(m_i \mid z_{1:t},x_{1:t})=\frac{p(z_t \mid m_i, z_{1:t-1}, x_{1:t})p(m_i \mid z_{1:t-1}, x_{1:t-1})}{p(z_t \mid z_{1:t-1}, x_{1:t})}
\\
= \frac{p(m_i \mid z_t, x_t)p(z_t \mid x_t)p(m_i \mid z_{1:t-1}, x_{1:t-1})}{p(m_i \mid x_t)p(z_t \mid z_{1:t-1}, x_{1:t})}
\]
另外，光是只有 \(x_t\) 不會影響計算地圖網格 \(m_i\) 的結果，所以 \(p(m_i \mid x_t) = p(m_i)\)，代回上式可得：
\[
p(m_i \mid z_{1:t},x_{1:t})= \frac{p(m_i \mid z_t, x_t)p(z_t \mid x_t)p(m_i \mid z_{1:t-1}, x_{1:t-1})}{p(m_i)p(z_t \mid z_{1:t-1}, x_{1:t})}
\]
如何從 t-1 時間的地圖推得 t 時間的地圖？
跟前文一樣，我們想要一步一步隨著時間更新地圖，也就是從 t-1 時間的地圖來推至 t 時間的地圖。這裡會用到的技巧是計算機率的發生比（Odds），也就是：\[\frac{p(x)}{1-p(x)}\]把 Occupancy Grid Mapping 的發生比寫出來可以得到以下式子：
\[
\frac{p(m_i \mid z_{1:t},x_{1:t})}{p(\neg m_i \mid z_{1:t},x_{1:t})}=\frac{p(m_i \mid z_t, x_t)p(z_t \mid x_t)p(m_i \mid z_{1:t-1}, x_{1:t-1})}{p(m_i)p(z_t \mid z_{1:t-1}, x_{1:t})}
\\
*
\frac{p(\neg m_i)p(z_t \mid z_{1:t-1}, x_{1:t})}{p(\neg m_i \mid z_t, x_t)p(z_t \mid x_t)p(\neg m_i \mid z_{1:t-1}, x_{1:t-1})}
\\
= \frac{p(m_i \mid z_t, x_t)p(m_i \mid z_{1:t-1}, x_{1:t-1})p(\neg m_i)}{p(\neg m_i \mid z_t, x_t)p(\neg m_i \mid z_{1:t-1}, x_{1:t-1})p(m_i)}
\]
接下來用一個函數 \(l(x)=log(p(x)/1-p(x))\) 來代換上式：
\[
l(m_i \mid z_{1:t},x_{1:t})=l(m_i \mid z_t,x_t) + l(m_i \mid z_{1:t-1},x_{1:t-1}) - l(m_i)
\]
上面的式子就是我們要的結果了！因為 t 時間的 \(l(m_i \mid z_{1:t},x_{1:t})\) 可以從 t-1 時間的 \(l(m_i \mid z_{1:t-1},x_{1:t-1})\) 推導而來。另外 \(l(m_i)\) 為此地圖的先驗，而 \(l(m_i \mid z_t,x_t)\) 稱為 inverse sensor model，計算的細節請參考 Probablistic Robotics 書中的介紹。
結論
本文介紹了 Occupancy Grid Mapping 的模型，並且也推導了演算法用到的式子。跟前文狀態估計一樣，我們推導的是遞迴式子，也就是從 t-1 時間可以推導出 t 時間的地圖機率。
沒有留言:
張貼留言