韓錕，章京濤，楊窮千

(中南大學(xué) 交通運(yùn)輸工程學(xué)院，湖南 長(zhǎng)沙 410075)

21世紀(jì)以來(lái)，機(jī)器人的功能越來(lái)越強(qiáng)大，已經(jīng)能夠?qū)崿F(xiàn)目標(biāo)跟蹤[1]、導(dǎo)航、路徑規(guī)劃等復(fù)雜功能，被廣泛應(yīng)用于工業(yè)、軍事、家庭服務(wù)等領(lǐng)域。SLAM[2]作為移動(dòng)機(jī)器人的基礎(chǔ)問題，是機(jī)器人實(shí)現(xiàn)其他功能的前提，SLAM精度直接影響其他功能實(shí)現(xiàn)的效果，因此，如何提高SLAM精度一直是國(guó)內(nèi)外學(xué)者的研究熱點(diǎn)。SLAM問題是指移動(dòng)機(jī)器人在無(wú)地圖信息的環(huán)境中運(yùn)動(dòng)時(shí)通過實(shí)時(shí)獲取周邊特征信息以實(shí)現(xiàn)對(duì)自身的定位，并在此基礎(chǔ)上創(chuàng)建周圍環(huán)境的增量式地圖。目前，解決SLAM問題較為成熟的方法主要有基于濾波的方法和基于圖優(yōu)化的方法。其中，Rao-Blackwellized粒子濾波 器(Rao-Blackwellized Particle Filter，RBPF)[3]將SLAM問題視為機(jī)器人軌跡估計(jì)和環(huán)境地圖估計(jì)這2個(gè)后驗(yàn)概率的乘積，降低了狀態(tài)估計(jì)問題的空間維數(shù)，大幅度減少了算法的計(jì)算量，得到了廣泛應(yīng)用。但是由于RBPF-SLAM采用的建議分布精度有限，導(dǎo)致粒子退化嚴(yán)重，并且頻繁的重采樣會(huì)引發(fā)樣本貧化問題。近年來(lái)涌現(xiàn)了大量改進(jìn)RBPF-SLAM的研究：王田橙等[4]提出了一種基于區(qū)域粒子群算法的SLAM方法，通過粒子群優(yōu)化驅(qū)使粒子向其所在區(qū)域的中心移動(dòng)，提高粒子種群的質(zhì)量，但在保證種群多樣性方面還存在不足。鄭兵等[5]將螢火蟲算法引入到粒子濾波器中，調(diào)整粒子的分布狀態(tài)，提高了濾波器的估計(jì)精度，但算法的全局尋優(yōu)能力不佳，當(dāng)觀測(cè)存在多個(gè)峰值時(shí)，易陷入局部最優(yōu)解而導(dǎo)致粒子估計(jì)出現(xiàn)偏差。孫弋等[6]利用退火參數(shù)對(duì)建議分布中運(yùn)動(dòng)模型和觀測(cè)模型的比例進(jìn)行調(diào)控，并對(duì)粒子集進(jìn)行交叉變異操作，增加了種群多樣性，但對(duì)建議分布的優(yōu)化還存在改進(jìn)的空間。針對(duì)當(dāng)前RBPF-SLAM改進(jìn)算法仍存在的粒子退化、種群多樣性匱乏導(dǎo)致移動(dòng)機(jī)器人位姿估計(jì)精度不足問題，本文提出一種基于果蠅優(yōu)化算法(Fruit Fly Optimization Algorithm，F(xiàn)OA)[7]改進(jìn)的RBPF算法：第一，引入FOA算法，利用其高尋優(yōu)能力使粒子聚集在高似然區(qū)域，從而使粒子分布更逼近機(jī)器人的真實(shí)位置；第二，引入遺傳算法的自適應(yīng)交叉和變異操作，維持粒子多樣性，對(duì)最優(yōu)粒子進(jìn)行柯西擾動(dòng)變異，防止算法陷入局部最優(yōu)解；第三，改進(jìn)了FOA算法的尋優(yōu)步長(zhǎng)，增大迭代時(shí)粒子的移動(dòng)距離，提高算法的收斂效率。改進(jìn)后的RBPF算法能夠有效減少SLAM所需的粒子數(shù)，并能得到更加精確的柵格地圖。

