秦立新

(海軍駐上海江南造船(集團(tuán))有限責(zé)任公司軍事代表室　上?！?01913)

?

一種基于卡爾曼濾波的慣導(dǎo)系統(tǒng)綜合校正算法*

秦立新

(海軍駐上海江南造船(集團(tuán))有限責(zé)任公司軍事代表室上海201913)

為了解除慣導(dǎo)系統(tǒng)在綜合校正時(shí)必須處于水平阻尼條件下的限制,論文提出了一種基于卡爾曼濾波的無阻尼綜合校正算法。首先,艦船在航行時(shí)處于無阻尼狀態(tài),并基于外測速度信息利用Kamlan濾波技術(shù)實(shí)時(shí)估測出系統(tǒng)的水平誤差角；其次,將估測出的水平誤差角引入到綜合校正算法中進(jìn)行解算,計(jì)算出陀螺漂移并進(jìn)行補(bǔ)償。通過仿真分析表明,系統(tǒng)在綜合校正的過程中不再受到水平阻尼條件的限制,提高了慣導(dǎo)系統(tǒng)的長期定位精度。

慣性導(dǎo)航系統(tǒng)(INS); 綜合校正; 卡爾曼濾波; 無阻尼

Class NumberTP301.6

1　引言

對于連續(xù)運(yùn)行時(shí)間很長時(shí)間的船用慣性導(dǎo)航系統(tǒng)(INS),慣性指示位置和航向角的主要誤差源是系統(tǒng)陀螺儀未被補(bǔ)償?shù)钠普`差。為了保持較高的位置精度和航向精度,需要對系統(tǒng)進(jìn)行重調(diào)[1～2]。目前,很多學(xué)者對慣性導(dǎo)航系統(tǒng)綜合校正技術(shù)進(jìn)行了大量的研究工作,文獻(xiàn)[3]介紹了基于OEPQ坐標(biāo)系下綜合校正算法,但此算法需要限制載體航行方式；文獻(xiàn)[4]對平臺式兩點(diǎn)校進(jìn)行了常值誤差分析；但采用兩點(diǎn)校等方法進(jìn)行海上系統(tǒng)重調(diào)校正只是一種理想方式,系統(tǒng)水平阻尼工作狀態(tài)要求艦艇在校正時(shí)必須處于勻速直航狀態(tài)[5]。文獻(xiàn)[6～7]雖然適用于捷聯(lián)慣導(dǎo)系統(tǒng)且解除了艦船在等緯度航行的限制,但是也是在水平阻尼條件下進(jìn)行綜合校正。所以,近年出現(xiàn)的一些方案,在算法上存在局限性且忽略了艦船在機(jī)動狀態(tài)下加速度對系統(tǒng)的干擾。

本文提出了一種新的綜合校正方法,能在校正過程中解除對艦艇航行方式的限制:在校正過程中,使慣導(dǎo)系統(tǒng)處于無阻尼狀態(tài),基于外部速度觀測信息,利用Kalman濾波估測出水平誤差角,由于慣導(dǎo)處于無阻尼狀態(tài),不僅滿足了舒拉振蕩條件,并且提高了Kalman濾波器的可觀測度[8～9],將Kalman濾波器估計(jì)出的水平誤差角引入到傳統(tǒng)綜合校正過程中,建立綜合校正方程,解算出導(dǎo)航系統(tǒng)的陀螺漂移和位置航向誤差并補(bǔ)償,完成慣導(dǎo)系統(tǒng)的綜合校正[10～11]。最后對該綜合校正方案進(jìn)行仿真驗(yàn)證,并給出結(jié)論。

2　慣導(dǎo)系統(tǒng)水平誤差角的估計(jì)

(1)

方程組(1)為動基座下無阻尼慣導(dǎo)系統(tǒng)誤差方程。

式中,δL、δλ分別為緯度誤差和經(jīng)度誤差；δVx、δVy分別為載體的東向速度誤差和北向速度誤差；ΔAx、ΔAy分別為東向和北向加速度計(jì)誤差；fx、fy、fz分別為加速度計(jì)在x軸、y軸和z軸測量的比力信息；φx、φy、φz分別為載體x軸、y軸和z軸姿態(tài)誤差角；εx、εy、εz分別為x軸、y軸和z軸陀螺漂移。

狀態(tài)向量為

X=[δLδλδVxδVyφxφyφzεxεyεzΔAxΔAy]T

由式(1)和狀態(tài)向量建立12維系統(tǒng)狀態(tài)方程:

(2)

其中

W=[00axaywxwywz00000]T,

W為零均值的高斯白噪聲。

其中系數(shù)矩陣A為

選取慣導(dǎo)系統(tǒng)解算速度和多普勒計(jì)程儀測量速度之差作為觀測量,系統(tǒng)量測方程為

Z=HX+ν

(3)

式中,Z為系統(tǒng)的量測向量；H為系統(tǒng)的量測矩陣；量測噪聲ν為零均值高斯白噪聲。

量測量為

其中,Vs為地理坐標(biāo)系下慣導(dǎo)系統(tǒng)解算的速度信息；Vsx,Vsy為Vs在地理系水平方向上的分量；Vd為地理坐標(biāo)系下多普勒計(jì)程儀輸出的載體艏艉向速度信息,其中

K為航向角。

量測矩陣為

把方程(2)和(3)離散化,得

(4)

最后,由式(4)采用Kalman濾波估計(jì)慣導(dǎo)系統(tǒng)水平誤差角。

3　無阻尼綜合校正方案設(shè)計(jì)

為了解除綜合校正對載體航行姿態(tài)的限制,同時(shí)減小引入的新的動態(tài)誤差,設(shè)計(jì)的方案中將式(4)估計(jì)出的慣導(dǎo)系統(tǒng)水平誤差角φx、φy帶入到校正算法中進(jìn)行解算,同時(shí)在校正時(shí)間段內(nèi),系統(tǒng)工作于無阻尼狀態(tài)。

慣導(dǎo)平臺坐標(biāo)系相對地理坐標(biāo)系誤差角為

(5)

式中,θx、θy、θz為計(jì)算機(jī)坐標(biāo)系與地理坐標(biāo)系的夾角；ψx、ψy、ψz為平臺坐標(biāo)系相對計(jì)算機(jī)坐標(biāo)系的誤差角。

引入Kalman濾波估計(jì)的結(jié)果,由于方位誤差角φz估計(jì)效果不理想,因此方位信息仍由外界信息獲得。對式(5)做變換:

(6)

由圖1可知,方位誤差角φz=δK,根據(jù)校正原理得到:

(7)

令

圖1　φz與δK的關(guān)系

因此,P(tN)=M(tN)ψ(tN),再由平臺漂移角ψ和陀螺漂移ε之間的關(guān)系可得

ΩU-1(tN,tN-1)M-1(tN)P(tN)

(8)

式中,

Ω為地球自轉(zhuǎn)角速度。

結(jié)合式(5)～式(8),即可估算出陀螺漂移并實(shí)現(xiàn)無阻尼條件下的綜合校正。

本方案整體流程圖如圖2所示。

通過綜合校正整體流程圖可以看出,首先利用外部觀測信息和卡爾曼濾波器估出水平誤差角,重調(diào)開始時(shí)利用外部提供的精確經(jīng)緯度信息和航向信息對系統(tǒng)進(jìn)行重調(diào),并利用估測出的水平誤差角建立位置誤差和陀螺漂移的關(guān)系,在第二點(diǎn)取得信息后利用估測出的水平誤差角和外部觀測信息計(jì)算出陀螺漂移并進(jìn)行補(bǔ)償。同時(shí)又對系統(tǒng)進(jìn)行重調(diào)。

圖2　綜合校正整體流程圖

4　仿真研究

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

圖3　定位誤差

圖4　估計(jì)出的水平誤差角

圖5　方位誤差角

艦船航行7小時(shí)的時(shí)候利用外部參考信息進(jìn)行第一次系統(tǒng)重調(diào)并將估測出的水平誤差角帶入綜合校正算法中進(jìn)行第一點(diǎn)解算,5小時(shí)后再利用外部位置信息進(jìn)行海上重調(diào),此時(shí)將7～5小時(shí)期間估測出的水平誤差角引綜合校正算法中進(jìn)行解算,估測出陀螺漂移并進(jìn)行補(bǔ)償。方位誤差角的估計(jì)如圖5所示。由圖3～圖5可以看出在第12小時(shí)綜合校正后艦船的速度及經(jīng)緯度都得到了很好的校正且定位誤差和方位誤差允許范圍內(nèi),說明此方法的可行性。

5　結(jié)語

本文提出的綜合校正方法,在校正過程中解除對艦艇航行時(shí)必須處于水平阻尼條件下的限制,基于外部速度觀測信息,利用Kalman濾波估測出水平誤差角,將其引入到傳統(tǒng)綜合校正過程中,在慣性坐標(biāo)系下建立綜合校正方程,由傳統(tǒng)校正算法解算得到導(dǎo)航系統(tǒng)的陀螺漂移和位置航向誤差并補(bǔ)償,完成慣導(dǎo)系統(tǒng)的綜合校正。

[1] 高鐘毓.慣性導(dǎo)航系統(tǒng)技術(shù)[M].北京:清華大學(xué)出版社,2012:143-144.GAO Zhongmin. Inertial navigation systems technology[M]. Beijing: Tsinghua University Press,2012:143-144.

[2] Donald R. Watland, Peoria, Ariz. Orthogoanl Polar Coordinate System to Accommodate polar navigation[P]. United States Patent,1995.

[3] 黃德鳴,程祿.慣性導(dǎo)航系統(tǒng)[M].北京:國防工業(yè)出版社,1986:66-90.

HUANG Deming, CHENG Lu. Inertial Navigation System[M]. Beijing: National Defense Industry Press,1986:66-90.

[4] 楊曉東,鄧太光.平臺式慣導(dǎo)系統(tǒng)兩點(diǎn)校的常值誤差分析[J].交通運(yùn)輸工程學(xué)報(bào),2011,11(3):100-104.

YANG Xiaodong, DENG Taiguang. Constant error Analysis of two-point calibration on Platform Inertial Navigation System[J]. Journal of Traffic and Transportation Engineering,2011,11(3):100-104.

[5] 鄧太光,楊曉東.一種平臺式慣導(dǎo)系統(tǒng)兩點(diǎn)校方法的改進(jìn)[J].控制工程,2011,18(SI):23-36.DENG Taiguang, YANG Xiaodong. A better two-point calibration method for Platform Inertial Navigation System[J]. Control Engineering,2011,18(SI):23-36.

[6] 高偉,史洪洋,張鑫,等.基于位置信息的捷聯(lián)慣導(dǎo)系統(tǒng)綜合校正技術(shù)[J].華中科技大學(xué)學(xué)報(bào)(自然科學(xué)版),2014,42(6):101-106.

GAO Wei, SHI Hongyang, ZHANG Xin, et al. Iintegrated correction based on position information SINS[J]. Journal of Huazhong University of Science and Technology(Natural Science Edition),2014,42(6):101-106.

[7] 張鑫.船用單軸旋轉(zhuǎn)光纖陀螺捷聯(lián)慣導(dǎo)初始對準(zhǔn)/測漂及綜合校正研究[D].哈爾濱:哈爾濱工程大學(xué),2012:103-105.

ZHANG Xin. Research on Initial Alignment/evaluation of the Inertial Sensor Error and Comprehensive Correction for Shipboard Single-axial Rotary Fiber-Gyroscope Inertial Navigation System[D]. Harbin: Harbin Engineering University,2012:103-105.

[8] 秦永元,張洪鉞,汪淑華.卡爾曼濾波與組合導(dǎo)航原理[M].西安:西北工業(yè)大學(xué)出版社,1998:48-55.QIN Yongyuan, ZHANG Hongyue, WANG Shuhua. Kalman filtering and navigation principle[M]. Xi’an: Northwestern Polytechnical University Press,1998:48-55.

[9] 付夢印,鄧志紅,閆莉萍.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用[M].北京:科學(xué)出版社,2010:92-100.

FU Mengyin, DENG Zhihong, YAN Liping. Kalman filtering theory and its application in navigation system[M]. Beijing: Science Press,2010:92-100.

[10] 吳俊偉,曾啟明,聶莉娟.慣性導(dǎo)航系統(tǒng)的誤差估計(jì)[J].中國慣性技術(shù)學(xué)報(bào),2002,10(6):1-5.

WU Junwei, ZENG Qiming, NIE Lijuan. Estimation of INS’s error[J]. Journal of Chinese Inertial Technology,2002,10(6):1-5.

[11] 徐景碩.固定點(diǎn)平滑技術(shù)在慣導(dǎo)對準(zhǔn)精度評估中的應(yīng)用[J].測控技術(shù),2004,23(11):74-76.

XU Jingsuo. Application of fixed point smoothing to the testing and evaluation of INS in-flight alignment[J]. Measurement & Control Technology,2004,23(11):74-76.

A Comprehensive Calibration Algorithm Based on Kalman fliter for Inertial Navigation System

QIN Lixin

(Naval Representative Office in Shanghai Jiangnan Shipyard(Group) Co., Ltd, Shanghai201913)

In order to lift the restrictions that the inertial system in comprehensive correction must be in a horizontal damping condition,this paper proposes an undamped comprehensive correction algthm based on Kalman filter. Firstly,the ship is sailing with the undamped state, and the Kalman fliter is used to real time estimate horizontal-angle error based on the velocity observations. Secondly horizontal error of estimated angle is taken into the comprehensive correction algorithm for solving, then the gyro drift is calculated and compensated. The simulation results show that the system is no longer subject to the horizontal damping conditions in the process of comprehensive correction and increase the long-term positioning accuracy of inertial navigation system is increased.

inertial navigation system(INS), comprehensive calibration, Kalman filter, undamped

2016年4月13日,

2016年5月20日

秦立新,男,工程師,研究方向:艦船電氣。

TP301.6

10.3969/j.issn.1672-9722.2016.10.010