交互多模型擴(kuò)展卡爾曼濾波算法的FPGA實(shí)現(xiàn)

,

(清華大學(xué) 自動(dòng)化系,北京 100084)

0 引 言

目標(biāo)跟蹤(target tracking)利用探測(cè)器對(duì)運(yùn)動(dòng)目標(biāo)的某些參數(shù)進(jìn)行測(cè)量，以此確定關(guān)注區(qū)域內(nèi)目標(biāo)個(gè)數(shù)、位置以及運(yùn)動(dòng)參數(shù)等狀態(tài),其在國(guó)防與民用領(lǐng)域均有重要應(yīng)用價(jià)值[1]。根據(jù)探測(cè)器是否主動(dòng)發(fā)射信號(hào)，又可分為主動(dòng)跟蹤與被動(dòng)跟蹤。在被動(dòng)跟蹤方法中，純方位角度跟蹤因其具有隱蔽性，備受各國(guó)軍方重視。

純方位角度跟蹤利用單站測(cè)向設(shè)備測(cè)量目標(biāo)的運(yùn)動(dòng)方位信息，并用這些方位信息序列來(lái)實(shí)時(shí)估計(jì)目標(biāo)軌跡[2]。目標(biāo)跟蹤的經(jīng)典方法有卡爾曼濾波(KF)、擴(kuò)展卡爾曼濾波(EKF)以及粒子濾波(PF)等。交互多模型擴(kuò)展卡爾曼濾波(IMMEKF)算法[3]是多模型算法中的一種經(jīng)典算法，其基本思想是建立一個(gè)由若干模型組成的模型集，目標(biāo)以一定概率符合某個(gè)運(yùn)動(dòng)模型。各模型均采用EKF算法進(jìn)行濾波。IMMEKF算法具有一定的非線性處理能力，且運(yùn)算速度較快。

當(dāng)前，對(duì)于純方位角度跟蹤問(wèn)題的研究大多集中在算法和仿真方面[4～6]，對(duì)于算法的硬件實(shí)現(xiàn)方面的研究則較少。而實(shí)際應(yīng)用中，大量的移動(dòng)平臺(tái)亟需準(zhǔn)確度、實(shí)時(shí)性均高的跟蹤算法。將跟蹤算法硬件化，乃至設(shè)計(jì)出專用處理芯片是目標(biāo)跟蹤領(lǐng)域的一個(gè)重要研究方向?，F(xiàn)場(chǎng)可編程門陣列(field-programmable gate array,FPGA)是一種擁有大量計(jì)算和存儲(chǔ)資源的高性能芯片，其最大的優(yōu)點(diǎn)是可以通過(guò)可重復(fù)配置快速實(shí)現(xiàn)設(shè)計(jì)驗(yàn)證。硬件算法如果要轉(zhuǎn)化成專用處理芯片，可以先通過(guò)FPGA驗(yàn)證其設(shè)計(jì)的正確性。文獻(xiàn)[7]對(duì)數(shù)種PF算法進(jìn)行了修改，提出新的重采樣方法，以期得到適于硬件實(shí)現(xiàn)并提升處理速度的結(jié)構(gòu)。同時(shí)，提出了一種分布式PF結(jié)構(gòu)，提高了濾波器的可擴(kuò)展性。文獻(xiàn)[2]針對(duì)純方位角度跟蹤問(wèn)題提出了一種簡(jiǎn)化的PF算法，并給出了FPGA上的實(shí)現(xiàn)方法。但是，上述研究都是基于單模型算法，對(duì)于多模型算法的硬件實(shí)現(xiàn)卻很少有人研究。作為多模型算法中最經(jīng)典的算法，研究IMMEKF算法的硬件實(shí)現(xiàn)能夠?yàn)槠渌嗄Ｐ退惴ǖ挠布?shí)現(xiàn)奠定基礎(chǔ)。本文針對(duì)IMMEKF算法，將其轉(zhuǎn)化為易于硬件實(shí)現(xiàn)的結(jié)構(gòu)，并用Verilog HDL語(yǔ)言將其在FPGA上硬件實(shí)現(xiàn)。

1 純方位角度跟蹤問(wèn)題

目標(biāo)跟蹤問(wèn)題的本質(zhì)是根據(jù)被噪聲污染的觀測(cè)數(shù)據(jù)來(lái)估計(jì)目標(biāo)的狀態(tài)[8]。假設(shè)目標(biāo)在二維水平面上運(yùn)動(dòng)，則目標(biāo)的運(yùn)動(dòng)狀態(tài)可以用如下向量來(lái)表示

(1)

(2)

