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

摘" 要： 在輔助駕駛系統中，行人軌跡跟蹤一直是一項有挑戰性的任務，因為行人的回波信號中往往存在著許多干擾噪聲。此外，行人在運動過程中可能會做出突然轉身或其他改變方向的行為，這將直接導致行人運動軌跡呈現出非線性特征。針對上述問題，文中提出一種基于擴展卡爾曼濾波的交互式多模型跟蹤（IMM?EKF）方法，適用于毫米波雷達對行人進行軌跡跟蹤。首先，在擴展卡爾曼濾波算法（EKF）的基礎上重構狀態預測協方差矩陣，來補償EKF非線性化過程中引入的誤差；然后將改進的EKF作為交互式多模型算法（IMM）中的濾波器，根據行人運動特性選擇勻速模型和協調轉彎模型作為跟蹤模型，利用所提出的IMM?EKF算法進行軌跡跟蹤。實驗結果表明，所提出的濾波算法較典型的EKF和改進的EKF算法，在跟蹤濾波精度方面均有所提升，同時具備更優的跟蹤魯棒性。

關鍵詞： 行人軌跡跟蹤； 擴展卡爾曼濾波； 交互式多模型； 毫米波雷達； 狀態預測協方差矩陣； 輔助駕駛

中圖分類號： TN953+.6?34" " " " " " " " " " " " 文獻標識碼： A" " " " " " " " " " " " 文章編號： 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" 引" 言

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

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

針對行人的運動狀態和方式復雜多變的問題，本文提出了一種基于擴展卡爾曼濾波的交互式多模型跟蹤方法。其首先對EKF的狀態預測協方差矩陣進行重構，來補償EKF非線性化過程中引入的誤差；然后將改進后的EKF作為IMM算法中的非線性濾波器，實現了毫米波雷達對行人運動軌跡的穩定跟蹤。

1" 系統模型

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

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

式中：[vkx]和[vky]分別表示第[k]幀的目標速度在[x]軸和[y]軸上的投影；[T]表示連續幀的間隔。此時，系統模型可以構造為：

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

式中：[Xk=[xk，vkx，yk，vky]T]表示第[k]幀的狀態向量；[Wk]表示第[k]幀的系統過程噪聲；[F]表示狀態轉移矩陣。在本文中將采用勻速（CV）模型和協調轉彎（CT）模型作為目標運動模型，它們的狀態轉移矩陣分別用[FCV]和[FCT]來表示。

