基于激光雷達的SLAM和融合定位方法綜述

代凱 申棋仁 吳官樸 趙陽 馬芳武

（1.吉林大學 汽車仿真與控制國家重點實驗室，長春130022；2.吉林大學 汽車工程學院，長春130022）

主題詞：激光雷達 無人駕駛 同步定位和建圖 多傳感器融合

縮略語

SLAM Simultaneous Localization And Mapping

LiDAR Light Detection and Ranging

GNSS Global Navigation Satellite System

IMU Inertial Measurement Unit

RTK Real Time Kinematic

ESKF Error State Kalman Filter

LIO LiDAR Inertial Odometry

UWB Ultra Wide Band

V2I Vehicle-to-Infrastructure

LOAM LiDAR Odometry and Mapping

NDT Normal Distribution Transform

DR Dead Reckoning

VINS Visual Inertial System

MSCKF Multi-State Constraint Kalman Filter

1 引言

自動駕駛是當下研究的熱點，車輛定位是自動駕駛必須解決的關鍵問題，良好的定位系統(tǒng)不僅能夠有效幫助車輛完成自動駕駛功能，更能夠有效提高車輛的安全性。目前，無人駕駛車輛廣泛采用多傳感器融合定位方案，以覆蓋不同的行駛工況。主流方案有2種，分別為基于傳感器的定位方法和基于車聯(lián)網(wǎng)的定位方法[1]。使用多傳感器融合定位時，主要采用的是基于GNSS、IMU、攝像頭、激光雷達、車輛編碼盤等傳感器融合定位的方案。IMU/GPS 融合定位方法，能夠彌補GPS 多路徑干擾、信號丟失等問題，增強基于衛(wèi)星定位方法的魯棒性、穩(wěn)定性。文獻[2]提出了LiDAR/GNSS/IMU 組合導航系統(tǒng)，采用ESKF 濾波器融合64線激光雷達定位信息、GNSS-RTK 定位信息、IMU 信息，實現(xiàn)5～10 cm 的連續(xù)穩(wěn)定車道級定位。文獻[3]提出了緊耦合3D激光慣性里程計（LIO），有效解決了純激光雷達定位長航程數(shù)據(jù)漂移、環(huán)境特征稀疏定位失效等問題。在基于車聯(lián)網(wǎng)定位研究領域，文獻[4]提出了基于多模型濾波算法的車輛協(xié)作定位增強方法，有利于提高車輛在“高樓峽谷”、隧道等弱衛(wèi)星信號區(qū)域的定位精度；文獻[5]提出了基于UWB的單錨點V2I定位算法，有利于提高基于GPS、V2I組網(wǎng)的車聯(lián)網(wǎng)定位精度。

在眾多的定位方法中，利用激光雷達構建地圖，并基于地圖定位的方法被廣泛運用在無人駕駛車輛開發(fā)中。在美國早期舉辦的DARPA 無人車挑戰(zhàn)賽中，包括斯坦福等在內(nèi)的高校利用激光雷達實現(xiàn)建圖及定位[6]，并借此提高定位精度及魯棒性，取得了較好的成績。文獻[7]提出了一種改進的激光里程計，結合了文獻[8]和[9]的優(yōu)點，采用SegMap 基于已知地圖檢測閉環(huán)，與LOAM[8]的里程計進行融合，減少純激光定位漂移問題，獲得良好的定位效果。

本文主要包括3 部分內(nèi)容：首先對基于激光雷達的同步定位和建圖進行討論，其次對如何利用車輛模型輔助無人駕駛車輛進行定位展開敘述，最后對基于地圖的融合定位技術進行分析，并對應用于無人駕駛車輛的激光SLAM及融合定位技術前景提出展望。

2 基于激光雷達的同步定位和建圖

高精度地圖是無人駕駛車輛開發(fā)與使用中必不可少的一環(huán)，也是定位和導航規(guī)劃的前提，當前，激光雷達是構建高精度地圖必不可少的傳感器。在文獻[6]中，作者采用GPS、IMU、輪式里程計、激光雷達等多種傳感器融合生成高精度地圖，也有像文獻[8]這樣采用純激光雷達構建地圖的方法。在地圖的構建方法中，最常用的是同步定位和建圖（SLAM），SLAM 又分為前端和后端，前端主要完成點云配準工作，本質(zhì)上來說是一個激光里程計，里程計是靠當前幀和地圖參考幀的幀間匹配算法完成位姿估計的，但是位姿估計會隨著里程計的增加產(chǎn)生漂移，導致建圖不準，后端會建立外部觀測約束，以此來減少前端里程計產(chǎn)生的漂移[10]。

