基于卡爾曼濾波的潛器運(yùn)動(dòng)狀態(tài)觀測(cè)器＊

劉常波 劉 培 丁風(fēng)雷

（海軍潛艇學(xué)院作戰(zhàn)指揮系1） 青島 266071）（海軍潛艇學(xué)院研究生隊(duì)2） 青島 266071）

1 引言

潛器的自動(dòng)控制，多是采用基于反饋構(gòu)成的閉環(huán)結(jié)構(gòu)，這種結(jié)構(gòu)對(duì)內(nèi)部參數(shù)變動(dòng)和外部環(huán)境影響都有良好的抑制作用。反饋的基本類型包括“狀態(tài)反饋”和“輸出反饋”，狀態(tài)反饋可以完整的反饋系統(tǒng)的結(jié)構(gòu)信息，在性能上也遠(yuǎn)優(yōu)于輸出反饋。

但某些狀態(tài)變量在系統(tǒng)中不可測(cè)量，需要引入附加的狀態(tài)觀測(cè)器。潛器的運(yùn)動(dòng)方程是非線性微分方程，經(jīng)典的卡爾曼濾波只能處理線性方程，通過對(duì)潛器運(yùn)動(dòng)方程線性化，建立相應(yīng)的狀態(tài)觀測(cè)器，給出狀態(tài)變量的估計(jì)值。

在弱機(jī)動(dòng)時(shí)潛器的運(yùn)動(dòng)可以分解成垂直面運(yùn)動(dòng)和水平面運(yùn)動(dòng)，本文以航速穩(wěn)定的垂直面運(yùn)動(dòng)方程為例，設(shè)計(jì)其狀態(tài)觀測(cè)器，并根據(jù)某一給出水動(dòng)力系數(shù)的深潛器，建立模型，給出仿真結(jié)果。

2 潛器垂直面運(yùn)動(dòng)方程及其線性化

考慮潛器的垂直面運(yùn)動(dòng)，假定航速近似穩(wěn)定，對(duì)潛器六自由度方程簡(jiǎn)化，建立運(yùn)動(dòng)方程如式（1）所示，取潛器的重心為坐標(biāo)原點(diǎn)。

方程組（1）是簡(jiǎn)化的垂直面潛器運(yùn)動(dòng)方程，將其看作一個(gè)系統(tǒng)的狀態(tài)方程，狀態(tài)變量為垂向速度w，縱傾角速度q，縱傾角θ，深度ζ?？v傾角θ和深度ζ可由儀器直接測(cè)量，為系統(tǒng)輸出。在一定航速u下，系統(tǒng)的輸入是首升降舵角δb和尾升降舵角δs。狀態(tài)w和q，不能直接測(cè)量，是需要估計(jì)的量。理論上對(duì)輸出的縱傾角θ和深度ζ求導(dǎo)可以得出w和q，但潛器上的深度計(jì)和縱傾儀精度有限，對(duì)其求導(dǎo)不能得到較為精確的W和q，可以用卡爾曼濾波建立相應(yīng)的狀態(tài)觀測(cè)器估計(jì)w和q。經(jīng)典的卡爾曼濾波只能處理線性系統(tǒng)，狀態(tài)方程（1）是一組非線性方程，對(duì)其線性化得到垂直面線性運(yùn)動(dòng)方程（2）。

整理成標(biāo)準(zhǔn)形式如式（3）所示，由卡爾曼濾波，即可給出潛器垂直面運(yùn)動(dòng)的狀態(tài)觀測(cè)器

3 潛器垂直面運(yùn)動(dòng)的狀態(tài)觀測(cè)器

采用連續(xù)系統(tǒng)的卡爾曼濾波，潛器垂直面運(yùn)動(dòng)非線性系統(tǒng)的數(shù)學(xué)模型如下：

w（t）為隨機(jī)性的干擾輸入，ν（t）為量測(cè)噪聲，其噪聲統(tǒng)計(jì)特性如下：

其中，R1（t），R2（t）分別是w（t）及ν（t）的譜密度，δ（t）是置于t＝0處的一階脈沖函數(shù)。

圖1 潛器垂直面運(yùn)動(dòng)狀態(tài)觀測(cè)器框圖

潛器在水中的運(yùn)動(dòng)，可分成水平面運(yùn)動(dòng)和垂直面運(yùn)動(dòng)，以垂直面運(yùn)動(dòng)方程為例，建立狀態(tài)觀測(cè)器，框圖如圖1所示。

