基于AF和AS算法優化的slam_gmapping研究

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

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

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

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

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

1 傳統Gmapping算法

1.1 RBPF算法

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

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

1.2 改進提議分布

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

進一步優化后的權重遞推公式就變成了：

1.3 選擇性重采樣

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

2 Gmapping算法的優化

2.1 自適應采樣

由前文可知，傳統Gmapping算法在RBPF的基礎上改進了提議分布和選擇性重采樣，極大緩解了粒子退化和粒子多樣性減少的問題。但是，由于算法中的采樣粒子數仍是固定不變的，而現實環境是復雜多變的，因此，以固定的粒子數構建復雜多變的環境地圖，不但會造成計算資源的浪費，而且也難以保證復雜環境的建圖完整性。

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

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

2.2 螢火蟲算法及優化

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

圖1 自適應采樣算法流程圖

螢火蟲算法的數學描述中涉及的兩個重要參數是發光強度和相互吸引度，其計算公式如下[11]：

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

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

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

表1 螢火蟲算法的偽代碼

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

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

其中，pbesti表示全局最優粒子的狀態值。此方法避免了兩兩粒子之間反復的計算過程，將各粒子只與全局最優粒子進行信息交互，大大減小了系統的計算量，是切實可行的。

此外，為了防止螢火蟲算法因高尋優性能導致粒子多樣性減少的問題，本文設置了一個閥值Nf，當局部粒子數超過閥值Nf時，則停止尋優操作；反之，則繼續進行尋優操作，直至達到迭代次數或者尋優精度。此方法在防止粒子多樣性減少的同時，也能夠減少粒子的尋優速度，進而減少系統的計算量。

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

3 仿真實驗與結果分析

3.1 實驗平臺與仿真環境

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

表2 優化后的slam_gmapping算法的具體步驟

圖2 Turlebot機器人模型圖

圖3 簡單的物理仿真環境

圖4 復雜的物理仿真環境

3.2 實驗結果與分析

仿真實驗主要包括三個方面；研究優化算法的定位精度；比較傳統Gmapping算法、文獻[9]中單純融合螢火蟲算法的Gmapping算法與本文優化后的算法在簡單環境中的建圖時間與效果；對比3種算法在多閉環、高相似度環境中的建圖效果與平均建圖時間。

3.2.1 優化算法定位精度研究

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

圖5 機器人軌跡路線圖

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

表3 系統預估值與真實定位值對比表

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

圖7 傳統算法在簡單環境的建圖效果

3.2.2 對比3種算法在簡單環境中的建圖效果

為了確保傳統Gmapping算法和單純融合AF的Gmapping算法的建圖效果，選取50個粒子作為實驗參數，而改進后的算法無需給定粒子數。此外，經多次實驗發現，點云波動量閥值X’為12時建圖效果更佳。3種算法在簡單物理環境中的建圖效果見圖7、圖8、圖9，其平均建圖時間對比如表4所示。

圖8 文獻[9]算法在簡單環境的建圖效果

圖9 優化后算法在簡單環境的建圖效果

表4 3種算法在簡單環境建圖時間對比表

從圖7、圖8、圖9可以看出，改進后的算法在簡單物理環境中的建圖效果比傳統算法和單純融合AF的Gmapping算法有所增強，其建圖能力能夠滿足現實需要。從表4可知，優化后的算法其采樣粒子數明顯減少，平均建圖時間為362 s，相比傳統算法和單純融合AF的Gmapping算法分別縮短了約14%和7%。但是，優化后的算法增加了代碼的數量和執行步驟，即增加了系統的復雜度，這也是其建圖時間縮短的幅度沒有文獻[2]那么大的原因之一。

3.2.3 對比3種算法在復雜環境中的建圖效果

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

圖10 傳統算法在復雜環境的建圖效果

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

圖11 文獻[9]算法在復雜環境的建圖效果

圖12 優化后算法在復雜環境的建圖效果

表5 3種算法在復雜環境的建圖時間對比

4 結語

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