船用捷聯慣性導航系統在線誤差校正算法設計及仿真*

扶文樹 楊 波

(1.南京埃斯頓自動化股份有限公司 南京 210016)(2.東南大學儀器科學與工程學院 南京 210096)

船用捷聯慣性導航系統在線誤差校正算法設計及仿真*

扶文樹1楊 波2

(1.南京埃斯頓自動化股份有限公司 南京 210016)(2.東南大學儀器科學與工程學院 南京 210096)

針對捷聯慣性導航系統的長期精度不能滿足艦載應用的問題,設計了利用羅經的航向信息及多譜勒計程儀的速度信息,通過卡爾曼濾波,在線校正捷聯慣性導航系統誤差的軟件算法,并進行了計算機仿真。

捷聯慣性導航系統; 平臺羅經; 多譜勒計程儀; 卡爾曼濾波器; 在線誤差校正

Class Number TP301.6

1 引言

捷聯慣性導航系統長期的導航精度不適于船舶長期航行的需要,有必要采用組合導航的方式來提高系統精度。組合系統一般采用卡爾曼濾波技術,即在兩個或兩個以上導航系統輸出的基礎上,利用卡爾曼濾波去估計系統的各種誤差,再用該誤差的估值去校正系統,達到組合的目的。

2 捷聯慣性導航系的載體姿態算法

船體的姿態表示的是載體坐標系OXbYbZb(左至右、尾至首、垂直載體向上)和當地地理坐標系OXnYnZn(東北天)之間的關系,即航向角φ、縱搖角θ、橫搖角γ。定義b系到n系的姿態矩陣為

(1)

對于方向余弦表示剛體轉動,則有[1]

(2)

而對于四元數法表示剛體轉動,定義轉動四元數Q=ξ+P1i+P2j+P3k,則有

(3)

由四元數法建立轉動微分方程[2]

(4)

(5)

(6)

式(6)中,L為當地緯度;Ve、Vn分別為載體的東向和北向速度;Rn為子午線曲率半徑,Re為與子午線垂直的法線平面的曲率半徑,且Rn=re(1-2e+3esin2L),Re=re(1+esin2L),e為地球的橢圓度,re為參考橢球體的長軸半徑。

經緯度的計算公式如下

(7)

利用畢卡逼近法求解(5)式并離散化,可以得到遞推式

(8)

式(6)中,Q(k)、Q(k-1)分別表示k時刻、k-1時刻的轉動四元數,Δαk的計算式如下

(9)

I為四階單位矩陣,M的計算式如下

(10)

由于計算誤差、不可交換性誤差等影響,式(6)計算的四元數會失去規范性,即其范數不再等于1,需對計算的四元數周期地進行規范化處理,其處理公式如下

(11)

(12)

由慣性系統基本方程有[4]：

(13)

式中,fn為載體加速度在導航坐標系上的投影,fn=[fEfNfU]T,Vn表示船體在導航坐標系中的速度矢量,Vn=[VEVNVU]T,gn為重力加速度矢量,gn=[0 0 -g]T。

3 卡爾曼濾波在誤差校正中的應用

(14)

W= [wvEwvNwφEwφNwφUwδLwδλ

0 0 0 0 0]T

(15)

(16)

(17)

(18)

(19)

式(16)～式(19)中

ω5=ωiesinL;

本文采用速度和航向誤差角校正模型,該模型是將捷聯導航系統輸出的速度與計程儀的速度差值和捷聯導航系統輸出的航向角與主慣導系統輸出的航向角的差值共同作為觀測量,觀測方程[7]為

(20)

式(20)中,VSE、VSN是捷捷聯慣性導航系統輸出的東向和北向速度,VJE、VJN是計程儀輸出的東向和北向速度,δVSE、δVSN是捷聯慣性導航系統東向和北向速度誤差,δVJE、δVJN是計程儀東向和北向速度誤差,φS是捷聯慣性導航系統輸出的航向角,φP是羅經輸出的航向角,δφS是捷聯慣性導航系統航向角誤差,δφP是羅經航向角誤差。

式(20)中,

在閉環校正模式下,卡爾曼濾波計算方程可以簡化為[8～10]

(21)

Kk=Pk,k-1HkT(HkPk,k-1HkT+Rk)-1

(22)

Xk,k-1=0

(23)

(24)

