巴浩麟，馬曉錄，吳立輝

（河南工業(yè)大學(xué) 機(jī)電工程學(xué)院，河南 鄭州 450001）

安全行駛是智能車(chē)輛最重要的性能要求之一。為保證智能車(chē)輛的安全行駛，定位精度及其可靠性已成為智能車(chē)輛設(shè)計(jì)中的關(guān)鍵。傳統(tǒng)的智能車(chē)輛定位技術(shù)主要基于衛(wèi)星定位，其定位精度僅為 5～10 m[1]。通過(guò)利用差分技術(shù)的實(shí)時(shí)動(dòng)態(tài)（Real Time Kinematic, RTK）雖然將定位精度提升至厘米級(jí)，但在具有峽谷特征的環(huán)境下依舊無(wú)法獲取準(zhǔn)確定位，不能滿足智能車(chē)輛的需求。同步定位與建圖（Simultaneous Localization And Mapping, SLAM）[2]在建立地圖的同時(shí)通過(guò)匹配對(duì)位姿進(jìn)行估計(jì)，在過(guò)去的幾年里，智能車(chē)輛采用視覺(jué)傳感器的SLAM技術(shù)進(jìn)行定位取得了重大進(jìn)展，通過(guò)運(yùn)用快速特征點(diǎn)提取和描述的算法（Oriented Fast and Rotated Brief, ORB）對(duì)圖像中的特征點(diǎn)進(jìn)行提取，再對(duì)特征點(diǎn)進(jìn)行匹配定位，但由于其光學(xué)性的特點(diǎn)，在缺乏顯著特征的環(huán)境下，其提取特征能力差，且在惡劣的天氣環(huán)境下無(wú)法正常工作，很難提供可靠的定位。

美國(guó)國(guó)防部高級(jí)研究計(jì)劃局地面挑戰(zhàn)賽[3]推動(dòng)將多線激光雷達(dá)用于智能汽車(chē)。于是用于智能汽車(chē)的三維激光SLAM應(yīng)運(yùn)而生。隨后基于三維激光雷達(dá)的智能車(chē)輛定位技術(shù)大力發(fā)展。例如：谷歌公司的智能車(chē)輛采用激光雷達(dá)定位方案，百度Apollo公司采用激光雷達(dá)與視覺(jué)傳感器共同定位的方案等。但當(dāng)前傳統(tǒng)基于概率特征的匹配定位方式很容易陷入局部迭代中，無(wú)法獲得最優(yōu)的全局定位。隨著三維激光雷達(dá)的量產(chǎn)和處理器算力的提升，可以相信三維激光SLAM技術(shù)在未來(lái)的智能車(chē)輛制造和使用中將擔(dān)負(fù)重要角色。故本文著重對(duì)三維激光SLAM中的關(guān)鍵模塊、部分開(kāi)源算法以及應(yīng)用過(guò)程中的一些關(guān)鍵問(wèn)題進(jìn)行分析總結(jié)。

1 三維激光SLAM算法框架及關(guān)鍵模塊

基于濾波器的SLAM方案利用從P(xi)中提取一組隨機(jī)狀態(tài)樣本。

其表達(dá)式為

式中，x1:i為從1時(shí)刻到i時(shí)刻的姿態(tài)；u1:i為時(shí)刻i-1為機(jī)器人在時(shí)刻i達(dá)到位姿xi的控制信號(hào)；z1:i為在未知環(huán)境中行駛會(huì)觀測(cè)到的許多觀測(cè)值，在i時(shí)刻觀測(cè)到的觀測(cè)值；為一個(gè)標(biāo)準(zhǔn)化常數(shù)，與位姿無(wú)關(guān)。

在式（1）中第二行和第三行分別使用了貝葉斯全概率規(guī)則和馬爾可夫假設(shè)。假設(shè)P(xi)為在i時(shí)刻姿態(tài)的先驗(yàn)數(shù)據(jù)，則可表達(dá)為

此式也依賴于貝葉斯全概率規(guī)則和馬爾可夫假設(shè)。貝葉斯分為兩個(gè)部分：

