一種火箭及上面級外彈道實時濾波算法

，

(1.宇航動力學國家重點實驗室， 陜西西安 710043；2.西安衛星測控中心， 陜西西安 710043)

0 引言

運載火箭上面級[1-2]具有自主導航能力強、推力大、較基礎級工作時間長、可多次起動等特點，上面級的使用是提高火箭性能及任務適應能力的有效解決途徑。已經在國外的航天發射任務中得到了廣泛應用，在我國也受到了越來越多的重視，如某顆新一代北斗導航星的發射即采用了上面級入軌技術。

運載火箭上面級及基礎級實時飛行彈道的計算是其飛行過程監視的重要內容，其本質上是一種非線性狀態估計[3]問題。這方面應用較多的有擴展卡爾曼濾波[4](Extended Kalman Filter, EKF)、不敏卡爾曼濾波[4-5](Unscented Kalman Filter, UKF)、容積卡爾曼濾波[6](Cubature Kalman Filter, CKF)等。EKF產生較早，它通過非線性模型離散化處理來建立線性化的濾波模型，容易產生高階截斷誤差，且常需要計算非線性函數的Jacob矩陣；UKF與CKF使用確定性采樣策略來近似函數非線性分布，但UKF在用于高維狀態向量估計時，存在計算效率較低、采樣點設置參數復雜、數值精度以及穩定性較差的問題。由Ito和Norgaard等人提出的中心差分卡爾曼濾波(Central Difference Kalman Filter, CDKF)算法具有比UKF稍高的理論精度，而且更易于實現，充分考慮了隨機變量的噪聲統計特性。

為此，本文將中心差分容積卡爾曼濾波引入火箭上面級飛行彈道實時跟蹤計算過程中，給出了一種適用于工程實際情況的實時狀態估計算法，并通過位置速度與加速度關系對模型方差Q進行自適應調整，以實現濾波對彈道機動過程的自適應跟蹤。仿真及實測數據計算結果表明，所給出的計算方法是可行的。

1 中心差分卡爾曼濾波

1.1 中心差分變換算法

中心差分變換是CDKF的基礎，其核心思想是通過隨機變量的非線性變換求解均值和方差等，具體就是用Sterling差值公式代替泰勒級數展開式中一階、二階導數，用多項式逼近非線性方程導數，從而避免了EKF濾波中雅可比矩陣的求導運算[3]，具有實現過程較為簡單的特點。

(1)

采用中心差分計算來代替式(1)中一階、二階導數項，可得到非線性方程的近似表達式：

(2)

(3)

(4)

(5)

(6)

式中：ei為第i個元素為1，其余元素為0的單位向量；Sxi為Sx的第i個列向量。由式(4)可知：

式中，Sxi為狀態協方差矩陣Cholesky分解后的第i列，則可得均值：

(7)

方差為

(8)

協方差為

(9)

1.2 CDKF濾波計算

CDKF實質上是利用基于先驗分布構造的Simga點進行線性回歸變換來表示狀態后驗分布的一種采樣濾波方法，其基本原理與UKF類似。

設非線性系統為

(10)

式中：xk為k時刻系統狀態向量，其維數為L； F為狀態向量的轉移函數；wk-1為狀態向量的高斯過程噪聲，其均值為零，協方差矩陣為Qk-1；yk為k時刻的m維觀測向量； H為觀測函數；vk為高斯量測噪聲，協方差矩陣為Rk。則該非線性估計的CDKF濾波算法主要有以下步驟：

1) 構造時間更新所需采樣點

(11)

其變換權系數為

Wx,0=(h2-L)/h2

Wx,i=1/(2h2),i=1,…,2L

(12)

2) 進行時間更新

(13)

3) 構造量測更新所需的采樣點集

(14)

4) 量測更新計算

(15)

量測與狀態的互協方差陣：

(16)

5) 計算殘差

6) 進行狀態更新

考慮到在上面級機動飛行中，濾波計算過程有一定的舍入誤差，以及初值誤差可能較大，從而可能導致濾波出現發散現象。為此，在中心差分濾波實現中，在上面的誤差協方差陣遞推計算中常采用平方根代替協方差矩陣進行遞推計算，從而可得到平方根中心差分卡爾曼濾波(SRCDKF)。

2 火箭及上面級彈道濾波模型

2.1 觀測方程及系統狀態方程

rE=(HG)r

HG=(EP)(ER)(NR)(PR)

HDG=(EP)(EDR)(NR)(PR)

(17)

式中，PR為歲差矩陣，NR為章動矩陣，ER為地球自轉矩陣，EP為極移矩陣，EDR為地球自轉矩陣的導數矩陣。

2) 將地固系位置速度轉到測站東北天坐標系下，由測站大地坐標可計算出地固系到測站東北天坐標系的轉換矩陣MES，則有

ρS=MES(rE-RS)

(18)

