基于2D激光雷達(dá)的SLAM算法研究綜述

沈斯杰，田 昕，魏國亮，袁千賀

(1.上海理工大學(xué) 光電信息與計(jì)算機(jī)工程學(xué)院，上海 200093；2.上海理工大學(xué) 理學(xué)院，上海 200093)

0 引 言

隨著人口老齡化日益嚴(yán)重和用人成本的上升，移動(dòng)機(jī)器人在生活中出現(xiàn)的頻率越來越高，如高端酒店中的迎賓機(jī)器人、疫情期間醫(yī)院中的消毒機(jī)器人和搶險(xiǎn)救災(zāi)的救援機(jī)器人等。在移動(dòng)機(jī)器人眾多應(yīng)用場(chǎng)景中，周圍環(huán)境不是一成不變的，有時(shí)候甚至是未知的。作為移動(dòng)機(jī)器人在未知環(huán)境下實(shí)現(xiàn)自主導(dǎo)航的關(guān)鍵技術(shù)，SLAM因此顯得尤為重要。

SLAM可以分為兩大類：基于相機(jī)的視覺SLAM和基于激光雷達(dá)的SLAM。視覺SLAM對(duì)光的依賴度高，在暗處或無紋理區(qū)域運(yùn)行效果較差，而激光SLAM可以在較差的光照條件下運(yùn)行，并且配合其他傳感器可以直接生成用于機(jī)器人導(dǎo)航的環(huán)境地圖。

激光SLAM按適用空間可以分為2D激光SLAM和3D激光SLAM。2D激光SLAM是在二維平面內(nèi)使用的SLAM方法，通過激光傳感器獲取機(jī)器人三個(gè)自由度的位姿信息及機(jī)器人所處的二維地圖。同理，3D激光SLAM是在三維空間中使用的SLAM方法，它可以得到六個(gè)自由度的位姿信息及三維地圖。由于2D激光雷達(dá)在成本方面要低于3D激光雷達(dá)，并且在室內(nèi)環(huán)境下，2D激光SLAM的建圖精度滿足使用需求，所以在移動(dòng)機(jī)器人領(lǐng)域得到了廣泛的應(yīng)用。因此，該文重點(diǎn)闡述了激光雷達(dá)的測(cè)距原理和2D激光SLAM的系統(tǒng)框架，并對(duì)主流的2D激光SLAM算法進(jìn)行分析和比較。最后展望未來激光SLAM的發(fā)展方向。

1 激光雷達(dá)的測(cè)距方法

激光雷達(dá)作為移動(dòng)機(jī)器人感知周圍環(huán)境信息的主要工具之一，在機(jī)器人避障中起著十分重要的作用。目前，激光雷達(dá)主流的測(cè)距方法可以分為三角法與飛行時(shí)間法(time of flight)。

1.1 三角法

激光以一定的角度射向被測(cè)物體，反射光以一定的角度反射回光源旁的接收器，因?yàn)榧す馄骱徒邮掌鏖g隔一段距離，所以不同距離的被測(cè)物體在接收器上的成像會(huì)在不同的位置。通過三角公式就可以計(jì)算出被測(cè)物體的距離，三角測(cè)距原理如圖1所示。

圖1 三角測(cè)距原理

1.2 飛行時(shí)間法

激光器發(fā)射一個(gè)激光脈沖，通過激光雷達(dá)中的計(jì)時(shí)器記錄下發(fā)射的時(shí)間，反射光由接收器接收，通過計(jì)時(shí)器得到接收的時(shí)間。飛行時(shí)間通過接收時(shí)間與發(fā)射時(shí)間相減得到。由于光速已知，因此就可以計(jì)算出被測(cè)物體的距離，飛行時(shí)間測(cè)距原理如圖2所示。

圖2 飛行時(shí)間測(cè)距原理

從測(cè)量距離、測(cè)量精度、抗強(qiáng)光能力和成本四個(gè)方面對(duì)三角法與飛行時(shí)間法進(jìn)行比較，如表1所示。

