SR-CDKF在組合導航直接法濾波中的應用

逯 嶠,馬國梁,張 平,葛敬飛

(1.南京理工大學 能源與動力工程學院,江蘇 南京 210094;2.中國人民解放軍63863部隊,吉林 白城137001;3.中國人民解放軍65156部隊,遼寧 凌源 122521)

SR-CDKF在組合導航直接法濾波中的應用

逯 嶠1,馬國梁1,張 平2,葛敬飛3

(1.南京理工大學 能源與動力工程學院,江蘇 南京 210094;2.中國人民解放軍63863部隊,吉林 白城137001;3.中國人民解放軍65156部隊,遼寧 凌源 122521)

為了解決SINS/GPS組合導航系統姿態、速度和位置等導航參數的非線性估計問題，提出了一種基于平方根中心差分卡爾曼濾波(SR-CDKF)的直接式濾波估計方法。系統狀態方程選取慣導力學編排方程,系統狀態向量和觀測向量分別選取SINS導航參數和GPS輸出參數,構建了SR-CDKF濾波器。該濾波器可以使導航參數動態過程得到直接反映,并使得直接式濾波計算流程得以實現。以MATLAB中areoblk_HL20模塊為基礎構建了仿真系統并進行了仿真。仿真結果表明,該算法具有較高的濾波精度、良好的濾波收斂性和穩定性,能使非線性模型下SINS/GPS組合導航系統的導航要求得到滿足。

組合導航;中心差分;卡爾曼濾波;直接校正

SINS/GPS組合導航系統是目前一種比較理想且應用廣泛的導航系統。組合導航系統信息處理方法有很多種,Kalman濾波及其擴展方法是目前應用比較廣泛的一種方法[1]。根據所選取的被估計系統狀態不同,估計方法分為以導航參數為狀態變量的直接法和以導航參數誤差量為狀態變量的間接法2種[2]。如果采用直接估計法,導航參數的動態變化過程可以被系統方程直接而準確地描述,在解算導航參數的同時又進行了濾波估計計算,比間接法簡化了計算。

毫無疑問,實際的SINS/GPS組合導航系統的數學模型是非線性方程,而擴展卡爾曼濾波(EKF)是解決非線性系統估計問題應用最多的一種方法。EKF的主要思路是采用Taylor展開將非線性方程線性化后再用標準Kalman濾波作處理,因此EKF的估計誤差會隨系統非線性程度增強而加大,甚至有時會導致計算發散[3-4],另外EKF需要求Jacobian矩陣,過程繁瑣且易出錯。

針對EKF的這些不足,由于一個高斯分布的逼近比一個非線性函數的逼近更容易,人們提出了Sigma-point卡爾曼濾波(SPKF)[5]。SPKF主要包括中心差分卡爾曼濾波(CDKF)和無跡卡爾曼濾波(UKF)。其中,CDKF的理論精度比UKF稍高,避開了復雜的Jacobian矩陣計算,更易于實現[6-8]。SR-CDKF不僅繼承了CDKF的優點,而且通過直接更新狀態變量協方差矩陣的平方根來進行濾波,計算量有所減少,提高了數值計算的穩定性[9-12],因此本文提出將SR-CDKF方法應用到SINS/GPS組合導航系統的導航參數估計中。

1 SR-CDKF算法原理及步驟

1.1 SR-CDKF原理

SR-CDKF的理論基礎是中心差分變換,中心差分變換根據數學中的插值理論來對非線性函數進行近似,將Taylor級數展開式中的一階和二階導數用中心差分來取代,非線性方程的導數用基于Sterling插值公式的多項式逼近,由此來解決解析微分運算問題[13]。

1.2 SR-CDKF濾波步驟

設狀態方程和量測方程具有一般形式:

xk=F(xk-1,uk-1,vk-1)

yk=H(xk,nk)

式中:xk為狀態變量,uk為系統輸入參數,vk和nk是期望為0且互不相關的高斯白噪聲,它們的協方差陣分別為Rv和Rn,即有:

設xk,vk和nk的維數分別為Lx、Lv和Ln,則執行SR-CDKF濾波計算的步驟如下[14]。

①確定濾波初值。

當k=0時,濾波初值為

式中:chol{*}表示對矩陣進行cholesky分解。

②計算時間更新所需要的sigma點集。

③確定權值。

④時間更新。

式中:qr(A)表示對矩陣A進行QR分解后得到的R矩陣下三角部分。

⑤計算量測更新所需要的sigma點。

⑥量測更新。

式中:

⑦計算增益和更新狀態。

式中:cholupdate{*}表示矩陣的cholesky更新。

