，

(1.宇航動(dòng)力學(xué)國(guó)家重點(diǎn)實(shí)驗(yàn)室， 陜西西安 710043；2.西安衛(wèi)星測(cè)控中心， 陜西西安 710043)

0 引言

運(yùn)載火箭上面級(jí)[1-2]具有自主導(dǎo)航能力強(qiáng)、推力大、較基礎(chǔ)級(jí)工作時(shí)間長(zhǎng)、可多次起動(dòng)等特點(diǎn)，上面級(jí)的使用是提高火箭性能及任務(wù)適應(yīng)能力的有效解決途徑。已經(jīng)在國(guó)外的航天發(fā)射任務(wù)中得到了廣泛應(yīng)用，在我國(guó)也受到了越來(lái)越多的重視，如某顆新一代北斗導(dǎo)航星的發(fā)射即采用了上面級(jí)入軌技術(shù)。

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

為此，本文將中心差分容積卡爾曼濾波引入火箭上面級(jí)飛行彈道實(shí)時(shí)跟蹤計(jì)算過(guò)程中，給出了一種適用于工程實(shí)際情況的實(shí)時(shí)狀態(tài)估計(jì)算法，并通過(guò)位置速度與加速度關(guān)系對(duì)模型方差Q進(jìn)行自適應(yīng)調(diào)整，以實(shí)現(xiàn)濾波對(duì)彈道機(jī)動(dòng)過(guò)程的自適應(yīng)跟蹤。仿真及實(shí)測(cè)數(shù)據(jù)計(jì)算結(jié)果表明，所給出的計(jì)算方法是可行的。

1 中心差分卡爾曼濾波

1.1 中心差分變換算法

中心差分變換是CDKF的基礎(chǔ)，其核心思想是通過(guò)隨機(jī)變量的非線(xiàn)性變換求解均值和方差等，具體就是用Sterling差值公式代替泰勒級(jí)數(shù)展開(kāi)式中一階、二階導(dǎo)數(shù)，用多項(xiàng)式逼近非線(xiàn)性方程導(dǎo)數(shù)，從而避免了EKF濾波中雅可比矩陣的求導(dǎo)運(yùn)算[3]，具有實(shí)現(xiàn)過(guò)程較為簡(jiǎn)單的特點(diǎn)。

(1)

采用中心差分計(jì)算來(lái)代替式(1)中一階、二階導(dǎo)數(shù)項(xiàng)，可得到非線(xiàn)性方程的近似表達(dá)式：

(2)

(3)

(4)

(5)

(6)

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

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

(7)

方差為

(8)

協(xié)方差為

(9)

1.2 CDKF濾波計(jì)算

CDKF實(shí)質(zhì)上是利用基于先驗(yàn)分布構(gòu)造的Simga點(diǎn)進(jìn)行線(xiàn)性回歸變換來(lái)表示狀態(tài)后驗(yàn)分布的一種采樣濾波方法，其基本原理與UKF類(lèi)似。

設(shè)非線(xiàn)性系統(tǒng)為

(10)

式中：xk為k時(shí)刻系統(tǒng)狀態(tài)向量，其維數(shù)為L(zhǎng)； F為狀態(tài)向量的轉(zhuǎn)移函數(shù)；wk-1為狀態(tài)向量的高斯過(guò)程噪聲，其均值為零，協(xié)方差矩陣為Qk-1；yk為k時(shí)刻的m維觀(guān)測(cè)向量； H為觀(guān)測(cè)函數(shù)；vk為高斯量測(cè)噪聲，協(xié)方差矩陣為Rk。則該非線(xiàn)性估計(jì)的CDKF濾波算法主要有以下步驟：

1) 構(gòu)造時(shí)間更新所需采樣點(diǎn)

(11)

其變換權(quán)系數(shù)為

Wx,0=(h2-L)/h2

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

(12)

2) 進(jìn)行時(shí)間更新

(13)

3) 構(gòu)造量測(cè)更新所需的采樣點(diǎn)集

(14)

4) 量測(cè)更新計(jì)算

(15)

量測(cè)與狀態(tài)的互協(xié)方差陣：

(16)

5) 計(jì)算殘差

6) 進(jìn)行狀態(tài)更新

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

2 火箭及上面級(jí)彈道濾波模型

2.1 觀(guān)測(cè)方程及系統(tǒng)狀態(tài)方程

rE=(HG)r

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

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

(17)

式中，PR為歲差矩陣，NR為章動(dòng)矩陣，ER為地球自轉(zhuǎn)矩陣，EP為極移矩陣，EDR為地球自轉(zhuǎn)矩陣的導(dǎo)數(shù)矩陣。

2) 將地固系位置速度轉(zhuǎn)到測(cè)站東北天坐標(biāo)系下，由測(cè)站大地坐標(biāo)可計(jì)算出地固系到測(cè)站東北天坐標(biāo)系的轉(zhuǎn)換矩陣MES，則有

