基于擴(kuò)展卡爾曼濾波的交互式多模型跟蹤算法研究

摘" 要： 在輔助駕駛系統(tǒng)中，行人軌跡跟蹤一直是一項(xiàng)有挑戰(zhàn)性的任務(wù)，因?yàn)樾腥说幕夭ㄐ盘?hào)中往往存在著許多干擾噪聲。此外，行人在運(yùn)動(dòng)過(guò)程中可能會(huì)做出突然轉(zhuǎn)身或其他改變方向的行為，這將直接導(dǎo)致行人運(yùn)動(dòng)軌跡呈現(xiàn)出非線性特征。針對(duì)上述問(wèn)題，文中提出一種基于擴(kuò)展卡爾曼濾波的交互式多模型跟蹤（IMM?EKF）方法，適用于毫米波雷達(dá)對(duì)行人進(jìn)行軌跡跟蹤。首先，在擴(kuò)展卡爾曼濾波算法（EKF）的基礎(chǔ)上重構(gòu)狀態(tài)預(yù)測(cè)協(xié)方差矩陣，來(lái)補(bǔ)償EKF非線性化過(guò)程中引入的誤差；然后將改進(jìn)的EKF作為交互式多模型算法（IMM）中的濾波器，根據(jù)行人運(yùn)動(dòng)特性選擇勻速模型和協(xié)調(diào)轉(zhuǎn)彎模型作為跟蹤模型，利用所提出的IMM?EKF算法進(jìn)行軌跡跟蹤。實(shí)驗(yàn)結(jié)果表明，所提出的濾波算法較典型的EKF和改進(jìn)的EKF算法，在跟蹤濾波精度方面均有所提升，同時(shí)具備更優(yōu)的跟蹤魯棒性。

關(guān)鍵詞： 行人軌跡跟蹤； 擴(kuò)展卡爾曼濾波； 交互式多模型； 毫米波雷達(dá)； 狀態(tài)預(yù)測(cè)協(xié)方差矩陣； 輔助駕駛

中圖分類號(hào)： TN953+.6?34" " " " " " " " " " " " 文獻(xiàn)標(biāo)識(shí)碼： A" " " " " " " " " " " " 文章編號(hào)： 1004?373X（2024）13?0071?06

Research on interacting multiple model tracking algorithm

based on extended Kalman filter

CHEN Xiaonan1， ZHANG Zikuo1， SUO Jidong1， LUO Chaofa2， DU Zhenbang1

（1. School of Information Science and Technology， Dalian Maritime University， Dalian 116026， China;

2. State Power Investment Group Shandong Marine Energy Development Co.， Ltd.， Qingdao 266034， China）

Abstract： Pedestrian track tracking is always a challenging task in the assisted driving systems， because there is often a lot of interference noise in the pedestrian echo signal. In addition， pedestrians may make sudden turns or other directional changes during movements， which will directly lead to nonlinear features of pedestrian movement trajectory. In view of the above， an interacting multiple model tracking method based on extended Kalman filter （IMM?EKF） is proposed， which is suitable for millimeter?wave radar to track pedestrians. On the basis of the extended Kalman filter （EKF） algorithm， the state prediction covariance matrix is reconstructed to compensate the errors brought in the nonlinear process of EKF， and then the improved EKF is used as the filter in the interacting multiple model algorithm （IMM）. According to pedestrian movement characteristics， uniform speed model and coordinated turn model are selected as tracking models， and the proposed IMM?EKF algorithm is used for track tracking. The experimental results show that the proposed filtering algorithm can improve the accuracy of tracking filter to some extent and has better tracking robustness in comparison with the typical EKF algorithm and the improved EKF algorithm.

Keywords： pedestrian track tracking; EKF; IMM; millimeter?wave radar; state prediction covariance matrix; assisted driving

0" 引" 言

