基于AF和AS算法優(yōu)化的slam_gmapping研究

顏 韓，汪 偉，崔金華，代迪迪，王汝佳

（江蘇理工學(xué)院 汽車與交通工程學(xué)院，江蘇 常州 213001）

近年來，隨著人工智能技術(shù)的快速發(fā)展，移動機(jī)器人被廣泛應(yīng)用于工業(yè)生產(chǎn)中，如倉儲搬運(yùn)機(jī)器人、掃地機(jī)器人等，為人們提供了切實(shí)的便利。但是，隨著“智能制造2025”的提出，傳統(tǒng)的磁導(dǎo)、慣導(dǎo)技術(shù)已經(jīng)無法滿足人們的需求，人們渴望機(jī)器人能夠無時無刻在任何環(huán)境中進(jìn)行自主定位與導(dǎo)航。

即時定位與建圖（SLAM）技術(shù)為自主定位與導(dǎo)航提供了扎實(shí)的理論基礎(chǔ)。SLAM具體指機(jī)器人憑借自身的傳感器（激光雷達(dá)或者攝像頭等）感知環(huán)境信息，并進(jìn)行自主定位與建圖。SLAM技術(shù)最早由Smith等人[1]提出，之后一直受到相關(guān)行業(yè)研究者的關(guān)注。早期的濾波研究著重于卡爾曼濾波，但是人們逐漸意識到卡爾曼濾波在處理非線性、非高斯噪聲時的局限性[2]。因此，粒子濾波（Particle Filter，簡稱PF）應(yīng)運(yùn)而生。粒子濾波也叫順序蒙特卡洛方法，該方法是通過一組在狀態(tài)空間中傳播的隨機(jī)樣本近似地表示概率密度函數(shù)，從而實(shí)現(xiàn)系統(tǒng)的狀態(tài)估計(jì)[3]。由于粒子濾波在處理非線性系統(tǒng)方面具有得天獨(dú)厚的優(yōu)勢，所以被廣泛應(yīng)用。Murphy等人[4]第一次提出RBPF算法，該算法創(chuàng)造性地將定位與建圖分開，即先利用自身傳感器進(jìn)行定位，再依據(jù)定位進(jìn)行環(huán)境建圖，最后融合建圖對定位進(jìn)行修正。Grisetti等人[5]提出了Gmapping算法，該算法在RBPF算法基礎(chǔ)上提出改進(jìn)提議分布和選擇性重采樣；實(shí)驗(yàn)結(jié)果表明，這種方法不僅有效減小了系統(tǒng)的計(jì)算量，而且有利于緩解粒子退化和粒子多樣性減少的問題。相較于國外，國內(nèi)對于SLAM的研究雖然起步較晚，但是發(fā)展迅速，如何佳澤、張壽明[6]針對傳統(tǒng)RBPF-SLAM算法構(gòu)建地圖存在重影、粒子出現(xiàn)衰退等問題，設(shè)計(jì)了閾值重采樣函數(shù)進(jìn)行優(yōu)化采樣。章弘凱等人[7]針對2D激光SLAM機(jī)器人導(dǎo)航中激光點(diǎn)云匹配計(jì)算量大、軌跡閉合效果差、位姿累積誤差大等問題，提出一種基于多層次傳感器數(shù)據(jù)融合的實(shí)時定位與建圖方法——Multilevel-SLAM；實(shí)驗(yàn)結(jié)果表明，Multilevel-SLAM算法有效提高了閉環(huán)處的軌跡閉合效果，具有更低的定位誤差。丁元浩等人[8]針對室內(nèi)環(huán)境下的2D激光同步定位與構(gòu)圖問題，提出一種改進(jìn)的掃描匹配方法，掃描到子圖匹配；實(shí)驗(yàn)結(jié)果表明，該方法可以有效提高定位與制圖的精度和激光匹配效率。

雖然越來越多的學(xué)者對SLAM展開了深入的研究，但是Gmapping算法在高相似度、多閉環(huán)等復(fù)雜環(huán)境下，仍存在建圖效果不太理想甚至建圖失敗的問題。針對此問題，鄭兵等人[9]提出將螢火蟲算法（AF）與Gmapping相融合的算法。但是，該方法與RBPF、Gmapping算法一樣都存在粒子數(shù)恒定的問題。所以，本文嘗試提出了一種自適應(yīng)采樣（Adaptive Sampling，簡稱AS）算法，即當(dāng)二維激光點(diǎn)云波動量大于某個閥值時，增加采樣粒子數(shù)，反之適當(dāng)減少采樣數(shù)。此外，為了使系統(tǒng)在復(fù)雜環(huán)境中有更好的建圖效果，本文還將AS、AF與Gmapping相融合，進(jìn)而提高系統(tǒng)復(fù)雜環(huán)境的建圖能力。

