毫米波雷達(dá)與視覺傳感器信息融合的車輛跟蹤

胡延平 劉 菲 魏振亞 趙林峰

1.合肥工業(yè)大學(xué)機(jī)械工程學(xué)院,合肥，2300092.合肥工業(yè)大學(xué)汽車與交通學(xué)院，合肥，230009

0 引言

在所有交通事故中，追尾事故占比最高，高達(dá)30%[1-2]。為減少追尾事故的發(fā)生，基于毫米波雷達(dá)檢測的跟蹤算法越來越多地應(yīng)用在車輛縱向防碰撞預(yù)警系統(tǒng)(forward collision warning system，F(xiàn)CWS)中，國內(nèi)外學(xué)者對此進(jìn)行了大量的研究。文獻(xiàn)[3-4]采用基于CV/CA(constant velo-city/constant acceleration)車輛運(yùn)動模型的卡爾曼濾波(Kalman filter，KF)算法對前方車輛的運(yùn)動狀態(tài)進(jìn)行跟蹤與預(yù)測，其中，文獻(xiàn)[4]解決了有雜波情況下的目標(biāo)濾波問題。文獻(xiàn)[5]針對車輛在行駛過程中經(jīng)常面臨的減速、超車等復(fù)雜交通狀況，采用基于當(dāng)前統(tǒng)計(jì)模型的自適應(yīng)卡爾曼濾波器對目標(biāo)進(jìn)行跟蹤，以期減小跟蹤誤差。車輛在行駛過程中，運(yùn)動狀態(tài)經(jīng)常發(fā)生改變，采用單一運(yùn)動模型模擬車輛運(yùn)動狀態(tài)是不現(xiàn)實(shí)的，因此文獻(xiàn)[6]提出了交互多模型算法，通過幾種運(yùn)動模型模擬目標(biāo)的運(yùn)動狀態(tài)，提高了跟蹤精度。基于雷達(dá)的目標(biāo)跟蹤算法具有一定可行性，但雷達(dá)不具備模式識別能力且雷達(dá)檢測結(jié)果存在虛假目標(biāo)，易產(chǎn)生誤報(bào)，造成駕駛員精神緊張。為彌補(bǔ)單傳感器的設(shè)計(jì)缺點(diǎn)與局限性，不少學(xué)者相繼提出多傳感器融合概念。文獻(xiàn)[7-10]利用毫米波雷達(dá)與攝像機(jī)的信息融合來檢測前方車輛，首先通過空間坐標(biāo)轉(zhuǎn)換將雷達(dá)目標(biāo)映射到攝像機(jī)圖像中，然后，確定雷達(dá)目標(biāo)在圖像中的目標(biāo)興趣區(qū)域(region of interest, ROI)，最后使用不同檢測算法在雷達(dá)ROI中進(jìn)行車輛檢測。文獻(xiàn)[8-10]提出不同算法來細(xì)化檢測車輛的邊界、校正雷達(dá)ROI位置，進(jìn)一步縮短車輛檢測時間。此類方法通過縮小目標(biāo)檢測區(qū)域，極大縮短目標(biāo)檢測時間，有效解決了視覺設(shè)備的目標(biāo)檢測實(shí)時性差的問題，但這種算法的性能受毫米波雷達(dá)能力的限制，一旦目標(biāo)被雷達(dá)遺漏，后面的檢測算法將不能檢測到車輛。

筆者提出一種基于毫米波雷達(dá)與視覺傳感器信息融合的車輛跟蹤方法。該方法采用雷達(dá)與視覺傳感器并行的策略，首先通過直接線性標(biāo)定算法對雷達(dá)與視覺傳感器進(jìn)行聯(lián)合標(biāo)定，確定二者的空間轉(zhuǎn)換矩陣，并基于視覺圖像提取Haar-like特征，利用Adaboost分類算法檢測車輛并生成車輛ROI；其次，雷達(dá)通過CAN總線采集目標(biāo)數(shù)據(jù)信息，通過提出的算法剔除干擾目標(biāo)后將獲取的雷達(dá)有效目標(biāo)投影至視覺圖像中并生成ROI，并分析雷達(dá)目標(biāo)ROI的對稱性，減小橫向位置誤差；然后采用提出的目標(biāo)匹配算法計(jì)算視覺傳感器獲取的車輛ROI與雷達(dá)目標(biāo)ROI的重合率，確定該目標(biāo)車輛(此類目標(biāo)車輛稱為融合車輛)是否同時被雷達(dá)與視覺傳感器檢測到；最后，通過提出的核相關(guān)濾波-卡爾曼濾波(kernalized correlation filters-Kalman filters，KCF-KF)組合濾波跟蹤算法對融合車輛進(jìn)行跟蹤。試驗(yàn)結(jié)果表明，該算法具有良好的環(huán)境適應(yīng)性與準(zhǔn)確率。

