基于地面快速魯棒特征的智能車全局定位方法

胡 兵, 楊 明, 郭林棟, 王春香, 王 冰

(上海交通大學 自動化系； 上海市北斗導航與位置服務重點實驗室， 上海 200240)

智能車作為新的研究熱潮，引起了學術界和工業界的關注.全局定位是智能車進行自主導航的基礎.傳統方法將GPS、慣導和里程計數據進行融合來實現定位[1].但是在城區環境下，GPS信號容易被高樓和樹木遮擋，產生多路徑效應，甚至導致定位失敗.

近些年，使用激光雷達和攝像頭進行定位均取得了良好的效果，但是激光雷達價格昂貴，體積巨大，目前難以實用化[2].基于視覺的定位方法具有特征豐富的優點，且攝像頭體積小，成本低，易于推廣.視覺定位方法主要有2種：一是基于視覺特征點的定位方法；二是基于車道線等地面語義特征的定位方法.文獻[3-4]中均采用基于視覺特征點的定位方法，其中：文獻[3]中將在線圖像與數據庫中的圖像進行快速魯棒特征 (SURF)匹配，但是數據庫存儲的是原圖，數據量巨大，并且車輛的行駛方向要與數據庫采集時的行駛方向一致；文獻[4]中使用雙目相機和高精度GPS構建帶有深度信息的快速魯棒特征SURF地圖，在線定位時使用低精度GPS和單目相機，但是雙目相機的深度精度隨著距離的增大而顯著降低.這些方法均采用前視相機，城市環境中的車輛、行人等動態目標會對定位產生巨大干擾.當動態目標占據前視相機大部分視野時，難以實現定位.文獻[5-8]中均采用基于車道線特征的定位方法，其中：文獻[5]中將車道線與開放街道地圖(OSM)的道路形狀進行配準來實現定位；文獻[6]中先構建車道線地圖，再進行匹配定位；文獻[7]中使用雙目相機檢測路面，改善了車道線的檢測效果，但是該方法只是對GPS定位的改善，在GPS失效的情況下不能使用；文獻[8]中使用環視相機獲取近處地面的圖像，可以抵抗動態目標的干擾.以上這些方法均需要檢測車道線，但是容易受到箭頭和斑馬線等其他標志的影響.

綜上所述，本文提出一種基于地面SURF地圖進行車輛全局定位的方法.該定位方法包含3部分：地面SURF點提取并構建地圖、匹配定位和定位濾波.首先從環視相機的鳥瞰圖中檢測出SURF點，結合高精度GPS構建SURF地圖.再使用迭代最近點(Iterative Closest Point, ICP)算法，將在線檢測結果與SURF地圖匹配獲得車輛全局定位，并通過擴展卡爾曼濾波(Extended Kalman Filter, EKF)將定位結果、慣導和里程計數據進行融合，提高定位精度.

1 地面SURF特征點檢測

本文使用環視相機獲得路面的圖像.通過對安裝在車輛前后左右4個方向上的相機進行內參標定、去畸變、外參標定和視角變換等諸多操作[8]，獲得車輛周圍路面的鳥瞰圖，如圖1(a)所示.

圖1 特征對比Fig.1 Features comparison

SURF是一種局部特征，具有旋轉、尺度不變性，對光照變化和仿射、透視變換也具有較強的魯棒性[9].與SIFT算法相比，SURF加入了Hessian近似矩陣和積分圖像，降低了計算的復雜度，提高了速度.

設圖像函數為I(X)，圖像左上頂點設為原點；M=(u,v)為圖像中的任一像素，設為右下頂點.原點與右下頂點構成的矩形區域所有像素的和即為積分圖像，可表示為

(1)

SURF算法需要計算Hessian近似矩陣行列式的局部極大值和局部極小值，所得的極大值和極小值所在的位置是SURF檢測的依據.取圖像函數I(X)中任意一點為M=(u,v)，σ為選用的尺度大小，則M=(u,v)在尺度σ上的Hessian近似矩陣可定義為

(2)

式中：Lxx(M,σ)為二階偏導數在函數點M和圖像函數I(X)之間所做的卷積;Lxy(M,σ)和Lyy(M,σ)有一樣意義.為了計算的高效，使用Dxx、Dxy和Dyy替代Lxx、Lxy和Lyy，則Hessian近似矩陣Happr的行列式可以表達為

det(Happr)=DxxDyy-(wDxy)2

(3)

式中：w為權重系數.根據式(3)，記錄圖像任意點處響應的計算結果值，得到不同大小尺度σ下的響應值.對得到的響應值進行非最大值抑制，選出特征候選點對應的極值點.再采用三維二次函數擬合的方法，得到特征點的具體位置.

圖1(b)中黑點為SURF點，可以看出SURF點集中分布在車道線，箭頭等地面標志上，與用圖1(c)所示的黑色直線描述的車道線相比，每個SURF均獨立，這有利于提高縱向定位精度.

2 SURF點地圖構建

SURF地圖的構建可以描述為一個對世界坐標系下SURF點集求并集的過程：

(4)

式中：Ft為t時刻世界坐標系下的SURF點集.建圖流程如圖2所示.

圖2 建圖流程Fig.2 Process of map building

圖3(a)定義了像素坐標系和車輛坐標系.像素坐標系原點位于圖像左上角，車輛坐標系原點位于車輛后輪中心.假設車輛后輪中心點在像素坐標系下的坐標為[uovo]T，t時刻第n個特征點在像素坐標系下的坐標為[unvn]T,則其在車輛坐標系下的坐標[xv,t,nyv,t,n]T可以表示為

xv,t,n=(vo-vn)s

(5)

yv,t,n=(vo-vn)s

(6)