式（3）表示預(yù)測(cè)步驟，式（4）表示更新步驟，在預(yù)測(cè)步驟中，為給定i-1時(shí)刻的位姿xi-1和i時(shí)刻控制信號(hào)ui，估計(jì)i時(shí)刻的位姿xi。在更新步驟中，對(duì)前一步的估計(jì)進(jìn)行誤差修正，使機(jī)器人能預(yù)測(cè)更加準(zhǔn)確的位置和姿態(tài)信息。

基于圖優(yōu)化的SLAM方案是三維激光SLAM主要采取的方案，如圖1所示。

圖1 三維激光SLAM框架

優(yōu)化的原理如圖2所示。其中三角形代表智能車(chē)輛的位姿，六角形代表智能車(chē)輛觀測(cè)到的觀測(cè)值，位姿與觀測(cè)值作為圖中的節(jié)點(diǎn)，實(shí)線為邊對(duì)SLAM系統(tǒng)起到約束的作用。

圖2 圖優(yōu)化原理

三維激光SLAM通常由三維激光雷達(dá)為主傳感器、搭配輪式里程計(jì)和慣性測(cè)量單元（InertialMeasurement Unit, IMU）等其余車(chē)載傳感器進(jìn)行同步定位與建圖。基于圖優(yōu)化的三維激光 SLAM框架分為掃描匹配（即激光里程計(jì)）、閉環(huán)檢測(cè)、后端優(yōu)化、地圖構(gòu)建四個(gè)部分。掃描匹配計(jì)算激光雷達(dá)等傳感器相鄰幀之間的數(shù)據(jù)關(guān)系，估計(jì)當(dāng)前時(shí)刻智能車(chē)輛的位姿。掃描匹配是一個(gè)隨時(shí)間增長(zhǎng)的數(shù)據(jù)問(wèn)題，存在誤差累積的問(wèn)題。后端優(yōu)化對(duì)掃描匹配進(jìn)行修正優(yōu)化，減少誤差的產(chǎn)生。閉環(huán)檢測(cè)是對(duì)全局的數(shù)據(jù)處理，通過(guò)對(duì)當(dāng)前時(shí)刻與過(guò)去時(shí)刻的對(duì)比，識(shí)別是否到達(dá)過(guò)當(dāng)前時(shí)刻環(huán)境，在校準(zhǔn)累積誤差的同時(shí)構(gòu)建閉環(huán)地圖。地圖構(gòu)建生成與環(huán)境一致的全局地圖。

1.1 掃描匹配

目前三維激光SLAM的掃描匹配方法主要分為基于距離、基于概率和基于特征的方法。

1）基于距離的掃描匹配方法，即通過(guò)求解兩臨近時(shí)刻點(diǎn)云的歐式距離極小值，求解點(diǎn)云的相對(duì)轉(zhuǎn)換關(guān)系，如 CHEN等人[4]提出的迭代最近點(diǎn)算法（Iterative Closest Point, ICP）。但I(xiàn)CP假設(shè)點(diǎn)云在迭代過(guò)程中數(shù)量及對(duì)應(yīng)關(guān)系不變，當(dāng)傳感器采樣頻率較低時(shí)，這種假設(shè)是不正確的。CENSI[5]在ICP的基礎(chǔ)上提出Point to Line ICP（PL-ICP），修改誤差函數(shù)為點(diǎn)到線的距離使得收斂速度更快，精度更高，但對(duì)初值更加敏感。LOW[6]提出的Point to Plane ICP（PP-ICP）構(gòu)建頂點(diǎn)到目標(biāo)頂點(diǎn)平面的距離為優(yōu)化函數(shù)，通過(guò)非線性優(yōu)化的方式進(jìn)行優(yōu)化求解，提高了收斂速度。

Generalized ICP（G-ICP）[7]假設(shè)源點(diǎn)云與目標(biāo)點(diǎn)云中的每個(gè)點(diǎn)與其相鄰的點(diǎn)組成的集合滿足高斯分布，同時(shí)假定每個(gè)點(diǎn)所處表面均可以擬合為平面，通過(guò)法向給目標(biāo)函數(shù)進(jìn)行匹配賦值，降低了錯(cuò)誤匹配的概率，提高了匹配精度和匹配速率。

