呂建強 鄧博煒 葉 松 高曉穎,3

1.北京航天自動控制研究所，北京 100854 2.北京科技大學(xué)自動化學(xué)院，北京 100083 3.宇航智能控制技術(shù)國家級重點實驗室，北京 100854

?

組合導(dǎo)航濾波器的在線初值設(shè)置及收斂判定方法

呂建強1鄧博煒2葉 松1高曉穎1,3

1.北京航天自動控制研究所，北京 100854 2.北京科技大學(xué)自動化學(xué)院，北京 100083 3.宇航智能控制技術(shù)國家級重點實驗室，北京 100854

組合導(dǎo)航濾波時需要設(shè)置初值并對濾波是否收斂進行判斷。針對初值設(shè)置問題，提出一種根據(jù)濾波模型遞推值對狀態(tài)變量協(xié)方差陣在線初始化的方法；針對判斷收斂問題，提出了一種利用狀態(tài)變量協(xié)方差陣進行在線判斷濾波是否收斂的方法。通過實時計算狀態(tài)量協(xié)方差的相對值，得出收斂條件，并給出收斂程度。最后通過數(shù)學(xué)仿真，驗證了本文方法的有效性。

組合導(dǎo)航；卡爾曼濾波；分段濾波；在線收斂判定；在線初值設(shè)置

組合導(dǎo)航是提高導(dǎo)航精度和可靠性，彌補單一導(dǎo)航方式缺陷的有效方法。慣性導(dǎo)航和衛(wèi)星導(dǎo)航因為其互補的特點被稱為黃金組合[1]。對于捷聯(lián)慣導(dǎo)與衛(wèi)星導(dǎo)航的松耦合，在某些特定條件下，觀測值會出現(xiàn)短時間或者較長時間的中斷。進行組合導(dǎo)航計算時，如果GPS信號中斷時間過長(10s以上)，信號恢復(fù)后再直接進行濾波，濾波器會產(chǎn)生發(fā)散現(xiàn)象[2]。對于這種情況，可以分段進行組合導(dǎo)航，即認為在衛(wèi)星信號中斷之前進行了1次組合，在信號恢復(fù)之后又進行了1次組合。

分段進行組合導(dǎo)航時，將面臨2個問題：1)再次進行組合導(dǎo)航時，濾波初值如何設(shè)置；2)第1次組合導(dǎo)航時，濾波是否收斂以及收斂的程度。

針對組合導(dǎo)航初值的設(shè)置問題，有學(xué)者做了大量的研究，給出了設(shè)置狀態(tài)協(xié)方差陣初值、系統(tǒng)噪聲協(xié)方差陣和觀測噪聲協(xié)方差陣的經(jīng)驗設(shè)置方法[4]。這種設(shè)置方法在首次組合導(dǎo)航濾波時被實踐證明是有效的。但是再次進行濾波時，根據(jù)經(jīng)驗進行線下設(shè)置的初值很不準確，這將影響濾波的收斂速度[4]。

針對如何在線判斷濾波收斂和發(fā)散的問題，目前的研究比較少，工程上通常根據(jù)經(jīng)驗以時間作為收斂條件，這種判斷方法一般比較保守[6]，而且在面對觀測值突然中斷的情況時，無法進行有效的判斷[6]。

針對初值設(shè)置問題，本文在所述的特定條件下提出了一種根據(jù)濾波模型遞推值對狀態(tài)變量協(xié)方差陣在線初始化的方法?？紤]到文獻[4]和[5]均指出適當放大狀態(tài)變量協(xié)方差陣有利于提高收斂速度。本文將在上述初值的基礎(chǔ)上乘以與時間相關(guān)的放大倍數(shù)，得到實際使用的狀態(tài)變量協(xié)方差陣的初值。針對在線收斂的判定問題，本文假設(shè)所用濾波器滿足文獻[7]所述的3條假設(shè)和4個定理，并根據(jù)文獻[7]所定義的濾波過程穩(wěn)定性，通過計算狀態(tài)量協(xié)方差的相對值，得到了在線判定濾波收斂的條件，并定量給出了濾波收斂程度。