2.1 激光雷達前端配準算法

2.1.1 基于最近鄰點迭代（ICP）的點云配準算法

ICP(Iterative Closest Point )算法被廣泛應用于初始位姿已知的三維點云幾何對準，其原理如下：假設2個點云數(shù)據(jù)集合P 和G，要通過P 轉換到G（2 組點云存在局部幾何特征相似的部分），可以通過P 叉乘四元矩陣進行旋轉平移變換到G，或者采用SVD 法將P轉換到G 位置，總體思想都是需要一個4×4 的旋轉平移矩陣。對于每次旋轉平移變換后計算P 的所有點到G 對應點的距離，用最小二乘法求出最小二乘誤差，看是否在要求的范圍內(nèi)，如果最小二乘誤差小于設定的值，則計算結束，否則繼續(xù)進行迭代。隨著匹配效率和精度的要求逐步提高，衍生出了多種改進算法[11]。文獻[12]提出了Generalized-ICP（GICP），將ICP 和PPICP 融合到了一個概率框架下，具有更靈活的參數(shù)配置方案，相比于單個系統(tǒng)，其魯棒性更高；文獻[13]提出了一種基于尺度的掃描匹配算法，通過實驗證明Point-to-Line ICP（PLICP）在計算效率和匹配精度方面均有提高，但該方法存在初始化誤差較大的缺點，為解決該問題，文獻[14]提出了一種改進的PLICP 算法，提高了初始化匹配時的魯棒性和計算效率。

ICP算法被廣泛運用在SLAM算法中，文獻[15]提出了一種基于ICP 的SLAM 算法，該算法僅利用激光點云數(shù)據(jù)完成定位和建圖工作；在文獻[15]的基礎上，文獻[16]加入了3自由度IMU 信息進行點云校正和位置預測，該方法對旋轉運動具有較強的魯棒性，適用于傳感器位置和方向變化幅度較大的場景。

2.1.2 基于正態(tài)分布變換（NDT）的點云配準算法

正態(tài)分布變換（NDT）算法[17]是一個配準算法，它應用于三維點的統(tǒng)計模型，使用標準最優(yōu)化技術來確定2個點云間的最優(yōu)的匹配，因為其在配準過程中不利用對應點的特征計算和匹配，所以時間比其他方法快。

文獻[18]提出了一種3D-NDT 掃描配準算法，該方法改進了由Biber[17]等人提出的2D-NDT算法，作者將該算法與標準ICP 算法進行了定性和定量的比較，對比結果表明該方法速度更快，可靠性更高；文獻[19]提出了一種快速、準確的點云配準算法，顯著提高了計算效率和精度，此外，作者還提出了一個基于3DNDT的全局描述子，用于實現(xiàn)迭代算法可靠的初始位姿估計。為了評價NDT 和ICP 的性能，文獻[20]通過實驗對2 種三維掃描配準算法進行了深入的對比分析，并提出了一種改進的NDT算法。

由于NDT 3D 點云配準算法計算效率高，初始化效果較好，因此備受開發(fā)者青睞，hdl_graph_slam 是使用3D LiDAR的實時6 DOF（Degree Of Freedom）SLAM的開源ROS 軟件包，它基于NDT 掃描匹配的方法實現(xiàn)位姿估計和閉環(huán)檢測。在理論研究方面，文獻[21]提出了一種基于NDT 的SLAM 方法，在動態(tài)環(huán)境中，該方法能夠實現(xiàn)長航程高精度建圖和定位；文獻[22]介紹了基于NDT 的圖優(yōu)化SLAM 在復雜城市工況下的性能，經(jīng)過試驗分析，結果表明，采用NDT 的激光SLAM 算法，其性能與交通環(huán)境和城鎮(zhèn)化率有明顯相關性，在復雜的交通環(huán)境下，定位和建圖性能明顯下降。

