融合視覺與雷達數據的改進粒子濾波車輛目標跟蹤

張 翔，鄭 玲，李以農,，張志達

(重慶大學 a.機械與運載工程學院;b.機械傳動國家重點實驗室, 重慶 400044)

隨著汽車數量的不斷增加，人們出行得到便利的同時，交通事故也引發越來越多的人員傷亡與財產損失[1]。對于智能汽車而言，實現對前方車輛快速準確地檢測與預警能夠有效降低交通事故概率[2]。國內外在智能車輛研究領域已經提出許多車輛檢測的方法，包括基于圖像特征的方法[3]、基于模型的方法[4]、基于機器學習的方法[5-6]等。近幾年基于深度學習的方法在通用目標檢測領域取得了很好的效果[7-10]，也逐漸應用于自動駕駛的環境感知算法中。

在基于單幀圖像的車輛檢測過程中，由于光照條件與道路交通背景變化復雜，可能會出現目標漏檢或誤檢的情況。此外，智能車輛在進行路徑規劃時需要考慮障礙目標的運動軌跡，需要提取車輛目標運動的時序特征。為了解決車輛檢測方法魯棒性不足與無法結合時序信息的問題，車輛跟蹤方法受到大量研究者的關注。解文華等[11]研究一種引入主動輪廓(C-V)模型的MeanShift車輛目標跟蹤算法，通過C-V模型實現對車輛多目標的跟蹤管理。但是MeanShift中的窗口尺寸是固定值，難以適應車距變化引起的圖像尺寸改變。丁曉娜[12]基于Gaussian模型及卡爾曼濾波實現車輛跟蹤，但卡爾曼濾波無法解決交通車輛遮擋情況下的收斂問題，對尺度和運動狀態波動性較大的車輛目標跟蹤的魯棒性和實時性不足。近年來基于誤差最小平方和的MOSSE(minimum output sum of squarederror)濾波器[13]、基于學習的TLD(tracking learning detection)跟蹤器[14]、核相關KCF(kernelized correlation filters)濾波器[15]、基于判別式尺度空間的DSST(discriminative scale space tracking)跟蹤器[16]等相關濾波方法被提出并在目標跟蹤中引起重視，由于其在頻域中點乘運算的方式極大減少了運算量，使得跟蹤的實時性得到極大提高。但是相關濾波方法不直接適用于多目標跟蹤，難以解決道路車輛跟蹤中目標加入、消失、遮擋等問題。鑒于粒子濾波( PF, particle filtering)算法具有高魯棒性與易拓展性，學者們將圖像特征與粒子濾波算法結合進行車輛多目標跟蹤[17-18]。為了提高跟蹤精度，一些改進粒子濾波算法也被提出。田夢楚等[19]將螢火蟲群體的吸引和移動機制引入粒子濾波，利用螢火蟲優化算法解決粒子濾波的粒子退化問題，提高了目標跟蹤的效率。韓錕等[20]利用果蠅優化算法約束粒子群的區域，提高了跟蹤精度。Zhou等[21]基于遺傳算法提出改進的粒子濾波重采樣策略，改善粒子退化，提高了目標跟蹤的精確度。然而，這些跟蹤算法在解決復雜背景下的目標跟蹤、目標長期遮擋等情況時仍存在問題，無法實現實時穩定的道路車輛跟蹤。

毫米波雷達具有環境適應性強、體積小、成本低等優點，因此也得到研究者的廣泛采用。Xu等[22]基于聯合集成概率數據關聯與交互式多模型(JIPDA-IMM)結合方法，使用毫米波雷達實現前方多目標準確跟蹤。Huang等[23]基于極坐標系下的卡爾曼濾波實現單目標跟蹤，并通過閾值方法進行目標的區分和關聯，實現車輛多目標跟蹤。Aihara等[24]提出將CNN卷積神經網絡模型應用于毫米波雷達，模擬激光雷達點云數據，實現道路可行駛區域分割。此外，利用毫米波雷達與視覺信息融合具有極大提升車載環境感知能力的潛力，信息融合的方法也成為研究熱點。王寶鋒等[25]首先根據雷達目標投影獲取圖像中的車輛識別感興趣區域，然后通過對稱性、底部陰影、車輛寬度等特征進行驗證，實現高魯棒性的車輛識別。Wang等[26]利用YOLOv2進行車輛檢測，針對雨天道路反光造成的圖像干擾，以雷達投影點提取水平線，利用與邊界框的交點坐標計算車輛寬度，提高寬度估計的精確。以上基于視覺與雷達信息融合的研究在車輛檢測領域表現成熟，但仍無法解決車輛跟蹤中尺度變化問題。Chen等[27]在圖像感知哈希編碼的跟蹤算法中引入毫米波雷達探測信息進行尺度更新，但是其僅針對單目標跟蹤問題，無法適用于道路環境中的多車輛目標跟蹤。

