一種新的INS/LOG組合導(dǎo)航算法

高　薪，卞鴻巍，劉文超

(海軍工程大學(xué) 導(dǎo)航工程系，湖北 武漢 430033)

一種新的INS/LOG組合導(dǎo)航算法

高薪，卞鴻巍，劉文超

(海軍工程大學(xué) 導(dǎo)航工程系，湖北 武漢 430033)

摘要：針對電磁計(jì)程儀引入的海流速度可能會(huì)導(dǎo)致INS/LOG組合導(dǎo)航系統(tǒng)的卡爾曼濾波器輸出發(fā)散問題，設(shè)計(jì)一種新的INS/LOG組合方式。以計(jì)程儀速度2次采樣的差分值(即速度增量)作為系統(tǒng)觀測量以抑制慢變海流的影響，采用基于延遲狀態(tài)卡爾曼濾波算法，推導(dǎo)這種組合方式的觀測方程，實(shí)現(xiàn)INS誤差的最優(yōu)估計(jì)。為抑制計(jì)程儀速度差分對其高頻噪聲的放大效應(yīng)，采用巴特沃斯低通濾波器對其輸出進(jìn)行平滑處理。通過對3種INS/LOG組合方式的仿真比較，驗(yàn)證該算法的有效性。

關(guān)鍵詞：組合導(dǎo)航；慣導(dǎo)；電磁計(jì)程儀；速度增量；延遲狀態(tài)卡爾曼濾波

0引言

慣性導(dǎo)航系統(tǒng)(INS)的誤差會(huì)隨著時(shí)間積累，長時(shí)間工作后因超差而無法滿足要求，因此須與其他導(dǎo)航設(shè)備組合才能得到精度更高的導(dǎo)航信息。目前，INS與GPS等衛(wèi)星導(dǎo)航系統(tǒng)組合方法的研究較成熟，效果也較好。但衛(wèi)星導(dǎo)航系統(tǒng)需依賴外界信息的支持，并且在水下導(dǎo)航場合無法使用。計(jì)程儀屬于自主式導(dǎo)航裝備，可為艦艇提供較高精度的速度信息，它可與慣導(dǎo)系統(tǒng)組成自主、隱蔽、可靠且精度較高的組合導(dǎo)航系統(tǒng)。

目前，得到廣泛應(yīng)用的計(jì)程儀與慣導(dǎo)的組合方案是通過設(shè)計(jì)阻尼網(wǎng)絡(luò)[1]，引入計(jì)程儀的速度信息對慣導(dǎo)水正回路進(jìn)行阻尼以抑制舒拉周期振蕩。慣導(dǎo)的水正回路是典型的二階臨界穩(wěn)定系統(tǒng)，在常值誤差源激勵(lì)下會(huì)產(chǎn)生舒拉周期形式的等幅振蕩(無阻尼振蕩)。而在陀螺隨機(jī)漂移、加速度計(jì)隨機(jī)誤差的激勵(lì)下，慣導(dǎo)系統(tǒng)的無阻尼振蕩幅值會(huì)隨著時(shí)間發(fā)散[2]，最終造成系統(tǒng)超差。采用經(jīng)典控制理論設(shè)計(jì)的阻尼網(wǎng)絡(luò)，可以改善水平回路的穩(wěn)定特性，使舒拉和付科周期性震蕩得到衰減。但這種方法只能抑制振蕩性誤差，對于常值或者隨時(shí)間增長的誤差量[1]，如經(jīng)度誤差隨時(shí)間增長的部分沒有抑制作用。

利用卡爾曼濾波器將慣導(dǎo)系統(tǒng)與計(jì)程儀進(jìn)行信息融合，不僅可以獲得與傳統(tǒng)阻尼網(wǎng)絡(luò)同樣的阻尼效果[3]，還可以進(jìn)一步提高系統(tǒng)的精度。目前，利用外部速度信息與慣導(dǎo)系統(tǒng)組合濾波的研究多是關(guān)于多普勒、聲相關(guān)等絕對計(jì)程儀的，如文獻(xiàn)[4]中建立了多普勒計(jì)程儀的誤差特性方程，并使用多普勒計(jì)程儀來設(shè)計(jì)組合導(dǎo)航系統(tǒng)，有效提高了系統(tǒng)精度。目前，仍在大量使用的電磁計(jì)程儀本身具有較高的測速精度，然而由于電磁計(jì)程只能提供對水速度，海流速度會(huì)嚴(yán)重影響組合系統(tǒng)的精度。但海流在較短的時(shí)間內(nèi)相對穩(wěn)定，流速、流向基本不變。因此，若將計(jì)程儀的2次采樣值進(jìn)行差分，就可消除慢變海流的影響，得到較為準(zhǔn)確的艦船對地速度增量。基于以上思想，本文以速度增量作為觀測量，建立系統(tǒng)觀測方程，并采用與之相應(yīng)的基于延遲狀態(tài)的卡爾曼濾波算法，建立了一種新的INS/LOG組合方式，以消除海流速度的影響，提高系統(tǒng)精度。

