韓錕，章京濤，楊窮千

(中南大學 交通運輸工程學院，湖南 長沙 410075)

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

1 RBPF算法簡介

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

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

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

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

2 基于FOA的改進RBPF算法

2.1 標準FOA算法

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

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

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

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

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

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

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

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

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

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

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

2.3 算法實現(xiàn)

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

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

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

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

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

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

2.4 算法步驟

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

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

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

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

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

3)復制K個最優(yōu)個體，按式(6)進行變異，若其適應度增加，則替換為最優(yōu)個體。

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

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

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

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

3.1 仿真實驗

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

3.1.1 本文算法的有效性驗證

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

圖2 10個粒子時ACESbuilding柵格地圖Fig.2 ACESbuilding raster map of ten particles

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

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

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

3.1.2 改進FOA算法的必要性驗證

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

圖4 10個粒子時ACESbuilding柵格地圖Fig.4 ACESbuilding raster map of ten particles

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

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

3.2 實機測試

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

圖6 機器人模型Fig.6 Robot model

圖7 實驗場景Fig.7 Laboratory environment

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

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

4 結(jié)論

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

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

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