1 毫米波雷達(dá)有效目標(biāo)的獲取

采用綜合寬視角中距離和窄視角長距離的多模式Delphi ESR毫米波雷達(dá)最多可探測64個目標(biāo)，通過計(jì)算接收信號與發(fā)射信號的時間差來獲得探測目標(biāo)的相對距離、相對速度、方位角。雷達(dá)探測的任一目標(biāo)數(shù)據(jù)可描述為

x=(r,α,v)

(1)

式中，r為目標(biāo)與雷達(dá)的相對距離；α為目標(biāo)與雷達(dá)的方位角；v為目標(biāo)與雷達(dá)的相對速度。

則當(dāng)前時刻雷達(dá)采集的一幀數(shù)據(jù)可表示為

X=(x1,x2,…,xn)

(2)

根據(jù)獲取的雷達(dá)目標(biāo)特征，可將雷達(dá)目標(biāo)分為四類[11]：①非危險目標(biāo)，造成追尾事故的車輛往往是與本車較近的同車道車輛或相鄰車道車輛，較遠(yuǎn)或其他車道車輛的追尾風(fēng)險較小，屬于非危險目標(biāo)；②空目標(biāo)即相對距離為零的目標(biāo)；③虛假目標(biāo)，該類目標(biāo)在極短時間內(nèi)出現(xiàn)并消失，但實(shí)際上并不存在，屬于雷達(dá)測量產(chǎn)生的噪聲；④有效目標(biāo)，除上述目標(biāo)外的其他目標(biāo)。有效目標(biāo)外的其他三類目標(biāo)屬于干擾目標(biāo)，若不剔除，既會增加程序計(jì)算量，又會對駕駛員的預(yù)判產(chǎn)生干擾。先后通過有效目標(biāo)初選與設(shè)定目標(biāo)生命周期來對目標(biāo)進(jìn)行篩選，以改善獲取的目標(biāo)信息質(zhì)量。

1.1 有效目標(biāo)的初選

由圖1可知毫米波雷達(dá)探測范圍較廣，保留其原始探測范圍會在探測到更多目標(biāo)信息的同時，增加對非危險目標(biāo)的分析與處理工作量，延長數(shù)據(jù)處理時間。因此，通過約束雷達(dá)目標(biāo)的縱向、橫向范圍來減少對非危險目標(biāo)的處理是非常必要的。將雷達(dá)探測目標(biāo)的相對距離分解為相對縱向距離dY與相對橫向距離dX：

圖1 毫米波雷達(dá)掃描障礙物范圍示意圖

(3)

再通過設(shè)定橫向范圍Xdist與縱向范圍Ydist來約束dX和dY的范圍：

(4)

空目標(biāo)的特征是相對距離為0，相對速度為81.91 m/s，方位角為0°，可通過檢驗(yàn)?zāi)繕?biāo)相對距離是否大于0來剔除。

1.2 目標(biāo)有效性檢驗(yàn)

雷達(dá)工作時存在不穩(wěn)定性，可能會檢測出虛假目標(biāo)。另外，短暫出現(xiàn)在雷達(dá)掃描范圍內(nèi)的目標(biāo)也會被檢測到。為縮短對這兩類目標(biāo)的處理時間，提出目標(biāo)有效性檢驗(yàn)方法，具體步驟如下：

(1)確定待跟蹤目標(biāo)。設(shè)定目標(biāo)連續(xù)被探測到的次數(shù)FT、目標(biāo)連續(xù)丟失的次數(shù)LT，以及FT、LT的判別閾值TF與TL。FT與LT初始值均為0，現(xiàn)規(guī)定FT>TF的目標(biāo)為待跟蹤目標(biāo)。

