基于HS?FCM模糊聚類的快速多目標(biāo)車輛跟蹤算法*

章軍輝，付宗杰，郭曉滿，李 慶，陳大鵬，3，趙 野

（1.中國科學(xué)院微電子研究所，北京100029；2.江蘇物聯(lián)網(wǎng)研究發(fā)展中心，無錫214135；3.無錫物聯(lián)網(wǎng)創(chuàng)新中心有限公司，無錫214135；4.昆山微電子技術(shù)研究院，蘇州215347）

前言

實現(xiàn)精準(zhǔn)可靠的目標(biāo)軌跡跟蹤是多目標(biāo)跟蹤（multi?object tracking，MOT）的主要任務(wù)，而強雜波環(huán)境下的數(shù)據(jù)關(guān)聯(lián)、多源數(shù)據(jù)融合策略等問題一直是MOT領(lǐng)域研究的熱難點［1-3］。MOT問題的一般處理流程如圖1所示。

圖1 多目標(biāo)跟蹤處理流程

目標(biāo)的表達(dá)通常是采用形狀或外觀來描述的。對于點表達(dá)的目標(biāo)跟蹤問題，點關(guān)聯(lián)的方法主要包括確定性方法與統(tǒng)計性方法。確定性方法通常是在一系列約束條件下尋求關(guān)聯(lián)成本最小化的解，如貪婪搜索法［4］。統(tǒng)計性方法利用位置、速度、加速度等目標(biāo)的屬性來建立狀態(tài)空間模型，并在一定程度上考慮了模型不確定性以及觀測噪聲，如聯(lián)合概率數(shù)據(jù)關(guān)聯(lián)（joint probability data association，JPDA）［5-6］、多假設(shè)跟蹤（multiple hypothesis tracking，MHT）［7-8］、概率多假設(shè)跟蹤（probabilistic multiple hypothesis tracking，PMHT）［9-10］等。對于量測數(shù)據(jù)落入跟蹤門相交區(qū)域的情況，將難以確定這些量測是源于哪個目標(biāo)?；谪惾~斯理論的JPDA算法能夠較好地解決雜波環(huán)境下相交波門內(nèi)的量測關(guān)聯(lián)問題，但在跟蹤目標(biāo)數(shù)較多或雜波較密集的情況下，存在確認(rèn)矩陣拆分組合爆炸的現(xiàn)象［6］?；谌徸顑?yōu)濾波器與Bar-Shalom確認(rèn)矩陣的MHT算法，通過建立多個候選關(guān)聯(lián)假設(shè)，并利用后續(xù)的量測來消除當(dāng)前掃描周期的不確定性，理論上其跟蹤效果是相對較優(yōu)的，不過與JPDA算法一樣，其計算復(fù)雜度隨著目標(biāo)與量測個數(shù)呈指數(shù)增長。PMHT算法是基于EM（expectation maximization）策略而設(shè)計的，并且認(rèn)為量測關(guān)聯(lián)過程是相互獨立的，故其關(guān)聯(lián)復(fù)雜度僅隨目標(biāo)個數(shù)呈線性增長，但在標(biāo)準(zhǔn)PMHT算法中所有量測結(jié)果皆源于不盡合理的目標(biāo)假設(shè)，所以跟蹤效果易受虛假量測的影響。

車輛跟隨行駛是日常道路交通環(huán)境中最主要的行車工況［11-12］。在強雜波環(huán)境下經(jīng)典數(shù)據(jù)關(guān)聯(lián)方法的跟蹤效果往往不太理想，主要原因如下：①關(guān)聯(lián)概率的計算復(fù)雜度較高，影響跟蹤效果的實時性［13-14］；②源于不同傳感器的數(shù)據(jù)在時空上存在著較大差異，加大了數(shù)據(jù)可靠關(guān)聯(lián)的難度［15-16］；③傳感器本身的局限性，導(dǎo)致量測存在著一定的不確定性［17-18］。