1狀態(tài)方程的建立

慣導(dǎo)的陀螺和加速度計(jì)誤差都可以用隨機(jī)常數(shù)加上白噪聲的形式表示[5-6]：

(1)

(2)

式中：εbi為陀螺常值漂移；▽Abi加速度計(jì)常值零偏；εwi為陀螺隨機(jī)漂移；wi為加速度計(jì)隨機(jī)誤差(i=x,y,z)。將εbi和▽Abi列為狀態(tài)量，則連同慣導(dǎo)7項(xiàng)誤差量一起構(gòu)成12維狀態(tài)量：

X=[δφ,δλ,δVx,δVy,φx,φy,φz,▽Abx,▽Aby,

εbx,εby,εbz]T。

(3)

式中：δφ和δλ分別為緯度、經(jīng)度誤差；δVx和δVy分別為東向、北向速度誤差；φx， φy， φz為平臺失準(zhǔn)角；▽Abx和▽Aby分別為東向、北向加速度計(jì)常值零偏；εbx,εby,εbz分別為東向、北向和天向陀螺常值漂移。則根據(jù)慣導(dǎo)誤差方程可得狀態(tài)方程為：

X)X(t)+W(t)。

(4)

其中狀態(tài)傳遞矩陣F(t)可參考文獻(xiàn)[7]，W(t)為系統(tǒng)噪聲：

W(t)=[0,0,wx,wy,εwx,εwy,εwz,0,0,0,0,0]。

(5)

2觀測方程的建立

2.1　觀測量的選取

電磁計(jì)程儀的測量值為艦船相對海水的速度，因而電磁計(jì)程儀輸出的速度值應(yīng)為艦船對地真實(shí)速度、海流速度及計(jì)程儀誤差的和：

Vj=Vr+Vf+δVj。

(6)

式中：Vj為計(jì)程儀測得的艦船對水速度；Vf為海流速度；Vr為艦船對地真實(shí)速度，δVj為計(jì)程儀測量誤差。

海水流速一般小于2 kn，但有些海流的流速可達(dá)4~5 kn以上[8]。根據(jù)海洋學(xué)原理，海樣是一個(gè)不可壓縮的流場，海流的空間尺度大，一般具有數(shù)百甚至數(shù)千千米的流動(dòng)范圍。在較短的時(shí)間內(nèi)海流相對穩(wěn)定，流速、流向基本不變[8]，因此可認(rèn)為采樣時(shí)間較短時(shí)，計(jì)程儀速度的2次采樣值中的海水流速不變。2次采樣值差分可得：

ΔVj(k)=Vj(k)-Vj(k-1)=

Vr(k)+Vf(k)+δVj(k)-[Vr(k-1)+

Vf(k-1)+δVj(k-1)]=[Vr(k)-Vr(k-1)]+

[δVj(k)+δVj(k-1)]。

(7)

計(jì)程儀速度差分后，已消除了海流的影響，ΔVj(k)較真實(shí)地反映了載體在1個(gè)采樣周期內(nèi)的對地速度增量[Vr(k)-Vr(k-1)]。但式(7)含有的[δVj(k)+δVj(k-1)]項(xiàng)可能會(huì)“放大”計(jì)程儀噪聲中的高頻成分，因而需要設(shè)計(jì)低通濾波器，消除計(jì)程儀輸出中的高頻成分，ΔVj(k)才能準(zhǔn)確表示艦艇的對地速度增量。

2.2　低通濾波器的設(shè)計(jì)