針對視覺圖像中車輛目標尺度變化導致跟蹤丟失問題，提出一種基于視覺與毫米波雷達信息融合的改進粒子濾波車輛跟蹤與尺度修正算法[28-29]。首先采用遺傳算法改進粒子濾波的重采樣策略，根據實時有效采樣粒子數求取動態自適應的遺傳交叉概率，并在交叉操作中利用單個子代替換小權重粒子，以提高平均精度。利用高斯函數計算種群適應度、計算遺傳變異狀態，使跟蹤估計窗口更符合真實運動。然后，在改進粒子濾波車輛多目標跟蹤算法中，引入毫米波雷達的深度信息對跟蹤邊界框尺寸進行修正。利用雷達目標投影點與視覺跟蹤框的位置關系，設計判別策略實現關聯匹配，通過推導目標的圖像尺寸與縱向距離呈現反比例關系，融合雷達探測的運動信息實現關聯目標的跟蹤邊界框尺寸修正，實現了智能汽車對道路車輛的連續準確跟蹤。

1 雷達與機器視覺的時空同步

德爾福ESR毫米波雷達集成了長距離雷達與中距離雷達的檢測功能，獲取目標的距離、角度、相對速度等運動信息。長距離雷達的最大探測距離為175 m，探測角度為20°；中距離雷達的最大探測距離為60米且探測角度為90°。選用的車載相機為羅技C920 Pro，其探測視角為78°。雷達安裝于車輛前方保險杠中部，相機的安裝位置于車輛前擋風玻璃上方，如圖1(a)所示。傳感器探測范圍見圖1(b)。

圖1 傳感器布置與探測范圍Fig. 1 Sensor layout and detection range

雷達與相機的空間坐標轉換需要標定內外參數。首先利用相機采集不同視角下的標準棋盤格圖片不少于20張，然后利用Matlab標定工具箱自動提取每張圖片的角點，其中需要輸入棋盤尺寸，即可通過張正友標定法的迭代計算可以得到相機內外部參數。再經過測量與迭代計算獲取雷達與相機的外部平移與旋轉參數后，代入透視變換中的內外參數矩陣，即可完成雷達坐標與圖像像素坐標的轉換

(1)

其中，(u,v)表示圖像像素坐標中的目標位置，(XR,YR,Z)表示雷達坐標系下的目標位置，M1與M2分別表示內外參數矩陣。

由于雷達與相機的工作頻率不同，要實現多傳感器數據關聯，還需要完成時間上的對準。毫米波雷達的采樣幀頻率為20 Hz，而相機的采樣幀頻率為30 Hz，按照采樣頻率低的傳感器向下兼容，提取雷達每間隔一幀數據、相機每間隔2幀數據為有效數據。

2 毫米波雷達數據濾波

2.1 雷達數據預處理

德爾福ESR雷達每一幀掃描時返回64個通道的目標數據，并包含每個目標的速度、距離、角度、加速度等運動信息。由于產生的數據量過大，若每次調用數據時全部讀取，將十分費時、且占用大量內存空間，因此需要對雷達原始數據進行預處理。具體過程如下：

1)提取運動參數。考慮到毫米波雷達所探測各運動參數的精確度，篩選出每個通道目標的速度、距離、角度三組參數。

2)去除空目標。根據雷達解算協議，雷達未探測到目標時返回的空信號數據為相對距離d=0、相對速度v=81.91、角度α=0。根據這些特征值，可以濾除返回信號中的空目標。

3)基于閾值的有效目標初選。在實際應用中通常需要識別的道路車輛目標為同車道的前方車輛、及左右兩側鄰車道的同向車輛，如圖2所示。為此需要設置橫向距離與相對速度閾值，濾除非目標區域的車輛與逆向車輛。

