構建地圖 Occupancy Grid Mapping（機器人學簡介）

本篇文章主要內容是簡介 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 時間的地圖機率。