含無(wú)序量測(cè)的多傳感器目標(biāo)跟蹤濾波算法

蔣春蕾

西昌學(xué)院汽車(chē)與電子工程學(xué)院，四川西昌 615013

在飛行器的分布式多傳感器融合跟蹤系統(tǒng)中，由于通信鏈路隨機(jī)時(shí)間延遲，以及各傳感器的量測(cè)預(yù)處理時(shí)間不同，多傳感器量測(cè)數(shù)據(jù)通過(guò)多條數(shù)據(jù)鏈路傳輸?shù)街行奶幚砥鲿r(shí)，常會(huì)發(fā)生多傳感器量測(cè)數(shù)據(jù)不按正常時(shí)序到達(dá)融合中心的情況，即出現(xiàn)所謂的無(wú)序量測(cè)現(xiàn)象(Out Of Sequence Measurement, OOSM)。目前針對(duì)此問(wèn)題主要的處理方法分為4類(lèi)[1]：重新濾波法、數(shù)據(jù)緩存法、丟棄滯后量法和直接更新法。其中，重新濾波法、數(shù)據(jù)緩存法的計(jì)算量較大，影響了飛行器跟蹤系統(tǒng)的實(shí)時(shí)性；丟棄滯后量法容易造成大量有用信息的丟失，導(dǎo)致跟蹤系統(tǒng)的精度嚴(yán)重下降；直接更新法的存儲(chǔ)量和計(jì)算量都很小，濾波輸出沒(méi)有滯后，而且具有潛在的高精度濾波性能，是實(shí)時(shí)多傳感器組合跟蹤系統(tǒng)的最佳選擇。本文在直接更新法的基礎(chǔ)上，提出一種新的飛行器跟蹤濾波方法，解決分布式多傳感器目標(biāo)跟蹤濾波中的無(wú)序量測(cè)問(wèn)題，實(shí)現(xiàn)對(duì)飛行目標(biāo)的高精度跟蹤。

1 描述飛行器空間狀態(tài)的常用坐標(biāo)系

用于測(cè)量飛行器狀態(tài)需要用到的坐標(biāo)系主要有：地心慣性坐標(biāo)系、星體坐標(biāo)系等。坐標(biāo)系的3個(gè)基本要素是坐標(biāo)原點(diǎn)、基本平面(x軸和y軸所在平面)及正法向(右手系)，基本平面上的主方向(z軸方向)。

1.1 地心慣性坐標(biāo)系

該坐標(biāo)系的坐標(biāo)原點(diǎn)為地球質(zhì)心Oe，其x軸指向標(biāo)準(zhǔn)歷元2000.0(即2000年1月1日12時(shí))的平春分點(diǎn)，基本平面為該標(biāo)準(zhǔn)歷元時(shí)刻的平赤道面，z軸與x軸垂直，并指向該標(biāo)準(zhǔn)歷元時(shí)刻的平天極，z軸在基本平面內(nèi)與x軸、y軸構(gòu)成右手系。

1.2 星體坐標(biāo)系

在該坐標(biāo)系中，坐標(biāo)原點(diǎn)Os為觀測(cè)衛(wèi)星的質(zhì)心，基本平面為觀測(cè)衛(wèi)星的軌道面，z軸從坐標(biāo)原點(diǎn)Os指向地球質(zhì)心Oe，x軸在基本平面內(nèi)與z軸垂直并指向衛(wèi)星運(yùn)動(dòng)方向，z軸垂直于基本平面并與x軸、y軸構(gòu)成右手系。

1.3 坐標(biāo)系的相互轉(zhuǎn)換

地心坐標(biāo)系與星體坐標(biāo)系的坐標(biāo)轉(zhuǎn)換可分2步進(jìn)行。首先進(jìn)行坐標(biāo)平移，將地心慣性系中的坐標(biāo)平移到以觀測(cè)衛(wèi)星質(zhì)心Os為原點(diǎn)的空間坐標(biāo)系；其次進(jìn)行坐標(biāo)旋轉(zhuǎn)，將以觀測(cè)衛(wèi)星質(zhì)心Os為原點(diǎn)的空間坐標(biāo)系的坐標(biāo)轉(zhuǎn)換到相應(yīng)的星體坐標(biāo)系。具體轉(zhuǎn)換方法詳見(jiàn)文獻(xiàn)[2]。

2 飛行器運(yùn)動(dòng)狀態(tài)模型和測(cè)量模型

2.1 飛行器運(yùn)動(dòng)狀態(tài)模型