表1 三角法與飛行時(shí)間法比較

2 2D激光SLAM的系統(tǒng)框架

2D激光SLAM的系統(tǒng)框架主要可以分為：前端掃描匹配、后端優(yōu)化、回環(huán)檢測(cè)和地圖構(gòu)建。其中，回環(huán)檢測(cè)不是必須存在的環(huán)節(jié)。前端掃描匹配是通過讀取相鄰兩幀激光傳感器掃描信息之間的關(guān)系來估計(jì)自身的位姿。后端優(yōu)化是用來減少掃描匹配后產(chǎn)生的累積誤差，并利用地圖構(gòu)建模塊生成環(huán)境地圖信息?；丨h(huán)檢測(cè)則是通過檢測(cè)當(dāng)前位置的估計(jì)值和歷史位置的估計(jì)值是否相同，來消除累積誤差，從而減少地圖漂移的現(xiàn)象。

2.1 前端掃描匹配

前端掃描匹配是實(shí)現(xiàn)同時(shí)定位與建圖的基礎(chǔ)。它可以分為：基于點(diǎn)的掃描匹配、基于特征的掃描匹配、基于數(shù)學(xué)特性的掃描匹配、基于相關(guān)性的掃描匹配和基于優(yōu)化方法的掃描匹配。

2.1.1 基于點(diǎn)的掃描匹配

基于點(diǎn)的掃描匹配可以認(rèn)為是對(duì)前后兩幀激光點(diǎn)集進(jìn)行匹配。其中，迭代最近點(diǎn)(ICP)的變種算法——PLICP(point-to-line ICP)，由于其原理簡(jiǎn)單、易于實(shí)現(xiàn)被廣泛使用。PLICP與經(jīng)典ICP相比，通過將點(diǎn)與點(diǎn)之間的距離改變成點(diǎn)到線之間的距離，從而加快求取閉合解的速度以及提高匹配精度。

2.1.2 基于特征的掃描匹配

基于特征的掃描匹配方法，其思想就是從激光掃描信息中提取點(diǎn)、線、面等元素進(jìn)行匹配。Zhao等人從掃描數(shù)據(jù)中提取隱函數(shù)作為特征，在具有閉合圖形的場(chǎng)合下，具有優(yōu)異的效果。針對(duì)傳統(tǒng)的平面掃描匹配方法缺乏足夠的邊緣和角點(diǎn)特征，Zhao等人提出基于二次曲線特征的匹配方法。

2.1.3 基于數(shù)學(xué)特性的掃描匹配

此類算法的核心是通過各種數(shù)學(xué)特性來描述幀與幀之間的位姿變化。正態(tài)分布變換(NDT)是基于數(shù)學(xué)特性的匹配算法中最為典型的。該算法將當(dāng)前幀掃描到的2D平面離散為柵格，根據(jù)每個(gè)柵格上點(diǎn)的分布，計(jì)算出該柵格的概率密度函數(shù)。通過牛頓的優(yōu)化方法求解出概率密度之和最大的變換參數(shù)，從而達(dá)到最優(yōu)匹配結(jié)果。

2.1.4 基于相關(guān)性的掃描匹配

相關(guān)性掃描匹配由Olson提出，首先構(gòu)造似然場(chǎng)模型，然后在指定的搜索空間內(nèi)進(jìn)行搜索，計(jì)算每個(gè)位姿的得分，最后根據(jù)位姿得分，計(jì)算本輪位姿匹配的方差。由于該方法使用暴力搜索，因此需要較長(zhǎng)的匹配時(shí)間。不過，該方法可以結(jié)合分枝定界策略加速匹配。

2.1.5 基于優(yōu)化方法的掃描匹配

將掃描匹配問題轉(zhuǎn)換為非線性最小二乘問題，通過梯度下降的方式來求解，因此需要選擇合適的初始位置，否則容易陷入局部極值。