圖2 道路前方車輛分布Fig. 2 Preceding vehicles distribution

此外，由于長距離毫米波雷達的最大探測距離為175 m，而其返回數據的精度不高；且遠處目標在相機采集圖像中尺寸過小，難以實現準確關聯。因此需要設置縱向距離閾值濾除遠處目標。綜上得到的閾值篩選模型如下

(2)

其中:dx為橫向相對距離;vy為縱向相對速度;dy為縱向相對距離；Wth為橫向距離閾值，由車道寬度與閾值的乘積表示(根據中國道路技術規范，設置行車道的寬度Wth為3.75 m);系數k通常取1.5；vth是相對速度閾值，為負數，取-20 m/s；dth為縱向距離閾值，取60 m。

2.2 車輛目標關聯濾波

除了交通車輛，毫米波雷達還會探測到道路上的其他障礙物目標，比如行人、交通標志、金屬護欄等。此外，數據中還存在誤檢無效目標、漏檢有效目標等非連續目標。因此，需要利用車輛目標關聯濾波從毫米波雷達數據中獲取更精確的車輛目標。

首先使用三階卡爾曼濾波方法對有效目標運動信息進行預測，選取的觀測向量為[dn,vn,an]，即毫米波雷達檢測到目標在n時刻的距離、速度、加速度。

(3)

其中，雷達的掃描頻率為20 Hz，則每個周期的間隔為0.05 s；而多傳感器時間對準后，雷達數據為每間隔一幀提取一次，所以取t=0.1 s。

由于毫米波雷達對加速度的分辨力較低，返回數據中同一個目標的加速度數值存在較大波動，以及車輛目標在道路的運動多為縱向運動，因此對濾波模型作出改進：剔除加速度數據a，由相對距離與角度計算出縱向距離dy與橫向距離dx

(4)

最終獲得關聯濾波模型如下

(5)

考慮到存在有效目標漏檢的情況，為檢驗有效目標的連續性，針對每一幀雷達掃描數據，分別提取其前后各3幀數據進行上述檢驗，從而濾除無效目標

(6)

其中，j={-3,-2,-1,1,2,3}表示前后各三幀的雷達數據，found_radar(j)表示第j時刻下是否存在目標滿足關聯濾波閾值要求，值為0或1。若相鄰6幀數據中存在至少4幀數據滿足式(5)的閾值要求，則判定當前幀的目標為連續有效的車輛目標(如圖3所示)。

圖3 雷達數據關聯濾波效果對比圖Fig. 3 Effect of radar data association filtering

3 基于改進粒子濾波的車輛跟蹤

3.1 改進粒子濾波算法

標準粒子濾波算法中，重要性采樣多次迭代后存在嚴重的粒子退化現象，即多數新生粒子的觀測值與跟蹤模板間的相似性越來越低，連續多幀后粒子權重逐漸變成0，其后驗概率也失去意義。為了解決粒子退化問題，經典重采樣方法采用復制大權重粒子、刪除小權重粒子的策略，但是多次濾波后會使得粒子多樣性降低，導致樣本枯竭，在場景劇變時難以準確跟蹤。遺傳算法是一種自適應全局優化搜索算法，模擬生物進化過程，將其引入粒子濾波重采樣，可以利用優勝劣汰的規則繁殖(復制)更優質的粒子，以產生更好的近似解，解決粒子退化問題同時保持粒子多樣性。

遺傳算法主要包括基因編碼、適應度評價、選擇、交叉、變異等運算步驟。對于選擇樣本參與交叉和變異操作的策略通常有2種，即全部參與、或以一固定概率參與，沒有考慮種群適應度。若定義有效粒子數為

(7)

考慮粒子退化程度，基于實時有效采樣粒子數Neff，求取動態自適應的交叉概率為[21]

(8)

其中，k是遺傳概率系數，Ns是粒子總數，Nth為設定閾值。

動態自適應的交叉概率能夠有效解決粒子退化問題，并保持多樣性，但是其計算成本會增大，因此不適用于實時的目標跟蹤系統。為了在不降低跟蹤精度的同時提升計算時效性，對遺傳交叉和變異部分進行改進。具體工作如下：

