周佳宇，韓俊慶，李 偉，孟凡彬

(1.海軍駐九江地區(qū)軍事代表室，江西九江 332007；2.天津航海儀器研究所九江分部，天津 300131)

0 引言

船舶在海上航行時，海浪的一階波浪力會造成船舶航向的高頻振蕩，若直接將此信號反饋給自動舵系統(tǒng)，航向控制器會輸出相應的高頻舵角指令。然而由于船舶是一個大慣性系統(tǒng)，其航向運動的響應頻率較小，當船舶的舵機系統(tǒng)接收到高于船舶航向響應頻率的舵角指令時，無論采取多大舵角，船舶的運動狀態(tài)都不會響應，這種操舵被稱為無效操舵[1]。

在自動舵系統(tǒng)中，解決無效操舵現(xiàn)象的通常做法是對海浪高頻干擾下的航向信號進行濾波，常用的濾波方法有均值濾波、低通濾波、帶通濾波與陷波濾波等[2]，但這幾種濾波方法會產(chǎn)生信號的相位滯后，這種滯后會惡化航向控制器的控制效果。為解決相位滯后問題，樊冀生等[3]在陷波濾波器基礎上進行改進，通過擴張一個狀態(tài)量對陷波濾波器的滯后進行補償，解決了陷波濾波器的滯后問題，但這種方法無法同時對船舶的速度狀態(tài)量進行重構。李楊等[4]通過分析海浪高頻模型，在潛艇線性化模型基礎上加入海浪高頻模型，通過狀態(tài)觀測器的方法將航向信號中高頻與低頻信號進行分離，并對低頻的航向角速度信號進行重構，有效減少了無效舵問題。但這種方法在風、流與海浪二階漂移力的情況下觀測結果會存在靜差，進而影響了航向控制器的控制效果。

本文針對海浪干擾下船舶控制系統(tǒng)存在的無效舵問題以及風、流干擾下觀測器存在的靜差問題，提出一種基于擴張狀態(tài)卡爾曼濾波的海浪濾波算法，并結合傳統(tǒng)的PID控制器驗證了該算法的優(yōu)勢。

1 船舶Nomoto模型與參數(shù)辨識

船舶的MMG 分離模型[5]描述如下：

式中：x，y，ψ分別為船舶在北東坐標系下的北向位移、東向位移以及首向角；u，v，r分別為船體坐標系下船舶的縱向速度、橫向速度和轉首角速度； m為船舶的質(zhì)量； mx與 my為船舶在縱向與橫向的附加質(zhì)量；Izz為船舶的轉首慣性矩； Jzz為附加慣性矩； XH，YH，NH為裸船力和力矩； XP，YP，NP為螺旋槳力和力矩；XR，YR，NR為 舵力和力矩；Xdisturb，Ydisturb，Ndisturb是干擾力和力矩。

由于船舶MMG 模型結構復雜，水動力參數(shù)居多，不能直接用于狀態(tài)觀測器與控制器的設計，且由于船舶在大洋航行時主要關注航向、航向角速度等狀態(tài)的信息，因此以Nomoto模型[6]代替MMG 模型進行狀態(tài)觀測器的設計，一階線性Nomoto模型如下式：

式中：K和T為Nomoto模型的2個重要參數(shù)，分別表示增益參數(shù)與時間常數(shù)；d為環(huán)境中風、流與海浪二階干擾力和未建模動態(tài)組成的綜合干擾項。

為了對Nomoto模型的K和T參數(shù)進行辨識，假設干擾項d=0，使用后向差分的方法對一階線性Nomoto模型進行離散化，離散化過程如下式：

式中Ts為 離散化的采樣時間。

將離散化的Nomoto方程轉化為參數(shù)估計的形式：

式中：

基于船舶巡航航速下的Z型實驗數(shù)據(jù)采用帶有遺忘因子的最下二乘算法[7]對式（4）進行參數(shù)估計，如下式：

式中：P(k)與K(k)為算法的輔助矩陣； λ為遺忘因子，其取值越小參數(shù)估計速度越快，但過小的遺忘因子會引起使估計結果震蕩，無法確定準確的估計值。

最終可求得船舶的K和T參數(shù)如下式：

2 海浪濾波算法設計

海浪對于航行中船舶的作用力分為一階波浪力與二階波浪力。其中，二階波浪力為漂移力，其作用可使船舶偏離原航向；一階波浪力為高頻作用力，其作用會使船舶航向高頻擺動。由Fossen[8]的研究成果可知，一階海浪力對船舶航向產(chǎn)生的高頻擺動運動可由如式(7)所示的二階傳遞函數(shù)表示。

式中：wH為零均值高斯白噪聲隨機過程； ωn為海浪高頻信號對船舶航向運動影響的主頻率； ζ為阻尼比。

將式(7)所示的二階傳遞函數(shù)以狀態(tài)方程的形式表示，如下式：

由船舶上的羅經(jīng)設備測得的航向角可認為是由操舵產(chǎn)生的航向角、一階海浪力產(chǎn)生的航向角和測量干擾疊加而成，如下式：

