張 峰，黃繼勛，王頌邦

（北京航天時(shí)代光電科技有限公司，北京 100094）

基于無跡粒子濾波的飛行器再入段傳遞對(duì)準(zhǔn)方法

張 峰，黃繼勛，王頌邦

（北京航天時(shí)代光電科技有限公司，北京 100094）

針對(duì)再入式飛行器再入返回前需要精確和快速的傳遞對(duì)準(zhǔn)需求以及空間環(huán)境特殊性，提出了一種適用于再入式飛行器空間平臺(tái)對(duì)準(zhǔn)方法，建立J2000坐標(biāo)系下光纖陀螺慣組的系統(tǒng)狀態(tài)方程，利用星敏感器輸出的姿態(tài)和光纖陀螺慣組輸出的姿態(tài)差值作為觀測(cè)量，實(shí)現(xiàn)飛行器的“姿態(tài)”匹配傳遞對(duì)準(zhǔn)算法。針對(duì)系統(tǒng)狀態(tài)方程和量測(cè)方程的非線性特性以及單獨(dú)使用無跡卡爾曼濾波(UKF)方法存在濾波不穩(wěn)定性和單獨(dú)使用粒子濾波(PF)方法存在粒子退化導(dǎo)致估計(jì)誤差變大的問題，提出了無跡粒子濾波(UPF)算法，采用衛(wèi)星工具包(STK)軟件模擬飛行器在軌運(yùn)行進(jìn)行仿真驗(yàn)證。仿真結(jié)果表明：該方法可以在3s內(nèi)使得失準(zhǔn)角的估計(jì)誤差小于0.6′，估計(jì)精度提高了30%，收斂時(shí)間縮短了70%，相比傳統(tǒng)方法提高了對(duì)準(zhǔn)的精確性和快速性。

再入式飛行器；傳遞對(duì)準(zhǔn)；空間平臺(tái)；無跡卡爾曼濾波；粒子濾波

再入式飛行器因其具備強(qiáng)大的突防能力和可重復(fù)使用的優(yōu)勢(shì)，作為高新科技發(fā)展的標(biāo)志，在我國國防和經(jīng)濟(jì)建設(shè)中扮演著極為重要的角色。飛行器在再入前需要通過傳遞對(duì)準(zhǔn)方式獲得高精度的姿態(tài)信息。傳遞對(duì)準(zhǔn)的速度和精度決定了飛行器的響應(yīng)速度和精度。飛行器再入階段的傳遞對(duì)準(zhǔn)屬于空間平臺(tái)的動(dòng)基座對(duì)準(zhǔn)方式。

光纖陀螺慣組和星敏感器是飛行器上最重要的傳感器。近幾年，有不少學(xué)者提出利用星敏感器輔助陀螺慣組傳遞對(duì)準(zhǔn)。馬閃等用彈載星敏感器引入的位置和姿態(tài)角信息，建立“位置+姿態(tài)角”匹配的對(duì)準(zhǔn)方法，實(shí)現(xiàn)了對(duì)天基導(dǎo)彈的精對(duì)準(zhǔn)。印度Chaudhuri等學(xué)者建立了NWV(North West Vertical)坐標(biāo)系下空間機(jī)動(dòng)平臺(tái)的傳遞對(duì)準(zhǔn)模型。胡正東博士等人研究了空間平臺(tái)下武器系統(tǒng)傳遞對(duì)準(zhǔn)的機(jī)動(dòng)方案，在當(dāng)?shù)厮阶鴺?biāo)系下建立了21維傳遞對(duì)準(zhǔn)模型，選取了“速度+姿態(tài)”匹配的方案進(jìn)行了仿真。上述文獻(xiàn)對(duì)空間平臺(tái)下的傳遞對(duì)準(zhǔn)技術(shù)開展了研究工作，但存在一定的局限性：由于空間環(huán)境具有微重力等特點(diǎn)，此時(shí)，引力提供了其軌道運(yùn)行加速度，此時(shí)加速度計(jì)的輸出為其自身的測(cè)量噪聲和向心加速度，因此，傳統(tǒng)的 “位置+姿態(tài)角”或“速度+姿態(tài)角”的匹配模型并不能適應(yīng)于空間平臺(tái)的傳遞對(duì)準(zhǔn)；相對(duì)于NWV坐標(biāo)系，采用J2000坐標(biāo)系更能適用于空間平臺(tái)傳遞對(duì)準(zhǔn)坐標(biāo)系；另外上述文獻(xiàn)未考慮飛行器傳遞對(duì)準(zhǔn)模型為非線性，傳統(tǒng)濾波方法線性化處理會(huì)造成濾波器精度降低且濾波穩(wěn)定性也難以保證。