1)基因編碼：針對視覺跟蹤問題，選用目標在圖像中的位置及尺寸為基本狀態模型。由于車輛跟蹤問題涉及到目標運動與尺度變化，選取基因編碼s=[x,y,h,w,vx,vy,sc]，包括目標窗口的位置及尺寸、運動速度、尺度變化因子。

2)個體適應度評價：選取圖像HSV顏色直方圖特征，設置bins=8進行色彩壓縮，可將360×100×100維的直方圖數據壓縮為8×8×8維，以減少存儲量、提高計算效率。再利用巴氏系數度量每個粒子表征窗口的直方圖pi與模板直方圖q之間的相似度

(9)

巴氏距離ρ越大則表明2個直方圖分布越相似。然后利用一維高斯函數處理，使適應度fi符合正態分布

(10)

3)選擇：計算前一幀粒子集中每個粒子的適應度(權重)，并據此計算當前有效粒子數，利用動態自適應概率式(8)，計算得到交叉概率Pc，再以輪盤賭策略執行N/2次隨機選擇粒子對作為父代，逐次參與遺傳交叉操作。

4)交叉：利用每對父代粒子交叉產生一個子代

C=α×P1+(1-α)×P2，

(11)

其中：P1與P2為父代粒子的基因編碼；C為生成的子代粒子基因編碼。為了彌補單個子代可能帶來種群多樣性的不足，設置比例因子α非固定值，其隨機數生成區間為[0.3,0.7]。

完成選擇與交叉操作后，利用生成的N/2個子代替換前一幀粒子集中個體適應度排名靠后的相應粒子，記錄形成新的粒子集，參與后續運算。此策略可以使重采樣結果的平均期望更接近跟蹤模板，提高跟蹤精度，同時減少每一幀運算量，提高目標跟蹤的時效性。

5)變異：以一定概率在種群內隨機挑選粒子，針對第i個粒子編碼si，對窗口坐標與尺度進行隨機變異，以提高種群的多樣性。利用正態分布計算粒子變異狀態，使概率分布更接近目標運動規律

(12)

其中，r為服從[0,1]均勻分布的隨機數，設置變異概率rth=0.1。

所提出的改進粒子濾波算法基于遺傳算法改進重采樣策略，以正態分布而非均勻分布來計算種群適應度、及實現遺傳變異操作，使數值計算更接近真實運動軌跡。且利用每對父代粒子生成單個子代，可以提高平均期望，同時避免大量迭代，能夠優化計算效率。改進算法能夠保證權重較大的粒子依然有更大概率保留，避免粒子退化導致的濾波發散，同時提高粒子的多樣性，避免樣本衰竭導致車載相機環境劇變時難以跟蹤。

3.2 改進粒子濾波在車輛跟蹤中的實現

所提出的改進粒子濾波算法在車輛跟蹤中的主要實現步驟如下：

1)首幀初始化：對于每一個目標，根據先驗密度函數采樣N個粒子。與前文遺傳算法的基因編碼相同，粒子狀態表征模型為s=[x,y,h,w,vx,vy,sc]。窗口位置與尺寸的初始化主要由檢測結果給定，在置初始速度vx=vy=0，并且由于車載相機中前方車輛尾部尺寸變化幅度一般不大，限定sc=0.01。此外需要設置粒子集初始化權重wi=1/N。

2)重采樣：依據前文提出的基于遺傳算法改進重采樣方法，針對前一幀粒子集的個體適應度進行自然選擇、遺傳交叉、變異等操作，得到新的粒子集合。

3)觀測權值：與前文個體適應度評價方法相同，利用式(9)度量窗口HSV直方圖與模板直方圖間相似度，并利用式(10)離散化，再進行歸一化處理，得到粒子權值wi。

(13)

式(13)返回Oid為被遮擋粒子的序號。若連續遮擋達到一定幀數，則判定該目標行駛出視野之外，刪除對應的直方圖模板。

5)狀態輸出：根據各粒子的權重與狀態模型計算加權平均狀態，求得的結果可作為目標在新一時刻的估計狀態輸出。

6)返回步驟2進入下一視頻幀的目標跟蹤，直至連續跟蹤幀數達到設定值。

4 基于信息融合的跟蹤結果修正