1 傳統(tǒng)Gmapping算法

1.1 RBPF算法

眾所周知，SLAM問題的核心是如何利用觀測量和運(yùn)動控制量來求解系統(tǒng)的位姿和地圖的聯(lián)合分布P（X1:t,m|Z1:t,U1:t?1），而同時求解X1:t和m很難。因此，Murphy等人[4]提出了RBPF算法，該算法運(yùn)用概率論的思想來分解問題，即將聯(lián)合后驗(yàn)概率分解成兩個后驗(yàn)概率密度乘積的形式，如公式（1）所示：

其中：P(X1:t|,Z1:t,U1:t?1)代表定位的問題，即機(jī)器人系統(tǒng)利用自身傳感器（里程計(jì)、激光或者攝像頭）信息對位姿進(jìn)行估計(jì)定位；P(m|X1:t,Z1:t)，即機(jī)器人依據(jù)位姿信息進(jìn)行增量式地圖m的創(chuàng)建。

1.2 改進(jìn)提議分布

由上述RBPF算法可知，為了估計(jì)下一時刻的位姿，需要從提議分布中采樣，因此提議分布的精確程度對系統(tǒng)位姿估計(jì)具有舉足輕重的作用。而RBPF中的提議分布僅僅考慮了里程計(jì)信息，隨著時間的積累會產(chǎn)生很大的誤差，導(dǎo)致其采樣粒子無法準(zhǔn)確估計(jì)系統(tǒng)當(dāng)前的狀態(tài)。Gmapping算法在RBPF的基礎(chǔ)上將最近一次的觀測信息融入到提議分布中，但由于激光觀測的數(shù)據(jù)是360°無死角的距離信息，在實(shí)際情況下無法進(jìn)行運(yùn)動模型的建立，而且當(dāng)觀測概率比較尖銳時，會產(chǎn)生和RBPF算法一樣的缺陷——計(jì)算量太大。所以，提出可以直接從峰值附近采K個值來模擬出提議分布，優(yōu)化后的高斯函數(shù)提議分布如下：

進(jìn)一步優(yōu)化后的權(quán)重遞推公式就變成了：

1.3 選擇性重采樣

RBPF算法根據(jù)每個粒子的權(quán)重進(jìn)行重采樣，保留權(quán)重值大的粒子，并復(fù)制權(quán)重值大的粒子代替權(quán)重小的粒子，使粒子的分布更加接近真實(shí)情況。但是，頻繁的重采樣會使系統(tǒng)的運(yùn)算量激增。同時，也有可能會濾去權(quán)重小的但更接近真實(shí)情況的粒子，使粒子退化和粒子多樣性減少。Gmapping算法在此基礎(chǔ)上融合了理論判定方法，提出了有效粒子數(shù)Neff的概念，設(shè)定只有當(dāng)有效粒子數(shù)低于某個閥值B時，才會進(jìn)行重采樣操作。有效粒子數(shù)計(jì)算公式如下：

2 Gmapping算法的優(yōu)化

2.1 自適應(yīng)采樣

由前文可知，傳統(tǒng)Gmapping算法在RBPF的基礎(chǔ)上改進(jìn)了提議分布和選擇性重采樣，極大緩解了粒子退化和粒子多樣性減少的問題。但是，由于算法中的采樣粒子數(shù)仍是固定不變的，而現(xiàn)實(shí)環(huán)境是復(fù)雜多變的，因此，以固定的粒子數(shù)構(gòu)建復(fù)雜多變的環(huán)境地圖，不但會造成計(jì)算資源的浪費(fèi)，而且也難以保證復(fù)雜環(huán)境的建圖完整性。

針對上述問題，本文提出了一種自適應(yīng)采樣（Adaptive Sampling，簡稱AS）算法，將采樣粒子數(shù)和現(xiàn)實(shí)環(huán)境的復(fù)雜程序進(jìn)行線性擬合，當(dāng)二維激光點(diǎn)云波動量大于某個閥值時，增加采樣粒子數(shù)，反之適當(dāng)減少采樣數(shù)。算法運(yùn)行步驟為：當(dāng)系統(tǒng)啟動時，里程計(jì)和激光雷達(dá)采集周圍環(huán)境信息，首先將激光點(diǎn)云進(jìn)行線性擬合處理，進(jìn)而判斷點(diǎn)云線性的波動量；當(dāng)波動量小于閥值X’時，認(rèn)定外界環(huán)境為簡單環(huán)境，此時減小采樣粒子數(shù)，達(dá)到減少系統(tǒng)運(yùn)算量的目的；當(dāng)點(diǎn)云波動量超過閥值時，增加采樣粒子數(shù)，進(jìn)而增強(qiáng)系統(tǒng)對于復(fù)雜環(huán)境的定位與建圖效果；接著再進(jìn)行點(diǎn)云匹配、權(quán)重計(jì)算、重采樣（由有效粒子數(shù)Neff決定）、地圖更新等操作。具體流程如圖1所示。