1 慣導(dǎo)與衛(wèi)星組合導(dǎo)航濾波模型

本文采用發(fā)射點重力慣性坐標系，選擇速度誤差、位置誤差、姿態(tài)角誤差、加速度計零漂和陀螺零漂作為狀態(tài)變量[9]，建立捷聯(lián)慣導(dǎo)與衛(wèi)星(SINS/GNSS)的松耦合濾波模型，如式(1)和(2)所示，

(1)

Z(t)=H(t)X(t)+v(t)

(2)

其中，

X(t)=[δV,δr,δf,δK,δD]T

(3)

w(t)=[▽x1,▽y1,▽z1,εx1,εy1,εz1]T

(4)

v(t)=[ΔVgx,ΔVgy,ΔVgz,Δrgx,Δrgy,Δrgz]T

(5)

式中，狀態(tài)向量X(t)中δVx,δVy,δVz為速度誤差；drx,dry,drz為位置誤差；δfx,δfy,δfz為姿態(tài)角誤差；δK0x,δK0y,δK0z為加速度零位誤差；δD0x,δD0y,δD0z為陀螺常值誤差；系統(tǒng)白噪聲w(t)中▽x1,▽y1,▽z1為加速度計的噪聲，εx1,εy1,εz1為陀螺的噪聲；觀測向量Z(t)中δVx,δVy,δVz為慣性解算速度與衛(wèi)星測得的速度之差；δrx,δry,δrz為慣性解算位置與衛(wèi)星測得的位置之差；觀測白噪聲v(t)中ΔVgx,ΔVgy,ΔVgz為衛(wèi)星導(dǎo)航的速度精度誤差，Δrgx,Δrgy,Δrgz為衛(wèi)星導(dǎo)航的位置精度誤差。

將上述模型離散化[11]，得

Xk+1=Φk+1,kXk+Γkwk

(6)

Zk+1=Hk+1Xk+1+vk+1

(7)

其中，狀態(tài)轉(zhuǎn)移矩陣，

(8)

系統(tǒng)輸入矩陣，

(9)

系統(tǒng)噪聲協(xié)方差矩陣，

(10)

觀測噪聲協(xié)方差陣

(11)

本文采用如下的濾波公式[5]，

(12)

(13)

(14)

(15)

Pk+1,k+1=Pk+1,k-Kk+1Hk+1Pk+1,k

(16)

2 狀態(tài)協(xié)方差陣的初值設(shè)置方法

狀態(tài)協(xié)方差陣P中主對角線元素實際上反映了相應(yīng)狀態(tài)變量的精度[5]。在捷聯(lián)慣性與衛(wèi)星組合導(dǎo)航中，P中主對角線元素反映了慣導(dǎo)的誤差積累情況。而濾波模型中的狀態(tài)變量實際上是慣導(dǎo)的誤差量，其與P中主對角線元素的開平方量綱一致。由此，在不進行組合導(dǎo)航的時間段內(nèi)，考慮式(12)，如果繼續(xù)遞推，則得到當前時刻的誤差。雖然這樣得到的誤差不夠準確，但也在一定程度上反映誤差的大小及變化方向。取

(17)

本文濾波模型的建立過程中，忽略了二階及以上的小量，因此是一種近似的模型。由其遞推得到的狀態(tài)變量隨著時間的增加，會越來越不準確。另外，目前的捷聯(lián)慣導(dǎo)遞推算法[9]本身會帶來一定的誤差，綜合考慮上述因素，可以根據(jù)經(jīng)驗離線設(shè)置μ(τ)。對μ(τ)影響較大的因素是彈道和慣性儀表的誤差參數(shù)。由于本文所用彈道變化比較平緩，在此條件下，慣性儀表誤差對μ(τ)的影響比較明顯。

記第l次濾波結(jié)束到第l+1次濾波啟動時間間隔為τ。根據(jù)本文中多次仿真結(jié)果，中斷時間τ在百秒級別時，這里取線性方式即可得到較好的效果，即

μ(τ)=Aτ+B

