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

代凱 申棋仁 吳官樸 趙陽(yáng) 馬芳武

（1.吉林大學(xué) 汽車仿真與控制國(guó)家重點(diǎn)實(shí)驗(yàn)室，長(zhǎng)春130022；2.吉林大學(xué) 汽車工程學(xué)院，長(zhǎng)春130022）

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

縮略語(yǔ)

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 引言

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

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

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

2 基于激光雷達(dá)的同步定位和建圖

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

2.1 激光雷達(dá)前端配準(zhǔn)算法

2.1.1 基于最近鄰點(diǎn)迭代（ICP）的點(diǎn)云配準(zhǔn)算法

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

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

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

正態(tài)分布變換（NDT）算法[17]是一個(gè)配準(zhǔn)算法，它應(yīng)用于三維點(diǎn)的統(tǒng)計(jì)模型，使用標(biāo)準(zhǔn)最優(yōu)化技術(shù)來(lái)確定2個(gè)點(diǎn)云間的最優(yōu)的匹配，因?yàn)槠湓谂錅?zhǔn)過(guò)程中不利用對(duì)應(yīng)點(diǎn)的特征計(jì)算和匹配，所以時(shí)間比其他方法快。

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

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

2.1.3 基于特征的點(diǎn)云配準(zhǔn)算法

激光點(diǎn)云特征提取有2 種思路：模型驅(qū)動(dòng)的模式和數(shù)據(jù)固有特征模式。

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

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

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

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

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

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

2.2.1 基于濾波法的后端

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

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

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

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

3 基于車輛模型定位技術(shù)

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

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

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

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

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

4.1 地圖種類

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

圖1 HD map分層示意[49]

圖2 車載激光雷達(dá)獲取的點(diǎn)云地圖

4.2 基于地圖的定位方法

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

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

4.3 多源融合定位方法

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

5 結(jié)論

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

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

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

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

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