SLAM精度的向量加權(quán)平均自適應(yīng)調(diào)節(jié)研究*

蔡 艷,楊光永,陳旭東,徐天奇

(云南民族大學(xué)電氣信息工程學(xué)院,昆明 650000)

0 引言

隨著社會(huì)不斷發(fā)展,越來(lái)越多場(chǎng)合下需要移動(dòng)機(jī)器人在不確定的陌生環(huán)境中進(jìn)行自主工作。因此,SLAM(simultaneous localization and mapping)即瞬時(shí)定位與建圖,成為了機(jī)器人導(dǎo)航與定位領(lǐng)域的一項(xiàng)重要技術(shù)[1]。目前SLAM的求解方法可大致分為基于卡爾曼濾波器的方法、基于粒子濾波器的方法、基于圖優(yōu)化的方法[2]。粒子濾波(particle filter,PF)基于蒙特卡羅方法,易實(shí)現(xiàn)且具有非線性[3-4],是常用于解決SLAM問(wèn)題的方法之一。

由于粒子濾波算法基于蒙特卡洛方法,因此要達(dá)到所需估計(jì)精度需要大量的粒子,而粒子數(shù)量越多,算法時(shí)間復(fù)雜度越高;此外,重采樣會(huì)不斷拷貝權(quán)重較大的樣本,拋棄權(quán)重較小的樣本[5],易導(dǎo)致樣本多樣性下降,出現(xiàn)粒子退化的問(wèn)題。而進(jìn)化問(wèn)題和粒子濾波器本質(zhì)上都是通過(guò)評(píng)價(jià)、選擇和更新的迭代過(guò)程獲得最優(yōu)解[5],因此很多研究者采用進(jìn)化方法來(lái)解決粒子濾波器存在的問(wèn)題。如陳志強(qiáng)等[6]將帝王蝶算法融入粒子濾波中,引入自適應(yīng)遺傳參數(shù)提高算法精度;田夢(mèng)楚等[7]采用螢火蟲算法智能優(yōu)化粒子濾波,利用螢火蟲群體的優(yōu)勝劣汰機(jī)制及個(gè)體的吸引和移動(dòng)的行為,提高粒子濾波的預(yù)測(cè)精度;劉海濤等[8]利用遺傳算法改進(jìn)粒子濾波,提出對(duì)低權(quán)值粒子的改進(jìn)的智能粒子濾波處理策略,以此提高精度等。上述方法均提高了粒子濾波算法的性能,但大多都是通過(guò)經(jīng)驗(yàn)值控制智能算法迭代次數(shù),易造成優(yōu)化不足或過(guò)度優(yōu)化而引起估計(jì)精度降低的問(wèn)題。

受這些方法的啟發(fā),本文引入向量加權(quán)平均算法這一智能算法來(lái)改善粒子濾波器的性能。向量加權(quán)平均算法(weighted mean of vectors algorithm,INFO)是一種新型智能優(yōu)化算法[11],該算法通過(guò)向量的不同加權(quán)平均規(guī)則,來(lái)達(dá)到尋優(yōu)目的。相比之前多數(shù)的智能算法,該算法的搜索精度、收斂速度、穩(wěn)定性更佳[11],為避免僅依靠經(jīng)驗(yàn)值設(shè)置迭代次數(shù),引入自適應(yīng)調(diào)整迭代次數(shù)策略。

本文為進(jìn)一步提高PF算法的性能,引入自適應(yīng)調(diào)整迭代次數(shù)的INFO收斂速度快、尋優(yōu)能力強(qiáng)的特性更新預(yù)測(cè)粒子集,提高粒子集質(zhì)量;當(dāng)粒子密集度過(guò)高時(shí),加入次優(yōu)粒子的作用合理調(diào)整粒子集分布,多樣化粒子;根據(jù)自調(diào)INFO獲取的最優(yōu)解調(diào)整預(yù)測(cè)粒子集,使粒子集在權(quán)重計(jì)算前就更接近期望值,以此提高路徑和路標(biāo)的估計(jì)精度;在重采樣階段通過(guò)重組粒子集來(lái)增加粒子多樣性。