1 RBPF算法簡(jiǎn)介

RBPF算法的目的是估計(jì)環(huán)境地圖m和機(jī)器人軌跡x1:t的聯(lián)合后驗(yàn)概率，表達(dá)式為p(x1:t,m|z1:t,u1:t-1)，z1:t=z1,z2,…,zt表示激光雷達(dá)獲取的觀測(cè)信息，u1:t=u1,u2,…,ut-1表示里程計(jì)信息。通過貝葉斯公式可將RBPF粒子濾波器分解如下：

RBPF-SLAM由于只采用運(yùn)動(dòng)模型p(xt|xt-1,ut-1)作為建議分布π，隨著時(shí)間的推移，里程計(jì)的累積誤差越來(lái)越大，使其估計(jì)性能下降。文獻(xiàn)[8]在RBPF算法的基礎(chǔ)上做了2方面的改進(jìn)，后被封裝為開源功能包Gmapping：第一，使用運(yùn)動(dòng)模型計(jì)算建議分布時(shí)加入了激光觀測(cè)信息，得到更優(yōu)的建議分布π′=p(xt|mt-1,zt,xt-1,ut-1)。第二，對(duì)重采樣步驟進(jìn)行了自適應(yīng)改進(jìn)，提出衡量粒子的退化嚴(yán)重性的變量Neff，當(dāng)Neff＜N/2時(shí)進(jìn)行重采樣，從而減少了不必要的重采樣，緩解了粒子耗盡的問題。改進(jìn)后算法的流程圖如圖1所示。

圖1 Gmapping的基本流程圖Fig.1 Basic flow chart of Gmapping

雖然Gmapping一定程度上解決了RBPF算法存在的問題，但在規(guī)模較大、相似度高的場(chǎng)合仍存在精度不佳的情況。因?yàn)樵擃悎?chǎng)景需要大量的采樣粒子才能較好地描述后驗(yàn)概率密度分布，導(dǎo)致算法復(fù)雜度大幅提高，引發(fā)粒子退化、種群多樣性降低、算法實(shí)時(shí)性差等問題[9]。針對(duì)上述問題，本文在Gmapping的基礎(chǔ)上利用FOA算法做出進(jìn)一步改進(jìn)。

2 基于FOA的改進(jìn)RBPF算法

2.1 標(biāo)準(zhǔn)FOA算法

果蠅覓食時(shí)，首先根據(jù)食物散發(fā)在空氣中的濃度，依靠自身嗅覺向食物的大致方向移動(dòng)，當(dāng)食物進(jìn)入視野后則依靠視覺精準(zhǔn)飛去。受到果蠅覓食行為的啟發(fā)，F(xiàn)OA算法將每個(gè)果蠅個(gè)體看作一個(gè)可行解，不斷向最佳位置移動(dòng)，尋求模型的全局最優(yōu)解。FOA算法的過程如下。

步驟1：初始化。設(shè)置果蠅初始位置、種群規(guī)模N，最大迭代次數(shù)Maxgen等參數(shù)。

步驟2：果蠅從當(dāng)前位置飛出，在周圍隨機(jī)搜尋濃度更高的位置，移動(dòng)公式如下：

步驟3：由果蠅的位置濃度得到個(gè)體的適應(yīng)度值，并找到種群的最優(yōu)個(gè)體。

步驟4：若當(dāng)前最優(yōu)適應(yīng)度值大于前一迭代最大適應(yīng)度值，則所有果蠅向該最優(yōu)個(gè)體飛去。重復(fù)步驟2～4，直到滿足終止條件。

相比于螢火蟲算法、蝴蝶算法等群智能算法[10]，F(xiàn)OA算法的優(yōu)點(diǎn)在于其算法復(fù)雜度低、機(jī)制簡(jiǎn)單，且參數(shù)相對(duì)較少，只需對(duì)種群規(guī)模、迭代步進(jìn)值和最大迭代次數(shù)等幾個(gè)參數(shù)進(jìn)行設(shè)置調(diào)整[11-12]。

2.2 果蠅優(yōu)化策略

