基于SUKF滤波的INS/DVL组合导航研究

1 引 言

惯性导航系统是水下自主潜器航行时可以依赖的自主式导航装备，其隐蔽性好、不怕干扰，但原理上存在长时间工作的误差积累，初始对准精度误差也将导致系统导航精度的下降[1]。多普勒速度计程仪使用方便，因其发射波束很窄，且能以很陡的角度发射到海底，隐蔽性和抗干扰性较好，测得的平均速度精度很高。综合INS和DVL的优缺点，采用优势互补的INS/DVL组合系统成为水下自主潜器自主导航的选择。

通常，针对组合导航的系统模型本质上具有的非线性特性，当前多采用扩展卡尔曼滤波(EKF)进行处理。EKF是对非线性模型进行近似线性化，其线性化误差会降低模型的准确性，随时间延长，其估计精度难以保证，且其需要计算非线性模型的Jacobi矩阵，过程繁琐。JULIER和UHLMAN等人基于多元函数代表点思想提出的UKF方法不是逼近非线性状态函数，而是采用一种UT变换技术，即采用确定的样本点(Sigma点)来完成状态变量沿时间的传播，可很好地克服EKF的上述缺点[2-5]。但当系统维数变高时，非局部的采样导致估计误差变大，为此可以引入基于样本点尺度伸缩的SUT变换方法的滤波器Scaled-UKF(SUKF)来解决这个问题[6-8]。

2 系统模型

2.1 惯性导航系统误差模型

选取12维系统状态矢量：XINS=[δφδλδveδvnφeφnφuenεeεnεu]T，分别为惯导系统输出的纬度误差、经度误差、东向和北向速度误差、平台东向、北向和方位失准角、东向和北向加速度计零偏以及东向、北向和方位陀螺漂移；系统噪声矢量为：WINS=[0 0wδvewδvnwφewφnwφu0 0 0 0 0]T，其中，wδve、wδvn、wφe、wφn、wφu均为零均值的高斯白噪声，分别看作加速度计误差和陀螺漂移中的白噪声成分。

系统误差模型选取半解析式当地水平指北平台式惯导系统，东北天坐标系。在上述条件下，惯导系统的非线性误差状态方程就简化为：

(1)

其中，状态函数f[XINS]定义为：

式中，φ为舰船所处纬度;g为当地重力加速度;RM、RN分别为地球卯酉圈和子午圈半径;ωie为地球自转角速度。

2.2 DVL误差模型

根据多普勒计程仪工作原理,它测量载体相对海底的速度和偏流角,测量误差主要有速度偏移误差δVd,偏流角误差δΔ,刻度系数误差Δkd。δVd和δΔ用一阶马尔可夫过程表示,Δkd为随机常数。相应误差方程为[9]：

(2)

2.3 组合导航系统状态方程量测方程

(3)

其中，状态函数f1[X]为：

f1[X]=[f[XINS]T-βdδVd-βΔδΔ0]T

取惯导在东向和北向上的解算速度vINSe、vINSn与多普勒计程仪在东向和北向上的速度vDVLe、vDVLn之差作为观测量，得到系统量测方程：

=HX+η

(4)

式中，量测噪声η=[ηeηn]T为零均值高斯白噪声;量测矩阵H为[10]：

式中，Kv表示考虑了偏流角的航迹向。

3 尺度变换方式的UKF(SUKF)滤波器设计

SUT是对标准UT变换的一种修正形式。这种方法能够对任意分布形式的Sigma点进行尺度变换，并且能保持随机变量的均值和方差不变。下面给出SUT变换选点策略和均值方差计算方法。

(5)

(6)

由式(1)确定的点和式(2)确定的加权，则SUT按照如下方式计算统计特性：

(7)

(8)

由于SUKF与UKF在测量更新阶段是相同的，区别仅在预测阶段。关于详细的UKF递推计算公式，可参见文献[2]和[3]。

4 仿真结果及结论

图1 惯导东向速度误差曲线

图2 惯导北向速度误差曲线

图3 惯导纬度误差曲线

图4 惯导经度误差曲线

图5 东向水平误差角曲线

图6 北向水平误差角曲线

图7 方位误差角曲线

图1～图7分别为惯导速度,经纬度与3个平台误差角变化曲线。其中点划线为SUKF滤波结果，实线为未滤波情况。

由仿真可见，经SUKF滤波后，惯导速度误差有所减小，经度误差由快速发散变为迅速减小，3个平台误差角的常值漂移消除。结果表明，采用SUKF滤波方法实现INS/DVL组合导航是可行的。

参考文献:

[1] 陈永冰，钟斌.惯性导航原理[M].北京：国防工业出版社，2007.

[2] JULIER S,UHLMAN J K.A general method for approximating nonlinear transformations of probability distributions[R].Robotics Research Group,Department of Engineering Science,University of Oxford,1994.

[3] JULIER S,UHLMAN J K,etc.A new method for the nonlinear transformation of means and covariances in filters and estimators[J].IEEE Trans A C,2000,45(3):477-482.

[4] 李涛.非线性滤波方法在导航系统中的应用研究[D].长沙：国防科技大学，2003.

[5] Zhang Chuan-bin,Tian Wei-feng,Jin Zhi-hua.Application of unscented kalman filter in state estimation for land vehicle navigation system[J].Journal of System Simulation,2005,17(6):1456-1458.

[6] JULIER S.The scaled unscented transformation[C].Proceedings of the American Control Conference.AK:Anchorage,2002.

[7] 张红梅，邓正隆.一种新的惯性导航初始对准滤波方法[J].中国惯性技术学报，2005，13(1)：1-4.

[8] Zhang Hong-mei,Deng Zheng-long.A UKF-based attitude determination method for gyroless satellite[J].Journal of Systems Engineering and Electronics,2004,15(2):105-109.

[9] 熊崴，庄良杰，翁海娜，刘玉峰.INS/ESGM/Doppler组合导航系统中的Kalman滤波方法[J].中国惯性技术学报，2000，8(4)：10-19.

[10] 杨跃轮.智能导航系统研究[D].哈尔滨：哈尔滨工程大学,2002.