(1)

則飛行器運(yùn)動(dòng)的狀態(tài)微分方程為：

(2)

其中F(·)表示狀態(tài)變量X的非線性變換。

μ

(3)

2.2 飛行器姿態(tài)測(cè)量模型

觀測(cè)模型描述了目標(biāo)的三維空間位置到目標(biāo)像平面位置的映射過(guò)程。設(shè)地心坐標(biāo)系下的目標(biāo)位置為r=[xyz]T，衛(wèi)星位置為rs=[xsyszs]T，(·)T表示矩陣轉(zhuǎn)置，則將目標(biāo)位置r映射到像平面位置需要經(jīng)過(guò)一系列坐標(biāo)系轉(zhuǎn)換，依次為地心坐標(biāo)系?軌道坐標(biāo)系?星體坐標(biāo)系?傳感器坐標(biāo)系?像平面坐標(biāo)系。

根據(jù)成像模型的逆過(guò)程計(jì)算出目標(biāo)飛行器所在的方位角βk和俯仰角εk，其定義分別如下：

(4)

(5)

將測(cè)量矢量定義為Z(k)=[βkεk]T，則測(cè)量矢量可以表示為狀態(tài)變量X的非線性函數(shù)：

Z(k)=H(X(k))+n(k)

(6)

3 跟蹤濾波算法與仿真

3.1 擴(kuò)展卡爾曼濾波算法

由于目標(biāo)飛行器中段的狀態(tài)方程是非線性連續(xù)方程，測(cè)量方程是非線性離散方程，這里采用擴(kuò)展卡爾曼濾波(Extend Kalman Filter, EKF)方法[3]來(lái)估計(jì)目標(biāo)飛行器的狀態(tài)，首先需要對(duì)狀態(tài)方程和測(cè)量方程進(jìn)行離散化和線性化，其處理過(guò)程如下：

對(duì)目標(biāo)飛行器的狀態(tài)微分方程(2)式進(jìn)行離散化，可得：

(7)

當(dāng)時(shí)間間隔tk+1-tk=T足夠短時(shí)，F(xiàn)(X(t))可以在tk附近展開(kāi)為T(mén)aylor級(jí)數(shù)：

F(X(t))≈f(X(k))+A(X(k))·

F(X(k))·(t-tk)

(8)

X(k+1)=X(k)+F(X(k))·T+

(9)

由(2)式及矢量微分法則[4]，A(X(k))可以表示如下：

(10)

//(k/k))·T+

(11)

根據(jù)狀態(tài)轉(zhuǎn)移矩陣Φ(t,tk)的定義[5]，可以將其在tk附近展開(kāi)為T(mén)aylor級(jí)數(shù)：

(t-tk) +O(t-tk)

(12)

根據(jù)狀態(tài)轉(zhuǎn)移矩陣的性質(zhì)可得：

Φ(tk,tk)=I

(13)

(14)

將上兩式分別代入(12)式，可表示為：

Φ(t,tk)=I+A(X(k))·(t-tk)+O(Δt)

(15)

同樣地，對(duì)連續(xù)狀態(tài)轉(zhuǎn)移矩陣進(jìn)行離散化后可得：

(16)

(17)

由式(4)和(5)及文獻(xiàn)[2]中的坐標(biāo)轉(zhuǎn)換公式ρ=GT(rT-ρO)，可得：

(18)

(19)

(20)

將狀態(tài)方程和測(cè)量方程線性化、離散化后，即可將其代入如下所示的擴(kuò)展卡爾曼濾波公式進(jìn)行迭代計(jì)算。為便于表示，這里將遞推的時(shí)刻轉(zhuǎn)換為矩陣的下標(biāo)來(lái)表示。

(21)

在這里Q矩陣表示由于非線性狀態(tài)方程線性化時(shí)引入的誤差，一般可通過(guò)經(jīng)驗(yàn)選取為較小的常數(shù)矩陣[6]。

計(jì)算濾波增益矩陣：

(22)

計(jì)算狀態(tài)濾波更新及相應(yīng)的協(xié)方差矩陣：

(23)

Pk+1=(I-Kk+1Hk+1/k)Pk+1/k

(24)

3.2 多步滯后無(wú)序量測(cè)處理算法

觀測(cè)衛(wèi)星系統(tǒng)在下傳角軌跡數(shù)據(jù)時(shí)，可能由于傳輸距離或者預(yù)處理時(shí)間的不一致，導(dǎo)致較早的測(cè)量數(shù)據(jù)反而較晚到達(dá)數(shù)據(jù)融合中心的現(xiàn)象，即所謂的無(wú)序量測(cè)現(xiàn)象，該情況如圖1所示。