文獻(xiàn)[13]將FOA算法融合進(jìn)粒子濾波算法并將其應(yīng)用于目標(biāo)跟蹤領(lǐng)域，取得了較好的效果，證明了FOA算法優(yōu)化粒子濾波算法的可行性，且目前將FOA算法引入SLAM問題的研究工作極少，因此本文嘗試將FOA算法與RBPF算法進(jìn)行融合。

融合的基本思路為：在采樣過程得到優(yōu)化后提議分布的采樣粒子后，將粒子集看作果蠅種群，掃描匹配的得分作為個(gè)體的適應(yīng)度值，適應(yīng)度值反映了個(gè)體與機(jī)器人真實(shí)狀態(tài)的符合程度。通過FOA算法優(yōu)化粒子分布，驅(qū)使果蠅個(gè)體向最優(yōu)位置飛去，不斷向周邊范圍隨機(jī)搜尋更優(yōu)位置，使果蠅不斷向移動(dòng)機(jī)器人的真實(shí)狀態(tài)逼近，緩解粒子退化。

為克服標(biāo)準(zhǔn)FOA算法在收斂過程中種群多樣性降低、易陷入局部最優(yōu)而導(dǎo)致粒子偏離真實(shí)狀態(tài)的局限，本文引入交叉變異操作對(duì)種群進(jìn)行適應(yīng)性改進(jìn)，將粒子隨機(jī)配對(duì)后根據(jù)自適應(yīng)概率進(jìn)行交叉操作，然后復(fù)制一定數(shù)量的最優(yōu)粒子并對(duì)其進(jìn)行變異操作，以保持種群的多樣性，防止陷入局部最優(yōu)解。最后采用指數(shù)函數(shù)步長(zhǎng)的狀態(tài)更新公式移動(dòng)果蠅個(gè)體，以增大尋優(yōu)步長(zhǎng)，提高算法的收斂速度，同時(shí)有利于算法跳出局部最優(yōu)。

通過上述改進(jìn)提高濾波器的估計(jì)精度，使得所需的粒子數(shù)減少，從而減小了算法計(jì)算量，在大規(guī)模、相似度高等環(huán)境同樣適用，有效解決了Gmapping存在的粒子退化、種群多樣性不足、實(shí)時(shí)性差等問題。

2.3 算法實(shí)現(xiàn)

FOA算法描述為：每個(gè)粒子狀態(tài)xi被視為一個(gè)可行解，根據(jù)適應(yīng)度值fi的大小，獲取種群迭代的最優(yōu)狀態(tài)x′，即利用N個(gè)粒子組成的集合在目標(biāo)搜索空間中搜索最優(yōu)解。

針對(duì)FOA算法存在迭代速度慢、搜索精度低等問題，本文在果蠅尋優(yōu)過程中采用了基于指數(shù)函數(shù)步長(zhǎng)的移動(dòng)公式[14]。由于指數(shù)函數(shù)的變化速率快，因而能夠增加粒子的移動(dòng)步長(zhǎng)，不但算法的收斂效率得到了提高，而且移動(dòng)后的新個(gè)體更有可能跳出局部最優(yōu)。改進(jìn)后的果蠅個(gè)體位置更新公式如下：

確定交叉概率后，將果蠅隨機(jī)配對(duì)，根據(jù)式(4)和式(5)進(jìn)行交叉操作。自適應(yīng)交叉概率使得分布較優(yōu)的粒子也能有一定的交叉概率，這些粒子交叉后，更有可能將優(yōu)勢(shì)基因遺傳到子代，提升種群個(gè)體的質(zhì)量。

式中：p1，p2為概率參數(shù)；fb為選擇算子中更優(yōu)個(gè)體的適應(yīng)度值；A為常數(shù)。

為避免早熟，復(fù)制K個(gè)當(dāng)前最優(yōu)個(gè)體，根據(jù)變異概率Pmu對(duì)其進(jìn)行柯西擾動(dòng)變異操作。若變異個(gè)體的適應(yīng)度值增加，則替換掉原來(lái)的最優(yōu)個(gè)體。采用的柯西變異公式如下[13]：

式中：xa(j)表示變異個(gè)體的狀態(tài)。

2.4 算法步驟

步驟1：根據(jù)運(yùn)動(dòng)模型進(jìn)行狀態(tài)估計(jì)。

