基于联邦滤波器的SINSGNSDVS水下组合导航研究.doc

基于联邦滤波器的SINS/GNS/DVS水下组合导航研究
刘明雍1，周志远1，赵 涛2
(1. 西北工业大学 航海学院，西安 710072;)
（2. 中国兵器工业集团第二零二研究所，咸阳 712099)
摘 要：针对现有的水下导航系统不能全面满足AUV导航的需要，为了提高水下航行器组合导航系统长时间远航程的隐蔽性，可靠性及精度，提出了一种基于联邦滤波器的惯性导航系统、地磁导航、多普勒速度声纳的组合导航方案，并采用简化自适应卡尔曼算法对水下航行器组合导航系统进行误差估计，解决了传统卡尔曼滤波容易发散的问题。仿真结果表明，在SINS/GNS/DVS组合导航中采用该算法，提高了定位精度，保证了滤波的快速性，验证了该算法的可行性和正确性。
关 键 词： 组合导航；自适应卡尔曼滤波器；地磁导航
中图分类号：U666. 11 文献标志码：A

Research on SINS/DNS/DVS underwater integrated navigation
based on federal filter
Liu Ming-yong1，Zhou Zhi-yuan1，Zhao Tao2
（1. College of Marine, Northwest Polytechnical University, Shaanxi Xi’an , 710072)
(2. NO.202 institute of China ordnance industry group corporation, Shaanxi Xiang-yang,712099)
Abstract: Considering of the fact that the existing underwater navigation system can not completely s the needs of the AUV navigation. In order to enhance the long range and deep water ability, reliability and accuracy of the underwater vehicle navigation system, in this paper, we propose a filter algorithm based on the Federation of inertial navigation system consisting of Doppler speed sonar, magnetic navigation method. Meanwhile, a simplified method is adopted for adaptive Kalman Underwater Vehicle navigation system error estimates to solve the divergence problem of traditional Kalman filter. The simulation results show that the proposed mehod can imporve the positioning accuracy. Moreover, the the speediness of the filter is guaranteed. In addition, the simulation results also demonstrate the effectiveness and correctness of the algorithm.
Key words: Integrated navigation; Adaptive Kalman filter; Geomagnetic Navigation

0引言
当前水下组合导航定位普遍采用SINS/GPS/DVL方案。由于GPS信号在水下很快衰减，故航行器需定期上爬到近水面,使GPS天线露出水面,来接收GPS的定位信息，对由陀螺罗经和多普勒速度仪组成的自主导航系统进行位置校准。然而随着水下航行器的应用范围、下潜深度不断扩展，特别是军事领域内对其隐蔽性的要求，常规组合方案越来越难以满足需求。作为无源导航技术的地磁导航为实现这一目标提供了新的技术途径。国内外的研究中对地磁导航的研究有很多成果。文献[1]将测量的地磁异常场强度序列与事先存储的地磁异常图实时进行相关匹配，确定飞机在地磁异常图上的经度和纬度；文献[2]中介绍了在对飞行弹体进行弹道控制中, 利用地磁探测滚转姿态来实时获取弹体的对地方位, 为弹体控制提供方位依据。但是，地磁导航在水下导航方面的应用，所做研究很少。
考虑上述问题，本文提出了一种SINS/GNS（地磁）/DVS组合方案，该方法以地磁场模型解算地磁场强度的方式来得到精确位置信息,辅以精确计时进而获得速度信息；以地磁系统获取的速度、位置信息与惯导系统输出的速度、位置信息的差值作为量测值,经过卡尔曼滤波,估计导航系统的误差,作为一个子滤波器；并与另一子滤波器SINS/DVL进行信息融合，从而构成联邦滤波器。考虑到水下环境的复杂多变使导航系统特别是地磁导航系统观测噪声的统计特性无法被准确获得，采用改进的自适应滤波算法代替标准卡尔曼滤波，并加以验证。仿真结果表明，在SINS/GNS/DVS组合导航中采用该算法，提高了定位精度，保证了滤波的快速性，验证了该算法的可行性和正确性。