圖1 無(wú)序測(cè)量示意圖

目標(biāo)飛行器的中段運(yùn)動(dòng)模型可視為二體運(yùn)動(dòng)模型，其狀態(tài)方程和測(cè)量方程可表達(dá)式為：

X(k+1)=f(X(k),k)+w(k)

(25)

Z(k)=h(X(k),k)+n(k)

(26)

其中噪聲協(xié)方差為：

E[w(k,j)w(k,j)T]=Q(k,j)

E[n(k)n(k)T]=R(k)

(27)

在無(wú)序量測(cè)Z(d)到達(dá)之前，系統(tǒng)狀態(tài)已經(jīng)更新到t(k)狀態(tài)。根據(jù)3.1節(jié)中擴(kuò)展卡爾曼濾波算法得到最新的預(yù)測(cè)估計(jì)狀態(tài)和協(xié)方差：

E[X(k),Zk]

(28)

P(k|k)cov[X(k),Zk]

(29)

Z(d)=h(X(d),d)+n(d)

(30)

計(jì)算從t(d)時(shí)刻到t(k)時(shí)刻的預(yù)測(cè)方程和預(yù)測(cè)協(xié)方差：

E[X(k),Zd]

(31)

P(k|d)cov[X(k)|Zd]

(32)

P(d|k-l)=(Φ(k-l))P(k-l|k-l)

(Φ(k-l))T+Q(d,k-l)

(33)

(34)

(35)

下面給出多步無(wú)序量測(cè)的計(jì)算步驟：

P-1(d|d)=P-1(d|k-l)+

(H(d))TR-1(d)(H(d))

(36)

(37)

(H(d))TR-1(d)Z(d)

(38)

Step 2：計(jì)算從t(d)到t(k)預(yù)測(cè)狀態(tài)方程及協(xié)方差：

P(k|d)=(Φ(d))P(d|d)(Φ(d))T+Q(k,d)

(39)

(40)

假設(shè)P(k|d)和P(k|k)不相關(guān)可得等式：

P-1(k|k,d)=P-1(k|k)+P-1(k|d)

(41)

但是P(k|d)和P(k|k)根據(jù)相同的預(yù)測(cè)模型分享共同的歷史數(shù)據(jù)(如P(k-l|k-l))，所以?xún)烧呤蔷哂邢嚓P(guān)性的，為了得到獨(dú)立的信息，就得去除無(wú)關(guān)的信息。

P-1(k|d)D=P-1(k|d)-P-1(k|k-l)

(42)

(43)

Step 4：計(jì)算狀態(tài)濾波更新及相應(yīng)的協(xié)方差矩陣：

P-1(k|k,d)=P-1(k|k)+P-1(k|d)D

(44)

(45)

3.3 算法的仿真分析

仿真場(chǎng)景設(shè)置：采用2顆衛(wèi)星同時(shí)對(duì)目標(biāo)飛行器進(jìn)行觀測(cè)，采樣時(shí)間間隔為4s，取飛行器在空間飛行的500～600s弧段作為觀測(cè)時(shí)間段，視線誤差為100μrad。仿真分單步延遲量測(cè)、兩步延遲量測(cè)和多步延遲量測(cè)3種情況進(jìn)行討論。

(1)單步延遲量測(cè)

衛(wèi)星1的量測(cè)數(shù)據(jù)按正常時(shí)序到達(dá)中心處理器，衛(wèi)星2的量測(cè)數(shù)據(jù)固定作單步延遲，圖2為衛(wèi)星測(cè)量時(shí)間與到達(dá)時(shí)間的時(shí)序關(guān)系描述。

圖2 衛(wèi)星的測(cè)量時(shí)間和到達(dá)時(shí)間描述圖

為了簡(jiǎn)化描述，將擴(kuò)展卡爾曼濾波算法簡(jiǎn)稱(chēng)為EKF，基于擴(kuò)展卡爾曼的前向預(yù)測(cè)多步滯后無(wú)序量測(cè)處理算法稱(chēng)為EKF-OOSM，CRLB為克拉美羅下限。將本文的EKF-OOSM算法與傳統(tǒng)的丟棄算法、EKF算法進(jìn)行對(duì)比，各算法的仿真結(jié)果如圖3所示。

圖3 單步延遲情況的位置速度均方根誤差

