蔣春蕾

西昌學院汽車與電子工程學院，四川西昌 615013

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

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

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

1.1 地心慣性坐標系

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

1.2 星體坐標系

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

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

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

2 飛行器運動狀態(tài)模型和測量模型

2.1 飛行器運動狀態(tài)模型

(1)

則飛行器運動的狀態(tài)微分方程為：

(2)

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

μ

(3)

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

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

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

(4)

(5)

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

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

(6)

3 跟蹤濾波算法與仿真

3.1 擴展卡爾曼濾波算法

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

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

(7)

當時間間隔tk+1-tk=T足夠短時，F(xiàn)(X(t))可以在tk附近展開為Taylor級數(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附近展開為Taylor級數(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)

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

(16)

(17)

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

(18)

(19)

(20)

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

(21)

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

計算濾波增益矩陣：

(22)

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

(23)

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

(24)

3.2 多步滯后無序量測處理算法

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

圖1 無序測量示意圖

目標飛行器的中段運動模型可視為二體運動模型，其狀態(tài)方程和測量方程可表達式為：

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)

在無序量測Z(d)到達之前，系統(tǒng)狀態(tài)已經(jīng)更新到t(k)狀態(tài)。根據(jù)3.1節(jié)中擴展卡爾曼濾波算法得到最新的預測估計狀態(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)

計算從t(d)時刻到t(k)時刻的預測方程和預測協(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)

下面給出多步無序量測的計算步驟：

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：計算從t(d)到t(k)預測狀態(tài)方程及協(xié)方差：

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

(39)

(40)

假設P(k|d)和P(k|k)不相關可得等式：

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

(41)

但是P(k|d)和P(k|k)根據(jù)相同的預測模型分享共同的歷史數(shù)據(jù)(如P(k-l|k-l))，所以兩者是具有相關性的，為了得到獨立的信息，就得去除無關的信息。

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

(42)

(43)

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

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

(44)

(45)

3.3 算法的仿真分析

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

(1)單步延遲量測

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

圖2 衛(wèi)星的測量時間和到達時間描述圖

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

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

(2)兩步延遲測量

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

(3)多步延遲測量

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

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

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

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

4 結(jié)束語

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

參 考 文 獻

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

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

[3] 李強.單星對衛(wèi)星目標的被動定軌與跟蹤關鍵技術研究[D].長沙:國防科技大學,2007.

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

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

[6] 盛衛(wèi)東，林兩魁，安瑋，周一宇.基于全局最優(yōu)的被動多傳感器多目標軌跡關聯(lián)算法[J]. 電子與信息學報,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.)