基于三維點云地圖和ESKF的無人車融合定位方法

崔文，薛棋文，李慶玲，王鳳棟，郝雪兒

（1. 國能準(zhǔn)能集團(tuán)有限責(zé)任公司 機(jī)電管理部，內(nèi)蒙古 鄂爾多斯 010300；2. 中國礦業(yè)大學(xué)（北京） 機(jī)電與信息工程學(xué)院，北京 100083）

0 引言

穩(wěn)定、精確、實時的定位能夠保證無人車行駛安全。目前無人車定位方法主要分為相對定位和絕對定位。相對定位主要采用航跡推算方法[1]和實時定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）方法。航跡推算方法主要采用車輪里程計或慣性測量單元（Inertial Measurement Unit，IMU）[2]得到車輛位姿，但車輪里程計在車輪打滑或側(cè)滑時會出現(xiàn)較大誤差，IMU則在長時間使用后產(chǎn)生較大的累計誤差，出現(xiàn)位置漂移。SLAM方法使用外界環(huán)境的特征來計算相對運動，對外界環(huán)境要求較高，并且每2幀數(shù)據(jù)的相對運動計算都會產(chǎn)生一定程度的誤差，且誤差隨車輛行駛距離的增加而增大。絕對定位指的是在已知全局參考信息的基礎(chǔ)上，通過傳感器獲得的信息計算車輛位姿的方法，不存在累計誤差[3]。絕對定位包括全球衛(wèi)星導(dǎo)航系統(tǒng)（Global Navigation Satellite System，GNSS）和地圖匹配[4]等定位方式。無人車采用GNSS定位對衛(wèi)星信號質(zhì)量要求較高，對于特定場景如煤礦井下、地下隧道則無法實現(xiàn)定位[5]。地圖匹配定位方法精度取決于已創(chuàng)建地圖的精度，受外界的影響較小，更適用于復(fù)雜場景下的無人車定位。

先驗地圖構(gòu)建是無人車實現(xiàn)地圖匹配定位的重要環(huán)節(jié)。常用的地圖包括二維柵格地圖和三維點云地圖。通過二維柵格地圖可直接獲取無人車的可行駛區(qū)域，但該地圖對周圍環(huán)境特征信息的描述較少，導(dǎo)致匹配定位精度低，且容易失效[6]。三維點云地圖的采集范圍更廣，環(huán)境特征信息豐富，不僅可用于環(huán)境感知、路徑規(guī)劃，還可用于高精度的無人車定位。相較于相機(jī)，三維激光雷達(dá)通過主動發(fā)射激光束來直接獲得與周圍環(huán)境特征之間的距離信息，數(shù)據(jù)更加準(zhǔn)確，且激光雷達(dá)幾乎不受光照變化的影響。這些優(yōu)良特性使得激光雷達(dá)常被用于制作精度較高的三維點云地圖?；诟呔热S點云地圖的無人車定位系統(tǒng)可以擺脫對衛(wèi)星信號的依賴，不受外界環(huán)境因素的干擾。

基于激光雷達(dá)的地圖匹配定位方法已成為學(xué)者研究的 重 點。M. Hentschel等[7]將OpenStreetMap數(shù)據(jù)集中的建筑物標(biāo)簽數(shù)據(jù)集成到機(jī)器人的定位任務(wù)中，通過激光雷達(dá)掃描得到的二維建筑物輪廓與地圖進(jìn)行匹配定位。F. Moosmann等[8]提出了基于迭代最近點（Iterative Closest Point，ICP）的三維激光雷達(dá)點云匹配算法，采用基于勻速運動模型的點云扭曲補(bǔ)償方法進(jìn)行地圖構(gòu)建和定位。Yang Jiaolong等[9]為了擴(kuò)大ICP算法的收斂范圍，結(jié)合分支定界法提出了一種全局最優(yōu)解迭代最近點（Globally Optimal Algorithm Iterative Closest Point，Go-ICP）算法，通過該算法對點云進(jìn)行配準(zhǔn)。D. Droeschel等[10]提出了一種基于點云表面元模型的概率配準(zhǔn)算法，并采用分層優(yōu)化的后端圖優(yōu)化策略對連續(xù)軌跡和地圖進(jìn)行優(yōu)化。然而，上述研究中所用的激光雷達(dá)點云匹配算法是以單一的特征為核心進(jìn)行匹配，對于大規(guī)模點云匹配準(zhǔn)確率較低，當(dāng)2幀數(shù)據(jù)之間變化劇烈時，點云匹配算法容易發(fā)生失效問題[11]。同時，進(jìn)行地圖匹配定位時沒有充分利用三維環(huán)境信息。因此，僅依靠基于激光雷達(dá)的地圖匹配定位方法不能實現(xiàn)穩(wěn)定、可靠的定位效果。