1 基于联邦滤波的SINS/DNS/DVS组合导航方案设计
本文采用的基于联邦卡尔曼滤波的水下航行器SINS/GNS/DVS组合方法如图1所示。该组合方案包括三个滤波器，即主滤波器、子滤波器1和子滤波器2。其中，子滤波器1负责SINS和多普勒速度仪的信息融合，其量测信息主要是指多普勒和SINS形成的速度信息；子滤波器2负责SINS和地磁导航系统融合，其量测信息是地磁导航系统和SINS形成的位置、速度观测信息。主滤波器一方面对各子滤波器进行信息综合与分配，另一方面将系统状态误差的估计值反馈给惯导系统，以校正其累积误差。水下航行器采用该组合方案相比集中滤波器不仅具有实时性好的特点，而且通过信息分配系数的选取可以具有较高的容错能力，便于对各传感器实施故障诊断和隔离。由子滤波器与主滤波器合成的全局估计值及其相应的协方差阵被放大为后再反馈到子滤波器以重置子滤波器的估计值，即
（1）

图1 SINS/DNS/DVS联邦滤波器结构图
2 地磁导航子系统的设计
当前的地磁导航技术主要是指地磁匹配定位,即将预先选定的区域地磁场某种地磁场特征值, 制成参考图并储存在水下航行器上的计算机中。 当航行器通过这些地区时, 地磁传感器实时测量地磁场的有关特征值, 并构成实时图, 实时图与预存的参考图在计算机中进行相关匹配, 确定实时图在参考图中的最相似点（匹配点）, 从而确定出航行器的精确实时位置。图2为地磁导航与捷联惯导的子系统示意图。

图2 地磁导航子系统设计原理图
本文中所设计的地磁导航的具体实现包括三个步骤：第一步，利用地磁传感器处理后所测得磁偏角和磁倾角作为匹配参数进行图匹配,获取粗位置信息；第二步，由地磁场模型解算地磁场强度,采用逐次迭代逼进地磁场强度测量值的方式以获取精确的位置信息,同时对2个点之间进行精确计时,进而解算出载体的即时速度并由此获得速度信息；第三步，以地磁匹配系统获取的速度、位置信息与惯导系统输出的位置、速度信息的差值作为量测值,经过卡尔曼滤波估计误差，作为联邦滤波器的一个子滤波器,再与惯导主滤器进行数据融合。

3组合导航自适应卡尔曼算法
对于水下航行器组合导航系统，由于海况的复杂性，系统量测噪声统计特性随实际工作环境而改变，初始的先验值并不能代表实际工作时的噪声情况。虽然经过对捷联式惯性导航系统和地磁匹配系统的大量反复试验，可以获得试验时系统噪声的统计特性，但是实际工作时量测噪声的统计特性仍然是未知的。针对此问题，采用简化的Sage-Husa自适应滤波方法，解决了传统卡尔曼滤波容易发散的问题。。
设系统的状态模型和量测模型方程为
（2）
和为互不相关的白噪声序列，其方差和均值分别为
，，，
标准卡尔曼算法描述如下：
（3）
（4）
（5）
（6）
（7）
（8）
Sage-Husa自适应滤波是在标准卡尔曼滤波的基础上，同时估计过程噪声和量测噪声的统计特性。事实上，组合导航系统中的系统噪声一般具有稳定性，所以本文仅对量测噪声进行估计。（9） （10）
其中，取0.95，式（3）至式（10）即为简化Sage-Husa自适应滤波算法。

4 自适应联邦滤波模型建立
4．1系统状态方程
导航坐标系取当地的地理坐标系，其中X Y Z 分别指向东北天建立状态方程如下
（11）
对于惯导系统，作为参考系统的惯导系统,取姿态误差角（）、速度误差（）位置误差（）陀螺常值漂移（）陀螺一阶马尔科夫过程（） 加速度计零位漂移（）作为系统的状态变量,其中：

和的推导过程及具体内容略。
对于多普勒子系统，取对于地磁子系统，取
。
INS/DVS子系统和INS/DNS子系统状态方程分别为：

