巴浩麟，馬曉錄，吳立輝

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

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

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

1 三維激光SLAM算法框架及關鍵模塊

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

其表達式為

式中，x1:i為從1時刻到i時刻的姿態(tài)；u1:i為時刻i-1為機器人在時刻i達到位姿xi的控制信號；z1:i為在未知環(huán)境中行駛會觀測到的許多觀測值，在i時刻觀測到的觀測值；為一個標準化常數(shù)，與位姿無關。

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

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

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

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

圖1 三維激光SLAM框架

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

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

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

1.1 掃描匹配

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

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

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

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

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

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

圖3 ICP大致流程圖

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

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

圖4 NDT大致流程圖

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

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

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

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

1.2 后端優(yōu)化

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

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

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

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

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

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

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

1.3 閉環(huán)檢測

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

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

1.4 地圖構建

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

圖7 點云地圖

2 三維激光SLAM算法

目前開源的三維激光SLAM包括雷達定位與建圖（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]提出了一種實時的 SLAM 方法，通過兩個算法將定位與建圖分成兩個問題進行求解。定位算法以高頻率得到一個精度較低的里程計，建圖算法以低頻率構建精度較高的地圖數(shù)據(jù)，二者結合實現(xiàn)一個實時高精度的定位。

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

2.2 LeGO-LOAM算法原理

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

2.3 M-LOAM算法原理

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

2.4 F-LOAM算法原理

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

2.5 rang-mcl算法原理

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

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

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

2.6 MULLS算法原理

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

2.7 幾種開源三維激光SLAM的優(yōu)缺點

表1對幾種開源三維激光SLAM方法的優(yōu)缺點進行了比較。

表1 開源三維激光SLAM方法的優(yōu)缺點對比

3 三維激光SLAM應用中的關鍵問題

激光雷達具有不受光照影響可靠性強的特點，但也具有局部點云稀疏、Z軸的漂移以及動態(tài)對象引發(fā)的噪聲影響的問題，這成了三維激光SLAM在應用于智能車輛時需要解決的關鍵問題。

3.1 地面分割

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

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

3.2 運動目標檢測

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

4 總結與展望

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

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

1）激光SLAM與深度學習的結合。深度學習具有學習能力強，可應用場景廣，可移植性強的特點。目前二者的結合多用于對動態(tài)物體的剔除，將掃描配準與深度學習相結合，實現(xiàn)端到端的幀間估計將是一個熱點問題。

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

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