本文提出了一種基于三維點云地圖和誤差狀態(tài)卡爾曼濾波（Error State Kalman Filter，ESKF）的無人車融合定位方法。采用正態(tài)分布變換（Normal Distribution Transform，NDT）算法進(jìn)行點云匹配，提升大規(guī)模點云匹配準(zhǔn)確率，并通過圖優(yōu)化算法解決匹配過程中誤差不斷累計的問題，提高三維點云地圖構(gòu)建精度；采用ESKF融合定位方法，實現(xiàn)無人車位姿信息持續(xù)穩(wěn)定輸出，提高無人車定位精度。

1 三維點云地圖構(gòu)建

1.1 激光點云匹配算法

在激光點云匹配中使用前后幀點云數(shù)據(jù)來計算車輛的相對位姿變換。激光點云匹配算法可分為3類——基于點的掃描配準(zhǔn)、基于特征的掃描配準(zhǔn)和基于數(shù)學(xué)特性的掃描配準(zhǔn)。當(dāng)前主要采用的是基于點的掃描配準(zhǔn)算法——ICP類算法[12]和基于數(shù)學(xué)特性的掃描配準(zhǔn)算法——NDT類算法[13]。ICP算法迭代計算過程比較復(fù)雜，且對初值要求較高，在大規(guī)模、高密度的點云場景下進(jìn)行點云匹配時，查找對應(yīng)點速度較慢，配準(zhǔn)效率較低。NDT點云匹配的核心思想是將三維網(wǎng)格中的點云數(shù)據(jù)轉(zhuǎn)換為連續(xù)可微的概率分布函數(shù)，即將采樣得到的參考點云數(shù)據(jù)和待匹配的點云數(shù)據(jù)轉(zhuǎn)換為網(wǎng)格中三維空間點位置的概率分布，并用正態(tài)分布表示，之后使用Hessian矩陣優(yōu)化2個點云集合之間變換參數(shù)的正態(tài)分布概率，最終得到NDT點云配準(zhǔn)結(jié)果。

利用NDT算法進(jìn)行三維點云匹配的一般過程簡述如下。

（1） 將掃描得到的點云集合進(jìn)行網(wǎng)格化劃分，即將三維空間中的點云劃分為一個個邊長相等的立方體，劃分的立方體邊長則為NDT算法的分辨率（分辨率大小應(yīng)根據(jù)點云集合的實際情況而定）。通過每一個劃分的立方體網(wǎng)格中的點，計算每一個網(wǎng)格的均值μl和協(xié)方差El：

（2） 將點云集合之間的變換參數(shù)用k表示，通過里程計數(shù)據(jù)給k賦初值或?qū)初始化為單位矩陣。將待配準(zhǔn)點云集合中所有點按變換T進(jìn)行變換，并轉(zhuǎn)換到參考點云的立方體網(wǎng)格中，從而得到新的點云根據(jù)下式計算新點云落在參考點云中的概率（即待配準(zhǔn)點云經(jīng)過變換后與參考點云的重合程度）：

將所有立方體網(wǎng)格計算出來的概率相加，得到NDT配準(zhǔn)得分[14]：

（3） 采用牛頓優(yōu)化算法對NDT配準(zhǔn)得分進(jìn)行優(yōu)化，得到最佳的變換參數(shù)k，使得配準(zhǔn)得分最大。

根據(jù)上述NDT配準(zhǔn)過程，正態(tài)分布計算是在掃描參考點時一次性完成的，相較于ICP算法，耗時穩(wěn)定且計算速度較快。另外，NDT算法的計算精度與初值相關(guān)性較小，適合處理大規(guī)模點云數(shù)據(jù)，因此本文通過NDT算法進(jìn)行幀間點云匹配，提高點云配準(zhǔn)效率。

1.2 圖優(yōu)化算法

將激光雷達(dá)采樣得到的環(huán)境點云信息和NDT點云匹配得到的無人車位姿進(jìn)行關(guān)聯(lián)來構(gòu)建三維點云地圖，在短時間內(nèi)得到的是清晰度和精度較高的局部點云地圖。隨著無人車行駛距離增加，點云地圖的構(gòu)建范圍逐漸擴(kuò)大，掃描配準(zhǔn)的位移誤差和旋轉(zhuǎn)誤差也在不斷累計，最終將導(dǎo)致不可避免的累計誤差。對于構(gòu)建大范圍場景的點云地圖而言，累計誤差將導(dǎo)致地圖局部偏移及閉環(huán)處無法閉合的現(xiàn)象，使得整個點云地圖的精度較低。