目前，在2D激光SLAM中，相關(guān)性掃描匹配結(jié)合梯度優(yōu)化是最主流的前端掃描匹配方法。

2.2 后端優(yōu)化

SLAM中的后端優(yōu)化環(huán)節(jié)主要是減少掃描匹配所產(chǎn)生的累積誤差。目前，可以分為兩大類：基于貝葉斯濾波的后端優(yōu)化方法和基于圖優(yōu)化的后端優(yōu)化方法。

2.2.1 基于貝葉斯濾波器的后端優(yōu)化

基于貝葉斯濾波的SLAM方法都是在馬爾可夫假設(shè)的前提下進(jìn)行的，即當(dāng)前時(shí)刻機(jī)器人的狀態(tài)只與當(dāng)前時(shí)刻的測(cè)量值和上一時(shí)刻的狀態(tài)相關(guān)，與其他時(shí)刻的無關(guān)。根據(jù)后驗(yàn)概率表示方法的不同，可以分為卡爾曼濾波和粒子濾波。濾波類方法最大的缺點(diǎn)就是無法建立大尺度地圖，因?yàn)橐坏┊?dāng)前時(shí)刻的估計(jì)出現(xiàn)偏差，偏差就無法消除，從而導(dǎo)致所建立的地圖有錯(cuò)位的情況。

2.2.2 基于圖優(yōu)化的后端優(yōu)化

基于圖優(yōu)化的SLAM方法則沒有馬爾可夫假設(shè)，當(dāng)前時(shí)刻機(jī)器人的狀態(tài)與之前所有的測(cè)量值都相關(guān)，圖優(yōu)化的系統(tǒng)框架如圖3所示。所謂圖優(yōu)化，就是將圖論的思想引入到SLAM的問題中。它將機(jī)器人的位姿表示成一個(gè)節(jié)點(diǎn)，相鄰節(jié)點(diǎn)之間的弧表示空間約束關(guān)系，以此構(gòu)成的圖就是位姿圖。通過調(diào)整位姿圖中的節(jié)點(diǎn)以最大程度滿足空間約束關(guān)系，從而獲得機(jī)器人的位姿信息和地圖。

圖3 圖優(yōu)化的系統(tǒng)框架

2.3 回環(huán)檢測(cè)

回環(huán)檢測(cè)是指機(jī)器人通過傳感器測(cè)量的數(shù)據(jù)檢測(cè)到自己又回到之前來過的位置。這里可以借助控制科學(xué)中經(jīng)典的反饋控制理論，來理解閉環(huán)檢測(cè)在SLAM中的作用，消除累積誤差，從而改善建圖效果。

2.3.1 幀與幀(scan-to-scan)的回環(huán)檢測(cè)

Scan-to-scan是利用Olson所提出的相關(guān)性掃描匹配，將兩幀激光雷達(dá)數(shù)據(jù)通過相對(duì)平移和旋轉(zhuǎn)角度進(jìn)行匹配，從而達(dá)到回環(huán)檢測(cè)的目的。然而，因?yàn)閱螏す鈹?shù)據(jù)包含的信息量太少，容易出現(xiàn)匹配失敗的情況。

2.3.2 幀與子圖(scan-to-map)的回環(huán)檢測(cè)

將當(dāng)前幀的激光雷達(dá)數(shù)據(jù)與子圖進(jìn)行匹配，該子圖由一段時(shí)間內(nèi)連續(xù)的激光數(shù)據(jù)幀構(gòu)成。其中最具代表性的是于2016年google提出的cartographer算法，通過scan-to-map的回環(huán)檢測(cè)方法，減少連續(xù)多幀激光雷達(dá)數(shù)據(jù)之間的冗余性，提高回環(huán)的效率。

2.3.3 子圖與子圖(map-to-map)的回環(huán)檢測(cè)

