邱 成，王 浩，劉鳳江，白冬杰，云 峰，李舒悅，趙 川

（1.北京交通大學(xué) 機(jī)械與電子控制工程學(xué)院，北京 100044；2.北京精密機(jī)電控制設(shè)備研究所，北京 100076；3.北京華航無(wú)線電測(cè)量研究所，北京 100013）

0 引言

伴隨著大眾對(duì)汽車主動(dòng)安全問(wèn)題的日益重視，車載毫米波雷達(dá)多目標(biāo)跟蹤也成為研究熱點(diǎn)，其性能直接影響著整個(gè)車輛路況環(huán)境監(jiān)測(cè)系統(tǒng)的可靠性。近年來(lái)，國(guó)內(nèi)外科研人員圍繞車載毫米波雷達(dá)進(jìn)行了大量研究[1?8]。這些研究成果對(duì)車載毫米波雷達(dá)的發(fā)展起到了積極的推動(dòng)作用。

在車載毫米波雷達(dá)對(duì)多個(gè)目標(biāo)車輛進(jìn)行跟蹤的過(guò)程中，需要從大量的噪聲中提取出有效的量測(cè)數(shù)據(jù)。在同樣的信噪比下，為了進(jìn)一步提高跟蹤精度，需要根據(jù)目標(biāo)運(yùn)動(dòng)特性進(jìn)行有效濾波。在線性系統(tǒng)中卡爾曼濾波器得到廣泛應(yīng)用，其濾波誤差的均方陣可通過(guò)算法直接獲得，是一種遞推最優(yōu)理論。然而，現(xiàn)實(shí)道路狀況中，車輛的運(yùn)行狀態(tài)多為非線性的，致使卡爾曼濾波器有時(shí)難以對(duì)目標(biāo)進(jìn)行有效濾波。對(duì)于非線性濾波問(wèn)題，至今也沒(méi)有完善的解決辦法。

針對(duì)以上分析，本文提出了基于擴(kuò)展卡爾曼濾波器（Extended Kalman Filter，EKF）的毫米波雷達(dá)多目標(biāo)跟蹤算法，其主要運(yùn)用擴(kuò)展卡爾曼濾波器解決目標(biāo)車輛非線性運(yùn)動(dòng)狀態(tài)的濾波問(wèn)題，以實(shí)現(xiàn)車載毫米波雷達(dá)對(duì)多個(gè)目標(biāo)的有效跟蹤。

1 跟蹤對(duì)象的數(shù)學(xué)模型

1）狀態(tài)方程

當(dāng)汽車在道路上作勻速直線行駛時(shí)，離散系統(tǒng)中tk時(shí)刻目標(biāo)車輛的運(yùn)動(dòng)狀態(tài)(xk,yk)為：

式中：(x0,y0)為跟蹤目標(biāo)的初始坐標(biāo)；vx及vy分別為目標(biāo)對(duì)象沿兩個(gè)方向的速度；T為采樣時(shí)間。

按照遞推形式改寫，式（1），式（2）可以表示為：

目標(biāo)的狀態(tài)方程可表示為：

式中：X(k)為狀態(tài)向量；F(k)為轉(zhuǎn)移矩陣；V(k)為系統(tǒng)過(guò)程噪聲，并假設(shè)目標(biāo)初始狀態(tài)、系統(tǒng)過(guò)程噪聲及量測(cè)噪聲之間相互獨(dú)立。

考慮目標(biāo)運(yùn)動(dòng)過(guò)程中的控制信號(hào)，其狀態(tài)方程可表示為：

利用增加狀態(tài)向量維數(shù)的方法，可以進(jìn)一步提高估計(jì)的準(zhǔn)確性，但也會(huì)造成計(jì)算量相繼變大的缺點(diǎn)。因而，在符合要求的前提下，盡可能選擇簡(jiǎn)易模型。

2）量測(cè)方程

雷達(dá)對(duì)測(cè)量過(guò)程的假設(shè)，用量測(cè)方程可表示為：

式中：Z(k)為量測(cè)向量；H(k)為量測(cè)矩陣；W(k)為量測(cè)噪聲，通常認(rèn)為量測(cè)噪聲服從高斯分布，其協(xié)方差用R(k)表示。

假設(shè)量測(cè)噪聲和過(guò)程噪聲序列互不相關(guān)，且系統(tǒng)的初始協(xié)方差矩陣為P(0| 0)，初始狀態(tài)估計(jì)為與線性情況類似，假設(shè)k時(shí)刻的估計(jì)為：