Normal ICP（N-ICP）[8]除了利用對(duì)應(yīng)點(diǎn)的距離作為約束，還通過(guò)計(jì)算該點(diǎn)所在面的法向量和曲率作為約束關(guān)系，在很大程度上去除了錯(cuò)誤的匹配關(guān)系，在減少計(jì)算量的同時(shí)提高了精確度。Implicit Moving Least Square ICP（IMLS-ICP）[9]使用高斯擬合和最小二乘重建出隱含曲面，通過(guò)點(diǎn)到對(duì)應(yīng)點(diǎn)在該曲面的投影點(diǎn)距離構(gòu)建誤差函數(shù)，提高了精確度。

傳統(tǒng)的 ICP及改進(jìn)算法假設(shè)一幀點(diǎn)云中的所有點(diǎn)是在同一時(shí)刻獲取的，但實(shí)際點(diǎn)云是從起始角度到結(jié)束角度依次獲得的，當(dāng)傳感器頻率較低或運(yùn)動(dòng)速度較快時(shí)，傳感器的位姿不是一成不變的。所以，傳統(tǒng)的 ICP及改進(jìn)算法會(huì)估計(jì)到不正確的位姿。Velocity ICP（VICP）[10]提出在進(jìn)行對(duì)應(yīng)點(diǎn)集合間的關(guān)系計(jì)算之前對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行畸變?nèi)コㄟ^(guò)在 ICP迭代求解的同時(shí)估計(jì)傳感器運(yùn)動(dòng)速度，通過(guò)該速度補(bǔ)償點(diǎn)云的畸變，該方法有效去除了異常點(diǎn)，提供了更準(zhǔn)確的位姿估計(jì)。

隨著ICP及其改進(jìn)算法的不斷發(fā)展，可將ICP算法流程大致歸納，如圖3所示。

圖3 ICP大致流程圖

2）基于概率的掃描匹配方法主要是正態(tài)分布變換（Normal Distribution Transformation, NDT）。該方法對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行體素化處理，建立點(diǎn)云的高斯分布模型，不建立點(diǎn)與點(diǎn)之間的數(shù)學(xué)關(guān)系，極大了縮減了因錯(cuò)誤對(duì)應(yīng)關(guān)系而對(duì)位姿估計(jì)產(chǎn)生的誤差。

MAGNUSSON等[11]將其應(yīng)用于三維掃描匹配，該方法首先把三維點(diǎn)云數(shù)據(jù)劃分為大小均勻的立方體，把每個(gè)立方體內(nèi)的點(diǎn)云數(shù)據(jù)轉(zhuǎn)換為概率密度函數(shù)，通過(guò)矩陣法求解出轉(zhuǎn)換關(guān)系。其流程如圖4所示。由于NDT配準(zhǔn)算法不需要兩組點(diǎn)云完全相同且計(jì)算速率高，被廣泛應(yīng)用于三維激光SLAM中。

圖4 NDT大致流程圖

NDT算法相比ICP類算法有更高的效率，但也存在因初值不佳而陷入局部極值，無(wú)法獲得最優(yōu)解的情況。針對(duì)此情況，通常在進(jìn)行NDT流程前增加粗配準(zhǔn)算法，對(duì)點(diǎn)云進(jìn)行初始粗配準(zhǔn)得到一個(gè)初值，以提高NDT配準(zhǔn)的精度及消除局部極值。

3）基于特征的方法根據(jù)點(diǎn)云的結(jié)構(gòu)，構(gòu)建不同特點(diǎn)的點(diǎn)云集合，例如平面、邊緣等，通過(guò)對(duì)各點(diǎn)云集合的點(diǎn)云進(jìn)行匹配，提高計(jì)算效率，如圖5所示。ZHANG等[12]通過(guò)基于曲率的方式提取邊緣線特征和平面特征用于配準(zhǔn)，實(shí)現(xiàn)了激光里程計(jì)，在多種環(huán)境中均取得出色的表現(xiàn)。劉今越等[13]提出從點(diǎn)云中提取面元，建立不確定性模型，使用隱式最小二乘求解幀間運(yùn)動(dòng)和全局位姿定位性能好，實(shí)時(shí)性好。

圖5 基于特征的掃描匹配方法流程圖

不同于直接對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行特征提取，SEHGAL 等人[14]提出將點(diǎn)云數(shù)據(jù)轉(zhuǎn)化為圖像數(shù)據(jù)，對(duì)圖像數(shù)據(jù)進(jìn)行特征提取，極大地增強(qiáng)了提取出特征的正確性，提高了匹配精度。

