方　峰，蔡遠(yuǎn)利

(西安交通大學(xué) 電子與信息工程學(xué)院，西安　710049)

?

機(jī)動(dòng)目標(biāo)多傳感器組網(wǎng)空間配準(zhǔn)方法

方峰，蔡遠(yuǎn)利

(西安交通大學(xué) 電子與信息工程學(xué)院，西安710049)

針對(duì)存在傳感器偏差的多傳感器組網(wǎng)系統(tǒng)跟蹤機(jī)動(dòng)目標(biāo)的問題，把目標(biāo)狀態(tài)與傳感器偏差進(jìn)行解耦估計(jì)，提出了一種基于交互多模型的兩階段擴(kuò)展Kalman濾波(IMM-TSEKF)算法。由于傳感器觀測(cè)方程的非線性，文中采用了兩階段擴(kuò)展Kalman濾波器(TSEKF)，針對(duì)機(jī)動(dòng)目標(biāo)，把IMM算法與TSEKF算法相結(jié)合用于目標(biāo)跟蹤與空間配準(zhǔn)。此外還對(duì)算法的時(shí)間復(fù)雜度進(jìn)行了分析，并以螺旋機(jī)動(dòng)戰(zhàn)術(shù)彈道導(dǎo)彈為目標(biāo)進(jìn)行組網(wǎng)空間配準(zhǔn)與目標(biāo)跟蹤。仿真結(jié)果表明，相比于常規(guī)的基于交互多模型的增廣Kalman濾波(IMM-ASEKF)算法，該文算法在估計(jì)性能相當(dāng)?shù)那闆r下，減小了計(jì)算的復(fù)雜度，提高了計(jì)算效率，更易于工程實(shí)現(xiàn)。

兩階段Kalman濾波；交互多模型；目標(biāo)跟蹤；空間配準(zhǔn)

0　引言

在目標(biāo)跟蹤系統(tǒng)中，多傳感器組網(wǎng)系統(tǒng)利用數(shù)據(jù)融合技術(shù)可增強(qiáng)跟蹤系統(tǒng)的性能。但在實(shí)際應(yīng)用中，組網(wǎng)系統(tǒng)的傳感器均可能存在系統(tǒng)偏差和隨機(jī)干擾，在進(jìn)行數(shù)據(jù)融合之前必須對(duì)傳感器進(jìn)行空間配準(zhǔn)[1-2]。

空間配準(zhǔn)又稱為傳感器配準(zhǔn)，就是借助于多傳感器關(guān)于共同目標(biāo)的測(cè)量對(duì)傳感器的偏差進(jìn)行估計(jì)和補(bǔ)償。針對(duì)空間配準(zhǔn)，常用的方法有離線配準(zhǔn)法和在線配準(zhǔn)法。離線配準(zhǔn)法主要包括最小二乘法[3]、精確極大似然法[4]等，但是這類方法的實(shí)時(shí)性不高。而在線配準(zhǔn)法能夠?qū)崟r(shí)進(jìn)行空間配準(zhǔn)，它主要有：增廣Kalman濾波算法，如增廣擴(kuò)展Kalman濾波(ASEKF)算法[5]、增廣UKF算法[6]等；基于高斯均值移動(dòng)的配準(zhǔn)算法[7]，基于EM準(zhǔn)則的配準(zhǔn)算法[8-9]，基于熵函數(shù)法的配準(zhǔn)算法[10]等。由于增廣Kalman濾波算法的可實(shí)現(xiàn)性強(qiáng)、估計(jì)精度高，它在實(shí)際應(yīng)用中最為廣泛。然而由于增廣Kalman濾波算法把傳感器偏差增廣到了系統(tǒng)狀態(tài)中，當(dāng)傳感器偏差個(gè)數(shù)較多時(shí)，會(huì)引起維數(shù)災(zāi)難，且在某些病態(tài)情況下會(huì)出現(xiàn)數(shù)值計(jì)算問題?；诖?，由Berand Friedland提出的兩階段卡爾曼濾波器[11-14](TSKF)能夠很好地解決狀態(tài)增廣帶來(lái)的計(jì)算問題。TSKF將狀態(tài)和偏差進(jìn)行解耦估計(jì)，減小了計(jì)算的復(fù)雜度，且穩(wěn)定性更好，更易于工程實(shí)現(xiàn)。此外，在進(jìn)行空間配準(zhǔn)時(shí)，上述文獻(xiàn)大多都只考慮了目標(biāo)做弱機(jī)動(dòng)的情況。然而，當(dāng)目標(biāo)做強(qiáng)機(jī)動(dòng)飛行時(shí)，就需要進(jìn)一步考慮濾波算法跟蹤機(jī)動(dòng)目標(biāo)的能力。