電磁計(jì)程儀測量誤差主要由零位誤差、刻度系數(shù)誤差和隨機(jī)誤差組成。文獻(xiàn)[6]對電磁計(jì)程儀誤差進(jìn)行了分析，并建立了電磁計(jì)程儀誤差的數(shù)學(xué)模型，將計(jì)程儀誤差分為常值誤差，一階馬爾科夫過程和白噪聲。在頻域上計(jì)程儀噪聲可分為低頻部分和高頻部分：

δVj=δVjL+δVjH。

(9)

式中：δVjL為計(jì)程儀誤差的低頻部分；δVjH為計(jì)程儀誤差的高頻部分。

巴特沃斯(Butterworth)濾波器是一種具有最大平坦響應(yīng)的低通濾波器，在實(shí)際工程中應(yīng)用廣泛[9]。巴特沃斯低通濾波器的幅頻特性為：

(10)

式中：n為濾波器階數(shù)；ωb為低通濾波器截止頻率。當(dāng)階次n增大時(shí)，濾波器的特性曲線會(huì)變得更加陡峭，其特性更接近理想的矩形幅頻特性，低頻信號的保真度越高。巴特沃斯低通濾波器的階數(shù)n由下式求得：

(11)

式中：αs為阻帶最小衰減；αp為通帶最大衰減；ωb為通帶截止頻率。顯然歸一化后，λp=ωb/ωb=1，λs=ωs/ωb，其中ωs為阻帶截止頻率。

由上分析可知：階數(shù)n越高，其幅頻特性越好 ，低頻檢測信號保真度越高。因此?？杀M量選擇階數(shù)較高的低通濾波器。設(shè)計(jì)低通數(shù)字濾波器的目的就是通過該濾波器獲得所需的低頻信號。為電磁計(jì)程儀的輸出信號設(shè)計(jì)低通濾波器的目的是為了獲取其 “直流”輸出，故其截止頻率應(yīng)為0。當(dāng)然，設(shè)計(jì)這種濾波器 ，不論在理論上還是在實(shí)踐上，都相當(dāng)困難。因此，本文的設(shè)計(jì)思想是歸一化后的頻率λs盡量小。

在船載條件下,大部分?jǐn)_動(dòng)頻率都在1/15 Hz以上[11]，為得到較為準(zhǔn)確的載體線運(yùn)動(dòng)信息，本文選擇ωb=0.1 Hz，ωs=0.2 Hz，帶通最大衰減為αp=3 dB，阻帶最小衰減為αs=35 dB。則由式(11)可算出n=3，將ωb， αp， n代入下式：

(12)

計(jì)算出濾波器3個(gè)極點(diǎn)分別為：

(13)

則可得巴特沃斯低通濾波器的傳遞函數(shù)為：

(14)

為驗(yàn)證低通濾波器效果，對上述濾波器進(jìn)行仿真，結(jié)果如圖1所示。由圖中可看出，本文設(shè)計(jì)的巴特沃斯低通濾波器可有效抑制信號中的高頻成分。

圖1　巴特沃斯低通濾波器輸出Fig.1　Result of Butterworth low pass filter

2.3　建立量測方程

由以上分析可知，計(jì)程儀2次采樣值的差值可以較為準(zhǔn)確地反映出艦船在采樣周期內(nèi)的對地速度增量。以計(jì)程儀速度差分得到的東、北向速度增量與慣導(dǎo)測得的東、北向速度增量的差值作為觀測量：

(15)

式中：ΔVIx和ΔVIy分別為慣導(dǎo)測得的一個(gè)采樣周期內(nèi)東、北向速度增量：

(16)

將式(7)和式(16)式代入式(15)得：

H(k)X(k)+L(k)X(k-1)+Vj(k)。

(17)

上式即為含有延遲狀態(tài)的觀測方程，其中δvjx(k)和δvjy(k)分別為計(jì)程儀東北向速度差分后量測噪聲。認(rèn)為是白噪聲，H(k)和L(k)分別為：

3延遲狀態(tài)卡爾曼濾波算法

由于式(16)觀測方程中含有延遲狀態(tài)X(k-1)項(xiàng)，即K時(shí)刻的觀測值與上一時(shí)刻的狀態(tài)量有關(guān)。因此不能使用普通形式的卡爾曼濾波器，含有延遲狀態(tài)量的觀測方程可使用延遲狀態(tài)卡爾曼濾波器[10]：

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

X^k/k-1=Φk,k-1X^k-1；

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

X^k=X^k/k-1+Kk(Zk-HkX^k/k-1-LkX^k-1)；

