SLAM中Gammping算法的優(yōu)化

耿志文 邱慧麗 姜飛

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

Perface

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

SLAM建圖常用算法

Gmapping算法

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

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

Karto SLAM算法

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

Hector SLAM算法

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

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

Cartographer SLAM算法

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

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

路徑規(guī)劃算法

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

全局路徑規(guī)劃

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

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

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

Gammping算法改進(jìn)

算法優(yōu)點(diǎn)

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

算法缺點(diǎn)

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

算法改進(jìn)

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

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

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

③選擇合理的建議密度。

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