基于擴(kuò)張狀態(tài)卡爾曼原理的海浪濾波算法

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

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

式中：

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

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

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

2 海浪濾波算法設(shè)計

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

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

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

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

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

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

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

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

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

基于卡爾曼濾波原理[9]根據(jù)船舶航行時的實(shí)時輸入輸出數(shù)據(jù)重構(gòu)運(yùn)動狀態(tài)信息?？柭鼮V波的遞推過程如下：

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

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

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

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

3 仿真驗(yàn)證

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

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

圖1 Z 型試驗(yàn)仿真結(jié)果Fig.1 Simulation result of Z?type test

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

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

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

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

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

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

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

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

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

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

4 結(jié) 語

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