自適應(yīng)采樣算法能夠充分合理地利用系統(tǒng)資源。在簡單環(huán)境中，線性地減少采樣粒子數(shù)，進(jìn)而緩解系統(tǒng)資源的壓力，同時減少定位和建圖時間；在面臨多閉環(huán)、高相似復(fù)雜環(huán)境時，也能有較好的建圖效果。除此之外，經(jīng)過預(yù)處理的激光點(diǎn)云高度線性化，能夠極大地減少掃描匹配階段的匹配時間。因此，該方法能夠有效緩解采樣粒子數(shù)恒定導(dǎo)致的運(yùn)算資源浪費(fèi)問題，同時也能夠增強(qiáng)系統(tǒng)面對復(fù)雜環(huán)境的建圖能力。

2.2 螢火蟲算法及優(yōu)化

螢火蟲算法（Firefly Algorithm，簡稱FA）最早是由崔昊楊[10]根據(jù)螢火蟲的發(fā)光特性提出的，其主要原理是利用發(fā)光強(qiáng)的螢火蟲吸引發(fā)光弱的螢火蟲，使發(fā)光弱的螢火蟲移動到最佳位置，以此來達(dá)到尋優(yōu)的目的。

圖1 自適應(yīng)采樣算法流程圖

螢火蟲算法的數(shù)學(xué)描述中涉及的兩個重要參數(shù)是發(fā)光強(qiáng)度和相互吸引度，其計(jì)算公式如下[11]：

其中：I表示螢火蟲的相對發(fā)光強(qiáng)度；I0表示最亮螢火蟲的亮度，即r=0處的熒光強(qiáng)度；γ表示光吸收系數(shù)，此參數(shù)是為了體現(xiàn)熒光會隨距離的增加而逐漸減弱的特性；rij表示螢火蟲i和螢火蟲j之間的距離；β()r表示螢火蟲之間的相互吸引度；β0則表示最大吸引度，即r=0處的吸引度。

此外，該算法是通過迭代的方法不斷使發(fā)光弱的螢火蟲移動到最優(yōu)位置，其最優(yōu)目標(biāo)迭代公式如下[12]：

其中：Xj(t)，Xi(t)分別表示t時刻螢火蟲j和i的空間位置；α為步長因子；rand則為[0，1]上服從均勻分布的隨機(jī)因子。一般螢火蟲算法的偽代碼如表1所示[13]。

表1 螢火蟲算法的偽代碼

本研究中，為了增強(qiáng)復(fù)雜環(huán)境的建圖效果，引入螢火蟲算法對粒子進(jìn)行尋優(yōu)操作，具體包括兩部分：區(qū)域劃分和選擇性尋優(yōu)。首先，本文將粒子權(quán)重與螢火蟲熒光亮度參數(shù)進(jìn)行信息交互，并根據(jù)亮度值將粒子劃分為高相似區(qū)和低相似區(qū)，同時，分別計(jì)算出高相似區(qū)和低相似區(qū)的局部最優(yōu)值，并進(jìn)行局部尋優(yōu)操作。由于在尋優(yōu)過程中要不斷計(jì)算出兩兩粒子的相對亮度與吸引度，從而使得系統(tǒng)的計(jì)算量大大增加。

文獻(xiàn)[14]提出將全局最優(yōu)值與各粒子進(jìn)行信息交互的方法，使每個粒子只和全局最優(yōu)粒子進(jìn)行信息交互，其位置迭代公式如下：

其中，pbesti表示全局最優(yōu)粒子的狀態(tài)值。此方法避免了兩兩粒子之間反復(fù)的計(jì)算過程，將各粒子只與全局最優(yōu)粒子進(jìn)行信息交互，大大減小了系統(tǒng)的計(jì)算量，是切實(shí)可行的。