它不是一個(gè)近似的條件均值，其相伴協(xié)方差矩陣為P(k|k)。因?yàn)?k|k)是非精確的條件均值，所以，P(k|k)是近似的均方誤差，而非協(xié)方差。但是，通常還是稱其為協(xié)方差。

2 多目標(biāo)跟蹤算法

車載毫米波雷達(dá)多目標(biāo)跟蹤算法主要包括航跡起始、航跡數(shù)據(jù)關(guān)聯(lián)、航跡跟蹤濾波、航跡管理等幾個(gè)方面。算法采用EKF 狀態(tài)估計(jì)方法，能夠在線跟蹤多個(gè)運(yùn)動(dòng)目標(biāo)，且可以去除靜止目標(biāo)。多目標(biāo)跟蹤算法處理的詳細(xì)流程如圖1 所示。

圖1 多目標(biāo)跟蹤算法處理的詳細(xì)流程圖

擴(kuò)展卡爾曼濾波的實(shí)質(zhì)是將非線性濾波問(wèn)題近似轉(zhuǎn)化為線性濾波問(wèn)題，以此可以參照卡爾曼濾波過(guò)程來(lái)解決工程上的非線性問(wèn)題，是應(yīng)對(duì)非線性濾波問(wèn)題較為可行的方法[9?12]。

考慮到實(shí)際情況中車載毫米波雷達(dá)多目標(biāo)跟蹤對(duì)計(jì)算量的要求，其計(jì)算過(guò)程如下：

狀態(tài)的一步預(yù)測(cè)：

協(xié)方差的一步預(yù)測(cè)：

量測(cè)的一步預(yù)測(cè)：

量測(cè)預(yù)測(cè)協(xié)方差的計(jì)算：

增益的計(jì)算：

狀態(tài)的更新：

協(xié)方差的更新：

海洋中生活著各種神奇的生物，有令人聞風(fēng)喪膽的“海怪”，也有優(yōu)雅的“美人魚”。不知你是否聽說(shuō)過(guò)，海洋中還有一種被人稱作“海天使”的神奇物種，它晶瑩剔透，嬌小可愛，雖然科學(xué)家已在北極冰海和日本北海道的冰原下發(fā)現(xiàn)了它的蹤跡，但至今仍摸不清它的來(lái)歷，這更給海天使增添了幾分神秘色彩。

式中I是與協(xié)方差相對(duì)應(yīng)的單位矩陣。

3 實(shí)驗(yàn)與分析

3.1 實(shí)驗(yàn)情況簡(jiǎn)介

車載實(shí)驗(yàn)所用的毫米波雷達(dá)為MFSK 雷達(dá)，通過(guò)工裝安裝在實(shí)驗(yàn)車輛的尾部。圖2 給出了毫米波雷達(dá)的車載安裝圖。

圖2 毫米波雷達(dá)的車載安裝圖

本實(shí)驗(yàn)共設(shè)置了4 種實(shí)驗(yàn)情景，分別就單目標(biāo)車輛和雙目標(biāo)車輛在遠(yuǎn)離和接近車載毫米波雷達(dá)的過(guò)程中進(jìn)行了回波采集，以獲取目標(biāo)車輛的距離、速度、角度等信息。實(shí)驗(yàn)參數(shù)設(shè)置如表1 所示。

表1 實(shí)驗(yàn)參數(shù)設(shè)置

3.2 實(shí)驗(yàn)結(jié)果分析

單個(gè)目標(biāo)車輛從遠(yuǎn)處以30 km/h 的速度向雷達(dá)駛近，對(duì)此過(guò)程中的目標(biāo)車輛進(jìn)行跟蹤，跟蹤結(jié)果如圖3所示?？梢钥闯?，當(dāng)前算法可以實(shí)現(xiàn)對(duì)目標(biāo)車輛的跟蹤。但在跟蹤過(guò)程中，位于50 m 左右的地方存在未知目標(biāo)，其返回波的能量幅值高于真實(shí)目標(biāo)，致使在跟蹤過(guò)程中將其當(dāng)作是靜止的車輛目標(biāo)。

圖3 單車30 km/h 動(dòng)態(tài)接近回波采集跟蹤結(jié)果圖

在單個(gè)目標(biāo)車輛駛近雷達(dá)之后，將其按照30 km/h的速度遠(yuǎn)離雷達(dá)，在此過(guò)程對(duì)目標(biāo)車輛進(jìn)行跟蹤，圖4為具體跟蹤結(jié)果。從跟蹤結(jié)果可以看出，當(dāng)前算法可以實(shí)現(xiàn)對(duì)目標(biāo)的跟蹤。在雷達(dá)開機(jī)過(guò)程中，有一未知目標(biāo)出現(xiàn)在雷達(dá)探測(cè)范圍內(nèi)，但由于所探測(cè)到的不連續(xù)點(diǎn)跡未達(dá)到跟蹤條件，算法未對(duì)其進(jìn)行跟蹤。