傳統(tǒng)的通過窮舉量測-目標(biāo)所有可能的關(guān)聯(lián)事件或關(guān)聯(lián)假設(shè)來估計最優(yōu)關(guān)聯(lián)的方法，無法完成精確求解，在一定程度上制約了技術(shù)的可實施性。考慮到點跡-航跡關(guān)聯(lián)判決本身存在著一定的模糊性與不確定性，本文中發(fā)展了一種快速多目標(biāo)車輛跟蹤算法。首先，對多目標(biāo)車輛跟蹤問題進行了數(shù)學(xué)描述，并建立了相機像素坐標(biāo)系與世界坐標(biāo)系的空間映射關(guān)系；其次，基于模糊理論將點跡-航跡關(guān)聯(lián)問題轉(zhuǎn)換成量測模糊聚類問題，通過求解各候選量測與聚類中心的模糊隸屬度，間接計算出JPDA算法中不確定性量測與各目標(biāo)的關(guān)聯(lián)概率，再利用概率加權(quán)融合對多目標(biāo)狀態(tài)進行濾波估計；再次，考慮到機動目標(biāo)的慣性因素，在車輛密集工況下通過合理調(diào)整卡爾曼增益對量測更新進行抑制，以解決目標(biāo)短暫跟丟問題；最后，通過實車試驗對該算法的跟蹤效果進行了驗證與探討。

1 多目標(biāo)觀測模型

1.1 數(shù)學(xué)建模

在車輛跟蹤過程中，車輛被視為一種小機動性目標(biāo)，常用的機動目標(biāo)運動模型有勻速運動模型、勻加速運動模型等。綜合考慮計算實時性與目標(biāo)的機動性，定義目標(biāo)運動模型為分段式勻速運動模型，并借助雷達(dá)或V2V模塊來動態(tài)更新各階段機動目標(biāo)的縱向速度。此外，對于視覺目標(biāo)跟蹤（visual object tracking，VOT）問題，考慮到相機測距的局限性，將輔以雷達(dá)來實時校正車間縱向距離。

1.1.1 多目標(biāo)觀測模型

在多目標(biāo)車輛跟蹤過程中，感興趣目標(biāo)（objects of interest，OOI）是位于主車道以及左右車道且距離自車最臨近的前方車輛。如圖2所示，忽略目標(biāo)車輛的垂向運動，以自車作為坐標(biāo)原點，建立車輛水平坐標(biāo)系。

圖2 多目標(biāo)車輛跟蹤的水平坐標(biāo)系

對于第i個感興趣的目標(biāo)車輛，不妨令該目標(biāo)的縱向距離是di，y(k)，縱向速度是vi，y(k)，橫向距離是di，x(k)，橫向速度是vi，x(k)，以xi(k)=[di，y(k)，vi，y(k)，di，x(k)，vi，x(k)]T作為狀態(tài)向量，yi(k)=[di，y(k)，di，x(k)]T作為觀測模型輸出，建立離散狀態(tài)空間方程：

式中：xi(k-1|k)表示基于k-1時刻對k時刻第i個目標(biāo)的狀態(tài)預(yù)測；yi(k)是對第i個目標(biāo)的量測預(yù)測；Fi是目標(biāo)i的狀態(tài)轉(zhuǎn)移矩陣；H是視覺傳感器的量測矩陣；Ci是狀態(tài)校正項；wi(k)是過程噪聲；v(k)是視覺傳感器的量測噪聲。其中，i∈[1，3]是目標(biāo)標(biāo)簽。

1.1.2 模型參數(shù)

假設(shè)過程噪聲wi(k)為服從N(0，Q)分布，量測噪聲v(k)服從N(0，R)分布。多目標(biāo)觀測模型中各系數(shù)矩陣滿足：

式中：Ts是采樣周期；Δvi，y是縱向相對速度，用于定期校正分段式勻速運動模型。

1.2 空間融合

如圖3所示，建立相機像素坐標(biāo)系與世界坐標(biāo)系的空間映射關(guān)系。

圖3 相機標(biāo)定模型

圖像數(shù)組（即像素坐標(biāo)）與像平面坐標(biāo)之間的關(guān)系如下：

式中：u、v分別表示像素在圖像數(shù)組中的列數(shù)、行數(shù)；(u0，v0)是投影中心點；dx、dy分別表示像素在像平面坐標(biāo)系中x軸、y軸上的物理尺寸。

像平面坐標(biāo)與相機坐標(biāo)之間的關(guān)系如下：

式中f是焦距。

相機坐標(biāo)與世界坐標(biāo)之間的關(guān)系如下：

式中：R3×3=Rx(α)Ry(β)Rz(θ)是旋轉(zhuǎn)矩陣；t3×1是平移向量。α、β、θ分別表示世界坐標(biāo)系中x軸、y軸、z軸的旋轉(zhuǎn)角，且滿足：

從而，三維世界點P與其在像平面上投影點p的關(guān)系滿足：

式中：ξ是比例因子；p=[u，v，1]T是二維圖像中投影點p的齊次坐標(biāo)；P=[Xw，Yw，Zw，1]T是三維世界點P的齊次坐標(biāo)是相機的內(nèi)參矩陣，用以實現(xiàn)相機3D坐標(biāo)到2D坐標(biāo)的轉(zhuǎn)換；[R3×3|t3×1]是相機的外參矩陣，用以實現(xiàn)將點從世界坐標(biāo)系到相機坐標(biāo)系的轉(zhuǎn)換。

