基于粒子濾波的機(jī)器人主動(dòng)定位算法

常天莉，黃浩暉，陳 瑋

(廣東工業(yè)大學(xué) 自動(dòng)化學(xué)院，廣東 廣州 510006)

0 引 言

移動(dòng)機(jī)器人定位問題[1]作為機(jī)器人系統(tǒng)中最重要的部分已經(jīng)被研究多年。在足球機(jī)器人領(lǐng)域，機(jī)器人需要獨(dú)自或者協(xié)同在同一坐標(biāo)系系統(tǒng)里執(zhí)行一系列任務(wù)，因此根據(jù)有限的環(huán)境信息預(yù)測(cè)機(jī)器人的當(dāng)前位置是非常重要的。

過去針對(duì)移動(dòng)機(jī)器人全局定位的求解問題，卡爾曼濾波定位算法(Kalman filter localization)[2]、馬爾卡夫定位算法(Markov localization)[3]、以及蒙特卡羅定位算法(Monte Carlo localization)[4]等都得到了廣泛的應(yīng)用。然而上述定位算法都屬于被動(dòng)定位算法，即機(jī)器人在環(huán)境中隨機(jī)移動(dòng)，根據(jù)傳感器隨機(jī)獲取的環(huán)境信息進(jìn)行定位，這樣就會(huì)降低系統(tǒng)的定位效率；對(duì)于具有多個(gè)局部相似或?qū)ΨQ的環(huán)境特征，甚至不能求出機(jī)器人的正確位置。

針對(duì)上述問題，基于馬爾科夫定位算法的主動(dòng)定位算法[3]可以很好地提高機(jī)器人定位系統(tǒng)的性能，該算法通過選擇機(jī)器人的行為使得機(jī)器人對(duì)位置估計(jì)的不確定性期望值達(dá)到最小，即機(jī)器人通過該算法可以選擇有利于識(shí)別不同位置的行為。然而該算法只考慮了執(zhí)行下一個(gè)行動(dòng)單位時(shí)對(duì)狀態(tài)環(huán)境的預(yù)估，考慮到機(jī)器人可能需要移動(dòng)到較遠(yuǎn)的位置或者通過多次迭代計(jì)算才能唯一確定它的位置，這種算法的求解深度是不夠的。因此學(xué)者們對(duì)主動(dòng)定位算法不斷進(jìn)行改進(jìn)[5]，通過使用啟發(fā)信息，主動(dòng)控制機(jī)器人的運(yùn)動(dòng)路徑，使機(jī)器人能夠區(qū)別不同的粒子簇，最終使機(jī)器人能收斂于真實(shí)的位置。

然而啟發(fā)信息制定的合理性影響著機(jī)器人定位的求解效率，為了減少算法對(duì)啟發(fā)信息的依賴以及增加算法的魯棒性，本文基于改進(jìn)的粒子濾波主動(dòng)定位算法，研究智能體如何從帶有噪聲的視覺信息中快速定位，同時(shí)把算法應(yīng)用到RoboCup2D足球仿真平臺(tái)上，優(yōu)化足球機(jī)器人在有限信息中的快速定位問題。

1 粒子濾波定位算法

在RoboCup2D仿真平臺(tái)中，每個(gè)智能體都可以從服務(wù)器端(soccer server)每隔150 ms獲取一次視覺信息：

(seeObjNameDistanceDirectionDistChngDirchngBodyDirHeadDir)

其中,ObjName表示所看到物體的類別(球、球員、標(biāo)志、球門、邊界)。而球場(chǎng)上每個(gè)標(biāo)志都是已知和固定的，球員可以通過視覺系統(tǒng)很快地獲取視覺范圍內(nèi)的任何標(biāo)志來進(jìn)行自定位。

1.1 非高斯噪聲的視覺模型

在RoboCup2D仿真平臺(tái)中，球員通過獲取球場(chǎng)上已知并且固定的標(biāo)志點(diǎn)信息[6]進(jìn)行自定位，為了模擬真實(shí)環(huán)境，Soccer Server會(huì)對(duì)球員獲得的視覺信息加入白噪聲。對(duì)于傳統(tǒng)的定位問題，這些噪聲都可以用高斯分布來建模，然后利用卡爾曼濾波或者粒子濾波算法來求解；而Soccer Server則會(huì)用更復(fù)雜的噪聲模型對(duì)視覺信息進(jìn)行量化處理。假設(shè)球員到某個(gè)標(biāo)志點(diǎn)的真實(shí)距離為d，球員通過噪聲量化所得到的視覺距離為d′，則該量化過程可定義為

