汪益平 陳 帥 趙 琛 屈新芬

1.南京理工大學(xué)自動化學(xué)院，南京 210094 2.中國工程物理研究院電子工程研究所，綿陽 621000

強機動條件下的彈載深組合ARCKF濾波方法研究*

汪益平1陳 帥1趙 琛1屈新芬2

1.南京理工大學(xué)自動化學(xué)院，南京 210094 2.中國工程物理研究院電子工程研究所，綿陽 621000

針對彈道導(dǎo)彈飛行中GNSS/SINS深組合導(dǎo)航系統(tǒng)呈現(xiàn)的強機動、非線性特性，引入了基于三階球面-徑向準則的容積卡爾曼(CKF)非線性濾波方法。同時，提出了一種自適應(yīng)抗差容積卡爾曼濾波(ARCKF)算法，該算法運用抗差M思想，調(diào)節(jié)量測噪聲陣，以抵御系統(tǒng)觀測異常擾動，采用自適應(yīng)因子對協(xié)方差陣進行調(diào)節(jié)，進一步處理動態(tài)擾動引入的誤差。實驗結(jié)果表明，該濾波算法有效提高了組合導(dǎo)航的動態(tài)性能，在加速度達到60g的強機動仿真環(huán)境中，仍能保持較高的導(dǎo)航精度和跟蹤性能。 關(guān)鍵詞 彈道導(dǎo)彈；深組合；強機動；ARCKF

強機動、高動態(tài)環(huán)境下對載體進行導(dǎo)航定位是一個熱點和難點問題，在軍事應(yīng)用中的地位尤其重要，已被國內(nèi)外眾多學(xué)者廣泛關(guān)注。強機動、高動態(tài)條件下的導(dǎo)航定位難點主要有：慣性導(dǎo)航系統(tǒng)解算誤差變大，對定位技術(shù)也提出了很高的要求；系統(tǒng)觀測傳遞函數(shù)為強非線性，線性化誤差導(dǎo)致系統(tǒng)發(fā)散，因此，對GNSS/SINS組合導(dǎo)航系統(tǒng)的非線性系統(tǒng)估計和濾波問題的研究具有重要的理論意義[1]。

迄今為止，在非線性估計領(lǐng)域尚沒有一種“最好”的算法，必須根據(jù)具體應(yīng)用場合和條件，在適用性、估計精度及計算量等各種指標之間綜合權(quán)衡。傳統(tǒng)組合導(dǎo)航中的非線性濾波[2-3]一般采取擴展卡爾曼濾波(EKF)、無跡卡爾曼濾波(UKF)以及粒波容積卡爾曼濾波(CKF)[4-5]等。與EKF相比，CKF不需計算Jacobian矩陣，更適合非線性系統(tǒng)；同時，CKF克服了UKF在高維系中出現(xiàn)的數(shù)值不穩(wěn)定及精度降低的問題，計算量較PF也更低。因此，非線性濾波CKF在組合導(dǎo)航中的應(yīng)用具有一定的研究價值。

由于在強機動、高動態(tài)等復(fù)雜環(huán)境中，有可能給載體帶來觀測異常以及由動力學(xué)模型不準確導(dǎo)致的誤差，本文在標準CKF算法基礎(chǔ)上，運用抗差估計原理與構(gòu)造自適應(yīng)因子對標準CKF算法進行改進，提出一種自適應(yīng)抗差容積卡爾曼濾波(ARCKF)[6]，以進一步提高組合導(dǎo)航濾波器性能。

1 GNSS/SINS深組合導(dǎo)航模型

與GNSS/SINS的松/緊組合相比，GNSS/ SINS深組合導(dǎo)航系統(tǒng)的顯著特征是信息融合程度的進一步深入，涉及到GNSS接收機內(nèi)部跟蹤環(huán)路，形成GNSS與SINS相互輔助模式。