2 多目標(biāo)狀態(tài)滾動估計

2.1 數(shù)據(jù)關(guān)聯(lián)

數(shù)據(jù)關(guān)聯(lián)是將源自各傳感器的量測數(shù)據(jù)與相應(yīng)的目標(biāo)航跡進行關(guān)聯(lián)的過程。對于回波落入跟蹤門重疊區(qū)域的情形，引入關(guān)聯(lián)概率，用以表示不確定性量測與各個源目標(biāo)之間的關(guān)聯(lián)程度。

2.1.1 定義

定義1.馬氏距離

定義L維模型預(yù)測向量與量測向量之間的馬氏距離為

式中：dM是馬氏距離；y是模型的預(yù)測向量；z是當(dāng)前周期內(nèi)的量測向量；Σ是協(xié)方差矩陣，且滿足

馬氏距離是一種加權(quán)歐氏距離，當(dāng)協(xié)方差矩陣為單位矩陣時，馬氏距離即為歐式距離。由于不同特征的度量標(biāo)準(zhǔn)之間存在著較大的差異，通過歐氏距離來量化評估往往缺乏一定的合理性，而馬氏距離在一定程度上歸一化了不同變量之間量綱的影響，使得特征之間的關(guān)系更加符合實際情況。然而在求解過程中存在協(xié)方差矩陣求逆運算，當(dāng)協(xié)方差矩陣為奇異矩陣時會影響到馬氏距離計算的數(shù)值穩(wěn)定性，為此要求總體樣本數(shù)大于樣本的維數(shù)，以確保協(xié)方差矩陣可逆。

定義2.有效觀測集

式中：z*是有效觀測集；δ是約束上界。

根據(jù)定義1與定義2，能夠為判斷回波是否落入目標(biāo)的跟蹤門內(nèi)提供一種量化依據(jù)。

2.1.2 問題描述

問題1.計算復(fù)雜度

隨著跟蹤目標(biāo)與有效回波數(shù)量的增多，對確認(rèn)矩陣的拆分次數(shù)將會驟增，一般呈指數(shù)增長趨勢，容易導(dǎo)致組合爆炸問題，影響計算實時性。

問題2.航跡合并現(xiàn)象

如圖4所示，三角形為目標(biāo)，黑點為量測。稀疏工況下，目標(biāo)之間相距較遠(yuǎn)，公共量測源來自于雜波的可能性較大；車輛密集工況下，落入公共區(qū)域的量測數(shù)相對偏多，用來更新目標(biāo)航跡的組合量測趨于接近，容易產(chǎn)生航跡合并問題，造成目標(biāo)短暫跟丟的現(xiàn)象。

圖4 測量分布特性

2.2 模糊聚類

對于一個航跡或目標(biāo)來說，所有落入其跟蹤波門內(nèi)的有效量測與目標(biāo)的關(guān)聯(lián)概率之和為1，構(gòu)成一個完備事件組，因此可將點跡-航跡關(guān)聯(lián)問題視為一種量測模糊聚類問題，通過求解量測樣本與聚類中心的隸屬度來間接地計算量測與目標(biāo)的關(guān)聯(lián)概率。

2.2.1 FCM算法

硬C-均值（hard c-means，HCM）聚類算法的隸屬度取值為0與1，聚類速度較快，但其對樣本的分類標(biāo)準(zhǔn)是硬性的，聚類結(jié)果往往不夠準(zhǔn)確。模糊C-均值（fuzzy c-means，F(xiàn)CM）聚類算法是基于HCM對樣本與聚簇之間的隸屬關(guān)系進行模糊化，將隸屬度取值從HCM算法的二值｛0，1｝擴展到［0，1］區(qū)間，使得聚類結(jié)果更為合理些。

令量測樣本集Z={z1，z2，…，zm}，簇類中心C={c1，c2，…，cn}，模糊隸屬度矩陣

定義聚類損失函數(shù)為

FCM算法是通過最小化聚類損失函數(shù)來實現(xiàn)量測的模糊分類。采用Lagrange乘子法，可得隸屬度、聚類中心的迭代式：

其中，迭代終止條件滿足|Δci|<ε，ε是約束上界，或者通過限制最大迭代次數(shù)來終止。

FCM算法存在的缺陷：①對初始聚類中心敏感；②目標(biāo)函數(shù)的非凸性易致求解極值陷入局部最優(yōu)，且收斂速度較慢；③未考慮特征的差異性，即有用特征與冗余特征對聚類的貢獻值相同。