式中， υH表示零均值高斯測量噪聲。

根據(jù)船舶的Nomoto響應性模型與海浪造成的船舶航向高頻運動狀態(tài)方程(8)，可得到海浪干擾和操舵聯(lián)合作用下的船舶航向運動狀態(tài)方程：

由于船舶在航行過程中會受到風、流、浪的二階干擾力影響，Nomoto 模型中環(huán)境與未建模的綜合干擾項d會使狀態(tài)觀測的結果存在誤差。因此綜合干擾項作為擴張狀態(tài)加入到運動狀態(tài)方程(10)中，補償狀態(tài)觀測誤差。擴張后的狀態(tài)方程如下式：

使用后向差分方法對擴張狀態(tài)的狀態(tài)空間表達式進行離散化處理，離散化后的表達式如下式：

式 中：Ad=I+hA；Bd=hB；Cd=C；Xk與Uk?1為 系統(tǒng)輸入輸出的離散量；h為采樣時間。

基于卡爾曼濾波原理[9]根據(jù)船舶航行時的實時輸入輸出數(shù)據(jù)重構運動狀態(tài)信息。卡爾曼濾波的遞推過程如下：

1）確定過程噪聲方差Q、測量噪聲方差R、初始狀態(tài)X0以及初始協(xié)方差矩陣P0。

2）根據(jù)前一時刻的狀態(tài)估計值推算當前時刻的狀態(tài)先驗估計值，如下式：

3）計算卡爾曼增益，如下式：

4）根據(jù)測量值與先驗估計值計算狀態(tài)后驗估計值與誤差協(xié)方差后驗估計值，如下式：

3 仿真驗證

為了驗證本文所設計的海浪濾波算法的有效性，以47.4 m 長的執(zhí)法船海巡0208輪作為仿真對象進行算法驗證。

設置船舶航行速度為12 kn，在無環(huán)境干擾的工況下進行Z型試驗，仿真結果如圖1所示。

圖1 Z 型試驗仿真結果Fig.1 Simulation result of Z?type test

以所設計的基于遺忘因子最小二乘的參數(shù)辨識算法進行K和T參數(shù)的辨識，辨識結果如圖2所示。

從K和T參數(shù)的辨識結果中可以看出，2個參數(shù)在50 s 后趨于穩(wěn)定。為了準確的求解K和T參數(shù)，對100 s后的數(shù)據(jù)求取平均值，最終求解出的K和T參數(shù)為：

為了驗證本文設計海浪濾波器的無差觀測能力，在風向為90°的5級風、無海浪干擾環(huán)境下進行變向控制試驗，控制器選用傳統(tǒng)的PID控制器，對比分別以四階狀態(tài)方程(10)與五階狀態(tài)方程(11)設計觀測器的觀測效果，仿真結果如圖3與圖4所示。

圖3 航向觀測結果對比圖Fig.3 Comparison of course observation results

圖4 航向角速度觀測結果對比圖Fig.4 Comparison of course velocity observation results

可以看出，五階狀態(tài)方程觀測出與實際航向、航向角速度穩(wěn)定值基本重合，而四階狀態(tài)方程觀測出的航向值穩(wěn)定后存在0.6°的靜差，航向角速度值穩(wěn)定后存在0.12°/s的靜差，證明本文設計的海浪濾波算法能夠在干擾環(huán)境下準確的估計出船舶的運動狀態(tài)。

為了驗證本文設計海浪濾波器的航向高頻信號濾波能力與無效舵抑制能力，在4級海況下進行定向控制試驗，控制器選用傳統(tǒng)的PID 控制器，航行速度設置為12 kn，500 s時開啟海浪濾波器，仿真結果如圖5與圖6所示。

從圖5可以看出，500 s 開啟海浪濾波器后觀測航向的頻率與幅度明顯降低，達到了航向中由海浪一階力造成的高頻干擾濾除的目的；由圖6可以看出，500 s開啟海浪濾波器后控制器輸出舵角指令的頻率與幅度均有明顯降低，舵角的均方根由3.28°降為0.19°，證明該方法能夠有效地降低控制器產(chǎn)生的無效舵現(xiàn)象。結合圖5實際航向仿真曲線可以看出，在無效舵降低的同時航向的控制精度并未發(fā)生變化。

圖5 航向角仿真曲線Fig.5 Simulation curve of course

圖6 舵角仿真曲線Fig.6 Simulation result of rudder

4 結 語

本文針對海浪干擾下船舶控制系統(tǒng)存在的無效舵問題以及風、流干擾下觀測器存在的靜差問題，設計一種基于擴張狀態(tài)卡爾曼濾波的波浪濾波算法，通過構建的五階擴張狀態(tài)方程與卡爾曼濾波器結合，同時完成運動狀態(tài)的誤差估計與海浪高頻干擾信號的濾除。仿真結果表明，該算法能夠有效濾除船舶航向的高頻信號并正確的估計出船舶的運動狀態(tài)，并顯著減少了船舶航行時的無效舵問題。