1.2 后端優(yōu)化

后端優(yōu)化的本質(zhì)是一個(gè)非線性優(yōu)化問(wèn)題，可以分為基于濾波器和基于圖優(yōu)化兩類。基于濾波器的SLAM方法主要應(yīng)用于工作場(chǎng)景較小的服務(wù)型機(jī)器人，對(duì)于智能車(chē)輛在室外環(huán)境下表現(xiàn)效果差。基于圖優(yōu)化的方法將智能車(chē)輛的位姿作為節(jié)點(diǎn)，位姿之間構(gòu)建約束即邊。圖優(yōu)化方法估計(jì)整個(gè)路徑和地圖上的所有位姿x0:i，而不是當(dāng)前的位姿xi。該方法通過(guò)一段時(shí)間內(nèi)的所有觀測(cè)值z(mì)i和控制信號(hào)ui，對(duì)所有的姿態(tài)進(jìn)行全局最優(yōu)估計(jì)，如圖6所示。該方法計(jì)算當(dāng)前位姿xi+1時(shí)，將考慮x0和xi之間所有可用的觀測(cè)值和控制信號(hào)。

圖6 基于圖優(yōu)化的SLAM方案

王忠立等[15]對(duì)基于圖優(yōu)化的后端優(yōu)化方法作了詳細(xì)介紹，基于圖優(yōu)化的后端優(yōu)化方法可以分為基于最小二乘法的優(yōu)化方法、基于松弛迭代的優(yōu)化方法、基于隨機(jī)梯度下降的優(yōu)化方法及基于流形迭代方法。

1）基于最小二乘法的優(yōu)化方法目標(biāo)是求解誤差的最小平方和，找一個(gè)（組）估計(jì)值，使得實(shí)際值與估計(jì)值之差的平方加權(quán)之后的值最小。SLAM 是一個(gè)非線性問(wèn)題，通過(guò)對(duì)目標(biāo)函數(shù)的泰勒展開(kāi)式線性化再利用高斯牛頓迭代法或萊文伯格-馬夸特（Levenberg-Marquardt, L-M）算法對(duì)其進(jìn)行求解。但在實(shí)際應(yīng)用時(shí)不能滿足實(shí)時(shí)性，于是 DELLAERT等[16]提出利用稀疏 Cholesky分解方法對(duì)SLAM問(wèn)題進(jìn)行求解，極大地增加了實(shí)時(shí)性。

2）基于松弛迭代的優(yōu)化方法將構(gòu)建的圖中每個(gè)節(jié)點(diǎn)與其相鄰節(jié)點(diǎn)間的關(guān)系進(jìn)行計(jì)算，每次迭代計(jì)算都遍歷全部節(jié)點(diǎn)，實(shí)驗(yàn)證明了該方法必定收斂于最優(yōu)解[17]。但當(dāng)構(gòu)建的圖中約束誤差量大時(shí)容易陷入多次迭代。

3）基于隨機(jī)梯度下降的優(yōu)化方法根據(jù)構(gòu)建的圖中隨機(jī)選取一個(gè)約束邊作為計(jì)算目標(biāo)函數(shù)的梯度下降方向，在每次迭代計(jì)算時(shí)都朝著正確方向行進(jìn)，在該方向上進(jìn)行目標(biāo)尋優(yōu)。該方法有較高的魯棒性且不易陷入局部極值。

4）基于流形迭代的方法不同于假定優(yōu)化過(guò)程在歐氏空間，GRISETTI等[18]提出一種在流形空間中進(jìn)行優(yōu)化的思想即我們觀察到的數(shù)據(jù)實(shí)際上是由一個(gè)低維流形映射到高維空間，由于數(shù)據(jù)內(nèi)部特征的限制，一些高維中的數(shù)據(jù)會(huì)產(chǎn)生維度上的冗余，實(shí)際上只需要比較低的維度就能唯一地表示。目前，General Graphic Optimization（g2o）是用于流形優(yōu)化的一個(gè)開(kāi)源工具。

1.3 閉環(huán)檢測(cè)

