基于樣條濾波的航天器相對(duì)運(yùn)動(dòng)檢測(cè)方法*

孫福煜，王華

(國防科技大學(xué) 航天科學(xué)與工程學(xué)院，湖南 長沙 410073)

基于樣條濾波的航天器相對(duì)運(yùn)動(dòng)檢測(cè)方法*

孫福煜，王華

(國防科技大學(xué) 航天科學(xué)與工程學(xué)院，湖南 長沙 410073)

近距離相對(duì)運(yùn)動(dòng)檢測(cè)在交會(huì)對(duì)接、航天器隨動(dòng)抓捕等領(lǐng)域有著重要的作用?；谌隣顟B(tài)樣條濾波算法，對(duì)接近航天器的運(yùn)動(dòng)狀態(tài)進(jìn)行實(shí)時(shí)預(yù)報(bào)并對(duì)其機(jī)動(dòng)進(jìn)行檢測(cè)。首先構(gòu)建兩航天器的相對(duì)動(dòng)力學(xué)模型并將其應(yīng)用于三狀態(tài)樣條濾波算法中，通過檢測(cè)濾波新息來對(duì)目標(biāo)機(jī)動(dòng)發(fā)生或消除進(jìn)行判斷，最后利用一個(gè)雙脈沖交會(huì)的實(shí)例進(jìn)行了仿真分析。仿真結(jié)果表明，該方法具有較高的精度和檢測(cè)效率。

樣條濾波；交會(huì)對(duì)接；機(jī)動(dòng)檢測(cè)；相對(duì)自主導(dǎo)航

0 引言

機(jī)動(dòng)目標(biāo)檢測(cè)的應(yīng)用領(lǐng)域非常廣泛，在國防和民用方面都有重要的價(jià)值。它與現(xiàn)代導(dǎo)航技術(shù)相結(jié)合，可以實(shí)現(xiàn)對(duì)狹窄航道和港口內(nèi)船只的預(yù)警和調(diào)度，也可以實(shí)現(xiàn)對(duì)空間運(yùn)動(dòng)目標(biāo)的跟蹤與攔截。它還常用于彈道導(dǎo)彈防御、精確制導(dǎo)和低空突防等[1]。

在追蹤航天器的逼近過程中，目標(biāo)航天器上安裝的相對(duì)測(cè)量敏感器通常可獲得兩航天器間的視線距離、俯仰角、方位角和視線速率等測(cè)量參數(shù)中的全部或部分，且這些測(cè)量參數(shù)均帶有一定的測(cè)量偏差[2]。而軌道規(guī)避控制需要的是兩航天器的相對(duì)位置和相對(duì)速度，所以相對(duì)導(dǎo)航系統(tǒng)需要基于上述測(cè)量參數(shù)對(duì)兩航天器間的相對(duì)位置和相對(duì)速度進(jìn)行估計(jì)。文獻(xiàn)[3-4]提出了一種模糊Kalman濾波新算法，利用航向角變化量和觀測(cè)殘差作為檢測(cè)目標(biāo)機(jī)動(dòng)的參數(shù)；文獻(xiàn)[5-7]針對(duì)濾波增益調(diào)整隨意性問題，提出了一種修正的基于機(jī)動(dòng)檢測(cè)的機(jī)動(dòng)目標(biāo)跟蹤算法；文獻(xiàn)[8-10]提出一種改進(jìn)的方差自適應(yīng)機(jī)動(dòng)目標(biāo)跟蹤算法，利用新息平方的統(tǒng)計(jì)量和當(dāng)前加速度均值，對(duì)目標(biāo)當(dāng)前運(yùn)動(dòng)狀態(tài)進(jìn)行估計(jì)。

本文以上述文獻(xiàn)為基礎(chǔ)，基于三狀態(tài)樣條濾波，采用噪聲協(xié)方差自適應(yīng)方法對(duì)測(cè)量數(shù)據(jù)進(jìn)行濾波，實(shí)現(xiàn)目標(biāo)航天器對(duì)追蹤航天器狀態(tài)的估計(jì)與機(jī)動(dòng)檢測(cè)，最后進(jìn)行了仿真驗(yàn)證。仿真結(jié)果表明，該方法具有較高的精度和檢測(cè)效率。

1 相對(duì)動(dòng)力學(xué)模型

1.1 坐標(biāo)系定義

航天器軌道坐標(biāo)系

航天器軌道坐標(biāo)系Oxyz，其原點(diǎn)位于航天器質(zhì)心，Oz軸由航天器質(zhì)心指向地心，Oy軸與航天器軌道的角動(dòng)量方向相反，Ox軸使得該坐標(biāo)系成為右手直角坐標(biāo)系，如圖1所示。