濾波增益：

一步預(yù)測均方誤差：

估計(jì)均方誤差：

其中：

本文采用輸出校正的方式將計(jì)程儀與慣導(dǎo)系統(tǒng)進(jìn)行組合，系統(tǒng)原理如圖2所示。

圖2　INS/LOG組合到導(dǎo)航系統(tǒng)原理圖Fig.2　Schematic diagram of the INS/LOG intergraded system

4系統(tǒng)仿真

為驗(yàn)證本文方法的效果，在考慮海流影響的情況下，分別對平臺式慣導(dǎo)與電磁計(jì)程儀3種不同組合方式在Simulink中進(jìn)行仿真：模式1為傳統(tǒng)的外速度水平阻尼方案；模式2為以計(jì)程儀速度值為觀測量的普通卡爾曼濾波方案；模式3為本文提出的組合方案方案。

仿真條件為：假定艦船勻速直航，航速15 kn，航向45°；初始位置：φ0=30°，λ0=120°；初始平臺角誤差：φx=1′,φy=1′，航向誤差φz=3′；加速度計(jì)常值零偏為100 μg，隨機(jī)零偏為50 μg；陀螺常值漂移均為0.001°/h，隨機(jī)漂移0.000 5°/h；電磁計(jì)程儀量測誤差0.2 kn(看作白噪聲)。計(jì)程儀速度值的采樣周期和濾波周期都為10 s，仿真時(shí)間48 h。假定海流Vf由3部分組成：

Vf=Vs+Vc+Vw。

式中：Vs=sin[(1/24×3 600)t]是周期為24 h、峰值為0.3 kn的潮汐流(看作是時(shí)間的正弦函數(shù))；Vc為1 kn恒定海流，Vw是強(qiáng)度為0.2 kn的隨機(jī)海流，海水流向?yàn)?5°。

仿真結(jié)果如圖3~圖5所示。

圖3　系統(tǒng)緯度誤差Fig.3　Latitude errors of the system

圖4　系統(tǒng)經(jīng)度誤差Fig.4　Longitude errors of the system

圖5　航向誤差Fig.5　Heading errors of the system

由圖可看出，組合模式1中的傳統(tǒng)阻尼網(wǎng)絡(luò)可以消除緯度舒拉震蕩成分，緯度誤差為地球周期震蕩，峰值約為2′，經(jīng)度誤差在經(jīng)過2 d以后發(fā)散到大約6′，航向誤差也為地球周期震蕩，并且含有常值誤差，誤差峰值約為3′；在組合模式2中，由于恒定海流及緩慢變化的潮汐流的影響，計(jì)程儀輸出速度中含有較大常值海流成分，造成緯度誤差緩慢發(fā)散，2 d后達(dá)到3′，經(jīng)度誤差則發(fā)散到了30′，航向誤差最大值為2.8′，表明海流對模式2的精度影響較大；組合模式3由于消除了穩(wěn)定海流的影響，位置誤差精度較高，其中緯度誤差控制在0.4′，經(jīng)度誤差抑制效果較明顯，經(jīng)過2 d后誤差控制在0.3′以內(nèi)，航向誤差則控制在0.5′以內(nèi)。

仿真結(jié)果表明，電磁計(jì)程儀引入的穩(wěn)定海流速度誤差對傳統(tǒng)阻尼網(wǎng)絡(luò)的精度影響不大；普通卡爾曼濾波器則由于海流影響，位置估計(jì)精度變差，特別是經(jīng)度誤差比傳統(tǒng)外速度阻尼誤差更大；模式3中采用速度增量為觀測量，基本可以消除海流速度的影響，位置及航向精度均優(yōu)于模式1和模式2。

5結(jié)語

為解決電磁計(jì)程儀引入的海流速度可能會(huì)導(dǎo)致INS/LOG組合導(dǎo)航系統(tǒng)精度發(fā)散的問題，本文提出了采用了計(jì)程儀2次采樣的差分(即速度增量)為觀測量，基于延遲狀態(tài)卡爾曼濾波算法將慣導(dǎo)和計(jì)程儀數(shù)據(jù)進(jìn)行信息融合。仿真結(jié)果證明本文方法可有抑制系統(tǒng)精度的發(fā)散，提高了系統(tǒng)精度。

參考文獻(xiàn)：

[1]陳永冰，鐘斌.慣性導(dǎo)航原理[M].北京：國防工業(yè)出版社，2007.