按照在深組合導(dǎo)航濾波中所用觀測量的差異，可將深組合導(dǎo)航算法分為基于位置、速度組合的深組合導(dǎo)航算法，基于偽距、偽距率組合的深組合導(dǎo)航算法以及基于I/Q值的深組合導(dǎo)航算法。在本文彈載組合系統(tǒng)中，選取偽距、偽距率為觀測量，以建立在發(fā)射慣性系下的GNSS/SINS深組合導(dǎo)航模型[7]。

1.1 深組合系統(tǒng)狀態(tài)方程

1.1.1 SINS的誤差狀態(tài)方程

SINS的15×1維誤差狀態(tài)方程為：

XI=[φxφyφzδVxδVyδVzδX

(1)

(2)

式中，F(xiàn)I(t)為SINS系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣,GI(t)為SINS系統(tǒng)噪聲驅(qū)動矩陣，wI(t)為SINS系統(tǒng)噪聲矩陣。各矩陣具體形式見文獻[8-9]。

1.1.2GNSS的誤差狀態(tài)方程

深組合系統(tǒng)中，GNSS接收機的誤差狀態(tài)量通常取為：與時鐘頻率誤差等效的距離率誤差Δlru與距離誤差Δlu，其狀態(tài)方程為：

(3)

式中，

(4)

(5)

式(2)和(3)即可構(gòu)成發(fā)射慣性系下深組合導(dǎo)航系統(tǒng)的狀態(tài)方程。

1.2 深組合系統(tǒng)觀測方程

(6)

ρg=ρ+δlu+vρ

(7)

式中，δlu為GNSS的時鐘誤差引起的距離誤差，vρ為偽距量測噪聲。ρ與ρi可由下式計算：

(8)

(9)

式中，位置誤差量δX，δY，δZ均可由式(1)中狀態(tài)量濾波估計得出。將式(9)代入式(8)，即可得到理想距離ρ，則偽距率差非線性觀測方程為：

δρ=ρi-ρg=ρi-ρ-δlu-vρ

(10)

同理，可推導(dǎo)出偽距率差非線性觀測方程為：

(11)

2 基于ARCKF的組合濾波算法

2.1 標準CKF濾波

根據(jù)第1節(jié)實際所建立的狀態(tài)模型線性、觀測模型非線性的深組合模型，離散化后，得到：

(12)

式中，Xk,Zk分別為系統(tǒng)的狀態(tài)向量與量測向量；Fk為狀態(tài)轉(zhuǎn)移矩陣，h(·)為系統(tǒng)非線性量測函數(shù)；wk，vk為互不相關(guān)的零均值高斯白噪聲序列，且滿足下列關(guān)系：

(13)

式中，Qk，Rk分別為系統(tǒng)狀態(tài)噪聲和量測噪聲序列的協(xié)方差陣，是對稱的非負定矩陣。

基于三階球面-徑向容積規(guī)則的CKF算法流程如下：

(1)濾波初始化

(14)

(15)

(2)時間更新

計算k-1時刻Cubature點：

(16)

(17)

狀態(tài)一步預(yù)測：

(18)

(19)

(20)

(3)量測更新

計算用于量測更新的Cubatue點：

(21)

(22)

式中，Sk|k-1通過Pk|k-1的Cholesky分解得到。

通過量測方程傳播Cubature點：

(23)

計算增益矩陣：

(24)

(25)

(26)

(27)

狀態(tài)估計值：

(28)

(29)

2.2 自適應(yīng)抗差CKF濾波算法

在正常環(huán)境中，標準CKF即可滿足組合導(dǎo)航的精度需求。但當遭遇到強機動、高動態(tài)等較惡劣環(huán)境時，系統(tǒng)極有可能遭遇較大的觀測異常誤差和由于系統(tǒng)模型不準確導(dǎo)致的動力學(xué)誤差。近年來，楊元喜等[11]中國學(xué)者建立了一種自適應(yīng)抗差濾波理論，該方法在標準線性Kalman濾波中效果顯著。延續(xù)此思路，針對高動態(tài)、強機動彈體運動情形下深組合導(dǎo)航系統(tǒng)，本文從抗差調(diào)節(jié)觀測噪聲、自適應(yīng)因子調(diào)節(jié)協(xié)方差陣兩步構(gòu)造自適應(yīng)抗差容積卡爾曼濾波。

2.2.1 抗差濾波算法

為抵制觀測粗差的影響，需要實時調(diào)節(jié)觀測噪聲。在標準CKF基礎(chǔ)上，對式(25)進行調(diào)整修正：

(30)

(31)

(32)

(33)

σvi=(Pyy,k|k-1)ii

(34)

(35)

抗差CKF算法是在標準CKF濾波方法的基礎(chǔ)上，對噪聲方差陣Rk進行在線修正，從而起到調(diào)節(jié)濾波增益陣，抵制導(dǎo)航系統(tǒng)觀測粗差，抑制濾波發(fā)散的作用。以下對系統(tǒng)中的潛在動力學(xué)誤差進行修正。

2.2.2 自適應(yīng)抗差濾波算法

自適應(yīng)濾波涉及到誤差判別統(tǒng)計量以及自適應(yīng)因子[12]。需要構(gòu)造動力學(xué)模型誤差學(xué)習統(tǒng)計量，即狀態(tài)不符值統(tǒng)計量、預(yù)測殘差統(tǒng)計量等，本文在2.2.1節(jié)中提出的抗差修正的基礎(chǔ)上，使預(yù)測殘差理論協(xié)方差矩陣等于或約等于估計的預(yù)測殘差協(xié)方差矩陣，求出自適應(yīng)因子，并修正一步預(yù)測協(xié)方差陣Pk|k-1，如下：

(36)

自適應(yīng)因子αk由如下方法確定：

αk=

(37)

(38)

下面，總結(jié)自適應(yīng)抗差CKF算法步驟：

1)由式(14)和(15)給定濾波初始值；