式中:s為一個像素代表的實際距離，為固定值;下標v、t和n分別為車輛坐標系、時間和第n個特征點.

圖3(b)定義了車輛坐標系與世界坐標系.假設車輛是在2D平面運動，若t時刻車輛在世界坐標系下的位姿為[xtytφt]T，則第n個特征點在世界坐標系下的坐標[xw,t,nyw,t,n]T為

(7)

圖3 坐標系Fig.3 Coordinate system

3 在線定位

在線定位通過將環視相機中提取出來的特征點集與先驗地圖點集進行匹配，從而確定車輛在世界坐標系下的位置.為了進一步提高精度，將匹配結果與慣導里程計數據通過EKF進行濾波.定位流程如圖4所示.

圖4 定位流程Fig.4 Process of localization

3.1 基于ICP的地圖匹配

ICP是一種廣泛應用于點集配準問題的方法[10]，該方法的實質是基于最小二乘法的最優匹配算法.其基本步驟為：

(1) 計算兩個點集中點之間的對應關系；

(2) 計算一個變換矩陣，能使對應點之間的距離函數最小.

重復迭代上述過程，直到距離函數小于設定值.本文中采用的距離函數為點到點之間的歐氏距離：

(8)

(9)

式中：φt為偏航角；[xtyt]T為t時刻車輛在世界坐標系下的坐標.

3.2 基于EKF的定位濾波

(10)

EKF的狀態方程可表示為

式中：Ut為控制輸入，具體為車輛的速度和角速度；Zt為觀測值，具體為ICP的匹配結果；Q為過程噪聲，主要受慣導和里程計影響;R為測量噪聲，主要受ICP匹配過程的影響.

4 實驗結果與分析

4.1 實驗平臺介紹

本文使用如圖5所示的實驗車進行實驗，該實驗車配備了高精度GPS、環視相機、慣導、里程計和前視相機等傳感器.計算平臺為一臺配備了i5-6300HQ和GTX960M的筆記本電腦.實驗使用的環視相機幀率為 12.5 f/s，分辨率為400像素×700像素，使用基于CUDA加速的開源計算機視覺庫Opencv 2.4.13 進行SURF特征檢測，單幀檢測時間為16 ms，可以滿足實時性要求.在實驗過程中，車輛的速度約為30 km/h，行駛距離為 2.5 km.

圖5 實驗平臺Fig.5 Experiment platform

4.2 建圖的實驗結果

實驗車沿實驗路徑行駛，同時記錄下高精度GPS的數據和環視相機的鳥瞰圖.離線處理時，從鳥瞰圖中提取出t時刻的SURF點集，通過式(4)～式(7)生成全局的特征點云地圖，如圖6所示.

圖6 全局SURF地圖Fig.6 Global SURF map

4.3 定位的實驗結果

本文在真實場景進行了在線定位實驗.圖7(a)為實驗路徑，從起點出發，逆時針行進經過4次左轉和3個直行路口.圖中：坐標系為世界坐標系，星型點為路口入口.圖7(b)為圖7(a)中黑色方框標注區域的地圖匹配結果.本文截取車輛周圍半徑60 m的點云作為局部SURF地圖.黑色的軌線為定位真值，白色的軌線為本算法的定位結果.

圖7 實驗路徑和地圖匹配結果Fig.7 Detailed trajectory and matching result

縱向定位誤差曲線如圖8(a)所示，在7次進入路口的過程中有5次的縱向定位誤差均保持在 ±0.2 m.需要指出，在通過路口的過程中，地面缺少SURF特征點，定位更依賴于歷史點云匹配和慣導、里程計的航位推算，因此出現了幾次縱向誤差曲線上的峰值.橫向定位誤差如圖8(c)所示，在直道上定位誤差保持在 ±0.1 m.與縱向定位結果類似，因為路口中缺少觀測，定位結果多依賴于航位推算，存在累計誤差，因此路口點后出現了橫向誤差曲線上的峰值.進入路段后，誤差會重新收斂.偏航角誤差如圖8(e)所示，與橫向定位誤差相似，在路段中的誤差保持在 ±1°，而在路口中的誤差由于缺少觀測，存在累計誤差，進入路段后，誤差會重新收斂.圖8(b)、(d)、(f)為本文方法定位結果的概率直方圖.可以看出，橫向定位誤差大部分小于 0.2 m，偏航角定位誤差大部分小于1°.

本文對比了基于SURF地圖和車道線地圖的定位方法[8]，如表1所示.實驗結果表明，本文提出方法在縱向精度上有了明顯提高，這主要是由于SURF點具有廣義性，箭頭、斑馬線和井蓋等元素都能形成SURF點，提供縱向的定位信息.兩者橫向定位精度接近，這主要是因為這2種方法均利用的是車道線產生的特征進行橫向定位.本文方法利用歷史SURF點，在換道場景下也能穩定地輸出定位結果，且精度與車道線定位方法相當.從實驗結果可以看出，本文提出的方法能夠滿足智能車在城市環境下的全局定位需求.

圖8 基于SURF地圖的定位結果Fig.8 Localization results based on SURF

表12種方法的定位誤差
Tab.1Localizationerrorsoftwomethods

方法縱向定位/m均值標準差橫向定位/m均值標準差偏航角/(°)均值標準差本文方法0.1830.360-0.1000.198-0.1040.612車道線方法[8]0.5390.607-0.0620.166-0.1460.514

5 結論

本文提出了一種使用環視相機構建地面SURF地圖進行在線定位的方法.針對現有的定位方法需要提取車道線和停車線，本文提出直接提取地面更廣義的SURF點構建地圖，再進行匹配定位，具有更高的魯棒性和準確性.真車實驗結果表明，定位精度可以達到dm級.未來將在解決初始定位依賴高精度GPS這一問題進行研究.