這樣，即可計算出外彈道測量的4個觀測量：

A=arctan(ρx/ρy)

(19)

濾波中對外彈道測量數據的使用，即觀測方程的建立過程，主要有兩種方法，一種是直接使用測距與測角轉換出的位置數據；另一種是直接將觀測方程建立在測站東北天坐標系中[7-8]。

考慮到上面級飛行高度較高，較小的測角誤差就會產生較大的位置偏差；并且各測元之間測量精度差異較大。為此本文觀測方程的建立采用后一種計算方法。

2.2 軌道機動過程彈道計算

(20)

對k時刻到k+1時刻的系統狀態外推預測可通過對上述動力學模型進行Runge-Kutta積分得到。

2.3 機動過程Q矩陣自適應處理

火箭及上面級飛行過程存在多次機動運動，且各次機動的加速度存在較大差異。這給彈道重建帶來了較大困難。對αβ模型，若將Q矩陣設置為恒定值，則機動發生時，若未能根據當前推力情況對af進行精確建模，并使外推模型及時切換到有推力作用的狀態，則濾波狀態更新將與實際情況出現較大偏差。從而導致濾波計算需要很長時間進行狀態調整，極大降低收斂速度。為此，需要在機動發生時及時調整濾波狀態參數。一種方法是實時接收火箭遙測判定的機動特征點，在特征點處對濾波P矩陣及模型中加速度按當前推力、比沖、火箭飛行姿態等參數計算初始化值。但這種方法應用于多次機動時顯得比較麻煩，特別是對姿態數據的引入。

考慮到機動發生時，火箭及上面級飛行的加速度變化趨勢會發生突變。此時，若濾波狀態外推未能及時反映出此變化，則原狀態外推模型中的狀態協方差矩陣Q將與當前系統真實狀態出現較大偏差，為此可通過對Q矩陣進行實時調整的方法來使得濾波計算實現對機動過程的自適應處理。

(21)

(22)

考慮到機動發生時，濾波狀態變量X中加速度迅速改變，而加速度的改變會影響速度與位置的改變。為此，可參照式(22)來調整Q矩陣中加速度分量。對αβ模型，其加速度包括機動推力加速度、三體引力加速度、大氣阻力加速度等，考慮到軌道機動過程中，推力加速度占主要地位，其他加速度為小量。為此可直接使用式(22)進行加速度增量的近似計算。

設k-1時刻到k時刻的時間增量為T，并在計算公式中引入調整系數λ，合理設置λ以增強自適應能力。從而可將Q矩陣中加速度x,y,z方向分量的方差分別自適應調整為位置與速度的函數：

(23)

式中，i=0,1,2表示x,y,z各方向分量。

實際計算中，若只對Q矩陣加速度方向進行調整，將使得震蕩加劇。為此，還需對速度方向進行限制性調整。計算公式為

(24)

對位置方向，可將其方差置為固定值。對b分量，在火箭基礎級飛行段，機動加速度大，可將其置為一非零固定值(如10-10)；對上面級，推力加速度稍小，可置為一更小的值或0。

3 仿真及實測數據計算

算例1：以某顆導航星發射過程中基礎級及上面級飛行為例進行仿真計算。針對基礎級及上面級分別作兩段數據的仿真：

1) 基礎級飛行段，包括了一、二、三級飛行及基礎級與上面級分離等階段；

2) 上面級后期飛行仿真，包括第二滑行段、第二主動段、末修段、調姿段、衛星分離等階段。

基礎級仿真為多站仿真，仿真時以某星發射的理論飛行彈道為依據，同一跟蹤區間設置雙站跟蹤(跟蹤值計算方法為根據測站坐標將理論彈道轉換為東北天坐標系下的測量值)，并對測距、方位角、仰角、測速仿真數據分別加上20 m，0.01°，0.01°，0.1 m/s的隨機噪聲(1σ)。此時濾波計算的飛行速度(v)、速度與地面夾角(θ)曲線如圖1所示，橫軸為相對起飛時刻的時間(t)。

(a)基礎級濾波速度曲線

對上面級飛行采用單站仿真，各測量量所加隨機差同基礎級。此時濾波計算的飛行速度(v)、速度與地面夾角(θ)曲線如圖2所示。

(b)速度地面夾角曲線圖2上面級濾波曲線

從圖1、圖2可以看出，濾波對基礎級二級關機、三級關機點、上面級第二主動段點火點等均較好地實現了自適應處理。

算例2：使用某MEO衛星發射時基礎級及上面級飛行過程實際跟蹤測量數據進行算法有效性驗證。

上面級在第二滑行段(1 600多秒至1萬多秒)為慢旋飛行狀態，導致地面外彈道測量設備跟蹤不穩定，測量數據震蕩較大，野值較多，給濾波適應能力帶來較大挑戰。