5)自適應(yīng)調(diào)節(jié)一步預(yù)測協(xié)方差矩陣。由式(30)和式(36)～(38)完成對Pk|k-1的自適應(yīng)調(diào)節(jié)。應(yīng)注意：由式(37)得出自適應(yīng)因子αk后進行判斷，若αk值等于1，則進行標準CKF量測更新的余下步驟：式(26)～(29)，濾波結(jié)束；若αk值小于1，則進入步驟6)；

3 ARCKF仿真實驗及結(jié)果分析

為了驗證GNSS/SINS深組合導(dǎo)航中自適應(yīng)抗差容積卡爾曼濾波器(ARCKF)的優(yōu)越性，本文在開發(fā)的全軟化仿真平臺系統(tǒng)中進行深組合導(dǎo)航仿真實驗，并與標準卡爾曼濾波(KF)效果進行比較。本實驗系統(tǒng)由飛行軌跡發(fā)生器、衛(wèi)星信號模擬器、軟件接收機、慣導(dǎo)解算模塊及組合模塊等幾個部分組成。仿真條件設(shè)置如下：彈體初始俯仰角為90°，初始滾轉(zhuǎn)角和偏航角均為0°；初始姿態(tài)誤差均設(shè)為0°；初始位置為：緯度31.98°，經(jīng)度118.8°，高度0m；發(fā)射慣性坐標系下初始速度前向為394.8917m/s(地球自轉(zhuǎn)速度)，天向和側(cè)向均為0m/s；發(fā)射方位角為90°；60～68s間最大加速度達到60g，完全符合強機動、高動態(tài)特性。發(fā)射慣性系下超緊組合導(dǎo)航仿真系統(tǒng)的仿真參數(shù)設(shè)置如表1所示，得到的飛行軌跡如圖1所示。

表1 仿真參數(shù)設(shè)置

圖1 發(fā)射點慣性系下的飛行軌跡

圖2 X方向速度誤差

圖3 Y方向速度誤差

圖4 Z方向速度誤差