此外，為了防止螢火蟲算法因高尋優(yōu)性能導(dǎo)致粒子多樣性減少的問題，本文設(shè)置了一個閥值Nf，當(dāng)局部粒子數(shù)超過閥值Nf時，則停止尋優(yōu)操作；反之，則繼續(xù)進(jìn)行尋優(yōu)操作，直至達(dá)到迭代次數(shù)或者尋優(yōu)精度。此方法在防止粒子多樣性減少的同時，也能夠減少粒子的尋優(yōu)速度，進(jìn)而減少系統(tǒng)的計(jì)算量。

基于螢火蟲算法（FA）和自適應(yīng)采樣（AS）算法優(yōu)化的slam_gmapping算法具體步驟如表2所示。

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

3.1 實(shí)驗(yàn)平臺與仿真環(huán)境

本文中仿真實(shí)驗(yàn)是以Ubuntu20.04操作系統(tǒng)為基礎(chǔ)，運(yùn)用Noetic版本的ROS平臺及系統(tǒng)自帶的Turlebot機(jī)器人模型展開實(shí)驗(yàn)，同時綜合運(yùn)用RVIZ、Gazebo和rqt等可視化模塊進(jìn)行實(shí)驗(yàn)探索和分析。在Gzaebo中，通過Building Editor模塊搭建簡單和復(fù)雜的物理仿真環(huán)境；在RVIZ和rqt中觀察算法的建圖效果。機(jī)器人仿真模型如圖2所示，簡單、復(fù)雜的物理仿真環(huán)境如圖3、圖4所示。

表2 優(yōu)化后的slam_gmapping算法的具體步驟

圖2 Turlebot機(jī)器人模型圖

圖3 簡單的物理仿真環(huán)境

圖4 復(fù)雜的物理仿真環(huán)境

3.2 實(shí)驗(yàn)結(jié)果與分析

仿真實(shí)驗(yàn)主要包括三個方面；研究優(yōu)化算法的定位精度；比較傳統(tǒng)Gmapping算法、文獻(xiàn)[9]中單純?nèi)诤衔灮鹣x算法的Gmapping算法與本文優(yōu)化后的算法在簡單環(huán)境中的建圖時間與效果；對比3種算法在多閉環(huán)、高相似度環(huán)境中的建圖效果與平均建圖時間。

3.2.1 優(yōu)化算法定位精度研究

為了驗(yàn)證優(yōu)化算法定位系統(tǒng)的可靠性與定位精度，選取在復(fù)雜環(huán)境中從起點(diǎn)沿預(yù)計(jì)軌跡運(yùn)動到終點(diǎn)的方案。采用A*算法進(jìn)行軌跡路線的預(yù)估，運(yùn)用move_base控制方法探索優(yōu)化算法位置的定位精度。如圖5所示：圖中綠色軌跡表示A*算法預(yù)估的軌跡路線，圖中①和②代表機(jī)器人的起點(diǎn)和終點(diǎn)。圖6為系統(tǒng)X，Y方向定位誤差曲線 圖，表3為系統(tǒng)預(yù)估值與真實(shí)定位值對比表。

圖5 機(jī)器人軌跡路線圖

圖6 X，Y方向定位誤差曲線圖

表3 系統(tǒng)預(yù)估值與真實(shí)定位值對比表

由圖6可知，隨著運(yùn)行時間的增加，系統(tǒng)定位誤差值也逐漸增加，在采樣點(diǎn)3處達(dá)到最大值0.024 m；但是，從采樣點(diǎn)3開始誤差值逐漸減少，這是因?yàn)榻K點(diǎn)約束對機(jī)器人的影響逐漸增大，迫使機(jī)器人運(yùn)動到終點(diǎn)位置。此外，由表3可以看出，系統(tǒng)的X，Y方向的平均定位誤差分別為0.013 6 m和0.013 2 m，表明系統(tǒng)定位真實(shí)值與系統(tǒng)預(yù)估值吻合度較高，可滿足一般環(huán)境的定位要求。

圖7 傳統(tǒng)算法在簡單環(huán)境的建圖效果

3.2.2 對比3種算法在簡單環(huán)境中的建圖效果

為了確保傳統(tǒng)Gmapping算法和單純?nèi)诤螦F的Gmapping算法的建圖效果，選取50個粒子作為實(shí)驗(yàn)參數(shù)，而改進(jìn)后的算法無需給定粒子數(shù)。此外，經(jīng)多次實(shí)驗(yàn)發(fā)現(xiàn)，點(diǎn)云波動量閥值X’為12時建圖效果更佳。3種算法在簡單物理環(huán)境中的建圖效果見圖7、圖8、圖9，其平均建圖時間對比如表4所示。

圖8 文獻(xiàn)[9]算法在簡單環(huán)境的建圖效果

圖9 優(yōu)化后算法在簡單環(huán)境的建圖效果