閉環(huán)檢測(cè)是實(shí)現(xiàn)SLAM魯棒性的關(guān)鍵問(wèn)題，通過(guò)判斷是否到達(dá)歷史環(huán)境使地圖閉環(huán)。BESL等[19]提出ICP算法，直接對(duì)當(dāng)前幀和歷史幀的數(shù)據(jù)逐個(gè)匹配，通過(guò)重合度判斷是否回環(huán)，該方法檢測(cè)精度高但實(shí)時(shí)性差。HESS等[20]提出子圖概念通過(guò)幀與子圖進(jìn)行匹配消除累積誤差。張劍華等[21]提出一種基于點(diǎn)云片段匹配約束的方法提升了回環(huán)效率但精度不足。KIM等[22]提出Scan Context算法通過(guò)行列向量分析特征矩陣提升了閉環(huán)檢測(cè)的精確度。李炯等[23]采用MF-RANSAC算法改進(jìn)Scan Context，利用區(qū)域生長(zhǎng)算法對(duì)扇形柵格化后的點(diǎn)云進(jìn)行分割，簡(jiǎn)化特征匹配計(jì)算，相比 Scan Context方法提升了60%的效率。

除了通過(guò)算法進(jìn)行閉環(huán)檢測(cè)，研究學(xué)家們提出通過(guò)其余傳感器對(duì)激光雷達(dá)的難閉環(huán)問(wèn)題進(jìn)行解決，將其余傳感器的信息數(shù)據(jù)通過(guò)圖因子的方式加入圖優(yōu)化中對(duì)其進(jìn)行約束和優(yōu)化達(dá)到閉環(huán)的目的。例如，通過(guò)高精度慣性傳感器、視覺(jué)傳感器等。本文主要說(shuō)明基于激光雷達(dá)的SLAM技術(shù)，對(duì)其余傳感器不做過(guò)多介紹。

1.4 地圖構(gòu)建

三維激光 SLAM 通過(guò)點(diǎn)云數(shù)據(jù)構(gòu)建點(diǎn)云地圖，如圖7所示，它可以更加詳細(xì)地描述環(huán)境，但因點(diǎn)云數(shù)據(jù)量過(guò)大及無(wú)法表示障礙物信息很難直接用于智能車(chē)輛的導(dǎo)航。根據(jù)不同的使用目的對(duì)點(diǎn)云地圖進(jìn)行處理，通過(guò)體素濾波和特征聚類可以將點(diǎn)云地圖處理為用于定位的特征地圖，也可以通過(guò)八叉樹(shù)[24]的方法構(gòu)建八叉樹(shù)柵格占據(jù)地圖用于導(dǎo)航。近些年，研究學(xué)者通過(guò)卷積神經(jīng)網(wǎng)絡(luò)構(gòu)建訓(xùn)練模型對(duì)點(diǎn)云地圖進(jìn)行分割，檢測(cè)出運(yùn)動(dòng)目標(biāo)實(shí)現(xiàn)了避障的功能。

圖7 點(diǎn)云地圖

2 三維激光SLAM算法

目前開(kāi)源的三維激光SLAM包括雷達(dá)定位與建圖（Lidar Odometry and Mapping, LOAM）、Lightweight and Groud-Optimized LOAM（LeGOLOAM）、Multi LOAM（M-LOAM）、Fast LOAM（F-LOAM）、Range-Monte Carlo Localization（Range-MCL）、Multi-Metric Linear Least Square（MULLS）等。

2.1 LOAM算法原理

LOAM 算法[12]提出了一種實(shí)時(shí)的 SLAM 方法，通過(guò)兩個(gè)算法將定位與建圖分成兩個(gè)問(wèn)題進(jìn)行求解。定位算法以高頻率得到一個(gè)精度較低的里程計(jì)，建圖算法以低頻率構(gòu)建精度較高的地圖數(shù)據(jù)，二者結(jié)合實(shí)現(xiàn)一個(gè)實(shí)時(shí)高精度的定位。

