Particle Filter 實現於單一機器人定位

主要分為四個階段：

粒子產生階段：

在機器人初始位置附近，以隨機亂數方式任意產生新的粒子，每一個粒子都 是機器人可能的位置，粒子的大小表示對這個機器人位置的權重，即機器人在這 個位置的機率，如圖 5-17 所示。

預測階段：

每一個粒子會根據機器人運動模型( 5-16)預測下一刻的位置，如圖 5-18 所

61

示。Vmax為機器人最快速度，經過量測最快速度為 0.0425m/s，由於地板的打滑 等外力因素，機器人並不一定是以最大速度行走，加入 w(t)為速度雜訊，表示機 器人受到外力因素所導致的速度變化，同樣在角度方面，也會受到外在影響使角 度產生變化，在此機器人的角度由電子羅盤提供，並且加入 v(t)為角度量測雜訊。

x

y ＝ 1 0 0 1 x

y + V^′ cosθ^′ sinθ^′

( 5-16) V^′ = Vmax − w(t) ( 5-17)

θ^′ = θ + v(t) ( 5-18)

圖 5-17、粒子產生階段 圖 5-18、預測階段

其中雜訊的散布將影響每一個粒子的分佈狀況，以 Matlab 建立一模擬環境，以 50 個粒子散佈於(1,1)，機器人最快速度為 0.0425 公尺，從(1,1)出發逆時鐘繞行 一個 2 公尺*2 公尺的方形，並且以兩組不同雜訊參數做模擬如表 5-1 所示，說 明雜訊的散布的影響，如表 5-1 所示。

表 5-1、雜訊參數 第一組 w(t)為一平均分佈於 0 到最快速度之 0.01 倍的

雜訊

v(t)為一平均分布於 正負 10 度雜訊 第二組 w(t)為一平均分佈於 0 到最快速度之 0.1 倍的

雜訊

v(t)為一平均分布於 正負 20 度雜訊

62

63 會接收到新的感測資訊，感測資訊為透過無線訊號強度為基礎之 Trilateration 計 算出來之位置。在假設透過無線訊號強度為基礎之 Trilateration 所得到位置沒有 誤差的狀況下，機器人位置之預測位置與 Trilateration 越接近者，表示機器人在 該點的可能性就越高，如圖 5-21 所示，因此以機器人位置與 Trilateration 之距離

64

其中

distance 表示機器人位置與 Trilateration 之距離。

圖 5-21、對環境資訊感測 圖 5-22、對環境資訊權重

重要取樣：

由於實際無線 Trilateration 所得到的位置資訊會有誤差，必頇加入物理限制作為 內部判斷的依據：

1. 機器人移動的最快速度：

因為定位具有連續性，考量到機器人最快移動速度之情況，可以將距離誤差 過大之無線 Trilateration 資訊捨棄，因為機器人不可能瞬間移動距離誤差過 大的地方，另外考量到會有打滑、或是外力干擾因素，機器人不一定是以最

Measurement likelihood

65

圖 5-23、速度權重 2. 機器人旋轉時的變化角度率：

因為機器人本身有電子羅盤可以得知目前角度資訊，也可以知道上一刻與這 一刻的角度差距，如果無線 Trilateration 所得到的位置與電子羅盤的位置角 度誤差太大，也會將無線 Trilateration 資訊濾除，因此可以得到對角度誤差 權重表示，權重表示方式如( 5-23)所示，曲線如圖 5-24 所示。

θ_p = ∠(P_k, P_k−1) ( 5-21)

∆θ＝ ∥ θ_k^′ − θ_k−1^′ ∥ ( 5-22)

1

∥ ∆θ − θ_p ∥ ( 5-23)

其中

P_k：表示無線 Trilateration 所計算之位置

θ_p：表示無線 Trilateration 所計算之目前位置與上一刻位置之角度差

∆θ：表示電子羅盤目前角度與上一刻角度位置角度差

0 0.005 0.01 0.015 0.02 0.025 0.03 0.035 0.04 0

0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8

Speed

Transmission Probability

66

圖 5-24、角度權重

3. 機器人目前的狀態：從機器人目前狀態是其中一項重要的參考指標。這邊要 特別提出來的是天線對無線訊號強度造成的影響，由於機器人的移動與旋轉 會造成天線方向的改變，天線方向的改變會影響到訊號強度的變化，導致無 線 Trilateration 產生誤差，但是藉由機器人本身目前的控制命令狀態（前進、

後退、旋轉）可以對天線所造成的影響做補償，減低天線對無線 Trilateration 所產生的誤差影響，補償的量為將機器人目前的座標反算為訊號強度，並且 與 ZigBee 所得到的訊號強度相減，即為所需要補償的量，如( 5-24)所示。

RSSI ^′ = RSSI − bias ( 5-24) 其中

RSSI：表示未經過補償之無線訊號強度 Bias：表示需要補償的量

RSSI ^′：表示經過補償之無線訊號強度

主要分為停止、前進或後退與旋轉，以下分別說明之：

1. 若是機器人目前是處於停止的狀態，無線 Trilateration 資訊應該是 會保持在一定的誤差範圍，如果無線定位的誤差變大，或是位置突 然改變，則表示可能有遮蔽物或干擾產生，可以將錯誤資訊濾除，

0 10 20 30 40 50 60 70 80 90

0 5 10 15 20 25

Angle

Transmission Probability

67 生變化，導致無線 Trilateration 所得到的位置誤差有可能變大，可 以將錯誤資訊濾除，並且對天線誤差做補償

68

E x_k z_1:k ：系統狀態的期望值

重新取樣階段：

為了防止所有粒子收斂在同一點，在重新取樣階段，會根據權重的大小重新產生 新的粒子並且加入雜訊，以保持其他的可能性，如圖 5-25 所示。

圖 5-25、重新取樣階段