船用捷联惯性导航系统在线误差校正算法设计及仿真*

扶文树 杨 波

(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