針對地圖局部偏移及地圖精度低等問題，本文采用圖優(yōu)化算法來提高建圖效果。圖優(yōu)化算法中的位姿圖由頂點和邊構(gòu)成：頂點表示需要優(yōu)化的參數(shù)，在建圖過程中則表示無人車位姿，不包括一般SLAM問題中的路標(biāo)點位姿；邊表示各種約束，例如無人車不同位姿之間的相對轉(zhuǎn)換關(guān)系。為減少位姿的累計漂移，提高三維點云地圖構(gòu)建精度，在由激光里程計獲得的地圖數(shù)據(jù)構(gòu)建的位姿圖基礎(chǔ)上，添加閉環(huán)檢測，得到閉環(huán)約束，然后通過圖優(yōu)化算法計算出最優(yōu)位姿。閉環(huán)檢測又稱為回環(huán)檢測，即檢測無人車是否回到之前的位置，通過閉環(huán)檢測可將當(dāng)前關(guān)鍵幀與存在閉環(huán)的歷史關(guān)鍵幀進(jìn)行配準(zhǔn)，以得到閉環(huán)位姿。將閉環(huán)位姿構(gòu)成的閉環(huán)約束作為約束邊添加到位姿圖中。添加閉環(huán)約束的圖優(yōu)化原理如圖1所示。

（52）合葉裂齒苔Odontoschisma denudatum（Dumort.）Dumort. 李粉霞等（2011）

圖1 圖優(yōu)化原理Fig. 1 Graph optimization principle

位姿圖中的位姿頂點和觀測約束邊共同構(gòu)成一個圖優(yōu)化問題，該問題本質(zhì)上就是一個最小二乘問題。令qi（i=0，1，…，N，N為位姿頂點序號）為無人車的位姿頂點，zi和Hi分別為位姿頂點qi的觀測值及觀測值對應(yīng)的信息矩陣。無人車位姿頂點qi和觀測值zi之間的誤差函數(shù)ei（qi，zi）即位姿圖中的約束邊。則優(yōu)化問題的總體目標(biāo)函數(shù)為

當(dāng)前針對最小二乘問題的求解，主要采用高斯牛頓（Gauss-Newton，GN）算法或列文伯格-馬夸爾特（Levenberg-Marquardt，LM）算法。GN算法針對非線性目標(biāo)函數(shù)采用二階泰勒展開方法，泰勒展開通常只在展開點的近似效果較好。LM算法通過添加信任區(qū)域?qū)N算法進(jìn)行改進(jìn)，使得目標(biāo)函數(shù)在信任區(qū)域內(nèi)的近似都有效。LM算法比GN算法魯棒性更好，得到的結(jié)果更加準(zhǔn)確，因此本文使用LM算法來求解整個優(yōu)化問題?；趫D優(yōu)化的三維點云地圖構(gòu)建算法減少了地圖的局部偏移，提高了建圖精度，將為ESKF融合定位部分提供準(zhǔn)確的三維點云地圖。

2 ESKF融合定位

通過慣性導(dǎo)航解算（即對IMU輸出角速度、加速度信息進(jìn)行解算），得到當(dāng)前時刻無人車的先驗位姿信息（即無人車的位置、姿態(tài)和速度）。本文中，設(shè)定世界坐標(biāo)系為w系，載體坐標(biāo)系為b系，根據(jù)三維運動的微分性質(zhì)[15]有

式中：R為b系到w系的旋轉(zhuǎn)矩陣，即無人車的姿態(tài)；[·]×為向量的反對稱矩陣；ω，a分別為IMU測量的角速度和加速度；v，p分別為無人車的速度和位置；g為重力加速度。

通過慣性導(dǎo)航解算可得到無人車的先驗位姿信息，但解算過程中誤差會隨著時間不斷累計。為得到更加準(zhǔn)確的無人車姿態(tài)、速度和位置信息，采用ESKF融合定位對得到的IMU先驗位姿的狀態(tài)變量進(jìn)行修正，需要預(yù)先建立慣性導(dǎo)航誤差方程（式（7））。該方程由三維運動微分方程推導(dǎo)得到。

式中：Δθ，Δv，Δp分別為無人車的姿態(tài)、速度和位置誤差；bω，Δbω，nω分別為陀螺儀的零偏、零偏誤差和白噪聲；ba，Δba，na分別為加速度計的零偏、零偏誤差和白噪聲。