(2)利用擴(kuò)展卡爾曼濾波算法預(yù)測下一周期目標(biāo)信息。Xn=(xn,yn,vx,n,vy,n)為描述目標(biāo)運(yùn)動的狀態(tài)向量，xn、yn、vx,n、vy,n分別為第n周期內(nèi)獲得的有效目標(biāo)的橫向相對距離、縱向相對距離、橫向相對速度和縱向相對速度。下一周期的目標(biāo)預(yù)測值為

(5)

其中，T為雷達(dá)掃描周期，T=50 ms；xn+1|n、yn+1|n、vx,n+1|n、vy,n+1|n為根據(jù)周期n計(jì)算所得的周期n+1狀態(tài)值。

(3)計(jì)算本周期目標(biāo)的狀態(tài)預(yù)測值與實(shí)際測量值的差值，判斷二者是否表示同一目標(biāo)，即

(6)

式中，xn+1、yn+1、vx,n+1、vy,n+1為本周期有效目標(biāo)實(shí)際測量值；Δx、Δy、Δvx、Δvy為目標(biāo)實(shí)際測量值與預(yù)測值之間的準(zhǔn)許誤差。

若是同一目標(biāo)，則對應(yīng)的FT加1；否則對應(yīng)的FT減1，LT加1。

(4)根據(jù)本周期每個目標(biāo)的FT與LT確定是否繼續(xù)跟蹤。若FT>TF且LTTL，則不再繼續(xù)跟蹤，重新確定跟蹤目標(biāo)。

毫米波雷達(dá)安裝于試驗(yàn)車保險杠，預(yù)先駕駛試驗(yàn)車在城市道路中采集原始目標(biāo)數(shù)據(jù)并將其存儲于.txt文件中。表1、表2所示分別為同一場景下雷達(dá)的原始目標(biāo)數(shù)據(jù)與有效目標(biāo)數(shù)據(jù)，圖2為對應(yīng)的雷達(dá)目標(biāo)圖像。由表1可知，ID為2、7和9的目標(biāo)短暫出現(xiàn)在雷達(dá)掃描范圍內(nèi)，ID為5、7、9、10、12的目標(biāo)的橫向距離均大于3 m，均屬非有效目標(biāo)，如圖2a所示。采用上述濾波算法獲取的有效目標(biāo)如圖2b所示，其中，設(shè)定參數(shù)Xdist=3 m，Ydist=150 m，TF=7，TL=20。對比表1、表2可知，該濾波算法可有效剔除超過橫向、縱向范圍的目標(biāo)與短暫出現(xiàn)在雷達(dá)視野中的目標(biāo)，減小了干擾目標(biāo)的影響，為后續(xù)的融合輸出穩(wěn)定可靠的目標(biāo)。

表1 雷達(dá)原始目標(biāo)數(shù)據(jù)

表2 雷達(dá)有效目標(biāo)數(shù)據(jù)

(a)原始雷達(dá)目標(biāo)圖像 (b)濾波后雷達(dá)目標(biāo)圖像

2 基于視覺的車輛檢測

采用Adaboost算法[11]對車輛進(jìn)行檢測，由于文獻(xiàn)[5]已充分闡述了基于Adaboost算法的車輛檢測器的形成過程，因此，下面僅簡述車輛檢測器訓(xùn)練步驟并展示訓(xùn)練效果。

基于Adaboost算法的車輛檢測主要包括3個步驟：正負(fù)樣本準(zhǔn)備、特征提取、訓(xùn)練分類器。首先，準(zhǔn)備正負(fù)樣本并完成樣本灰度化與歸一化處理，其中，所有樣本歸一化為相同尺寸(20pixel×20pixel)。正樣本為車輛圖片，以轎車為主，共3972張；負(fù)樣本為非車輛圖片，如樹木、柵欄、路牌等，共8543張。其次，分別從正負(fù)樣本中提取Haar-like矩形特征并存儲在文件中。然后基于Adaboost算法，從Haar-like特征中提取有效特征生成同等數(shù)量的弱分類器。最后，將弱分類器加權(quán)組合成強(qiáng)分類器。至此，一個車輛檢測器即可產(chǎn)生。