(18)

式中，A=[a1,a2,…an]T，B=[b1,b2,…bn]T。A中與速度對應(yīng)的參數(shù)可參考慣性儀表誤差指標，與位置對應(yīng)的參數(shù)可在速度基礎(chǔ)上縮小τ倍。當τ達百秒時，B可取為0；當τ比較小時，B可取小于1的數(shù)值，限制P0的過度膨脹。

由上述方法得到實際使用的狀態(tài)協(xié)方差陣，

(19)

3 濾波收斂的在線判定方法

隨著濾波次數(shù)的增加，狀態(tài)協(xié)方差矩陣趨于0或者趨于穩(wěn)定，同時系統(tǒng)誤差不再減小，則判斷濾波收斂，如圖1所示。本文假設(shè)濾波器滿足文獻[7]所述的假設(shè)條件，所以只需要考查狀態(tài)協(xié)方差陣P，就可以判斷濾波是否收斂。

圖1 收斂曲線示意圖

記狀態(tài)協(xié)方差陣P的主對角線元素為p1,p2,…,pn。其中,pi(1≤i≤n)描述了第i個狀態(tài)的精度，記pi(k)為pi第k時刻的值。

由于濾波收斂后，P的大小幾乎不再變化，即其增量接近0。但是由于狀態(tài)變量量綱的不同，其對應(yīng)的pi在數(shù)值上差別很大。這里將pi進行無量綱化處理

(20)

式中，1≤i≤n。當pi的增量接近0時，ηi接近數(shù)值1。判斷ηi與數(shù)值1的接近程度，即可以判斷濾波是否收斂。在濾波收斂過程中，ηi的值將從0逐漸接近1。ηi實際上反映了濾波的實時收斂情況，可以用來表征收斂程度。由上述分析可以得到

(21)

將上述方法概括為判定方法1：隨著濾波次數(shù)k的增加，當滿足ηi(k)-1≤εi時，則判定第i個狀態(tài)已經(jīng)收斂，當所有進行濾波修正的狀態(tài)都收斂時，認為濾波收斂。其中,εi是根據(jù)實際需要設(shè)置的一個較小閾值，ηi的定義如式(20)所示。

實際飛行中，觀測值可能出現(xiàn)野值，實際噪聲也可能出現(xiàn)尖峰等現(xiàn)象，這些都可能使P陣發(fā)生跳變。此時計算出的ηi值也會發(fā)生跳變，導(dǎo)致收斂誤判。為了避免ηi突然跳變造成的誤判，需要收斂判定方法2：當連續(xù)出現(xiàn)Ni個ηi小于預(yù)先設(shè)置的閾值εi時，則判定第i個量收斂。當所有進行修正的狀態(tài)變量收斂時，則判定濾波收斂。其中，參數(shù)Ni根據(jù)需要進行預(yù)先設(shè)置，εi的含義同判定方法1。

判定方法1與2的關(guān)系：εi固定的條件下，Ni越大，則判定的收斂時間越長，需要根據(jù)導(dǎo)航的需要進行設(shè)置；Ni固定的條件下，εi越大，則判定收斂的時間越短，但是εi不能太大，否則失去了判斷收斂的功能。通過調(diào)節(jié)參數(shù)Ni和εi，可以靈活的得到期望的收斂時刻，如圖1所示的3個時刻。

4 仿真驗證

仿真條件：慣導(dǎo)計算周期為0.02s，濾波周期為

1s，仿真時間1000s，濾波器啟動2次，第1次啟動時間在起飛后200s，第2次啟動時間在起飛后800s。觀測值噪聲方差陣真實值與仿真值均設(shè)為：

R=[0.052,0.052,0.052,52,52,52]T；

系統(tǒng)噪聲方差陣真實值與仿真值均設(shè)為：

Q=[4E-6,4E-6,4E-6,2.5E-9,
2.5E-9,2.5E-9]T

濾波收斂判定條件：

N=[5,9,9,5,5,5]T，
ε=[0.05,0.05,0.05,0.001,0.001,0.001]T。