行人軌跡跟蹤作為輔助駕駛感知系統(tǒng)中的重要組成部分，在事故預(yù)防中發(fā)揮著重要作用，已成為近幾年的熱門(mén)研究方向[1?8]。毫米波雷達(dá)是輔助駕駛系統(tǒng)中常見(jiàn)的感知設(shè)備之一，能夠直接獲取目標(biāo)距離、徑向速度及方位角信息，且具備全天候的性能優(yōu)勢(shì)[9?10]。此外，調(diào)頻連續(xù)波多輸入多輸出雷達(dá)還具有硬件成本低和封裝尺寸小的優(yōu)點(diǎn)。因此，本文利用毫米波雷達(dá)實(shí)現(xiàn)行人軌跡跟蹤。

由于行人在移動(dòng)過(guò)程中可能會(huì)經(jīng)歷各種復(fù)雜的動(dòng)作和變化，例如突然轉(zhuǎn)身、避障和避讓，這將直接導(dǎo)致行人的運(yùn)動(dòng)軌跡呈現(xiàn)非線性的特征。因此，典型的線性軌跡跟蹤方法可能表現(xiàn)不佳，需要建立適用于非線性系統(tǒng)下的濾波算法。文獻(xiàn)[11]中提出了一種自適應(yīng)的跟蹤門(mén)，利用交互式多模型算法（IMM）跟蹤目標(biāo)，但由于選用卡爾曼濾波作為IMM的濾波器，其跟蹤濾波精度有待提高。文獻(xiàn)[12]中提出了一種基于微多普勒的腿部跟蹤框架，并運(yùn)用擴(kuò)展卡爾曼濾波（Extended Kalman Filter， EKF）算法進(jìn)行軌跡跟蹤，但由于需要不斷地推導(dǎo)腿部的運(yùn)動(dòng)模型，會(huì)因此帶來(lái)系統(tǒng)的響應(yīng)滯后。文獻(xiàn)[13]中提出了一種加權(quán)相似度算法，提高了相鄰幀之間同一目標(biāo)的匹配率，解決了運(yùn)動(dòng)過(guò)程中目標(biāo)軌跡修正問(wèn)題，但該算法對(duì)量測(cè)值的依賴性過(guò)高，會(huì)導(dǎo)致跟蹤的魯棒性不足。

針對(duì)行人的運(yùn)動(dòng)狀態(tài)和方式復(fù)雜多變的問(wèn)題，本文提出了一種基于擴(kuò)展卡爾曼濾波的交互式多模型跟蹤方法。其首先對(duì)EKF的狀態(tài)預(yù)測(cè)協(xié)方差矩陣進(jìn)行重構(gòu)，來(lái)補(bǔ)償EKF非線性化過(guò)程中引入的誤差；然后將改進(jìn)后的EKF作為IMM算法中的非線性濾波器，實(shí)現(xiàn)了毫米波雷達(dá)對(duì)行人運(yùn)動(dòng)軌跡的穩(wěn)定跟蹤。

1" 系統(tǒng)模型

在笛卡爾坐標(biāo)系下，MIMO雷達(dá)位于原點(diǎn)（0，0）處，[xk=rkcosθk]和[yk=rksinθk]分別為目標(biāo)在[x]軸和[y]軸下的距離投影。其中，[rk]為第[k]幀的目標(biāo)與雷達(dá)之間的距離；[θk]為目標(biāo)與[x]坐標(biāo)軸之間的夾角。因此，[xk+1]和[yk+1]可以表示為：

[xk+1=xk+vkxTyk+1=yk+vkyT] （1）

式中：[vkx]和[vky]分別表示第[k]幀的目標(biāo)速度在[x]軸和[y]軸上的投影；[T]表示連續(xù)幀的間隔。此時(shí)，系統(tǒng)模型可以構(gòu)造為：

[Xk+1=F?Xk+Wk] （2）

式中：[Xk=[xk，vkx，yk，vky]T]表示第[k]幀的狀態(tài)向量；[Wk]表示第[k]幀的系統(tǒng)過(guò)程噪聲；[F]表示狀態(tài)轉(zhuǎn)移矩陣。在本文中將采用勻速（CV）模型和協(xié)調(diào)轉(zhuǎn)彎（CT）模型作為目標(biāo)運(yùn)動(dòng)模型，它們的狀態(tài)轉(zhuǎn)移矩陣分別用[FCV]和[FCT]來(lái)表示。