1 自調(diào)向量加權(quán)平均優(yōu)化的粒子重組PF

1.1 自調(diào)向量加權(quán)平均算法

向量加權(quán)平均算法(weighted mean of vectors algorithm,INFO),是一種新型智能優(yōu)化算法[11]。該算法通過(guò)向量的不同加權(quán)平均規(guī)則,來(lái)達(dá)到尋優(yōu)目的,具有尋優(yōu)能力強(qiáng),收斂速度快等特點(diǎn)。為自適應(yīng)調(diào)整迭代次數(shù)與粒子分布,提出一種自調(diào)INFO算法,在原算法基礎(chǔ)上增加了粒子密度監(jiān)測(cè)與自調(diào)粒子分布策略,該算法包括以下5個(gè)階段。

(1)粒子更新規(guī)則階段。AINFO使用基于均值的規(guī)則(MeanRule)更新粒子的位置,此外,為了提高全局搜索能力,在更新規(guī)則算子中加入了收斂加速部分。更新規(guī)則的主要公式如式(1)和式(2)所示。

(1)

(2)

(3)

(3)局部粒子搜索階段。AINFO使用局部搜索階段來(lái)防止陷入局部最優(yōu)解。根據(jù)該算子,如果rand<0.5,則可以圍繞生成一個(gè)新的粒子。新生成粒子表達(dá)式為:

(4)

(4)粒子密度監(jiān)測(cè)階段。為避免僅依賴經(jīng)驗(yàn)值設(shè)置迭代次數(shù)造成優(yōu)化不足或過(guò)度優(yōu)化的問(wèn)題,引入粒子密度監(jiān)測(cè)階段。實(shí)時(shí)監(jiān)測(cè)最優(yōu)粒子附近的粒子密度,當(dāng)密度達(dá)到閾值時(shí),停止迭代。粒子密度計(jì)算表達(dá)式為:

(5)

式中:Nbest表示最優(yōu)粒子周圍的粒子數(shù),rbest表示密度半徑,find操作表示查找密度半徑內(nèi)的粒子數(shù)量。

(6)

式中:ρbest表示最優(yōu)粒子周圍的粒子密度,N表示粒子總數(shù)。

(5)自調(diào)粒子分布階段。當(dāng)粒子集過(guò)于集中于最優(yōu)粒子附近時(shí),考慮次優(yōu)粒子的影響自適應(yīng)調(diào)整部分粒子分布,從而增加整體粒子多樣性。當(dāng)粒子密度超過(guò)所設(shè)置的最大集中密度時(shí),根據(jù)下式調(diào)整部分粒子分布:

(7)

式中:σ為[0,1]之間的隨機(jī)數(shù)。

1.2 粒子重組重采樣

本文針對(duì)粒子濾波算法在重采樣階段去除小權(quán)值粒子而降低估計(jì)性能的問(wèn)題,在重采樣階段設(shè)計(jì)如下粒子篩選策略:

(1)計(jì)算有效粒子數(shù)Neff以及粒子權(quán)重,并將權(quán)重按降序排列;

(8)

1.3 基于自調(diào)向量加權(quán)平均的粒子重組粒子濾波

針對(duì)傳統(tǒng)粒子濾波算法存在需要大量粒子來(lái)提高精度與粒子多樣性缺失的問(wèn)題,本文引入INFO算法對(duì)其進(jìn)行優(yōu)化。為避免優(yōu)化不足或過(guò)度優(yōu)化且減少時(shí)間復(fù)雜度,采用自適應(yīng)調(diào)整迭代次數(shù)策略;為增加粒子多樣性引入自適應(yīng)調(diào)整粒子集分布與重采樣階段粒子重組策略。算法步驟為:

步驟1:初始化粒子集X0,每個(gè)粒子初始權(quán)重為w0=1/M;

步驟2:從proposal分布中采樣粒子集,t時(shí)刻的采樣粒子集Xt如下:

