基于JSD自適應(yīng)粒子濾波的移動(dòng)機(jī)器人定位算法

劉紅林，凌有鑄，陳孟元

(安徽工程大學(xué) 安徽省電氣傳動(dòng)與控制重點(diǎn)實(shí)驗(yàn)室，安徽 蕪湖 241000)

移動(dòng)機(jī)器人定位就是確定機(jī)器人在其運(yùn)動(dòng)環(huán)境中的世界坐標(biāo)系的坐標(biāo)[1]，是機(jī)器人利用上一刻位姿和最新觀測信息來估計(jì)當(dāng)前位姿的過程。移動(dòng)機(jī)器人定位可以通過多種方法實(shí)現(xiàn)。蒙特卡羅[2]定位作為粒子濾波在移動(dòng)機(jī)器人領(lǐng)域的成功應(yīng)用,最早由Dellaertt[3]等提出。粒子濾波對(duì)后驗(yàn)概率密度函數(shù)直接進(jìn)行蒙特卡羅采樣使其逼近真實(shí)的概率分布,因而被廣泛應(yīng)用在非線性系統(tǒng)。但是基于粒子濾波的MCL算法利用觀測似然評(píng)估每個(gè)粒子的重要性權(quán)重,在采樣環(huán)節(jié)中對(duì)先驗(yàn)分布進(jìn)行采樣,忽視了當(dāng)前觀測信息,在預(yù)測過程中可能會(huì)出現(xiàn)粒子分布在似然函數(shù)的尾部的情況,從而引起粒子退化問題。目前，針對(duì)粒子退化問題主要有增加粒子數(shù)目、改進(jìn)提議分布以及改進(jìn)重采樣算法。第一種方法是擴(kuò)大粒子數(shù)，但是比較低效，所以主要用選擇合適提議分布和對(duì)重采樣環(huán)節(jié)進(jìn)行改進(jìn)的方法來抑制粒子退化問題[4]。目前，提議分布的選取策略有將狀態(tài)轉(zhuǎn)移函數(shù)當(dāng)作提議分布、把似然函數(shù)當(dāng)作提議分布和混合提議分布等[5]。由于混合提議分布既考慮了狀態(tài)轉(zhuǎn)移函數(shù)，又考慮了似然函數(shù)，所以得到的重要性權(quán)值方差更小。雖然得到了較小的方差，但是在計(jì)算重要性權(quán)重時(shí)，難以實(shí)現(xiàn)計(jì)算過程中的積分運(yùn)算。如果似然函數(shù)的分布呈峰態(tài)分布，那么采樣效率低下[6]。粒子濾波是用樣本來近似表示概率密度函數(shù)，用樣本均值取代積分過程，得到最小方差估計(jì)。如果粒子數(shù)較小，算法的誤差大，估計(jì)精度低，理論上如果粒子數(shù)目可以趨于無窮大，算法能夠得到最優(yōu)估計(jì)，估計(jì)精度最高。可是粒子數(shù)量的大幅增加使得系統(tǒng)的計(jì)算量劇增，算法在實(shí)時(shí)性方面不能達(dá)標(biāo)，其不足之處在此體現(xiàn)。Rao-Blackwellised[7]濾波方法的提出則從另外一個(gè)方向提高了算法的實(shí)時(shí)性。此方法通過HMM濾波、KF、PF算法三者結(jié)合，分開估計(jì)狀態(tài)向量中成員，使得估計(jì)維度減小，從而系統(tǒng)計(jì)算負(fù)擔(dān)得以減輕。Kwok[8]等提出RTPF算法，將粒子集通過擴(kuò)展估計(jì)窗口進(jìn)行劃分，使其成為一個(gè)個(gè)小粒子集，然后將小粒子集的估計(jì)結(jié)果通過賦予不同的權(quán)值重新組合獲得估計(jì)結(jié)果。研究者們將粒子集分割成相同大小的集合，以KL距離(Kullback-Leibler distance)為指標(biāo)來獲取各集合的接近程度，從而動(dòng)態(tài)調(diào)整粒子的數(shù)量提高實(shí)時(shí)性能。當(dāng)算法性能較高時(shí)，采用較少的粒子數(shù)，否則采用較多的粒子數(shù)，這樣既滿足了濾波精度的要求，又減小了系統(tǒng)的計(jì)算負(fù)擔(dān)。由于KL距離沒有對(duì)稱特性[9]，所以當(dāng)概率密度先后次序改變時(shí)，所得到的值也會(huì)改變，那么對(duì)集合間接近程度的衡量結(jié)果就會(huì)不準(zhǔn)，采樣粒子數(shù)量的選擇也會(huì)受到影響，最終影響到估計(jì)結(jié)果。