表4 3種算法在簡單環(huán)境建圖時間對比表

從圖7、圖8、圖9可以看出，改進(jìn)后的算法在簡單物理環(huán)境中的建圖效果比傳統(tǒng)算法和單純?nèi)诤螦F的Gmapping算法有所增強(qiáng)，其建圖能力能夠滿足現(xiàn)實(shí)需要。從表4可知，優(yōu)化后的算法其采樣粒子數(shù)明顯減少，平均建圖時間為362 s，相比傳統(tǒng)算法和單純?nèi)诤螦F的Gmapping算法分別縮短了約14%和7%。但是，優(yōu)化后的算法增加了代碼的數(shù)量和執(zhí)行步驟，即增加了系統(tǒng)的復(fù)雜度，這也是其建圖時間縮短的幅度沒有文獻(xiàn)[2]那么大的原因之一。

3.2.3 對比3種算法在復(fù)雜環(huán)境中的建圖效果

針對傳統(tǒng)Gmapping算法在復(fù)雜環(huán)境建圖效果不佳的情況，本文將螢火蟲算法與Gmapping算法融合，利用螢火蟲算法的集聚能力，增強(qiáng)算法的建圖能力。但是，螢火蟲算法是通過不斷迭代的方法尋優(yōu)的，所以合理地選擇迭代次數(shù)，有利于縮短算法的建圖速度。通過多次實(shí)驗(yàn)發(fā)現(xiàn)，在上述復(fù)雜環(huán)境中，4次迭代數(shù)在節(jié)省建圖時間的同時也能夠滿足建圖的需求，故本實(shí)驗(yàn)選擇迭代次數(shù)為4次。此外，因?yàn)槲灮鹣x算法集聚能力過強(qiáng)，為了避免粒子多樣性缺少，本文將迭代終止閥值設(shè)置為0.02。3種算法在復(fù)雜環(huán)境的建圖效果見圖10、圖11、圖12，其建圖運(yùn)行時間對比見表5。

圖10 傳統(tǒng)算法在復(fù)雜環(huán)境的建圖效果

從圖10、圖11、圖12可以看出，優(yōu)化后算法的建圖效果比傳統(tǒng)算法更優(yōu)，將墻壁的厚度都能詳細(xì)描述出來，且建圖也更加完整。此外，從表5可以看出，優(yōu)化后的算法在面臨復(fù)雜環(huán)境時，平均建圖時間為408 s，相比傳統(tǒng)算法和單純?nèi)诤螦F的Gmapping算法，其建圖效率分別提高了約3%和0.9%。需要說明的是：由于本算法在面臨復(fù)雜環(huán)境時融合了自適應(yīng)采樣和螢火蟲算法，導(dǎo)致采樣粒子數(shù)和代碼復(fù)雜度略微增加；但是，經(jīng)過預(yù)處理的激光點(diǎn)云高度線性化，能夠極大減少掃描匹配階段的匹配時間，因此其平均建圖時間則有所縮短。

圖11 文獻(xiàn)[9]算法在復(fù)雜環(huán)境的建圖效果

圖12 優(yōu)化后算法在復(fù)雜環(huán)境的建圖效果

表5 3種算法在復(fù)雜環(huán)境的建圖時間對比

4 結(jié)語

針對傳統(tǒng)Gmapping算法粒子數(shù)恒定的問題，提出了一種自適應(yīng)采樣算法（AS），該算法可根據(jù)環(huán)境的復(fù)雜程度線性地選擇采樣粒子的數(shù)量。實(shí)驗(yàn)結(jié)果表明，優(yōu)化后的算法能夠提高簡單環(huán)境的建圖速度。此外，還將螢火蟲算法（AF）融合到優(yōu)化算法當(dāng)中，利用螢火蟲算法的高集聚能力，將采樣粒子通過不斷迭代的方法進(jìn)行尋優(yōu)操作；同時，為了防止粒子過度集聚，依據(jù)粒子權(quán)重將全部粒子劃分為高相似區(qū)和低相似區(qū)，并設(shè)置迭代閥值。實(shí)驗(yàn)結(jié)果表明：融合優(yōu)化后的算法在面臨多閉環(huán)、高相似復(fù)雜環(huán)境時，在滿足定位精度的前提下，建圖效果得到了顯著提升，平均建圖時間也有所縮短。但是，該算法也有不足之處，如在面臨復(fù)雜環(huán)境時，會因采樣粒子數(shù)和代碼復(fù)雜度增加等問題導(dǎo)致平均建圖時間提升緩慢，后續(xù)將針對此問題作進(jìn)一步的探索與研究。