劉常波 劉 培 丁風雷

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

1 引言

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

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

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

2 潛器垂直面運動方程及其線性化

考慮潛器的垂直面運動，假定航速近似穩(wěn)定，對潛器六自由度方程簡化，建立運動方程如式（1）所示，取潛器的重心為坐標原點。

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

整理成標準形式如式（3）所示，由卡爾曼濾波，即可給出潛器垂直面運動的狀態(tài)觀測器

3 潛器垂直面運動的狀態(tài)觀測器

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

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

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

圖1 潛器垂直面運動狀態(tài)觀測器框圖

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

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

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

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

4 仿真結(jié)果

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

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

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

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

圖2 縱傾角速度q的真實值與估計值比較

圖3 垂向速度w的真實值與估計值比較

5 結(jié)語

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

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

[2]胡坤，吳超.潛艇垂直面運動仿真及分析[J].青島大學學報，2003，16（4）

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

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

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