在天氣晴朗、光線良好的情況下采集3段城市道路視頻，以此測試車輛檢測器的識別正確率。視頻圖片輸入格式為.bmp，大小為640pixel×480pixel。表3所示為該算法基于3段視頻的檢測結(jié)果，車輛總數(shù)指視頻中出現(xiàn)的車輛總數(shù)；正確檢測數(shù)指被正確識別的車輛數(shù)；誤檢數(shù)指被錯誤識別的車輛總數(shù)；車輛正確識別率PZ指正確檢測數(shù)占車輛總數(shù)的比例，誤檢率PW指誤檢數(shù)占車輛總數(shù)的比例，PZ=91.28%，PW=2.54%說明基于Adaboost算法訓(xùn)練的車輛檢測器可有效識別前方車輛。

表3 檢測結(jié)果統(tǒng)計(jì)

3 雷達(dá)與視覺傳感器檢測結(jié)果融合

3.1 雷達(dá)與攝像機(jī)坐標(biāo)系標(biāo)定

傳統(tǒng)的坐標(biāo)轉(zhuǎn)換是基于小孔成像的逆向思想、將三維空間中的點(diǎn)映射到像素坐標(biāo)系的變換，其轉(zhuǎn)換矩陣依賴于攝像機(jī)標(biāo)定中的外參數(shù)矩陣與內(nèi)參數(shù)矩陣。由于外參數(shù)矩陣可能因車輛俯仰而改變，因此當(dāng)使用外參數(shù)矩陣計(jì)算世界坐標(biāo)系下的視覺目標(biāo)位置時，可能會出現(xiàn)較大誤差，所以，為了高效、方便地解決雷達(dá)與攝像機(jī)標(biāo)定問題，本文采用直接線性標(biāo)定算法[12]解決雷達(dá)與視覺傳感器間的對準(zhǔn)問題。

記錄雷達(dá)坐標(biāo)系與視覺坐標(biāo)系中標(biāo)記物的位置，在對標(biāo)記位置進(jìn)行多次測量的基礎(chǔ)上，得到標(biāo)記位置數(shù)據(jù)。假設(shè)從雷達(dá)與視覺傳感器共同視場捕獲M個雷達(dá)目標(biāo){Ri=(xi,yi)|i=1, 2, …，M}及其對應(yīng)的M個像素坐標(biāo){Ci=(ui,wi)|i=1, 2, …，M}。記錄M組坐標(biāo)數(shù)據(jù)并歸一化處理之后，進(jìn)行雷達(dá)坐標(biāo)與像素坐標(biāo)的轉(zhuǎn)換：

[uw1]T=H[xiyi1]T

(7)

其中，H為雷達(dá)與視覺傳感器的空間轉(zhuǎn)換矩陣。通過Aih=0求得矩陣A最小奇異值的奇異向量h=(h11,h12,h13,h21,h22,h23,h31,h32,h33)，其中：

Ai=[Ai1Ai2Ai3]T

(8)

3.2 雷達(dá)橫向位置的修正

毫米波雷達(dá)可精確探測目標(biāo)的相對縱向距離與相對速度，但其橫向探測能力較差，這是因?yàn)楹撩撞ǚ瓷潼c(diǎn)可出現(xiàn)在探測目標(biāo)上的任何位置。精確的橫向位置可使雷達(dá)ROI更完整地包圍目標(biāo)車輛，也是雷達(dá)目標(biāo)與視覺目標(biāo)準(zhǔn)確匹配的重要前提。

預(yù)先根據(jù)雷達(dá)與車輛的距離估計(jì)車輛ROI大小：以5 m為間隔，估算0～150 m范圍內(nèi)車輛在圖像中所占像素大小，然后，以雷達(dá)目標(biāo)投影到像素坐標(biāo)系中的位置坐標(biāo)為中心，畫出ROI。現(xiàn)假設(shè)雷達(dá)投影到像素坐標(biāo)系中的坐標(biāo)點(diǎn)為P(x0,y0)。由于雷達(dá)安裝在車輛保險杠，導(dǎo)致雷達(dá)反射回來的目標(biāo)位置并非車輛中心，而在車輛中心以下，所以需通過坐標(biāo)修正公式x1=x0,y1=y0-Δl獲得雷達(dá)目標(biāo)修正后的投影位置坐標(biāo)P(x1,y1)，其中，變量Δl隨目標(biāo)相對縱向距離變化而變化，需通過實(shí)驗(yàn)確定。