Ns:d→d′=Q(eQ(log(d),Δ1),Δ2)

(1)

其中，量化步長Δ1和Δ2分別為0.01和0.1，量化函數(shù)Q為

(2)

通過Server的噪聲量化，若標(biāo)志點(diǎn)與球員的距離有10m時(shí)，最大的噪聲誤差可以達(dá)到10cm。因此球員便難以從視覺信息中獲取標(biāo)志點(diǎn)的準(zhǔn)確位置，針對(duì)該問題，本文將采用門限濾除技術(shù)搭建視覺的誤差模型。

當(dāng)球員在位置pagent(x1,y1)上觀察到標(biāo)志點(diǎn)Oi時(shí)，其位置范圍的誤差可用截尾扇形E來表示，如圖1所示。扇形E的數(shù)學(xué)表達(dá)式如下

(3)

其中，Δr和Δθ分別表示最大距離誤差和最大角度誤差。在SoccerServer中，球員所獲得的視覺信息和距離的有很大關(guān)系的，因此本文中的Δr和Δθ參數(shù)也會(huì)隨著視覺距離而變化。

圖1 標(biāo)志點(diǎn)的誤差區(qū)域E

在自定位過程中，球員通過獲取球場(chǎng)上的標(biāo)志點(diǎn)來預(yù)估自己的位置，因此相對(duì)于標(biāo)志點(diǎn)可能的位置，我們更感興趣的是當(dāng)獲取標(biāo)志點(diǎn)信息后，球員所預(yù)估的位置。如圖2所示，本文通過一定的幾何等價(jià)方法，把以標(biāo)志點(diǎn)為中心的扇形區(qū)域轉(zhuǎn)換為以球員坐標(biāo)為中心的扇形區(qū)域，同樣的該扇形區(qū)域表示是球員預(yù)估坐標(biāo)的誤差范圍。而在比賽過程中，球員所獲得的標(biāo)志點(diǎn)通常不止一個(gè)，我們假設(shè)標(biāo)志點(diǎn)有N個(gè)，從而會(huì)產(chǎn)生N個(gè)誤差區(qū)域E1…EN，因此球員通過N個(gè)標(biāo)志點(diǎn)所得到的誤差位置區(qū)域?yàn)?/p>

(4)

圖2 球員的誤差區(qū)域E

1.2 粒子濾波定位算法

(5)

(6)

其中，dt-1表示機(jī)器人通過執(zhí)行動(dòng)作at-1后所得到的位置變化矢量。

(2)獲取視覺信息并計(jì)算粒子權(quán)重：機(jī)器人通過視覺感知模塊獲取觀察信息Ot來更新機(jī)器人可能位置的置信度bel(xt)。由于SoccerServer中對(duì)于觀察信息以及動(dòng)作都會(huì)帶有噪聲誤差，對(duì)于該誤差范圍利用圖2中的誤差區(qū)域E來表示，然后根據(jù)所獲得的觀察信息重新計(jì)算每個(gè)粒子的權(quán)重值

(7)

在RoboCup2D仿真系統(tǒng)中，如果球員獲得的觀察信息不夠理想，即所獲取的特征點(diǎn)不夠多或者特征點(diǎn)的誤差范圍比較大時(shí)，則將增加上述粒子濾波定位算法中粒子權(quán)重的計(jì)算量和采樣時(shí)間，從而嚴(yán)重影響系統(tǒng)的實(shí)時(shí)性。因此，本文將引入主動(dòng)定位算法來提高求解效率。

2 基于視覺噪聲的機(jī)器人主動(dòng)定位算法

傳統(tǒng)的粒子濾波定位算法屬于被動(dòng)定位算法，盡管它可以完成移動(dòng)機(jī)器人的定位任務(wù)，但卻需要花費(fèi)較多的動(dòng)作和時(shí)間來實(shí)現(xiàn)，因此學(xué)者們提出了基于熵值的主動(dòng)定位算法[8]，該算法通過估計(jì)不同信念狀態(tài)Bel(ξk)下的不確定度，得出機(jī)器人的最優(yōu)行為動(dòng)作集合，使得狀態(tài)的不確定性期望值最小，從而有利于機(jī)器人辨識(shí)自身的位置。熵的期望值可以通過下面公式得到