圖1 目標(biāo)航天器軌道坐標(biāo)系Fig.1 Target orbit coordinate system

特別的，當(dāng)航天器軌道偏心率很小，可作近圓軌道假設(shè)時(shí)，該坐標(biāo)系的Ox軸方向與航天器的速度方向是一致的。該坐標(biāo)系常用于近程段相對(duì)運(yùn)動(dòng)的研究，此時(shí)Ox軸稱為V-bar，Oy軸稱為H-bar，Oz軸稱為R-bar。

1.2 相對(duì)運(yùn)動(dòng)方程

在追蹤航天器與目標(biāo)航天器相對(duì)距離較近時(shí)，一般用追蹤航天器、目標(biāo)航天器相對(duì)動(dòng)力學(xué)方程來描述2個(gè)航天器的相對(duì)狀態(tài)。本文采用C-W方程[11]作為相對(duì)動(dòng)力學(xué)方程，即

(1)

對(duì)于圓軌道或近圓軌道，當(dāng)相對(duì)距離較小時(shí)，使用該方程計(jì)算的精度可以滿足動(dòng)力學(xué)計(jì)算的要求。

將式(1)可以寫成狀態(tài)方程的形式：

(2)

由狀態(tài)方程式(2)可以得到狀態(tài)轉(zhuǎn)移矩陣：

(3)

式中：τ=ω(t-t0)。

若將狀態(tài)轉(zhuǎn)移矩陣分成4個(gè)3×3的方陣，即

(4)

令

(5)

并令初始條件X(t0)=X0，則可以求解任意時(shí)刻的狀態(tài)變量：

(6)

對(duì)于n次脈沖機(jī)動(dòng)，假設(shè)在ti時(shí)刻施加了脈沖Δvi，i=1,2,…,n，則式(6)可以表示為

(7)

2 基于樣條濾波機(jī)動(dòng)檢測(cè)方法

三狀態(tài)樣條濾波是兩狀態(tài)樣條濾波方法的擴(kuò)展，即將二維狀態(tài)變量(位置和速度)中增加加速度，對(duì)狀態(tài)變量的位置、速度和加速度進(jìn)行估計(jì)。

2.1 狀態(tài)方程

在每個(gè)觀測(cè)間隔T內(nèi)，假設(shè)位置參數(shù)的三階磨光函數(shù)(加速度的變化率)為常數(shù)S(k)，并計(jì)入一個(gè)隨機(jī)加速度變化率εk，為正態(tài)白噪聲序列。為了直接估計(jì)機(jī)動(dòng)加速度，有狀態(tài)方程

(8)

記狀態(tài)方程為

Xk+1=ΦXk+ΒSk+Wk，

(9)

則

Wk=Βεk。

2.2 觀測(cè)方程

假設(shè)狀態(tài)方程和觀測(cè)方程在目標(biāo)航天器軌道坐標(biāo)系下建立，則每個(gè)時(shí)刻目標(biāo)的狀態(tài)為軌道坐標(biāo)系下的相對(duì)位置和速度矢量，觀測(cè)方程即為相對(duì)測(cè)量設(shè)備的觀測(cè)量(r，A，E)與目標(biāo)狀態(tài)(p，v，a)的關(guān)系。

相對(duì)測(cè)量設(shè)備探測(cè)數(shù)據(jù)(r，A，E)與航天器軌道系下的相對(duì)位置矢量(xr，yr，zr)T有如下轉(zhuǎn)換關(guān)系：

(10)

2.3 三狀態(tài)樣條濾波算法

(1) 選取初值

(2) 狀態(tài)預(yù)測(cè)

(11)

(12)

(3) 觀測(cè)值預(yù)測(cè)

(13)

(4) 觀測(cè)矩陣計(jì)算

(14)

(5) 計(jì)算增益矩陣

(15)

當(dāng)檢測(cè)到目標(biāo)機(jī)動(dòng)時(shí)，可以適當(dāng)增大增益矩陣的模值。R為觀測(cè)噪聲協(xié)方差矩陣，為對(duì)角陣，對(duì)角線元素為噪聲方差。

(6) 狀態(tài)估計(jì)

(16)

(17)

2.4 機(jī)動(dòng)檢測(cè)方法

目標(biāo)機(jī)動(dòng)的發(fā)生使原來模型與實(shí)際的目標(biāo)動(dòng)態(tài)特性不一致，從而導(dǎo)致跟蹤誤差增大，新息過程發(fā)生急劇變化。通過檢測(cè)濾波新息可對(duì)目標(biāo)的機(jī)動(dòng)作出判斷，這是樣條濾波機(jī)動(dòng)檢測(cè)決策的基本思想。

