第三章 自走式機器人之定位設計
3.2 位置估測方法
本節介紹以粒子濾波器理論為基礎而設計的機器人位置估測方法，其中包 含了粒子濾波器演算法的兩個主要步驟，預測階段(Prediction)與更新(Update)階 段。說明如何利用感測器的資訊與前一時刻機器人的姿態，經由粒子濾波器的處 理來估測機器人的位置資訊。而機器人朝向角的部分，將在下一節以線段擷取與 比對的方式加入此節產生的位置資訊當作輸入來求得，並未包含在粒子濾波器的 設計中。
3.2.1 預測(Prediction)
將粒子濾波器應用在機器人位置估測問題中，粒子在環境中散佈的位置就 是粒子濾波器對於機器人位置的估測結果。如下式所示：
) , ( m m
m
t x y
s = (3-1)
其中 s 代表在時間 t 的第 m 個粒子 tm
一開始因為我們已知機器人的初始位置，所以將粒子濾波器的所有粒子 (Particles)均勻散佈在初始位置的周圍，並且我們將每一個粒子的權重(Weight)皆 設定為M
1 ，如下式所示：
wtm = M1 (3-2)
W 為 機 器 人 的 兩 輪 間 距
3.2.2 更新(Update)
在這個階段中，我們使用預測階段得到的粒子組(Particle Set)與雷射掃描儀 的量測資訊來估測機器人的位置。首先將雷射掃描儀量測的距離資訊以最小平方 法產生一表示環境的線段組，並以極座標的直線方程式參數(α,ρ)來表示線段(詳 述於 3.3 節)。其中ρ為機器人與線段的垂直距離，另外以dRL代表。為了得到機 器人的位置參考資訊，我們必須知道由量測資訊產生的環境線段組與內建地圖的 對應關係。所以將環境線段組的直線方程式參數由以機器人為中心的區域座標系 轉換到以內建地圖原點為中心的全域座標系來表示，轉換公式如下：
2) (θ π α
αG = L+ − (3-10)
G G
L
G ρ x α y α
ρ = + cos + sin (3-11)
其中(x, y,θ )為機器人的姿態，而(αL,ρL)與(αG,ρG)分別為區域座標系與全域座 標系的直線方程式參數，如圖 3-3 所示。所以我們可以得知dRL是機器人與內建 地圖中的那一條線段之間的相對距離，藉此可以得到機器人在環境的位置參考資 訊。接著我們就可以對預測階段產生的粒子組做權重給予(Assign weight)與更新 (Update)的步驟。
圖 3-3 擷取線段座標轉換關係圖
在粒子濾波器演算法中，每一個粒子的權重是根據下列式子來決定：
中的位置，而存留下來的粒子其位置能夠符合於機器人在環境中的真實位置，所 以存留下來的粒子來產生新的粒子，會使得新的粒子組的位置分佈亦集中在真實 機器人位置的周圍。而實際做法是先設定一個距離範圍，然後在存留下來粒子的 位置的設定之距離範圍內隨機地產生新的粒子。如此新的粒子的位置也會集中在 實際機器人的位置周圍。不過所設定的距離範圍不可過大，若過大會造成新的粒 子與存留粒子相距太大，使新的粒子組的位置分佈過於分散而與機器人的實際位 置不符合。當新的粒子組產生後，再將所有粒子的權重都設為
M
1 ，接著新的粒
子組便進入下一個循環。以上的處理過程，即是粒子濾波器演算法中的重新取樣 (Resampling)步驟。
重新取樣後，可得一組新的粒子組，並將粒子組中每個粒子的位置座標取平 均值，而得到平均位置的座標即是粒子濾波器對機器人位置的估測結果，如下式 所示：
M x
x m
m t t
=
∑
(3-14)M y
y m
m t t
=