[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）

式中[η]表示轉換率。通過二維FFT、CFAR檢測以及 DOA估計算法可得到目標在第[k]幀的距離、徑向速度、方位角。

根據測量向量與狀態的關系，測量模型可表示為：

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

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

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

2" 跟蹤濾波算法

2.1" 擴展卡爾曼濾波算法及改進

擴展卡爾曼濾波是雷達目標跟蹤常用的方法之一，其基本思想是利用線性化方法將表現為非線性規律的系統轉化為一個近似的線性濾波問題，具體算法步驟如下所示。

1） 進行狀態一步預測：

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

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

式中：[Xpk+1]為預測狀態變量；[Ppk+1]為預測協方差矩陣；[Q]為過程噪聲協方差矩陣。

2） 計算卡爾曼增益：

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

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

3） 更新狀態估計：

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

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

上述為典型的EKF濾波算法步驟，但是EKF算法在線性化過程中忽略了泰勒級數展開式中的高階項，不可避免地引入了誤差。隨著誤差的不斷累積，甚至會導致EKF濾波發散。針對以上問題，本文提出了一種改進方法，通過利用對角陣[Γ=diag?k+1， ?k+1gt;1]重構狀態預測協方差矩陣，此時的狀態預測協方差矩陣[Ppk+1]可表示為：

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

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

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

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

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

2.2" IMM?EKF算法

交互式多模型算法由針對每個模型運用的濾波模塊、模型概率評估模塊和估計組合模塊組成。將改進的EKF算法作為IMM算法中的非線性濾波器，即為本文所提出的IMM?EKF算法。每個濾波器分別在測量的開始使用混合估計值，以便計算新的估計值和濾波器內模型的可能性。依次使用概率、先驗模型概率和模型轉換概率計算新模型的概率。然后，使用新的狀態估計計算總體狀態估計。IMM?EKF算法流程圖如圖1所示。

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

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

式中：[cj]為歸一化常數；[μi]為模型概率；[pij]為模型轉換概率。

通過使用前一時刻的狀態估計[Xikk]來計算與每個濾波器相對應的混合狀態：

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

當[m]=2時，將上述估計作為[m]個模型的輸入，此時輸出為[Xjk+1k+1]。然后計算[m]個濾波器對應的似然函數為：

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

式中：[zj]為測量殘差；[Sj]為殘差協方差矩陣。此時更新每個模型的概率。

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

式中歸一化常數[c]定義為：

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

最后，[m]個估計值合并為式（20）。這些值用作系統的實際輸出，但不作為IMM?EKF算法遞歸的一部分。

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

3" 實驗驗證

3.1" 濾波算法仿真

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

圖3為不同濾波算法的均方根誤差（RMSE）對比圖。從圖3中可以看出，IMM?EKF在整個過程中收斂性更好，且有效解決了EKF線性化過程中由于誤差累積導致的濾波發散問題。為了進一步說明本文所提出的IMM?EKF算法的有效性，將改進前后不同運動模型下的EKF算法和IMM?EKF算法的RMSE均值對比結果在表1中列出。

從表1中的數據可知，IMM?EKF算法的RMSE均值小于單模型下的EKF濾波算法。同時，采用重構后的狀態預測協方差矩陣的EKF濾波算法的RMSE均值更小。相比于采用典型的EKF作為IMM算法的濾波器，以改進的EKF為濾波器的IMM算法的跟蹤精度提高了22.32%。驗證了本文所提出的IMM?EKF濾波算法可以獲取更優的目標估計狀態，并適合用于對非線性運動目標的跟蹤。

3.2" 實測驗證及分析

本實驗所用的77 GHz調頻連續波多輸入多輸出雷達（MIMO?FMCW）系統搭載德州儀器公司的AWR1843芯片。本文共選取了兩個實驗場景，如圖4所示，實驗場景一中只有一個行人且作曲線運動。如圖5所示，實驗場景二中有兩個行人分別位于雷達兩側，兩人都是先遠離雷達然后轉身再朝向雷達運動。

跟蹤濾波過程中分別采用EKF濾波算法、改進的EKF濾波算法以及改進的IMM?EKF濾波算法，對兩個實驗場景中的行人軌跡進行了濾波跟蹤。實驗所用的雷達采集參數配置如表2所示，不同濾波算法下的單人運動軌跡如圖6所示。

從圖6中可以看出，所提出的IMM?EKF濾波算法對行人目標進行跟蹤相比于EKF和改進的EKF算法的濾波軌跡更為平滑。對量測點分別沿[x]和[y]軸方向進行狀態分解并使用三次多項式曲線擬合計算行人的估計軌跡，將擬合結果作為真實軌跡的估計值，然后分別計算不同濾波算法的RMSE并進行對比。實驗場景一下不同濾波算法的RMSE對比如圖7所示。從圖7中的RMSE值來看，所提出的IMM?EKF算法估計誤差值在整體上比較穩定，大多數時候其誤差值最小且收斂性更好。

不同濾波算法下的雙人運動軌跡如圖8所示。圖8中的局部放大圖反映了行人轉身由原來的遠離雷達變為朝向雷達行走過程中的不同濾波算法軌跡跟蹤的細節，從圖中可以看出，在行人轉身過程中使用EKF濾波算法的跟蹤精度急劇降低，而使用改進的EKF和改進的IMM?EKF則可以有效優化該運動過程的跟蹤軌跡，提升跟蹤的性能。

實驗場景二下的不同濾波算法的距離RMSE對比如圖9所示，圖9中目標1為圖8中的左側目標，目標2為圖8中的右側目標。從圖9中可以明顯看出，采用本文提出的改進IMM?EKF濾波算法時，整體上的跟蹤魯棒性更好，收斂性也優于其他兩種濾波算法。另外，在行人轉身過程中，可以明顯地看出采用改進的IMM?EKF濾波算法的距離RMSE較小，提高了跟蹤濾波精度。

不同場景下的濾波算法RMSE均值對比如表3所示。從表3中可知，本文中提出的改進IMM?EKF濾波算法相較于其他兩種算法，在兩種實驗場景下所表現的濾波跟蹤性能均有所提升。

在實驗場景一中，采用改進的IMM?EKF算法的跟蹤精度相比于EKF算法提高了60.43%；和改進的EKF算法的相比，跟蹤精度提高了21.62%。

在實驗場景二中，采用改進的IMM?EKF算法后，雷達前方左側行人軌跡跟蹤精度相對于其他兩種濾波算法分別提高了44.30%和26.72%；雷達前方右側行人軌跡跟蹤精度相對于其他兩種濾波算法分別提高了57.38%和34.67%。

4" 結" 語

為解決由于行人運動的不規律性導致的跟蹤誤差大的問題，尤其在改變運動方向時，比如進行曲線運動和轉身。本文提出了一種基于擴展卡爾曼濾波的交互式多模型跟蹤方法，減少了行人在突然改變運動方向時的跟蹤誤差，提高了目標跟蹤的魯棒性。通過仿真和實測實驗證明了所提出算法的有效性，并具有較高的實際應用價值。

參考文獻

[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的毫米波雷達多目標跟蹤算法研究[J].現代電子技術，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] 沈一心.智能輔助駕駛系統中的行人檢測[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] 鞠夕強，孟文.基于毫米波雷達的機動目標跟蹤算法研究[J].計算機與數字工程，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.