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

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

（1. 國能準能集團有限責任公司 機電管理部，內蒙古 鄂爾多斯 010300；2. 中國礦業大學（北京） 機電與信息工程學院，北京 100083）

0 引言

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

先驗地圖構建是無人車實現地圖匹配定位的重要環節。常用的地圖包括二維柵格地圖和三維點云地圖。通過二維柵格地圖可直接獲取無人車的可行駛區域，但該地圖對周圍環境特征信息的描述較少，導致匹配定位精度低，且容易失效[6]。三維點云地圖的采集范圍更廣，環境特征信息豐富，不僅可用于環境感知、路徑規劃，還可用于高精度的無人車定位。相較于相機，三維激光雷達通過主動發射激光束來直接獲得與周圍環境特征之間的距離信息，數據更加準確，且激光雷達幾乎不受光照變化的影響。這些優良特性使得激光雷達常被用于制作精度較高的三維點云地圖。基于高精度三維點云地圖的無人車定位系統可以擺脫對衛星信號的依賴，不受外界環境因素的干擾。

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

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

1 三維點云地圖構建

1.1 激光點云匹配算法

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

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

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

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

將所有立方體網格計算出來的概率相加，得到NDT配準得分[14]：

（3） 采用牛頓優化算法對NDT配準得分進行優化，得到最佳的變換參數k，使得配準得分最大。

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

1.2 圖優化算法

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

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

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

圖1 圖優化原理Fig. 1 Graph optimization principle

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

當前針對最小二乘問題的求解，主要采用高斯牛頓（Gauss-Newton，GN）算法或列文伯格-馬夸爾特（Levenberg-Marquardt，LM）算法。GN算法針對非線性目標函數采用二階泰勒展開方法，泰勒展開通常只在展開點的近似效果較好。LM算法通過添加信任區域對GN算法進行改進，使得目標函數在信任區域內的近似都有效。LM算法比GN算法魯棒性更好，得到的結果更加準確，因此本文使用LM算法來求解整個優化問題。基于圖優化的三維點云地圖構建算法減少了地圖的局部偏移，提高了建圖精度，將為ESKF融合定位部分提供準確的三維點云地圖。

2 ESKF融合定位

通過慣性導航解算（即對IMU輸出角速度、加速度信息進行解算），得到當前時刻無人車的先驗位姿信息（即無人車的位置、姿態和速度）。本文中，設定世界坐標系為w系，載體坐標系為b系，根據三維運動的微分性質[15]有

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

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

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

使用預先構建的慣性導航誤差方程對無人車的狀態變量誤差進行時間更新，設狀態變量x=[p v R babω]T，狀態變量誤差Δx=[ΔpΔvΔθΔbaΔbω]T，系統高斯噪聲w=[nanωnbanbω]T， 其中nba為加速度計的零偏不穩定噪聲，nbω為陀螺儀的零偏不穩定噪聲。由慣性導航誤差方程和IMU模型，構建濾波器誤差方程：

式中：F為狀態變量誤差轉移矩陣；B為系統噪聲雅可比矩陣。

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

對狀態變量誤差進行時間更新后，再對其進行觀測更新，即對基于地圖的掃描配準數據與觀測方程得到的觀測值進行卡爾曼濾波。觀測方程為

式中：y為狀態變量誤差的觀測值；G為觀測矩陣；C為觀測噪聲轉移矩陣；n為觀測噪聲。

通過狀態變量誤差Δx對 狀態變量x進行修正，獲得無人車后驗位姿，實現對傳感器參數誤差的補償。

3 實驗分析

3.1 實驗環境和硬件平臺

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

圖2 數據采集環境Fig. 2 Data collection environment

圖3 數據采集平臺Fig. 3 Data collection platform

3.2 三維點云地圖構建實驗

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

不同算法基于KITTI數據集構建的三維點云地圖如圖4所示。可看出與單一特征建圖算法相比，采用本文算法得到的點云地圖的重影部分明顯減少且輪廓清晰，其中矩形標記部分對比最為明顯。KITTI數據集的實驗結果表明，采用本文算法可有效提高大規模點云匹配準確率，降低累計誤差和局部地圖的偏移，對地圖一致性的提升具有重要作用。

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

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

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

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

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

3.3 ESKF融合定位實驗

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

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

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

表1 定位軌跡誤差統計結果Table 1 Positioning trajectory error statistics m

基于校園實測數據的融合定位軌跡如圖9所示。基于地圖匹配的定位方法和ESKF融合定位方法得到的定位軌跡對比如圖10所示。數據采集平臺實際工作時，沿圖10矩形框標記路段的同一路線行駛2次，而圖10（a）中矩形框標記的同一行駛路線的2條定位軌跡并沒有完全重合，圖10（b）中同一行駛路線的2條定位軌跡則完全重合。經對比發現，基于地圖匹配的定位方法在無人車旋轉時出現了配準偏差，造成2條定位軌跡不重合，而ESKF融合定位方法的定位精度及穩定性更優。

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

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

4 結論

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

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

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