Xt～p(xt|xt-1,ut)

(9)

步驟3:AINFO優(yōu)化,更新粒子集:

①根據(jù)粒子集初始化向量集;

②對(duì)粒子集中所有粒子根據(jù)式(9)計(jì)算適應(yīng)度函數(shù);

③粒子更新規(guī)則階段,根據(jù)式(1)、式(2)更新粒子位置;

④粒子合并階段,通過(guò)更新規(guī)則階段得到的粒子根據(jù)式(3)合并成新的粒子;

⑤局部粒子搜索階段,如果rand<0.5,則根據(jù)式(4)生成一個(gè)新的粒子;

⑥粒子密度監(jiān)測(cè)階段,根據(jù)式(5)、式(6)計(jì)算粒子密度,當(dāng)密度大于閾值ρth時(shí),停止迭代;

⑦自調(diào)粒子分布階段,當(dāng)粒子密度大于ρmax時(shí),根據(jù)式(7)調(diào)整粒子分布。

步驟4:根據(jù)AINFO的輸出更新粒子集分布;

步驟5:計(jì)算粒子權(quán)重;

步驟6:粒子重組重采樣,根據(jù)式(8)重組粒子集;

步驟7:迭代更新預(yù)測(cè)值,根據(jù)下式輸出預(yù)測(cè)值。

(10)

基于AINFO優(yōu)化的粒子重組粒子濾波算法流程圖如圖1所示。

圖1 基于AINFO的粒子重組PF算法流程圖

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

進(jìn)行仿真實(shí)驗(yàn)驗(yàn)證本文算法在SLAM中的估計(jì)性能,由于基于粒子濾波的FastSLAM因定位精度較高、魯棒性較好等特性已發(fā)展成SLAM技術(shù)的主流算法[12],因此將傳統(tǒng)FastSLAM與本文AINFO-FastSLAM算法進(jìn)行仿真對(duì)比實(shí)驗(yàn)。

2.1 密度半徑設(shè)定

引入智能算法優(yōu)化粒子濾波時(shí),一般設(shè)置最大迭代次數(shù),而智能算法迭代過(guò)程中具有隨機(jī)性,僅通過(guò)最大迭代次數(shù)可能會(huì)造成過(guò)度優(yōu)化或優(yōu)化不足問(wèn)題。本文引入自適應(yīng)調(diào)整策略,設(shè)置粒子密度監(jiān)測(cè)階段,自適應(yīng)調(diào)整迭代次數(shù)。粒子密度的衡量標(biāo)準(zhǔn)取決于密度半徑的設(shè)定,表1統(tǒng)計(jì)了當(dāng)粒子數(shù)為100時(shí),不同時(shí)刻不同密度半徑下的粒子密度占比值。

表1 不同時(shí)刻不同密度半徑下的粒子密度占比

從表1可知,當(dāng)密度半徑為0.01時(shí),粒子密度總體隨著迭代次數(shù)的增加而增大,但局部存在波動(dòng)且隨著迭代次數(shù)增加粒子密度增長(zhǎng)較緩慢,達(dá)到期望優(yōu)化程度時(shí)間復(fù)雜度較高;當(dāng)密度半徑為0.05與0.1時(shí),隨著迭代次數(shù)的增加粒子密度的增長(zhǎng)速度較快,但后者粒子密度在前幾次迭代就達(dá)到100%,迭代次數(shù)過(guò)少會(huì)導(dǎo)致精度下降問(wèn)題。當(dāng)密度半徑為0.05時(shí),隨著迭代次數(shù)的增加,粒子密度增長(zhǎng)趨勢(shì)相對(duì)平緩,能較好反映粒子分布情況,因此,本文選取密度半徑為0.05。

2.2 自適應(yīng)調(diào)整迭代次數(shù)分析

為進(jìn)一步驗(yàn)證本文算法自適應(yīng)調(diào)整迭代次數(shù)的準(zhǔn)確性,分別在粒子數(shù)為20、50和80的情況下進(jìn)行30次仿真實(shí)驗(yàn),計(jì)算該算法的平均迭代次數(shù),最后計(jì)算30次實(shí)驗(yàn)的平均迭代次數(shù),結(jié)果如表2所示。