接下來分析雷達(dá)目標(biāo)ROI內(nèi)圖像的對稱性，找到目標(biāo)ROI的對稱軸，調(diào)整其在圖像中的橫向位置，使其中心與被探測目標(biāo)車輛的中心重合。ROI對稱性的分析可分為以下3個步驟：遮擋推理、對稱檢測及對稱檢驗(yàn)。

3.2.1遮擋推理

對象被遮擋時，視覺傳感器只能獲取其部分未被遮擋圖像，對此進(jìn)行對稱性分析，很容易出現(xiàn)對稱軸錯誤檢測，增加不必要的處理時間。

假設(shè)ROI1和ROI2分別為2個不同探測目標(biāo)對應(yīng)的感興趣區(qū)域，假設(shè)ROI1左上角與右上角的坐標(biāo)分別為(a1，b1)、(c1，d1)；ROI2左上角與右上角的坐標(biāo)分別為(a2，b2)、(c2，d2)。根據(jù)以下步驟，判斷ROI1和ROI2是否存在相互遮擋。

(1)判斷ROI1與ROI2是否相交。若ROI1與ROI2相交，則其相交結(jié)果必然是一個矩形。假設(shè)ROI1與ROI2的相交矩形為R，矩形R左上角與右上角的坐標(biāo)為(a，b)、(c，d)。參數(shù)a、b、c、d滿足

(9)

(2)ROI1與ROI2的相交面積Ajion1=(c-a)(d-b)，若Ajion>min(A1,A2)/3(A1、A2分別為ROI1和ROI2的面積)，則繼續(xù)執(zhí)行步驟(3)；否則判定ROI1與ROI2沒有相互遮擋。

(3)分別計(jì)算ROI1與ROI2的面積，并比較ROI1與ROI2相應(yīng)目標(biāo)的縱向距離。若該目標(biāo)ROI的面積較小且其對應(yīng)的縱向距離更大，則認(rèn)為擁有較小ROI的目標(biāo)被遮擋，否則未被遮擋。

3.2.2對稱檢測

(1)確定對稱軸搜索范圍。雷達(dá)橫向探測距離誤差較大，導(dǎo)致投影在像素坐標(biāo)系中的點(diǎn)可能會出現(xiàn)在車身任意位置，因此擴(kuò)大對稱軸搜索范圍以防止原ROI范圍內(nèi)不存在車輛對稱軸。現(xiàn)以原ROI為中心，在原ROI左右各擴(kuò)展一個與原ROI大小相同的ROI，將此區(qū)域命名為ROIS，ROIS即為對稱軸搜索范圍。

(2)對稱檢測。在對稱軸搜索范圍內(nèi)掃過與原ROI大小相同的窗口，掃描步長為D，利用求和歸一化互相關(guān)匹配(summed normalized cross-correlation，SNCC)算法計(jì)算每個位置的對稱相關(guān)值。對稱相關(guān)值的峰值所對應(yīng)的橫坐標(biāo)即為雷達(dá)目標(biāo)ROI對稱軸位置。

3.2.3對稱檢驗(yàn)

以對稱相關(guān)值的峰值位置為基準(zhǔn)，分別向上、下、左、右擴(kuò)展，形成與原ROI大小一致的區(qū)域，將該區(qū)域命名為ROID。

由圖3a可知，雷達(dá)反射點(diǎn)并非車輛中心，生成的ROI產(chǎn)生位置偏移。利用上述對稱軸檢測算法，在對稱軸搜索范圍內(nèi)，從左向右移動固定位移，并計(jì)算位移為XS時對應(yīng)的SNCC值，從而得到圖4所示的該車輛ROI的對稱度曲線,圖中的曲線峰值對應(yīng)位置為ROI對稱軸所在位置，ROI橫向位置改進(jìn)后的圖像如圖3b所示。

(a)改進(jìn)前 (b)改進(jìn)后

圖4 車輛感興趣區(qū)域?qū)ΨQ度曲線

3.3 毫米波雷達(dá)與攝像機(jī)的目標(biāo)匹配

本文通過計(jì)算雷達(dá)目標(biāo)ROI與基于視覺檢測的車輛ROI的重疊率來判斷該車輛是否被雷達(dá)與視覺傳感器同時檢測到。假設(shè)ROI3、ROI4分別為雷達(dá)目標(biāo)ROI與攝像機(jī)目標(biāo)ROI，判斷ROI3和ROI4是否為同一目標(biāo)的步驟如下：