2.1.3 基于特征的點云配準算法

激光點云特征提取有2 種思路：模型驅動的模式和數(shù)據(jù)固有特征模式。

模型驅動模式的核心是確定點云數(shù)據(jù)與模型在全局或局部的匹配關系，典型的參數(shù)域提取方法有Hough、Randon 等，原本是用在二維圖像處理中，用于檢測直線、圓形、橢圓等，后有研究人員將其引入到點云數(shù)據(jù)處理中，可用于檢測點云中蘊含的空間平面、球體、柱體等。但此種方法缺乏普適性，只能實現(xiàn)對特定參數(shù)的提取，而且需要有集合模型。

利用數(shù)據(jù)固有特征描述點云特征不需要預先建模，幾乎可以應用于所有場景，即充分利用幾何特性及微分幾何特性，例如曲面的平均曲率、高斯曲率、表面法向等。根據(jù)點云選取范圍的不同，可分為全局特征和局部特征，對于激光雷達特征，由于無法預先獲取全局場景信息，采用局部特征更為合適。由于點云表面的法線、曲率等計算受點云噪聲影響較大，因此，在進行特征提取時，需要對點云進行平滑操作，去除噪聲的同時保留特征，主流的方法有：基于法向約束和PCA[23]的面擬合、特征邊緣識別算法；采用KNN[24]鄰域算法實現(xiàn)法向量的多尺度平滑計算，并利用平滑后的法向量重構表面提取特征。

在基于特征匹配的前端激光里程計算法中，LOAM[8]是純激光匹配算法中較為出色的，長期霸占KITTI[25]數(shù)據(jù)集純激光里程計算法榜首，作者利用角和面2種特征，建立特征點云地圖，新加入的點云特征與已建立的2 種特征點云地圖匹配得到相對精確的位姿。為獲得最優(yōu)位姿，通過點-線和點-面關聯(lián)共同構建約束,以點線距離和點面距離作為損失函數(shù)，基于非線性最小二乘的方法進行優(yōu)化。LeGO-LOAM[26]在LOAM 的基礎上對地面特征進行提取，主要利用平面和邊特征，提出了一種優(yōu)化的輕量級里程計和建圖方法，相比于LOAM，該方法減少了計算資源，且精度有所提升。文獻[27]提出了一種LiDAR-IMU 緊耦合融合方法，在LOAM 的基礎上加入IMU，通過聯(lián)合優(yōu)化LiDAR、IMU 測量值，配合旋轉約束優(yōu)化算法（LIOmapping），使雷達位姿與全局地圖對齊，有效減少長航程里程計漂移，得到良好的建圖效果。LIO-SAM[28]在LeGO-LOAM[26]、LIO[27]的基礎上，采用因子圖優(yōu)化算法，結合滑動窗口（Sliding window）方法，融合了IMU預積分因子、激光里程計因子、GPS因子、閉環(huán)檢測因子，實現(xiàn)多傳感器融合及全局優(yōu)化，以獲得良好的位姿估計和地圖構建效果，作者對該算法進行了開源，在該算法框架下，可以再加入航向計、高度計等絕對測量儀，輔助降低里程計漂移。

2.2 激光雷達SLAM后端優(yōu)化

對于一個建圖系統(tǒng)來講，有了后端才有核心，前端能夠根據(jù)相鄰的2幀點云判斷出此時此刻的位姿，且可以給出一個增量式的地圖，由于不可避免的誤差累計，這個地圖在長時間內(nèi)是不準確的。而SLAM 致力于構建一個長生命周期的可靠的解決方案，因此只有前端是遠遠不夠的。當?shù)貓D增長到一定程度后，累計誤差會使后來的數(shù)據(jù)越來越不準確。這時我們需要把所有地圖數(shù)據(jù)放到一起做一次完整的優(yōu)化，從而降低各部分的誤差。

后端優(yōu)化有很多種方案，過去采用以擴展卡爾曼濾波（Extended Kalman Filter，EKF）為主的濾波器方案，現(xiàn)在大多都采用非線性優(yōu)化方案。EKF由于假設了馬爾可夫性質(zhì)，只利用前一時刻狀態(tài)來估計當前狀態(tài)的值，很難做到全局的優(yōu)化。目前常用的非線性優(yōu)化方法則是把所有數(shù)據(jù)都考慮進來，進行全局優(yōu)化，雖然會增大計算量，但融合精度較高。