[FCV=1T000100001T0001] （3）

[FCT=1sin（ηT）η0-1-cos（ηT）η0cos（ηT）0-sin（ηT）01-cos（ηT）η1sin（ηT）η0sin（ηT）0cos（ηT）] （4）

式中[η]表示轉(zhuǎn)換率。通過(guò)二維FFT、CFAR檢測(cè)以及 DOA估計(jì)算法可得到目標(biāo)在第[k]幀的距離、徑向速度、方位角。

根據(jù)測(cè)量向量與狀態(tài)的關(guān)系，測(cè)量模型可表示為：

[Zk=h（Xk）+ξk] （5）

式中：[ξk]表示第[k]幀的測(cè)量噪聲；[h（Xk）]表示非線性映射函數(shù)。[h（Xk）]可表示為：

[h（Xk）=rk=x2k+y2kθk=arctan（ykxk）] （6）

2" 跟蹤濾波算法

2.1" 擴(kuò)展卡爾曼濾波算法及改進(jìn)

擴(kuò)展卡爾曼濾波是雷達(dá)目標(biāo)跟蹤常用的方法之一，其基本思想是利用線性化方法將表現(xiàn)為非線性規(guī)律的系統(tǒng)轉(zhuǎn)化為一個(gè)近似的線性濾波問(wèn)題，具體算法步驟如下所示。

1） 進(jìn)行狀態(tài)一步預(yù)測(cè)：

[Xpk+1=F?Xk+Wk] （7）

[Ppk+1=FPkFT+Q] （8）

式中：[Xpk+1]為預(yù)測(cè)狀態(tài)變量；[Ppk+1]為預(yù)測(cè)協(xié)方差矩陣；[Q]為過(guò)程噪聲協(xié)方差矩陣。

2） 計(jì)算卡爾曼增益：

[Kk+1=Ppk+1HT（HPpk+1HT+R）-1] （9）

式中：[Kk]為第[k]幀的卡爾曼增益；[H]為雅可比矩陣；[R]為測(cè)量協(xié)方差矩陣。

3） 更新?tīng)顟B(tài)估計(jì)：

[Xk+1=Xpk+1+Kk+1（Zk+1-h（Xpk+1））] （10）

[Pk+1=Ppk+1-Kk+1HPpk+1] （11）

上述為典型的EKF濾波算法步驟，但是EKF算法在線性化過(guò)程中忽略了泰勒級(jí)數(shù)展開(kāi)式中的高階項(xiàng)，不可避免地引入了誤差。隨著誤差的不斷累積，甚至?xí)?dǎo)致EKF濾波發(fā)散。針對(duì)以上問(wèn)題，本文提出了一種改進(jìn)方法，通過(guò)利用對(duì)角陣[Γ=diag?k+1， ?k+1gt;1]重構(gòu)狀態(tài)預(yù)測(cè)協(xié)方差矩陣，此時(shí)的狀態(tài)預(yù)測(cè)協(xié)方差矩陣[Ppk+1]可表示為：

[Ppk+1=ΓTPpk+1Γ] （12）

式（9）中卡爾曼增益可表示為：

[Kk+1=Ppk+1HT（HPpk+1HT+R）-1] （13）

式（11）中更新后的協(xié)方差矩陣可表示為：

[Pk+1=Ppk+1-Kk+1HPpk+1] （14）

2.2" IMM?EKF算法

交互式多模型算法由針對(duì)每個(gè)模型運(yùn)用的濾波模塊、模型概率評(píng)估模塊和估計(jì)組合模塊組成。將改進(jìn)的EKF算法作為IMM算法中的非線性濾波器，即為本文所提出的IMM?EKF算法。每個(gè)濾波器分別在測(cè)量的開(kāi)始使用混合估計(jì)值，以便計(jì)算新的估計(jì)值和濾波器內(nèi)模型的可能性。依次使用概率、先驗(yàn)?zāi)Ｐ透怕屎湍Ｐ娃D(zhuǎn)換概率計(jì)算新模型的概率。然后，使用新的狀態(tài)估計(jì)計(jì)算總體狀態(tài)估計(jì)。IMM?EKF算法流程圖如圖1所示。

