基于粒子濾波的室內(nèi)機(jī)器人定位研究*

蕭志聰 蘇成悅 葉 迅

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

1 引言

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

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

本文以文獻(xiàn)［12，15］中提出的 KLD（Kullback-Leibler Distance）采樣為基礎(chǔ)。通過(guò)引入基于KLD 的實(shí)時(shí)粒子數(shù)目作為判斷機(jī)器人誤差大小的依據(jù)，在誤差較大階段，結(jié)合t-1 和t-2 兩個(gè)時(shí)刻雷達(dá)激光測(cè)量信息，選取權(quán)值較大的粒子的方位融合進(jìn)粒子濾波的采樣更新階段，使粒子更新能以更快速度收斂至高似然區(qū)域，實(shí)現(xiàn)對(duì)粒子濾波的改進(jìn)。

2 基于KLD粒子濾波的改進(jìn)

2.1 KLD采樣

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

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

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

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

2.2 帶測(cè)量信息的粒子采樣更新

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

圖1 兩種極端運(yùn)動(dòng)下粒子的分布情況

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

其中，b 取值在 0 到 1 區(qū)間。b 取值越大，則選取到的粒子數(shù)目越多。

xm，t-1，ym，t-1為第m個(gè)粒子的真實(shí)坐標(biāo)。

設(shè)機(jī)器人在一個(gè)周期內(nèi)運(yùn)動(dòng)可分為旋轉(zhuǎn)分量rot1，rot2，線性分量tran，則融合進(jìn)測(cè)量信息：

上式中g(shù) 為標(biāo)準(zhǔn)正態(tài)分布，ε為自定義方差，取值按文獻(xiàn)［13］所給，xm，t，ym，t即為采樣更新后的新粒子位置。綜上，改進(jìn)的KLD 粒子濾波可由下面幾行步驟描述。

3 仿真實(shí)驗(yàn)及討論

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

圖2 gazebo仿真場(chǎng)景

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

圖3 機(jī)器人行駛路徑

圖4 粒子使用數(shù)目比較

其橫坐標(biāo)為時(shí)間，縱坐標(biāo)為粒子數(shù)目。紅色折線代表KLDS粒子濾波，藍(lán)色折線代表KLDRS粒子濾波，綠色折線代表本文改進(jìn)算法?？梢娙龡l折線都隨著時(shí)間的遞進(jìn)，粒子數(shù)總體逐步遞減。而紅線折線在后期會(huì)在一個(gè)較小的數(shù)目區(qū)間內(nèi)出現(xiàn)明顯的波動(dòng)。同時(shí)，穩(wěn)定階段藍(lán)線及綠線的粒子數(shù)目維持在小于300 的區(qū)間，粒子計(jì)算消耗小，滿足實(shí)際需要。

機(jī)器人定位誤差由圖5 給出。按文獻(xiàn)［16］的實(shí)驗(yàn)結(jié)果，以固定不變的粒子數(shù)目N =20000 運(yùn)行粒子濾波算法，所得的路徑可作為真實(shí)路徑。其他算法產(chǎn)生的誤差可按下式計(jì)算：

其中，x，y 為每一時(shí)刻各算法的機(jī)器人坐標(biāo)，x'，y'為真實(shí)路徑坐標(biāo)，distmax為機(jī)器人距真實(shí)路徑最大距離。

圖5 定位誤差比較

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

圖6 不同b取值對(duì)收斂時(shí)間的影響

圖6表示了本文修改算法在不同的b取值時(shí)對(duì)收斂時(shí)間的影響，其中橫坐標(biāo)為b 值，縱坐標(biāo)為按error遞減到0.1時(shí)相對(duì)于KLDS及KLDRS所勝出的時(shí)間?？梢婋S著b 的遞增，收斂勝出時(shí)間也隨之增大，當(dāng)b等于0.75以后，勝出時(shí)間幾乎停滯。當(dāng)b等于1，即把前時(shí)刻的所有粒子都計(jì)算一遍，造成效率浪費(fèi)，故本文選取b等于0.75。

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

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

4 結(jié)語(yǔ)

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