表2 不同粒子數(shù)的AINFO-PF平均迭代次數(shù)

從表2可以看出,隨著粒子數(shù)目的增加,AINFO-PF算法的平均迭代次數(shù)會(huì)相應(yīng)減少,這是因?yàn)?粒子數(shù)的增加會(huì)提高粒子濾波的估計(jì)精度,達(dá)到期望優(yōu)化程度的迭代次數(shù)會(huì)相應(yīng)減少,由此可見,本文算法可根據(jù)當(dāng)前優(yōu)化程度自適應(yīng)調(diào)整迭代次數(shù),避免過(guò)度優(yōu)化或優(yōu)化不足導(dǎo)致的估計(jì)精度降低問(wèn)題,且可自適應(yīng)減少迭代次數(shù),降低計(jì)算復(fù)雜度。

2.3 仿真模型

(1)運(yùn)動(dòng)模型,本文移動(dòng)機(jī)器人運(yùn)動(dòng)模型為:

(11)

式中:[xtytθt]T表示t時(shí)刻機(jī)器人預(yù)測(cè)位姿,ΔT為機(jī)器人里程計(jì)采樣時(shí)間間隔。

(2)觀測(cè)模型,描述了機(jī)器人觀測(cè)值與當(dāng)前位姿估計(jì)和環(huán)境路標(biāo)之間的關(guān)系。

(12)

式中:dt與βt分別為t時(shí)刻機(jī)器人觀測(cè)到的路標(biāo)距離和角度。

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

為驗(yàn)證本文算法有較好的估計(jì)性能,將INFO-FastSLAM、AINFO-FastSLAM算法與傳統(tǒng)的FastSLAM算法進(jìn)行對(duì)比實(shí)驗(yàn),并進(jìn)行誤差對(duì)比分析。

根據(jù)Bailey開發(fā)的SLAM算法通用模擬器[16]設(shè)計(jì)了尺寸為100 m×82 m,并具有42個(gè)路標(biāo)特征的環(huán)境地圖及機(jī)器人運(yùn)行參照路徑,模擬機(jī)器人在真實(shí)場(chǎng)景中進(jìn)行瞬時(shí)定位與建圖,仿真環(huán)境如圖2所示,其中黑色星號(hào)代表路標(biāo),灰色折線代表機(jī)器人的控制輸入量,即規(guī)定路徑。灰色圓圈為機(jī)器人位姿點(diǎn),共有14個(gè)。仿真環(huán)境的移動(dòng)機(jī)器人相關(guān)參數(shù)設(shè)置如表3所示。

表3 仿真實(shí)驗(yàn)機(jī)器人參數(shù)設(shè)置

圖2 仿真環(huán)境地圖

設(shè)置機(jī)器人相關(guān)參數(shù)后,機(jī)器人從坐標(biāo)原點(diǎn)出發(fā)沿預(yù)設(shè)路徑逆時(shí)針運(yùn)動(dòng)一圈,通過(guò)運(yùn)動(dòng)模型與觀測(cè)信息實(shí)現(xiàn)實(shí)時(shí)定位與建圖。仿真實(shí)驗(yàn)結(jié)果對(duì)比如圖3所示。

(a) 傳統(tǒng)FastSLAM算法預(yù)測(cè)結(jié)果 (b) INFO-FastSLAM算法預(yù)測(cè)結(jié)果

