br>[0041]最后，根據(jù)所述預(yù)測測量值點(diǎn)集0y,k+1以及所述均值權(quán)重參數(shù)1，并通過以下公式 計(jì)算獲得所述第k+Ι個(gè)時(shí)刻的測量值向量預(yù)測值
[0043]在上述汽車?yán)走_(dá)目標(biāo)跟蹤方法中，所述步驟S5包括:首先，將所述西格瑪點(diǎn)〇x,kR 入所述目標(biāo)車輛運(yùn)動(dòng)模型的狀態(tài)轉(zhuǎn)移函數(shù)f( ·)中，以獲得第k+1個(gè)時(shí)刻的預(yù)測采樣點(diǎn)集 〇x,k+i，其中，所述狀態(tài)轉(zhuǎn)移函數(shù)f( ·)為：
[0045] 其中，dt表示所述汽車?yán)走_(dá)的采樣周期；
[0046] 然后，將所述預(yù)測采樣點(diǎn)集~,!^代入所述汽車?yán)走_(dá)測量模型的測量函數(shù)h( ·)中， 以獲得第k+Ι個(gè)時(shí)刻的預(yù)測測量值點(diǎn)集〇y,k+1，其中，所述測量函數(shù)h( ·)為：
[0048]最后，根據(jù)所述預(yù)測測量值點(diǎn)集〇y,k+1、所述測量值向量預(yù)測值、所述測量噪 聲協(xié)方差矩陣R以及所述方差權(quán)重參數(shù)W。，并通過以下公式計(jì)算獲得所述第k+Ι個(gè)時(shí)刻的測 量預(yù)測分解矩陣+1
[0051 ]在上述汽車?yán)走_(dá)目標(biāo)跟蹤方法中，所述步驟S6包括:首先，首先，將所述西格瑪點(diǎn) 〇^代入所述目標(biāo)車輛運(yùn)動(dòng)模型的狀態(tài)轉(zhuǎn)移函數(shù)f( ·)中，以獲得第k+Ι個(gè)時(shí)刻的預(yù)測采樣 點(diǎn)集〇x,k+1，其中，所述狀態(tài)轉(zhuǎn)移函數(shù)f( ·)為：
[0053]其中，dt表示所述汽車?yán)走_(dá)的采樣周期；
[0054]其次，將所述預(yù)測采樣點(diǎn)集〇x,k+1代入所述汽車?yán)走_(dá)測量模型的測量函數(shù)h( ·)中， 以獲得第k+Ι個(gè)時(shí)刻的預(yù)測測量值點(diǎn)集〇y,k+1，其中，所述測量函數(shù)h( ·)為：
[0056]然后，根據(jù)所述預(yù)測采樣點(diǎn)集〇x,k+1、所述預(yù)測測量值點(diǎn)集^,!^、所述方差權(quán)重參 數(shù)W。、所述運(yùn)動(dòng)狀態(tài)向量預(yù)測值ffc+i以及所述測量值向量預(yù)測值！^+1，并通過以下公式 計(jì)算獲得運(yùn)動(dòng)狀態(tài)向量與測量值向量的互協(xié)方差函數(shù)Pxy:
[0058]最后，根據(jù)所述互協(xié)方差函數(shù)Pxy以及測量預(yù)測分解矩陣$3^+1，通過以下公式計(jì) 算獲得所述卡爾曼增益Kk+1:
[0060] 在上述汽車?yán)走_(dá)目標(biāo)跟蹤方法中，所述步驟S7包括:根據(jù)所述運(yùn)動(dòng)狀態(tài)向量預(yù)測 值戈fc+1、#述測量值向量預(yù)測值jfc+1、所述卡爾曼增益Kk+1以及所述測量值向量Y k+1，并通 過以下公式計(jì)算獲得所述目標(biāo)車輛在第k+Ι個(gè)時(shí)刻的運(yùn)動(dòng)狀態(tài)向量估計(jì)值戈fc+1::
[0062]在上述汽車?yán)走_(dá)目標(biāo)跟蹤方法中，所述步驟S7包括:根據(jù)所述狀態(tài)預(yù)測分解矩陣 所述測量預(yù)測分解矩陣以及所述卡爾曼增益Kk+1，并通過以下公式計(jì)算獲 得所述第k+Ι個(gè)時(shí)刻的分解矩陣估計(jì)值：^+1:
[0064]由于采用了上述的技術(shù)解決方案，本發(fā)明通過將狀態(tài)誤差協(xié)方差矩陣Pk在算法迭 代執(zhí)行之前進(jìn)行喬利斯基分解，以獲得分解矩陣，進(jìn)而在算法迭代過程中，將該分解矩陣進(jìn) 行非線性迭代，自動(dòng)更新，而不需要在每一次步驟執(zhí)行過程中都去計(jì)算狀態(tài)誤差協(xié)方差矩 陣的喬利斯基分解，從而避免了狀態(tài)誤差協(xié)方差矩陣非正定所導(dǎo)致的分解失敗、算法停滯 的問題，綜上，本發(fā)明一方面減少了計(jì)算量，提高了汽車?yán)走_(dá)目標(biāo)跟蹤的實(shí)時(shí)性，另一方面 基本消除了矩陣非正定導(dǎo)致算法停滯的隱患，提高了汽車?yán)走_(dá)目標(biāo)跟蹤的穩(wěn)定性。
【附圖說明】
[0065] 圖1是采用本發(fā)明仿真單目標(biāo)車輛運(yùn)動(dòng)過程的濾波效果圖；
[0066] 圖2是在采用本發(fā)明仿真單目標(biāo)車輛運(yùn)動(dòng)過程中X方向距離的濾波誤差與測量誤 差對比圖；
[0067] 圖3是在采用本發(fā)明仿真單目標(biāo)車輛運(yùn)動(dòng)過程中Y方向距離的濾波誤差與測量誤 差對比圖；
[0068] 圖4是在真實(shí)車輛環(huán)境下，使用毫米波雷達(dá)測量單目標(biāo)車輛運(yùn)動(dòng)并通過本發(fā)明得 到的濾波效果示意圖。
【具體實(shí)施方式】
[0069]下面結(jié)合附圖，給出本發(fā)明的較佳實(shí)施例，并予以詳細(xì)描述。
[0070] 首先，對本發(fā)明中涉及到的一些概念進(jìn)行說明：
[0071] 1、系統(tǒng)運(yùn)動(dòng)模型:其描述了動(dòng)態(tài)系統(tǒng)的運(yùn)動(dòng)規(guī)律性，具體來說，就是使用系統(tǒng)狀態(tài) 轉(zhuǎn)移函數(shù)f( ·)或者狀態(tài)轉(zhuǎn)移矩陣F描述系統(tǒng)由第k個(gè)時(shí)刻的狀態(tài)到第k+Ι個(gè)時(shí)刻的狀態(tài)的 過渡關(guān)系:Xk+i = f(Xk)或者Xk+i = F · Xk。
[0072] 在汽車行駛中，由于加速、減速、變道、超車等駕駛動(dòng)作的頻繁發(fā)生，目標(biāo)車輛相對 于自車的運(yùn)動(dòng)過程是變化的（自車:安裝雷達(dá)傳感器的車輛，目標(biāo)車輛:被雷達(dá)傳感器探測 的車輛）。但是，一般認(rèn)為，除個(gè)別例外，并且在高采樣數(shù)據(jù)的情況下，目標(biāo)車輛相對于自車 的加加速度(加速度的變化率)可以在較長的時(shí)間內(nèi)保持為定值。因此，考慮車輛相對加加 速度恒定并受到白噪聲干擾，可以構(gòu)造一個(gè)變加速度的四階運(yùn)動(dòng)模型，又稱CJ模型(Const Jerk Model，常加加速度模型，簡稱CJ模型），其表示如下：
[0073] Xk+i = f (Xk)+Wk
[0074] 其中，系統(tǒng)在第k個(gè)時(shí)刻的狀態(tài)向量為：
[0076] 該狀態(tài)向量的維度L = 8,其中，在車體坐標(biāo)系下，X,之元f分別表示目標(biāo)車輛相 對自車在X方向上的橫向距離、橫向速度、橫向加速度和加加速度；y,夕，夕:y分別表示目標(biāo) 車輛相對自車在y方向上的縱向距離、縱向速度、縱向加速度和加加速度。
[0077] 根據(jù)系統(tǒng)運(yùn)動(dòng)模型，狀態(tài)轉(zhuǎn)移函數(shù)f( ·)可定義為：
[0079]其中，dt表示雷達(dá)傳感器的采樣周期。
[0080]在CJ模型中，向量叫=G匕,k ，其中，G, G·，G 分別 表示橫向距離、速度、加速度和加加速度受到的高斯白噪聲的方差；，&分別表示 縱向距離、速度、加速度和加加速度受到的高斯白噪聲的方差。
[0081]假設(shè)狀態(tài)向量各維度受到的高斯白噪聲相互獨(dú)立，則過程噪聲協(xié)方差矩陣Q(其表 征了實(shí)際運(yùn)動(dòng)過程與所選擇的運(yùn)動(dòng)模型之間的不符合程度)表示為：
[0083] 2、系統(tǒng)測量模型:其指系統(tǒng)狀態(tài)向量到測量值向量的映射，具體來說，其使用系統(tǒng) 測量函數(shù)h( ·)或者系統(tǒng)測量矩陣Η描述系統(tǒng)狀態(tài)到測量值的映射關(guān)系:Yk = h(Xk)或者Yk = Η · Xk〇
[0084] 自車的雷達(dá)傳感器可以測得目標(biāo)車輛的距離、角度和徑向速度信息，即系統(tǒng)在第k 個(gè)時(shí)刻的測量值向量可表示為：
[0085] Yk= [rk,ak, vk]T
[0086] 該測量值向量的維度0 = 3,其中，r表示目標(biāo)車輛的距離，a表示目標(biāo)車輛的角度，v 表示目標(biāo)車輛的徑向速度。
[0087]因此，系統(tǒng)測量模型可表示如下：
[0088]
其中 ξΓ、ξ3、ξν為 距離、角度、徑向速度的測量誤差，分別服從相互獨(dú)立的叭0上）4(0丄）4(0丨)高斯白噪 聲，因此，測量噪聲協(xié)方差矩陣R(表征了雷達(dá)傳感器實(shí)際測量值與所選擇的測量模型之間 的不符合程度)表示為：
[0090]下面對本發(fā)明的具體步驟進(jìn)行詳細(xì)說明。
[0091 ]本發(fā)明，即一種汽車?yán)走_(dá)目標(biāo)跟蹤方法，包括以下步驟：
[0092] 步驟S0，構(gòu)建目標(biāo)車輛運(yùn)動(dòng)模型以及汽車?yán)走_(dá)測量模型，并將目標(biāo)車輛的運(yùn)動(dòng)狀 態(tài)標(biāo)記為L維的運(yùn)動(dòng)狀態(tài)向量X，將汽車?yán)走_(dá)測得的目標(biāo)車輛信息標(biāo)記為測量值向量Y;根據(jù) 目標(biāo)車輛運(yùn)動(dòng)模型獲得狀態(tài)誤差協(xié)方差矩陣P和過程噪聲協(xié)方差矩陣Q，根據(jù)汽車?yán)走_(dá)測量 模型獲得測量噪聲協(xié)方差矩陣R(該獲得過程為如上所述的現(xiàn)有技術(shù)，此處不再贅述）；
[0093] 具體來說，構(gòu)建目標(biāo)車輛運(yùn)動(dòng)模型為:Xk+1 = f(Xk)+Wk，其中，Xk+1為第k+Ι個(gè)時(shí)刻的 運(yùn)動(dòng)狀態(tài)向量，Xk為第k個(gè)時(shí)刻的運(yùn)動(dòng)狀態(tài)向量，f( ·)為狀態(tài)轉(zhuǎn)移函數(shù)，wk為第k個(gè)時(shí)刻目標(biāo) 車輛的運(yùn)動(dòng)狀態(tài)受到的高斯白噪聲的方差矩陣；構(gòu)建汽車?yán)走_(dá)測量模型為:Yk = h(Xk)+Uk， 其中，Yk為第k個(gè)時(shí)刻的測量值向量，h( ·)為測量函數(shù)，uk為測量誤差矩陣；
[0094]將所述運(yùn)動(dòng)狀態(tài)向量X表示為：X二[X, i,戈,y, j,f,:p]T，其中，Λ?, 分別 表示目標(biāo)車輛相對于安裝有汽車?yán)走_(dá)的自車在X方向上的橫向距離、橫向速度、橫向加速度 和加加速度；y，夕,分別表示目標(biāo)車輛相對于自車在y方向上的縱向距離、縱向速度、縱 向加速度和加加速度；
[0095] 將所述測量值向量Y表示為4=[^&，￥]7，其中4表示目標(biāo)車輛的距離，&表示目 標(biāo)車輛的角度，V表示目標(biāo)車輛的徑向速度。
[0096] 步驟S1，將過程噪聲協(xié)方差矩陣Q和測量噪聲協(xié)方差矩陣R分別設(shè)置為常數(shù)矩陣， 并