A=[1.8E-5,1.8E-5,1.8E-5,
1.8E-7,1.8E-7,1.8E-7]T，
B=[0,0,0,0,0,0]T。

4.1 驗證濾波的狀態(tài)協(xié)方差陣初值設(shè)置方法

2.76E-8,2.33E-6,2.21E-8,
3.88E-11,4.41E-8,3.99E-8,
1.27E-15,1.52E-14,1.30E-17])

圖2 速度誤差曲線

圖3 位置誤差曲線

4.2 驗證濾波收斂判定方法

判斷濾波的收斂時刻。從200s開始持續(xù)濾波到400s，得到的相應(yīng)速度位置方差如圖4和5所示。由判定方法進行濾波收斂判斷可以得到x向、y向和z向速度和位置分別在第8次、第9次、第35次、第17次、第12次濾波時收斂。最終得到本次濾波在第35次修正后，速度位置均已收斂。相應(yīng)的速度位置誤差隨時間的變化曲線如圖6和7所示。

濾波中斷時，判斷狀態(tài)變量的收斂程度。假設(shè)進行了3次濾波后，觀測值突然消失，考查ηi值，得到3個方向的速度位置收斂程序如下η=[0.7604,0.9960,1.0047,0.7860,0.8166,0.8186]T

此時對應(yīng)的速度位置誤差分別為：-0.0536m/s, -0.0797m/s, 0.0862m/s, -2.1194m, -3.7508m, 0.8357m。雖然誤差已經(jīng)減小，但是由局部誤差圖(圖8和9)可以看出，濾波仍然存在震蕩，并沒有穩(wěn)定收斂。η較好的表征了每個量收斂的程度。

圖4 速度誤差收斂曲線

圖5 位置誤差收斂曲線

圖6 速度誤差隨時間變化曲線

圖7 位置誤差隨時間變化曲線

圖8 速度誤差局部放大曲線

圖9 位置誤差局部曲線

5 結(jié)論

以SINS/GNSS組合導(dǎo)航分段濾波為背景，提出了一種在線判定濾波收斂的方法和狀態(tài)變量協(xié)方差陣的初值設(shè)置方法。根據(jù)濾波遞推模型，給出了在特定條件下再次濾波狀態(tài)協(xié)方差陣初值的參考值。通過計算卡爾曼濾波狀態(tài)協(xié)方差陣不同時刻的相對值，提出了在線判斷濾波收斂的判定方法，并定量的給出了不同時刻的濾波收斂程度。仿真結(jié)果表明，本文所述方法是合理有效的。

[1] 董緒榮, 張守信, 陳哲.GPS/INS組合導(dǎo)航定位及其應(yīng)用[M]. 長沙：國防科技大學(xué)出版社，1998.(DongXurong,ZhangShouxin,ChenZhe.GPS/INSIntegratedNavigationandPositioningandItsApplications[M].Changsha:NationalDefenseTechnologyPress,1998.)

[2] 李增科, 王堅, 高井祥, 譚興龍.利用SVM的GPS/INS組合導(dǎo)航濾波發(fā)散抑制方法研究[J]. 武漢大學(xué)學(xué)報自然科學(xué)版, 2013, 38(10): 1216-1220.(LIZengke,WangJian,GaoJingxiang,TanXinglong.AMethodtoPreventGPS/INSIntegratedNavigationFilteringDivergenceBasedonSVM[J].GeomaticsandInformationScienceofWuhanUniversity, 2013, 38(10): 1216-1220.)

[3]AndersonBDO.StabilitypropertiesofKalman-Bucyfilters[J].JournalofFranklinInstitute, 1971, 291(2): 137-144.

[4] 程向紅, 鄭梅.捷聯(lián)慣導(dǎo)系統(tǒng)初始對準中Kalman參數(shù)優(yōu)化方法[J].中國慣性技術(shù)學(xué)報, 2006, 14(4): 12-17.(ChengXianghong,ZhengMei.OptimizationonKalmanFilterParametersofSINSDuringInitialAlignment[J].JournalofChineseInertialTechnology, 2006, 14(4): 12-17.)