基于上述情況，本文提出了以J2000慣性坐標(biāo)系為導(dǎo)航系的星敏感器輔助的飛行器再入階段傳遞對(duì)準(zhǔn)方法，利用星敏感器的高精度姿態(tài)信息修正光纖陀螺慣組的誤差。針對(duì)模型的非線性化，單獨(dú)使用 UKF存在濾波不穩(wěn)定和使用 PF方法存在粒子退化導(dǎo)致估計(jì)誤差變大的問題，提出了 UPF濾波方法，通過將UKF作為其重要性分布函數(shù)來解決粒子退化問題，相比單獨(dú)使用UKF或PF方法，在傳遞對(duì)準(zhǔn)時(shí)間和精度上都有很大的提高。

1 飛行器再入段傳遞對(duì)準(zhǔn)模型

1.1 傳遞對(duì)準(zhǔn)方案

利用星敏感器測(cè)量恒星天體位置與飛行器相對(duì)位置，獲得飛行器的姿態(tài)信息，并與光纖陀螺慣組輸出的姿態(tài)信息進(jìn)行比較，得到飛行器的姿態(tài)角誤差，作為量測(cè)量。建立光纖陀螺慣組在空間平臺(tái)下的系統(tǒng)誤差方程，通過濾波估計(jì)將數(shù)學(xué)平臺(tái)偏離導(dǎo)航坐標(biāo)系的失準(zhǔn)角估計(jì)出來，校正飛行器的數(shù)學(xué)平臺(tái)使之與導(dǎo)航坐標(biāo)系精確對(duì)準(zhǔn)，對(duì)準(zhǔn)方案如圖1所示。

圖1 空間平臺(tái)傳遞對(duì)準(zhǔn)方法Fig.1 Alignment method on space platform

1.2 系統(tǒng)誤差方程

傳遞對(duì)準(zhǔn)誤差方程包括姿態(tài)誤差方程、速度誤差方程和位置誤差方程，以下建立J2000地心慣性坐標(biāo)系(i系)下有關(guān)導(dǎo)航系統(tǒng)誤差方程。

1.2.1 姿態(tài)誤差方程

姿態(tài)誤差方程可表示為

式(8)為慣性坐標(biāo)系下的姿態(tài)失準(zhǔn)角誤差方程，該式表明：慣性坐標(biāo)系下的捷聯(lián)慣導(dǎo)系統(tǒng)姿態(tài)失準(zhǔn)角誤差是由載體陀螺儀的角速度誤差所引起的。

1.2.2 速度誤差方程

考慮飛行器在軌運(yùn)行，由地球自轉(zhuǎn)引起的向心加速度以及哥氏加速度的影響，速度誤差方程為：

式中，K=diag(kx,ky,kz)為加速度計(jì)輸出的比力標(biāo)度因數(shù)誤差陣，b為加速度計(jì)零位誤差。δGi如下式所示：

式中：GM為萬有引力常數(shù)與地球質(zhì)量乘積，GM=398600.5e9m3s2；r為地球半徑， r=6371km。結(jié)合上述式(5)(11)(12)可得：