假設(shè)有[m]個(gè)相互作用的模型，且模型[j]在[k]+1時(shí)刻有效，此時(shí)計(jì)算模型[i]在[k]時(shí)刻有效的概率可定義為：

[μijkk=1cjpijμi（k），" " "i，j=1，2，…，m] （15）

式中：[cj]為歸一化常數(shù)；[μi]為模型概率；[pij]為模型轉(zhuǎn)換概率。

通過(guò)使用前一時(shí)刻的狀態(tài)估計(jì)[Xikk]來(lái)計(jì)算與每個(gè)濾波器相對(duì)應(yīng)的混合狀態(tài)：

[X0jkk=i=1mXikkμijkk，" "j=1，2，…，m] （16）

當(dāng)[m]=2時(shí)，將上述估計(jì)作為[m]個(gè)模型的輸入，此時(shí)輸出為[Xjk+1k+1]。然后計(jì)算[m]個(gè)濾波器對(duì)應(yīng)的似然函數(shù)為：

[Λj（k）=exp-zTj（k）S-1j（k）zj（k）22πSj（k）] （17）

式中：[zj]為測(cè)量殘差；[Sj]為殘差協(xié)方差矩陣。此時(shí)更新每個(gè)模型的概率。

[μj（k+1）=1cΛj（k+1）cj，" " j=1，2，…，m] （18）

式中歸一化常數(shù)[c]定義為：

[c=i=1mΛi（k+1）ci] （19）

最后，[m]個(gè)估計(jì)值合并為式（20）。這些值用作系統(tǒng)的實(shí)際輸出，但不作為IMM?EKF算法遞歸的一部分。

[X（k+1k+1）=j=1mXjk+1k+1μj（k+1）] （20）

3" 實(shí)驗(yàn)驗(yàn)證

3.1" 濾波算法仿真

本文選取典型的EKF和改進(jìn)的EKF算法作為對(duì)比算法，驗(yàn)證所提出的IMM?EKF算法的有效性。建立[o?xy]直角坐標(biāo)系，雷達(dá)位于原點(diǎn)（0，0）處，不同濾波算法的軌跡如圖2所示。模擬目標(biāo)位于（1，11）處，運(yùn)動(dòng)模型之間的轉(zhuǎn)換先后經(jīng)歷了由CV到CT，CT到CV，CV到CT的過(guò)程。從圖2的局部放大圖中可以看出，相比于其他兩種算法，所提出的IMM?EKF算法在目標(biāo)轉(zhuǎn)彎過(guò)程中的效果更優(yōu)。

圖3為不同濾波算法的均方根誤差（RMSE）對(duì)比圖。從圖3中可以看出，IMM?EKF在整個(gè)過(guò)程中收斂性更好，且有效解決了EKF線性化過(guò)程中由于誤差累積導(dǎo)致的濾波發(fā)散問(wèn)題。為了進(jìn)一步說(shuō)明本文所提出的IMM?EKF算法的有效性，將改進(jìn)前后不同運(yùn)動(dòng)模型下的EKF算法和IMM?EKF算法的RMSE均值對(duì)比結(jié)果在表1中列出。

從表1中的數(shù)據(jù)可知，IMM?EKF算法的RMSE均值小于單模型下的EKF濾波算法。同時(shí)，采用重構(gòu)后的狀態(tài)預(yù)測(cè)協(xié)方差矩陣的EKF濾波算法的RMSE均值更小。相比于采用典型的EKF作為IMM算法的濾波器，以改進(jìn)的EKF為濾波器的IMM算法的跟蹤精度提高了22.32%。驗(yàn)證了本文所提出的IMM?EKF濾波算法可以獲取更優(yōu)的目標(biāo)估計(jì)狀態(tài)，并適合用于對(duì)非線性運(yùn)動(dòng)目標(biāo)的跟蹤。

3.2" 實(shí)測(cè)驗(yàn)證及分析

本實(shí)驗(yàn)所用的77 GHz調(diào)頻連續(xù)波多輸入多輸出雷達(dá)（MIMO?FMCW）系統(tǒng)搭載德州儀器公司的AWR1843芯片。本文共選取了兩個(gè)實(shí)驗(yàn)場(chǎng)景，如圖4所示，實(shí)驗(yàn)場(chǎng)景一中只有一個(gè)行人且作曲線運(yùn)動(dòng)。如圖5所示，實(shí)驗(yàn)場(chǎng)景二中有兩個(gè)行人分別位于雷達(dá)兩側(cè)，兩人都是先遠(yuǎn)離雷達(dá)然后轉(zhuǎn)身再朝向雷達(dá)運(yùn)動(dòng)。

跟蹤濾波過(guò)程中分別采用EKF濾波算法、改進(jìn)的EKF濾波算法以及改進(jìn)的IMM?EKF濾波算法，對(duì)兩個(gè)實(shí)驗(yàn)場(chǎng)景中的行人軌跡進(jìn)行了濾波跟蹤。實(shí)驗(yàn)所用的雷達(dá)采集參數(shù)配置如表2所示，不同濾波算法下的單人運(yùn)動(dòng)軌跡如圖6所示。

從圖6中可以看出，所提出的IMM?EKF濾波算法對(duì)行人目標(biāo)進(jìn)行跟蹤相比于EKF和改進(jìn)的EKF算法的濾波軌跡更為平滑。對(duì)量測(cè)點(diǎn)分別沿[x]和[y]軸方向進(jìn)行狀態(tài)分解并使用三次多項(xiàng)式曲線擬合計(jì)算行人的估計(jì)軌跡，將擬合結(jié)果作為真實(shí)軌跡的估計(jì)值，然后分別計(jì)算不同濾波算法的RMSE并進(jìn)行對(duì)比。實(shí)驗(yàn)場(chǎng)景一下不同濾波算法的RMSE對(duì)比如圖7所示。從圖7中的RMSE值來(lái)看，所提出的IMM?EKF算法估計(jì)誤差值在整體上比較穩(wěn)定，大多數(shù)時(shí)候其誤差值最小且收斂性更好。

不同濾波算法下的雙人運(yùn)動(dòng)軌跡如圖8所示。圖8中的局部放大圖反映了行人轉(zhuǎn)身由原來(lái)的遠(yuǎn)離雷達(dá)變?yōu)槌蚶走_(dá)行走過(guò)程中的不同濾波算法軌跡跟蹤的細(xì)節(jié)，從圖中可以看出，在行人轉(zhuǎn)身過(guò)程中使用EKF濾波算法的跟蹤精度急劇降低，而使用改進(jìn)的EKF和改進(jìn)的IMM?EKF則可以有效優(yōu)化該運(yùn)動(dòng)過(guò)程的跟蹤軌跡，提升跟蹤的性能。