2.2.1 基于濾波法的后端

在計算資源受限、待估計量比較簡單的情況下，以擴展卡爾曼濾波（EKF）為代表的濾波法比較有效，通常應用在激光SLAM中。但它的一大缺點是存儲量和狀態(tài)量是平方增長關系，因此不適合大型場景。而現(xiàn)在基于視覺的SLAM 方案，路標點（特征點）數(shù)據(jù)很大，所以此時濾波的方法效率非常低。其余濾波算法也被應用在SLAM 算法中，文獻[29]提出的UKFSLAM 降低了計算的復雜度，解決了狀態(tài)估計不一致問題，應用基于可觀性約束的UKF 算法，保證了濾波器的系統(tǒng)模型有一個適當維數(shù)的不可觀測子空間；文獻[30]提出了一種自適應減少粒子數(shù)的粒子濾波器，以分布式計算精度的方式，有選擇地進行重采樣，減少粒子耗盡問題，在室內(nèi)外大尺度場景中取得了良好的建圖效果。

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

當前，優(yōu)化法是SLAM 算法后端的主流方案，SLAM 的狀態(tài)估計是一個非線性最小二乘問題，一般使用梯度下降法、高斯牛頓法(G-N)、列文伯格-馬夸爾特方法(Levenberg-Marquardt Method, LM)、以及Dog-Leg 方法等。在實際應用中，通常使用谷歌的Ceres 庫以及基于圖優(yōu)化的g2o 庫[31]來實現(xiàn)這些求解方法?；趦?yōu)化的方法一般利用所有的觀測信息來優(yōu)化估計機器人完整的運動軌跡，因此也稱為全SLAM 方法。

文獻[32]提出的2D激光SLAM算法，采用Ceres求解器實現(xiàn)后端優(yōu)化，其開源的Cartographer 算法在2D SLAM 中應用較為廣泛。在基于優(yōu)化法的SLAM 后端中，g2o的應用是更為廣泛的，基于g2o的SLAM方法，是利用圖模型對SLAM 問題進行建模，模型中的節(jié)點對應不同時刻機器人及環(huán)境組成系統(tǒng)的狀態(tài)，圖優(yōu)化的邊則描述了系統(tǒng)狀態(tài)(節(jié)點)之間的約束關系[33]。當前基于緊耦合的激光SLAM 算法LIO[27]、LIO-SLAM[28]均采用g2o方法實現(xiàn)后端優(yōu)化。

3 基于車輛模型定位技術

基于車輛模型的定位技術在自動駕駛車輛上已被廣泛使用。無論是基于傳感器定位還是基于車聯(lián)網(wǎng)定位，車輛模型均可為融合定位系統(tǒng)提供穩(wěn)定可靠的數(shù)據(jù)源，它不需要依賴外部條件，自主性和適用性較強，且不增加車輛硬件成本。文獻[34]分別對2 自由度車輛運動學、動力學模型進行理論分析和仿真試驗，并基于2自由度車輛模型將Kalman濾波器運用到車輛定位方法中，以提高基于航跡推算（DR）定位的精度；文獻[35]分別分析了2 自由度車輛運動學與動力學模型的適用范圍與優(yōu)缺點，并利用交互式多模型（IMM）概率算法結合車輛運動學與動力學模型，提高基于車輛模型定位精度；文獻[36]采用2 自由度車輛運動學模型與GPS、LiDAR 融合定位，并成功應用在高速自動駕駛賽車上；文獻[37]采用2 自由度車輛模型進行航跡推算（DR），并利用GNSS 定位結果與DR結果進行對比，得出DR 精度高，但準確度較低的結論，采用無跡卡爾曼濾波（UKF）融合GNSS、DR 定位結果，仿真結果表明，融合算法能夠有效彌補2者的缺點，提高定位精度及穩(wěn)定性；文獻[38]提出了同時采用2自由度車輛運動學模型和車輛動力學模型輔助慣導系統(tǒng)的方法，以提高導航定位精度，增強定位系統(tǒng)穩(wěn)定性；文獻[39]以3 自由度非線性車輛動力學模型為基礎，建立了車輛動力學模型輔助的慣性導航系統(tǒng)，用于提高車輛航向角精度；文獻[40]以9 自由度車輛動力學模型為基礎，為車載融合定位系統(tǒng)提供狀態(tài)信息。

