機(jī)動(dòng)目標(biāo)跟蹤算法在車載毫米波雷達(dá)防追尾系統(tǒng)中的應(yīng)用
IMM算法可歸納如下4 個(gè)步驟。
步驟1 輸入交互:
根據(jù)兩模型(k-l)時(shí)刻的濾波值和模型概率,計(jì)算交互混合后的濾波初始值,包括模型1 的濾波初始值:濾波估計(jì)值X 01
(k - 1)和估計(jì)協(xié)方差μ1(k - 1);模型2 的濾波初始值:濾波估計(jì)值X 02
(k - 1)和估計(jì)誤差協(xié)方差P02
(k - 1)。設(shè)系統(tǒng)在(k-1)時(shí)刻模型1 概率為μ1(k - 1),濾波值X1
(k - 1),估計(jì)誤差協(xié)方差為P2(k - 1)。模型2 的概率為μ2(k - 1),濾波值為X 2
(k - 1),系統(tǒng)估計(jì)誤差協(xié)方差為P2(k - 1)。則進(jìn)一步推廣到r 個(gè)模型,交互后r模型的濾波初始值為:
步驟2 模型條件濾波:
對(duì)應(yīng)于模型Mj(k),以X 0j
(k - 1|k - 1),P0j(k - 1|k - 1)及Z(k)作為輸入進(jìn)行卡爾曼濾波。
卡爾曼預(yù)測方程:
i = 1rΛj(k)cj_,而Λj(k)為觀測Z(k)的似然函數(shù):
圖2 為IMM算法結(jié)構(gòu)原理圖
4 車輛運(yùn)動(dòng)模型分析與IMM算法跟蹤仿真
試驗(yàn)設(shè)計(jì):考慮兩輛車在道路上同向行駛,在0~10 s 時(shí),兩車均保持勻速直線運(yùn)動(dòng),由安裝在后車上的車載毫米波雷達(dá)檢測出與前車的距離為100 m,相對(duì)速度為-3 m/s,方位角2°。
在10~15 s 時(shí),前車向右偏轉(zhuǎn),與后車的相對(duì)角加速度為1° s2。
后車加速,與前車的縱向相對(duì)加速度為a = -1.8 m/s2。雷達(dá)的掃描周期為T=0.1 s,系統(tǒng)噪聲為σα = 0.3 m/s,σβ = 0.3°/s。量測誤差為σ1 = 1 mσ2 = 0.5 m/sσ3 = 0.2°/s。
評(píng)論