實(shí)驗(yàn)場(chǎng)景二下的不同濾波算法的距離RMSE對(duì)比如圖9所示，圖9中目標(biāo)1為圖8中的左側(cè)目標(biāo)，目標(biāo)2為圖8中的右側(cè)目標(biāo)。從圖9中可以明顯看出，采用本文提出的改進(jìn)IMM?EKF濾波算法時(shí)，整體上的跟蹤魯棒性更好，收斂性也優(yōu)于其他兩種濾波算法。另外，在行人轉(zhuǎn)身過(guò)程中，可以明顯地看出采用改進(jìn)的IMM?EKF濾波算法的距離RMSE較小，提高了跟蹤濾波精度。

不同場(chǎng)景下的濾波算法RMSE均值對(duì)比如表3所示。從表3中可知，本文中提出的改進(jìn)IMM?EKF濾波算法相較于其他兩種算法，在兩種實(shí)驗(yàn)場(chǎng)景下所表現(xiàn)的濾波跟蹤性能均有所提升。

在實(shí)驗(yàn)場(chǎng)景一中，采用改進(jìn)的IMM?EKF算法的跟蹤精度相比于EKF算法提高了60.43%；和改進(jìn)的EKF算法的相比，跟蹤精度提高了21.62%。

在實(shí)驗(yàn)場(chǎng)景二中，采用改進(jìn)的IMM?EKF算法后，雷達(dá)前方左側(cè)行人軌跡跟蹤精度相對(duì)于其他兩種濾波算法分別提高了44.30%和26.72%；雷達(dá)前方右側(cè)行人軌跡跟蹤精度相對(duì)于其他兩種濾波算法分別提高了57.38%和34.67%。

