基于雷達與相機融合的行人軌跡跟蹤

李宏暉，張昊宇

(1.浙江海洋大學 海洋工程裝備學院，浙江 舟山 316000； 2.浙江海洋大學 信息工程學院，浙江 舟山 316000)

0 引 言

目前，大多數實時多目標跟蹤算法是基于相機傳感器設計的，但相機在一幀中可以捕捉到的像素數量是固定的，目標與傳感器之間的距離會對跟蹤精度產生很大的影響。事實上，代表同一目標的像素量將隨著傳感器和目標之間距離的增加而呈平方遞減。然而，毫米波雷達能夠通過目標與雷達的距離信息來區分不同的目標，且毫米波雷達的一個重要優勢是不易受環境條件的影響，如雨、灰塵、煙霧、霧或霜[1]。但與激光雷達相比，毫米波雷達角分辨率太低。激光雷達分辨率高、抗干擾能力強，但也存在數據交換量大, 軟、硬件成本高等問題。

基于上述原因，研究人員開始探索異類傳感器融合在目標跟蹤領域的應用。Heuer等使用24 GHz雷達和先跟蹤后探測的方法來實現目標跟蹤。他們的方法是基于一個粒子濾波器[2]，其權重受人體行走引起的多普勒信號的影響。但該系統僅限于跟蹤單個目標，而不考慮軌跡的創建和終止等事件。Gruyer等提出了一種基于激光雷達和相機的融合方法。但該方法不執行任何類型的對象分類，并且依賴基于檢測區域大小和運動模型的假設。Yenkanchi等[3]提出了一種基于雷達和相機的協同測距成像探測器，用于道路障礙物檢測。作者使用試探法將矩形遮罩應用于各種對象，如汽車、卡車、行人等。得到的圖像遮罩在視覺上與圖像內容相匹配。然而，還不清楚這些遮罩如何用于對特定目標進行分類，以及是否有性能改進。

綜合考慮上述方法出現的問題，本文設計了一種先檢測后跟蹤的算法。基于Felix等[4]的研究，利用CRFNet神經網絡代替傳統純圖像網絡對目標進行檢測。借鑒Martin等[5]和Alessio等論文中描述的結構，采取在特征層對雷達數據與圖像進行融合的方法，并受到Michael等[6]工作的啟發，通過粒子濾波算法處理這些融合信息最終實現對行人多目標的精確跟蹤。

1 算法流程圖

本文的跟蹤算法采用先檢測后跟蹤的工作模式，先借助CRFNet神經網絡對圖片進行檢測得到目標邊界框，再根據針孔模型估計目標在地平面上的位置坐標。接著利用目標的位置坐標對雷達點云進行修正與篩選，并根據處理后得到的點云數據設計了一種改進的粒子濾波算法。算法流程如圖1所示。

2 基于傳感器融合的目標檢測

2.1 雷達點云與圖像融合

本文所使用的CRFNet網絡通過對圖像和雷達信號進行融合以提高網絡的目標檢測性能，其融合方法如下。

雷達傳感器的輸出包括三維點云和相關的目標參數，如速度、信號強度等。考慮到雷達點云的高度并不能反映物體的實際高度，所以首先將雷達點云高度置為0，然后將雷達數據從三維空間中映射到圖像平面。根據歐盟的數據，人類的平均身高為1.7 m，因此我們將雷達投影由投影點向上、下延伸為1.8 m長的線段，使雷達投影盡可能完全覆蓋行人目標。

雷達點云的投影以像素值的形式存儲在融合圖像中。在無雷達回波的圖像像素位置，將投影雷達通道值置為0。圖像由紅、綠、藍3個通道組成，在此基礎上，我們將雷達通道與圖像通道相連作為神經網絡的輸入，如圖2所示。

分別對毫米波雷達與相機進行空間標定，得到旋轉矩陣R與平移矩陣T。再通過張正友標定法對相機進行內參標定，得到圖像空間坐標系到像素坐標系的轉換矩陣。最后將投影以投影點為中心，往豎直方向上、下分別延伸0.9 m，生成一個1.8 m長的豎線。圖3(a)為雷達測得的三維點云圖，圖3(b)為將點云投影到圖片上的效果圖。

2.2 基于傳感器融合的神經網絡架構