狀態(tài)估計(jì)方程如式（7）所示，估計(jì)誤差協(xié)方差陣P（t）的傳播方程如式（8）所示，卡爾曼增益陣K（t）如式（9）所示，初始條件如式（10）所示：

式中A，B，C，如式（4）所示，過程噪聲w（t）的統(tǒng)計(jì)特性R1（t）由潛器航行時(shí)的內(nèi)外環(huán)境所定，量測(cè)噪聲ν（t）的統(tǒng)計(jì)特性R2（t）由縱傾角θ深度ζ的精度確定。由框圖可知，狀態(tài)觀測(cè)器通過簡(jiǎn)化的線性模型預(yù)測(cè)狀態(tài)值，然后通過量測(cè)信息更新預(yù)測(cè)值，得到估計(jì)的狀態(tài)值。以上采用的線性模型比較簡(jiǎn)單，由此得出的預(yù)測(cè)值不夠精確，主要靠量測(cè)信息來(lái)修正。量測(cè)信息（深度和縱傾）的精度高，其中包含的信息就多，由此得到的狀態(tài)估計(jì)值就準(zhǔn)確。

以Fossen在“Guidance and Control of Ocean Vehicles”書中給出的水下潛體為例，建立模型仿真。建立非線性運(yùn)動(dòng)方程，描述潛體的真實(shí)運(yùn)動(dòng)，用上述狀態(tài)觀測(cè)器，給出系統(tǒng)狀態(tài)的估計(jì)值。

4 仿真結(jié)果

編程仿真，分析比較非線性模型計(jì)算出的系統(tǒng)狀態(tài)量和觀測(cè)器給出的狀態(tài)估計(jì)量，狀態(tài)量縱傾角θ和深度ζ可由儀器量測(cè)，只比較垂向速度w和縱傾角速度q的估計(jì)值與真實(shí)值。

運(yùn)行程序，給出量測(cè)輸出θ（縱傾角）和ζ（深度）的精度分別為1、0.1、0.001、0.001（度或米）的仿真結(jié)果，潛器速度4節(jié)，艏升降舵0°，艉升降舵打上浮舵5°。

潛器速度4節(jié)，艏升降舵0度，艉升降舵打上浮舵5度時(shí)，不同量測(cè)精度下，狀態(tài)觀測(cè)器的估計(jì)誤差，如表1所示。

表1 不同精度下狀態(tài)觀測(cè)器的估計(jì)誤差

圖2 縱傾角速度q的真實(shí)值與估計(jì)值比較

圖3 垂向速度w的真實(shí)值與估計(jì)值比較

5 結(jié)語(yǔ)

根據(jù)實(shí)際的仿真結(jié)果可以看出，觀測(cè)器的估計(jì)誤差與量測(cè)輸出的精度有關(guān)，量測(cè)輸出的精度越高，觀測(cè)器的估計(jì)誤差就越小。因此采用高精度的深度計(jì)和縱傾儀，可以降低觀測(cè)器的估計(jì)誤差。卡爾曼濾波主要靠模型預(yù)測(cè)和新的信息修正來(lái)估計(jì)狀態(tài)，優(yōu)化預(yù)測(cè)模型，采用擴(kuò)展卡爾曼濾波，能得到更好的效果。由仿真結(jié)果還可以看出，當(dāng)潛器的運(yùn)動(dòng)狀態(tài)變化緩慢時(shí)，觀測(cè)器能更好地跟蹤潛器的真實(shí)運(yùn)動(dòng)狀態(tài)。

[1]施生達(dá).潛艇操縱性[M].北京：國(guó)防工業(yè)出版社，1995

[2]胡坤，吳超.潛艇垂直面運(yùn)動(dòng)仿真及分析[J].青島大學(xué)學(xué)報(bào)，2003，16（4）

[3]張晶.潛艇運(yùn)動(dòng)建模及簡(jiǎn)化技術(shù)研究[D].哈爾濱工程大學(xué)碩士研究生學(xué)位論文，2009

[4]賈欣樂，楊鹽生.船舶運(yùn)動(dòng)數(shù)學(xué)模型[M].大連：大連海事出版社，1999

[5]Thor I.Fossen.Guidance and Control of Ocean Vehicles[M].University of Trondheim，1994