2 SINS/GPS組合導航非線性模型

2.1 直接式SINS/GPS組合導航系統濾波器結構

在直接式SINS/GPS組合導航濾波器中[15],首先由慣性器件和GPS分別測量運載體的運動狀態,其次SR-CDKF濾波器接受慣性器件測得的角速率、比力和GPS測得的位置速度信息,經SR-CDKF濾波器處理計算后輸出組合導航系統的導航參數最優估計值,如圖1所示。

2.2 組合系統狀態方程

本文根據文獻[16],捷聯慣導系統(SINS)導航坐標系(n系)選用“北東地”地理坐標系建立的非線性姿態、速度和位置方程如下。

姿態方程為

(1)

(2)

陀螺儀測量值模型為

(3)

速度方程為

(4)

加速度計的測量模型為

(5)

位置方程為

(6)

式中:RN和RM分別為參考橢球卯酉圈和子午圈曲率半徑;L,λ,h分別為載體所在的緯度、經度和高度。

④關于陀螺常值漂移ε和加速度計零偏Δ,本文中用隨機常數來表示:

(7)

以式(1)～式(7)為狀態,選取組合導航系統方程狀態變量x(t)為

x(t)=(φθψvNvEvDLλh
εxεyεzΔxΔyΔz)T

因此,系統狀態方程可以表示為

2.3 觀測方程

量測值y取GPS輸出的速度參數和位置參數,即y=(vN,GPSvE,GPSvD,GPSLGPSλGPShGPS)T,基于前面所選取的系統方程狀態變量x和量測值y,觀測方程可寫成

yk=H(xk,nk)=HPVxk+nk

式中:HPV=(O6×3I6×3O6×6),yk和xk分別為y和x在k時刻的值,nk=(nvNnvEnvDnLnλnh)T為測量噪聲。

3 仿真分析

1)真值的生成。

為了對SR-CDKF濾波算法作進一步分析,本文采用MATLAB中aeroblk_HL20模塊中的子模塊6DOF(Euler Angles)產生的數據作為原始數據。由于該模塊輸出的運動參數均是在忽略地球自轉和地球曲率的假設下得出的,因此如果采用這些數據作為參考真值,需要將前述的慣性導航方程(即系統狀態方程)簡化,方程的非線性也會減弱,體現不出SR-CDKF方法相對于其他非線性濾波方法的優勢。另外,簡化的導航方程由于忽略了地球曲率,因此只適合于短距離導航,而完整的導航方程無此限制,拓寬了參考真值的數據適用范圍。綜上所述,需要將原始數據進行轉化,以得到完整的慣性導航方程下的運載體真實的運動參數,具體的轉換思路和方法參見文獻[16]。

2)仿真初始條件。

設定SINS和GPS的采樣周期分別為0.01 s和0.1 s,仿真時間70 s,設置仿真初始條件:初始滾轉角、俯仰角、偏航角誤差分別為3°,3°和5°;初始經度、緯度、高度誤差為10 m,10 m和20 m;初始北向、東向、地向速度誤差均為0.3 m/s。陀螺儀三軸白噪聲均方差均為0.1°/s;三軸常值漂移分別為-0.1 °/s,0.1 °/s和-0.1 °/s;加速計白噪聲均方差均為0.05 m/s2;三軸零偏分別為-0.01 m/s2,0.01 m/s2和-0.02 m/s2;GPS位置和速度精度分別取5 m和0.1 m/s,初始協方差矩陣P為對角陣,除了對角線元素,其他元素均為0,其對角線元素為

P(1,1)=P(2,2)=P(3,3)=52
P(4,4)=P(5,5)=P(6,6)=0.32
P(7,7)=P(8,8)=P(9,9)=102
P(10,10)=P(11,11)=P(11,12)=0.22
P(13,13)=P(14,14)=P(15,15)=0.32

3)仿真結果。

在前文給出的仿真條件下,由于UKF、CDKF仿真曲線與SR-CDKF仿真曲線比較接近,在同一幅圖中會相互重疊,不能明顯看出優劣,因此只給出了SINS/GPS組合導航系統基于SR-CDKF的濾波仿真曲線,如圖2～圖6所示。

從圖2～圖6可看出,組合導航系統的運載體姿態角參數在10 s左右就收斂了,速度和位置參數則收斂得更快,陀螺和加速度計零偏基本都在20 s內收斂,且各參數的計算均未發散,表明計算穩定,從一定程度上驗證了SR-CDKF方法的數值穩定性。從圖中還可看出,在有測量噪聲和固定零偏的情況下,各導航參數(姿態、速度、位置等)上下波動范圍較小,表明濾波計算的誤差比較小。