CRFNet(如圖4所示)神經網絡架構建立在RetinaNet[7]上，并以VGG16為基礎網絡。網絡的輸出端分別為邊界框回歸子網絡和目標分類子網絡。如文獻[7]中所建議的，網絡使用聚焦損失(focal loss)作為目標分類的損失函數，并使用smooth L1作為邊界框回歸的損失函數。

融合網絡的主干為圖4中特征提取子網絡的第二列，由VGG16模塊組成。相機數據流和雷達數據流通過連接器連接，并在第一層輸入VGG16的模塊1。網絡的這一分支通過VGG16層處理相機和雷達的融合數據。在左邊的分支中，原始雷達數據通過最大池化以相應的比例輸入到網絡的更深層，同時與VGG16層的輸出相連接。

接下來再在輸出端引入特征金字塔網絡(FPN)[8]由模塊P3到P7表示，通過在每個級別上的級聯來額外地融合雷達通道。FPN模塊的輸出連接了兩個子網絡，分別是邊界框回歸子網絡和目標分類子網絡。

2.3 數據關聯與針孔模型

借助CRFNet網絡檢測到行人目標后，需進行以下兩步操作，為后續粒子濾波算法提供必要數據：一是借助KM算法實現前、后幀目標的數據關聯，并得到目標a、b之間的色差Cola,b； 二是借助針孔模型計算行人在地平面上的位置坐標。

(1)根據匈牙利算法的改進算法KM算法對相機前、后幀檢測出的目標進行關聯。KM算法即是在一個二分圖內，每一組左右連接有權值ωab， 求一種匹配使得所有ωab的和最小。本實驗中，設定ωab為前一幀目標a與后一幀目標b之間的顏色差值Cola,b。 為了減小背景對算法的干擾，取目標邊界框的中間部分(水平方向為[1/3,2/3]，豎直方向為[1/5,7/10])作為目標的顏色取樣區間。分別對目標a、b的顏色取樣區間中的所有像素的紅(r)、綠(g)、藍(b)這3個顏色通道取平均值 [ra,ga,ba]、 [rb,gb,bb]， 且已知每個像素的通道顏色的取值范圍為1～256。Cola,b如式(1)所示

(1)

(2)“針孔模型”首先為目標的實際高度設置一個估計值H，再借助目標邊界框的位置和大小來估計目標在地平面上的位置坐標。

針孔模型如圖5所示，通過一個代表相機鏡頭的點，將三維世界映射到圖像平面。其中，左部分人像表示行人在圖片平面上的投影，右部分表示人在三維空間的位置。f表示相機焦距，D表示行人的中心位置到相機的距離，h表示目標邊界框的寬度，H表示行人身高的估計值，d表示目標邊界框中心到相機鏡頭的距離，c表示邊界框中心到相機鏡頭在圖片上垂直投影的距離，F表示行人到相機的距離。邊界框中心點在圖像平面上的位置可表示為 (px,py)。 則行人在地平面上的位置坐標 (x,y) 為

(2)

(x,y)=(D·sin(θx),D·cos(θx))

(3)

3 基于傳感器融合的粒子濾波

3.1 傳感器誤差

本節對相機傳感器與雷達傳感器的測量誤差做簡要介紹，為后續雷達點云的預處理提供必要參數。

(1)目標距離誤差：

借助針孔模型可以得到行人在地平面上的位置，但其中有兩步操作會產生誤差：①用目標邊界框的高度h來近似行人在圖片中的高度；②在針孔模型中將行人的身高統一假設為H。經過多次實驗發現第一類誤差很小可忽略不計。針對第二類誤差，先假設行人真實身高為γH， 如圖6所示。

再由針孔模型可得

f=dcos(θx)

(4)

(5)

根據歐盟的數據人類的平均身高為1.7 m，因此本文設定H=1.7m。考慮到多數行人的身高區間為[1.5m,1.9m]，根據公式可得目標距離誤差ΔD最大為d/5hm。

(2)雷達點云的切向誤差：

根據本實驗所使用毫米波雷達的參數說明書，本款雷達的角分辨率為1.4°，因此雷達點云點相對于雷達的切向誤差最大約為1/40Dm。

3.2 雷達點云預處理

本文設計的粒子濾波算法使用雷達點云的相關參數對粒子的權值進行更新。雷達點云含有大量噪聲，本文設計了一種算法，借助上一節介紹的參數對雷達點云進行校正、篩選。

相機傳感器具有很好的角度分辨率，但距離分辨率較差。而雷達傳感器正好相反，距離分辨率優良角度分辨率較差。為融合二者優點本文采取以下融合方法：