以上基于車輛模型輔助的定位方法在各自領域均不同程度地提升了自動駕駛車輛的定位性能，但在車輛模型選擇上，并未系統(tǒng)論述選擇車輛模型的依據(jù)。文獻[41]從理論角度分析了車輛模型對導航系統(tǒng)的性能影響，基于2自由度車輛運動學模型拓展了無側滑、有側滑、有明確指向性側滑3個類型車輛模型，并進行了仿真試驗，分析了不同模型的定位誤差，得出高階車輛模型對導航定位性能有正面促進作用，且需要合理選擇參數(shù)可觀性較高的車輛模型。

車輛模型在SLAM 領域中也有應用。文獻[42]提出了一種結合2自由度車輛動力學模型的視覺慣性估計器，以解決車輛在勻速、勻加速等工況下視覺慣性導航系統(tǒng)（VINS）不可觀測導致定位精度降低等問題。作者通過將車輛運動學信息擴展到IMU 預積分文獻[43]模型中，提高VINS 的初始化能力，同時采用基于滑動窗口的優(yōu)化法，提高車輛動力學輔助VINS系統(tǒng)的里程計精度；同理，為解決VINS 性能退化問題，文獻[44,45]在VINS的基礎上加入輪速信息；文獻[46]在濾波緊耦合算法MSCKF的基礎上，加入車輛阿克曼模型，結合標定后的車速、轉角信息，提高了車輛視覺慣性里程計精度。在車輛工程領域，車輛滑移是比較常見的現(xiàn)象，如何準確估計滑移率，給輪速合理的補償，以提高輪速精度，是值得探討的問題。文獻[47]提出了一個帶有滑移的車輛模型，準確估計車輛滑移率，避免由于車輛滑移導致VINS 系統(tǒng)定位精度降低的問題。文獻[48]采用交互式多模型擴展卡爾曼濾波器融合車輛運動學模型和車輛動力學模型，實現(xiàn)車輛模型優(yōu)勢互補，將該模型作為預測模型，以視覺、IMU、GPS 為觀測更新模型，實現(xiàn)多傳感器融合，以提高車輛定位性能。

4 基于地圖的多傳感器融合定位技術

基于地圖的定位是無人駕駛車輛中常用的技術方法，以激光雷達為例，通過當前激光掃描與預先構建的點云地圖匹配，可獲得無人駕駛車輛在地圖中的位置。對于室外無人駕駛車輛來說，由于環(huán)境特征減少或相似的環(huán)境特征導致掃描匹配定位信息錯誤，容易造成重大安全事故，因此需要融合來自GPS、IMU等外部傳感器信息，提高定位精度和魯棒性。

4.1 地圖種類

一般地圖分為2類，第1類是平面地圖，即依賴地理信息系統(tǒng)上的圖層或平面的地圖，例如高精度地圖（HD map），如圖1。這類地圖一般分4層：第1層代表靜態(tài)地圖，包含路網(wǎng)、建筑地標等信息，第2層包含準靜態(tài)信息，如路側基礎設施、可變限速等，第3層包含通過與基礎設施交互獲得的瞬間動態(tài)數(shù)據(jù)，如天氣信息、紅綠燈時間、擁擠程度等，第4層是一個高度動態(tài)的局部動態(tài)地圖，通過V2X 通訊可以獲得車與車之間、車與路之間、車與云之間等萬物互聯(lián)的信息。第2類是點云地圖，一般由3D掃描儀或激光傳感器生成，用來表示物體在3D 空間中的外表面特征信息，數(shù)據(jù)包含X、Y、Z坐標及點云強度信息[1]。

圖1 HD map分層示意[49]

圖2 車載激光雷達獲取的點云地圖

4.2 基于地圖的定位方法

