劉紅林，凌有鑄，陳孟元

(安徽工程大學(xué) 安徽省電氣傳動與控制重點實驗室，安徽 蕪湖 241000)

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

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

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

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

(1)

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

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

(2)

(3)

(4)

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

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

(5)

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

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

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

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

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

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

(6)

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

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

(7)

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

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

(8)

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

(9)

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

2.2 基于JSD的重采樣

重采樣過程的引入會產(chǎn)生粒子多樣性缺失問題，現(xiàn)采用遺傳學(xué)中的變異操作[13]對粒子集進(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)行變異的個體的適應(yīng)度值;Pm1和Pm2對應(yīng)的是變異概率的最大值和最小值;β是常數(shù)。

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

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

(12)

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

(13)

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

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

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

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

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

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

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

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

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

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

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

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

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

3 實驗結(jié)果及分析

3.1 仿真實驗

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

(14)

其中,系統(tǒng)噪聲ωk～(0,10)，觀測噪聲vk～(0,1)。根據(jù)經(jīng)驗將算法中出現(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)行對比研究，每種算法分別進(jìn)行100次試驗，用誤差均方根值(RMSE)以及有效粒子百分比(NEFF)作為指標(biāo)對算法性能進(jìn)行評價。

(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)算法具有較好的估計精度。不同PF算法粒子數(shù)目的變化情況如圖3所示。從圖3中可以看出，兩種算法都可以在線調(diào)整采樣粒子數(shù)目，相比之下文中改進(jìn)算法的采樣粒子數(shù)目明顯較小。粒子多樣性對比圖如圖4所示。從圖4可以看出,改進(jìn)算法的粒子多樣性較好。對兩種粒子濾波器的性能進(jìn)行比較，結(jié)果如表1所示。由表1可知，改進(jìn)算法耗時較少，有較高的有效粒子數(shù)，粒子退化問題得以減弱。據(jù)此能夠得出研究改進(jìn)PF算法擁有較高估計精度，減弱了粒子退化問題，減輕了算法計算負(fù)擔(dān)。

圖4 粒子多樣性對比圖

表1 不同算法的性能對比

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

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

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

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

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

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

表2 兩種算法的定位誤差

4 結(jié)束語

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