圖4 單車30 km/h 動(dòng)態(tài)遠(yuǎn)離回波采集跟蹤結(jié)果圖

圖5 和圖6 為雙車以30 km/h 的車速進(jìn)行動(dòng)態(tài)接近回波采集，目前算法能夠?qū)δ繕?biāo)進(jìn)行跟蹤。但由于在跟蹤過(guò)程中，雷達(dá)在部分區(qū)域未探測(cè)到連續(xù)點(diǎn)跡，導(dǎo)致有部分航跡缺失。

圖5 雙車（間距10 m）30 km/h 動(dòng)態(tài)接近回波采集跟蹤結(jié)果圖

圖6 雙車（間距20 m）30 km/h 動(dòng)態(tài)接近回波采集跟蹤結(jié)果圖

圖7 和圖8 為雙車以30 km/h 的車速進(jìn)行動(dòng)態(tài)遠(yuǎn)離回波采集，目前算法能夠?qū)δ繕?biāo)進(jìn)行跟蹤。但是，雙車間距10 m 跟蹤時(shí)，近距離航跡偏離目標(biāo)實(shí)際位置；雙車間距20 m 跟蹤時(shí)，部分區(qū)域未完全形成連續(xù)航跡。

圖7 雙車（間距10 m）30 km/h 動(dòng)態(tài)遠(yuǎn)離回波采集跟蹤結(jié)果圖

圖8 雙車（間距20 m）30 km/動(dòng)態(tài)遠(yuǎn)離回波采集跟蹤結(jié)果圖

1）量測(cè)噪聲協(xié)方差矩陣的計(jì)算中，距離測(cè)量誤差方差和角度測(cè)量誤差方差的值選取困難，造成量測(cè)噪聲協(xié)方差矩陣的初始狀態(tài)與其真實(shí)值有較大的誤差，從而導(dǎo)致預(yù)測(cè)偏差較大的問(wèn)題。

2）在實(shí)際的濾波過(guò)程中，距離和角度的精度會(huì)直接影響量測(cè)矩陣。但是由于雷達(dá)的測(cè)量精度有限，兩者的值存在較大的誤差，而在角度矯正公式中：

參數(shù)α及β的大小是依據(jù)經(jīng)驗(yàn)值而定，使得矯正后的角度相比其真實(shí)值仍存在較大誤差，難以滿足要求。

3）擴(kuò)展卡爾曼協(xié)方差矩陣初值選取困難。在擴(kuò)展卡爾曼濾波器的使用過(guò)程中，其穩(wěn)定性和狀態(tài)估計(jì)精度受到了過(guò)程中潛在的非線性因素的較大影響，而噪聲的統(tǒng)計(jì)特性也會(huì)對(duì)其濾波效果產(chǎn)生影響。在濾波過(guò)程中，量測(cè)噪聲協(xié)方差R(k)和過(guò)程噪聲協(xié)方差Q(k)均不發(fā)生變化，假設(shè)兩矩陣預(yù)先估計(jì)的準(zhǔn)確度不高，將會(huì)在后續(xù)過(guò)程引起誤差累計(jì)，進(jìn)而影響整個(gè)過(guò)程，產(chǎn)生濾波發(fā)散。

4）運(yùn)用泰勒級(jí)數(shù)展開的方法，對(duì)雅克比矩陣hX(k+1)進(jìn)行展開，在這個(gè)過(guò)程中對(duì)其高階項(xiàng)的忽略，也可能是濾波發(fā)散的原因。

4 結(jié)語(yǔ)

本文針對(duì)非線性系統(tǒng)，對(duì)擴(kuò)展卡爾曼濾波算法的濾波過(guò)程及其濾波參數(shù)計(jì)算進(jìn)行了推導(dǎo)，結(jié)合航跡起始、航跡數(shù)據(jù)關(guān)聯(lián)、航跡管理等算法，提出了一種基于擴(kuò)展卡爾曼濾波器的車載毫米波雷達(dá)多目標(biāo)跟蹤算法，并設(shè)計(jì)實(shí)驗(yàn)對(duì)其濾波跟蹤效果進(jìn)行了驗(yàn)證。通過(guò)實(shí)驗(yàn)結(jié)果可知，該算法可以對(duì)車載毫米波雷達(dá)探測(cè)到的目標(biāo)車輛信息進(jìn)行跟蹤濾波，但仍然存在一些需要完善的地方，這也是下一步的研究?jī)?nèi)容。