第二章 粒子濾波器
2.1 貝氏濾波器
在介紹貝氏濾波器演算法[12]之前，必須先說明幾個貝氏濾波器應用在自走 式機器人領域的重要名詞，以便後續的介紹：
(1) 狀態轉移機率(State transition probability)：
以條件機率函數p(xt |xt−1,ut)來表示，其中x 與t xt−1分別表示在 t 和 t-1 時刻 的系統狀態，在機器人定位系統中即是機器人在 t 與 t-1 時刻的姿態(Pose)，
而u 則是在 t 時刻的機器人控制命令。此機率函數說明了在已知目前控制命t 令與前一時刻機器人狀態的條件下，目前機器人狀態的機率分佈情形。
(2) 量測機率(Measurement probability)：
以條件機率函數p(zt|xt)來表示，其中z 為系統在 t 時刻的量測資訊，在機t 器人定位系統中即是機器人在時刻 t 由感測器測得的感測資訊。量測機率函 數說明了機器人在目前的狀態下(x )，從感測器量得的環境資訊的機率分佈t
z 。 t
(3) 相信度函數(Belief)：
以bel(xt)符號表示狀態x 的相信度函數，並可寫成 t
) ,
| ( )
(xt p xt z1:t u1:t
bel = (2-1)
其中p(xt|z1:t,u1:t)通常稱作 Posterior，是系統狀態x 在已知所有過去的感測t 資訊z1:t與控制命令u1:t的情況下的機率分佈函數。在未包含 t 時刻的量測資 訊z 前的 Posterior 通常稱做 Prior，而未包含t z 的相信度函數以t bel(xt)符號 表示，可寫成如下的式子：
t t t t
t p x z u
x
bel( )= ( | 1:−1, 1:) (2-2)
在貝氏濾波器演算法中bel(xt)是 Prediction 階段之後得到的，而在加入 t 時
刻的量測資訊z 後，便可從t bel(xt)算出bel(xt)，這個過程稱作 Measurement update。
圖 2-1 說明了在貝氏濾波器中系統狀態、控制命令與量測資訊之間隨時間的 演化過程，在時間 t 的系統狀態是根據同時刻的控制命令u 與前一時刻的系統狀t 態而來，而 t 時刻的量測資訊z 則是根據系統在狀態為t x 時的量測而來。如此的t 演化模式稱作動態貝氏網路(Dynamic Bayes network,DBN)[12]。
x
tz
t+1x
t-1x
t+1u
t+1z
tz
t-1u
tu
t-1圖 2-1 動態貝氏網路示意圖
貝氏濾波器演算法的目的是要求得系統狀態的相信度函數bel(xt)，並且以遞
迴的方式從感測資訊與控制命令來求得bel(xt)，所以貝氏濾波器的輸入是 t-1 時刻的相信度函數bel(xt−1)與 t 時刻的感測資訊與控制命令，而輸出即是 t 時刻 的相信度函數bel(xt)。
貝氏濾波器演算法主要分成兩個步驟來求得bel(xt)，分別是 Prediction 與 Update：
1.預測階段(Prediction)：
∫
− − −= ( | , 1) ( 1) 1 )
(xt p xt ut xt bel xt dxt
bel (2-3)
2.更新階段(Update)：
) ( )
| ( )
(xt p zt xt bel xt
bel =η (2-4)
在 Prediction 階段，貝氏濾波器利用了前一時刻的相信度函數bel(xt−1)與控制命 令u ，並對兩個機率函數，狀態轉移函數t p(xt |xt−1,ut)與相信度函數bel(xt−1)相 乘的結果做積分，從積分的結果我們可以得到bel(xt)。而在 Update 階段，貝氏
濾波器則是將前一步驟得到的bel(xt)乘上量測機率p(zt |xt)，相乘後的結果可以 得到最後的結果相信度函數bel(xt)。因為兩個機率函數相乘的結果不一定仍為一 機率函數，即將兩機率函數相乘的結果積分不一定為 1，所以在(2-4)式中必須要 乘上一係數η使得bel(xt)為正常的機率函數。
接著是數學的推導，將証明時刻 t 的相信度函數bel(xt)確實可由前一時刻的 相信度函數bel(xt−1)經由貝氏濾波器演算法的計算求出。以下的推導必須有兩個 假設為前提：
1. 假設系統狀態
x
t包含了所有過去時間的量測資訊與控制命令的資訊(Markov Assumption)。2. 假設控制命令是隨機產生的並且已知。
制命令與量測資訊，但不包括u ，可以得到： t
(2-11)式即是使用在貝式濾波器中 Prediction 階段的式子。所以經由上面的証明可 以得到貝氏濾波器確實可以正確地由前一時刻的相信度函數bel(xt−1)加上控制命 令與量測資訊，得到目前時刻的bel(xt)。