Map-to-map就是將當(dāng)前連續(xù)時(shí)間內(nèi)的激光數(shù)據(jù)幀構(gòu)建成子圖，與之前生成的子圖進(jìn)行匹配，從而改善單幀激光數(shù)據(jù)所含信息量少的問題。文國成所提出的map-to-map的回環(huán)檢測(cè)方法，使用定位篩選和壓縮表對(duì)回環(huán)候選集進(jìn)行優(yōu)化，在構(gòu)建大尺度地圖時(shí)，可以加快回環(huán)速度，降低回環(huán)出錯(cuò)的可能性。

2.3.4 特殊的回環(huán)檢測(cè)

Olson提出一種基于多分辨率掃描匹配的方法，將當(dāng)前激光數(shù)據(jù)幀和多分辨率地圖進(jìn)行匹配。該方法即使在位置不確定的場(chǎng)合下，也能通過枚舉法找到最佳匹配。Yin等人提出一種基于孿生神經(jīng)網(wǎng)絡(luò)的激光雷達(dá)點(diǎn)云半手工表示學(xué)習(xí)方法，在此基礎(chǔ)上，進(jìn)一步建立KD樹，加速回環(huán)檢測(cè)。

目前，在2D激光SLAM 領(lǐng)域中，使用最多的回環(huán)檢測(cè)方法是幀與子圖之間的匹配，利用分枝定界法加速回環(huán)并且通過延時(shí)決策(lazy decision)降低回環(huán)出錯(cuò)的可能性。

2.4 地圖構(gòu)建

SLAM所構(gòu)建的地圖是移動(dòng)機(jī)器人實(shí)現(xiàn)自主導(dǎo)航與定位的前提與基礎(chǔ)。地圖可以分為：尺度地圖、拓?fù)涞貓D和語義地圖。在2D激光SLAM中，主要針對(duì)尺度地圖中的柵格地圖和特征地圖進(jìn)行研究。

2.4.1 柵格地圖

柵格地圖是尺度地圖中最為常用的一種。柵格地圖是指將環(huán)境空間劃分成一個(gè)個(gè)大小相等的柵格單元，每個(gè)柵格單元中含有被障礙物占據(jù)的概率值。如果柵格被占用，則概率值越接近于1。當(dāng)柵格中不含物體時(shí)，則概率值越接近于0。若不確定柵格中是否有物體，則概率值等于0.5。柵格地圖具有高準(zhǔn)確性并且能夠充分反映出環(huán)境的結(jié)構(gòu)特征，因此柵格地圖可以直接用于移動(dòng)機(jī)器人的自主導(dǎo)航與定位。

2.4.2 特征地圖

特征地圖又稱為幾何地圖，一般由環(huán)境信息中提取到的點(diǎn)、線或圓弧等幾何特征構(gòu)成。當(dāng)構(gòu)建小場(chǎng)景地圖時(shí)，特征地圖占用的資源較少且建圖精度尚可；當(dāng)構(gòu)建環(huán)境較大的地圖時(shí)，由于特征地圖無法有效表現(xiàn)出真實(shí)環(huán)境中的詳細(xì)信息，其建圖精度會(huì)大打折扣，因此不適用于大場(chǎng)景建圖。

3 主流的2D激光SLAM算法

主流的2D激光SLAM可以分為：基于貝葉斯濾波、基于圖優(yōu)化和基于高斯牛頓優(yōu)化的激光SLAM。

3.1 基于貝葉斯濾波的激光SLAM

擴(kuò)展卡爾曼濾波(EKF)被用來解決SLAM問題最早可追溯到1987年，通過聯(lián)合狀態(tài)向量來同時(shí)表示機(jī)器人的位姿和路標(biāo)所處的位置。雖然EKF SLAM開創(chuàng)了將概率統(tǒng)計(jì)運(yùn)用于SLAM上的先河，但是此方法存在著一些問題，如所占用的計(jì)算資源較大、算法穩(wěn)定性較差和所建立的特征地圖無法直接用于機(jī)器人的自主導(dǎo)航等。