由于交互多模型(IMM)算法[15]對(duì)機(jī)動(dòng)目標(biāo)有很好的跟蹤能力，故本文將IMM算法與兩階段擴(kuò)展Kalman濾波(TSEKF)算法相結(jié)合，提出了IMM-TSEKF算法用于機(jī)動(dòng)目標(biāo)跟蹤和傳感器偏差估計(jì)。IMM-TSEKF將系統(tǒng)狀態(tài)和傳感器偏差進(jìn)行解耦估計(jì)，避免了由狀態(tài)增廣引起的維數(shù)災(zāi)難。針對(duì)戰(zhàn)術(shù)彈道導(dǎo)彈(TBM)在再入段進(jìn)行螺旋機(jī)動(dòng)的情況進(jìn)行目標(biāo)跟蹤與空間配準(zhǔn)，仿真結(jié)果表明，本文算法能夠很好地跟蹤螺旋機(jī)動(dòng)TBM，且相比于常規(guī)的交互多模型增廣擴(kuò)展Kalman濾波(IMM-ASEKF)算法，本文算法在估計(jì)精度相當(dāng)?shù)耐瑫r(shí)，降低了計(jì)算的時(shí)間復(fù)雜度，算法的穩(wěn)定性更好，更易于工程實(shí)現(xiàn)。

1　兩階段擴(kuò)展Kalman濾波器

1.1問題描述

考慮多傳感器組網(wǎng)系統(tǒng)，假設(shè)有L個(gè)傳感器，且認(rèn)為各傳感器之間同步工作。系統(tǒng)采用集中式融合估計(jì)對(duì)目標(biāo)進(jìn)行跟蹤，目標(biāo)的狀態(tài)方程和傳感器的觀測(cè)方程可表示如下：

(1)

(2)

式中Xk為目標(biāo)狀態(tài)向量；Fk為狀態(tài)轉(zhuǎn)移陣；h(·)為非線性函數(shù)；Zk+1為觀測(cè)向量，Zk+1=[z1，k+1，z2，k+1，…，zL，k+1]T，其中zi，k+1表示第i個(gè)傳感器的觀測(cè)向量；b為傳感器的固定偏差，b=[b1，b2，…，bL]T，其中bi表示第i個(gè)傳感器的偏差向量；ωk為過(guò)程噪聲；vk為觀測(cè)噪聲。

過(guò)程噪聲ωk和觀測(cè)噪聲vk相互獨(dú)立，且都是零均值的高斯白噪聲，協(xié)方差矩陣分別為Qk和Rk。ωk、vk與狀態(tài)初值X0相互獨(dú)立。

1.2TSEKF算法

由于TSKF只適用于線性系統(tǒng)，為了將其推廣到非線性系統(tǒng)，本文引入擴(kuò)展Kalman濾波的思想，從而得到兩階段擴(kuò)展Kalman濾波器(TSEKF)。TSEKF的基本思想是先將系統(tǒng)線性化，即將非線性系統(tǒng)在狀態(tài)預(yù)測(cè)值和偏差預(yù)測(cè)值處進(jìn)行泰勒級(jí)數(shù)展開，保留線性主導(dǎo)項(xiàng)；然后對(duì)線性化后得到的系統(tǒng)運(yùn)用TSKF算法進(jìn)行濾波。

1.2.1系統(tǒng)線性化