圖2～4為X，Y，Z三個方向的速度誤差對比圖。由上述仿真結(jié)果可以看出，在非高動態(tài)時段，即在0～60s仿真時段，自適應(yīng)抗差容積卡爾曼濾波(ARCKF)與卡爾曼濾波(KF)有較相似的仿真結(jié)果；在高動態(tài)時段60～68s，彈體加速度最高達到60g，觀察KF濾波X，Y，Z三個方向的速度誤差最大依次達到-1.5m/s，-0.25m/s，-1.1m/s；ARCKF濾波的X，Y，Z三個方向的速度誤差依次為-0.7m/s，0.15m/s，-0.2m/s，誤差絕對值均比KF誤差要小，較KF濾波結(jié)果有一定改善。由此可證明，本文所提出的自適應(yīng)抗差CKF算法有效提高了導(dǎo)航系統(tǒng)在高動態(tài)、強機動條件下的濾波性能。

4 結(jié)論

針對彈道導(dǎo)彈GNSS/SINS深組合導(dǎo)航系統(tǒng)的強機動、高動態(tài)特性，設(shè)計了一種適用于非線性系統(tǒng)的自適應(yīng)抗差容積卡爾曼(ARCKF)組合導(dǎo)航濾波器。仿真結(jié)果表明，與卡爾曼濾波相比，該濾波方法在載體機動較強的情形下，具有更優(yōu)越的魯棒性能，進一步提高了組合導(dǎo)航在復(fù)雜環(huán)境中的跟蹤性能與導(dǎo)航精度。

[1] 李子月. 高動態(tài)下GPS/SINS超緊組合導(dǎo)航系統(tǒng)研究[D]. 浙江大學(xué), 2013. (Li Ziyue. The Research of Ultra-Tightly Coupled GPS/SINS Intergration System in High Dynamic Conditions [D]. Zhe Jiang University, 2013.)

[2] 王小旭,潘泉,黃鶴,等.非線性系統(tǒng)確定采樣型濾波算法綜述[J].控制與決策,2012, 6(27): 801-812. (Wang Xiaoxu, Pan Quan, Huang He, et al. Overview of Deterministic Sampling Filtering Algorithms for Nonlinear System[J]. Control and Decision, 2012, 6(27): 801-812.)

[3] 趙琳, 王小旭, 丁繼成, 等. 組合導(dǎo)航系統(tǒng)非線性濾波算法綜述[J]. 中國慣性技術(shù)學(xué)報, 2009, 17(1): 46-52. (Zhao Lin, Wang Xiaoxu, Ding Jicheng, et al. Overview of Nonlinear Filter Methods Applied in Integrated Navigation System[J]. Journal of Chinese Insertial Technology, 2009, 17(1): 46-52.)

[4] Arasaratnam I, Simon H. Cubature Kalman Filters[J]. IEEE Transactions on Automatic Control, 2009, 54(6): 1254-1269.

[5] Arasaratnam I, Simon H, Hurd T R. Cubature Kalman Filtering for Continuous Discrete Systems: Theory and Simulations[J]. IEEE Transactions on Signal Processing, 2010, 58(10): 4977-4993.

[6] 趙欣, 王仕成, 廖守億, 等. 基于抗差自適應(yīng)容積卡爾曼濾波的超緊耦合跟蹤方法[J]. 自動化學(xué)報, 2014, 40(11): 2530-2540. (Zhao Xin, Wang Shicheng, Liao Shouyi, et al. An Ultra-tightly Coupled Tracking Method Based on Robust Adaptive Cubature Kalman Filter[J]. Acta Automation Sinica, 2014, 40(11): 2530-2540.)

[7] 楊洋. GPS/INS 深組合導(dǎo)航中關(guān)鍵技術(shù)研究[D]. 南京: 南京理工大學(xué), 2013. (Yang Yang. Research on Key Technologies of SINS/GPS Deeply Coupled Navigation System [D]. Nanjing: Nanjing University of Science & Technology, 2013.)

[8] 董亮, 陳帥, 韓乃龍. 發(fā)射慣性系下GPS/SINS組合導(dǎo)航方法研究[J]. 電光與控制, 2016, 23(7):11-14. (Dong Liang, Chen Shuai, Han Nailong. GPS/SINS Integrated Navigation in Launch Inertial Coordinate System[J]. Electronics Opptics & Control, 2016, 23(7): 11-14.)