基于粒子濾波器的激光SLAM方法對(duì)上述問題進(jìn)行改善。2003年，Montemerlo等人提出Fast SLAM1.0算法，將SLAM問題分解成機(jī)器人位姿的估計(jì)和位置路標(biāo)的估計(jì)，分別使用粒子濾波器和卡爾曼濾波器求解。但是粒子濾波器存在粒子退化的問題，為此Fast SLAM2.0算法提出一種求取重要性函數(shù)的方法。為了繼續(xù)優(yōu)化Fast SLAM，Gmapping于2007年被提出，它通過改進(jìn)提議分布和選擇性重采樣，進(jìn)一步改善粒子退化的情況，建圖效果如圖4所示。

圖4 Gmapping算法

由于每個(gè)粒子都攜帶機(jī)器人的軌跡和對(duì)應(yīng)的環(huán)境地圖，所以占用的計(jì)算資源較大。2010年提出的Core SLAM，是一種只有200行代碼的輕量化SLAM方法，因此性能損耗較小。

不管是基于卡爾曼濾波器還是粒子濾波器的SLAM算法，由于缺少回環(huán)檢測(cè)，因此在構(gòu)建大尺度地圖時(shí)無法取得理想的效果。

3.2 基于圖優(yōu)化的激光SLAM

圖優(yōu)化的思想在SLAM中的使用最早可追溯到1997年，其通過最小二乘法來解決前端匹配所產(chǎn)生的累積誤差。由于沒有考慮到矩陣稀疏性，當(dāng)時(shí)采用矩陣求逆的方法來求解，只能離線使用，無法滿足實(shí)時(shí)構(gòu)建地圖的需求。1999年，Gutmann等人提出與當(dāng)前相似的圖優(yōu)化框架，但仍忽視矩陣稀疏性。千禧年以后，隨著研究者們的深入探索，SLAM中的矩陣稀疏性得以發(fā)現(xiàn)，使得高效求解成為可能。正是站在巨人的肩膀上，在2010年，Konolige等人提出Karto SLAM算法。這是首個(gè)基于圖優(yōu)化的開源算法，利用高度優(yōu)化和非迭代平方根法分解從而進(jìn)行稀疏化解耦求解，建圖效果如圖5所示。但是該算法依然有不足之處，在回環(huán)檢測(cè)部分，需要預(yù)先構(gòu)建局部子圖。

圖5 Karto算法

一般的圖優(yōu)化方法都需要初始假設(shè)，即起始點(diǎn)經(jīng)過多次迭代后得到局部代價(jià)函數(shù)的最小值。然而在2011年，Carlone等人提出一種近似線性化的圖優(yōu)化SLAM方法。2016年，Google團(tuán)隊(duì)開源了Cartographer算法，通過相關(guān)性掃描匹配結(jié)合梯度優(yōu)化完成前端匹配，利用分枝定界法加速回環(huán)檢測(cè)以及結(jié)合延時(shí)決策(lazy decision)降低回環(huán)出錯(cuò)的可能性，建圖效果如圖6所示。2020年，Li等人利用高斯過程(Gaussian process，GP)回歸，提出了一種基于區(qū)域化GP地圖重建算法的新型地圖表示方法。該方法可以采用簡(jiǎn)潔的數(shù)學(xué)方法完成位姿估計(jì)和地圖更新。

圖6 Cartographer算法

3.3 基于高斯牛頓優(yōu)化的激光SLAM

2011年，Kohlbrecher等人提出Hector SLAM算法，該算法無后端優(yōu)化部分且不需要借助額外的傳感器，所以需要高更新頻率且測(cè)量噪聲小的激光雷達(dá)。Hector SLAM將當(dāng)前幀的激光雷達(dá)數(shù)據(jù)與子圖進(jìn)行匹配，通過高斯牛頓法進(jìn)行位姿優(yōu)化。為了避免陷入局部最優(yōu)解，求得全局最優(yōu)解，Hector SLAM采取多分辨率地圖的策略。此算法在旋轉(zhuǎn)過快時(shí)，建圖易發(fā)生漂移，建圖效果如圖7所示。

圖7 Hector算法