在仿真條件下,UKF、CDKF和SR-CDKF 3種估計方法得到的運動參數估計誤差均方差如表1～表5所示。

表1 姿態角估計誤差標準差比較

表2 速度估計誤差標準差比較

表3 位置估計誤差標準差比較

表4 陀螺儀漂移估計值標準差比較

表5 加速度計零偏估計值標準差比較

由誤差數據表1～表5看出,CDKF比UKF估計誤差標準差要小,精度稍微好一些,驗證了CDKF理論精度稍高于UKF這一事實。而總體上看SR-CDKF對各個運動參數的估計誤差的標準差均小于UKF和CDKF,即估計精度均高于CDKF和UKF,同時也表明SR-CDKF的數值穩定性好于UKF和CDKF。綜上,SR-CDKF能提高非線性系統的濾波精度,使復雜的非線性系統濾波估計問題得以有效解決,同時使組合導航系統具有較高的導航精度。

4 結論

本文采用平方根中心差分卡爾曼濾波方法,研究了直接式的SINS/GPS組合導航估計問題,分別以姿態、速度、位置和零偏等導航參數和GPS的速度和位置為系統狀態向量和觀測向量,將慣導力學編排方程作為系統狀態方程,以MATLAB中aeroblk_HL20模塊產生的數據經過適當轉換后作為仿真的參考真值,并在此基礎上生成SINS和GPS的測量值,經濾波器估計后得到系統輸出,即導航參數的最優估計值。仿真結果表明,SR-CDKF可以較好地處理非線性模型的濾波問題,擺脫了傳統的對非線性方程進行Taylor展開的線性化過程,計算量較小,且能保證濾波過程的數值穩定,簡化了濾波過程,使系統導航精度得到了提高。

[1] 付心如,徐愛功,孫偉.抗差自適應UKF的INS/GNSS組合導航算法[J].導航定位學報,2017,5(2):111-116.

FU Xin-ru,XU Ai-gong,SUN Wei.An INS/GNSS integrated navigation algorithm based on robust adaptive UKF[J].Journal of Navigation and Positioning,2017,5(2):111-116.(in Chinese)

[2] 孟陽,高社生,高兵兵,等.基于UKF的INS/GNSS/CNS組合導航最優數據融合方法[J].中國慣性技術學報,2016,24(6):746-751.

MENG Yang,GAO She-sheng,GAO Bing-bing,et al.UKF-based optimal data fusion method for integrated INS/GNSS/CNS[J].Journal of Chinese Inertial Technology,2016,24(6):746-751.(in Chinese)

[3] 袁贛南,趙自超,賈鶴鳴,等.CDKF算法在四旋翼飛行器組合導航系統中應用[J].吉林大學學報(信息科學版),2015,33(2):161-167.

YUAN Gan-nan,ZHAO Zi-chao,JIA He-ming,et al.CDKF algorithm applied to quadrotor integrated navigation system[J].Journal of Jilin University(Information Science Edition),2015,33(2):161-167.(in Chinese)

[4] 潘加亮,熊智,王麗娜,等.一種簡化的發射系下SINS/GPS/CNS組合導航系統無跡卡爾曼濾波算法[J].兵工學報,2015,36(3):484-491.

PAN Jia-liang,XIONG Zhi,WANG Li-na,et al.A simplified UKF algorithm for SINS/GPS/CNS integrated navigation system in launch inertial coordinate system[J].Acta Armamentarii,2015,36(3):484-491.(in Chinese)

[5] JULIER S,UHLMANN J,DURRANT-WHYTE H F.A new method for the nonlinear transformation of means and covariances in filters and estimators[J].IEEE Transactions on Automatic Control,2000,45(3):477-482.

[6] 賈鶴鳴,楊澤文,宋文龍.基于中心差分卡爾曼濾波的初始對準方法研究[J].森林工程,2016,32(6):66-70.

JIA He-ming,YANG Ze-wen,SONG Wen-long.Study on initial alignment method based on central difference Calman filter[J].Forest Engineering,2016,32(6):66-70.(in Chinese)

[7] 張召友,郝燕玲,吳旭.3種確定性采樣非線性濾波算法的復雜度分析[J].哈爾濱工業大學學報,2013,45(12):111-115.

ZHANG Zhao-you,HAO Yang-ling,WU Xu.Complexity analysis of three deterministic sampling nonlinear filtering algorithms[J].Journal of Harbin Institute of Technology,2013,45(12):111-115.(in Chinese)

[8] 韓萍,干浩亮,何煒琨,等.基于迭代中心差分卡爾曼濾波的飛機姿態估計[J].儀器儀表學報,2015,36(1):187-193.