(1)借助針孔模型得到行人位置坐標。

(2)(考慮到相機有很高的角分辨率，因此本文以相機檢測到的目標方位角作為基準，對雷達點云進行篩選)以相機為坐標原點，向行人位置坐標射出一條射線。

(3)由前文可知目標距離誤差最大為d/5hm, 雷達點云切向誤差最大為1/40Dm。因此以預估的目標位置為中心建立一個長為2d/5hm、寬為1/20Dm的方框作為采樣區間，并將方框所覆蓋的點云點映射到第2步確定的射線上(點云點與雷達的距離保持不變)，其余的點云點丟棄。

(4)將第3步所得的點云點作為本次迭代的觀測值。以上操作如圖7所示。

3.3 從運動模型采樣

粒子濾波分為3步：采樣、權值更新、重采樣。

假設粒子在每次檢測間隔ΔT內速度恒定，即粒子速度等于粒子在ΔT內運動的距離除以ΔT。然而這種運動模型存在誤差，因此引入高斯噪聲εi。εi的協方差定義如式(8)所示，這樣設計有兩個原因：①基于恒速假設得到的速度與實際速度有誤差，可以借助參數α進行部分補償；②粒子有加速度，所以檢測間隔ΔT越短，誤差越小。

εi的均值為粒子以當前速度在ΔT內的運動距離。定義了所有參數后，位置預測公式如下

(6)

(7)

(8)

3.4 通過觀測模型修正粒子的權值

(9)

μj=[xj,yj]

(10)

(11)

其中，M表示當前雷達點云的點的個數。μj=[xj,yj] 表示當前幀第j個點云點的坐標。Ij為第j個點云點的信號強度。Cola,b為不同目標間的顏色差值。參數δ=28。

4 實驗結果和分析

4.1 實驗設備和參數

本實驗使用的設備是TI公司的MMWCAS-RF-EVM 4D毫米波雷達，該款雷達由4塊AWR2243毫米波雷達芯片級聯而成，具有高穿透特性、高信噪比和高角分辨率的特點。其擁有192個信道是普通毫米波雷達的16倍，并且可在140°水平、60°縱向的寬視場中提供0.8°水平、1.4°縱向的角分辨率。雷達的優異性能大大提高了本文算法的跟蹤精度。此外，該款雷達能探測距離超過350 m的大型物體(例如汽車和卡車)，距離分辨率為35 cm；人體RCS可探測物體能在150 m的距離內被探測到。雷達有波束成形和MIMO兩種工作模式，本實驗主要在MIMO模式下進行。

本實驗使用的視覺傳感器型號為PointGrey Blackfly S USB3.0，采集分辨率為2048×2048像素，RGB模式下幀頻率為25幀/s。

本實驗控制平臺配置如下：Intel(R)Core(TM)i5-10210U CPU@1.60 GHz，16 G內存，Ubuntu 16.04操作系統。目標檢測程序運行環境如下：Python版本為3.5，Anaconda版本為4.2.0。訓練神經網絡選用的數據集為nuscenes[9]。粒子濾波程序運行環境為matlab。

4.2 參數優化

對式(8)中的參數α和式(11)中的參數δ進行優化。取濾波的結果與真實運動軌跡的均方根誤差(RMSE)作為評價標準，RMSE反映了濾波器粒子在跟蹤過程中偏離真實軌跡的程度。

為了平衡不同運動場景對參數的影響，為參數優化選取4個典型運動場景：①單個行人做直線運動；②單個行人做圓周運動；③兩個行人做十字交叉運動；④兩個行人做相向運動。為了防止過擬合，將4個測試場景中得到的4個RMSE的平均值作為最終值。

經過多次測試，發現當α被設置為大于4的值時，粒子不會收斂到任何目標上，因此設置α測試區間為[0,4]，結果如圖8(a)所示。觀察圖8(a)發現當α設置為較小的值時能得到更好的結果，因此修改測試區間為[0,0.5]再次進行實驗。

在實驗場地測試得到的雷達信號強度的變化范圍為[20,50]，所以設置δ的測試區間為[1,100]，使得測試的標準偏差區間為[0.14 m,2.24 m]在可接受范圍內。

最終實驗結果如圖8(b)、圖8(c)所示，可得當α=0.1、δ=28時對應的均方根誤差最小。

4.3 算法測試結果與對比