其中，Φ為狀態(tài)轉(zhuǎn)移矩陣，而Γ為系統(tǒng)噪聲轉(zhuǎn)移矩陣，H為觀測(cè)方程狀態(tài)轉(zhuǎn)移矩陣，G為觀測(cè)方程噪聲轉(zhuǎn)移矩陣，Wk-1和Vk均為均值為零的高斯白噪聲，方差分別為Qk-1和Rk。觀測(cè)方程是

(3)

觀測(cè)值為k時(shí)刻目標(biāo)與觀測(cè)點(diǎn)連線和Y軸夾角的正切值。

2 IMMEKF算法與仿真

2.1 IMMEKF算法

IMMEKF算法的每個(gè)模型都匹配擴(kuò)展卡爾曼濾波器。計(jì)算流程可分為4個(gè)步驟：

(4)

(5)

(6)

(7)

4)合成：將每個(gè)模型的估計(jì)結(jié)果合成

(8)

(9)

IMMEKF算法流程如圖1所示。

圖1 IMMEKF算法流程圖

2.2 仿真結(jié)果

表1 運(yùn)動(dòng)過(guò)程

從圖2可以看出:估計(jì)軌跡與真實(shí)軌跡基本重合，算法較準(zhǔn)確地跟蹤了目標(biāo)。從圖3可以看到:在前1/3時(shí)間內(nèi)，模型1(CV)的概率值分布在0.8～0.9之間，處于絕對(duì)優(yōu)勢(shì)地位;第二個(gè)1/3時(shí)間內(nèi)，模型2(CT)的概率值分布在0.7～0.8之間;而在最后1/3時(shí)間內(nèi)，模型1(CV)的概率值又分布在0.8～0.9之間。這說(shuō)明算法能夠較準(zhǔn)確地對(duì)模型進(jìn)行估計(jì)。而在模型轉(zhuǎn)換時(shí)刻，概率值迅速發(fā)生跳變，這也保證了跟蹤精度。

圖2 IMMEKF算法跟蹤效果圖

圖3 IMMEKF算法各模型概率圖

3 IMMEKF算法FPGA實(shí)現(xiàn)方案與仿真結(jié)果

本節(jié)介紹IMMEKF算法硬件實(shí)現(xiàn)的過(guò)程與在FPGA上進(jìn)行硬件實(shí)現(xiàn)的方案和仿真結(jié)果,IMMEKF硬件算法模塊框圖如圖4所示。

圖4 IMMEKF硬件算法模塊框圖

算法所需的常量，如狀態(tài)轉(zhuǎn)移矩陣等，全部存儲(chǔ)在FPGA的ROM中,當(dāng)接收到新測(cè)量值Zk時(shí)，將所有數(shù)據(jù)輸入濾波器進(jìn)行處理。經(jīng)過(guò)混合、濾波、更新權(quán)重、合成4個(gè)步驟后得到估計(jì)值k,然后等待下一次測(cè)量值的輸入。

3.1 FPGA浮點(diǎn)數(shù)矩陣運(yùn)算

IMMEKF算法需要用到大量的浮點(diǎn)數(shù)矩陣運(yùn)算[9,10]。在FPGA上進(jìn)行浮點(diǎn)數(shù)矩陣運(yùn)算，如果采用芯片廠商提供的矩陣運(yùn)算IP核，會(huì)消耗大量的硬件資源。所以,本文以芯片廠商提供的32位浮點(diǎn)數(shù)運(yùn)算IP核為基礎(chǔ)，編寫了浮點(diǎn)數(shù)矩陣運(yùn)算模塊。計(jì)算4維向量?jī)?nèi)積和矩陣相乘的模塊算法圖見(jiàn)圖5、圖6。其中，圖5為4維向量?jī)?nèi)積計(jì)算流程，圖6為4維矩陣乘積計(jì)算流程。

圖5 4維向量?jī)?nèi)積計(jì)算流程

圖6 4維矩陣乘積計(jì)算流程

向量?jī)?nèi)積和矩陣乘積計(jì)算的第一步都是將向量或矩陣中的元素讀取出來(lái)。對(duì)于向量?jī)?nèi)積計(jì)算，分別調(diào)用浮點(diǎn)數(shù)乘加模塊進(jìn)行計(jì)算,而對(duì)于矩陣乘積計(jì)算，則調(diào)用向量?jī)?nèi)積計(jì)算模塊得到結(jié)果矩陣中的每一個(gè)元素。

3.2 仿真結(jié)果

在用Verilog HDL語(yǔ)言對(duì)上述硬件算法進(jìn)行描述之后，對(duì)上文中所示純方位角度跟蹤問(wèn)題，運(yùn)用硬件算法進(jìn)行跟蹤。選擇了Altera公司的Cyclone IV GX系列的FPGA芯片EP4CGX150DF31I7進(jìn)行了綜合和時(shí)序仿真，系統(tǒng)時(shí)鐘設(shè)為50 MHz。

