一種新的INS/LOG組合導航算法

高　薪，卞鴻巍，劉文超

(海軍工程大學 導航工程系，湖北 武漢 430033)

一種新的INS/LOG組合導航算法

高薪，卞鴻巍，劉文超

(海軍工程大學 導航工程系，湖北 武漢 430033)

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

關鍵詞：組合導航；慣導；電磁計程儀；速度增量；延遲狀態卡爾曼濾波

0引言

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

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

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

1狀態方程的建立

慣導的陀螺和加速度計誤差都可以用隨機常數加上白噪聲的形式表示[5-6]：

(1)

(2)

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

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

εbx,εby,εbz]T。

(3)

式中：δφ和δλ分別為緯度、經度誤差；δVx和δVy分別為東向、北向速度誤差；φx， φy， φz為平臺失準角；▽Abx和▽Aby分別為東向、北向加速度計常值零偏；εbx,εby,εbz分別為東向、北向和天向陀螺常值漂移。則根據慣導誤差方程可得狀態方程為：

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

(4)

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

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

(5)

2觀測方程的建立

2.1　觀測量的選取

電磁計程儀的測量值為艦船相對海水的速度，因而電磁計程儀輸出的速度值應為艦船對地真實速度、海流速度及計程儀誤差的和：

Vj=Vr+Vf+δVj。

(6)

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

海水流速一般小于2 kn，但有些海流的流速可達4~5 kn以上[8]。根據海洋學原理，海樣是一個不可壓縮的流場，海流的空間尺度大，一般具有數百甚至數千千米的流動范圍。在較短的時間內海流相對穩定，流速、流向基本不變[8]，因此可認為采樣時間較短時，計程儀速度的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)

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

2.2　低通濾波器的設計

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

δVj=δVjL+δVjH。

(9)

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

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

(10)

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

(11)

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

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

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

(12)

計算出濾波器3個極點分別為：

(13)

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

(14)

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

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

2.3　建立量測方程

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

(15)

式中：ΔVIx和ΔVIy分別為慣導測得的一個采樣周期內東、北向速度增量：

(16)

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

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

(17)

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

3延遲狀態卡爾曼濾波算法

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

狀態一步預測：

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

狀態估計：

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

濾波增益：

一步預測均方誤差：

估計均方誤差：

其中：

本文采用輸出校正的方式將計程儀與慣導系統進行組合，系統原理如圖2所示。

圖2　INS/LOG組合到導航系統原理圖Fig.2　Schematic diagram of the INS/LOG intergraded system

4系統仿真

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

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

Vf=Vs+Vc+Vw。

式中：Vs=sin[(1/24×3 600)t]是周期為24 h、峰值為0.3 kn的潮汐流(看作是時間的正弦函數)；Vc為1 kn恒定海流，Vw是強度為0.2 kn的隨機海流，海水流向為45°。

仿真結果如圖3~圖5所示。

圖3　系統緯度誤差Fig.3　Latitude errors of the system

圖4　系統經度誤差Fig.4　Longitude errors of the system

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

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

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

5結語

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

參考文獻：

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

[2]查峰，許江寧，覃方君,等.捷聯慣導系統內水平阻尼網絡設計[J].兵工學報，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]李佩娟，徐曉蘇， 張濤.信息融合技術在水下組合導航系統中的應用[J].中國慣性技術學報，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]秦永元，張洪鉞，汪叔化.卡爾曼濾波與組合導航原理[M].西安：西北工業大學出版社，1998：286-301.

[6]汪鳳林，蔡體菁，羅斌.速率方位慣性平臺/GPS/計程儀組合導航系統仿真研究[J].中國慣性技術學報，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]卞鴻巍，李安，許江寧,等.現代信息融合技術在組合導航中的應用[M].北京：國防工業出版社，2010：100-102.

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

[9]李鐘慎.基于MATLAB設計巴特沃斯低通濾波器[J].信息技術，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-)，男，博士研究生，從事導航制導與控制研究。

基金項目：航空科學基金資助項目(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

文獻標識碼：A