基于濾波器的激光SLAM方案

01 ? 激光SLAM簡介

基于激光雷達(dá)的同時(shí)定位與地圖構(gòu)建技術(shù)（simultaneous localization and mapping， SLAM）以其準(zhǔn)確測量障礙點(diǎn)的角度與距離、 無須預(yù)先布置場景、可融合多傳感器、 在光線較差環(huán)境工作、 能夠生成便于導(dǎo)航的環(huán)境地圖等優(yōu)勢，成為目前定位方案中不可或缺的新技術(shù)。

激光 SLAM 任務(wù)是搭載激光雷達(dá)的主體于運(yùn)動(dòng)中估計(jì)自身的位姿，同時(shí)建立周圍的環(huán)境地圖。而準(zhǔn)確的定位需要精確的地圖，精確的地圖則來自于準(zhǔn)確的定位，定位側(cè)重自身位姿估計(jì)，建圖側(cè)重外部環(huán)境重建。

SLAM系統(tǒng)一般分為五個(gè)模塊：傳感器數(shù)據(jù)、視覺里程計(jì)、后端、建圖及回環(huán)檢測。

傳感器數(shù)據(jù)：主要用于采集實(shí)際環(huán)境中的各類型原始數(shù)據(jù)。包括激光掃描數(shù)據(jù)、視頻圖像數(shù)據(jù)、點(diǎn)云數(shù)據(jù)等。

視覺里程計(jì)：主要用于不同時(shí)刻間移動(dòng)目標(biāo)相對位置的估算。包括特征匹配、直接配準(zhǔn)等算法的應(yīng)用。

后端：主要用于優(yōu)化視覺里程計(jì)帶來的累計(jì)誤差。包括濾波器、圖優(yōu)化等算法應(yīng)用。

建圖：用于三維地圖構(gòu)建。

回環(huán)檢測：主要用于空間累積誤差消除

SLAM工作的主要流程為：

傳感器讀取數(shù)據(jù)后，視覺里程計(jì)估計(jì)兩個(gè)時(shí)刻的相對運(yùn)動(dòng)（Ego-motion），后端處理視覺里程計(jì)估計(jì)結(jié)果的累積誤差，建圖則根據(jù)前端與后端得到的運(yùn)動(dòng)軌跡來建立地圖，回環(huán)檢測考慮了同一場景不同時(shí)刻的圖像，提供了空間上約束來消除累積誤差。

激光雷達(dá)采集到的物體信息呈現(xiàn)出一系列分散的、具有準(zhǔn)確角度和距離信息的點(diǎn)，被稱為點(diǎn)云。通常，激光SLAM系統(tǒng)通過對不同時(shí)刻兩片點(diǎn)云的匹配與比對，計(jì)算激光雷達(dá)相對運(yùn)動(dòng)的距離和姿態(tài)的改變，也就完成了對機(jī)器人自身的定位。

激光雷達(dá)距離測量比較準(zhǔn)確，誤差模型簡單，在強(qiáng)光直射以外的環(huán)境中運(yùn)行穩(wěn)定，點(diǎn)云的處理也比較容易。同時(shí)，點(diǎn)云信息本身包含直接的幾何關(guān)系，使得機(jī)器人的路徑規(guī)劃和導(dǎo)航變得直觀。激光SLAM理論研究也相對成熟，落地產(chǎn)品更豐富。

02 ? 激光SLAM（二維）框架整理

03 ? 開源激光SLAM方案

根據(jù)所采用的凸優(yōu)化框架， 激光 SLAM 可分為兩大類：基于濾波器（Filter-based）和基于圖優(yōu)化（Graph-based）的激光 SLAM。

3.1?基于濾波器的激光SLAM方案

3.2? 基于圖優(yōu)化激光SLAM方案

在真實(shí)環(huán)境中，由于周圍特征點(diǎn)能夠輔助機(jī)器人定位和建圖，構(gòu)建的柵格地圖會(huì)比仿真環(huán)境中效果好。Hector-SLAM 方案的缺點(diǎn)是對初值敏感，同時(shí)難以處理閉環(huán)問題。Hector-SLAM 的整體建圖精度高于Gmapping，但對參數(shù)配置要求較高。Hector -SLAM 適用于對地圖要求較高的場合， Gmapping 易用性更好。

