SLAM中Gammping算法的優化

耿志文 邱慧麗 姜飛

本文簡述了移動機器人運動同步定位與地圖構建（Simultaneous Localization AndMapping，SLAM）模塊的基本原理，介紹了幾種常用的算法，并對現在最常用的Gammping算法進行了解釋和優化，提出了對Gammping算法的優化方案，使用Robot Operation System（ROS）軟件平臺，使移動機器人擁有了更高的建圖效率和更加準確的路徑規劃。

Perface

SLAM在移動機器人地圖構建中被經常使用，使用類型和算法主要有激光和視覺2種。二者原理不同，激光SLAM依賴于激光雷達通過對周圍環境進行探測從而通過珊格建圖;視覺SLAM依靠前端視覺里程計，本文研究的主要是激光SLAM建圖算法。Gamappmg算法相對于其他算法的優勢在于可以依靠里程計更精準的建圖，并且對激光傳感器頻率要求相對較低。當移動機器人構建現實空間較小場景時擁有高計算精度且計算量不大，所以本文重點研究對Gamapping的調優改進方案。

SLAM建圖常用算法

Gmapping算法

Gmapping算法由Giorgio等人首次提出，關于算法求解的主要問題在由map坐標系計算得到odom坐標系并推算出base link坐標系與laser坐標系。Gmapping算法相較于其他算法的特點表現在其利用里程計位姿信息來減少構建小場景地圖時的計算量。其中最常見的算法之一就是粒子濾波算法（Rao-Blackwelised Particle Filter，RBPF），把定位與建圖時的整個過程進行了分離，直接進行定位然后再建圖。RBPFSLAM的一個核心理念就是如何利用激光雷達檢測信息。

Gmapping算法依靠激光雷達和里程計獲取環境位姿信息，優化粒子分布并且引入自適應重采樣機制，提高了算法效率。

Karto SLAM算法

Karto SLAM是Karto Robotics開發，主要思想是圖優化方法。

Hector SLAM算法

Hector SLAM是一種魯棒性比較好的算法。使用激光雷達的高更新精度來對圖形進行構建，這種算法不需要里程計數據，但是對激光雷達的性能有很高的要求，需要高更新率且噪聲比較低的多線激光雷達。

Hector算法通過構建柵格地圖來模擬真實環境，可對任意的環境進行地圖構建。當對柵格地圖完成構建后還會對后續的激光雷達掃描地圖來匹配掃描，Hector SLAM的匹配算法通過迭代來完成對地圖的精確構建。通過確定機器人到驗地圖對最佳轉換矩陣，ζ=[pxpyΨ]T使傳感器數據和環境地圖達到最佳匹配。

Cartographer SLAM算法

Cartographer是基于圖優化對方法的激光SLAM算法，通過獲取傳感器測量數據生成分辨率為厘米級的實時柵格地圖。前端包括子圖掃描匹配和回環檢測，同時生成子圖且數據輸入完成局部回環檢測。后端則進行位姿估計優化實現全局閉環檢測。

SLAM自主導航系統

路徑規劃算法

激光SLAM的幾種算法通過構建柵格地圖來模擬真實場景，并通過確定任意起點和任意終點來進行路徑規劃。路徑規劃分為全局和局部，通過構建全局路徑來確定機器人到達終點的最優算法，局部路徑規劃算法對全局路徑來實時動態調節，對路線進行細微的修正且檢測路徑中的障礙并進行避障。

全局路徑規劃

全局路徑規劃能在確定環境地圖信息和初始位姿前提下來對路徑進行規劃。A*算法結合多種算法思想，廣泛應用于各種路徑規劃場景，它使用貪婪最佳優先搜索（Greedy Best-First Search，GBFS）提高效率，同時使用評估函數計算最優路徑。A*算法的規劃路徑為：

f（n）= g（n）+ h（n）

其中，g（n）表示從起始節點nstart到節點n到實際代價;h（n）表示節點n到目標節點start的估算代價;f（n）表示從起始節點n到目標節點的估算代價。

Gammping算法改進

算法優點

Gmapping的主要優勢體現在對里程計位姿信息的有效利用，在坐標轉換和獲取最優粒子位姿勢上可以極大提升效率。算法開始接受數據對地圖進行初始化并獲取粒子位姿計算map to odom，并進行第二次處理激光數據來進行優化，如果滿足時間間隔就對地圖進行更新，并用流程化處理對地圖進行不斷優化。而在地圖初始化上首先獲取激光坐標系名稱然后來獲得機器人坐標系下激光位姿，計算激光中心坐標作為laser系下的坐標參考從而設置激光掃描角度和激光傳感器位姿、角度數組。再根據laser系下激光中心位姿得到odom下激光位姿get odom pose，此時才獲得到真正有用的位姿信息，從而對地圖參數進行初始化。

算法缺點

算法的主要缺點表現在構建大場景地圖時往往需要的內存和計算量都會倍增，并且由于缺失回環檢測可能導致地圖錯位。

算法改進

真實分布不能直接得到，但可以通過其他分布計算真實分布，評價標準為權重f/g，f為目標分布，g表示提議分布。粒子權值的方差會不斷增加，而激光具有單峰特性會導致方差更小，可以改善基本粒子濾波算法退化問題，所以采取以下措施優化：

①增加采樣點，更多的采樣可以獲得更多粒子，從而改善退化問題;

②增加重采樣，本質是增加粒子多樣性;

③選擇合理的建議密度。

本文介紹了幾種常用SLAM算法，并提出了對Gmappmg算法的改進建議，通過更多采樣來延緩退化并更準確地構建地圖，增加重采樣來增加粒子多樣性，并選擇合理的建議密度來減少粒子濾波器中不必要的重采樣次數。