(3)

其中

將式(3)取一階線性項(xiàng)并進(jìn)行整理，可得

(4)

其中

通過(guò)上述變化，式(4)就是線性化后的觀測(cè)方程。

1.2.2TSKF

將線性化后得到的式(1)和式(4)所表征的線性系統(tǒng)，運(yùn)用TSKF算法進(jìn)行濾波，具體步驟如下：

(1)“無(wú)偏差濾波器”

(5)

(6)

(7)

(8)

(9)

(2)偏差濾波器

(10)

Sk=HkUk+Ck

(11)

(12)

(13)

(14)

(15)

(16)

(17)

(3)系統(tǒng)狀態(tài)真實(shí)估計(jì)值

(18)

(19)

(20)

(21)

2　IMM-TSEKF

本節(jié)將TSEKF算法與IMM算法相結(jié)合，推導(dǎo)得到了IMM-TSEKF算法。IMM-TSEKF算法包括輸入初值交互、TSEKF濾波器、模型概率估計(jì)器和估計(jì)混合器。

圖1　IMM-TSEKF算法流程圖

IMM-TSEKF算法從k-1時(shí)刻到k時(shí)刻的遞推步驟如下：

(1)輸入初值交互

IMM-TSEKF的輸入初值交互包括“無(wú)偏差濾波器”的輸入初值交互和偏差濾波器的輸入初值交互這兩部分。

首先，進(jìn)行“無(wú)偏差濾波器”的輸入初值交互。由IMM算法的輸入交互計(jì)算式，可得

(22)

(23)

又由TSEKF中的式(20)、式(21)可知模型j的狀態(tài)估計(jì)值和協(xié)方差矩陣分別為

(24)

(25)

如式(24)和式(25)所示，TSEKF將無(wú)偏差狀態(tài)估計(jì)與偏差估計(jì)進(jìn)行耦合得到真正的系統(tǒng)狀態(tài)估計(jì)，故IMM-TSEKF在進(jìn)行狀態(tài)的輸入初值交互時(shí)，也按照這種耦合形式進(jìn)行計(jì)算。由此，模型j的狀態(tài)輸入初值交互定義為

(26)

(27)

將式(24)、式(25)代入式(22)、式(23)中，并與式(26)、式(27)對(duì)比，可得模型j的“無(wú)偏差濾波器”的輸入交互初值和相應(yīng)的協(xié)方差陣：

(28)

(29)

然后，進(jìn)行偏差濾波器的輸入初值交互。模型j的偏差濾波器的輸入交互初值和相應(yīng)的協(xié)方差矩陣為

(30)

(31)

(2)TSEKF濾波

(3)模型概率更新

(32)

將式(11)和式(18)代入可得

(33)

將式(33)與式(15)對(duì)比發(fā)現(xiàn)，模型j的新息就是偏差濾波器的新息。

(34)

由此，可得似然函數(shù)為

(35)

模型概率更新為

(36)

其中

(4)組合濾波輸出

(37)

其中

(38)

其中

(39)

(40)

3　算法的時(shí)間復(fù)雜度分析

算法復(fù)雜度是一種衡量算法有效性的概念，可以分為算法的空間復(fù)雜度和算法的時(shí)間復(fù)雜度。在實(shí)際中，最廣泛使用的算法復(fù)雜度概念一般是指算法的時(shí)間復(fù)雜度。本文把執(zhí)行算法過(guò)程中所需要的基本運(yùn)算次數(shù)表示為時(shí)間復(fù)雜度[13]，并且假定所需的基本運(yùn)算次數(shù)為加法和乘法的運(yùn)算次數(shù)之和。

