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

(上海交通大學(xué) 自動(dòng)化系； 上海市北斗導(dǎo)航與位置服務(wù)重點(diǎn)實(shí)驗(yàn)室， 上海 200240)

智能車(chē)作為新的研究熱潮，引起了學(xué)術(shù)界和工業(yè)界的關(guān)注.全局定位是智能車(chē)進(jìn)行自主導(dǎo)航的基礎(chǔ).傳統(tǒng)方法將GPS、慣導(dǎo)和里程計(jì)數(shù)據(jù)進(jìn)行融合來(lái)實(shí)現(xiàn)定位[1].但是在城區(qū)環(huán)境下，GPS信號(hào)容易被高樓和樹(shù)木遮擋，產(chǎn)生多路徑效應(yīng)，甚至導(dǎo)致定位失敗.

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

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

1 地面SURF特征點(diǎn)檢測(cè)

本文使用環(huán)視相機(jī)獲得路面的圖像.通過(guò)對(duì)安裝在車(chē)輛前后左右4個(gè)方向上的相機(jī)進(jìn)行內(nèi)參標(biāo)定、去畸變、外參標(biāo)定和視角變換等諸多操作[8]，獲得車(chē)輛周?chē)访娴镍B(niǎo)瞰圖，如圖1(a)所示.

圖1 特征對(duì)比Fig.1 Features comparison

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

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

(1)

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

(2)

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

det(Happr)=DxxDyy-(wDxy)2

(3)

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

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

2 SURF點(diǎn)地圖構(gòu)建

SURF地圖的構(gòu)建可以描述為一個(gè)對(duì)世界坐標(biāo)系下SURF點(diǎn)集求并集的過(guò)程：

(4)

式中：Ft為t時(shí)刻世界坐標(biāo)系下的SURF點(diǎn)集.建圖流程如圖2所示.

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

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

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

(5)

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

(6)

式中:s為一個(gè)像素代表的實(shí)際距離，為固定值;下標(biāo)v、t和n分別為車(chē)輛坐標(biāo)系、時(shí)間和第n個(gè)特征點(diǎn).

圖3(b)定義了車(chē)輛坐標(biāo)系與世界坐標(biāo)系.假設(shè)車(chē)輛是在2D平面運(yùn)動(dòng)，若t時(shí)刻車(chē)輛在世界坐標(biāo)系下的位姿為[xtytφt]T，則第n個(gè)特征點(diǎn)在世界坐標(biāo)系下的坐標(biāo)[xw,t,nyw,t,n]T為

(7)

圖3 坐標(biāo)系Fig.3 Coordinate system

3 在線(xiàn)定位

在線(xiàn)定位通過(guò)將環(huán)視相機(jī)中提取出來(lái)的特征點(diǎn)集與先驗(yàn)地圖點(diǎn)集進(jìn)行匹配，從而確定車(chē)輛在世界坐標(biāo)系下的位置.為了進(jìn)一步提高精度，將匹配結(jié)果與慣導(dǎo)里程計(jì)數(shù)據(jù)通過(guò)EKF進(jìn)行濾波.定位流程如圖4所示.

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

3.1 基于ICP的地圖匹配

ICP是一種廣泛應(yīng)用于點(diǎn)集配準(zhǔn)問(wèn)題的方法[10]，該方法的實(shí)質(zhì)是基于最小二乘法的最優(yōu)匹配算法.其基本步驟為：

(1) 計(jì)算兩個(gè)點(diǎn)集中點(diǎn)之間的對(duì)應(yīng)關(guān)系；

(2) 計(jì)算一個(gè)變換矩陣，能使對(duì)應(yīng)點(diǎn)之間的距離函數(shù)最小.

重復(fù)迭代上述過(guò)程，直到距離函數(shù)小于設(shè)定值.本文中采用的距離函數(shù)為點(diǎn)到點(diǎn)之間的歐氏距離：

(8)

(9)

式中：φt為偏航角；[xtyt]T為t時(shí)刻車(chē)輛在世界坐標(biāo)系下的坐標(biāo).

3.2 基于EKF的定位濾波

(10)

EKF的狀態(tài)方程可表示為