(25)

圖1 閉環校正濾波示意圖

4 在線校正算法的仿真

海上航行的船體,由于受到風浪運動等影響時常經歷不規則的搖擺運動,使用單一的正弦運動模型顯然不能逼近其航行的實際情況。因此,這里采用多個正弦波疊加組合作為艦船運動模型：

其中,Ai為振幅,fi為頻率,phi為相位,它們的各自取值如表1所示。

表1 艦船運動模型振幅、頻率和相位取值

船舶線速度取V=10m/s,初始經度118°,初始緯度32°,水平加表常值偏置,400μg,隨機誤差50μg,三個陀螺常值漂移0.2°/h,隨機誤差0.05°/h,初始誤差Δθ0=Δγ0=10′、Δφ0=15′,仿真時間60h。

卡爾曼濾波計算的初值選取如下：

P0= diag{(0.1m/s)2,(0.1m/s)2,(1.5°)2,(1.5°)2, (1.5°)2,(0.00254°)2,(0.00446°)2,(100μg)2, (100μg)2,(10°/h)2,(10°/h)2,(10°/h)2}

Q=diag{(50μg)2,(50μg)2,(10°/h)2, (10°/h)2,(10°/h)2,(50μg)2,

(50μg)2,0, 0, 0, 0, 0}

R=diag{(0.4m/s)2,(0.4m/s)2,(0.3°)2}

仿真結果如圖2～圖9所示。

圖5 橫搖角誤差曲線

圖6 航向角誤差曲線

圖7 東向陀螺漂移估計曲線

圖8 北向陀螺漂移估計曲線

圖9 天向陀螺漂移估計曲線

5 結語

經仿真驗證,通過在線校正算法,捷聯慣性導航系統的水平速度誤差、縱搖角誤差、橫搖角誤差和航向角誤差都能以較高的精度、在較短的時間內達到穩定,這說明利用羅經提供的航向信息,計程儀提供的速度信息,經卡爾曼濾波計算,能實時校正慣性導航系統的導航誤差。

[1] 陳哲.捷聯慣導系統原理[M].北京:宇航出版社,1986:40-41.

[2] 張樹俠,孫靜.捷聯式慣性導航系統[M].北京:國防工業出版社,1993:75-76.

[3] 崔中興.慣性導航系統[M].北京:國防工業出版社,1982:88-89.

[4] 黃德鳴,程祿.慣性導航系統[M].北京:國防工業出版社,1986:112-113.

[5] 萬德鈞,房建成.慣性導航初始對準[M].南京:東南大學出版社,1998:42-43.

[6] 陸愷,田蔚風.最優估計理論及其在導航中的應用[M].上海:上海交通大學出版社,1990:62-63.

[7] 付夢印,鄧志紅,張繼偉.Kalman濾波理論及其在導航系統中的應用[M].北京:科學出版社,2003:22-23.

[8] Guangrong C, Charles K. A modified adaptive Kalman filiter for realtime application[J]. IEEE Transaction on Aerospace and Electronics Systems,1991,27(6):31-33.

[9] Chui C K, Chen G. Kalman Filtering with Real-time Applications[M]. New York: Springer-Verlag, 1987:89-90.

[10] Petersen I R, Savkin A V. Robust Kalman Filtering for Signal and System with Large Uncertainties[M]. Birkhauser Boston,1999:34-35.

Design and Emualtion of Online Error Correction Algorithm for Shipborne SINS

FU Wenshu1YANG Bo2

(1. Nanjing Estun Automation Co., Ltd, Nanjing 211106) (2. School of Instrument Science & Engineering, Southeast University, Nanjing 210096)

Aiming at resolving the problem that the long-term precision of strapdown inertial navigation system is incapable of fulfilling shipborne application, an online error correction algorithm is designed via the Kalman Filter with the course afforded by Gyrocompass and the velocity afforded by Doppler Odometer. Furthermore, the algorithm’s emulation is performed.

strapdown inertial navigation system, stabilized gyrocompass, doppler odometer, kalman filter, online error correction

2014年5月8日,

2014年6月23日 作者簡介:扶文樹,男,博士,工程師,研究方向:測控技術及計算機輔助工程。楊波,男,博士,副教授,研究方向:導航、定位與測控技術。

TP301.6

10.3969/j.issn1672-9730.2014.11.013