2.2.2 S-FCM算法

抑制式模糊C-均值（suppressed fuzzy c-means，S-FCM）通過調(diào)整隸屬度來突出主要因素并抑制次要因素，以增強量測樣本對第p類聚類中心的吸引力，減弱對其余類聚類中心的吸引力，從而提高了算法的收斂速度［19］。

對隸屬度進行修正：

式中：μp，j=max{μi，j}；i∈[1，n]表示量測樣本對第p類聚類中心的隸屬度是極大的；α是抑制因子，α∈[0，1]，當(dāng)α=0時，S-FCM退化為HCM，當(dāng)α=1時，S-FCM即為FCM。

S-FCM算法存在的缺陷：①S-FCM提高了模糊聚類算法的收斂速度，但其聚類效果不及FCM；②當(dāng)最大隸屬度較小時，S-FCM存在過早收斂的隱患。

2.2.3 HS-FCM算法

半抑制式模糊C均值聚類（half suppressed fuzzy c-means，HS-FCM）算法的設(shè)計目標(biāo)是在收斂速度與聚類效果之間尋求平衡。引入抑制門限，對隸屬度進一步修正，即

當(dāng)i=p時，

當(dāng)i≠p時，

式中β是抑制門限，當(dāng)β越小時，HS-FCM算法效果越趨近于S-FCM，當(dāng)β越大時，HS-FCM算法效果越趨近于FCM。建議抑制因子α選取0.5，抑制門限β選取0.5。

2.3 狀態(tài)滾動估計算法

2.3.1 滾動估計

基于JPDA算法框架，通過互聯(lián)規(guī)則構(gòu)造統(tǒng)計距離，然后基于模糊數(shù)學(xué)中的模糊聚類方法，計算各候選量測與觀測區(qū)域內(nèi)不同目標(biāo)的關(guān)聯(lián)概率，再利用概率加權(quán)融合方法對各目標(biāo)狀態(tài)與協(xié)方差進行更新。下一次采樣時刻，時域移動一步，并重復(fù)上述過程，以實現(xiàn)對各目標(biāo)狀態(tài)的滾動估計。一步迭代算式如下。

（a）預(yù)測更新

式中：xi(k|k-1)表示對k時刻目標(biāo)i的狀態(tài)進行預(yù)測；Pi(k|k-1)是對k時刻目標(biāo)i狀態(tài)預(yù)測的協(xié)方差；Q是過程噪聲的協(xié)方差。

（b）關(guān)聯(lián)概率更新

基于HS-FCM算法得到量測樣本的隸屬度，對關(guān)聯(lián)概率進行更新。

（c）量測更新

式中：Si(k)是量測協(xié)方差；Gi(k)是卡爾曼增益；zj(k)是量測向量；δi，j(k)是量測新息；δi(k)是組合新息；μi，j(k)是k時刻量測j源于目標(biāo)i的概率；μi，0(k)表示沒有量測與目標(biāo)i關(guān)聯(lián)；xi，j(k)表示基于量測j對目標(biāo)i的標(biāo)準(zhǔn)卡爾曼濾波估計；xi(k)、Pi(k)分別為k時刻的濾波估計和狀態(tài)預(yù)測的協(xié)方差，作為下一步迭代的輸入。其中：

2.3.2 算法改進

為了減少計算量，對式（18）中的狀態(tài)估計協(xié)方差Pi(k)進行優(yōu)化：

相比式（18）而言，式（21）將無須額外計算各個量測j對目標(biāo)i的卡爾曼濾波估計xi，j(k)，從而在一定程度上降低了計算復(fù)雜度。

車輛密集工況下，雖然量測樣本與目標(biāo)之間的歐式距離較近，但該量測未必源于這個目標(biāo)，比如總體方差較小時，需要很接近才歸為該類，因此建議在車輛密集工況下采用馬氏距離代替歐式距離進行聚類。稀疏工況下仍采用歐式距離進行聚類，以期在聚類可靠性與計算復(fù)雜度之間尋求平衡。此外，考慮到機動目標(biāo)的慣性因素，在車輛密集工況下通過合理調(diào)整卡爾曼增益對量測更新進行抑制，以克服目標(biāo)短暫跟丟的問題。

3 仿真驗證

試驗場景為一段市郊道路工況，通過實車采集雷達(dá)與相機數(shù)據(jù)，再進行離線仿真驗證。雷達(dá)與相機的主要參數(shù)見表1。

表1 雷達(dá)與相機的主要參數(shù)