(18)

式中：n為觀測(cè)維數(shù)，代表觀測(cè)量的個(gè)數(shù)，對(duì)于位置x，y，z觀測(cè)，n=3。

檢測(cè)到機(jī)動(dòng)以后，通過調(diào)節(jié)Qk和Rk，重新分配狀態(tài)估計(jì)中預(yù)測(cè)部分和觀測(cè)補(bǔ)償部分的權(quán)重。

3 仿真結(jié)果

式(8)～(18)闡述了基于三狀態(tài)樣條濾波的航天器機(jī)動(dòng)檢測(cè)方法。本節(jié)通過一個(gè)近距離雙脈沖接近的仿真算例來驗(yàn)證上述方法的合理性和準(zhǔn)確性。

3.1 算例配置

表1 仿真試驗(yàn)參數(shù)設(shè)置Table 1 Simulation configuration parameters

3.2 仿真結(jié)果及分析

基于以上算例，采用三狀態(tài)樣條濾波算法對(duì)機(jī)動(dòng)目標(biāo)的狀態(tài)進(jìn)行濾波估計(jì)，得到如下圖2，3所示結(jié)果。

圖2 相對(duì)位置變化曲線Fig.2 Change in relative position

圖3 濾波檢測(cè)結(jié)果Fig.3 Result of filter

由圖2可以看出，經(jīng)過2次脈沖變軌后，追蹤器與目標(biāo)器的相對(duì)距離達(dá)到了預(yù)定的5 000 m左右。

由圖3可以看出，當(dāng)機(jī)動(dòng)脈沖為正值，速度估值與真值之差曲線第1波峰為負(fù)，且幅度與脈沖值呈一定正比關(guān)系。即正值脈沖施加瞬間，速度估值與真值之差為負(fù)，脈沖值越大，速度估值與真值之差越小。原因是濾波所計(jì)算出的估計(jì)值中，一部分是依靠預(yù)測(cè)值，當(dāng)脈沖為正，真實(shí)速度大于預(yù)估值，所以濾波所得到的估計(jì)值要小于真值。當(dāng)脈沖為負(fù)值，現(xiàn)象和原因與上述相反。

位置估值與真值之差的正負(fù)和機(jī)動(dòng)脈沖正負(fù)關(guān)系，與速度的現(xiàn)象一樣。不同之處是位置估值與真值之差在首脈沖施加后，變化的幅值要大于第2次脈沖施加后變化的幅值。原因是在測(cè)量角度(仰角和方位角)誤差一定的情況下，距離越大，位置誤差就越大。并且測(cè)量距離誤差也是隨著位置的變化而變化，位置越遠(yuǎn)，測(cè)量距離誤差就越大，這一點(diǎn)增強(qiáng)了上述現(xiàn)象的發(fā)生趨勢(shì)。

將濾波估計(jì)值重新反算為測(cè)量數(shù)據(jù)，并與標(biāo)準(zhǔn)測(cè)量數(shù)據(jù)相減，得到濾波誤差，與施加的量測(cè)噪聲相比，可以用來比較濾波算法的性能。結(jié)果如圖4～6所示。

由圖4～6可以看出：

(1) 測(cè)量角度誤差在第2次脈沖施加后，變化的幅值要遠(yuǎn)大于第1次脈沖施加后變化的幅值。原因是位置誤差一定，距離越小，測(cè)量角度誤差越大。這也與前面所述的位置誤差與測(cè)量角度誤差變化相一致。

(2)相對(duì)距離在2 397 s時(shí)刻達(dá)到最小值，約為4 824.421 m，此時(shí)距離誤差中量測(cè)噪聲和濾波誤差均達(dá)到最小，分別為1 m和0.3 m。

圖4 仰角濾波誤差與量測(cè)噪聲比較曲線Fig.4 Comparison of filter errors with measurement noise in pitching

圖5 方位角濾波誤差與量測(cè)噪聲比較曲線Fig.5 Comparison of filter errors with measurement noise in azimuth

圖6 距離濾波誤差與量測(cè)噪聲比較曲線Fig.6 Comparison of filter error with measurement noise in range