Ea[H]=-?Bela,s(ξk)log(Bela,s(ξk))p(s)dξds

(8)

然而機(jī)器人在定位過程中可能需要移動(dòng)較遠(yuǎn)的距離才能夠唯一地確定機(jī)器人當(dāng)前的位置，也就是需要一個(gè)較長的基本行為隊(duì)列，而在不能確定機(jī)器人位姿前提下的行為決策是毫無意義的，無目地“游走”只會(huì)降低機(jī)器人的定位效率。因此本文結(jié)合上一節(jié)的粒子濾波算法，基于GDUT_TiJi2014[9]的啟發(fā)式行為決策系統(tǒng)，提出一種魯棒的啟發(fā)式搜索定位算法，使粒子能盡快收斂到真實(shí)位姿。

在啟發(fā)式搜索的求解過程中，啟發(fā)式信息的選取，即獎(jiǎng)勵(lì)機(jī)制與代價(jià)機(jī)制的確定在很大程度上制約著算法的求解效率；文獻(xiàn)[9]中啟發(fā)式函數(shù)定義如下

(9)

(10)

(11)

(12)

其中，α為最優(yōu)權(quán)重值，且0≤α<1，決定了誤差函數(shù)對(duì)決策選擇的影響?；谝曈X噪聲的機(jī)器人主動(dòng)定位算法流程如圖3所示，算法每完成一次粒子濾波算法會(huì)計(jì)算一次粒子簇的方差，當(dāng)方差大于閾值ε時(shí)，表示粒子過于分散，定位算法將利用特征點(diǎn)信息進(jìn)行全局定位，從而提高定位精度，此時(shí)α≠0，即采用式(12)對(duì)策略系統(tǒng)進(jìn)行求解；當(dāng)方差小于等于ε時(shí)，粒子已經(jīng)開始趨向收斂，此時(shí)問題可表示為機(jī)器人的位置跟蹤問題，可令α=0，即采用式(9)對(duì)策略系統(tǒng)進(jìn)行求解，提高機(jī)器人的定位效率。

圖3 基于視覺噪聲的機(jī)器人主動(dòng)定位算法流程

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

本文實(shí)驗(yàn)把基于視覺噪聲的機(jī)器人主動(dòng)定位算法應(yīng)用到GDUT_TiJi2016球隊(duì)上，并通過自定位平均誤差以及球隊(duì)的進(jìn)球能力兩個(gè)方面來驗(yàn)證算法的有效性及可行性。

圖4給出了使用了本文算法的GDUT_TijI2016與自帶定位系統(tǒng)的agent2D[10]所進(jìn)行的一場(chǎng)比賽中球員自定位的平均定位誤差統(tǒng)計(jì)，GDUT_TiJi是以agent2D為底層所開發(fā)的2D仿真球隊(duì)，本文定位算法代碼使用C++設(shè)計(jì)，比賽場(chǎng)景在i5-5200CPU(2.20GHz)和Ubuntu12.04平臺(tái)下進(jìn)行。由圖4對(duì)比可以看出，基于視覺噪聲的主動(dòng)定位算法大大提高了球員的自定位誤差(由1.3m提高到0.4m)，當(dāng)球員獲得比較準(zhǔn)確的定位后，在啟發(fā)式函數(shù)的作用下，能一直保持在一個(gè)合理的誤差范圍內(nèi)。而在定位過程中agent2D和GDUT_TijI2016在第70s時(shí)都出現(xiàn)了一個(gè)峰值，這是由于GDUT_TijI2016在第70s的時(shí)候產(chǎn)生了一個(gè)進(jìn)球，soccerserver把兩隊(duì)的球員都進(jìn)行重置(瞬間回到初始位置)，這體現(xiàn)了機(jī)器人中的綁架問題，然而該問題并沒有影響機(jī)器人的定位，相比agent2D隊(duì)伍，本文算法能讓機(jī)器人更快地確定自己的位姿。

