一種汽車雷達目標跟蹤方法
【技術領域】
[0001] 本發明涉及目標跟蹤領域，尤其涉及一種基于平方根不敏卡爾曼濾波（SRUKF)的 汽車雷達目標跟蹤方法。
【背景技術】
[0002] 卡爾曼濾波(KF)是目標跟蹤領域應用廣泛的一種跟蹤算法。卡爾曼濾波器的作用 在于，通過對運動目標運動過程和傳感器測量過程建模，利用間接、含噪聲的測量值，盡量 準確地估計目標的運動狀態。運動狀態包括目標的位置、速度、加速度信息。傳感器測量模 型中包含有噪聲(稱為測量噪聲），這代表著對目標的測量含有一定的不準確性；目標運動 過程模型中也包含有噪聲(稱為過程噪聲），這代表運動目標的運動過程并不完全符合運動 模型的假設。雖然真實世界的很多動態系統都并不能確切地符合所假設的運動模型，但是 由于卡爾曼濾波器被設計在有噪聲的情況下工作，因此一個近似的符合已經可以使這個濾 波器非常有用了。濾波的意義在于，通過假設運動模型和測量模型，減少測量噪聲和過程噪 聲對目標跟蹤效果的影響，從而更精確的估計目標車輛的運動狀態。
[0003] 傳統的卡爾曼濾波器只適用于線性系統，即運動模型和測量模型必須是線性系 統。而在車輛目標跟蹤領域，這常常是難以達到的。多數情況下，目標車輛運動狀態可以用 一個簡單的八維狀態空間向量X表示：X= [X,之元元y,久兌刃7\其中包含在車體坐標 系下的目標的橫向距離X、橫向速度:?、橫向加速度i、橫向加加速度:1、縱向距離y、縱向速度 j、縱向加速度歹以及縱向加加速度f。而車載傳感器（以毫米波雷達為例），測得的目標信 息則用測量值空間向量Y表示:Y=[r，a，v] T，其中包含目標距離r、目標角度a以及目標徑向 速度V。由于狀態空間向量X和測量值空間向量Y之間不滿足線性關系，狀態空間向量X到測
量值空間向量Y的映射由測量模型h表示 ，因此，該測量模型h也不是線性系統，從而 7. 使得傳統的卡爾曼濾波器無法使用。
[0004] 為此，需要對傳統的卡爾曼濾波器進行改進，由此產生了擴展卡爾曼濾波器 (EKF)，這是為了解決非線性問題而提出的一種濾波器。然而，EKF應對非線性問題是通過將 非線性系統進行一階線性化來進行濾波的。這種方式雖然能得到結果，但是在后驗狀態均 值和后驗狀態協方差上往往會出現較大誤差，甚至出現濾波器發散的情況。
[0005] 鑒于上述情況，現有技術中進一步開發了不敏卡爾曼濾波器(UKF)，這是另一種適 用于非線性系統的卡爾曼濾波器。UKF相比于EKF在后驗狀態均值和后驗狀態協方差上有了 很大的提高，而且和EKF有同階的計算復雜度。UKF并不對非線性系統進行線性化，而是在狀 態空間向量和測量值空間向量進入非線性系統之前，用一組特殊的權值進行采樣，這組采 樣點稱為西格瑪點，這組西格瑪點能準確的反映原分布的均值和協方差。讓這組采樣點通 過非線性系統后，再對輸出值加權求和，這樣得到的后驗均值和后驗協方差能達到三階泰 勒展開式的精度。這一整個過程稱為UT變換。然而在實際使用UKF進行濾波時，仍會遇到一 些困難。具體來說，在UT變換中，需要對協方差矩陣求喬利斯基分解，因此，一方面，這要求 運算過程中協方差矩陣始終保持正定，另一方面，這帶來的計算量也較大。由于目標跟蹤領 域對實時性的要求較高，傳感器頻率比較高，對算法迭代速度的要求也比較高，因此UT變換 的高復雜度和運算過程中可能導致的不正定問題就成了限制UKF算法應用的瓶頸。
【發明內容】
[0006] 為了解決上述現有技術存在的問題，本發明旨在提供一種汽車雷達目標跟蹤方 法，以克服現有的汽車雷達目標跟蹤方法中UKF濾波器運算性能上的不足，提高該跟蹤方法 的實時性和穩定性。
[0007] 本發明所述的一種汽車雷達目標跟蹤方法，其包括:步驟so，構建目標車輛運動模 型以及汽車雷達測量模型，并將目標車輛的運動狀態標記為L維的運動狀態向量X，將汽車 雷達測得的目標車輛信息標記為測量值向量Y;根據所述目標車輛運動模型獲得狀態誤差 協方差矩陣P和過程噪聲協方差矩陣Q，根據所述汽車雷達測量模型獲得測量噪聲協方差矩 陣R，其特征在于，該方法還包括以下步驟：
[0008] 步驟S1，將所述過程噪聲協方差矩陣Q和所述測量噪聲協方差矩陣R分別設置為常 數矩陣，并設置采樣權重參數α、β和Θ;
[0009] 步驟S2,初始化第k個時刻的運動狀態向量Xk;對第k個時刻的狀態誤差協方差矩 陣Pk進行喬利斯基分解以獲得分解矩陣S k，并初始化該分解矩陣Sk;
[0010] 步驟S3，計算獲得初始化后的運動狀態向量Xk所對應的2L+1個西格瑪點σχ, k及其 均值權重參數Wm和方差權重參數W。；
[0011] 步驟S4,計算獲得目標車輛在第k+Ι個時刻的運動狀態向量預測值ffc+1，并根據 該運動狀態向量預測值.1& +1、所述過程噪聲協方差矩陣Q、所述西格瑪點〇x,k以及所述方差 權重參數W。，計算獲得第k+Ι個時刻的狀態預測分解矩陣
[0012] 步驟S5,計算獲得第k+Ι個時刻的測量值向量預測值?^+1，并根據該測量值向量 預測值％+1、所述測量噪聲協方差矩陣R、所述西格瑪點〇 x,k以及所述方差權重參數W。，計 算獲得第k+Ι個時刻的測量預測分解矩陣33^+1;
[0013]步驟S6,根據所述西格瑪點〇x,k、所述方差權重參數W。、所述運動狀態向量預測值 &+1、所述測量值向量預測值ffc+1以及所述測量預測分解矩陣\#+1，計算獲得卡爾曼 增益Kk +i;
[0014] 步驟S7,計算獲得目標車輛在第k+1個時刻的運動狀態向量估計值:根據所 述狀態預測分解矩陣測量預測分解矩陣$^+1以及卡爾曼增益K k+1，計算獲得第k +1個時刻的分解矩陣估計值.>^+1;
[0015] 步驟S8,返回執行所述步驟S2,并使k = k+l，直至目標車輛停止運動。
[0016] 在上述汽車雷達目標跟蹤方法中，所述步驟S2包括：當k = 0時，采用通過所述汽車 雷達獲得的測量值向量Υο初始化所述運動狀態向量Xk，且將所述分解矩陣Sk初始化為L維的 單位矩陣；當k矣0時，將所述運動狀態向量X k和分解矩陣S k分別設置為： = I，，其中，f fc:表示目標車輛在第k個時刻的運動狀態向量估計值，^:表 示第k個時刻的分解矩陣估計值。
[0017]在上述汽車雷達目標跟蹤方法中，所述步驟S3包括:根據以下公式可獲得2L+1個 西格瑪點〇x,k及其均值權重參數Wm和方差權重參數W。：
[0024] 其中，A = a2(L+0)-L，
，L為所述運動狀態向量X的維度，α，β，θ為所述 采樣權重參數；和分別對應第i個西格瑪點丨的均值權重參數和方差權重參 數。
[0025] 在上述汽車雷達目標跟蹤方法中，所述步驟S4包括:首先，將所述西格瑪點(^代 入所述目標車輛運動模型的狀態轉移函數f( ·)中，以獲得第k+Ι個時刻的預測采樣點集 〇x,k+i，其中，所述狀態轉移函數f( ·)為：
[0027]其中，dt表示所述汽車雷達的采樣周期；
[0028]然后，根據所述預測采樣點集〇x,k+1以及所述均值權重參數^，并通過以下公式計 算獲得所述目標車輛在第k+Ι個時刻的運動狀態向量預測值無fc+1:
[0030] 在上述汽車雷達目標跟蹤方法中，所述步驟S4包括:將所述西格瑪點心^代入所述 目標車輛運動模型的狀態轉移函數f( ·)中，以獲得第k+1個時刻的預測采樣點集~,!^，其 中，所述狀態轉移函數f( ·)為：
[0032]其中，dt表示所述汽車雷達的采樣周期；
[0033]然后，根據所述預測采樣點集〇x,k+1、所述運動狀態向量預測值^+1、所述過程噪 聲協方差矩陣Q以及所述方差權重參數W。，并通過以下公式計算獲得所述第k+Ι個時刻的狀 態預測分解矩陣&^+1:
[0036]在上述汽車雷達目標跟蹤方法中，所述步驟S5包括:首先，將所述西格瑪點〇x,kR 入所述目標車輛運動模型的狀態轉移函數f( ·)中，以獲得第k+1個時刻的預測采樣點集 〇x,k+i，其中，所述狀態轉移函數f( ·)為：
[0038]其中，dt表示所述汽車雷達的采樣周期；
[0039]然后，將所述預測采樣點集〇x,k+1代入所述汽車雷達測量模型的測量函數h( ·)中， 以獲得第k+Ι個時刻的預測測量值點集〇y,k+1，其中，所述測量函數h( ·)為：<