目前，主流的激光SLAM算法的優(yōu)缺點(diǎn)分析如表2所示。

表2 激光SLAM算法的優(yōu)缺點(diǎn)分析

4 激光SLAM的發(fā)展趨勢(shì)

激光SLAM的系統(tǒng)框架已經(jīng)成熟，但是仍有很多工程應(yīng)用問題有待解決，目前的發(fā)展趨勢(shì)有以下三種：

(1)多傳感器融合。

在復(fù)雜環(huán)境下，多傳感器融合是一種必然的趨勢(shì)。李陸君等人提出將二維激光雷達(dá)與深度相機(jī)融合，從而提高傳感器的抗干擾能力、探測(cè)范圍和建圖精度。陳志鍵等人針對(duì)室內(nèi)定位中UWB存在非視距的影響以及激光雷達(dá)有誤差累積問題，將UWB絕對(duì)定位技術(shù)和激光雷達(dá)相對(duì)定位技術(shù)相融合，提高定位準(zhǔn)確度。

(2)與深度學(xué)習(xí)的結(jié)合。

近些年，深度學(xué)習(xí)得到蓬勃的發(fā)展和廣泛的關(guān)注度。深度學(xué)習(xí)在特征提取和特征學(xué)習(xí)方面性能突出，不少研究者在SLAM中引入深度學(xué)習(xí)改善前端匹配與回環(huán)檢測(cè)的性能。Wang等人將深度學(xué)習(xí)用于激光里程計(jì)系統(tǒng)，提高預(yù)測(cè)位姿的魯棒性與準(zhǔn)確度。鄒勤提出一種基于深度學(xué)習(xí)的激光SLAM回環(huán)檢測(cè)方法，將SLAM回環(huán)檢測(cè)的問題轉(zhuǎn)化為SLAM數(shù)據(jù)樣本的檢索問題。

(3)優(yōu)化激光SLAM算法。

為了推動(dòng)移動(dòng)機(jī)器人在日常生活中的普及，需要進(jìn)一步提升SLAM算法的穩(wěn)定性和實(shí)時(shí)性，降低硬件成本。劉哲銘通過速度估計(jì)補(bǔ)償法和里程計(jì)插值模型去除激光雷達(dá)中的運(yùn)動(dòng)畸變，提升前端掃描匹配的精確度。Zhang等人提出一種新穎的序列數(shù)據(jù)處理流程，該方法有效減少了建圖漂移現(xiàn)象，提升了系統(tǒng)的魯棒性。Pang等人通過對(duì)激光掃描數(shù)據(jù)進(jìn)行分割，提取出特殊的邊緣或角點(diǎn)特征，使其在低成本的設(shè)備下可以保持較高的建圖精度。

5 結(jié)束語

文中主要對(duì)2D激光SLAM的系統(tǒng)框架和發(fā)展現(xiàn)狀進(jìn)行了概述，總結(jié)分析了主流2D激光SLAM算法，不難發(fā)現(xiàn)，基于圖優(yōu)化的激光SLAM將是未來的主流。若要進(jìn)一步提高SLAM算法的實(shí)時(shí)性、魯棒性和準(zhǔn)確性，結(jié)合多傳感器融合與深度學(xué)習(xí)技術(shù)符合當(dāng)下的發(fā)展趨勢(shì)。

移動(dòng)機(jī)器人是人工智能的重要載體，而激光SLAM是機(jī)器人學(xué)中的關(guān)鍵技術(shù)，是實(shí)現(xiàn)機(jī)器人自主探索的基石?，F(xiàn)如今激光SLAM技術(shù)的建圖精度和硬件成本仍呈正比關(guān)系，相信在未來，隨著激光SLAM技術(shù)的深入發(fā)展和硬件成本的降低，會(huì)出現(xiàn)低成本高精度的激光SLAM技術(shù)。到那時(shí)，越來越多的移動(dòng)機(jī)器人會(huì)出現(xiàn)在人們的視野中，便利你我的生活。