[5] 秦永元, 張洪鉞, 汪叔華.卡爾曼濾波與組合導(dǎo)航原理[M].西安：西北工業(yè)大學(xué)出版社，2012.(QinYongyuan,ZhangHongyue,WangShuhua.TheoryofKalmanFilteringandIntegratedNavigation[M].Xi′an:NorthwesternPolytechnicalUniversityPress, 2012.)

[6] 朱立華, 程向紅, 何應(yīng)云.傳遞對準中濾波器的收斂判據(jù)[J]. 中國慣性技術(shù)學(xué)報, 2011, 19(3): 277-285.(ZhuLihua,ChengXianghong,HeYingyun.FilterConvergenceCriterioninTransferAlignment[J].JournalofChineseInertialTechnology, 2011, 19(3): 277-285.)

[7] 邱凱, 黃國榮, 陳天如, 楊亞莉.卡爾曼濾波過程的穩(wěn)定性研究[J].系統(tǒng)工程與電子技術(shù), 2005, 27(1): 33-35.(QiuKai,HuangGuorong,ChenTianru,YangYali.StudyoftheStabilityofKalmanFilteringProcess[J].SystemsEngineeringandElectronics, 2005, 27(1): 33-35.)

[8] 邱愷, 黃國榮, 陳天如, 楊亞莉.基于濾波過程的卡爾曼濾波發(fā)散判定方法[J].系統(tǒng)工程與電子技術(shù), 2005, 27(2): 229-231.(QiuKai,HuangGuorong,ChenTianru,YangYali.MethodofdivergencedetectionforKalmanfilterbasedonfilteringprocess[J].SystemsEngineeringandElectronics, 2005, 27(2): 229-231.)

[9]DavidTitterton,JohnWeston.StrapdownInertialNavigationTechnology[M].AmerInstofAeronautics, 2004.

[10] 胡小平. 自主導(dǎo)航理論與應(yīng)用[M].長沙：國防科技大學(xué)出版社, 2002. (HuXiaoping.PrinciplesandApplicationsofAutonomousNavigation[M].Changsha:NationalUniversityofDefenseTechnologyPress, 2002.)

[11]Chi_TsongChen.Linersystemtheoryanddesign[M].NewYork:OxfordUniversityPress, 1999.

A Method of Online Setting up Initial Values and Online Convergence Criterion of Integrated Navigation Filter

Lv Jianqiang1, Deng Bowei2, Ye Song1, Gao Xiaoying1,3

1.Beijing Aerospace Automatic Control Institute, Beijing 100854, China 2.School of Automation Engineering, University of Science and Technology Beijing, Beijing 100083, China 3.National Key Laboratory of Science and Technology on Aerospace Intelligent Control, Beijing 100854, China

Theinitialvaluesandjudgingconvergencearerequiredduringintegratednavigationfiltering.Regardingthesetupoftheinitialvalues,anonlinemethodbasedonfilteringmodel’srecursivevaluesisproviedtosetthecovariancematrixofstatevariables.Fortheproblemofhowtojudgeconvergence,anewmethodispresentedbyusingstatevariablecovariancematrix.Bycalculatingtherelativevaluesofstatevariablecovarianceinrealtime,convergencecriterionandthedegreeofconvergenceareobtained.Intheend,mathematicalsimulationisusedtoverifythevalidityofthenewmethods.

Integratednavigation; Kalmanfilter;Segmentfiltering;Onlineconvergencecriterion;Onlinesettinginitialvalues

2015-11-25

呂建強(1988-)，男，山東東營人，碩士研究生，主要研究方向為飛行器導(dǎo)航與制導(dǎo)；鄧博煒(1995-)，男，本科生，自動化專業(yè)；葉 松(1979-)，男，安徽南陵人，碩士，高級工程師，主要研究方向為飛行器制導(dǎo)與控制；高曉穎(1969-)，男，河北深縣人，博士，研究員，主要研究方向為飛行器制導(dǎo)與控制。

TN967.2

A

1006-3242(2016)03-0056-06