研究在一般粒子濾波算法基礎(chǔ)上,通過對(duì)建議分布及重采樣過程加以改進(jìn)，提出基于改進(jìn)粒子濾波的移動(dòng)機(jī)器人定位算法。該算法將先驗(yàn)轉(zhuǎn)移概率密度和觀測似然概率密度的混合分布作為提議分布，將當(dāng)前觀測信息融入進(jìn)去，在混合分布基礎(chǔ)上，利用退火參數(shù)調(diào)控兩者在混合提議分布中的比例，對(duì)于退火參數(shù)取定值時(shí)的不足，通過優(yōu)化機(jī)制對(duì)參數(shù)進(jìn)行在線變更。同時(shí)，利用JS距離獲取子集間接近程度，確定采樣粒子的總數(shù)，動(dòng)態(tài)改變粒子的數(shù)量。實(shí)驗(yàn)結(jié)果顯示，改進(jìn)算法在一定程度上解決了粒子退化以及多樣性缺失問題，減少了定位所需粒子數(shù)量，減輕了運(yùn)算負(fù)擔(dān)，提升了算法精度。

1 移動(dòng)機(jī)器人蒙特卡羅定位

定義移動(dòng)機(jī)器人系統(tǒng)含有噪聲的運(yùn)動(dòng)學(xué)方程和觀測方程為：

(1)

式中,xk表示k時(shí)刻的運(yùn)動(dòng)狀態(tài);f表示狀態(tài)轉(zhuǎn)移函數(shù);uk表示k時(shí)刻的控制變量;ωk表示控制噪聲;zk表示系統(tǒng)在k時(shí)刻的觀測信息;g表示觀測函數(shù);vk表示觀測過程中產(chǎn)生的噪聲。

定位的最終目的就是要獲得機(jī)器人在當(dāng)前時(shí)刻的狀態(tài)xk。從貝葉斯濾波角度，問題求解的核心是估計(jì)后驗(yàn)分布Bel(xk)。蒙特卡羅定位將具有不同權(quán)重的粒子重新整合，以此對(duì)k時(shí)刻機(jī)器人狀態(tài)的后驗(yàn)概率進(jìn)行估計(jì)。首先是重要性采樣[10]環(huán)節(jié)，從重要性概率密度q(xk|xk-1,zk,uk-1)采集加權(quán)粒子集合。再經(jīng)過重采樣步驟，得到此刻狀態(tài)后驗(yàn)Bel(xk)的離散估計(jì)。最后，為了解決積分難的問題，采用蒙特卡羅方法以采樣粒子平均值作為狀態(tài)估計(jì)結(jié)果。

(2)

(3)

(4)

蒙特卡羅定位中建議分布的設(shè)計(jì)沒有融入當(dāng)前觀測信息，把狀態(tài)轉(zhuǎn)移函數(shù)作為建議分布,即

q(xk|xk-1,zk,uk-1)=p(xk|xk-1,uk-1),

(5)