3.3 3D 激光SLAM?

在 3D 激光 SLAM 領(lǐng)域中， 由 Zhang J 等人提出的LOAM?方案，利用 3D 激光雷達(dá)采集數(shù)據(jù)， 進(jìn)行基于特征點(diǎn)的掃描匹配， 利用非線性優(yōu)化方法進(jìn)行運(yùn)動(dòng)估計(jì)， 激光里程計(jì)的輸出與地圖進(jìn)行匹配， 包括直線匹配和平面匹配， 無回環(huán)檢測模塊，點(diǎn)面特征還不夠可靠。

小結(jié) ? ?

Gmapping可以實(shí)時(shí)構(gòu)建室內(nèi)地圖，在構(gòu)建小場景地圖所需的計(jì)算量較小且精度較高。相比Hector SLAM對激光雷達(dá)頻率要求低、魯棒性高（Hector 在機(jī)器人快速轉(zhuǎn)向時(shí)很容易發(fā)生錯(cuò)誤匹配，建出的地圖發(fā)生錯(cuò)位，原因主要是優(yōu)化算法容易陷入局部最小值）；而相比Cartographer在構(gòu)建小場景地圖時(shí)，Gmapping不需要太多的粒子并且沒有回環(huán)檢測因此計(jì)算量小于Cartographer而精度并沒有差太多。

Gmapping有效利用了車輪里程計(jì)信息，這也是Gmapping對激光雷達(dá)頻率要求低的原因：里程計(jì)可以提供機(jī)器人的位姿先驗(yàn)。而Hector和Cartographer的設(shè)計(jì)初衷不是為了解決平面移動(dòng)機(jī)器人定位和建圖，Hector主要用于救災(zāi)等地面不平坦的情況，因此無法使用里程計(jì)。而Cartographer是用于手持激光雷達(dá)完成SLAM過程，也就沒有里程計(jì)可以用。

隨著場景增大所需的粒子增加，因?yàn)槊總€(gè)粒子都攜帶一幅地圖，因此在構(gòu)建大地圖時(shí)所需內(nèi)存和計(jì)算量都會(huì)增加。因此不適合構(gòu)建大場景地圖。并且沒有回環(huán)檢測，因此在回環(huán)閉合時(shí)可能會(huì)造成地圖錯(cuò)位，雖然增加粒子數(shù)目可以使地圖閉合但是以增加計(jì)算量和內(nèi)存為代價(jià)。所以不能像Cartographer那樣構(gòu)建大的地圖，雖然論文生成幾萬平米的地圖，但實(shí)際我們使用中建的地圖沒有幾千平米時(shí)就會(huì)發(fā)生錯(cuò)誤。

Gmapping和Cartographer一個(gè)是基于濾波框架SLAM另一個(gè)是基于優(yōu)化框架的SLAM，兩種算法都涉及到時(shí)間復(fù)雜度和空間復(fù)雜度的權(quán)衡。Gmapping犧牲空間復(fù)雜度保證時(shí)間復(fù)雜度，這就造成Gmapping不適合構(gòu)建大場景地圖，試想一下你要構(gòu)建200乘200米的環(huán)境地圖，柵格分辨率選擇5厘米，每個(gè)柵格占用一字節(jié)內(nèi)存，那么一個(gè)粒子攜帶的地圖就需要16M內(nèi)存，如果是100個(gè)粒子就需要1.6G內(nèi)存。

如果地圖變成500乘500米，粒子數(shù)為200個(gè)，可能電腦就要崩潰了。翻看Cartographer算法，優(yōu)化相當(dāng)于地圖中只用一個(gè)粒子，因此存儲(chǔ)空間比較Gmapping會(huì)小很多倍，但計(jì)算量大，一般的筆記本很難跑出來好的地圖，甚至根本就跑不動(dòng)。優(yōu)化圖需要復(fù)雜的矩陣運(yùn)算，這也是谷歌為什么還有弄個(gè)ceres庫出來的原因。

審核編輯：黃飛

?