使用預(yù)先構(gòu)建的慣性導(dǎo)航誤差方程對無人車的狀態(tài)變量誤差進(jìn)行時間更新，設(shè)狀態(tài)變量x=[p v R babω]T，狀態(tài)變量誤差Δx=[ΔpΔvΔθΔbaΔbω]T，系統(tǒng)高斯噪聲w=[nanωnbanbω]T， 其中nba為加速度計的零偏不穩(wěn)定噪聲，nbω為陀螺儀的零偏不穩(wěn)定噪聲。由慣性導(dǎo)航誤差方程和IMU模型，構(gòu)建濾波器誤差方程：

式中：F為狀態(tài)變量誤差轉(zhuǎn)移矩陣；B為系統(tǒng)噪聲雅可比矩陣。

式 中：O6×3，O3×3為零矩陣；I3×3為單位矩陣；=a-ba；=ω-bω。

對狀態(tài)變量誤差進(jìn)行時間更新后，再對其進(jìn)行觀測更新，即對基于地圖的掃描配準(zhǔn)數(shù)據(jù)與觀測方程得到的觀測值進(jìn)行卡爾曼濾波。觀測方程為

式中：y為狀態(tài)變量誤差的觀測值；G為觀測矩陣；C為觀測噪聲轉(zhuǎn)移矩陣；n為觀測噪聲。

通過狀態(tài)變量誤差Δx對 狀態(tài)變量x進(jìn)行修正，獲得無人車后驗位姿，實現(xiàn)對傳感器參數(shù)誤差的補(bǔ)償。

3 實驗分析

3.1 實驗環(huán)境和硬件平臺

為全面客觀地評估本文提出的三維點云地圖構(gòu)建和ESKF融合定位方法的性能，針對2組不同數(shù)據(jù)展開實驗分析。采用由德國卡爾斯魯厄理工學(xué)院和豐田美國技術(shù)研究院聯(lián)合創(chuàng)辦的KITTI數(shù)據(jù)集對建圖及融合定位效果進(jìn)行分析；在校園內(nèi)開展無人車的三維點云地圖構(gòu)建和融合定位實驗，數(shù)據(jù)采集環(huán)境如圖2所示，通過定位軌跡評估融合定位效果。數(shù)據(jù)采集工作由自行搭建的數(shù)據(jù)采集平臺（圖3）完成，該平臺搭載了Xsens MTi-300 IMU和Velodyne VLP-16激光雷達(dá)，采用軟同步方式進(jìn)行時間對齊。校園實測數(shù)據(jù)采集時間約為1 000 s，平臺運行速度約為2 m/s，總距離約為2 km。激光雷達(dá)和IMU的外參數(shù)（即相對位姿）是數(shù)據(jù)融合的前提條件，外參標(biāo)定在設(shè)置了特殊靶標(biāo)的結(jié)構(gòu)化環(huán)境中預(yù)先進(jìn)行，因此在數(shù)據(jù)處理中將外參數(shù)作為已知量。

圖2 數(shù)據(jù)采集環(huán)境Fig. 2 Data collection environment

圖3 數(shù)據(jù)采集平臺Fig. 3 Data collection platform

3.2 三維點云地圖構(gòu)建實驗

為驗證本文三維點云地圖構(gòu)建算法的有效性，與以單一特征進(jìn)行匹配的建圖算法（以下簡稱單一特征建圖算法）進(jìn)行對比實驗。

不同算法基于KITTI數(shù)據(jù)集構(gòu)建的三維點云地圖如圖4所示?？煽闯雠c單一特征建圖算法相比，采用本文算法得到的點云地圖的重影部分明顯減少且輪廓清晰，其中矩形標(biāo)記部分對比最為明顯。KITTI數(shù)據(jù)集的實驗結(jié)果表明，采用本文算法可有效提高大規(guī)模點云匹配準(zhǔn)確率，降低累計誤差和局部地圖的偏移，對地圖一致性的提升具有重要作用。

圖4 基于 KITTI 數(shù)據(jù)集的三維點云地圖Fig. 4 3D point cloud map based on KITTI dataset

不同算法基于校園實測數(shù)據(jù)構(gòu)建的三維點云地圖如圖5、圖6所示（矩形框處為無人車沿道路行駛軌跡的閉環(huán)位置）。

圖5 單一特征建圖算法構(gòu)建的三維點云地圖Fig. 5 3D point cloud map constructed by single characteristic mapping algorithm

圖6 本文算法構(gòu)建的三維點云地圖Fig. 6 3D point cloud map constructed by the proposed algorithm

