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

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

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

0 引言

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

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

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

1 船舶Nomoto模型與參數(shù)辨識(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ù)雜，水動(dòng)力參數(shù)居多，不能直接用于狀態(tài)觀測(cè)器與控制器的設(shè)計(jì)，且由于船舶在大洋航行時(shí)主要關(guān)注航向、航向角速度等狀態(tài)的信息，因此以Nomoto模型[6]代替MMG 模型進(jìn)行狀態(tài)觀測(cè)器的設(shè)計(jì)，一階線性Nomoto模型如下式：

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

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

式中Ts為 離散化的采樣時(shí)間。

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

式中：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3）計(jì)算卡爾曼增益，如下式：

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

3 仿真驗(yàn)證

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

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

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

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

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

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

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

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

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

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

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

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

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

4 結(jié) 語(yǔ)

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