(1)結(jié)合式(9)判斷ROI3與ROI4是否相交，若相交，則執(zhí)行步驟(2)；若不相交，則ROI3與ROI4所包圍的車輛不是雷達(dá)與攝像機(jī)共同檢測車輛。

(2)采用Ajion2=(c-a)(d-b)計(jì)算ROI3與ROI4相交矩形面積。

(3)計(jì)算重合率S，即Ajion2與ROI3面積A3的比值：

S=Ajion2/A3

(10)

先根據(jù)試驗(yàn)估計(jì)閾值ΔS，若S>ΔS，則ROI3與ROI4對應(yīng)同一目標(biāo)。以10 m為間隔，統(tǒng)計(jì)目標(biāo)位于不同距離時，雷達(dá)與攝像機(jī)目標(biāo)檢測區(qū)域的重合率，20次試驗(yàn)的統(tǒng)計(jì)結(jié)果如表4所示，為保證目標(biāo)匹配結(jié)果的準(zhǔn)確性，應(yīng)適當(dāng)減小ΔS。

表4 雷達(dá)與視覺共同檢測區(qū)域的重合率

(4)將雷達(dá)目標(biāo)ROI與攝像機(jī)目標(biāo)ROI的相交矩形作為融合車輛ROI。

3.4 基于KCF-KF組合濾波算法的車輛跟蹤

核相關(guān)濾波器(kernelized correlation filters，KCF)跟蹤效果好且實(shí)時性高，但出現(xiàn)遮擋、快速運(yùn)動、方向改變等多種情況時，KCF跟蹤器可能無法跟蹤目標(biāo)車輛，出現(xiàn)目標(biāo)車輛跟蹤丟失的情況。針對該缺點(diǎn)，采用KCF-KF組合濾波算法跟蹤目標(biāo)車輛。確定目標(biāo)車輛后，KCF算法、KF算法分別對目標(biāo)車輛進(jìn)行跟蹤。若目標(biāo)車輛短暫離開視野，導(dǎo)致KCF跟蹤車輛的方框丟失或方框中心坐標(biāo)與原先方框中心坐標(biāo)相差過大，則使用卡爾曼濾波器預(yù)估方框中心坐標(biāo)；若目標(biāo)車輛重新回到視野中，則繼續(xù)使用KCF算法跟蹤目標(biāo)車輛；若坐標(biāo)偏差存在一段時間，則認(rèn)為是物體自身運(yùn)動變化，繼續(xù)使用KCF算法跟蹤目標(biāo)車輛；若方框丟失時間過長，則認(rèn)為目標(biāo)已離開視覺檢測區(qū)域，重新確立跟蹤目標(biāo)。

3.4.1KCF跟蹤器

核相關(guān)濾波算法[13]是相關(guān)濾波算法的一種。KCF算法在目標(biāo)跟蹤過程中訓(xùn)練目標(biāo)檢測器，屬于鑒別式跟蹤算法。首先利用循環(huán)矩陣在目標(biāo)周圍區(qū)域采集正負(fù)樣本，并利用脊回歸對目標(biāo)檢測器進(jìn)行訓(xùn)練；然后采用目標(biāo)檢測器檢測下一幀圖像中的目標(biāo)位置；最后采用新檢測結(jié)果去更新訓(xùn)練樣本，進(jìn)而更新目標(biāo)檢測器。訓(xùn)練目標(biāo)檢測器時，一般選取目標(biāo)區(qū)域?yàn)檎龢颖荆繕?biāo)周圍區(qū)域?yàn)樨?fù)樣本。

3.4.2卡爾曼濾波器

卡爾曼濾波器的輸入是跟蹤方框中心的坐標(biāo)(x，y)。在目標(biāo)跟蹤過程中，卡爾曼濾波算法通過t-1時刻系統(tǒng)狀態(tài)信息來預(yù)測t時刻的系統(tǒng)狀態(tài)。

t時刻的系統(tǒng)狀態(tài)由方框中心坐標(biāo)值組成：

Xt=[xtytΔxtΔyt]T

(11)

式中，Δxt為時刻t與時刻t-1對應(yīng)橫坐標(biāo)的差值；Δyt為時刻t與時刻t-1的縱坐標(biāo)差值。

