基于卡爾曼濾波的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.