考慮到毫米波雷達(dá)較優(yōu)的縱向測距能力，本文中將雷達(dá)的縱向測距數(shù)據(jù)作為縱向參考，記為REF，將基于YOLO框架的視覺多目標(biāo)檢測算法，記為YOLO估計。

圖5 為在一段市郊道路工況下采用YOLO框架對多車道OOI目標(biāo)車輛進行檢測的結(jié)果。在車輛檢測跟蹤過程中，僅標(biāo)識感興趣的目標(biāo)車輛，這是由于OOI目標(biāo)車輛相對自車來說所構(gòu)成的追尾威脅較為明顯。

圖5 多車道OOI目標(biāo)車輛檢測結(jié)果

圖6 為主車道OOI目標(biāo)車輛縱向運動估計的對比情況。

圖6 主車道OOI目標(biāo)車輛縱向運動估計對比

由圖可見，在接近第300個采樣點時刻，雷達(dá)采樣點出現(xiàn)目標(biāo)短暫丟失現(xiàn)象，這是由于雷達(dá)可能探測到鄰近車輛，而妨礙了對前方車輛的精準(zhǔn)跟蹤。在300-400采樣區(qū)間，由于鄰道車輛的切入，更新了當(dāng)前跟蹤的OOI目標(biāo)車輛，進而導(dǎo)致跟蹤軌跡發(fā)生了陡變。

圖7 為基于改良JPDA濾波的主車道OOI目標(biāo)車輛縱向運動估計的對比情況。在車輛密集工況下，通過調(diào)整卡爾曼增益對量測更新進行抑制，以增加預(yù)測的置信度，基于改良JPDA濾波后的雷達(dá)估計能夠較好地解決圖6中雷達(dá)估計出現(xiàn)的目標(biāo)短暫丟失問題。此外，在200-800采樣區(qū)間，基于改良JPDA濾波后的YOLO估計曲線抖動較小，縱向距離的均方根誤差由0.118 3降為0.070 7，較好地抑制了視覺量測誤差，從而在一定程度上增強了視覺跟蹤性能。

圖7 改良后的主車道OOI目標(biāo)車輛縱向運動估計對比

圖8 為基于改良JPDA濾波與標(biāo)準(zhǔn)JPDA濾波估計的對比情況。兩者濾波后的軌跡都更接近于雷達(dá)的狀態(tài)估計，也進一步表明JPDA濾波對量測抖動有較好的抑制效果。此外，改良JPDA濾波避免了標(biāo)準(zhǔn)JPDA濾波過程中確認(rèn)矩陣復(fù)雜的拆分組合環(huán)節(jié)。

圖8 改良JPDA與標(biāo)準(zhǔn)JPDA濾波估計對比

圖9 為主車道OOI目標(biāo)車輛橫向運動估計的對比情況。在200-800采樣區(qū)間，雷達(dá)估計的橫向距離的均方根誤差為0.023 3，YOLO估計的橫向距離的均方根誤差為0.010 2。相對來說，雷達(dá)橫向跟蹤性能易受擾動因素影響。

圖9 主車道OOI目標(biāo)車輛橫向運動估計對比

圖10 與圖11分別為左右車道OOI目標(biāo)車輛運動估計的對比情況。整體上來看，基于改良JPDA的濾波算法能夠較好地實現(xiàn)多目標(biāo)跟蹤，且視覺的橫向跟蹤效果較優(yōu)，雷達(dá)的縱向跟蹤效果較佳。

圖10 左車道OOI目標(biāo)車輛運動估計對比

圖11 右車道OOI目標(biāo)車輛運動估計對比

4 結(jié)論

為了提高密集雜波環(huán)境下多目標(biāo)數(shù)據(jù)關(guān)聯(lián)的可靠性與實時性，本文中設(shè)計了一種多目標(biāo)車輛跟蹤算法，主要結(jié)論如下。

（1）基于模糊理論將點跡-航跡關(guān)聯(lián)問題轉(zhuǎn)換成量測模糊聚類問題，利用模糊隸屬度來代替JPDA算法中不確定性量測與各目標(biāo)的關(guān)聯(lián)概率，能夠一定程度上降低計算復(fù)雜度。

（2）在車輛密集工況下通過合理調(diào)整卡爾曼增益對量測更新進行抑制，能夠較好地解決目標(biāo)短暫跟丟的問題。

（3）在調(diào)試過程中發(fā)現(xiàn)，合理選取HS-FCM算法中的抑制因子與抑制門限以改善聚類效果，能夠一定程度上避免航跡合并現(xiàn)象，后續(xù)將進一步深入對該問題的研究。