對于視覺跟蹤算法，通常在長時間跟蹤后會出現跟蹤框尺寸過大或過小的情況，致使跟蹤窗口出現目標漂移問題，導致目標丟失。針對此問題，引入毫米波雷達獲取目標的深度信息，對跟蹤邊界框尺寸進行修正。

4.1 雷達與視覺跟蹤目標關聯匹配

首先需要將濾波后的毫米波雷達數據與視覺跟蹤結果關聯。根據式(4)，可利用雷達探測到的目標距離與角度信息，由雷達坐標轉換得到以車直角坐標系下。同時由式(1)的多傳感器空間坐標轉換關系，可將雷達目標點投影到圖像上，得到其圖像坐標[u,v]。同時，基于遺傳算法改進的粒子濾波在車輛跟蹤中的實現，可得到當前幀的跟蹤結果，包括位置與尺寸參數[x,y,w,h]。

如圖4所示，綠色圓點表示雷達目標中心點投影，紅色矩形框表示視覺跟蹤結果。根據雷達投影點與視覺跟蹤框的位置關系，可以確定目標關聯匹配算法。

取“2.2”項下固定后的肝組織，脫水，石蠟包埋，按4 μm厚度切片。然后進行常規蘇木精-伊紅（HE）染色，于倒置顯微鏡下觀察，并依據《非酒精性脂肪性肝病診療指南》[10]，判定肝細胞脂肪變性程度。

(14)

設計的目標關聯匹配判別策略如下：

判據1：若跟蹤框內沒有雷達投影點，則該視覺跟蹤目標沒有雷達目標與之關聯匹配；判據2：若一個跟蹤框內存在且僅有一個雷達投影點，則直接將其關聯匹配；判據3：若一個跟蹤框內存在多個雷達投影點，則將距離中心點(x+w/2,y+h/2)處最近的雷達目標與之關聯。

其中判據2的優先級高于判據3。若兩跟蹤框存在部分重疊(如圖4a)，通常遠處車輛目標(跟蹤框也較小)投影點可能同時包含于近處目標跟蹤框(尺寸較大)中，為避免匹配錯誤，可以通過處理判據2實現遠處目標匹配。其次，為解決同一車道出現多輛車導致圖像中遠處目標被完全遮擋的情況(如圖4b)，可采用判據3解決大多數遮擋目標的匹配。

圖4 雷達投影點與視覺跟蹤框Fig. 4 Radar projection points and visual tracking bounding boxes

4.2 跟蹤邊界框修正

根據相機透視變換關系式(1)，相機內外參數矩陣確定時，雷達坐標系與圖像像素坐標之間的轉換為線性關系。因此對于一個尺度不變的目標，在縱向距離更大的時候，其在圖像中尺寸呈現對應反比例的縮小。

W·D=Con，

(15)

其中：W為目標在相機成像中的寬度；D為縱向距離；Con表示常數。

鑒于此，在視覺跟蹤中引入毫米波雷達探測的目標深度信息，實現對跟蹤邊界框的修正。假設前一時刻下，跟蹤目標窗口的位置與尺寸由向量[x1,y1,w1,h1]表示，對應的縱向距離為D1；而當前時刻，目標的位置與尺寸表示為[x2,y2,w2,h2]，對應的縱向距離為D2。則可由式(15)推導的反比例關系得

w1·D1=w2·D2。

(16)

在跟蹤修正中，主要考慮當前時刻的跟蹤框寬度與高度不準確，但是仍要依據跟蹤框的中點位置。此外，由于前一時刻的檢測結果作為真實值、或跟蹤結果已經過修正，也可直接沿用。由此，得到修正后的當前時刻下車輛目標的位置與尺寸[x’,y’,w’,h’]如下

(17)

對每一幀下所有關聯匹配的跟蹤目標邊界框完成位置與尺寸修正后，即可以迭代循環方式進入下一幀，實現車輛多目標的連續跟蹤。

5 實驗結果分析

為驗證所提融合算法的有效性，與標準粒子濾波和改進粒子濾波的跟蹤效果進行對比實驗。實驗數據采集于重慶市日間內環快速路、高速公路及部分城市道路場景，天氣為晴天或多云。視頻幀數為30FPS，圖像分辨率為1280*720，包含了部分樹木陰影、天橋、路面損毀等場景，有效涵蓋多數車輛高速運行的復雜場景。ESR毫米波雷達數據離線存儲為csv格式文件。實驗軟件平臺為MATLAB 2016B，硬件平臺為8 G內存、I5處理器的筆記本電腦。