下面對(duì)IMM-TSEKF算法與常規(guī)的IMM-ASEKF算法進(jìn)行時(shí)間復(fù)雜度分析。常規(guī)的IMM-ASEKF算法是將傳感器偏差增廣到系統(tǒng)狀態(tài)中去，然后再應(yīng)用IMM-EKF進(jìn)行估計(jì)。由于這2種方法在計(jì)算模型交互概率，似然函數(shù)以及模型概率時(shí)所需的基本運(yùn)算次數(shù)相同，故在接下來(lái)的時(shí)間復(fù)雜度分析中不再予以考慮。借鑒文獻(xiàn)[13]，且近似地認(rèn)為2個(gè)n維方陣相乘所需的運(yùn)算次數(shù)為2n3，對(duì)算法時(shí)間復(fù)雜度進(jìn)行定量計(jì)算。

首先，計(jì)算交互多模型標(biāo)準(zhǔn)Kalman濾波算法的時(shí)間復(fù)雜度，主要分3個(gè)階段。

(41)

(42)

(43)

OPT(n，m，p，N)=(2n2p+4nmp+np+mp)N

(44)

式(41)～式(44)中的n、m、p、N分別表示狀態(tài)的維數(shù)、觀測(cè)數(shù)據(jù)的維數(shù)、傳感器偏差個(gè)數(shù)及模型個(gè)數(shù)。

由以上分析可得IMM-ASEKF的時(shí)間復(fù)雜度：

(45)

本文算法IMM-TSEKF的時(shí)間復(fù)雜度為

(46)

比較式(45)和式(46)可看出，IMM-TSEKF與IMM-ASEKF相比，節(jié)省的運(yùn)算次數(shù)為

(47)

4　仿真實(shí)驗(yàn)

以多無(wú)源傳感器組網(wǎng)跟蹤系統(tǒng)為例進(jìn)行仿真。無(wú)源傳感器只能得到目標(biāo)的方位角和俯仰角的觀測(cè)值，且存在固定的傳感器系統(tǒng)偏差。無(wú)源傳感器的觀測(cè)方程為

(48)

式中φi、θi分別表示第i個(gè)傳感器的方位角和俯仰角的量測(cè)值；Δφi、Δθi為相應(yīng)的傳感器固定偏差；Si=(xsi，ysi，zsi)為第i個(gè)傳感器所在的位置；ν1i、ν2i為觀測(cè)噪聲。

設(shè)三維空間內(nèi)有3個(gè)無(wú)源傳感器，分別靜止地位于S1=(0 km，0 km，50 km)，S2=(50 km，0 km，0 km)，S3=(0 km，0 km，-50 km)這3處。3個(gè)傳感器的偏差分別為Δφ1=Δθ1=2°， Δφ2=Δθ2=2°，Δφ3=Δθ3=3°。傳感器的觀測(cè)噪聲是零均值的高斯白噪聲，方差為(0.1°)2。3個(gè)無(wú)源傳感器同步工作，采樣周期為T=0.4 s。

假設(shè)跟蹤目標(biāo)為戰(zhàn)術(shù)彈道導(dǎo)彈，TBM在再入過(guò)程中做螺旋機(jī)動(dòng)。本文按照TBM的動(dòng)力學(xué)方程、運(yùn)動(dòng)學(xué)方程以及姿態(tài)運(yùn)動(dòng)方程等產(chǎn)生機(jī)動(dòng)螺旋彈道，詳細(xì)的TBM再入機(jī)動(dòng)螺旋彈道建模見參考文獻(xiàn)[16]。TBM螺旋彈道數(shù)據(jù)是在地面坐標(biāo)系下生成的。TBM螺旋彈道初始參數(shù)設(shè)置如下：

(xT0=0 km，yT0=32 km，zT0=36 km)，VT0=1 km/s，彈體初始自旋角γT=0°，彈體初始自旋角速度ω1xT=0(°)/s，速度偏航角φT0=0°，速度傾角θT0=175°，彈道系數(shù)B=5 000 kg/m2，滾動(dòng)阻尼系數(shù)b=0.008 m2/kg，滾動(dòng)干擾力矩系數(shù)c=2.3×10-6m/kg。

圖2　TBM真實(shí)彈道與估計(jì)彈道的對(duì)比

圖3　各傳感器的偏差估計(jì)

圖4　TBM位置估計(jì)的均方根誤差

(a)傳感器1

(b)傳感器2

(c)傳感器3