算法首先基于點(diǎn)的曲率進(jìn)行特征點(diǎn)的提取，再對(duì)所提取的點(diǎn)進(jìn)行分類，分為邊緣點(diǎn)與平面點(diǎn)。然后進(jìn)行特征匹配，通過(guò)scan to scan的方式對(duì)每幀掃描數(shù)據(jù)的特征點(diǎn)進(jìn)行匹配，求解得到一個(gè)旋轉(zhuǎn)矩陣R和一個(gè)平移矩陣T。當(dāng)獲取到若干相鄰幀的位姿信息后，將位姿信息與全局地圖進(jìn)行匹配，從而優(yōu)化位姿信息得到一個(gè)更加精準(zhǔn)的定位數(shù)據(jù)。但是由于其缺少閉環(huán)檢測(cè)功能，積累誤差會(huì)隨著時(shí)間的增長(zhǎng)不斷增長(zhǎng)。

2.2 LeGO-LOAM算法原理

LeGO-LOAM 算法[25]提出了一種基于地面優(yōu)化的輕量級(jí)SLAM方法，實(shí)現(xiàn)實(shí)時(shí)六自由度里程計(jì)。首先根據(jù)激光雷達(dá)的水平精度和線數(shù)，將點(diǎn)云數(shù)據(jù)投影為一幅深度圖像，然后進(jìn)行地面的提取，這一過(guò)程去除了一些不穩(wěn)定的特征點(diǎn)。使用L-M 算法進(jìn)行優(yōu)化計(jì)算位姿。同時(shí)利用 Point Cloud Library（PCL）庫(kù)中基于半徑的近鄰搜索算法，實(shí)現(xiàn)了閉環(huán)檢測(cè)功能。

2.3 M-LOAM算法原理

基于激光雷達(dá)的方法常受到數(shù)據(jù)稀疏和垂直視場(chǎng)有限的問(wèn)題。針對(duì)這些問(wèn)題，M-LOAM算法[26]提出了多激光雷達(dá)的SLAM技術(shù)。整個(gè)系統(tǒng)首先對(duì)點(diǎn)云進(jìn)行分割聚類、去除噪聲、提取邊緣特征和平面特征。其中分割與LeGO-LOAM相同，特征提取與LOAM相同。然后進(jìn)行系統(tǒng)初始化，不需要事先進(jìn)行配置，對(duì)多個(gè)激光雷達(dá)進(jìn)行標(biāo)定，移植性得到了提升。同時(shí)提出了一種基于滑動(dòng)窗口的里程計(jì)，聯(lián)合利用多個(gè)激光雷達(dá)數(shù)據(jù)，減少了飄移量。

2.4 F-LOAM算法原理

基于激光雷達(dá)的SLAM通常將問(wèn)題分為兩個(gè)部分，scan-to-scan匹配和scan-to-map優(yōu)化，這兩個(gè)模塊依賴迭代的方法進(jìn)行計(jì)算，大量的迭代降低了計(jì)算效率。針對(duì)這個(gè)問(wèn)題，F(xiàn)-LOAM算法[27]提出了采用非迭代兩步法實(shí)現(xiàn)畸變補(bǔ)償降低計(jì)算效率。其假設(shè)上一幀的運(yùn)動(dòng)速度作為當(dāng)前幀的預(yù)測(cè)，對(duì)當(dāng)前幀所有特征點(diǎn)進(jìn)行去畸變，得到位姿估計(jì)后利用得到的位姿重新對(duì)特征點(diǎn)進(jìn)行去畸變。相比較LOAM和LeGO-LOAM，在一幀數(shù)據(jù)的處理周期中，有效降低了參與迭代的點(diǎn)數(shù)和迭代的次數(shù)，提升了實(shí)時(shí)性。

2.5 rang-mcl算法原理

range-mcl算法[28]的主要思路是將地圖利用結(jié)構(gòu)更加緊湊的三角形網(wǎng)格對(duì)地圖進(jìn)行表示。range-mcl不使用原始的點(diǎn)云數(shù)據(jù)，而是對(duì)三維激光雷達(dá)掃描的數(shù)據(jù)通過(guò)球坐標(biāo)進(jìn)行投影生成深度圖像。然后將用三角形網(wǎng)格表示的地圖和當(dāng)前掃描得到的深度圖像進(jìn)行合成渲染，同時(shí)建立一種新的觀測(cè)模型，通過(guò)粒子濾波實(shí)現(xiàn)基于先驗(yàn)地圖的精確定位。

其中球面投影法的原理如下：