圖4 球隊(duì)自定位平均誤差統(tǒng)計(jì)

在第二個(gè)實(shí)驗(yàn)中，我們使用了GDUT_TiJi2014[9]作對(duì)比，因?yàn)榕cGDUT_TiJi2014相比，GDUT_TiJi2016唯一改動(dòng)的地方就是加入了基于視覺噪聲的機(jī)器人主動(dòng)定位算法。該實(shí)驗(yàn)使用GDUT_TiJi2014和GDUT_TiJi2016分別與agent2D球隊(duì)進(jìn)行了200場(chǎng)模擬比賽，表1統(tǒng)計(jì)了兩個(gè)版本的平均進(jìn)球數(shù)和失球數(shù)，由表1可得，定位算法也對(duì)球隊(duì)的整體性能有了很大的提高。

表1 比賽結(jié)果統(tǒng)計(jì)

4 結(jié)束語

本文基于視覺噪聲的粒子濾波算法和啟發(fā)式搜索，提出了一個(gè)定位算法，運(yùn)用門限濾除技術(shù)，對(duì)各粒子進(jìn)行篩選，再利用結(jié)合了誤差函數(shù)的啟發(fā)式搜索來加快球員的自定位速度。通過實(shí)驗(yàn)驗(yàn)證了該算法在特征點(diǎn)的幫助下能有效地提高機(jī)器人的定位速度和精度，同時(shí)也提高了GDUT_TiJi2016球隊(duì)的總體性能，驗(yàn)證了該算法的可行性。

然而，機(jī)器人足球是一個(gè)多智能體協(xié)作的平臺(tái)，每個(gè)機(jī)器人所執(zhí)行的動(dòng)作都會(huì)對(duì)其它機(jī)器人產(chǎn)生影響。因此，下一步工作將要研究如何融合多智能體的所收集回來的狀態(tài)信息[11]，進(jìn)一步提高機(jī)器人的定位精度和速度。

[1]Wang Y,Chen W,Wang JWH.Active global localization based on localizability for mobile robots[J].Robotica,2015,33(8):2071-2076.

[2]MahmoudII,Salama M,Tawab AAE.Particle/Kalman filter for efficient robot localization[J].International Journal of Computer Applications,2014,106(2):20-27.

[3]Burgard W,Fox D,Thrun S.Markov localization for mobile robots in dynamic environments[J].Journal of Artificial Intelligence Research,2011,11(1):391-427.

[4]Kim TG,Ko NY,Noh SW,et al.Localization on an underwater robot using Monte Carlo localization algorithm[J].The Journal of the Korea Institute of Electronic Communication Scie-nces,2011,6(2):288-295.

[5]Liu Y,Zhang H,Cui P,et al.Mobile robot active global localization based on heuristic search[J].Journal of Computational Information Systems,2012,8(2):739-746.

[6]Akiyama H.RoboCup soccer 2D simulation league:Introduction to the RoboCup soccer simulator[J].Journal of Japan Society for Fuzzy Theory & Intelligent Informatics,2011,23:714-720.

[7]Zhao ZS,Shen JH,Wang JZ,et al.Research on self-localization methods for mobile robots based on Bayes filter[J].Computer Science,2011,38(2):199-201.

[8]Seekircher A,Laue T,R?fer T.Entropy-based active vision for a humanoid soccer robot[M]//RoboCup 2010:Robot Soccer World Cup XIV.Berlin:Springer Berlin Heidelberg,2011:1-12.

[9]HUANG Haohui,YANG Wanlu,CHEN Wei.Study on the A* average reward reinforcement learning algorithm based on performance potentials[J].Computer Simulation,2014,31(7):338-341(in Chinese).[黃浩暉,楊宛璐,陳瑋.基于性能勢(shì)的A*平均獎(jiǎng)賞強(qiáng)化學(xué)習(xí)算法研究[J].計(jì)算機(jī)仿真,2014,31(7):338-341.]

[10]Akiyama H,Nakashima T.HELIOS base:An open source package for the RoboCup soccer 2D simulation[M]//RoboCup 2013:Robot World Cup XVII,2014:528-535.

[11]Franchi A,Oriolo G,Stegagno P.Mutual localization in multi-robot systems using anonymous relative measurements[J].International Journal of Robotics Research,2013,32(11):1302-1322.