利用IMM-TSEKF進(jìn)行50次蒙特卡洛仿真，并將其與IMM-ASEKF進(jìn)行比較，得到仿真結(jié)果如圖2～圖5所示。從圖2可見，利用本文算法進(jìn)行空間配準(zhǔn)以后，系統(tǒng)能夠很好地跟蹤做螺旋機(jī)動(dòng)的TBM。由圖3可見，本文算法可很好地對(duì)傳感器偏差進(jìn)行估計(jì)。圖4、圖5為本文算法與常規(guī)的IMM-ASEKF算法的動(dòng)態(tài)性能比較圖，可見本文算法與IMM-ASEKF算法估計(jì)效果相當(dāng)。

表1給出了采樣時(shí)刻k=240時(shí)的2種算法的性能指標(biāo)。對(duì)比可看出，2種算法的狀態(tài)估計(jì)和偏差估計(jì)的均方根誤差基本相等。另外，由表1的運(yùn)算次數(shù)對(duì)比可看出，當(dāng)進(jìn)行一步遞推濾波時(shí)，本文算法可以節(jié)約32 633次運(yùn)算操作。同時(shí)，利用第四節(jié)的算法時(shí)間復(fù)雜度進(jìn)行分析，當(dāng)n=9，m=6，p=6，N=2時(shí)，由式(47)計(jì)算可得，進(jìn)行一步遞推濾波時(shí)，本文算法可以節(jié)省34 404次運(yùn)算操作，與實(shí)驗(yàn)結(jié)果相近(只有5%的相對(duì)誤差)。

表1　2種算法的性能比較

5　結(jié)論

(1)針對(duì)機(jī)動(dòng)目標(biāo)多傳感器組網(wǎng)跟蹤與空間配準(zhǔn)的問題，提出了IMM-TSEKF算法，該算法將目標(biāo)狀態(tài)與傳感器偏差進(jìn)行解耦估計(jì)，避免了狀態(tài)增廣帶來(lái)的計(jì)算復(fù)雜及數(shù)值計(jì)算精度低的問題。另外還對(duì)IMM-TSEKF算法與IMM-ASEKF算法進(jìn)行了時(shí)間復(fù)雜度的定量分析。

(2)以螺旋機(jī)動(dòng)戰(zhàn)術(shù)彈道導(dǎo)彈為機(jī)動(dòng)目標(biāo)進(jìn)行仿真，結(jié)果表明本文算法能很好地完成空間配準(zhǔn)與機(jī)動(dòng)目標(biāo)跟蹤，且相較于IMM-ASEKF算法，本文算法在估計(jì)精度相當(dāng)?shù)那闆r下，減小了計(jì)算的時(shí)間復(fù)雜度，提高了計(jì)算效率，更適用于工程應(yīng)用。

[1]韓崇昭,朱洪艷,段戰(zhàn)勝.多源信息融合[M].北京: 清華大學(xué)出版社,2006.

[2]王成飛,王航宇,石章松.基于兩級(jí)擴(kuò)展Kalman濾波的海上多平臺(tái)誤差配準(zhǔn)算法[J].系統(tǒng)工程與電子技術(shù),2011,33(4):851-855.

[3]Leung H,Blanchette.M.A least squares fusion of multiple radar data[C]//Proceedings of RADAR,Paris,1994: 364-369.

[4]Zhou Y F,Leung H.An exact maximum likelihood registration algorithm for data fusion[J].IEEE Trans on Signal Processing.1997,45(6): 1560-1572.

[5]NABAA N,BISHOP R H.Solution to a multisensor tracking problem with sensor registration errors [J].IEEE Trans on Aerospace and Electronic Systems,1999,35(1):354-363.

[6]胡洪濤,敬忠良,胡士強(qiáng).一種基于Unscented卡爾曼濾波的多平臺(tái)多傳感器配準(zhǔn)算法[J].上海交通大學(xué)學(xué)報(bào),2005,39(9):1518-1521.