式中，r為掃描范圍；f=fup+fdown為傳感器垂直角度；ω和h為將點(diǎn)云投影到頂點(diǎn)圖VD的寬度和高度。當(dāng)給定VD和每個(gè)坐標(biāo)（u,v）上點(diǎn)的范圍r，即生成深度圖像。

2.6 MULLS算法原理

針對(duì)現(xiàn)有的激光SLAM缺乏通用性，MULLS算法[29]提出了一種通用的三維激光SLAM系統(tǒng)。首先對(duì)點(diǎn)云進(jìn)行投影到參考平面，將參考平面劃分為等格的2D體素格，記錄每個(gè)體素格內(nèi)最小點(diǎn)的高度和對(duì)應(yīng)的三維體素格，根據(jù)值的大小判斷其是否屬于地面，再使用RANSAC對(duì)地面擬合進(jìn)行優(yōu)化。地面擬合完成后使用主成分分析（Principal Component Analysis, PCA）的方法進(jìn)行特征提取，最后利用多度量線性最小二乘迭代最臨近點(diǎn)算法實(shí)現(xiàn)當(dāng)前掃描幀與目標(biāo)點(diǎn)云的配準(zhǔn)。構(gòu)建點(diǎn)點(diǎn)、點(diǎn)線、點(diǎn)面誤差，通過(guò)高斯馬爾可夫參數(shù)估計(jì)的方法進(jìn)行優(yōu)化求解。

2.7 幾種開(kāi)源三維激光SLAM的優(yōu)缺點(diǎn)

表1對(duì)幾種開(kāi)源三維激光SLAM方法的優(yōu)缺點(diǎn)進(jìn)行了比較。

表1 開(kāi)源三維激光SLAM方法的優(yōu)缺點(diǎn)對(duì)比

3 三維激光SLAM應(yīng)用中的關(guān)鍵問(wèn)題

激光雷達(dá)具有不受光照影響可靠性強(qiáng)的特點(diǎn)，但也具有局部點(diǎn)云稀疏、Z軸的漂移以及動(dòng)態(tài)對(duì)象引發(fā)的噪聲影響的問(wèn)題，這成了三維激光SLAM在應(yīng)用于智能車(chē)輛時(shí)需要解決的關(guān)鍵問(wèn)題。

3.1 地面分割

對(duì)于智能車(chē)輛車(chē)載激光雷達(dá)由于其工作在室外環(huán)境，地面分割起到了關(guān)鍵性作用，分割的地面對(duì)Z軸的位姿進(jìn)行約束，同時(shí)以地面為基準(zhǔn)可以過(guò)濾掉一些噪聲及對(duì)局部稀疏點(diǎn)云進(jìn)行聚類。

地面分割方法通常分為基于柵格地圖、基于平面模型及基于深度圖像?；跂鸥竦貓D的方法通過(guò)計(jì)算投影到柵格的差值，根據(jù)人為設(shè)定閥值去判斷是否為地面，該方法簡(jiǎn)單高效但柵格大小以及閥值單一易造成過(guò)分割或欠分割[30]。HIMMELSBACH等[31]對(duì)原始點(diǎn)云進(jìn)行劃分柵格，對(duì)離散點(diǎn)進(jìn)行直線擬合，通過(guò)距離分割出地面，快速且高效?；谄矫婺Ｐ偷姆椒ㄍㄟ^(guò)建立一個(gè)模型表示地面，將符合該模型的點(diǎn)歸為地面點(diǎn)。李孟迪等[32]通過(guò)抽樣隨機(jī)一致性算法構(gòu)建平面模型對(duì)地面進(jìn)行分割，可以很好地處理異常值問(wèn)題?；谏疃葓D像的方法通過(guò)將三維點(diǎn)云轉(zhuǎn)換為深度圖像，利用相鄰兩點(diǎn)連線與水平線間的夾角構(gòu)建閥值，當(dāng)角度小于經(jīng)驗(yàn)值時(shí)將該點(diǎn)歸為平面。但上述方法都存在閥值設(shè)置的問(wèn)題，不能自適應(yīng)。張凱等[33]針對(duì)復(fù)雜道路提出一種基于自適應(yīng)閥值的三維激光點(diǎn)云地面分割的方法，通過(guò)構(gòu)建路面波動(dòng)幅度方程，實(shí)現(xiàn)了自適應(yīng)閥值。朱株等[34]提出一種基于馬爾可夫隨機(jī)場(chǎng)的路面分割算法，通過(guò)采用最大模糊線段法對(duì)每條激光雷達(dá)掃描線在X-Y平面投影，利用角點(diǎn)定位線段端點(diǎn)，建立能量方程，用圖分割的方法將線段標(biāo)記為地面與非地面。