式中：Ut為控制輸入，具體為車(chē)輛的速度和角速度；Zt為觀測(cè)值，具體為ICP的匹配結(jié)果；Q為過(guò)程噪聲，主要受慣導(dǎo)和里程計(jì)影響;R為測(cè)量噪聲，主要受ICP匹配過(guò)程的影響.

4 實(shí)驗(yàn)結(jié)果與分析

4.1 實(shí)驗(yàn)平臺(tái)介紹

本文使用如圖5所示的實(shí)驗(yàn)車(chē)進(jìn)行實(shí)驗(yàn)，該實(shí)驗(yàn)車(chē)配備了高精度GPS、環(huán)視相機(jī)、慣導(dǎo)、里程計(jì)和前視相機(jī)等傳感器.計(jì)算平臺(tái)為一臺(tái)配備了i5-6300HQ和GTX960M的筆記本電腦.實(shí)驗(yàn)使用的環(huán)視相機(jī)幀率為 12.5 f/s，分辨率為400像素×700像素，使用基于CUDA加速的開(kāi)源計(jì)算機(jī)視覺(jué)庫(kù)Opencv 2.4.13 進(jìn)行SURF特征檢測(cè)，單幀檢測(cè)時(shí)間為16 ms，可以滿(mǎn)足實(shí)時(shí)性要求.在實(shí)驗(yàn)過(guò)程中，車(chē)輛的速度約為30 km/h，行駛距離為 2.5 km.

圖5 實(shí)驗(yàn)平臺(tái)Fig.5 Experiment platform

4.2 建圖的實(shí)驗(yàn)結(jié)果

實(shí)驗(yàn)車(chē)沿實(shí)驗(yàn)路徑行駛，同時(shí)記錄下高精度GPS的數(shù)據(jù)和環(huán)視相機(jī)的鳥(niǎo)瞰圖.離線(xiàn)處理時(shí)，從鳥(niǎo)瞰圖中提取出t時(shí)刻的SURF點(diǎn)集，通過(guò)式(4)～式(7)生成全局的特征點(diǎn)云地圖，如圖6所示.

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

4.3 定位的實(shí)驗(yàn)結(jié)果

本文在真實(shí)場(chǎng)景進(jìn)行了在線(xiàn)定位實(shí)驗(yàn).圖7(a)為實(shí)驗(yàn)路徑，從起點(diǎn)出發(fā)，逆時(shí)針行進(jìn)經(jīng)過(guò)4次左轉(zhuǎn)和3個(gè)直行路口.圖中：坐標(biāo)系為世界坐標(biāo)系，星型點(diǎn)為路口入口.圖7(b)為圖7(a)中黑色方框標(biāo)注區(qū)域的地圖匹配結(jié)果.本文截取車(chē)輛周?chē)霃?0 m的點(diǎn)云作為局部SURF地圖.黑色的軌線(xiàn)為定位真值，白色的軌線(xiàn)為本算法的定位結(jié)果.

圖7 實(shí)驗(yàn)路徑和地圖匹配結(jié)果Fig.7 Detailed trajectory and matching result

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

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

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

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

方法縱向定位/m均值標(biāo)準(zhǔn)差橫向定位/m均值標(biāo)準(zhǔn)差偏航角/(°)均值標(biāo)準(zhǔn)差本文方法0.1830.360-0.1000.198-0.1040.612車(chē)道線(xiàn)方法[8]0.5390.607-0.0620.166-0.1460.514

5 結(jié)論

本文提出了一種使用環(huán)視相機(jī)構(gòu)建地面SURF地圖進(jìn)行在線(xiàn)定位的方法.針對(duì)現(xiàn)有的定位方法需要提取車(chē)道線(xiàn)和停車(chē)線(xiàn)，本文提出直接提取地面更廣義的SURF點(diǎn)構(gòu)建地圖，再進(jìn)行匹配定位，具有更高的魯棒性和準(zhǔn)確性.真車(chē)實(shí)驗(yàn)結(jié)果表明，定位精度可以達(dá)到dm級(jí).未來(lái)將在解決初始定位依賴(lài)高精度GPS這一問(wèn)題進(jìn)行研究.