[7]Qi Yong-qing,Jing Zhong-liang,Hu Shi-qiang ,et al.A new method for dynamic bias estimation: Gaussian mean shift registration [J].Optical Engineering,2008,47(2):401-408.

[8]Zia A,Kirubarajan T,et al.An EM algorithm for nonlinear state estimation with model uncertainties [J].IEEE Trans on Signal Processing,2008,56(3):921-936.

[9]Li Zhen-hua,Chen Si-yue,Leung H.Joint data association,registration,and fusion using EM-EKF[J].IEEE Trans on Aerospace and Electronic Systems,2010,46(2): 496-507.

[10]郭軍軍,元向輝,韓崇昭.采用熵函數(shù)法的多傳感器空間配準(zhǔn)算法的研究[J].西安交通大學(xué)學(xué)報(bào),2014,48,(11):128-134.

[11]Friedland B.Treatment of bias in recursive filtering[J].IEEE Trans on Automatic Control,1969,14(4): 359-367.

[12]Hsieh C S,Chen F C.Optimal solution of the two-stage Kalman estimator [J].IEEE Trans on Automatic Control,1999,44(1): 194-199.

[13]Karsaz A,Khaloozadeh H,Darbandi M.Performance comparison of the two-stage Kalman filtering techniques for target tracking[C]//Proceeding of International Symposium on Systems and Control in Aeronautics and Astronautics.2010:942-947.

[14]Kim K H,Lee J G,Park C G.Adaptive two-stage extended Kalman filter for a fault-tolerant INS-GPS loosely coupled system[J].IEEE Trans on Aerospace and Electronic Systems,2009,45(1):125-137.

[15]Foo P H,Ng G W.Combining the interacting multiple model method with particle filters for manoeuvring target tracking[J].IET Radar Sonar Navig.,2011,5(3):234-255.

[16]熊柯,郭振云.反導(dǎo)導(dǎo)彈末制導(dǎo)與控制[D].國(guó)防科技大學(xué),2006.

[17]王偉平,徐毓,王杰.基于改進(jìn)“當(dāng)前”統(tǒng)計(jì)模型的的非線性機(jī)動(dòng)目標(biāo)跟蹤算法[J].控制理論與應(yīng)用,2011,28(12):1723-1728

(編輯：呂耀輝)

Multi-sensor space registration for maneuvering target tracking

FANG Feng，CAI Yuan-li

(School of the Electronic and Information Engineering，Xi’an Jiaotong University，Xi’an710049，China)

For the maneuvering target tracking problem with multi-sensor bias,a two-stage extended Kalman filter based on interacting multiple model(IMM-TSEKF)algorithm was proposed in this paper.The algorithm decoupled the augmented filter into two parallel reduced-order filter-one for state estimation and another for bias estimation.Firstly,the two-stage extended Kalman filter for the case of nonlinear system was employed.Secondly,the IMM-TSEKF algorithm for the purpose of maneuvering target tracking and space registration was adopted.Besides,the time complexity of the algorithm was analyzed.Lastly,the tactical ballistic missile using spiral maneuver was utilized as maneuvering target for space registration and target tracking.Simulation results show that the presented algorithm has higher computation efficiency and similar performance compared with traditional augmented state Kalman filter.

two-stage Kalman filter；interacting multiple model；maneuvering target tracking；space registration

2015-05-06；

2015-08-20。

國(guó)家自然科學(xué)基金(61202128)；宇航動(dòng)力學(xué)國(guó)家重點(diǎn)實(shí)驗(yàn)室開放基金(2011ADL-JD0202)。

方峰(1991—)，男，博士生，研究方向?yàn)樾畔⑷诤稀w行器制導(dǎo)與控制。E-mail：arrowfang @stu.xjtu.edu.cn

蔡遠(yuǎn)利(1963—)，男，博士生導(dǎo)師，研究方向包括復(fù)雜系統(tǒng)建模與仿真、飛行器制導(dǎo)與控制、飛行動(dòng)力學(xué)等。

V443；TN953

A

1006-2793(2016)04-0574-06

10.7673/j.issn.1006-2793.2016.04.023