基于粒子濾波的室內機器人定位研究*

蕭志聰 蘇成悅 葉 迅

（1.廣東工業大學物理與光電工程學院 廣州 510006）（2.華南理工大學廣州學院汽車與交通工程學院 廣州 510800）

1 引言

定位在機器人執行任務時起著重要作用。實時建圖、路徑規劃和自動避障都需要機器人在世界坐標中準確的地理坐標［1～3］。其基于概率的定位算法主要有擴展卡爾曼濾波算法，無跡卡爾曼濾波算法、直方圖濾波算法及粒子濾波算法。前兩者需要基于特征的地標輔助，不適于全局定位［4～5］，而直方圖濾波的好壞嚴重依賴于柵格地圖的粗糙度，對于硬件低廉的普通機器人，一個精細的地圖意味著需要更長的計算時間。

粒子濾波采用蒙特卡洛機制實現了遞推貝葉斯濾波算法。它不受非高斯噪聲和非線性模型的限制［6］，并以維護粒子集合Хt來近似后驗置信度bel（xt）。目前針對粒子濾波算法的優化主要集中在濾波過程中對粒子數目的減少、粒子收斂速度的提高、粒子的抗干擾能力等方面［7］。Liu［8］等實施殘差策略只對部分關鍵粒子進行更新，而其余粒子保持不變，減少了計算方面的冗余，但只更新部分粒子降低了算法的抗干擾能力。Li T［9～10］等考慮了粒子的空間信息，使用分組策略對粒子進行合并，保持了粒子的多樣性，但計算合并過程占用了相當大的開銷。文獻［11］使用并行分布式計算，利用GPU為粒子濾波提高計算速度，但該改進對硬件要求過高。文獻［12，15］則旨在通過對真實后驗的擬合，盡可能調整粒子濾波中每次迭代所需的粒子數目。但由于多數室內機器人體積小成本低廉，上述改進研究并非都符合室內定位需求。

本文以文獻［12，15］中提出的 KLD（Kullback-Leibler Distance）采樣為基礎。通過引入基于KLD 的實時粒子數目作為判斷機器人誤差大小的依據，在誤差較大階段，結合t-1 和t-2 兩個時刻雷達激光測量信息，選取權值較大的粒子的方位融合進粒子濾波的采樣更新階段，使粒子更新能以更快速度收斂至高似然區域，實現對粒子濾波的改進。

2 基于KLD粒子濾波的改進

2.1 KLD采樣

KLD 可用于描述兩個隨機分布q（x），p（x）間的相似性：

根據式（1），將期望粒子所代表的后驗分布與真實后驗分布作相似性比較，推出下式［12］：

其中，nx即為采樣分布所需要的粒子數目，而z1-δ可由標準正態分布表查閱得到。式（2）用于決定在以概率為1-δ、由粒子所代表的分布與真實后驗分布的相近度小于ε的條件下，粒子濾波所需要的粒子數目。ParticleSample（PS）和 ParticleResample（PRS）均將KLD 采樣算法放進了粒子濾波各自的步驟中［12，15］，然而這兩種改進在粒子的采樣更新過程中均沒有考慮測量信息的影響，所以，粒子的收斂性并非最優。而關于變化的粒子數目可成為我們能利用的有效數據。

在粒子濾波的每次迭代過程中，當nx數目急劇變化，表明當前分布與后驗分布相差很大，即機器人定位誤差變大，需要更大的粒子數目來適應新變化的分布，此時，可添加進測量信息，使粒子快速收斂回正常水平。設 nx，t、nx，t-1、nx，t-2分別代表 t、t-1、t-2 時刻的粒子數目，若式（3）條件為真，則說明粒子變化程度明顯，需采用帶測量信息的粒子采樣更新。

2.2 帶測量信息的粒子采樣更新

粒子的采樣更新可以以離散的概率分布給出，圖1（a）及圖1（b）分別表示在旋轉遠大于直線運動及直線運動遠大于旋轉的情況下采樣更新得到的粒子群。

圖1 兩種極端運動下粒子的分布情況

設在t-1 時刻由測量傳感器更新的粒子群為Xt-1。由文獻［13］可看出，更新后的粒子攜帶了當刻測量的概率信息。為有效利用這些信息，可找出該粒子群中最大及最小權重，計算權重較大的下邊界w_bound，并選取權重在最大權重與w_bound 之間的粒子群

其中，b 取值在 0 到 1 區間。b 取值越大，則選取到的粒子數目越多。

xm，t-1，ym，t-1為第m個粒子的真實坐標。

設機器人在一個周期內運動可分為旋轉分量rot1，rot2，線性分量tran，則融合進測量信息：