[2]查峰，許江寧，覃方君,等.捷聯(lián)慣導(dǎo)系統(tǒng)內(nèi)水平阻尼網(wǎng)絡(luò)設(shè)計(jì)[J].兵工學(xué)報(bào)，2011，32(8)：997-1001.

ZHA Feng，XU Jiang-ning，QIN Fang-jun,et al.Damp Network design of horizontal loop in strapdown inertial navigation system[J]. Acta Armamen-Tarii， 2011，32(8)： 996-1001.

[3]BROWN R G，HWANG P Y C.Introduction for random signals and applied kalman filtering [M].New York：John Wiley & Sons，1997：255-276.

[4]李佩娟，徐曉蘇， 張濤.信息融合技術(shù)在水下組合導(dǎo)航系統(tǒng)中的應(yīng)用[J].中國慣性技術(shù)學(xué)報(bào)，2009，17(3)：344-349.

LI Pei-juan，XU Xiao-su，ZHANG Tao.Application of information fusion to integrated navigation system of underwater vehicle[J].Journal of Chinese Inertial Technology，2009，17(3)：344-349.

[5]秦永元，張洪鉞，汪叔化.卡爾曼濾波與組合導(dǎo)航原理[M].西安：西北工業(yè)大學(xué)出版社，1998：286-301.

[6]汪鳳林，蔡體菁，羅斌.速率方位慣性平臺/GPS/計(jì)程儀組合導(dǎo)航系統(tǒng)仿真研究[J].中國慣性技術(shù)學(xué)報(bào)，2005，13(2)：35-40.

WANG Feng-lin，CAI Ti-jing，LUO Bin.Rate azimuth inerial platforn/GPS/log intergrated navigation system[J].Journal of Chinese Inertial Technology，2005，13(2)：35-40.

[7]卞鴻巍，李安，許江寧,等.現(xiàn)代信息融合技術(shù)在組合導(dǎo)航中的應(yīng)用[M].北京：國防工業(yè)出版社，2010：100-102.

[8]郭亞娜.航海氣象與海洋學(xué)[M].大連：大連海事出版社，2011：145-152.

[9]李鐘慎.基于MATLAB設(shè)計(jì)巴特沃斯低通濾波器[J].信息技術(shù)，2003，27(3)：49-52.

LI Zhong-shen.The design of butterworth low-pass filter based on MATLAB[J].Information Technology，2003，27(3)：49-52.

[10]STANWAY M J.Dalayed-state sigma point Kalman filter for underwater navigation[C]//Autonomous Underwater Vehicles(AUV).IEEE，2010：1-9.

[11]WAN D，HUANG Y，REN T.Study on new kalman filter mechanization for initial alignment of INS in random rocking vehicles[C]//ΠΠStuttgart, West Germany，1989.

A new algorithm for INS/LOG integrated navigation system

GAO Xin,BIAN Hong-wei,LIU Wen-chao

(Department of Navigation Engineering, Naval University of Engineering, Wuhan 430033,China)

Abstract：Since the measurement of electromagnetic LOG contain ocean current velocity, and this might cause the divergence of the Kalman filter of the INS/LOG integrated navigation system. A new INS/LOG integrated navigation system is designed, which base on the Delayed-state Kalman filter, twice sampling of LOG speed difference is adopted as its measurement variables (i.e. velocity increment) to restrain the error caused from the . The paper deduces the observation equation of this new integrated navigation system in detail and applies this INS/LOG integrated navigation system to estimate INS error. To restrain amplification effect on the high frequency noise, butterworth low pass filter is introduced to smooth the LOG data. Comparing the perfomance of this new INS/LOG system integrated system with that of other two INS/LOG system, and the simulation result verified the effectiveness of the proposed algorithm.

Key words：intergraded navigation system; INS; electromagnetic LOG; velocity increment; delayed-state kalman filter

作者簡介：高薪(1982-)，男，博士研究生，從事導(dǎo)航制導(dǎo)與控制研究。

基金項(xiàng)目：航空科學(xué)基金資助項(xiàng)目(920120816001)

收稿日期：2013-11-06； 修回日期： 2013-12-12

文章編號：1672-7649(2015)02-0053-05

doi：10.3404/j.issn.1672-7649.2015.02.011

中圖分類號：U666.1

文獻(xiàn)標(biāo)識碼：A