ρS=MES(rE-RS)

(18)

這樣，即可計(jì)算出外彈道測(cè)量的4個(gè)觀(guān)測(cè)量：

A=arctan(ρx/ρy)

(19)

濾波中對(duì)外彈道測(cè)量數(shù)據(jù)的使用，即觀(guān)測(cè)方程的建立過(guò)程，主要有兩種方法，一種是直接使用測(cè)距與測(cè)角轉(zhuǎn)換出的位置數(shù)據(jù)；另一種是直接將觀(guān)測(cè)方程建立在測(cè)站東北天坐標(biāo)系中[7-8]。

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

2.2 軌道機(jī)動(dòng)過(guò)程彈道計(jì)算

(20)

對(duì)k時(shí)刻到k+1時(shí)刻的系統(tǒng)狀態(tài)外推預(yù)測(cè)可通過(guò)對(duì)上述動(dòng)力學(xué)模型進(jìn)行Runge-Kutta積分得到。

2.3 機(jī)動(dòng)過(guò)程Q矩陣自適應(yīng)處理

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

考慮到機(jī)動(dòng)發(fā)生時(shí)，火箭及上面級(jí)飛行的加速度變化趨勢(shì)會(huì)發(fā)生突變。此時(shí)，若濾波狀態(tài)外推未能及時(shí)反映出此變化，則原狀態(tài)外推模型中的狀態(tài)協(xié)方差矩陣Q將與當(dāng)前系統(tǒng)真實(shí)狀態(tài)出現(xiàn)較大偏差，為此可通過(guò)對(duì)Q矩陣進(jìn)行實(shí)時(shí)調(diào)整的方法來(lái)使得濾波計(jì)算實(shí)現(xiàn)對(duì)機(jī)動(dòng)過(guò)程的自適應(yīng)處理。

(21)

(22)

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

設(shè)k-1時(shí)刻到k時(shí)刻的時(shí)間增量為T(mén)，并在計(jì)算公式中引入調(diào)整系數(shù)λ，合理設(shè)置λ以增強(qiáng)自適應(yīng)能力。從而可將Q矩陣中加速度x,y,z方向分量的方差分別自適應(yīng)調(diào)整為位置與速度的函數(shù)：

(23)

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

實(shí)際計(jì)算中，若只對(duì)Q矩陣加速度方向進(jìn)行調(diào)整，將使得震蕩加劇。為此，還需對(duì)速度方向進(jìn)行限制性調(diào)整。計(jì)算公式為

(24)

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

3 仿真及實(shí)測(cè)數(shù)據(jù)計(jì)算

算例1：以某顆導(dǎo)航星發(fā)射過(guò)程中基礎(chǔ)級(jí)及上面級(jí)飛行為例進(jìn)行仿真計(jì)算。針對(duì)基礎(chǔ)級(jí)及上面級(jí)分別作兩段數(shù)據(jù)的仿真：

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

2) 上面級(jí)后期飛行仿真，包括第二滑行段、第二主動(dòng)段、末修段、調(diào)姿段、衛(wèi)星分離等階段。

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

(a)基礎(chǔ)級(jí)濾波速度曲線(xiàn)

對(duì)上面級(jí)飛行采用單站仿真，各測(cè)量量所加隨機(jī)差同基礎(chǔ)級(jí)。此時(shí)濾波計(jì)算的飛行速度(v)、速度與地面夾角(θ)曲線(xiàn)如圖2所示。

(b)速度地面夾角曲線(xiàn)圖2上面級(jí)濾波曲線(xiàn)

從圖1、圖2可以看出，濾波對(duì)基礎(chǔ)級(jí)二級(jí)關(guān)機(jī)、三級(jí)關(guān)機(jī)點(diǎn)、上面級(jí)第二主動(dòng)段點(diǎn)火點(diǎn)等均較好地實(shí)現(xiàn)了自適應(yīng)處理。

算例2：使用某MEO衛(wèi)星發(fā)射時(shí)基礎(chǔ)級(jí)及上面級(jí)飛行過(guò)程實(shí)際跟蹤測(cè)量數(shù)據(jù)進(jìn)行算法有效性驗(yàn)證。

上面級(jí)在第二滑行段(1 600多秒至1萬(wàn)多秒)為慢旋飛行狀態(tài)，導(dǎo)致地面外彈道測(cè)量設(shè)備跟蹤不穩(wěn)定，測(cè)量數(shù)據(jù)震蕩較大，野值較多，給濾波適應(yīng)能力帶來(lái)較大挑戰(zhàn)。

