一種汽車?yán)走_(dá)目標(biāo)跟蹤方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及目標(biāo)跟蹤領(lǐng)域，尤其涉及一種基于平方根不敏卡爾曼濾波（SRUKF)的 汽車?yán)走_(dá)目標(biāo)跟蹤方法。
【背景技術(shù)】
[0002] 卡爾曼濾波(KF)是目標(biāo)跟蹤領(lǐng)域應(yīng)用廣泛的一種跟蹤算法?？柭鼮V波器的作用 在于，通過對運動目標(biāo)運動過程和傳感器測量過程建模，利用間接、含噪聲的測量值，盡量 準(zhǔn)確地估計目標(biāo)的運動狀態(tài)。運動狀態(tài)包括目標(biāo)的位置、速度、加速度信息。傳感器測量模 型中包含有噪聲(稱為測量噪聲），這代表著對目標(biāo)的測量含有一定的不準(zhǔn)確性；目標(biāo)運動 過程模型中也包含有噪聲(稱為過程噪聲），這代表運動目標(biāo)的運動過程并不完全符合運動 模型的假設(shè)。雖然真實世界的很多動態(tài)系統(tǒng)都并不能確切地符合所假設(shè)的運動模型，但是 由于卡爾曼濾波器被設(shè)計在有噪聲的情況下工作，因此一個近似的符合已經(jīng)可以使這個濾 波器非常有用了。濾波的意義在于，通過假設(shè)運動模型和測量模型，減少測量噪聲和過程噪 聲對目標(biāo)跟蹤效果的影響，從而更精確的估計目標(biāo)車輛的運動狀態(tài)。
[0003] 傳統(tǒng)的卡爾曼濾波器只適用于線性系統(tǒng)，即運動模型和測量模型必須是線性系 統(tǒng)。而在車輛目標(biāo)跟蹤領(lǐng)域，這常常是難以達(dá)到的。多數(shù)情況下，目標(biāo)車輛運動狀態(tài)可以用 一個簡單的八維狀態(tài)空間向量X表示：X= [X,之元元y,久兌刃7\其中包含在車體坐標(biāo) 系下的目標(biāo)的橫向距離X、橫向速度:?、橫向加速度i、橫向加加速度:1、縱向距離y、縱向速度 j、縱向加速度歹以及縱向加加速度f。而車載傳感器（以毫米波雷達(dá)為例），測得的目標(biāo)信 息則用測量值空間向量Y表示:Y=[r，a，v] T，其中包含目標(biāo)距離r、目標(biāo)角度a以及目標(biāo)徑向 速度V。由于狀態(tài)空間向量X和測量值空間向量Y之間不滿足線性關(guān)系，狀態(tài)空間向量X到測
量值空間向量Y的映射由測量模型h表示 ，因此，該測量模型h也不是線性系統(tǒng)，從而 7. 使得傳統(tǒng)的卡爾曼濾波器無法使用。
[0004] 為此，需要對傳統(tǒng)的卡爾曼濾波器進行改進，由此產(chǎn)生了擴展卡爾曼濾波器 (EKF)，這是為了解決非線性問題而提出的一種濾波器。然而，EKF應(yīng)對非線性問題是通過將 非線性系統(tǒng)進行一階線性化來進行濾波的。這種方式雖然能得到結(jié)果，但是在后驗狀態(tài)均 值和后驗狀態(tài)協(xié)方差上往往會出現(xiàn)較大誤差，甚至出現(xiàn)濾波器發(fā)散的情況。
[0005] 鑒于上述情況，現(xiàn)有技術(shù)中進一步開發(fā)了不敏卡爾曼濾波器(UKF)，這是另一種適 用于非線性系統(tǒng)的卡爾曼濾波器。UKF相比于EKF在后驗狀態(tài)均值和后驗狀態(tài)協(xié)方差上有了 很大的提高，而且和EKF有同階的計算復(fù)雜度。UKF并不對非線性系統(tǒng)進行線性化，而是在狀 態(tài)空間向量和測量值空間向量進入非線性系統(tǒng)之前，用一組特殊的權(quán)值進行采樣，這組采 樣點稱為西格瑪點，這組西格瑪點能準(zhǔn)確的反映原分布的均值和協(xié)方差。讓這組采樣點通 過非線性系統(tǒng)后，再對輸出值加權(quán)求和，這樣得到的后驗均值和后驗協(xié)方差能達(dá)到三階泰 勒展開式的精度。這一整個過程稱為UT變換。然而在實際使用UKF進行濾波時，仍會遇到一 些困難。具體來說，在UT變換中，需要對協(xié)方差矩陣求喬利斯基分解，因此，一方面，這要求 運算過程中協(xié)方差矩陣始終保持正定，另一方面，這帶來的計算量也較大。由于目標(biāo)跟蹤領(lǐng) 域?qū)崟r性的要求較高，傳感器頻率比較高，對算法迭代速度的要求也比較高，因此UT變換 的高復(fù)雜度和運算過程中可能導(dǎo)致的不正定問題就成了限制UKF算法應(yīng)用的瓶頸。

【發(fā)明內(nèi)容】

[0006] 為了解決上述現(xiàn)有技術(shù)存在的問題，本發(fā)明旨在提供一種汽車?yán)走_(dá)目標(biāo)跟蹤方 法，以克服現(xiàn)有的汽車?yán)走_(dá)目標(biāo)跟蹤方法中UKF濾波器運算性能上的不足，提高該跟蹤方法 的實時性和穩(wěn)定性。
[0007] 本發(fā)明所述的一種汽車?yán)走_(dá)目標(biāo)跟蹤方法，其包括:步驟so，構(gòu)建目標(biāo)車輛運動模 型以及汽車?yán)走_(dá)測量模型，并將目標(biāo)車輛的運動狀態(tài)標(biāo)記為L維的運動狀態(tài)向量X，將汽車 雷達(dá)測得的目標(biāo)車輛信息標(biāo)記為測量值向量Y;根據(jù)所述目標(biāo)車輛運動模型獲得狀態(tài)誤差 協(xié)方差矩陣P和過程噪聲協(xié)方差矩陣Q，根據(jù)所述汽車?yán)走_(dá)測量模型獲得測量噪聲協(xié)方差矩 陣R，其特征在于，該方法還包括以下步驟：
[0008] 步驟S1，將所述過程噪聲協(xié)方差矩陣Q和所述測量噪聲協(xié)方差矩陣R分別設(shè)置為常 數(shù)矩陣，并設(shè)置采樣權(quán)重參數(shù)α、β和Θ;
[0009] 步驟S2,初始化第k個時刻的運動狀態(tài)向量Xk;對第k個時刻的狀態(tài)誤差協(xié)方差矩 陣Pk進行喬利斯基分解以獲得分解矩陣S k，并初始化該分解矩陣Sk;
[0010] 步驟S3，計算獲得初始化后的運動狀態(tài)向量Xk所對應(yīng)的2L+1個西格瑪點σχ, k及其 均值權(quán)重參數(shù)Wm和方差權(quán)重參數(shù)W。；
[0011] 步驟S4,計算獲得目標(biāo)車輛在第k+Ι個時刻的運動狀態(tài)向量預(yù)測值ffc+1，并根據(jù) 該運動狀態(tài)向量預(yù)測值.1& +1、所述過程噪聲協(xié)方差矩陣Q、所述西格瑪點〇x,k以及所述方差 權(quán)重參數(shù)W。，計算獲得第k+Ι個時刻的狀態(tài)預(yù)測分解矩陣
[0012] 步驟S5,計算獲得第k+Ι個時刻的測量值向量預(yù)測值?^+1，并根據(jù)該測量值向量 預(yù)測值％+1、所述測量噪聲協(xié)方差矩陣R、所述西格瑪點〇 x,k以及所述方差權(quán)重參數(shù)W。，計 算獲得第k+Ι個時刻的測量預(yù)測分解矩陣33^+1;
[0013]步驟S6,根據(jù)所述西格瑪點〇x,k、所述方差權(quán)重參數(shù)W。、所述運動狀態(tài)向量預(yù)測值 &+1、所述測量值向量預(yù)測值ffc+1以及所述測量預(yù)測分解矩陣\#+1，計算獲得卡爾曼 增益Kk +i;
[0014] 步驟S7,計算獲得目標(biāo)車輛在第k+1個時刻的運動狀態(tài)向量估計值:根據(jù)所 述狀態(tài)預(yù)測分解矩陣測量預(yù)測分解矩陣$^+1以及卡爾曼增益K k+1，計算獲得第k +1個時刻的分解矩陣估計值.>^+1;
[0015] 步驟S8,返回執(zhí)行所述步驟S2,并使k = k+l，直至目標(biāo)車輛停止運動。
[0016] 在上述汽車?yán)走_(dá)目標(biāo)跟蹤方法中，所述步驟S2包括：當(dāng)k = 0時，采用通過所述汽車 雷達(dá)獲得的測量值向量Υο初始化所述運動狀態(tài)向量Xk，且將所述分解矩陣Sk初始化為L維的 單位矩陣；當(dāng)k矣0時，將所述運動狀態(tài)向量X k和分解矩陣S k分別設(shè)置為： = I，，其中，f fc:表示目標(biāo)車輛在第k個時刻的運動狀態(tài)向量估計值，^:表 示第k個時刻的分解矩陣估計值。
[0017]在上述汽車?yán)走_(dá)目標(biāo)跟蹤方法中，所述步驟S3包括:根據(jù)以下公式可獲得2L+1個 西格瑪點〇x,k及其均值權(quán)重參數(shù)Wm和方差權(quán)重參數(shù)W。：
[0024] 其中，A = a2(L+0)-L，
，L為所述運動狀態(tài)向量X的維度，α，β，θ為所述 采樣權(quán)重參數(shù)；和分別對應(yīng)第i個西格瑪點丨的均值權(quán)重參數(shù)和方差權(quán)重參 數(shù)。
[0025] 在上述汽車?yán)走_(dá)目標(biāo)跟蹤方法中，所述步驟S4包括:首先，將所述西格瑪點(^代 入所述目標(biāo)車輛運動模型的狀態(tài)轉(zhuǎn)移函數(shù)f( ·)中，以獲得第k+Ι個時刻的預(yù)測采樣點集 〇x,k+i，其中，所述狀態(tài)轉(zhuǎn)移函數(shù)f( ·)為：
[0027]其中，dt表示所述汽車?yán)走_(dá)的采樣周期；
[0028]然后，根據(jù)所述預(yù)測采樣點集〇x,k+1以及所述均值權(quán)重參數(shù)^，并通過以下公式計 算獲得所述目標(biāo)車輛在第k+Ι個時刻的運動狀態(tài)向量預(yù)測值無fc+1:
[0030] 在上述汽車?yán)走_(dá)目標(biāo)跟蹤方法中，所述步驟S4包括:將所述西格瑪點心^代入所述 目標(biāo)車輛運動模型的狀態(tài)轉(zhuǎn)移函數(shù)f( ·)中，以獲得第k+1個時刻的預(yù)測采樣點集~,!^，其 中，所述狀態(tài)轉(zhuǎn)移函數(shù)f( ·)為：
[0032]其中，dt表示所述汽車?yán)走_(dá)的采樣周期；
[0033]然后，根據(jù)所述預(yù)測采樣點集〇x,k+1、所述運動狀態(tài)向量預(yù)測值^+1、所述過程噪 聲協(xié)方差矩陣Q以及所述方差權(quán)重參數(shù)W。，并通過以下公式計算獲得所述第k+Ι個時刻的狀 態(tài)預(yù)測分解矩陣&^+1:
[0036]在上述汽車?yán)走_(dá)目標(biāo)跟蹤方法中，所述步驟S5包括:首先，將所述西格瑪點〇x,kR 入所述目標(biāo)車輛運動模型的狀態(tài)轉(zhuǎn)移函數(shù)f( ·)中，以獲得第k+1個時刻的預(yù)測采樣點集 〇x,k+i，其中，所述狀態(tài)轉(zhuǎn)移函數(shù)f( ·)為：
[0038]其中，dt表示所述汽車?yán)走_(dá)的采樣周期；
[0039]然后，將所述預(yù)測采樣點集〇x,k+1代入所述汽車?yán)走_(dá)測量模型的測量函數(shù)h( ·)中， 以獲得第k+Ι個時刻的預(yù)測測量值點集〇y,k+1，其中，所述測量函數(shù)h( ·)為：<