強(qiáng)機(jī)動(dòng)條件下的彈載深組合ARCKF濾波方法研究*

汪益平 陳 帥 趙 琛 屈新芬

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

強(qiáng)機(jī)動(dòng)條件下的彈載深組合ARCKF濾波方法研究*

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

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

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

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

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

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

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

與GNSS/SINS的松/緊組合相比，GNSS/ SINS深組合導(dǎo)航系統(tǒng)的顯著特征是信息融合程度的進(jìn)一步深入，涉及到GNSS接收機(jī)內(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ū)動(dòng)矩陣，wI(t)為SINS系統(tǒng)噪聲矩陣。各矩陣具體形式見文獻(xiàn)[8-9]。

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

深組合系統(tǒng)中，GNSS接收機(jī)的誤差狀態(tài)量通常取為：與時(shí)鐘頻率誤差等效的距離率誤差Δ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的時(shí)鐘誤差引起的距離誤差，vρ為偽距量測噪聲。ρ與ρi可由下式計(jì)算：

(8)

(9)

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

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

(10)

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

(11)

2 基于ARCKF的組合濾波算法

2.1 標(biāo)準(zhǔn)CKF濾波

根據(jù)第1節(jié)實(shí)際所建立的狀態(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é)方差陣，是對稱的非負(fù)定矩陣。

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

(1)濾波初始化

(14)

(15)

(2)時(shí)間更新

計(jì)算k-1時(shí)刻Cubature點(diǎn)：

(16)

(17)

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

(18)

(19)

(20)

(3)量測更新

計(jì)算用于量測更新的Cubatue點(diǎn)：

(21)

(22)

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

通過量測方程傳播Cubature點(diǎn)：

(23)

計(jì)算增益矩陣：

(24)

(25)

(26)

(27)

狀態(tài)估計(jì)值：

(28)

(29)

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

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

2.2.1 抗差濾波算法

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

(30)

(31)

(32)

(33)

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

(34)

(35)

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

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

自適應(yīng)濾波涉及到誤差判別統(tǒng)計(jì)量以及自適應(yīng)因子[12]。需要構(gòu)造動(dòng)力學(xué)模型誤差學(xué)習(xí)統(tǒng)計(jì)量，即狀態(tài)不符值統(tǒng)計(jì)量、預(yù)測殘差統(tǒng)計(jì)量等，本文在2.2.1節(jié)中提出的抗差修正的基礎(chǔ)上，使預(yù)測殘差理論協(xié)方差矩陣等于或約等于估計(jì)的預(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后進(jìn)行判斷，若αk值等于1，則進(jìn)行標(biāo)準(zhǔn)CKF量測更新的余下步驟：式(26)～(29)，濾波結(jié)束；若αk值小于1，則進(jìn)入步驟6)；

3 ARCKF仿真實(shí)驗(yàn)及結(jié)果分析

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

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

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

圖2 X方向速度誤差

圖3 Y方向速度誤差

圖4 Z方向速度誤差

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

4 結(jié)論

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

[1] 李子月. 高動(dòng)態(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é)報(bào), 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]. 自動(dòng)化學(xué)報(bào), 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ìn)展[J]. 導(dǎo)航定位學(xué)報(bào), 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]. 計(jì)算機(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

*國家自然基金委員會(huì)和中國物理研究院聯(lián)合基金資助(U1330133);中央高?；究蒲袠I(yè)務(wù)費(fèi)專項(xiàng)資金資助(30916011336);江蘇博士后科研資助計(jì)劃(1501050B);中國博士后基金(2015M580434);中國博士后特別資助(2016T90461)

2017-01-19

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

V249.3

A

1006-3242(2017)03-0003-06