上式中g 為標準正態分布，ε為自定義方差，取值按文獻［13］所給，xm，t，ym，t即為采樣更新后的新粒子位置。綜上，改進的KLD 粒子濾波可由下面幾行步驟描述。

3 仿真實驗及討論

本研究采用GAZEBO軟件進行仿真，它是一款可模擬現實物理世界的軟件。本次仿真使用turtlebot_world 場景［14］，考慮到硬件配置較低的情況，采用如下條件實驗：cpu 型號為i5 4210m，內存為4g的筆記本和節能模式下進行，處理能力接近目前較好的下位機嵌入式系統。本次實驗路徑如圖3 所示，實驗機器人帶有編碼器、雷達激光傳感器。

圖2 gazebo仿真場景

為了做出比較，本文同時也編程實現了KLDS（kld-sample）算法及 KLDRS（kld-resample）算法。在機器人運行伊始，設置b 為0.75，為機器人的位置添加距真實地點2m、真實方向約45°的噪聲，并且令機器人以0.3m/s 的速度和0.5rad/s 的角速度沿圖3 所示路徑行駛，行駛時間約100s。得出圖4 所示粒子使用數量變化圖。

圖3 機器人行駛路徑

圖4 粒子使用數目比較

其橫坐標為時間，縱坐標為粒子數目。紅色折線代表KLDS粒子濾波，藍色折線代表KLDRS粒子濾波，綠色折線代表本文改進算法。可見三條折線都隨著時間的遞進，粒子數總體逐步遞減。而紅線折線在后期會在一個較小的數目區間內出現明顯的波動。同時，穩定階段藍線及綠線的粒子數目維持在小于300 的區間，粒子計算消耗小，滿足實際需要。

機器人定位誤差由圖5 給出。按文獻［16］的實驗結果，以固定不變的粒子數目N =20000 運行粒子濾波算法，所得的路徑可作為真實路徑。其他算法產生的誤差可按下式計算：

其中，x，y 為每一時刻各算法的機器人坐標，x'，y'為真實路徑坐標，distmax為機器人距真實路徑最大距離。

圖5 定位誤差比較

圖6 中，橫軸為時間，縱軸（error）是按二十次實驗平均所得偏差，折線顏色代表算法與圖5 相同。在0～15s時間段，機器人定位偏差較大，如圖5所示在此階段粒子數目會以一個較大梯度急劇遞減，為使粒子快速收斂至高似然區域，本算法考慮采取測量信息與里程信息的結合，所得收斂速度比其他兩種算法更快。按誤差遞減到0.1 階段，本算法比 KLDS 快約 5s，約 55%，比 KLDRS 快約 9s，約60%。在15s～30s，KLDS 算法誤差出現了小幅度的回升，而本文算法與KLDRS 都在一個穩定階段。在30s 之后，各算法定位誤差趨于一致，且誤差的波動也較小，可滿足室內機器人定位實際需要。

圖6 不同b取值對收斂時間的影響

圖6表示了本文修改算法在不同的b取值時對收斂時間的影響，其中橫坐標為b 值，縱坐標為按error遞減到0.1時相對于KLDS及KLDRS所勝出的時間。可見隨著b 的遞增，收斂勝出時間也隨之增大，當b等于0.75以后，勝出時間幾乎停滯。當b等于1，即把前時刻的所有粒子都計算一遍，造成效率浪費，故本文選取b等于0.75。

圖7 修改算法的抗干擾能力

為了測試本算法的魯棒性，在本算法運行階段的第0s、40s、80s、120s及160s分別添加距真實位置約3m、4.5m、0.5m、1m 及2m 的噪聲，其中誤差率仍按式（15）計算。如圖7 所示，橫坐標為運行時間，縱坐標誤差率error，運行中的機器人在每次受到噪聲干擾后，均能快速修復回誤差率在0.1的水平，本修改算法在室內場景可靠穩定。

4 結語

在KLD 采樣中，迭代需要的粒子數目會實時變化，當預測分布與真實后驗分布相差太大，則所需粒子數目就會增多，反之所需粒子數目則少。若粒子數目劇烈變化，則機器人處在定位誤差較大階段。但對包含DS 和KLDRS 的傳統KLD 采樣算法，對于粒子的預測更新僅考慮了當前時刻及上一時刻的里程信息作為粒子更新的判斷依據，而把測量信息直接忽略掉。本文根據粒子數目變化程度有效結合了里程信息和測量信息，使粒子以更快速度進入高似然區域，以達到機器人快速定位修復的效果，并通過仿真實驗驗證了其有效性，新算法的定位速度大于KLDRS 約50%以上，確認了本算法利用測量信息使算法快速收斂和實現快速定位修復的重要價值。同時，與傳統KLD 采樣一樣，較小的粒子數目需求、較小的定位誤差波動也滿足了普通家用機器人的需求。