(2)兩步延遲測(cè)量

將衛(wèi)星2的測(cè)量數(shù)據(jù)作兩步固定延遲，其它參數(shù)設(shè)置與單步延遲相同，各算法的仿真結(jié)果如圖4所示。

(3)多步延遲測(cè)量

將衛(wèi)星2的測(cè)量數(shù)據(jù)作多步固定延遲，其它參數(shù)設(shè)置與單步延遲相同，各算法的仿真結(jié)果如圖5所示。

圖4 兩步延遲情況的位置速度均方根誤差

圖5 多步延遲情況的位置速度均方根誤差

從圖3、圖4和圖5的仿真結(jié)果可以看出，丟棄滯后量數(shù)據(jù)的濾波算法(即丟棄算法)的濾波效果很差，測(cè)量目標(biāo)飛行器時(shí)得到的位置誤差和速度誤差與克拉美羅下限CRLB相差太大，特別是位置誤差還有可能導(dǎo)致濾波結(jié)果不收斂。擴(kuò)展卡爾曼濾波算法EKF在整個(gè)觀測(cè)時(shí)間內(nèi)，測(cè)得的位置誤差和速度誤差有抖動(dòng)，濾波效果不平滑，有起伏現(xiàn)象，測(cè)量精度不高，其測(cè)量的位置誤差、速度誤差較大。與丟棄算法、擴(kuò)展卡爾曼濾波算法EKF相比，采用EKF-OOSM濾波算法得到的目標(biāo)飛行器的位置誤差、速度誤差較小，接近理想狀態(tài)的克拉美羅下限，而且在整個(gè)觀測(cè)時(shí)間內(nèi)，濾波結(jié)果較為平滑，測(cè)量誤差收斂性較好，能夠滿足對(duì)目標(biāo)飛行器數(shù)據(jù)的精確處理要求。

4 結(jié)束語(yǔ)

本文提出了一種針對(duì)無(wú)序量測(cè)數(shù)據(jù)的多傳感器目標(biāo)跟蹤處理算法。首先針對(duì)多傳感器系統(tǒng)對(duì)目標(biāo)飛行器的跟蹤測(cè)量問(wèn)題，介紹了地心慣性坐標(biāo)系和星體坐標(biāo)系的定義及轉(zhuǎn)換公式，在此基礎(chǔ)上給出目標(biāo)飛行器的運(yùn)動(dòng)狀態(tài)方程和測(cè)量方程，然后結(jié)合擴(kuò)展卡爾曼濾波算法，推導(dǎo)了基于擴(kuò)展卡爾曼濾波的前向預(yù)測(cè)多步滯后無(wú)序量測(cè)處理算法，最后分別對(duì)不同時(shí)間步長(zhǎng)滯后的情況下目標(biāo)飛行器跟蹤測(cè)量問(wèn)題進(jìn)行了仿真分析，仿真結(jié)果表明采用該算法處理目標(biāo)飛行器的位置和飛行速度，得到的誤差較小，在整個(gè)觀測(cè)時(shí)間內(nèi)，測(cè)量誤差的收斂性較好，可以實(shí)現(xiàn)對(duì)目標(biāo)飛行器的精確測(cè)量和跟蹤。

參 考 文 獻(xiàn)

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

[2] 肖業(yè)倫.航天器飛行動(dòng)力學(xué)原理[M].北京:宇航出版社,2005:203-205.

[3] 李強(qiáng).單星對(duì)衛(wèi)星目標(biāo)的被動(dòng)定軌與跟蹤關(guān)鍵技術(shù)研究[D].長(zhǎng)沙:國(guó)防科技大學(xué),2007.

[4] 葉其孝,沈永歡.實(shí)用數(shù)學(xué)手冊(cè)(第2版)[M].北京:科學(xué)出版社,2006.

[5] Steven M. K. Fundamentals of Statistical Signal Processing Volume I: Estimation Theory [M].Pearson Education, Inc, 1993.

[6] 盛衛(wèi)東，林兩魁，安瑋，周一宇.基于全局最優(yōu)的被動(dòng)多傳感器多目標(biāo)軌跡關(guān)聯(lián)算法[J]. 電子與信息學(xué)報(bào),2010,37(7):1621-1625.(Sheng Wei-dong,Lin Liang-kui,An Wei,Zhou Yi-yu.A Passive Multisensor Multitarget Track Association Algorithm Based on Global Optimization[J].Journal of Electronics and Information Technology,2010,37(7):1621-1625.)