4" 結(jié)" 語(yǔ)

為解決由于行人運(yùn)動(dòng)的不規(guī)律性導(dǎo)致的跟蹤誤差大的問(wèn)題，尤其在改變運(yùn)動(dòng)方向時(shí)，比如進(jìn)行曲線運(yùn)動(dòng)和轉(zhuǎn)身。本文提出了一種基于擴(kuò)展卡爾曼濾波的交互式多模型跟蹤方法，減少了行人在突然改變運(yùn)動(dòng)方向時(shí)的跟蹤誤差，提高了目標(biāo)跟蹤的魯棒性。通過(guò)仿真和實(shí)測(cè)實(shí)驗(yàn)證明了所提出算法的有效性，并具有較高的實(shí)際應(yīng)用價(jià)值。

參考文獻(xiàn)

[1] ENGELS F， HEIDENREICH P， WINTERMANTEL M， et al. Automotive radar signal processing： Research directions and practical challenges [J]. IEEE journal of selected topics in signal processing， 2021， 15（4）： 865?878.

[2] WILL C， VAISHNAV P， CHAKRABORTY A， et al. Human target detection， tracking， and classification using 24?GHz FMCW radar [J]. IEEE sensors journal， 2019， 19（17）： 7283?7299.

[3] 邱成，王浩，劉鳳江，等.基于EKF的毫米波雷達(dá)多目標(biāo)跟蹤算法研究[J].現(xiàn)代電子技術(shù)，2021，44（15）：7?11.

[4] CUI H， DAHNOUN N. High precision human detection and tracking using millimeter?wave radars [J]. IEEE aerospace and electronic systems magazine， 2021， 36（1）： 22?32.

[5] 沈一心.智能輔助駕駛系統(tǒng)中的行人檢測(cè)[J].中阿科技論壇（中英文），2023（7）：107?111.

[6] BU F， LE T， DU X X， et al. Pedestrian planar LiDAR pose （PPLP） network for oriented pedestrian detection based on planar LiDAR and monocular images [J]. IEEE robotics and automation letters， 2020， 5（2）： 1626?1633.

[7] SINGH A D， SANDHA S S， GARCIA L， et al. RadHAR： Human activity recognition from point clouds generated through a millimeter?wave radar [C]// Proceedings of the 3rd ACM Workshop on Millimeter?wave Networks and Sensing Systems. New York： IEEE， 2019： 51?56.

[8] VAISHNAV P， SANTRA A. Continuous human activity classification with unscented Kalman filter tracking using FMCW radar [J]. IEEE sensors letters， 2020， 4（5）： 1?4.

[9] SUN S Q， PETROPULU A P， POOR H V. MIMO radar for advanced driver?assistance systems and autonomous driving： Advantages and challenges [J]. IEEE signal processing magazine， 2020， 37（4）： 98?117.

[10] VENON A， DUPUIS Y， VASSEUR P， et al. Millimeter wave FMCW RADARs for perception， recognition and localization in automotive applications： A survey [J]. IEEE transactions on intelligent vehicles， 2022， 7（3）： 533?555.

[11] 鞠夕強(qiáng)，孟文.基于毫米波雷達(dá)的機(jī)動(dòng)目標(biāo)跟蹤算法研究[J].計(jì)算機(jī)與數(shù)字工程，2023，51（6）：1297?1301.

[12] HELD P， STEINHAUSER D， KOCH A， et al. A novel approach for model?based pedestrian tracking using automotive radar [J]. IEEE transactions on intelligent transportation systems， 2022， 23（7）： 7082?7095.

[13] CHEN R Q， ZOU Y Q， GAO A Y， et al. A cluster?based weighted feature similarity moving target tracking algorithm for automotive FMCW radar [C]// 2022 IEEE 95th Vehicular Technology Conference （VTC2022?Spring）. New York： IEEE， 2022： 1?5.