基于卡爾曼濾波的UUV導(dǎo)航定位系統(tǒng)GPS異常點剔除方法

張晉斌

【摘要】本文闡述了一種基于卡爾曼濾波算法的水下航行體導(dǎo)航定位系統(tǒng)中GPS軌跡異常點剔除方法。主要說明了基于卡爾曼濾波導(dǎo)航定位系統(tǒng)的算法模型及軟件處理流程，包括數(shù)據(jù)處理方法、數(shù)據(jù)處理模型、數(shù)據(jù)處理流程及仿真結(jié)果分析。由于采用了卡爾曼濾波算法，設(shè)計的水下航行體導(dǎo)航定位系統(tǒng)不但提高了自動控制實時性而且增強了航跡控制的精確性，該方法在實際應(yīng)用中取得了良好的效果。

【關(guān)鍵詞】UUV;導(dǎo)航定位系統(tǒng);GPS

1.引言

水下航行體（Unmanned Undersea Vehicle，簡稱為UUV）UUV最早出現(xiàn)于20世紀(jì)60年代。發(fā)展初期，主要用于深水勘探、沉船打撈等民用領(lǐng)，近年來，隨推進(jìn)器、導(dǎo)航、控制系統(tǒng)和傳感器的發(fā)展，及對降低戰(zhàn)斗人員傷亡的考慮，UUV的軍事用途得到高度重視，在水下偵查、反潛、反水雷領(lǐng)域中得到空前發(fā)展。自主航行控制系統(tǒng)是典型的嵌入式控制系統(tǒng)，對軟硬件提出很高的要求，由于計算機技術(shù)的飛速發(fā)展，硬件可靠性技術(shù)日趨成熟，UUV利用GPS進(jìn)行導(dǎo)航定位技術(shù)得到廣泛的應(yīng)用，精度也得到了普遍的認(rèn)可。而軟件處理能力方面，尤其是導(dǎo)航定位系統(tǒng)異常點方面，對水下航行體的使用安全性和穩(wěn)定性產(chǎn)生很大的負(fù)面影響，甚至造成丟失等災(zāi)難性的后果，因此如何確保水下航行體控制軟件的導(dǎo)航定位的精確性具有重要的現(xiàn)實意義。

2.卡爾曼濾波

卡爾曼濾波的一般形式為：

（1）

其中.k=0，1，……

（2）

由Kalman所給出的最優(yōu)線性濾波估計是由下面的遞歸矩陣公式?jīng)Q定的，即：

（3）

初始條件，這里K（k+1）稱為卡爾曼增益。其表達(dá)式為：

（4）

卡爾曼增益中有一項是，表示的是單步預(yù)測誤差協(xié)方差矩陣。

（5）

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

濾波誤差的誤差協(xié)方差矩陣的表達(dá)式為：

（6）

k=0，1……

3.GPS軌跡異常數(shù)據(jù)處理

3.1 數(shù)據(jù)處理方法

采用卡爾曼濾波判別法來處理GPS的異常數(shù)據(jù)點。計劃航線是航行體預(yù)設(shè)的規(guī)定路線，航行偏差是指航行體隨波浪等起伏引起的相對位置偏移的、允許發(fā)生偏移的距離。直接從GPS終端接收的數(shù)據(jù)是摻雜著波浪、搖擺以及畸變等數(shù)據(jù)的，我們要把真實的航跡數(shù)據(jù)提取出來，就需要做一些濾波的處理。普通的濾波器雖然可以把波浪的數(shù)據(jù)提取出來，但由于GPS受搖擺、天氣等因素的影響很大，數(shù)據(jù)中可能摻雜著很多無效或異常的數(shù)據(jù)，我們在剔除這一些點之后仍然需要將它們補齊以保持資料的完整性和真實性，從而可以精確的計算出水下航行體的航跡數(shù)據(jù)。

3.2 數(shù)據(jù)處理模型

卡爾曼濾波是一種很好處理異常數(shù)據(jù)的方式，它是在時域中通過狀態(tài)空間構(gòu)造問題來解決非平穩(wěn)和矢量估計問題。它不僅需要給出自相關(guān)函數(shù)或者能量譜密度，同時也要以狀態(tài)變量的形式給出卡爾曼濾波所需要的一些信息。這些信息是從所形成的狀態(tài)空間中的狀態(tài)轉(zhuǎn)移矩陣、激勵矩陣和觀測矩陣中得到的。這里我們的預(yù)測模型為：

（7）

令，則：

（8）

觀測模型為：

（9）

參數(shù)模型為：

圖1 控制軟件總體流程圖

3.3 數(shù)據(jù)處理流程

通過軟件編程和試驗來檢驗本方法，仿真所用數(shù)據(jù)來自于受各種環(huán)境因數(shù)影響航行體定位的GPS數(shù)據(jù)，采用VC++及Matlab等工具進(jìn)行仿真計算，數(shù)據(jù)處理軟件控制總體流程如圖1所示。

3.4 仿真結(jié)果分析

為驗證本濾波方法的可行性及可靠性，我們對水下航行體的航行姿態(tài)控制進(jìn)行了模擬仿真試驗，圖2中藍(lán)色虛點為未開濾波控制算法時的GPS軌跡數(shù)據(jù)曲線，明顯可以看出此時數(shù)據(jù)較為發(fā)散且受到外界影響偏差較大，偶爾有畸變點產(chǎn)生，紅色實線為經(jīng)過濾波后軌跡數(shù)據(jù)曲線，此時水下航行體的航跡控制較為收斂和平穩(wěn)。由圖中可以看出，經(jīng)過把卡爾曼濾波算法結(jié)合水下航行體主控軟件的航跡控制模式相結(jié)合明顯提高了系統(tǒng)自動控制的穩(wěn)定性和控制精度。

4.總結(jié)

經(jīng)驗證，基于卡爾曼濾波算法導(dǎo)航定位系統(tǒng)的水下航行體控制系統(tǒng)，將導(dǎo)航定位系統(tǒng)的穩(wěn)定性和精確性與主控軟件的任務(wù)特性有機的結(jié)合起來，極大的提高了水下航行體控制系統(tǒng)的質(zhì)量和航跡控制精度，在實際應(yīng)用中能夠充分發(fā)揮各個模塊之間的功能，并滿足水下航行體控制系統(tǒng)設(shè)計需求。

參考文獻(xiàn)

[1]許真珍，封錫盛.多UUV網(wǎng)絡(luò)的研究現(xiàn)狀與發(fā)展[J].機器人，2007，29（2）：186-192.

[2][美]Lonnie C Ludeman.隨機過程濾波、估計與檢測[M].北京：電子工業(yè)出版社，2005：279-309.

[3]張滿生，張學(xué)莊，陳保平等.新型GPS動態(tài)定位自適應(yīng)卡爾曼濾波方法[J].中南工業(yè)大學(xué)學(xué)報，2003（5）：544-547.

[4]俞海紅，陳素珊，何勇.GPS定位試驗及提高定位精度的方法研究[J].浙江大學(xué)學(xué)報，2004，30（6）：562-667.

[5]孫寧，劉潔瑜，錢培賢.多線程技術(shù)在加速度計數(shù)據(jù)采集中的實現(xiàn)口[J].國外電子測量技術(shù)，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.