組合導航濾波器的在線初值設置及收斂判定方法

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

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

?

組合導航濾波器的在線初值設置及收斂判定方法

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

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

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

組合導航；卡爾曼濾波；分段濾波；在線收斂判定；在線初值設置

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

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

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

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

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

1 慣導與衛星組合導航濾波模型

本文采用發射點重力慣性坐標系，選擇速度誤差、位置誤差、姿態角誤差、加速度計零漂和陀螺零漂作為狀態變量[9]，建立捷聯慣導與衛星(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)

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

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

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

(6)

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

(7)

其中，狀態轉移矩陣，

(8)

系統輸入矩陣，

(9)

系統噪聲協方差矩陣，

(10)

觀測噪聲協方差陣

(11)

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

(12)

(13)

(14)

(15)

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

(16)

2 狀態協方差陣的初值設置方法

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

(17)

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

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

μ(τ)=Aτ+B

(18)

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

由上述方法得到實際使用的狀態協方差陣，

(19)

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

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

圖1 收斂曲線示意圖

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

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

(20)

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

(21)

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

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

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

4 仿真驗證

仿真條件：慣導計算周期為0.02s，濾波周期為

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

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

系統噪聲方差陣真實值與仿真值均設為：

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 驗證濾波的狀態協方差陣初值設置方法

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

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

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

圖4 速度誤差收斂曲線

圖5 位置誤差收斂曲線

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

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

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

圖9 位置誤差局部曲線

5 結論

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

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

[2] 李增科, 王堅, 高井祥, 譚興龍.利用SVM的GPS/INS組合導航濾波發散抑制方法研究[J]. 武漢大學學報自然科學版, 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] 程向紅, 鄭梅.捷聯慣導系統初始對準中Kalman參數優化方法[J].中國慣性技術學報, 2006, 14(4): 12-17.(ChengXianghong,ZhengMei.OptimizationonKalmanFilterParametersofSINSDuringInitialAlignment[J].JournalofChineseInertialTechnology, 2006, 14(4): 12-17.)

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

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

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

[8] 邱愷, 黃國榮, 陳天如, 楊亞莉.基于濾波過程的卡爾曼濾波發散判定方法[J].系統工程與電子技術, 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] 胡小平. 自主導航理論與應用[M].長沙：國防科技大學出版社, 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-)，男，山東東營人，碩士研究生，主要研究方向為飛行器導航與制導；鄧博煒(1995-)，男，本科生，自動化專業；葉 松(1979-)，男，安徽南陵人，碩士，高級工程師，主要研究方向為飛行器制導與控制；高曉穎(1969-)，男，河北深縣人，博士，研究員，主要研究方向為飛行器制導與控制。

TN967.2

A

1006-3242(2016)03-0056-06