步驟2：進(jìn)行掃描匹配。在混合提議分布π′中采樣N個(gè)粒子xi(i=1,…,N)，并將粒子的匹配得分作為適應(yīng)度值fi。

步驟3：調(diào)整粒子分布。

1)找到適應(yīng)度最大fmax的個(gè)體，所有粒子向該個(gè)體飛去，根據(jù)式(3)給出果蠅個(gè)體搜尋食物的步長(zhǎng)。

2)更新種群的適應(yīng)度值，根據(jù)式(4)和式(5)進(jìn)行自適應(yīng)交叉操作。

3)復(fù)制K個(gè)最優(yōu)個(gè)體，按式(6)進(jìn)行變異，若其適應(yīng)度增加，則替換為最優(yōu)個(gè)體。

4)進(jìn)行迭代尋優(yōu)，直到滿足適應(yīng)度條件或達(dá)到最大迭代次數(shù)Maxgen。

步驟4：計(jì)算優(yōu)化后粒子的權(quán)重并進(jìn)行歸一化，若Neff＜N/2，則進(jìn)行重采樣。

步驟6：進(jìn)入下一時(shí)刻，轉(zhuǎn)到步驟1。

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

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

采用激光SLAM中典型的數(shù)據(jù)集：ACES building和MΙT Killian Court，對(duì)比不同算法的地圖拓?fù)浣Y(jié)構(gòu)的正確性。ACES數(shù)據(jù)集環(huán)境規(guī)模較小、結(jié)構(gòu)較為簡(jiǎn)單，MΙT數(shù)據(jù)集地圖規(guī)模較大且相似度高，包含較多的閉環(huán)，調(diào)整參數(shù)linearUpdate=1,angularUpdate=0.5以取得更好的效果[15]。進(jìn)行實(shí)驗(yàn)的計(jì)算機(jī)的操作系統(tǒng)為Ubuntu16.04，主頻為2.70 GHz。本文算法中，取Maxgen=10，p1=0.3，p2=0.7，Pmu=0.05，α=0.7。

3.1.1 本文算法的有效性驗(yàn)證

對(duì)RBPF算法和本文算法進(jìn)行仿真對(duì)比實(shí)驗(yàn)，驗(yàn)證本文算法的有效性。在ACES數(shù)據(jù)集上的仿真結(jié)果如圖2所示，可以看出，RBPF算法得到的地圖在標(biāo)記1，2和3處存在錯(cuò)亂、分層現(xiàn)象，效果較差。而本文算法得到的地圖邊緣清晰，無(wú)重疊等現(xiàn)象。圖3為MΙT數(shù)據(jù)集的仿真結(jié)果。可以看出，RBPF算法的構(gòu)圖一致性較差，未能正確完成閉環(huán)。相比之下，本文算法在粒子數(shù)為60時(shí)，雖然地圖在標(biāo)記1，2和3處出現(xiàn)了一些重疊，但整體效果較好，粒子數(shù)為100時(shí)得到的地圖能夠很好地反映真實(shí)環(huán)境。

圖2 10個(gè)粒子時(shí)ACESbuilding柵格地圖Fig.2 ACESbuilding raster map of ten particles

圖3 MΙT Killian Court柵格地圖Fig.3 MΙT Killian Court raster map

此外，本文采用CPU占用率來(lái)衡量算法的性能，具體方法為：等時(shí)間采集3組CPU的占用率數(shù)據(jù)，每一組包含50個(gè)數(shù)值，樣本數(shù)量為150，最后求取平均值。2種算法在各數(shù)據(jù)集下的粒子數(shù)和CPU占用率對(duì)比如表1所示。由表1可以看出，本文算法構(gòu)建一致性地圖所需的粒子數(shù)大幅度減少，減少了算法復(fù)雜度，CPU占用率較RBPF算法降低了約20%。

表1 各數(shù)據(jù)集粒子數(shù)和CPU使用率Table 1 Particle count and CPU usage per dataset

3.1.2 改進(jìn)FOA算法的必要性驗(yàn)證