重采樣算法被用來克服樣本退化問題，在實(shí)際操作中，直接將評(píng)分過低的粒子舍棄，對(duì)評(píng)分高的粒子進(jìn)行復(fù)制，重采樣之后保持粒子群數(shù)量基本不變。蒙特卡羅定位算法中重要性密度函數(shù)的設(shè)計(jì)，忽略了最新觀測zk對(duì)算法的影響，通過預(yù)測過程獲得的粒子集合會(huì)分布于似然函數(shù)的尾部,出現(xiàn)粒子退化現(xiàn)象，進(jìn)而影響定位結(jié)果。

2 改進(jìn)的蒙特卡羅定位算法

2.1 改進(jìn)的粒子提議分布

針對(duì)粒子濾波中出現(xiàn)的粒子退化現(xiàn)象，采用設(shè)計(jì)更為合理的提議分布的方法來抑制粒子退化。學(xué)者們對(duì)先驗(yàn)分布和后驗(yàn)分布進(jìn)行研究，分析兩種分布所具有的優(yōu)點(diǎn)和缺點(diǎn)，在此基礎(chǔ)上混合提議分布思想出現(xiàn)了[11]?；旌咸嶙h分布將兩種不同的分布放在一起進(jìn)行研究。當(dāng)前最新觀測信息的引入，使得混合提議分布比兩種單獨(dú)的提議分布具有較小的權(quán)重方差?？墒?，重要性權(quán)重的運(yùn)算過程中出現(xiàn)了積分環(huán)節(jié)，難以完成。另外，在似然函數(shù)分布呈現(xiàn)出峰值分布的時(shí)候，采樣的效率極低[12]，所以使用退火參數(shù)來調(diào)整混合提議分布中各分布之間的比例。如果提議分布中的退火參數(shù)取值不變，因?yàn)楦怕史植季哂胁淮_定特性，所以濾波算法的性能較差。鑒于此，依據(jù)預(yù)測數(shù)據(jù)和觀測數(shù)據(jù)的關(guān)系來自動(dòng)調(diào)整退火參數(shù)定位數(shù)值，實(shí)現(xiàn)自適應(yīng)優(yōu)化的目標(biāo)。

在選擇建議分布q(xk|xk-1,zk,uk-1)時(shí)，將最新觀測zk融入到建議分布里，讓采樣環(huán)節(jié)得到的粒子集中地分布于高似然區(qū)域，能有效減輕粒子集合退化問題,提升濾波器性能。將先驗(yàn)轉(zhuǎn)移概率密度和觀測似然概率密度的混合分布作為重要性密度函數(shù)，既考慮到運(yùn)動(dòng)模型又考慮到觀測模型，因此建議分布為：

q=p(zk|xk)p(xk|xk-1,ut-1)Bel(xk-1),

(6)

其相應(yīng)的重要性權(quán)值計(jì)算式為：

ωk=ωk-1p(xk|xk-1,zk-1),

(7)

當(dāng)使用退火參數(shù)對(duì)不同分布的混合率進(jìn)行調(diào)整時(shí)，建議分布選擇為：

q=p(xk|xk-1,ut-1)cp(zk|xk)1-c,

(8)

則重要性權(quán)值計(jì)算式為：

(9)

其中，退火參數(shù)0≤C≤1。自適應(yīng)調(diào)整退火參數(shù)的核心思想是在重要性采樣之后，對(duì)預(yù)測值和觀測值進(jìn)行觀察比較，當(dāng)觀測值比預(yù)測值大時(shí)，減小樣本重要性權(quán)重的占比，將C的數(shù)值往下降。反之，將C的數(shù)值往上升。

2.2 基于JSD的重采樣

重采樣過程的引入會(huì)產(chǎn)生粒子多樣性缺失問題，現(xiàn)采用遺傳學(xué)中的變異操作[13]對(duì)粒子集進(jìn)行優(yōu)化。將粒子分為高權(quán)重、低權(quán)重兩種，高權(quán)重粒子直接參與重采樣，低權(quán)重粒子經(jīng)過變異操作后再參與重采樣。按式(10)確定低權(quán)重閾值，劃分出高權(quán)重粒子和低權(quán)重粒子。