3.2 運(yùn)動(dòng)目標(biāo)檢測(cè)

現(xiàn)在的大多數(shù) SLAM 方法都是假設(shè)靜態(tài)環(huán)境，對(duì)于智能車(chē)輛而言其所處的環(huán)境不可能是靜態(tài)環(huán)境，于是環(huán)境中的運(yùn)動(dòng)目標(biāo)會(huì)對(duì)SLAM過(guò)程產(chǎn)生噪聲導(dǎo)致定位和建圖精度下降甚至失真。CHEN等[35]提出對(duì)每幀點(diǎn)云集成語(yǔ)義信息利用RangeNet++分割出動(dòng)態(tài)物體并進(jìn)行剔除，提高了構(gòu)建地圖的精確度。ZHAO等[36]通過(guò)建立動(dòng)態(tài)目標(biāo)檢測(cè)模塊，通過(guò)卷積神經(jīng)網(wǎng)絡(luò)（Convolution Neural Network, CNN）從每幀掃描中剔除動(dòng)態(tài)目標(biāo)，但實(shí)時(shí)性較差。王忠立等[37]提出融合激光雷達(dá)與慣性傳感器，利用慣性傳感器補(bǔ)償激光雷達(dá)運(yùn)動(dòng)誤差，在補(bǔ)償后的點(diǎn)云中通過(guò) Fully CNN（FCNN）檢測(cè)運(yùn)動(dòng)目標(biāo)基于無(wú)跡卡爾曼濾波的方法區(qū)分動(dòng)、靜目標(biāo)。

4 總結(jié)與展望

激光SLAM的精度和可靠性是智能車(chē)輛實(shí)現(xiàn)自主導(dǎo)航的前提。傳統(tǒng)的SLAM模塊還有很大的提高空間。實(shí)時(shí)的高精度SLAM方法將是未來(lái)智能車(chē)輛大眾化的關(guān)鍵。

隨著智能車(chē)輛的大力發(fā)展，激光SLAM發(fā)展趨勢(shì)大致如下：

1）激光SLAM與深度學(xué)習(xí)的結(jié)合。深度學(xué)習(xí)具有學(xué)習(xí)能力強(qiáng)，可應(yīng)用場(chǎng)景廣，可移植性強(qiáng)的特點(diǎn)。目前二者的結(jié)合多用于對(duì)動(dòng)態(tài)物體的剔除，將掃描配準(zhǔn)與深度學(xué)習(xí)相結(jié)合，實(shí)現(xiàn)端到端的幀間估計(jì)將是一個(gè)熱點(diǎn)問(wèn)題。

2）對(duì)于場(chǎng)景復(fù)雜的環(huán)境，多傳感器的融合必然是一個(gè)趨勢(shì)。視覺(jué)傳感器具有良好的語(yǔ)義信息可以給激光雷達(dá)提供高精度的深度信息幫助激光進(jìn)行閉環(huán)。高精度慣性傳感器在短時(shí)間內(nèi)的運(yùn)動(dòng)估計(jì)十分準(zhǔn)確，可以用來(lái)修正激光雷達(dá)通過(guò)掃描匹配方式得到的位姿估計(jì)誤差。

3）在范圍較大的環(huán)境下，多車(chē)同時(shí)構(gòu)建SLAM 是一個(gè)必然趨勢(shì)。目前，通過(guò)單個(gè)智能汽車(chē)構(gòu)建局部地圖，再對(duì)所有的局部地圖進(jìn)行融合以達(dá)到多車(chē) SLAM，但其在動(dòng)態(tài)環(huán)境中易出現(xiàn)錯(cuò)誤。因此如何解決多車(chē)SLAM中動(dòng)態(tài)環(huán)境下的錯(cuò)誤是一個(gè)研究難點(diǎn)。