作為對比，同時采用3種計算方法進行計算。方法1：采用EKF算法，外推采用當前統計模型；方法2：采用UKF算法，計算時未進行方差矩陣自適應調整；為使結果曲線光滑，對基礎級飛行段(1 200 s前)與上面級跟蹤弧段設置不同的P矩陣初值，并給基礎級濾波設定較大的遺忘因子不斷放大P矩陣(未放大時此方法對基礎級常常濾波發散)；方法3：采用本文算法，給定調整系數λ取值0.1。

此時幾種濾波方法計算的實時飛行軌道的近地點高度(Hp，此值對精度較敏感)曲線如圖3所示，橫軸為相對起飛時刻的時間t。

圖3 某星實測數據濾波計算的Hp曲線

從圖中看出，方法1對基礎級較有效，但由于上面級觀測數據質量太差，導致此方法計算結果質量也很差。原因在于此方法未進行動力學建模，抗野值能力較差[10]。

方法2與方法3結果質量明顯好于方法1。但方法2在特征點附近適應性要差于方法3，且方法2對基礎級與上面級分別設定了不同濾波參數，顯得較為麻煩。

對于方法3，在上面級主發動機第二次點火點(約11 790 s)、第二主動段結束點(約12 700 s)，以及基礎級三級關機等關鍵特征點處，濾波算法較好地實現了自適應跟蹤。但由于上面級跟蹤數據穩定性很差，對Hp曲線局部放大時會發現該曲線震蕩仍較大，平滑性還有待進一步改進。

4 結束語

本文對中心差分卡爾曼濾波計算方法進行了闡述，給出了針對火箭機動飛行過程實時定軌計算的濾波狀態外推模型和觀測模型，以及一種基于位置速度計算的Q矩陣自適應處理方法。仿真及實測數據計算結果表明所給出的算法和模型是有效的。算法具有較好的穩定性、自適應能力和抗差能力。

應該看到，算法還有很多需要進一步改進或對性能影響較大的地方。如在Q矩陣自適應調整時，對調整系數等參數較為敏感，系數較大時，對機動反映及時，但結果震蕩增大，故實際中需合理設置該值。下一步將重點在濾波模型及參數的進一步優化上進行研究。

[1] 林木. 運載火箭上面級功能與技術發展分析[J]. 上海航天, 2013, 30(3):33-38.

[2] 馬昆,郭武,關嵩,等. 上面級發展現狀及趨勢分析[J]. 導彈與航天運載技術, 2013(6):24-28.

[3] NORGAARD M, POULSEN N K, RAVN O. New Developments in State Estimation for Nonlinear Systems[J]. Automatica, 2000, 36(11):1627-1638.

[4] 淡鵬,李恒年,張定波,等. 基于多元非完備信息的實時濾波定軌方法[J]. 飛行力學, 2014, 32(3):285-288.

[5] 劉偉,劉寧. 一種基于UKF交互多模型算法[J]. 雷達科學與技術, 2015, 13(3):302-304, 309.

LIU Wei, LIU Ning. A UKF Based Interactive Multi-Model Algorithm[J]. Radar Science and Technology, 2015, 13(3):302-304, 309.(in Chinese)

[6] ARASARATNAM I, HAYKIN S. Cubature Kalman Filters[J]. IEEE Trans on Automatic Control, 2009, 54(6):1254-1269.

[7] 淡鵬,李恒年,張智斌. 一種火箭外測彈道實時重建的自適應濾波算法[J]. 彈箭與制導學報, 2013, 33(6):186-188, 192.

[8] 淡鵬,李恒年,張定波. 多源觀測數據融合星箭分離初軌計算[J]. 載人航天, 2015, 21(4):398-403.

[9] 李恒年,李濟生,黃永宣. 有連續推力控制的衛星軌道確定算法[J]. 系統工程與電子技術, 2010, 32(9):1957-1961.

[10] 淡鵬,李恒年,李志軍. 應用三向測量數據的深空探測器實時濾波定位算法[J]. 航天器工程, 2015, 24(2):21-26.

[11] 胡振濤,劉先省. 基于“當前”統計模型的一種改進機動目標跟蹤算法[J]. 山東大學學報(工學版), 2005, 35(3):111-114.

[12] 隋紅波,房曉穎,吳瑛. 改進的當前統計模型及自適應跟蹤算法[J]. 雷達科學與技術, 2008, 6(3):202-205.

SUI Hongbo, FANG Xiaoying, WU Ying. A Modified Adaptive Tracking Algorithm Based on Current Statistic Model[J]. Radar Science and Technology, 2008, 6(3):202-205.(in Chinese)

[13] 金亮亮,劉亞云. 一種改進自適應機動目標跟蹤算法[J]. 雷達科學與技術, 2014, 12(1):97-100.

JIN Liangliang, LIU Yayun. An Improved Tracking Algorithm for Maneuvering Target[J]. Radar Science and Technology, 2014, 12(1):97-100.(in Chinese)