硬件仿真與軟件仿真結(jié)果對(duì)比如圖7、圖8、圖9所示。其中，圖7表示軟件仿真、硬件仿真跟蹤結(jié)果，圖8表示軟、硬件仿真得到的跟蹤結(jié)果之差，圖9表示模型概率之間的差。

圖7 軟硬件仿真跟蹤結(jié)果對(duì)比

圖8 軟硬件仿真跟蹤結(jié)果之差

圖7表明：IMMEKF算法較準(zhǔn)確地跟蹤了目標(biāo)，沒(méi)有出現(xiàn)失跟，同時(shí)，軟硬件算法得到的坐標(biāo)估計(jì)值基本重合。這說(shuō)明硬件算法的跟蹤效果與軟件算法相當(dāng)。從圖8可以看到,位置估計(jì)的誤差在10-4數(shù)量級(jí)，而速度估計(jì)的誤差在10-5數(shù)量級(jí)，而圖9顯示模型概率誤差也在10-6數(shù)量級(jí)。

圖9 軟硬件仿真模型概率之差

軟硬件算法跟蹤誤差絕對(duì)值最大值如表2所示。

表2 軟硬件算法跟蹤最大誤差絕對(duì)值

這表明硬件算法能夠達(dá)到與軟件仿真相當(dāng)?shù)木取Ｔ?0 MHz脈沖激勵(lì)下，每一步運(yùn)算耗費(fèi)時(shí)間530 898 876 ps。而同樣的算法在Matlab環(huán)境運(yùn)行時(shí)，每一步耗費(fèi)時(shí)間約為0.01 s?？梢?jiàn)，硬件算法能夠?qū)崿F(xiàn)對(duì)本文中機(jī)動(dòng)目標(biāo)的跟蹤，而且相比軟件仿真具有明顯的速度優(yōu)勢(shì)，這在對(duì)運(yùn)算速度有較高要求的軍事場(chǎng)合是極為重要的。

4 結(jié) 論

本文針對(duì)純方位角度跟蹤問(wèn)題，將IMMEKF算法分解成為適于硬件處理的結(jié)構(gòu)，并給出了該算法在FPGA上硬件實(shí)現(xiàn)的方案。FPGA上的仿真實(shí)驗(yàn)表明：IMMEKF硬件算法能夠?qū)C(jī)動(dòng)目標(biāo)進(jìn)行跟蹤，并取得與軟件仿真相當(dāng)?shù)木?，同時(shí)，運(yùn)算時(shí)間大大減少。IMMEKF算法在FPGA上的硬件實(shí)現(xiàn)為該算法流片奠定了基礎(chǔ)。

參考文獻(xiàn):

[1]權(quán)太范.目標(biāo)跟蹤新理論與技術(shù)[M].北京：國(guó)防工業(yè)出版社,2009.

[2]洪少華,史治國(guó),陳抗生.用于純方位跟蹤的簡(jiǎn)化粒子濾波算法及其硬件實(shí)現(xiàn)[J].電子與信息學(xué)報(bào),2009,31(1):96-100.

[3]Blom H,Bar-Shalom Y.The interacting multiple model algorithm for systems with Markovian switching coefficients[J].IEEE Transactions on Automatic Control,1988,33(3):780-783.

[4]Habtemariam B,Aravinthan A,Tharmarasa R,et al.Distributed tra-cking with a PHD filter using effcient measurement encoding[J].Journal of Advances in Information Fusion,2012,7(2):114-130.

[5]Georgescu R,Willett P,Marano S,et al.Predetection fusion in large sensor networks with unknown target locations[J].Journal of Advances in Information Fusion,2012,7(1):61-77.

[6]Yuan Ting,Bar-Shalom Y,Tian Xin. Heterogeneous track-to-track[J].Journal of Advances in Information Fusion,2011,6(2):131-149.

[7]Bolic M.Architectures for efficient implementation of particle filters[D].Stony Brook:Stony Brook University,2004.

[8]Arulampalam M,Ristic B,Gordon N,et al.Bearings-only tracking of maneuvering targets using particle filters[J].EURASIP Journal on Applied Signal Processing,2004(15):2351-2365.

[9]鄔貴明.FPGA矩陣計(jì)算并行算法與結(jié)構(gòu)[D].長(zhǎng)沙:國(guó)防科學(xué)技術(shù)大學(xué),2011.

[10] Dou Yong,Vassiliadis S,Kuzmanov G,et al.64-bit floating-point FPGA matrix multiplication[C]∥Proceedings of the 13th ACM/SIGDA International Symposium on Field Programmable Gate Arrays(FPGA’05)，Monterey:ACM,2005:86-95.