HAN Ping,GAN Hao-liang,HE Wei-kun,et al.Iterated central difference Kalman filter based aircraft attitude estimation[J].Chinese Journal of Scientific Instrument,2015,36(1):187-193.(in Chinese)

[9] 陳曉威,張國毅,李彥志,等.基于SRCDKF的相位差變化率無源定位算法[J].電子信息對抗技術,2016,31(1):8-11.

CHEN Xiao-wei,ZHANG Guo-yi,LI Yan-zhi,et al.Passive location algorithm of phase difference rate of change based on SRCDKF[J].Electronic Warfare Technology,2016,31(1):8-11.(in Chinese)

[10] 賈鶴鳴,宋文龍,牟宏偉,等.ISR-CDKF在SINS大方位失準角初始對準中的應用[J].吉林大學學報(信息科學版),2013,31(2):196-202.

JIA He-ming,SONG Wen-long,MOU Hong-wei,et al.Application of ISR-CDKF in initial alignment of large azimuth misalignment in the SINS[J].Journal of Jilin University(Information Science Edition),2013,31(02):196-202.

[11] DUAN J,LIU D,YU H,et al.An improved fast SLAM algorithm for autonomous vehicle based on the strong tracking square root central difference Kalman filter[C]//2015 IEEE 18th International Conference on Intelligent Transportation Systems(ITSC).Spain:IEEE,2015:693-698.

[12] TANG X,YAN J,ZHONG D.Square-root sigma-point Kalman filtering for spacecraft relative navigation[J].Acta Astronautica,2010,66(5-6):704-713.

[13] 郝燕玲,牟宏偉.自適應平方根中心差分卡爾曼濾波算法在捷聯慣性導航系統大方位失準角初始對準中的應用[J].吉林大學學報(工學版),2013,43(1):261-266.

HAO Yang-ling,MOU Hong-wei.Application of adaptive SRCDKF in SINS initial alignment with large azimuth misalignment[J].Journal of Jilin University(Engineering and Technology Edition),2013,43(1):261-266.(in Chinese)

[14] HAO Y L,YANG J W,CHEN L,et al.Initial alignment of large azimuth misalignment based on square root central difference Kalman filter[J].Journal of Chinese Inertial Technology,2011,19(2):180-184.

[15] 王林,林雪原.基于UKF的GPS/SINS組合導航直接式濾波算法[J].導航定位與授時,2015(3):43-48.

WANG Lin,LIN Xue-yuan.Direct Kalman filtering algorithm for the GPS/SINS integrated navigation based on UKF[J].Navigation Positioning and Timing,2015(3):43-48.(in Chinese)

[16] 劉可可.基于Kalman濾波的滾轉彈運動參數GPS/IMU組合測量方法研究[D].南京:南京理工大學,2016.

LIU Ke-ke.Research on GPS/IMU integrated measurement method of spinning projectile motion parameters based on Kalman filter[D].Nanjing:Nanjing University of Science and Technology,2016.(in Chinese)

ApplicationofSR-CDKFinDirectFilteringofIntegratedNavigation

LU Qiao1,MA Guo-liang1,ZHANG Ping2,GE Jing-fei3

(1.School of Energy and Power Engineering,Nanjing University of Science and Technology,Nanjing 210094,China;2.Unit 63863 of PLA,Baicheng 137001,China;3.Unit 65156 of PLA,Lingyuan 122521,China)

In order to solve the nonlinear estimation problem of the navigation-parameters about SINS/GPS integrated navigation system such as attitudes,speeds and positions,a direct filtering estimation method was proposed based on Square Root Center Differential Kalman Filter(SR-CDKF).The system state vectors and observation vectors respectively consisted of SINS navigation-parameters and GPS output parameters,and the SR-CDKF filter was constructed.The filter can directly reflect the dynamic process of navigation parameters and realize the direct filtering calculation-process.Based on the areoblk_HL20 module in the MATLAB,the simulation system was built,and the simulation was carried out.The simulation results show that the algorithm has high filtering-accuracy,good convergence and stability of filtering,and the navigation requirements of SINS/GPS integrated navigation system under the condition of the nonlinear model can be satisfied.

integrated navigation;central difference;Kalman filter;direct correction

TP273

A

1004-499X(2017)04-0029-06

2017-05-10

逯嶠(1990-),男,碩士研究生,研究方向為卡爾曼濾波與組合導航。E-mail:lqnust900605@126.com。

馬國梁(1976-),男,副教授,博士,研究方向為導航、制導與控制。E-mail:mgl@njust.edu.cn。