分別采用標準粒子濾波算法、改進粒子濾波算法、及融合雷達運動信息的粒子濾波方法進行道路車輛多目標跟蹤實驗，在典型工況下采集的視頻數據與毫米波雷達數據中選取25段數據，每段長度約為20幀。基于標準粒子濾波算法和改進粒子濾波算法的部分車輛跟蹤結果圖5和圖6所示，在改進粒子濾波基礎上引入毫米波雷達信息的跟蹤修正實驗結果如圖7所示。

圖5 基于標準粒子濾波算法的車輛跟蹤Fig. 5 Vehicle tracking based on standard particle filter algorithm

圖6 基于改進粒子濾波算法的車輛跟蹤Fig. 6 Vehicle tracking based on improved particle filter algorithm

圖7 融合雷達信息的跟蹤修正實驗Fig. 7 Tracking bounding box correction fusing radar data

從圖5(b)、圖6(b)和圖7(b)中可以看出，連續跟蹤10幀后，標準粒子濾波算法的跟蹤框與真實值存在一定偏差，而改進粒子濾波算法與融合雷達信息的跟蹤框仍能準確表征車輛尾部。由圖5(c)與圖6(c)可見，由于連續跟蹤產生的累積誤差，視覺跟蹤邊界框的位置與尺寸均已經與真實目標偏差較大。相比之下，融合雷達信息的跟蹤框修正后尺寸更符合目標實際尺寸，如圖7(c)所示，所提出的融合算法能夠有效避免僅采用視覺跟蹤時窗口尺寸的累積誤差導致目標丟失。

對應時刻的毫米波雷達測距信息曲線如圖8所示。

圖8 雷達測量目標縱向距離Fig. 8 Longitudinal distance of vehicle targets by radar measurements

在25段數據測試后，3種算法的平均準確率與平均精度隨幀數的關系如圖9(a)與圖9(b)所示，統計實驗結果數據如表1所示。其中平均準確率(MOTA, multi-object tracking accuracy)表示跟蹤算法濾除目標誤報與漏報的能力，平均精度(MOTP, multi-object tracking precision)表示跟蹤框與真實目標邊界框的重疊度[29]。

圖9 3種跟蹤算法平均準確率與平均精度的視頻序列變化曲線Fig. 9 MOTA and MOTP performance of 3 tracking algorithms in video sequences

表1 3種跟蹤算法性能比較

分析實驗結果可以看出，相比于標準粒子濾波算法，改進粒子濾波算法在跟蹤平均準確率與跟蹤精度上有顯著提升，分別提高22.1%與21.1%。但是跟蹤一段時間后精度會降低，跟蹤框尺寸累積誤差導致難以有效表征跟蹤目標。而引入毫米波雷達深度信息后，跟蹤平均精度獲得進一步提升，達到87.8%，表明修正后的視覺跟蹤框位置與尺寸更加接近車輛目標尾部在圖像中呈現的真實值。雖然兩種改進算法在耗時上有小幅增加，但是能夠基本滿足傳感器時間對準后每秒10幀的實時性需求。

6 結 論

為提高智能汽車對道路車輛多目標跟蹤精度，基于視覺與毫米波雷達信息融合，提出一種引入深度信息的改進粒子濾波車輛跟蹤與尺寸修正方法。主要結論如下：

1)提出一種基于遺傳算法改進重采樣的粒子濾波算法。根據粒子退化程度計算動態自適應交叉概率，利用高斯替代平均分布以逼近真實運動規律。實驗結果表明，改進算法顯著提升了車輛多目標跟蹤的平均準確率與精度，并保證了實時性。

2)引入毫米波雷達深度信息對跟蹤邊界框尺寸修正。利用雷達目標透視投影點與視覺跟蹤框的位置關系實現關聯匹配，通過圖像尺寸與縱向距離的反比例關系完成關聯目標的跟蹤邊界框坐標與尺寸修正。實驗結果表明，此方法進一步提高了平均跟蹤精度，避免車輛連續跟蹤時的由于窗口尺寸累積誤差導致目標丟失。