作為對(duì)比，同時(shí)采用3種計(jì)算方法進(jìn)行計(jì)算。方法1：采用EKF算法，外推采用當(dāng)前統(tǒng)計(jì)模型；方法2：采用UKF算法，計(jì)算時(shí)未進(jìn)行方差矩陣自適應(yīng)調(diào)整；為使結(jié)果曲線(xiàn)光滑，對(duì)基礎(chǔ)級(jí)飛行段(1 200 s前)與上面級(jí)跟蹤弧段設(shè)置不同的P矩陣初值，并給基礎(chǔ)級(jí)濾波設(shè)定較大的遺忘因子不斷放大P矩陣(未放大時(shí)此方法對(duì)基礎(chǔ)級(jí)常常濾波發(fā)散)；方法3：采用本文算法，給定調(diào)整系數(shù)λ取值0.1。

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

圖3 某星實(shí)測(cè)數(shù)據(jù)濾波計(jì)算的Hp曲線(xiàn)

從圖中看出，方法1對(duì)基礎(chǔ)級(jí)較有效，但由于上面級(jí)觀(guān)測(cè)數(shù)據(jù)質(zhì)量太差，導(dǎo)致此方法計(jì)算結(jié)果質(zhì)量也很差。原因在于此方法未進(jìn)行動(dòng)力學(xué)建模，抗野值能力較差[10]。

方法2與方法3結(jié)果質(zhì)量明顯好于方法1。但方法2在特征點(diǎn)附近適應(yīng)性要差于方法3，且方法2對(duì)基礎(chǔ)級(jí)與上面級(jí)分別設(shè)定了不同濾波參數(shù)，顯得較為麻煩。

對(duì)于方法3，在上面級(jí)主發(fā)動(dòng)機(jī)第二次點(diǎn)火點(diǎn)(約11 790 s)、第二主動(dòng)段結(jié)束點(diǎn)(約12 700 s)，以及基礎(chǔ)級(jí)三級(jí)關(guān)機(jī)等關(guān)鍵特征點(diǎn)處，濾波算法較好地實(shí)現(xiàn)了自適應(yīng)跟蹤。但由于上面級(jí)跟蹤數(shù)據(jù)穩(wěn)定性很差，對(duì)Hp曲線(xiàn)局部放大時(shí)會(huì)發(fā)現(xiàn)該曲線(xiàn)震蕩仍較大，平滑性還有待進(jìn)一步改進(jìn)。

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

本文對(duì)中心差分卡爾曼濾波計(jì)算方法進(jìn)行了闡述，給出了針對(duì)火箭機(jī)動(dòng)飛行過(guò)程實(shí)時(shí)定軌計(jì)算的濾波狀態(tài)外推模型和觀(guān)測(cè)模型，以及一種基于位置速度計(jì)算的Q矩陣自適應(yīng)處理方法。仿真及實(shí)測(cè)數(shù)據(jù)計(jì)算結(jié)果表明所給出的算法和模型是有效的。算法具有較好的穩(wěn)定性、自適應(yīng)能力和抗差能力。

應(yīng)該看到，算法還有很多需要進(jìn)一步改進(jìn)或?qū)π阅苡绊戄^大的地方。如在Q矩陣自適應(yīng)調(diào)整時(shí)，對(duì)調(diào)整系數(shù)等參數(shù)較為敏感，系數(shù)較大時(shí)，對(duì)機(jī)動(dòng)反映及時(shí)，但結(jié)果震蕩增大，故實(shí)際中需合理設(shè)置該值。下一步將重點(diǎn)在濾波模型及參數(shù)的進(jìn)一步優(yōu)化上進(jìn)行研究。

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

[2] 馬昆,郭武,關(guān)嵩,等. 上面級(jí)發(fā)展現(xiàn)狀及趨勢(shì)分析[J]. 導(dǎo)彈與航天運(yùn)載技術(shù), 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] 淡鵬,李恒年,張定波,等. 基于多元非完備信息的實(shí)時(shí)濾波定軌方法[J]. 飛行力學(xué), 2014, 32(3):285-288.

[5] 劉偉,劉寧. 一種基于UKF交互多模型算法[J]. 雷達(dá)科學(xué)與技術(shù), 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] 淡鵬,李恒年,張智斌. 一種火箭外測(cè)彈道實(shí)時(shí)重建的自適應(yīng)濾波算法[J]. 彈箭與制導(dǎo)學(xué)報(bào), 2013, 33(6):186-188, 192.

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

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

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

[11] 胡振濤,劉先省. 基于“當(dāng)前”統(tǒng)計(jì)模型的一種改進(jìn)機(jī)動(dòng)目標(biāo)跟蹤算法[J]. 山東大學(xué)學(xué)報(bào)(工學(xué)版), 2005, 35(3):111-114.

[12] 隋紅波,房曉穎,吳瑛. 改進(jìn)的當(dāng)前統(tǒng)計(jì)模型及自適應(yīng)跟蹤算法[J]. 雷達(dá)科學(xué)與技術(shù), 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ìn)自適應(yīng)機(jī)動(dòng)目標(biāo)跟蹤算法[J]. 雷達(dá)科學(xué)與技術(shù), 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)