對比圖5（a）與圖6（a）發(fā)現(xiàn)，單一特征建圖算法未經(jīng)圖優(yōu)化閉環(huán)約束得到的三維點云地圖在矩形框處重力方向上出現(xiàn)明顯偏差；圖5（b）可發(fā)現(xiàn)道路的點云沿重力方向的反方向出現(xiàn)明顯偏移，而圖6（b）中道路的點云則在同一水平面上；通過圖5（c）與圖6（c）的對比則更加直觀地觀測到單一特征建圖算法未經(jīng)圖優(yōu)化閉環(huán)約束得到的三維點云地圖沿重力方向的反方向向上偏移。校園實測數(shù)據(jù)的實驗結(jié)果表明，本文算法在激光里程計得到的數(shù)據(jù)基礎(chǔ)上經(jīng)圖優(yōu)化添加閉環(huán)約束后，能夠有效處理室外大場景閉環(huán)造成的累計誤差，提升三維點云地圖質(zhì)量。

3.3 ESKF融合定位實驗

基于KITTI數(shù)據(jù)集的融合定位軌跡如圖7所示。使用KITTI數(shù)據(jù)集中的GNSS數(shù)據(jù)作為定位結(jié)果的真值，選取行駛距離約為1 300 m時，基于地圖匹配的定位方法和ESKF融合定位方法得到的定位數(shù)據(jù)（每隔100 m選取1個定位數(shù)據(jù)）分別與真值進(jìn)行對比，得到的相對位姿誤差如圖8所示（橫坐標(biāo)為按順序選取定位數(shù)據(jù)的序號），具體統(tǒng)計結(jié)果見表1。從圖8和表1可看出，與基于地圖匹配的定位方法相比，ESKF融合定位軌跡相對位姿誤差最大值減小了0.176 9 m，平均誤差減小了0.027 1 m，表明定位精度有所提升，且均方根誤差減小了0.059 4 m，表明誤差波動范圍較小。

圖7 基于 KITTI 數(shù)據(jù)集的融合定位軌跡Fig. 7 Fusion positioning trajectory based on KITTI dataset

圖8 不同定位方法的相對位姿誤差對比Fig. 8 Comparison of relative pose error of different positioning methods

表1 定位軌跡誤差統(tǒng)計結(jié)果Table 1 Positioning trajectory error statistics m

基于校園實測數(shù)據(jù)的融合定位軌跡如圖9所示?；诘貓D匹配的定位方法和ESKF融合定位方法得到的定位軌跡對比如圖10所示。數(shù)據(jù)采集平臺實際工作時，沿圖10矩形框標(biāo)記路段的同一路線行駛2次，而圖10（a）中矩形框標(biāo)記的同一行駛路線的2條定位軌跡并沒有完全重合，圖10（b）中同一行駛路線的2條定位軌跡則完全重合。經(jīng)對比發(fā)現(xiàn)，基于地圖匹配的定位方法在無人車旋轉(zhuǎn)時出現(xiàn)了配準(zhǔn)偏差，造成2條定位軌跡不重合，而ESKF融合定位方法的定位精度及穩(wěn)定性更優(yōu)。

圖9 基于校園實測數(shù)據(jù)的融合定位軌跡Fig. 9 Fusion positioning trajectory based on campus measured data

圖10 不同定位方法的定位軌跡對比Fig. 10 Comparison of positioning trajectories of different positioning methods

4 結(jié)論

（1） 針對三維點云地圖構(gòu)建過程中累計誤差較大的問題，通過激光里程計得到的數(shù)據(jù)來構(gòu)建位姿圖中的基本頂點和約束邊，并添加閉環(huán)約束來優(yōu)化位姿圖。通過優(yōu)化后的位姿圖構(gòu)建三維點云地圖，地圖重影部分明顯減少。

（2） 針對基于地圖匹配的無人車定位方法精度差且在無人車旋轉(zhuǎn)時配準(zhǔn)偏差較大的問題，采用ESKF融合定位方法，通過融合IMU數(shù)據(jù)和三維點云地圖數(shù)據(jù)，實現(xiàn)對無人車先驗位姿的修正并輸出后驗位姿。

（3） 實驗結(jié)果表明，基于三維點云地圖和ESKF的無人車融合定位方法能夠有效降低相對位姿誤差，在定位方面具有更穩(wěn)定可靠的表現(xiàn)。下一步將在ESKF融合定位的基礎(chǔ)上融合更多觀測信息，如編碼器測量數(shù)據(jù)，進(jìn)一步提升無人車定位精度。