基于卡尔曼滤波的UUV导航定位系统GPS异常点剔除方法

张晋斌

【摘要】本文阐述了一种基于卡尔曼滤波算法的水下航行体导航定位系统中GPS轨迹异常点剔除方法。主要说明了基于卡尔曼滤波导航定位系统的算法模型及软件处理流程，包括数据处理方法、数据处理模型、数据处理流程及仿真结果分析。由于采用了卡尔曼滤波算法，设计的水下航行体导航定位系统不但提高了自动控制实时性而且增强了航迹控制的精确性，该方法在实际应用中取得了良好的效果。

【关键词】UUV;导航定位系统;GPS

1.引言

水下航行体（Unmanned Undersea Vehicle，简称为UUV）UUV最早出现于20世纪60年代。发展初期，主要用于深水勘探、沉船打捞等民用领，近年来，随推进器、导航、控制系统和传感器的发展，及对降低战斗人员伤亡的考虑，UUV的军事用途得到高度重视，在水下侦查、反潜、反水雷领域中得到空前发展。自主航行控制系统是典型的嵌入式控制系统，对软硬件提出很高的要求，由于计算机技术的飞速发展，硬件可靠性技术日趋成熟，UUV利用GPS进行导航定位技术得到广泛的应用，精度也得到了普遍的认可。而软件处理能力方面，尤其是导航定位系统异常点方面，对水下航行体的使用安全性和稳定性产生很大的负面影响，甚至造成丢失等灾难性的后果，因此如何确保水下航行体控制软件的导航定位的精确性具有重要的现实意义。

2.卡尔曼滤波

卡尔曼滤波的一般形式为：

（1）

其中.k=0，1，……

（2）

由Kalman所给出的最优线性滤波估计是由下面的递归矩阵公式决定的，即：

（3）

初始条件，这里K（k+1）称为卡尔曼增益。其表达式为：

（4）

卡尔曼增益中有一项是，表示的是单步预测误差协方差矩阵。

（5）

初始条件，，k=0，1……

滤波误差的误差协方差矩阵的表达式为：

（6）

k=0，1……

3.GPS轨迹异常数据处理

3.1 数据处理方法

采用卡尔曼滤波判别法来处理GPS的异常数据点。计划航线是航行体预设的规定路线，航行偏差是指航行体随波浪等起伏引起的相對位置偏移的、允许发生偏移的距离。直接从GPS终端接收的数据是掺杂着波浪、摇摆以及畸变等数据的，我们要把真实的航迹数据提取出来，就需要做一些滤波的处理。普通的滤波器虽然可以把波浪的数据提取出来，但由于GPS受摇摆、天气等因素的影响很大，数据中可能掺杂着很多无效或异常的数据，我们在剔除这一些点之后仍然需要将它们补齐以保持资料的完整性和真实性，从而可以精确的计算出水下航行体的航迹数据。

3.2 数据处理模型

卡尔曼滤波是一种很好处理异常数据的方式，它是在时域中通过状态空间构造问题来解决非平稳和矢量估计问题。它不仅需要给出自相关函数或者能量谱密度，同时也要以状态变量的形式给出卡尔曼滤波所需要的一些信息。这些信息是从所形成的状态空间中的状态转移矩阵、激励矩阵和观测矩阵中得到的。这里我们的预测模型为：

（7）

令，则：

（8）

观测模型为：

（9）

参数模型为：

图1 控制软件总体流程图

3.3 数据处理流程

通过软件编程和试验来检验本方法，仿真所用数据来自于受各种环境因数影响航行体定位的GPS数据，采用VC++及Matlab等工具进行仿真计算，数据处理软件控制总体流程如图1所示。

3.4 仿真结果分析

为验证本滤波方法的可行性及可靠性，我们对水下航行体的航行姿态控制进行了模拟仿真试验，图2中蓝色虚点为未开滤波控制算法时的GPS轨迹数据曲线，明显可以看出此时数据较为发散且受到外界影响偏差较大，偶尔有畸变点产生，红色实线为经过滤波后轨迹数据曲线，此时水下航行体的航迹控制较为收敛和平稳。由图中可以看出，经过把卡尔曼滤波算法结合水下航行体主控软件的航迹控制模式相结合明显提高了系统自动控制的稳定性和控制精度。

4.总结

经验证，基于卡尔曼滤波算法导航定位系统的水下航行体控制系统，将导航定位系统的稳定性和精确性与主控软件的任务特性有机的结合起来，极大的提高了水下航行体控制系统的质量和航迹控制精度，在实际应用中能够充分发挥各个模块之间的功能，并满足水下航行体控制系统设计需求。

参考文献

[1]许真珍，封锡盛.多UUV网络的研究现状与发展[J].机器人，2007，29（2）：186-192.

[2][美]Lonnie C Ludeman.随机过程滤波、估计与检测[M].北京：电子工业出版社，2005：279-309.

[3]张满生，张学庄，陈保平等.新型GPS动态定位自适应卡尔曼滤波方法[J].中南工业大学学报，2003（5）：544-547.

[4]俞海红，陈素珊，何勇.GPS定位试验及提高定位精度的方法研究[J].浙江大学学报，2004，30（6）：562-667.

[5]孙宁，刘洁瑜，钱培贤.多线程技术在加速度计数据采集中的实现口[J].国外电子测量技术，2006.

[6]Guan R，Pruehsner W R，Enderle J A.The computerized environmental remote control[C]//Bioengineering Conference.Proceedings of the IEEE 26th Annual North- east.USA：Storrs，CT，2000：147-148.