4．2 SINS/DVS量测方程的建立

4．3 SINS/DNS量测方程的建立
由于利用三轴捷联磁场计测量地磁场获得地磁场在载体坐标系3 个坐标轴方向的分量分别为以磁偏角和磁倾角匹配获取的粗略位置的经纬度作为循环计算的经纬度初始值，以高度计测量的高度作为循环计算的高度初始值 ,逐次代入地磁场模型解算地磁场强度，当最小时,最后一次代入的经纬度和高度即为地磁导航系统输出的位置，辅以2个点精确计时则可得到速度。

5 仿真
根据提出的自适应联邦滤波算法，对水下航行器组合导航进行了仿真。初始仿真条件为:陀螺漂移为0. 1°/ h ,载体位置北纬34.5°,东经105.3°高度10m ,初始东向速度为50m/s, 北向速度为50 m/s，天向速度为零。陀螺漂移白噪声漂移均方根0. 1°/ h ,加速度计零偏0. 001 g ,随机均方误差为0. 001 g ,速度误差为0. 1 m/。在进行自适应滤波时，主滤波器初始分配系数取0.5，子滤波器均取0.25。仿真时间300秒。

图 3 速度误差曲线图

图 4 位置误差曲线图
仿真结果表明，在300秒的仿真时间里，其中东向速度、北向速度误差在初始阶段抖动是比较剧烈的，主要原因是卡尔曼滤波的初始P阵取的较大，滤波器不能保持无偏造成的。在40秒左右之后误差增长趋于稳定，是因为初始值P对滤波的影响逐渐减弱，且自适应算法对量测噪声R的在线修正。纬度、经度误差则基本保持在导航精度内，误差保持在5米范围之内。
6结论：
本文以AUV组合导航系统为应用背景,针对水下导航对组合导航特殊要求, 引入地磁导航，设计了SINS/GNS/DVS组合导航系统。为了克服地磁匹配系统和惯导系统测量方差为时变的影响，采用自适应卡尔曼联邦滤波器进行误差估计和数据融合，通过实时估计和修正观测噪声的统计特性达到降低模型误差、抑制滤波发散的目的。仿真表明,改进的自适应算法能够同时保证信息融合的精度和稳定性,滤波性能得到了改善. 通过在MATLAB环境下仿真，本文所提出的方案得到了实现，也验证了改进的自适应信息融合算法的优良特性和较高的实际应用价值。
目前地磁组合导航技术在导航、制导领域具有很大的潜力。它可以作为GPS的备份系统, 在GPS 失效的情况下校正惯导的积累误差,可以用于姿态方位的测量,以及远程车辆的监控和无人机的导航。随着地磁传感器技术的提高,地磁测量精度和速度都会得到提高，地磁导航系统的应用范围还将进一步被扩展。

参考文献
[1]Goldenberg F. Geomagnetic Navigation beyond Magnetic Compass[C]. PLANS 2006, San Diego, California. 684-694.
[2]Zheng Yufeng, Edward A Essock, Bruce C Hansen. Advanced discrete wavelet transform fusion algorithm and its optimization by using the metric of image quality index[J] .Optical Engineering, 2005, 44 (3) : 1-12.
[3]鲁平,赵龙，陈哲.改进的Sage-Husa 自适应滤波及其应用[J].系统仿真学报，2007 ，19（15）：13-15.
[4]晏登洋，任建新，牛尔卓.惯性/地磁组合导航系统自适应卡尔曼滤波算法研究[J].电光与控制，2007，14（6）:74-79.

作者简介：
刘明雍(1971-),男，湖北石首人，教授，博士生导师.主要从事控制，制导与仿真方向的研究.
周志远(1984-)，男，内蒙古呼和浩特人，硕士研究生.研究方向为先进导航与控制.
赵涛（1982-），男，陕西西安人，硕士研究生.主要从事先进控制理论与应用研究.
联系方式：
通信地址：西北工业大学航海学院409室
邮编：710072
手机：[phone]
Email: zhiyuan_zhou@