卡爾曼濾波算法分為預(yù)測階段與更新階段。先根據(jù)系統(tǒng)之前狀態(tài)規(guī)律預(yù)測出當(dāng)前時刻的系統(tǒng)狀態(tài)值，再通過比較系統(tǒng)預(yù)測狀態(tài)值與實(shí)際測量狀態(tài)值來減小預(yù)測誤差。利用系統(tǒng)之前的狀態(tài)值對下一時刻系統(tǒng)狀態(tài)進(jìn)行預(yù)測：

Xt|t-1=AXt-1|t-1+Bu(t)

(12)

其中，Xt|t-1是基于t-1時刻的預(yù)測結(jié)果，Xt-1|t-1是t-1時刻對應(yīng)的最優(yōu)狀態(tài)結(jié)果或初始狀態(tài)，B為控制矩陣。注意，若系統(tǒng)沒有控制輸入，則狀態(tài)控制矩陣u(t)可以忽略。

接下來則更新Xt|t-1對應(yīng)的協(xié)方差矩陣：

Pt|t-1=APt-1t-1AT+Q

(13)

式中，Pt|t-1為Xt|t-1對應(yīng)的協(xié)方差矩陣；Pt-1|t-1為Xt-1|t-1對應(yīng)的協(xié)方差矩陣；Q為系統(tǒng)過程噪聲。

利用當(dāng)前時刻的系統(tǒng)實(shí)際測量值與測量值計(jì)算當(dāng)前時刻的系統(tǒng)最優(yōu)估算值：

Xt|t=Xt|t-1+Kt(Zt-HXt|t-1)

(14)

Kt=Pt|t-1H(HPt|t-1HT+R)-1

(15)

其中，Kt為卡爾曼增益；H為測量矩陣；R為測量噪聲矩陣。

該跟蹤算法是迭代進(jìn)行的，因此，需實(shí)時更新Xt|t對應(yīng)的協(xié)方差矩陣：

Pt|t=(I-KtH)Pt|t-1

(16)

式中，I為單位矩陣。

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

天氣晴朗時，采集城市道路中車輛正常行駛的視頻與雷達(dá)數(shù)據(jù)，以驗(yàn)證毫米波雷達(dá)與攝像機(jī)目標(biāo)融合算法與跟蹤算法的有效性。毫米波雷達(dá)為美國Delphi 76-77GHz電子掃描雷達(dá)(electronically scanning radar，ESR)；攝像機(jī)選用全瑞視訊的RER-USB13M02，輸出圖像分辨率為640pixel×480 pixel，幀率為每秒30幀；計(jì)算機(jī)采用工業(yè)控制計(jì)算機(jī)，處理器型號為Inter Core i7，內(nèi)存為16GB；系統(tǒng)環(huán)境為Windows，編譯環(huán)境為Visual Stidio，編譯語言為C++。

4.1 雷達(dá)與攝像機(jī)融合結(jié)果與分析

圖5所示為毫米波雷達(dá)與攝像機(jī)車輛的檢測結(jié)果(黃色矩形框?yàn)榛谲囕v檢測器生成的ROI，藍(lán)色矩形框?yàn)槔走_(dá)目標(biāo)橫向位置修正后的ROI)。雷達(dá)目標(biāo)ROI與攝像機(jī)目標(biāo)ROI的重合率如表5所示，其中，圖5c與圖5d的目標(biāo)重合率按照由左到右的順序依次計(jì)算。對比表4內(nèi)的重合率可知，圖5內(nèi)的車輛均屬于雷達(dá)與攝像機(jī)共同檢測目標(biāo)，其融合結(jié)果如圖6所示。

(a) (b)

(a) (b)

表5 重合率計(jì)算結(jié)果統(tǒng)計(jì)

為進(jìn)一步證明雷達(dá)與攝像機(jī)目標(biāo)融合算法的有效性，分別對3個數(shù)據(jù)集的融合結(jié)果采用匹配成功率和誤匹配率進(jìn)行評價。目標(biāo)匹配成功率P1與目標(biāo)誤匹配率P2分別為

P1=N1/N

(17)

P2=N2/N

(18)

