耿志文 邱慧麗 姜飛

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

Perface

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

SLAM建圖常用算法

Gmapping算法

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

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

Karto SLAM算法

Karto SLAM是Karto Robotics開發(fā)，主要思想是圖優(yōu)化方法。

Hector SLAM算法

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

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

Cartographer SLAM算法

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

SLAM自主導(dǎo)航系統(tǒng)

路徑規(guī)劃算法

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

全局路徑規(guī)劃

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

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

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

Gammping算法改進

算法優(yōu)點

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

算法缺點

算法的主要缺點表現(xiàn)在構(gòu)建大場景地圖時往往需要的內(nèi)存和計算量都會倍增，并且由于缺失回環(huán)檢測可能導(dǎo)致地圖錯位。

算法改進

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

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

②增加重采樣，本質(zhì)是增加粒子多樣性;

③選擇合理的建議密度。

本文介紹了幾種常用SLAM算法，并提出了對Gmappmg算法的改進建議，通過更多采樣來延緩?fù)嘶⒏鼫蚀_地構(gòu)建地圖，增加重采樣來增加粒子多樣性，并選擇合理的建議密度來減少粒子濾波器中不必要的重采樣次數(shù)。