(10)

(11)

式中，Lmax是群體中最大的適應(yīng)度值；Lavg是每一代群體中平均的適應(yīng)度值;L是要進(jìn)行變異的個(gè)體的適應(yīng)度值;Pm1和Pm2對(duì)應(yīng)的是變異概率的最大值和最小值;β是常數(shù)。

粒子濾波使用采樣粒子與其權(quán)值的組合來表示系統(tǒng)后驗(yàn)估計(jì)，其結(jié)果總存在誤差。理論上，粒子數(shù)目的大小對(duì)最終值的精確度會(huì)起到一定作用，當(dāng)集合中粒子數(shù)目比較低的時(shí)候，最終值的誤差會(huì)較大，反之當(dāng)集合中粒子數(shù)目比較高的時(shí)候，同時(shí)在粒子數(shù)目逼近無窮大時(shí)，算法精度最高[14]。因此，當(dāng)選擇粒子數(shù)目的時(shí)候，必須更加全面地去思考，如果選擇的粒子數(shù)目比較低，那么算法的精度就會(huì)降低。但是如果選取過多的粒子，算法在每次迭代過程中要對(duì)每個(gè)粒子進(jìn)行運(yùn)算，導(dǎo)致計(jì)算量大幅增加，不能滿足算法的實(shí)時(shí)性處理需求。

FOX D利用KLD距離采樣對(duì)其重采樣部分進(jìn)行改進(jìn)，自適應(yīng)地改變重采樣過程中需要的粒子數(shù)目。KL距離具有不對(duì)稱性，所以不同的概率密度順序?qū)?yīng)著不同的距離值，依此得出的相似度結(jié)果并不準(zhǔn)確，那么粒子數(shù)目的確定就會(huì)不合理，對(duì)算法性能產(chǎn)生影響。JSD可以克服KLD不對(duì)稱性的缺陷，使算法獲得更好的精度。JSD可以計(jì)算分布S={s1,s2,s3,…,sn}與分布O={o1,o2,o3,…,on}的相似程度：

(12)

JS距離采樣核心思想是將樣本集合一分為二，利用JS距離計(jì)算兩者的相似程度，并且設(shè)置兩個(gè)門限Sl、Sh。如果JS距離小于低限Sl，那么兩者具有高相似度，有一些非必須粒子存在，此時(shí)選取少量粒子進(jìn)行系統(tǒng)狀態(tài)估計(jì)，反之JS距離大于高限Sh，說明兩者具有低相似程度，應(yīng)適當(dāng)增加粒子總數(shù)進(jìn)行系統(tǒng)狀態(tài)估計(jì)，粒子數(shù)按式(13)計(jì)算：

(13)

式中,n為粒子總數(shù)。

圖1 改進(jìn)粒子濾波流程圖

圖1是對(duì)標(biāo)準(zhǔn)粒子濾波算法進(jìn)行改進(jìn)后的算法流程圖。在更新過程之后，判斷是否滿足重采樣的條件，如果條件滿足，則對(duì)更新后的粒子集進(jìn)行變異操作，得到新的粒子集。然后將粒子集合隨機(jī)劃分為大小相等的兩個(gè)子集，計(jì)算它們之間的相似度，根據(jù)式(13)調(diào)整粒子數(shù)，最后進(jìn)入重采樣過程。

2.3 改進(jìn)后的蒙特卡羅定位算法

改進(jìn)算法的具體流程如下：

步驟1 在k=0時(shí)，初始化系統(tǒng)。

步驟2 重要性采樣以及權(quán)重計(jì)算。根據(jù)式(8)求取提議分布，并從中采樣粒子。對(duì)每個(gè)粒子重新分配權(quán)重即權(quán)重更新。

步驟3 權(quán)重歸一化。計(jì)算權(quán)重總和，進(jìn)行歸一化運(yùn)算。