(3)目標(biāo)每次開始機(jī)動(dòng)時(shí)，估計(jì)值存在一個(gè)巨大的誤差(第1個(gè)波峰幅值很大)。對(duì)于樣條濾波方法來說，誤差第一波峰是不可避免的，因?yàn)闃訔l濾波的當(dāng)前時(shí)刻的加速度變化率是由上個(gè)時(shí)刻的數(shù)據(jù)預(yù)估的。也就是說，即便觀測(cè)數(shù)據(jù)是完全精確的，機(jī)動(dòng)檢測(cè)也會(huì)有一個(gè)時(shí)刻的延遲。如果當(dāng)前時(shí)刻實(shí)際變化與上個(gè)時(shí)刻相反，則預(yù)測(cè)與真實(shí)情況就完全相反，再加上由于觀測(cè)誤差引起的機(jī)動(dòng)檢測(cè)延遲，就出現(xiàn)了誤差第1波峰。通過增加系統(tǒng)噪聲進(jìn)行補(bǔ)償，可以有效減小第1波峰，但是在保證精度情況下調(diào)節(jié)能力有限。

(4)如果目標(biāo)沒有發(fā)生機(jī)動(dòng)，僅從預(yù)測(cè)的角度來說，觀測(cè)十幾個(gè)點(diǎn)后就會(huì)收斂，收斂時(shí)間約為7 s。如果目標(biāo)發(fā)生機(jī)動(dòng)，就會(huì)出現(xiàn)誤差第1波峰的現(xiàn)象，收斂時(shí)間就會(huì)相應(yīng)的延長，需要觀測(cè)幾十個(gè)點(diǎn)的時(shí)間，約為200 s。

4 結(jié)束語

本文針對(duì)交會(huì)對(duì)接逼近段追蹤器對(duì)目標(biāo)器的接近過程，首先構(gòu)建了兩個(gè)航天器的相對(duì)動(dòng)力學(xué)模型，然后采用三狀態(tài)樣條濾波算法對(duì)兩航天器間的相對(duì)位置、速度和加速度進(jìn)行實(shí)時(shí)估計(jì)，通過檢測(cè)濾波新息來對(duì)追蹤器機(jī)動(dòng)的發(fā)生或消除進(jìn)行判斷，最后通過仿真驗(yàn)證了本文所提出方法的有效性。今后的工作可以圍繞系統(tǒng)噪聲自適應(yīng)方面來開展。目標(biāo)發(fā)生機(jī)動(dòng)前，保持系統(tǒng)噪聲為一個(gè)小量，保證估計(jì)精度；檢測(cè)到目標(biāo)發(fā)生機(jī)動(dòng)后，適當(dāng)增加系統(tǒng)噪聲，可以有效減小第1波峰對(duì)系統(tǒng)帶來的不利影響。

[1] 沈瑩.機(jī)動(dòng)目標(biāo)跟蹤算法與應(yīng)用研究[D].西安：西北工業(yè)大學(xué)，2007. SHEN Ying. Tracking Algorithm and Application Research for Maneuvering Target[D]. Xi’an:Northwestern Polytechnical University, 2007.

[2] 劉也，余安喜，朱炬波，等.彈道目標(biāo)實(shí)時(shí)跟蹤中的濾波方法綜述[J].宇航學(xué)報(bào)，2013，34(11)：1417-1426. LIU Ye, YU An-xi, ZHU Ju-bo. Survey of Filter Algorithms for Ballistic Target Real-Time Tracking[J]. Journal of Astronautics, 2013, 34(11): 1417-1426.

[3] 李鵬飛，黃建軍，黃敬雄，等.一種模糊Kalman濾波機(jī)動(dòng)目標(biāo)跟蹤的新算法[J].數(shù)據(jù)采集與處理，2009，24(4)：483-486. LI Peng-fei, HUANG Jian-jun, HUANG Jing-xiong, et al. Fuzzy Kalman Filter for maneuvering Target Tracking[J]. Journal of Data Acquisition&Processing, 2009,24(4):483-486.

[4] 王宏強(qiáng)，黎湘，劉丹，等.非線性系統(tǒng)中的機(jī)動(dòng)目標(biāo)跟蹤算法[J].國防科技大學(xué)學(xué)報(bào)，2002，24(4)：57-60. WANG Hong-qiang, LI Xiang, LIU Dan, et al. Maneuvering Target Tracking in the Nonlinear System[J]. Journal of National University of Defense Technology, 2002, 24(4): 57-60.

[5] 關(guān)成斌，王國宏，王曉博.一種新的基于機(jī)動(dòng)檢測(cè)的機(jī)動(dòng)目標(biāo)跟蹤算法[J].海軍航空工程學(xué)院學(xué)報(bào)，2007，22(1)：148-150. GUAN Cheng-bin, WANG Guo-hong, WANG Xiao-bo. A New Algorithm on Tracking Maneuvering Target Based on Maneuver Detection[J]. Journal of Naval Aeronautical Engineering Institute, 2007, 22(1):148-150.

