李艷玲，方 遒，屠亞杰
（廈門理工學院機械與汽車工程學院， 福建 廈門 361024）
目標跟蹤技術被廣泛應用于自動駕駛和視頻監控等領域。在自動駕駛領域中，利用目標跟蹤技術可以及時感知車輛環境變化，從而降低事故發生率。車載毫米波雷達目標跟蹤主要是對行駛車輛、行人等目標進行跟蹤，且目標具有機動性強和靈活性高的特點。隨著毫米波雷達分辨率的不斷提高，車載毫米波雷達的應用將呈現多樣化［1-3］。
車載毫米波雷達多目標跟蹤算法的計算主要為非線性系統近似和數據關聯兩方面。受限于車載毫米波雷達的測量原理［4］，車載毫米波雷達目標跟蹤采用非線性卡爾曼濾波算法對目標進行距離和速度估計，典型算法有擴展卡爾曼濾波器（extended Kalman filter， EKF）、無跡卡爾曼濾波器（unscented Kalman filter， UKF）和容積卡爾曼濾波器（cubature Kalman filter， CKF）。劉華軍等［4］提出快速平方根CKF算法，簡化預測階段的狀態和協方差陣計算，提高了運行效率和跟蹤精度。戴雪梅等［5］提出強跟蹤平方根CKF 算法，目標狀態突變時仍能保持對目標的跟蹤能力，減少了運行時間。傳統數據關聯算法［6-8］是基于關聯概率的組合方法，鄭丹陽等［9］提出變分推斷的聯合概率數據關聯（JPDA）算法，解決JPDA 算法的組合爆炸問題，但通過優化方法解決變分問題沒有將數據關聯的計算量降至較低范圍。蔣凱等［10］提出改進強跟蹤無跡卡爾曼的交互式多模型聯合概率數據關聯（IMM-JPDAISTUKF）算法，提高了距離和速度的跟蹤精度，但交互式多模型算法需已知各模型的概率轉移矩陣，JPDA 算法存在較大的計算量，UKF 算法在強非線性系統表現為較低的跟蹤精度。傳統數據關聯算法有較大的計算量，只適用于特定場景。而基于隨機有限集的數據關聯算法［11-14］可以有效避免復雜的關聯計算問題，還可以動態地表示目標與量測數量的變化。張昱等［15］提出多模型高斯混合概率假設密度（MM-GMPHD）算法，提高了防撞雷達預警系統的可靠性，但基于高斯假設的高斯混合概率假設密度（GMPHD）算法僅適用于線性運動目標，應用于車載毫米波雷達時跟蹤精度較低。張強等［16］提出高斯混合-勢概率假設密度（GM-CPHD）算法，能有效抑制雜波和穩定地估計目標狀態及目標數量，但計算復雜度高。王海環等［17］提出均方根容積卡爾曼序貫蒙特卡羅概率假設密度（SCK-SMC-PHD）算法，有效抑制經典序貫蒙特卡羅概率假設密度的粒子退化問題，但序貫重要性采樣方法具有較大的計算量，適用性窄。綜上所述，雷達多目標跟蹤算法的非線性系統近似與數據關聯兩方面具有較大的計算量。本文提出改進強跟蹤平方根容積卡爾曼多伯努利的雷達多目標跟蹤算法，將改進漸消因子的強跟蹤（IST）引入快速平方根容積卡爾曼濾波（RSCKF）中，聯合多伯努利（multi-Bernoulli，MB）算法進行多目標跟蹤，以解決多目標跟蹤算法計算量大的問題。
1.1 IST-RSCKF算法
考慮如下的非線性離散系統
式（1）中：Xk和Zk分別為系統k時刻的狀態向量和k時刻的量測向量；
Fk為k時刻的狀態轉移矩陣；
h為量測的非線性函數；
wk和vk為k時刻互不相關的高斯白噪聲序列，其協方差矩陣分別為Qk和Rk。
RSCKF算法基于三階球面-徑向容積準則選取容積點和權值，容積點ξi和權值wi表示為
式（2）中：[1]=[In，-In]，m= 2n，m表示容積點數，是狀態維數n的2 倍，In表示n維的單位矩陣，[1]i表示[1]的第i列。快速RSCKF算法步驟如下。
步驟1：初始化。初始化狀態向量Xk，k、誤差平方根協方差矩陣Sk，k、過程噪聲Qk和量測噪聲Rk。
步驟2：時間更新。計算系統在k+ 1時刻的預測狀態和平方根協方差矩陣分別為
式（4）中：Tria(?)為矩陣的三角化運算；
SQ，k為Qk的平方根，SQ，k=Qk。
步驟3：量測更新。計算系統在k+ 1時刻的狀態向量和誤差平方根協方差矩陣分別為
式（5）中：為狀態預測值；
Kk+1為增益；
z為量測值；
為量測預測值。式（6）中：X*和Z*為互協方差參數；
SR，k為量測噪聲協方差。式（5）和式（6）相關參數的計算式為
針對非線性系統，強跟蹤理論上需進行3次采樣完成一次濾波。第一次采樣針對狀態方程，計算預測狀態值和預測誤差協方差陣。第二次采樣針對量測方程，計算預測量測值、量測誤差協方差陣和互協方差陣。根據第二次采樣結果計算漸消因子和引入漸消因子后的預測誤差協方差陣。第三次采樣根據預測誤差協方差陣計算預測量測值、量測誤差協方差陣和互協方差陣，進而更新狀態值和誤差協方差陣來完成一次濾波過程。考慮式（1）的非線性系統，狀態方程采樣簡化為一般卡爾曼濾波形式。根據量測方程進行第二次采樣計算預測量測值、量測誤差協方差陣、互協方差陣和漸消因子，量測更新結果通過第二次采樣結果與漸消因子的簡單計算獲得。因此，ISF-RSCKF 算法僅針對量測方程采樣，在一定程度上減少了計算量。在式（13）計算完成之后計算漸消因子，有
式（17）~（18）中：tr(?)為矩陣求跡運算；
Hk表示量測更新獲得雅可比矩陣；
Vk為實時輸出殘差序列的協方差矩陣；
上標T為矩陣轉置。相關參數計算為
式（19）中：(?)-1為矩陣求逆運算。式（20）中：ek=zk-為每次濾波新息；
ρ表示遺忘因子，通常取ρ= 0.95。根據文獻［18］改進漸消因子的方法，其λk、Nk和Mk在快速RSCKF的表達如下
量測更新中獲得平方根協方差矩陣后，根據式（13）計算漸消因子λk。計算引入漸消因子的互協方差矩陣、平方根協方差、增益和量測更新的雅可比矩陣，相關參數的計算式為
量測更新式為
式（27）：I為同維度的單位矩陣。
1.2 MB算法
在多目標跟蹤過程中，因目標的新生、消失和繼續存在引起目標狀態和數量的動態變化，使得傳感器獲得量測數量也發生變化，可采用隨機有限集描述多目標跟蹤過程中目標狀態變化和量測變化。基于共軛屬性的多伯努利混合（MBM）算法［14］可采用單個目標的后驗分布表示整個后驗分布，避免時間更新和量測更新的復雜積分問題。因MBM 算法在整個濾波過程中的全局假設數量為L個，每次均與下一時刻的量測值進行關聯，導致該算法存在較大的計算量。許多學者致力于研究獲得一個全局假設的方法來減少計算量［19-20］。本文提出使用新息自相關矩陣和Murty 算法［21］共同確定最佳假設的多伯努利（MB）算法。MB 算法濾波過程與MBM 算法濾波過程類似，主要在Murty算法執行后引入新息自相關矩陣選擇此次濾波的最佳假設。新息自相關矩陣是目標與測量值的誤差自相關矩陣，選擇新息自相關矩陣跡的較小者且為全局假設中的關聯假設作為此次濾波的最佳假設。最佳假設索引流程如圖1所示。圖1中，V0為每個目標與量測的自相關矩陣，Opt_index為全局假設。
圖1 索引流程圖Fig. 1 Index flowchart
1.3 ISF-RSCKF-MB 算法
通過在數據關聯和非線性系統近似兩方面進行改進以減少目標跟蹤算法的計算量。將改進漸消因子的強跟蹤思想引入快速平方根容積卡爾曼濾波中形成IST-RSCKF 算法，并聯合MB 算法進行多目標跟蹤。ISF-RSCKF-MB算法步驟如下。
步驟1：初始化。初始化k時刻多目標狀態，目標被探測的概率Pd和存活概率Ps。
步驟2：時間更新。k時刻多目標狀態由IST-RSCKF 算法的時間更新完成，其他時間更新參數可參考文獻［14］。
步驟3：量測更新。為避免大量計算，利用馬氏距離將步驟2中的預測狀態值與量測值進行分組，不屬于多目標組內的量測判為雜波，無需進行更新。預測狀態值未與任何量測分為一組，屬于漏檢目標；
預測狀態值與某個量測分為一組，屬于目標關聯到測量。k時刻多目標狀態由IST-RSCKF 算法的量測更新完成，其他量測更新參數可參考文獻［14］。
步驟4：確定濾波假設。根據多目標權值構建代價矩陣作為Murty算法的參數，Murty算法輸出的全局假設與新息自相關矩陣共同確定最佳假設。計算每個目標的新息自相關矩陣跡的最小值的關聯假設，查找存在于Murty算法全局假設中的關聯假設作為最佳假設，刪除其余全局假設。
步驟5：強跟蹤量測更新。根據步驟4獲得的最佳假設計算漸消因子，k時刻多目標狀態由引入改進漸消因子的IST-RSCKF算法的量測更新完成。
步驟6：計算目標跟蹤算法的評價指標。采用廣義最優子模式分配（GOSPA）度量［22］評價目標跟蹤算法性能。
步驟7：修剪伯努利項。移除最佳假設中不存在的伯努利項。
2.1 實驗場景及評價指標
實驗采用TI公司的AWR1843BOOST和DCA1000EVM 采集行人軌跡數據，毫米波雷達可用帶寬為4 GHz，其他實驗參數設置如表1所示。
表1 雷達數據采集參數設置Table 1 Parameter settings for radar data acquisition
實驗場景為常見的水泥路面，行人軌跡范圍為：x1=-4 m，x2= 4 m，y1= 2 m，y2= 30 m，行人以近似勻速的速度遠離雷達，其初始狀態分別為X1= [1，2，0，0]，X2= [0，2，0，0]，X3= [-1，2，0，0]。建立勻速模型（CV）的運動方程和量測方程（式29）進行實驗。
式（29）中：R為徑向距離；
x為橫向坐標；
y為縱向坐標；
θ為方位角；
vr為徑向速度；
x?為橫向速度；
y?為縱向速度。
本文采用GOSPA 度量評價車載毫米波雷達多目標跟蹤算法性能，包括位置誤差、漏檢誤差和誤檢誤差［23］。即有
2.2 仿真實驗
圖2 是關于數據關聯的GOSPA 誤差對比，將改進強跟蹤平方根容積卡爾曼分別結合MB算法和MBM 算法進行多目標跟蹤。表2 為改進強跟蹤平方根容積卡爾曼多伯努利混合（IST-RSCKF-MBM） 算法與本文算法的GOSPA 誤差。由圖2 可知，上述2 種算法的GOSPA 整體誤差趨于12.247 4 以下，表明能有效跟蹤目標。由表2可知，本文算法的誤檢誤差小于IST-RSCKF-MBM 算法的誤檢誤差，漏檢誤差小于IST-RSCKF-MBM 算法的漏檢誤差，表明所提MB 算法能改善數據關聯欠佳的問題。與IST-RSCKF-MBM 算法相比，本文算法的GOSPA誤差減少4.71%，表明Murty算法與新息自相關矩陣共同確定最佳假設方法有效。IST-RSCKF-MBM 算法與本文算法耗時依次為5.507 1、3.656 3 s。
表2 2種算法的GOSPA度量誤差Table 2 GOSPA errors by the two algorithms
圖2 2種算法的GOSPA誤差比較Fig. 2 Comparison of GOSPA errors of the two algorithms
將改進漸消因子［18］的強跟蹤思想引入簡化的平方根容積卡爾曼濾波中，聯合MB 算法稱為IST-RSCKF-MB2算法。圖3是關于系統模型的GOSPA 誤差對比，分別為平方根容積卡爾曼多伯努利（RSCKF-MB） 算法、ISTRSCKF-MB2 算法和本文算法的GOSPA 誤差。表3 是RSCKF-MB 算法、IST-RSCKF-MB2 算法和本文算法的GOSPA 誤差。由圖3 可知，以上3 種算法的GOSPA 整體誤差趨于12.247 4以下，表明算法能有效跟蹤目標。由表3 可知，與RSCKF-MB 和IST-RSCKF-MB2 算法相比，本文算法的GOSPA 誤差分別減少0.36%和1.99%，表明改進漸消因子計算方法能有效減少系統模型誤差。RSCKF-MB 算法、ISTRSCKF-MB2 算法與本文算法耗時依次為3.843 1、4.139 6和3.656 3s。
表3 3種算法的GOSPA度量誤差Table 3 GOSPA errors by the four algorithms
圖3 3種算法的 RMS-GOSPA總體誤差比較Fig. 3 Comparison of RMS-GOSPA errors of the three algorithms
圖4 為無跡卡爾曼多伯努利（UKF-MB）算法、擴展卡爾曼多伯努利（EKF-MB）算法和本文算法的GOSPA 誤差對比，表4 為EKF-MB、UKF-MB 和本文算法的GOSPA 誤差。由圖4 可知，EKF-MB 算法的GOSPA 誤差在200 幀之前圍繞12.247 4 波動，在200幀之后整體趨于12.247 4 以上，表明EKFMB算法不能有效跟蹤目標；
UKF-MB算法的GOSPA 誤差在200 幀之前逐漸增加，在200幀之后圍繞12.247 4 波動，表明UKF-MB 算法不能較好地跟蹤目標；
本文算法的GOSPA整體誤差趨于12.247 4 以下，表明能較好地跟蹤目標。由表4 可知，與EKF-MB 和UKFMB 相比，本文算法的GOSPA 誤差分別減少14.75%和0.17%，即本文算法提高了多目標跟蹤精度。EKF-MB 算法、UKF-MB 算法與本文算法耗時依次為6.800 8、5.466 8 和3.656 3 s。
表4 3種算法的GOSPA度量誤差Table 4 GOSPA errors by the three algorithms
圖4 3種算法的GOSPA誤差比較Fig.4 Comparison of GOSPA errors by the three algorithms
本文提出改進強跟蹤平方根容積卡爾曼多伯努利的雷達多目標跟蹤算法，將改進漸消因子的強跟蹤引入簡化的平方根容積卡爾曼濾波中，并聯合MB 算法進行多目標跟蹤。結果表明，所提算法的GOSPA 誤差與平方根容積卡爾曼多伯努算法、改進強跟蹤平方根容積卡爾曼多伯努利混合算法、擴展卡爾曼多伯努利算法和無跡卡爾曼多伯努利算法相比分別減少0.36%、4.71%、14.75%和0.17%。本文算法耗時最少，提高了雷達多目標跟蹤精度，可推進基于隨機有限集的算法在實際工程中的應用。
猜你喜歡 卡爾曼伯努利平方根 狀態變換擴展卡爾曼平滑算法在AUV水下航跡修正中的應用中國慣性技術學報(2020年2期)2020-07-24“平方根”學習法升級版中學生數理化·七年級數學人教版(2018年3期)2018-05-30平方根易錯點警示中學生數理化·七年級數學人教版(2018年3期)2018-05-30幫你學**方根中學生數理化·七年級數學人教版(2017年3期)2018-01-20如何學好平方根中學生數理化·七年級數學人教版(2017年3期)2018-01-20一種伯努利原理研究的實驗裝置發明與創新(2016年5期)2016-08-21基于卡爾曼算法的動力鋰電池SOC估算工業設計(2016年4期)2016-05-04淺談關于n重伯努利試驗概率計算問題課程教育研究·學法教法研究(2016年7期)2016-04-26基于卡爾曼預測的逆變焊接電源信號處理方法焊接(2016年4期)2016-02-27基于擴展卡爾曼估計的飛機防滑剎車系統模糊控制西北工業大學學報(2015年3期)2015-12-14