本篇文章主要內容是簡介 Probabilistic Robotics 書中的第二章：Recursive State Estimation。
如何描述機器人的狀態？
- 狀態（state）指的是所有會對於機器人及環境造成未來影響的因素。狀態分成靜態狀態以及動態狀態：靜態狀態指的是不會變的狀態，像是環境中建築物的牆壁的位置；而動態狀態會隨著時間而改變，像是機器人周遭的行人，或是機器人本身的位置或速度等等，以下用 \(x_t\) 表示機器人在時間 t 的狀態。
- 感測器（sensor）的測量指的是機器人利用各種感測器取得環境的資訊，像是照相機或是雷達等等，以下用 \(z_t\) 表示感測器在時間 t 取得的資料。
- 控制資訊（control data）指的是機器人實際上做的事情，比如說機器人移動或旋轉。但是實際上機器人的移動可能會與真實世界中不同，比如說機器人可能認為自己走了一公尺，但是因為地面環境打滑的關係並沒有真正走到一公尺，這也是為什麼在機器人學中我們都用機率模型來描述機器人的狀態變化。以下會用 \(u_t\) 表示在 t 時間的控制資訊。
機器人的狀態轉換式
我們用 \(bel(x_t)\) 表示在 狀態 \(x_t\) 的可信度：
\[
bel(x_t) = p(x_t \mid z_{1:t}, u_{1:t})
\]
接著我們要做的是一步步地推導這個式子，最後會得到的結果是在 t 時間的可信度可以由 t-1 時間的可信度計算而來。這邊先列出一些之後會用到的數學工具：
貝氏定理
在前文談最大似然估計與最大後驗機率估計時就是從貝氏定理開始：
\[
p(x \mid y) = \frac{p(y \mid x)p(x)}{p(y)}
\]
而當條件為兩個變數時的貝氏定理為：
\[
p(x \mid y,z) = \frac{p(y \mid x, z)p(x \mid z)}{p(y \mid z)}
\]
馬可夫假設
馬可夫假設的意思是假如我們已知現在的狀態 \(x_t\)，則過去以及未來的觀測資料都是獨立的，在式子推導中將會用到馬可夫假設。
全機率公式
全機率公式為：
\[
p(x) = \int p(x \mid y)p(y)dy
\]
而如果有條件 z 時的式子為：
\[
p(x \mid z) = \frac{p(x,z)}{p(z)} = \int \frac{p(x,y,z)p(y,z)}{p(y,z)p(z)}dy
\\
= \int \frac{p(x,y,z)}{p(y,z)}\frac{p(y,z)}{p(z)}dy
\\
= \int p(x \mid y,z)p(y \mid z) dy
\]
轉換式的推導
現在要開始來推導 \(bel(x_t)\)：
\[
bel(x_t) = p(x_t \mid z_{1:t}, u_{1:t}) = p(x_t \mid z_{t}, z_{1:t-1}, u_{1:t})
\\
= \frac{p(z_t \mid x_t, z_{1:t-1}, u_{1:t})p(x_t \mid z_{1:t-1},u_{1:t})}{p(z_t \mid z_{1:t-1}, u_{1:t})}
\]
意思是利用上述的貝氏定理，把 \(x_t\) 當成 \(x\)，\(z_t\) 當成 \(y\)，\(z_{1:t-1}, u_{1:t}\) 當成 \(z\)。接下來我們讓分母的 \(p(z_t \mid z_{1:t-1}, u_{1:t})\) 為 \(1/\eta\)，因為這個值與 \(x_t\) 完全無關，因此在狀態轉換的推導過程中可以當成一個常數。把此常數代回去可得：
\[
bel(x_t) = \frac{p(z_t \mid x_t, z_{1:t-1}, u_{1:t})p(x_t \mid z_{1:t-1},u_{1:t})}{p(z_t \mid z_{1:t-1}, u_{1:t})}
\\
= \eta\ p(z_t \mid x_t, z_{1:t-1}, u_{1:t})p(x_t \mid z_{1:t-1},u_{1:t})
\\
= \eta\ p(z_t \mid x_t)p(x_t \mid z_{1:t-1},u_{1:t})
\]
上式中用了馬可夫假設將 \(p(z_t \mid x_t, z_{1:t-1}, u_{1:t})\) 代換成 \(p(z_t \mid x_t)\)，因為在已知狀態 \(x_t\) 的情況之下，現在的觀測資料 \(z_t\) 與所有過去的觀測資料 \(z_{1:t-1},u_{1:t}\) 都是獨立的。
下一步要用到全機率公式，把 \(x_t\) 當成 \(x\)，\(x_{t-1}\) 當成 \(y\)，把\(z_{1:t-1}, u_{1:t}\) 當成 \(z\)：
\[
bel(x_t) = \eta\ p(z_t \mid x_t)p(x_t \mid z_{1:t-1},u_{1:t})
\\
= \eta\ p(z_t \mid x_t)\int p(x_t \mid x_{t-1}, z_{1:t-1},u_{1:t})p(x_{t-1} \mid z_{1:t-1},u_{1:t}) dx_{t-1}
\]
接下來要做兩件事情：
- 再用一次馬可夫假設，將 \(p(x_t \mid x_{t-1}, z_{1:t-1},u_{1:t})\) 代換成 \(p(x_t \mid x_{t-1}, u_{t})\)，意思是在知道 \(x_{t-1}\) 的情況下，所有 t-1 以前的觀測 \(z_{1:t-1}, u_{1:t-1}\) 都是獨立的。
- 在 \(p(x_{t-1} \mid z_{1:t-1},u_{1:t})\) 中，狀態 \(x_{t-1}\) 不會被未來的觀測 \(u_t\) 所影響，因此 \(p(x_{t-1} \mid z_{1:t-1},u_{1:t})\) 被代換成 \(p(x_{t-1} \mid z_{1:t-1},u_{1:t-1})\)
把以上兩點代換進式子中：
\[
bel(x_t) = \eta\ p(z_t \mid x_t)\int p(x_t \mid x_{t-1}, z_{1:t-1},u_{1:t})p(x_{t-1} \mid z_{1:t-1},u_{1:t}) dx_{t-1}
\\
= \eta\ p(z_t \mid x_t)\int p(x_t \mid x_{t-1}, u_{t})p(x_{t-1} \mid z_{1:t-1},u_{1:t-1}) dx_{t-1}
\\
= \eta\ p(z_t \mid x_t)\int p(x_t \mid x_{t-1}, u_{t})\ bel(x_{t-1}) dx_{t-1}
\]
這就是我們推導出來的結果了，意思就是在 t 時間的可信度 \(bel(x_t)\) 可以由 t-1 時間的可信度 \(bel(x_{t-1})\) 計算而來。
結論
在從狀態 \(x_{t-1}\) 計算 \(x_t\) 的過程中，我們會把式子分成兩個部分：
- 預測：利用 \(x_{t-1}\) 及控制資訊 \(u_t\) 先做出一個大概的估計 \(\overline{bel}(x_t)\)：
\[
\overline{bel}(x_t) = \int p(x_t \mid x_{t-1}, u_{t})\ bel(x_{t-1}) dx_{t-1}
\]
其中 \(p(x_t \mid x_{t-1}, u_{t})\) 也稱做運動模型（motion model）。
- 校正：從大概的估計 \(\overline{bel}(x_t)\) 以及感測器得到的資料 \(z_t\) 得到更準確的狀態估計 \(bel(x_t)\)：
\[
bel(x_t) = \eta\ p(z_t \mid x_t)\ \overline{bel}(x_t)
\]
其中 \(p(z_t \mid x_t)\) 也稱做觀測模型 observation model，或 measurement model。
沒有留言:
張貼留言