[6] 李理敏，龔文斌，劉會(huì)杰，等.基于自適應(yīng)擴(kuò)展卡爾曼濾波的載波跟蹤算法[J].航空學(xué)報(bào)，2012，33(7)：1319-1328. LI Li-min, GONG Wen-bin, Liu Hui-jie, et al. A Carrier Tracking Algorithm Based on Adaptive Extended Kalman Filter[J]. Acta Aeronautica et Astronautica Sinica, 2012, 33(7): 1319-1328.

[7] 于洋，胡曉芳，劉硯菊.機(jī)動(dòng)目標(biāo)跟蹤的改進(jìn)自適應(yīng)濾波算法研究[J].沈陽理工大學(xué)學(xué)報(bào)，2013，32(1)：56-59. YU yang, HU Xiao-fang, LIU Yan-ju.An Improved Adaptive Filtering Algorithm Research of Maneuvering Target Tracking[J]. Journal of Shenyang Ligong University, 2013, 32(1): 56-59.

[8] 孟慶虎，劉寶光.方差自適應(yīng)機(jī)動(dòng)目標(biāo)跟蹤算法研究[J].計(jì)算機(jī)仿真，2012，29(9)：10-13. MENG Qing-hu, LIU Bao-guang.Research on Improved Adaptive Tracking Algorithm for Maneuvering Targets[J]. Computer Simulation, 2012, 29(9):10-13.

[9] 唐政，郝明，周鵬，等.基于自適應(yīng)卡爾曼濾波的機(jī)動(dòng)目標(biāo)跟蹤算法[J].電子科技，2013，26(4)：78-81. TANG Zheng, HAO Ming, ZHOU Peng, et al.Tracking Algorithm for Maneuvering Target Based on Adaptive Kalman Filter[J].Electronic Science & Technology, 2013,26(4):78-81.

[10] 周航，馮新喜，王蓉，等.基于自適應(yīng)擴(kuò)展H∞濾波的機(jī)動(dòng)目標(biāo)被動(dòng)跟蹤[J].系統(tǒng)工程與電子技術(shù)，2013，35(2)：256-262. ZHOU Hang, FENG Xin-xi, WANG Rong, et al. Maneuvering Target Passive Tracking Based on Adaptive ExtendedH∞Filter[J]. Systems Engineering and Electronics, 2013, 35(2):256-262.

[11] 郗曉寧，王威.近地航天器軌道基礎(chǔ)[M].長沙：國防科技大學(xué)出版社，2003:242-250. XI Xiao-ning, WANG Wei. Fundamentals of Near-Earth Spacecraft Orbit[M]. Changsha: National University of Defense Technology Press, 2003: 242-250.

[12] 汪雄良，周海銀，朱炬波.三狀態(tài)樣條濾波與平滑[J].數(shù)學(xué)理論與應(yīng)用，2002，22(1)：109-112. WANG Xiong-liang, ZHOU Hai-yin, ZHU Ju-bo. Three-States Pline Filter and Smooth Method[J]. Mathematical Theoryand Applications, 2002, 22(1): 109-112.

Relative Motion Detection Method of Spacecraft Based on Spline-Filter

SUN Fu-yu， WANG Hua

(National University of Defense Technology，College of Aerospace Science and Engineering，Hunan Changsha 410073，China)

The relative motion detection plays an important role in close rendezvous and docking, spacecraft follow-arresting and other fields. Based on three-state spline filter, the real-time forecasting of the chaser motion and detecting its maneuver is proposed. Firstly, a relative dynamic model of two spacecrafts is built and is applied to three-state spline filter algorithm, then the occurrence or elimination of chaser maneuver is judged by filter innovation. Finally, an example of double pulse rendezvous is presented and the filtering results are compared with the real measured values. The results show that this method has higher accuracy and detection efficiency.

spline filter；rendezvousand docking；maneuver detection；relative auto-navigation

2013-12-26；

2014-06-13

孫福煜(1989-)，男，遼寧沈陽人。碩士生，主要從事軌道動(dòng)力學(xué)與控制、導(dǎo)彈攻防系統(tǒng)仿真等方面的研究。

通信地址：410073 長沙市開福區(qū)德雅路109號(hào)國防科大航天科學(xué)與工程學(xué)院 E-mail：sunfuyu89@126.com

10.3969/j.issn.1009-086x.2015.03.013

TN713；V448.2

A

1009-086X(2015)-03-0070-07