[9] 秦永元, 張洪鉞, 汪叔華. 卡爾曼濾波與組合導(dǎo)航原理(第2版)[M]. 西安: 西北工業(yè)大學(xué)出版社, 2012. (Qin Yongyuan, Zhang Hongyue, Wang Shuhua. Kalman Filter and Integrated Navigation Principle (Second Edition)[M]. Xi’an:Northwestern Polytechnical University Press Lo. Ltd, 2012.)

[10] 王楠, 王龍. 慣性系下GPS/SINS緊組合導(dǎo)航算法研究[J]. 現(xiàn)代檢測技術(shù), 2011, 34(20): 93-96. (Wang Nan, Wang Long. Rrsearch of GPS/SINS Tightly Integrated Navigation Algorithm in Inertial Coordinate[J]. Modern Electronics Technique, 2011, 34(20): 93-96.)

[11] 楊元喜, 任夏, 許燕. 自適應(yīng)抗差濾波理論及應(yīng)用的主要進展[J]. 導(dǎo)航定位學(xué)報, 2013, 1(1): 9-15. (Yang Yuanxi, Ren Xia, Xu Yan. Main Progress of Adaptively Robust Filter with Applications in Navigation[J]. Journal of Navigation and Positioning, 2013, 1(1): 9-15.)

[12] 黃盼, 張曉, 藤云龍, 等. 基于AR-SRCKF的SINS/GPS深耦合抗轉(zhuǎn)發(fā)式干擾研究[J]. 計算機工程與應(yīng)用, 2016, 52(2): 31-35. (Huang Pan, Zhang Xiao, Teng Yunlong, et al. Research on Countermeasure for Repeater Jamming in Deeply Coupled SINS/GPS System Based on AR-SRCKF[J]. Computer Engineering and Applications, 2016, 52(2): 31-35.)

Research on the Method of Missile-loaded Deeply-Integrated ARCKF Filtering under Strong Maneuver

Wang Yiping1，Chen Shuai1， Zhao Chen1，Qu Xinfen2

1. School of Automation, Nanjing University of Science and Technology, Nanjing 210094, China 2. Institute of Electronic Engineering, China Academy of Engineering Physics, Mianyang 621000, China

AimingatthestrongmaneuveringandnonlinearcharacteristicsofGNSS/SINSdeeplyintegratednavigationsysteminballisticmissileflight,thecubatureKalmanfilter(CKF)basedonthird-degreespherical-radialcubatureruleisintroducedinthispaper.Meanwhile,anadaptiverobustcubatureKalmanfilter(ARCKF)isproposed.TherobustnessMisusedinthisalgorithmtoadjustthemeasurementnoisematrixtoresistsystemdisturbance,andtheadaptivefactorisusedtoadjustthecovariancematrixtofurtherdealwiththeerrorintroducedbythedynamicdisturbance.Theexperimentalresultsshowthatthedynamicperformanceofintegratednavigationsystemcanbeeffectivelyimprovedbyusingproposedalgorithm,andhighnavigationprecisionandtrackingperformancecanbemaintainedunderthestrongmaneuversimulationenvironmentwithaccelerationupto60g.

Ballisticmissile;Deeplyintegrated;Strongmaneuvering; ARCKF

*國家自然基金委員會和中國物理研究院聯(lián)合基金資助(U1330133);中央高校基本科研業(yè)務(wù)費專項資金資助(30916011336);江蘇博士后科研資助計劃(1501050B);中國博士后基金(2015M580434);中國博士后特別資助(2016T90461)

2017-01-19

汪益平(1990-)，男，安徽池州人，碩士研究生，主要研究方向為組合導(dǎo)航；陳 帥(1980-)，男，江蘇南通人，博士，副教授,碩士生導(dǎo)師，主要研究方向為導(dǎo)航、制導(dǎo)與控制；趙 琛(1991-)，男，湖北仙桃人，碩士研究生，主要研究方向為組合導(dǎo)航；屈新芬(1971-)，女，四川鄰水人，博士，副研究員，主要研究方向為系統(tǒng)辨識、信息融合及慣性傳感器研究和應(yīng)用。

V249.3

A

1006-3242(2017)03-0003-06