步驟4 重采樣。判斷是否滿足重采樣條件，如果條件滿足，執(zhí)行步驟5，反之，執(zhí)行步驟6。

步驟5 根據(jù)式(10)劃分高、低權(quán)重粒子，低權(quán)重粒子按照式(11)的變異概率進(jìn)行變異操作。根據(jù)式(13)自適應(yīng)改變粒子數(shù)目，對(duì)高權(quán)重粒子和變異產(chǎn)生的粒子進(jìn)行重采樣，得到新的粒子集合，同時(shí)更新權(quán)重。執(zhí)行步驟7。

步驟6 根據(jù)式(13)自適應(yīng)改變粒子數(shù)目。

步驟7 狀態(tài)估計(jì)。把樣本期望值當(dāng)做此刻k的狀態(tài)估計(jì)xk。

步驟8 令k=k+1,返回步驟2。

3 實(shí)驗(yàn)結(jié)果及分析

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

采用仿真實(shí)驗(yàn)來驗(yàn)證改進(jìn)粒子濾波算法的有效性，并將其與利用KL距離采樣的自適應(yīng)PF算法作對(duì)比研究。使用Matlab 7.0軟件進(jìn)行仿真，機(jī)器人系統(tǒng)的狀態(tài)模型和觀測模型如式(14)所示[15]。

(14)

其中,系統(tǒng)噪聲ωk～(0,10)，觀測噪聲vk～(0,1)。根據(jù)經(jīng)驗(yàn)將算法中出現(xiàn)的參數(shù)作出如下設(shè)定：Sl=0.1，Sh=0.8，初始退火參數(shù)設(shè)置為0.3，即C=0.3，Pm1=0.1，Pm2=0.01，粒子總數(shù)設(shè)定為500。在上述參數(shù)條件下將采用KL距離采樣的自適應(yīng)PF算法與研究改進(jìn)PF算法進(jìn)行對(duì)比研究，每種算法分別進(jìn)行100次試驗(yàn)，用誤差均方根值(RMSE)以及有效粒子百分比(NEFF)作為指標(biāo)對(duì)算法性能進(jìn)行評(píng)價(jià)。

(15)

(16)

采用KL距離采樣的自適應(yīng)PF算法[16](圖2～圖8中記為KLPF)以及研究改進(jìn)PF算法(圖2～圖8中記為AMJSPF)的誤差均方根曲線如圖2所示。由圖2可知，在同等誤差和狀態(tài)下，改進(jìn)PF算法的RMSE低于采用KL距離采樣的自適應(yīng)PF算法的RMSE。與采用KL距離采樣的自適應(yīng)PF算法相比，改進(jìn)算法具有較好的估計(jì)精度。不同PF算法粒子數(shù)目的變化情況如圖3所示。從圖3中可以看出，兩種算法都可以在線調(diào)整采樣粒子數(shù)目，相比之下文中改進(jìn)算法的采樣粒子數(shù)目明顯較小。粒子多樣性對(duì)比圖如圖4所示。從圖4可以看出,改進(jìn)算法的粒子多樣性較好。對(duì)兩種粒子濾波器的性能進(jìn)行比較，結(jié)果如表1所示。由表1可知，改進(jìn)算法耗時(shí)較少，有較高的有效粒子數(shù)，粒子退化問題得以減弱。據(jù)此能夠得出研究改進(jìn)PF算法擁有較高估計(jì)精度，減弱了粒子退化問題，減輕了算法計(jì)算負(fù)擔(dān)。

圖4 粒子多樣性對(duì)比圖

表1 不同算法的性能對(duì)比

3.2 ROS上基于AMLSPF-MCL的機(jī)器人定位研究