地圖是無人車導航、定位的基礎。在平面地圖中，車輛從車載傳感器和V2X設備收集有關環(huán)境和其它道路使用者的信息，與預先存儲的高精度地圖（HD map）進行對比，從而實現(xiàn)車輛自身定位。

點云地圖是目前無人駕駛車輛中應用較為廣泛的地圖，可以實時與當前激光掃描幀進行匹配。文獻[50]提出了3D-NDT掃描匹配算法，相比于ICP點云掃描匹配算法[51]，NDT算法將掃描點分割為體素，每個體素格內(nèi)的掃描點近似為正態(tài)分布，其效率及精度更高，且更容易完成初始化，當前點云與預先存儲的點云地圖掃描匹配，即可得到車輛相對于點云地圖的位姿。文獻[6]提出利用粒子濾波器將當前激光測量值預先存儲的地圖進行關聯(lián)，實驗結果表明，該方法的相對精度比傳統(tǒng)的基于GPS-IMU 定位的方法高出一個數(shù)量級，在10 cm范圍內(nèi)實現(xiàn)了可靠的實時定位，提升了車輛在GPS干擾環(huán)境、惡劣天氣和密集交通條件下的定位性能。相比于只利用X、Y、Z 坐標點進行距離匹配的方式，百度自動駕駛團隊[2]提出了利用激光雷達強度值和高度信息與點云地圖匹配的方法，顯著提高了定位性能。

4.3 多源融合定位方法

利用互補性的傳感器進行融合定位是當前無人駕駛車輛常用的技術，LiDAR、IMU、GNSS、Camera 是較為常用的傳感器。文獻[2]提出了一種基于誤差狀態(tài)卡爾曼濾波器的多傳感器融合定位系統(tǒng)，主要融合來自激光雷達與地圖匹配的位姿信息、RTK-GNSS 信息、IMU 信息，輸出位置、速度、航向（PVA）信息，實現(xiàn)穩(wěn)定可靠的厘米級定位。除高樓、密集交通、極端氣候等復雜環(huán)境，在一般的交通場景下，現(xiàn)有傳感器（例如差分慣導）采用一個標準的濾波的方式即可獲得較高的定位精度。但是，要實現(xiàn)全場景的高精度定位，比如城市密集區(qū)域，一個穩(wěn)定的數(shù)據(jù)融合框架非常重要。文獻[52]提出了基于因子圖優(yōu)化的融合定位框架，并且加入了場景變化檢測和實時激光慣性里程計，即使在GNSS 信號丟失或者高精度地圖由于環(huán)境變化失效的情況下，激光里程計依舊可以在短時間內(nèi)提供可靠的里程計信息；此外，作者提出的圖優(yōu)化融合定位框架，相比于濾波法，可以有效減少線性化的誤差，且多次迭代可以更好地趨近最優(yōu)解，在復雜場景下，定位系統(tǒng)穩(wěn)定性更高。

5 結論

縱覽激光SLAM和融合定位30余年的研究發(fā)展，不難發(fā)現(xiàn)，從傳感器配置到算法均取得了長足發(fā)展。

（1）在傳感器配置方面經(jīng)歷了從單一傳感器到多傳感器互補的發(fā)展歷程，激光雷達經(jīng)歷了從2D 到3D 的變化，GNSS 定位精度經(jīng)歷了從米級到厘米級的提升；

（2）在激光雷達掃描匹配算法方面，經(jīng)歷了從ICP迭代匹配到NDT和基于特征匹配，再到當前基于點云深度學習匹配的方式；

（3）數(shù)據(jù)融合算法從線性濾波法逐步向非線性優(yōu)化方法發(fā)展，其中因子圖優(yōu)化與滑動窗口結合的方法以其良好的實時性和魯棒性，被廣泛應用在各類融合定位和建圖系統(tǒng)中。

未來，以激光雷達為基礎的SLAM 和融合定位系統(tǒng)仍然是無人駕駛技術發(fā)展的重要研究方向，尤其是基于點云的深度學習定位和基于非線性聯(lián)合優(yōu)化的算法，將加速該技術的發(fā)展；在高速無人駕駛車輛中，如何利用車輛運動學和動力學模型彌補自動駕駛車輛定位系統(tǒng)的穩(wěn)定性也會是值得探討的課題。