式中，N為雷達(dá)與攝像機(jī)共同檢測目標(biāo)數(shù)；N1為目標(biāo)成功匹配數(shù)目；N2為目標(biāo)錯誤匹配數(shù)目。

P1越大，融合算法越好；P2越大，融合算法效果越差。表6所示為融合算法在3個數(shù)據(jù)集下的融合結(jié)果，雷達(dá)與攝像機(jī)目標(biāo)融合算法效果良好。

表6 雷達(dá)與視覺傳感器目標(biāo)的匹配結(jié)果

4.2 車輛跟蹤結(jié)果與分析

為便于說明目標(biāo)被遮擋的問題，從采集的數(shù)據(jù)集中選取目標(biāo)未被遮擋→部分被遮擋→完全被遮擋的特殊場景數(shù)據(jù)進(jìn)行分析。如圖7a～圖7d所示(紅色矩形框?yàn)槔走_(dá)與視覺目標(biāo)匹配后生成的ROI，綠色矩形框?yàn)檐囕v跟蹤框)，當(dāng)被跟蹤車輛在第70幀被部分遮擋時，僅采用KCF算法無法對其進(jìn)行繼續(xù)跟蹤，導(dǎo)致目標(biāo)丟失。本文算法在被跟蹤車輛遮擋時，采用卡爾曼濾波算法對融合車輛的ROI中心位置坐標(biāo)進(jìn)行跟蹤，從而實(shí)現(xiàn)穩(wěn)定跟蹤融合車輛，圖7e～圖7h中顯示的坐標(biāo)為利用卡爾曼濾波預(yù)測的車輛坐標(biāo)。

(a) (b) (c) (d)

記錄隨著圖片幀數(shù)遞增的車輛實(shí)際坐標(biāo)與跟蹤坐標(biāo)(xS，yS)的變化，圖8、圖9所示分別為KCF算法與KCF-KF組合濾波算法的跟蹤融合車輛。0～69幀時，目標(biāo)車輛在道路上穩(wěn)定行駛且未被遮擋，KCF算法與KCF-KF組合濾波算法均能穩(wěn)定跟蹤融合車輛；70～78幀時，目標(biāo)車輛由部分遮擋過渡到完全遮擋，KCF算法無法繼續(xù)跟蹤目標(biāo)車輛，KCF-KF組合濾波算法利用KF算法繼續(xù)跟蹤目標(biāo)車輛ROI的中心位置坐標(biāo)，直到恢復(fù)KCF模式。

圖8 KCF算法跟蹤目標(biāo)車輛

圖9 KCF-KF組合濾波算法跟蹤目標(biāo)車輛

如圖10所示，采用KCF-KF算法跟蹤目標(biāo)車輛，坐標(biāo)誤差更小，跟蹤精度更高。車輛位置坐標(biāo)跟蹤的準(zhǔn)確率P3=W1/W2，其中，W1為預(yù)測坐標(biāo)，W2為實(shí)際坐標(biāo)，P3越大，算法跟蹤效果越好。該算法對X坐標(biāo)、Y坐標(biāo)的跟蹤準(zhǔn)確率97.34%與95.19%。70～78幀時，目標(biāo)車輛被短暫遮擋，但是KCF-KF組合濾波算法仍能穩(wěn)定跟蹤目標(biāo)車輛。

(a)橫坐標(biāo)誤差

5 結(jié)論

(1)提出的濾除雷達(dá)干擾目標(biāo)算法可剔除空目標(biāo)、非危險目標(biāo)及虛假目標(biāo)，獲取有效目標(biāo)。

(2)為減小與基于視覺檢測的車輛興趣區(qū)域(ROI)的匹配誤差，提出一種改進(jìn)雷達(dá)目標(biāo)ROI橫向位置算法來對雷達(dá)映射至視覺圖像中的ROI進(jìn)行對稱性分析。

(3)提出一種目標(biāo)匹配算法來計(jì)算視覺傳感器獲取的車輛ROI與雷達(dá)目標(biāo)ROI的重合率，確定雷達(dá)與視覺傳感器共同檢測的車輛。利用核相關(guān)濾波與卡爾曼濾波聯(lián)合跟蹤算法對融合車輛進(jìn)行跟蹤。研究結(jié)果表明，在光照良好的情況下，該算法跟蹤效果良好，具有較強(qiáng)適應(yīng)性與較高準(zhǔn)確率。