為驗證本文算法在行人目標被遮擋情況下的檢測性能，在實驗場地對算法進行測試，并與Martin[10]開發的最新的融合算法結果作對比。此外，為提高測試樣本多樣性，選取nuscenes數據集中存在行人目標被遮擋情況的樣本集進行測試。考慮到雷達采樣頻率為10幀/s，相機采樣頻率為25幀/s，本實驗首先通過時間配準將雷達和相機的采樣頻率同步為10幀/s。單次實驗的雷達采樣數設置為400幀，粒子濾波器的粒子總數設置為5000。測試結果如下：

圖9～圖12為4種不同場景中行人目標被遮擋情況下的測試結果，其中每幅圖的圖(a)、圖(c)為Martin算法的檢測和跟蹤結果，圖(b)、圖(d)為本文算法的檢測和跟蹤結果。圖9為室內場景中的測試結果對比，可以看到圖9(a)中被部分遮擋的目標沒有被檢測出來，導致圖9(c)中的跟蹤軌跡出現了明顯的間斷，并且當被遮擋的目標再次被檢測到時，Martin算法將其錯誤判斷成新出現的目標。從圖9(b)、圖9(d)中可發現本文設計的跟蹤算法能在目標被部分遮擋時，仍然對目標精確跟蹤。圖10為道路場景中的測試結果對比，可以發現Martin算法錯誤的將3個距離很近的目標識別為同一目標，而本文的算法能夠準確區分每個目標。圖11為夜間場景中的測試結果對比，可以發現相機傳感器幾乎看不到兩個行人，因此僅通過圖像無法檢測到他們，但基于融合數據的網絡能夠檢測到它們。圖12為雨天場景中的測試結果對比，與夜晚的場景類似兩個行人只能被雷達探測到，Martin算法只能在目標距離較近時正確檢測，當目標距離較遠時跟蹤失敗，但本文算法始終能成功檢測并跟蹤目標。

為了進一步驗證本文算法的優勢，采取對照實驗，將本文提出的跟蹤算法分別與Martin等[10]、吳憲等[11]、劉志強等[12]開發的融合算法進行對比。在實驗場地通過改變行人的個數和行人與傳感器的距離進行了多組實驗，實驗包括4種運動模式：①實驗1和實驗2為單個行人做圓周運動；②實驗3和實驗4為兩個行人做十字交叉運動；③實驗5和實驗6為3個行人做“川”字運動，其中最外圍的兩個行人與中間行人的運動方向相反；④實驗7和實驗8為4個行人做“豐”字運動，其中橫向運動的三人各自的運動方向與相鄰的行人相反。對比跟蹤結果與真實運動軌跡的均方根誤差(RMSE)得到表1。我們發現雖然4種方法的跟蹤精度都隨距離的增加而降低，但本文算法的平均RMSE達到了1.2 m，相比之下，Martin跟蹤算法的平均RMSE為1.38 m，吳的為1.33 m，劉的為1.39 m。顯然，本文提出的融合算法對目標的跟蹤效果更精確，RMSE提高了超過10%。相較于其它算法，本文的算法使用CRFNet作為檢測網絡，大大降低了光線等外部條件對算法的干擾，提高了跟蹤精度。此外，本文的融合算法借助相機測量結果對點云數據進行了矯正，并能夠在相機丟失目標時利用雷達點云繼續對目標進行跟蹤，最終使精度提高了約10%。

最后對本文、Martin、吳和劉的算法在時間延遲和丟失率上進行了定量比較，結果見表2。劉的算法相比于Martin、吳的算法在時間延遲方面有了很大改善，但在丟失率方面改進不大。本文算法相比于Martin、吳和劉的算法在目標丟失率方面大大改善，并將時間延遲縮短至4 ms，滿足了系統對實時性的要求。

表1 對照實驗的均方根誤差

表2 各檢測方法性能對比

5 結束語

本文利用CRFNet神經網絡對相機和雷達信號進行融合，提高了算法在復雜光線與惡劣氣候條件下的跟蹤精度，增強了系統的魯棒性。設計了一種基于針孔模型與傳感器誤差參數的融合算法，大大降低了雷達噪聲與傳感器誤差對跟蹤結果的影響。此外，本文還設計了一種改進的粒子濾波算法，在相機丟失目標時仍能借助雷達數據繼續跟蹤目標。

與傳統融合跟蹤算法相比精度上提高了10%,并一定程度上解決了Martin等融合跟蹤算法在目標被遮擋情況下易跟蹤失敗的問題。