在室內(nèi)環(huán)境下利用機(jī)器人對(duì)基于KL距離自適應(yīng)粒子濾波的蒙特卡羅定位和基于改進(jìn)粒子濾波的蒙特卡羅定位算法進(jìn)行對(duì)比試驗(yàn)驗(yàn)證。實(shí)驗(yàn)采用的平臺(tái)為Turtblebot2機(jī)器人，其搭載了型號(hào)為Hokuyo UTM-30LX的激光傳感器，在配有Windows7操作系統(tǒng)、i7處理器、4G內(nèi)存的筆記本電腦上運(yùn)行Liunx(Ubuntu 12.04)操作系統(tǒng)和ROS(hydro)。

利用Bailey發(fā)布的開源地圖編輯器設(shè)計(jì)路標(biāo)地圖,地圖區(qū)域?yàn)?00 m×100 m,仿真中設(shè)定移動(dòng)機(jī)器人從全局坐標(biāo)(-10,-20)處開始運(yùn)動(dòng),按規(guī)劃點(diǎn)運(yùn)動(dòng)一周后返回起始點(diǎn)。過程噪聲的方差設(shè)定為40，觀測噪聲的方差設(shè)定為2，移動(dòng)機(jī)器人運(yùn)行速度設(shè)定為0.35 m/s，粒子的數(shù)量設(shè)定為2 000，其他參數(shù)參照仿真實(shí)驗(yàn)進(jìn)行設(shè)定。

采用KLPF-MCL算法進(jìn)行定位的結(jié)果如圖5所示。由圖5可知，實(shí)線代表機(jī)器人的真實(shí)運(yùn)動(dòng)路徑，虛線代表的是估計(jì)路徑，由圖5可知此次跟蹤定位失敗。AMJSPF-MCL定位結(jié)果如圖6所示。由圖6可知，機(jī)器人在采用AMJSPF-MCL算法進(jìn)行定位時(shí)，虛線表示的估計(jì)軌跡與實(shí)線表示的機(jī)器人實(shí)際運(yùn)動(dòng)軌跡基本一致，跟蹤定位成功。不同定位算法的粒子數(shù)目隨著時(shí)間變動(dòng)的改變情況如圖7所示。由圖7可以看出，AMJSPF-MCL算法所需粒子較少，穩(wěn)定在80。不同算法的定位精度隨時(shí)間變動(dòng)的改變情況如圖8所示。由圖8可以看出，粒子的退化使得KLPF-MCL算法的估計(jì)精度降低，最終導(dǎo)致定位失??；AMJSPF-MCL算法由于有最新觀測數(shù)據(jù)融入，使得估計(jì)精度平均值為19.051 cm，在一定程度上解決了粒子退化問題，可以得到較為精確的定位結(jié)果。兩種算法的定位誤差如表2所示。由表2可以看出，AMJSPF-MCL算法的精度比較好，耗時(shí)較少。

圖5 KLPF-MCL定位結(jié)果 圖6 AMJSPF-MCL定位結(jié)果

圖7 粒子數(shù)隨時(shí)間變化情況 圖8 定位精度隨時(shí)間變化情況

表2 兩種算法的定位誤差

4 結(jié)束語

研究所提的定位算法將先驗(yàn)概率分布和似然概率分布的混合分布作為提議分布，兩者的混合比例用退火參數(shù)進(jìn)行優(yōu)化，利用自適應(yīng)優(yōu)化機(jī)制對(duì)退火參數(shù)進(jìn)行動(dòng)態(tài)優(yōu)化，當(dāng)前觀測信息融入，對(duì)粒子退化現(xiàn)象起到了一定抑制作用。利用JS距離進(jìn)行采樣，動(dòng)態(tài)調(diào)整粒子數(shù)量，減小了算法的計(jì)算量，增強(qiáng)了算法實(shí)時(shí)性，算法精度也得到提升。通過遺傳變異操作，粒子多樣性得到一定程度的保持。經(jīng)過實(shí)驗(yàn)，改進(jìn)算法被證明是有效的。研究中JS距離閾值所用的門限值沒有進(jìn)行嚴(yán)密的分析論證，并且在定位的過程中忽略了車輪打滑等影響算法精度的因素，后期將對(duì)算法繼續(xù)進(jìn)行完善。