式中，ΔGi表示引力擾動(dòng)矢量?；喌盟俣日`差微分方程：

式中，F(xiàn)i為fi的反對(duì)稱陣。

1.2.3 位置誤差微分方程

慣性坐標(biāo)系下位置誤差微分方程由位置方程得到，如下式所示：

1.3 狀態(tài)方程

為了提高傳遞對(duì)準(zhǔn)速度，必須對(duì)全階狀態(tài)方程的維數(shù)進(jìn)行降維處理，否則將會(huì)給光纖陀螺慣組的計(jì)算帶來很大負(fù)擔(dān)，但又必須保證對(duì)準(zhǔn)精度，因此，設(shè)計(jì)了9維傳遞對(duì)準(zhǔn)模型。

離散的狀態(tài)方程可以描述為1.4 量測(cè)方程

式中，v為姿態(tài)角量測(cè)噪聲，v～N(0,R)。建立姿態(tài)角誤差觀測(cè)量[δθδγδψ]與系統(tǒng)狀態(tài)模型中的狀態(tài)變量之間的關(guān)系，設(shè)姿態(tài)轉(zhuǎn)換矩陣T為

易得：

由(20)和(21)可得：

將式(22)左端按照一階泰勒展開可得：

再由式(22)可得：

由式(23)和式(24)可得：

則姿態(tài)角匹配的測(cè)量矩陣為

從上述狀態(tài)方程和量測(cè)方程推導(dǎo)結(jié)果來看，系統(tǒng)狀態(tài)方程和量測(cè)方程均為非線性方程，本文針對(duì)方程非線性的問題，提出了UPF濾波算法。

2 UPF空間平臺(tái)傳遞對(duì)準(zhǔn)濾波算法

2.1 算法思路

無跡卡爾曼濾波(UKF)是采用逼近非線性函數(shù)概率密度分布的濾波方法。其核心是UT變換，即根據(jù)先驗(yàn)分布采樣一組確定的Sigma點(diǎn)，這些點(diǎn)保留了原狀態(tài)變量的先驗(yàn)分布特性；將這些點(diǎn)通過非線性函數(shù)傳播，對(duì)所得結(jié)果作加權(quán)回歸運(yùn)算使其逼近非線性狀態(tài)后驗(yàn)分布。然而，當(dāng) Sigma點(diǎn)采樣涉及協(xié)方差陣的平方根運(yùn)算時(shí)，數(shù)值計(jì)算中的舍入誤差會(huì)破壞協(xié)方差陣的非負(fù)定性和對(duì)稱性，降低濾波算法的穩(wěn)定性。同時(shí)，高階矩陣的平方根運(yùn)算也將導(dǎo)致系統(tǒng)的計(jì)算負(fù)擔(dān)增加。

粒子濾波(PF)具有非常好的處理非線性模型的能力，魯棒性強(qiáng)，但存在粒子退化的問題，經(jīng)過幾步迭代遞推之后，許多粒子的權(quán)重變得非常小，它們?cè)讷@得估計(jì)值過程中起到的作用將變得微乎其微，可以稱之為無效粒子。

結(jié)合UKF和PF的優(yōu)勢(shì)，取長補(bǔ)短，采用了UPF算法，其思路則是通過UKF 算法設(shè)計(jì)重要性函數(shù)，產(chǎn)生粒子。首先利用UKF實(shí)時(shí)的進(jìn)行狀態(tài)的均值和方差估計(jì)，以作為PF粒子采樣的重要性函數(shù)，從而可以將采樣粒子從高先驗(yàn)密度區(qū)向高似然密度區(qū)移動(dòng)。

2.2 UPF算法流程

建立離散時(shí)間系統(tǒng)狀態(tài)空間模型：

具體解算流程如下：

式中，i=1,…,n。由系統(tǒng)狀態(tài)方程為n=9維變量，故sigma采樣點(diǎn)個(gè)數(shù)為2n+1=19。對(duì)應(yīng)的權(quán)值為：

一階統(tǒng)計(jì)量權(quán)值：

二階統(tǒng)計(jì)量權(quán)值：

式中：λ為比例因子；κ在多維狀態(tài)變量下通常取為κ=3；α決定采樣點(diǎn)與均值的離散程度，通常取α=0.01；β用來描述的分布信息，通常取β=2；為協(xié)方差加權(quán)，為均值加權(quán)。

3）時(shí)間更新：

4）量測(cè)量更新：

5）更新sigma點(diǎn)：

6）重要性采樣，并計(jì)算重要性權(quán)值：

7）計(jì)算估計(jì)誤差，定義Neff來衡量有效粒子數(shù)量并運(yùn)用重采樣算法對(duì)加權(quán)粒子進(jìn)行重新采樣。

8）更新估計(jì)值：

該算法改進(jìn)了一般PF濾波方法中重要性函數(shù)的選擇策略，有利于將采樣粒子從高先驗(yàn)密度區(qū)向高似然密度區(qū)移動(dòng)，可以提高濾波精度，同時(shí)又避免了UKF線性化帶來的濾波不穩(wěn)定性的問題。

算法具體流程如圖2所示。

圖2 算法流程圖Fig.2 Flowchart of the algorithm

3 仿真與試驗(yàn)

仿真過程采用STK軟件來仿真空間平臺(tái)軌道數(shù)據(jù)[12-14]，軌道參數(shù)為：近地點(diǎn)距離300km，遠(yuǎn)地點(diǎn)距離350km，軌道傾角15°。選擇J2000坐標(biāo)系，定義oxyz為飛行器本體坐標(biāo)系，其中：ox軸通過坐標(biāo)原點(diǎn)，指標(biāo)飛行器飛行方向；oy軸按照右手定則，垂直軌道面方向；oz軸指向地心方向。飛行器仿真軌跡為繞oz軸座搖擺機(jī)動(dòng)運(yùn)動(dòng)，最大搖擺角度為2°，搖擺周期為1s。

根據(jù)衛(wèi)星運(yùn)動(dòng)軌跡參數(shù)，可得到飛行器理論的姿態(tài)四元數(shù)和姿態(tài)角信息。對(duì)光纖陀螺慣組誤差參數(shù)定義如下：X軸方向失準(zhǔn)角xφ為4°，Y軸方向失準(zhǔn)角yφ為5°，Z軸方向失準(zhǔn)角zφ為10°。光纖陀螺慣組的性能參數(shù)如表1所示。

表1 光纖陀螺慣組的性能參數(shù)Tab.1 Parameter specification of slave FOG IMU

圖3 X軸失準(zhǔn)角估計(jì)曲線Fig.3 Estimation curve of X-axis misalignment

仿真濾波初值設(shè)定：狀態(tài)變量X初始值均取為0，初始方差陣的對(duì)角線元素取P(0)=103Ι9×9，姿態(tài)角量測(cè)噪聲方差陣取R=0.01·Ι3×3，光纖陀螺慣組姿態(tài)更新時(shí)間為10ms，星敏感器更新時(shí)間為250ms，濾波周期為250 ms，N=300，Nthr=100，仿真時(shí)間為50 s。圖3～圖5為三軸失準(zhǔn)角的估計(jì)結(jié)果。由圖3～圖5所示以及表2失準(zhǔn)角估計(jì)誤差結(jié)果可知，本文提出的基于無跡粒子濾波方法比僅用粒子濾波或無跡卡爾曼濾波可以在3 s內(nèi)使得失準(zhǔn)角估計(jì)誤差達(dá)到優(yōu)于0.6′，估計(jì)精度方面提高了30%，收斂時(shí)間縮短了70%。

圖4 Y軸失準(zhǔn)角估計(jì)曲線Fig.4 Estimation curves of Y-axis misalignment

圖5 Z軸失準(zhǔn)角估計(jì)曲線Fig.5 Estimation curves of Z-axis misalignment

表2為失準(zhǔn)角的估計(jì)值。圖6為三個(gè)軸失準(zhǔn)角角度的估計(jì)誤差曲線。

表2 失準(zhǔn)角估計(jì)誤差Tab.2 Estimation errors of three-axis misalignments

圖6 三軸失準(zhǔn)角估計(jì)誤差Fig.6 Estimation errors of three-axis misalignments

4 結(jié) 論

針對(duì)再入飛行器再入段環(huán)境的特點(diǎn)，提出 J2000慣性坐標(biāo)系下基于星敏感器和光纖陀螺慣組“姿態(tài)”匹配的傳遞對(duì)準(zhǔn)方法，建立了光纖陀螺慣組的系統(tǒng)狀態(tài)方程，利用星敏感器輸出的姿態(tài)和光纖陀螺慣組輸出的姿態(tài)差值作為觀測(cè)量，實(shí)現(xiàn)飛行器的傳遞對(duì)準(zhǔn)。針對(duì)系統(tǒng)狀態(tài)方程和量測(cè)方程的非線性特性，提出了無跡卡爾曼濾波和粒子濾波相結(jié)合濾波算法，采用STK軟件模型飛行器在軌運(yùn)行進(jìn)行仿真驗(yàn)證。仿真結(jié)果表明：該方法可以在3 s內(nèi)使得失準(zhǔn)角估計(jì)誤差達(dá)到優(yōu)于0.6′, 估計(jì)精度提高了約30%，收斂時(shí)間縮短了70%，相比傳統(tǒng)方法提高了仿真驗(yàn)證了該算法的有效性，精確性和快速性。該方案同樣適用于其他飛行器的傳遞對(duì)準(zhǔn)，具有較好的參考價(jià)值。

（References）：

[1] 戴晨曦, 程向紅, 陳紅梅, 等. 天文觀測(cè)角輔助的高超聲速飛行器傳遞對(duì)準(zhǔn)方法[J]. 中國慣性技術(shù)學(xué)報(bào), 2015, 23(4): 446-450. Dai Chen-xi, Cheng Xiang-hong, Chen Hong-mei, et al. Transfer alignment approach of hypersonic vehicle aided by celestial angle[J]. Journal of Chinese Inertial Technology, 2015, 23(4): 446-450.

[2] 胡正東, 楊華波, 張士峰, 等. 空間平臺(tái)武器系統(tǒng)傳遞對(duì)準(zhǔn)的機(jī)動(dòng)方案設(shè)計(jì)[J]. 中國慣性技術(shù)學(xué)報(bào), 2007, 15(4): 398-402. Hu Zheng-dong, Yang Hua-bo, Zhang Shi-feng, et al. Maneuvering scheme of transfer alignment for weapon system on space platform[J]. Journal of Chinese Inertial Technology, 2007, 15(4): 398-402.

[3] 梁冬生, 劉朝暉, 劉文, 等. 航空飛行器天文自主導(dǎo)航定位技術(shù)[J]. 紅外與激光工程, 2014, 43(9): 3020-3025. Liang Dong-sheng, Liu Zhao-hui, Liu Wen, et al. Aerial vehicle astronomy autonomous navigation technology[J]. Infrared and Laser Engineering, 2014, 43(9): 3020-3025.

[4] Wang Rong, Xiong Zhi, Liu Jian-ye, et al. SINS/GPS/CNS information fusion system based on improved Huber filter with classified adaptive factors for high-speed UAVs[C]// IEEE/ION Position Location and Navigation Symposium. 2012: 441-446.

[5] Zhang Jin-liang, Qin Yong-yuan, Wu Feng. Technique for in-flight calibrating installation errors in multi-IMU redundancy system[C]//Proceedings of the 2012 International Conference on Information Technology and Software Engineering. 2012: 99-108.

[6] 何小飛, 王巍, 黃繼勛. 聯(lián)邦濾波在光纖陀螺捷聯(lián)慣導(dǎo)系統(tǒng)傳遞對(duì)準(zhǔn)中的應(yīng)用[J]. 紅外與激光工程, 2013, 42(4): 993-997. He xiao-fei, Wang Wei, Huang Ji-xun. Application for transfer alignment of FOG SINS based on federated filter [J]. Infrared and Laser Engineering, 2013, 42(4): 993-997.

[7] Richman M S, Kenyon J A, Sega R M. High speed and hypersonic science and technology[C]//2005 AIAA Guidance, Navigation and Control Conference and Exhibit. San Francisco, CA: AIAA 2005-4099.

[8] Bahm C, Baumann E. The X-43A Hyper-X Mach 7 Flight 2 guidance, navigation and control overview and flight test results[C]//AIAA/CIRA 13thInternational Space Planes and Hypersonics Systems and Technology: AIAA 2005-3275.

[9] 吳楓, 秦永元, 成研, 等. 基于GPS的彈載捷聯(lián)慣導(dǎo)動(dòng)基座傳遞對(duì)準(zhǔn)技術(shù)[J]. 中國慣性技術(shù)學(xué)報(bào), 2013, 21(1): 56-60. Wu Feng, Qin Yong-yuan, Chen Yan, et al. Transfer alignment for missile-borne SINS using airborne GPS on moving base[J]. Journal of Chinese Inertial Technology, 2013, 21(1): 56-60.

[10] Lai Ji-zhou, Xiong Jian, Liu Jian-ye, et al. Improved arithmetic of two-position fast initial alignment for SINS using unscented Kalman filter[J]. International Journal of Innovative Computing, Information and Control, 2012, 8(4): 2929-2940.

[11] Ilewicz W, Nawrat A. Direct method of IMU calibration. In: Advanced technologies for intelligent systems of national border security studies in computational intelligence[M]. Springer, Heidelberg, 2013: 155-171.

Transfer alignment for reentry spacecraft based on unscented particle filter in space platform

ZHANG Feng, HUANG Ji-xun, WANG Song-bang
(Beijing Aerospace Times Optical-Electronic Technology Co., Ltd., Beijing 100094,China)

A transfer alignment method for reentry spacecraft in view of the space environment’s particularity is proposed to serve the need of rapid and precise transfer alignment. The state and measurement equations of the fiber optic gyroscope inertial measurement unit (FOG IMU) are established, and by using the output attitude difference between the outputs of FOG IMU and star sensor, a transfer alignment algorithm is achieved. Since only using UKF has the problems of filtering instability, and only using PF has the problems of particle degradation, an algorithm based on unscented particle filter (UPF) method is presented on account of the nonlinear feature of the state and measurement equations. Simulations are made by using a satellite tool kit (STK) tool to simulate the spacecraft its operations on-orbit, which show that, compared with its traditional method, the misalignment estimation can converge to 0.6 arcmin within 3 s, the estimation accuracy is increased by 30%, and convergence time is shorten by 70%。

reentry spacecraft; transfer alignment; space platform; unscented Kalman filter; particle filter

U666.1

：A

2016-04-11；

：2016-07-28

國家高技術(shù)研究發(fā)展計(jì)劃項(xiàng)目（863-706）（2013AA706503）

張峰（1982—），男，高工，博士研究生，從事導(dǎo)航、制導(dǎo)與控制技術(shù)研究。E-mail: guyansnow@126.com

1005-6734(2016)04-0443-06

10.13695/j.cnki.12-1222/o3.2016.04.005