為驗(yàn)證FOA算法引入RBPF算法所作出改進(jìn)的必要性，對(duì)標(biāo)準(zhǔn)FOA優(yōu)化的RBPF算法和本文算法進(jìn)行對(duì)比實(shí)驗(yàn)。標(biāo)準(zhǔn)FOA優(yōu)化的RBPF算法在ACES數(shù)據(jù)集下的仿真結(jié)果如圖4所示?？梢钥闯觯撍惴ㄏ噍^于RBPF算法無(wú)明顯改進(jìn)，地圖一致性較差。標(biāo)準(zhǔn)FOA優(yōu)化的RBPF算法在MΙT數(shù)據(jù)集下的仿真結(jié)果如圖5所示，可以看出，粒子數(shù)為60時(shí)該算法的建圖效果并不理想，地圖在標(biāo)記1，2，3和4處未完成閉環(huán)，粒子數(shù)為100時(shí)，地圖在標(biāo)記2和4的對(duì)應(yīng)位置仍存在較大的偏差。

圖4 10個(gè)粒子時(shí)ACESbuilding柵格地圖Fig.4 ACESbuilding raster map of ten particles

圖5 MΙT Killian Court柵格地圖Fig.5 MΙT Killian Court raster map

綜上，只使用FOA算法進(jìn)行優(yōu)化不能取得理想的效果，而作出適應(yīng)性改進(jìn)后的算法能有效改善地圖效果，驗(yàn)證了本文對(duì)FOA算法作出的步長(zhǎng)改進(jìn)和遺傳改進(jìn)的必要性。

3.2 實(shí)機(jī)測(cè)試

實(shí)驗(yàn)平臺(tái)采用如圖6所示的移動(dòng)機(jī)器人，該機(jī)器人采用雙輪差分驅(qū)動(dòng)方式，裝載RPLΙDARA2激光雷達(dá)，配有ΙntelΙ5-2430M處理器，CPU 2.4 GHz，板載2 GB內(nèi)存的工控機(jī)，系統(tǒng)安裝Ubuntu16.04操作系統(tǒng)，激光雷達(dá)安裝在高度約為0.2 m處。實(shí)驗(yàn)環(huán)境選擇室內(nèi)，大小為6.6 m×7.2 m，如圖7所示。

圖6 機(jī)器人模型Fig.6 Robot model

圖7 實(shí)驗(yàn)場(chǎng)景Fig.7 Laboratory environment

RBPF算法和本文算法對(duì)實(shí)驗(yàn)場(chǎng)景的建圖結(jié)果如圖8所示。由圖8可知，RBPF算法取粒子數(shù)為10時(shí)，地圖產(chǎn)生了嚴(yán)重的不一致現(xiàn)象；粒子數(shù)為20時(shí)，地圖在標(biāo)記1，2和3處仍出現(xiàn)“假墻”、未閉合現(xiàn)象。而本文算法在粒子數(shù)為10時(shí)就能夠取得很好的建圖效果。

圖8 2種算法的真實(shí)場(chǎng)景建圖結(jié)果Fig.8 Realistic scenario building results of two algorithms

4 結(jié)論

1)利用FOA算法改善粒子分布，結(jié)合自適應(yīng)交叉變異操作，對(duì)RBPF算法進(jìn)行優(yōu)化，提高了濾波器的估計(jì)精度?；诜抡鎸?shí)驗(yàn)和實(shí)機(jī)測(cè)試結(jié)果表明，對(duì)相同復(fù)雜度的環(huán)境地圖，本文算法能在粒子數(shù)減少50%甚至更多的情況下，取得比RBPF算法更好的建圖結(jié)果。

2)本文算法通過改善粒子分布，減少了算法所需粒子數(shù)，在ACES和MΙT數(shù)據(jù)集的仿真實(shí)驗(yàn)結(jié)果表明，本文算法CPU的占用率較RBPF算法降低了約20%，提升了SLAM的效率和實(shí)時(shí)性。

3)通過仿真實(shí)驗(yàn)結(jié)果可以看出，直接將FOA算法引入RBPF算法效果不明顯，而作出適應(yīng)性改進(jìn)后算法得到的地圖效果明顯改善，表明利用標(biāo)準(zhǔn)FOA算法優(yōu)化RBPF算法存在局限性，應(yīng)用時(shí)需要作出適當(dāng)改進(jìn)。