從圖3可以看出,傳統(tǒng)FastSLAM隨著誤差的累積,機(jī)器人路徑隨著時(shí)間推移誤差增大,路標(biāo)估計(jì)也出現(xiàn)較大誤差;INFO-FastSLAM與AINFO-FastSLAM的機(jī)器人路徑與真實(shí)路徑基本一致,且路標(biāo)估計(jì)的誤差明顯減少;但I(xiàn)NFO-FastSLAM存在估計(jì)魯棒性不佳現(xiàn)象,路徑估計(jì)會(huì)突然偏離真實(shí)路徑,AINFO-FastSLAM相比INFO-FastSLAM,其路徑估計(jì)更穩(wěn)定且誤差更小,同時(shí)環(huán)境路標(biāo)估計(jì)位置與實(shí)際路標(biāo)位置基本一致。其原因在于:INFO-FastSLAM運(yùn)用向量加權(quán)平均優(yōu)化這一思想優(yōu)化傳統(tǒng)算法,能有效提高路徑與路標(biāo)估計(jì)性能,但僅依靠經(jīng)驗(yàn)值會(huì)存在過(guò)度優(yōu)化導(dǎo)致精度下降或優(yōu)化不夠?qū)е抡`差較大的問(wèn)題;AINFO-FastSLAM采用自適應(yīng)調(diào)整策略的INFO算法調(diào)整預(yù)測(cè)粒子集,避免了過(guò)度優(yōu)化或優(yōu)化不足的問(wèn)題且減少時(shí)間復(fù)雜度,能實(shí)時(shí)糾正機(jī)器人路徑估計(jì),降低路標(biāo)估計(jì)誤差。

2.5 實(shí)驗(yàn)誤差對(duì)比

為進(jìn)一步驗(yàn)證本文算法應(yīng)用到實(shí)際場(chǎng)合中的有效性,在該仿真環(huán)境中,分別對(duì)傳統(tǒng)FastSLAM、INFO-FastSLAM以及AINFO-FastSLAM三種算法進(jìn)行位姿預(yù)測(cè)誤差對(duì)比與路標(biāo)位置預(yù)測(cè)誤差對(duì)比,如圖4～圖6所示。

圖4 x方向位姿估計(jì)誤差 圖5 y方向位姿估計(jì)誤差

圖6 路標(biāo)位置估計(jì)誤差

圖4和圖5為3種算法的位姿預(yù)測(cè)誤差比較圖?？梢钥闯?INFO-FastSLAM在x軸的估計(jì)誤差非常不穩(wěn)定,相比之下AINFO-FastSLAM的位姿預(yù)測(cè)誤差穩(wěn)定且更小;從圖6可以看出,AINFO-FastSLAM算法的路標(biāo)位置預(yù)測(cè)誤差相比INFO-FastSLAM算法更小。

考慮到實(shí)驗(yàn)結(jié)果的偶然性,為進(jìn)一步證明本文提出的算法性能的優(yōu)越性,引入均方根誤差(RMSE)對(duì)機(jī)器人路徑與路標(biāo)位置估計(jì)進(jìn)行評(píng)價(jià)[17],將3種算法分別進(jìn)行30次仿真實(shí)驗(yàn)并取平均值,采用RMSE作為衡量指標(biāo),表達(dá)式為:

(13)

表4 3種算法x軸、y軸、路標(biāo)位置均方根誤差對(duì)比

3 結(jié)論

本文提出一種基于自調(diào)INFO優(yōu)化的粒子濾波算法,引入自適應(yīng)調(diào)整策略的INFO更新預(yù)測(cè)粒子集,當(dāng)粒子密集度過(guò)高時(shí),自適應(yīng)調(diào)整粒子集分布,使預(yù)測(cè)粒子集更快向移動(dòng)機(jī)器人真實(shí)位置聚集,提高粒子集質(zhì)量;在重采樣階段,考慮大權(quán)值粒子和部分小權(quán)值粒子的共同作用重組粒子集。將該算法應(yīng)用到SLAM中,仿真實(shí)驗(yàn)結(jié)果表明,當(dāng)粒子數(shù)為20時(shí),AINFO-FastSLAM相比傳統(tǒng)FastSLAM在x軸位姿估計(jì)精度提高了78.5%,在y軸位姿估計(jì)精度提高了80.3%,路標(biāo)估計(jì)精度提高了81.6%。在同等條件下,該算法定位及地圖構(gòu)建精度更高,綜合性能更強(qiáng)。