鄭涵，俞孟蕻，袁偉

江蘇科技大學(xué)電子信息學(xué)院，江蘇鎮(zhèn)江212003

0 引言

目前，許多工程船都安裝了動(dòng)力定位系統(tǒng)。動(dòng)力定位系統(tǒng)主要用于在不借助錨泊系統(tǒng)的情況下，通過(guò)控制器的計(jì)算使船舶依靠自身推進(jìn)系統(tǒng)抵抗風(fēng)、浪、流等環(huán)境的影響，以保持在某目標(biāo)位置或者按照一定的軌跡航行，從而完成各種作業(yè)功能。動(dòng)力定位系統(tǒng)的主要目的是實(shí)現(xiàn)船舶縱蕩、橫蕩和艏搖3個(gè)自由度的控制，其優(yōu)點(diǎn)是不受水深的限制，定位精確，機(jī)動(dòng)性強(qiáng)。若要在動(dòng)力定位系統(tǒng)中建立船舶運(yùn)動(dòng)模型，就需確定模型中的參數(shù)值［1］。近年來(lái)，系統(tǒng)辨識(shí)理論的快速發(fā)展為船舶運(yùn)動(dòng)模型的建立提供了新的思路和方法。Fossen等［2］采用的擴(kuò)展卡爾曼濾波方法已被廣泛用于船舶運(yùn)動(dòng)模型參數(shù)的辨識(shí)中。隨后，邊信黔等［3］提出了動(dòng)力定位控制系統(tǒng)模型參數(shù)的離線最速下降尋優(yōu)辨識(shí)方法，采用該方法不僅節(jié)約了辨識(shí)時(shí)間，減少了試驗(yàn)費(fèi)用，而且還提高了動(dòng)力定位系統(tǒng)的工作效率。邢家偉［4］采用遺忘因子最小二乘法與自適應(yīng)遺傳算法相結(jié)合的參數(shù)辨識(shí)方法，對(duì)船舶運(yùn)動(dòng)數(shù)學(xué)模型中的慣性矩陣和線性水動(dòng)力阻尼矩陣進(jìn)行了辨識(shí)，證明該方法對(duì)其矩陣的辨識(shí)有效、可行。李春風(fēng)等［5］利用免疫遺傳神經(jīng)網(wǎng)絡(luò)對(duì)艦船水動(dòng)力系數(shù)進(jìn)行了辨識(shí)。丁彥侃［6］提出了一種改進(jìn)的并行擴(kuò)展卡爾曼濾波算法，用來(lái)辨識(shí)船舶運(yùn)動(dòng)模型參數(shù)。由于非線性誤差積累等方面的問(wèn)題，參數(shù)辨識(shí)的效果往往不好。本文將根據(jù)動(dòng)力定位系統(tǒng)設(shè)計(jì)的需要，提出一種辨識(shí)效果更好的反饋粒子濾波算法［7-12］來(lái)辨識(shí)模型中的參數(shù)，通過(guò)辨識(shí)實(shí)驗(yàn)，仿真驗(yàn)證該方法的有效性和可靠性。

1 船舶數(shù)學(xué)模型

船舶動(dòng)力定位系統(tǒng)主要控制船舶在水平面的3個(gè)自由度的運(yùn)動(dòng)，分別為縱蕩、橫蕩和艏搖。為了描述船舶的水平面運(yùn)動(dòng)，建立了如圖1所示的坐標(biāo)系。首先，建立大地（Earth-Centered，Earth-Fixed，ECEF）坐標(biāo)系 XOY；其次，在船體上建立船體坐標(biāo)系xoy，坐標(biāo)平面xoy在水平面內(nèi)，原點(diǎn)o位于船舶重心處，ox軸指向船艏，oy軸指向船舶右舷正橫方向。

圖1 兩種坐標(biāo)系的定義Fig.1 The definition of ECEF coordinate system and body coordinate system

以船的重心為參考點(diǎn)，根據(jù)船舶操縱性理論和動(dòng)力定位系統(tǒng)的特征，建立船舶數(shù)學(xué)模型［13］：

式中：TKμ為推進(jìn)器、舵的控制力和力矩矢量，其中T為實(shí)船控制配置矩陣，μ為由實(shí)船操縱中給出的控制命令，K為待辨識(shí)的推進(jìn)器系數(shù)矩陣；w為由風(fēng)、浪、流等作用力組成的環(huán)境干擾力；，包括大地坐標(biāo)下的位置和艏搖角度ψ；，包括船舶在隨船坐標(biāo)系下的縱蕩速度u、橫蕩速度υ和艏搖角速度r；R(ψ)為大地坐標(biāo)系和隨船坐標(biāo)系的轉(zhuǎn)換矩陣，

M，D分別為質(zhì)量矩陣和阻尼參數(shù)矩陣，

式中：m為船舶質(zhì)量；Iz為隨船坐標(biāo)系xoy的轉(zhuǎn)動(dòng)慣量；xg為船舶質(zhì)心到隨船坐標(biāo)原點(diǎn)的距離，將隨船坐標(biāo)系的原點(diǎn)取在船舶重心上，則為船舶在各個(gè)運(yùn)動(dòng)方向上的附加質(zhì)量系數(shù)，可以通過(guò)理論估算［14］得出；Xu，Yv，Yr，Nv，Nr為船舶在各個(gè)運(yùn)動(dòng)方向上的水動(dòng)力阻尼系數(shù)，線性水動(dòng)力阻尼矩陣D為需要辨識(shí)的未知量。

2 反饋粒子濾波算法

由于擴(kuò)展卡爾曼濾波的非線性誤差積累問(wèn)題，參數(shù)辨識(shí)的精度往往不高［15］。本文將采用一種免重采樣非線性濾波算法——反饋粒子濾波（Feedback Particle Filter，F(xiàn)PF）算法來(lái)避免該問(wèn)題。FPF算法是將每個(gè)粒子作為一個(gè)可控的隨機(jī)系統(tǒng)，通過(guò)求解歐拉—拉格朗日邊界值問(wèn)題，得到在Kullback-Leibler距離最小意義下的最優(yōu)反饋控制律，然后再根據(jù)前一時(shí)刻對(duì)粒子自身的狀態(tài)進(jìn)行校正和更新。不同于粒子濾波，反饋粒子濾波算法不需要重采樣步驟，可以有效避免因估計(jì)狀態(tài)維數(shù)增加而導(dǎo)致的計(jì)算量增大［15］問(wèn)題，易于實(shí)際工程應(yīng)用。

假設(shè)連續(xù)時(shí)間動(dòng)態(tài)系統(tǒng)的狀態(tài)方程和觀測(cè)方程如下：

式中：xt為系統(tǒng)狀態(tài)向量，xt∈Rd，其中d為系統(tǒng)的狀態(tài)維數(shù)；zt為觀測(cè)向量；zt∈Rm，其中m為系統(tǒng)的觀測(cè)維數(shù)；ft(·)為系統(tǒng)狀態(tài)方程；ht(·)為觀測(cè)方程；wt和rt分別為系統(tǒng)過(guò)程噪聲和觀測(cè)噪聲。對(duì)連續(xù)時(shí)間系統(tǒng)采用歐拉法進(jìn)行離散化處理，粒子的離散化方程為

式中：Kt為一個(gè)d×m維的矩陣；p=1,2,3,…,d；q=1,2,3,…,m；分別為系統(tǒng)的狀態(tài)預(yù)測(cè)向量和觀測(cè)預(yù)測(cè)向量：

由以上FPF的算法公式可以看出，F(xiàn)PF基于誤差更新的反饋增益控制結(jié)構(gòu)仍然存在，其與線性卡爾曼濾波（Linear Kalman Filter，LKF）的反饋增益結(jié)構(gòu)對(duì)比如圖2所示。

圖2 反饋增益結(jié)構(gòu)比較Fig.2 Comparison of feedback gain structure

3 運(yùn)動(dòng)模型參數(shù)辨識(shí)

為便于進(jìn)行實(shí)驗(yàn)，將式（1）、式（2）中的船舶運(yùn)動(dòng)數(shù)學(xué)模型解耦，分離出縱蕩運(yùn)動(dòng)數(shù)學(xué)模型以及橫蕩與艏搖運(yùn)動(dòng)數(shù)學(xué)模型。船舶在僅主推進(jìn)器工作的情況下采用縱蕩運(yùn)動(dòng)數(shù)學(xué)模型，在僅側(cè)推進(jìn)器工作的情況下采用橫蕩與艏搖運(yùn)動(dòng)數(shù)學(xué)模型。分別對(duì)這2個(gè)模型進(jìn)行參數(shù)辨識(shí)海上實(shí)驗(yàn)，記錄船舶在這2種狀態(tài)下的實(shí)驗(yàn)數(shù)據(jù)。利用數(shù)學(xué)模型和實(shí)驗(yàn)數(shù)據(jù)，在Matlab平臺(tái)上編寫(xiě)FPF算法程序，進(jìn)行仿真實(shí)驗(yàn)。

3.1 縱蕩參數(shù)辨識(shí)

船舶只使用主推進(jìn)器，使船舶縱向前進(jìn)，辨識(shí)縱蕩阻尼參數(shù)Xu和2個(gè)主推進(jìn)器系數(shù)k1，k2。

由船舶運(yùn)動(dòng)數(shù)學(xué)模型分離出的縱蕩運(yùn)動(dòng)模型，得到狀態(tài)方程和觀測(cè)方程為

式中：u為狀態(tài)變量；μ1為輸入變量；z1為輸出變量；w1，r1分別為縱蕩運(yùn)動(dòng)模型中的過(guò)程噪聲和觀測(cè)噪聲；Xu與K1為需要辨識(shí)的參數(shù)。

3.2 橫蕩與艏搖參數(shù)辨識(shí)

船舶只使用側(cè)推進(jìn)器，使船舶橫向前進(jìn)并產(chǎn)生艏搖角度，辨識(shí)橫蕩與艏搖阻尼系數(shù)Yv，Yr，Nv，Nr和側(cè)推進(jìn)器系數(shù)k3。

由于橫蕩與艏搖運(yùn)動(dòng)的速度項(xiàng)和加速度項(xiàng)的系數(shù)有耦合，為便于求解，引入了一個(gè)動(dòng)量h，定義h=Mv。從船舶運(yùn)動(dòng)數(shù)學(xué)模型中分離出橫蕩與艏搖運(yùn)動(dòng)模型，得到狀態(tài)方程和觀測(cè)方程為

其中，

式中：hvr為狀態(tài)變量；μvr為輸入變量；zvr為輸出變量；wvr，rvr分別為橫蕩與艏搖運(yùn)動(dòng)模型中的過(guò)程噪聲和觀測(cè)噪聲；矩陣Dvr與Kvr為需要辨識(shí)的參數(shù)。

4 實(shí)驗(yàn)及分析

4.1 研究對(duì)象

本實(shí)驗(yàn)數(shù)據(jù)由一艘大型自航耙吸挖泥船“新?；?”號(hào)于海上實(shí)驗(yàn)中獲得。該船總長(zhǎng)128 m，型寬25.6 m，型深9.2 m，設(shè)計(jì)吃水6.8 m，總載重量13 710 t，滿載航速15 kn。推進(jìn)系統(tǒng)由2個(gè)主推進(jìn)器和1個(gè)側(cè)推進(jìn)器組成。選取合適的實(shí)船數(shù)據(jù)600組，得到船舶在固定坐標(biāo)系的位置(X,Y)，船的艏搖角度ψ，船舶航速ν，左、右兩側(cè)主推進(jìn)器推力μ11和μ12，以及側(cè)推進(jìn)器推力μvr。設(shè)置采樣間隔時(shí)間Δt=0.1 s，粒子數(shù)N=500，過(guò)程噪聲方差Q=0.1Id×d，觀測(cè)噪聲方差R=0.01Im×m。

4.2 參數(shù)辨識(shí)實(shí)驗(yàn)

采用FPF算法分別進(jìn)行縱蕩辨識(shí)實(shí)驗(yàn)及橫蕩與艏搖辨識(shí)實(shí)驗(yàn)，辨識(shí)結(jié)果如表1所示。

表1 參數(shù)辨識(shí)結(jié)果Table 1 Parameter identification results

4.3 仿真結(jié)果驗(yàn)證

分別采用FPF和擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）這2種算法對(duì)推進(jìn)器系數(shù)k1，k2，k3進(jìn)行辨識(shí)，辨識(shí)結(jié)果如圖3所示。

圖3（a）和圖3（b）中的k1，k2由縱蕩模型實(shí)驗(yàn)得出，圖3（c）中的k3由橫蕩與艏搖模型實(shí)驗(yàn)得出。由圖3可以看出，F(xiàn)PF算法的振蕩幅度較EKF算法小很多，在遞推100次左右趨于平穩(wěn)，而EKF算法則在遞推200次左右才趨于穩(wěn)定，且圖3（b）中顯示，EKF算法在趨于穩(wěn)定之后仍然存在明顯的波動(dòng)?？梢?jiàn)，F(xiàn)PF算法的收斂速度更快，穩(wěn)定性更好。

圖3 參數(shù)值k1，k2，k3的仿真結(jié)果對(duì)比Fig.3 Comparison of simulation results ofk1，k2，k3

4.4 模型可靠性分析

通過(guò)對(duì)模型參數(shù)的估計(jì)，建立船舶運(yùn)動(dòng)數(shù)學(xué)模型，用模型進(jìn)行運(yùn)動(dòng)仿真。由于實(shí)驗(yàn)結(jié)果的數(shù)值較大，將仿真值與實(shí)測(cè)數(shù)據(jù)進(jìn)行對(duì)比不易于觀察，因此，繪制了如圖4所示的二者的差值仿真圖。在縱蕩實(shí)驗(yàn)中得到船舶運(yùn)動(dòng)距離和航速的仿真值，然后與實(shí)測(cè)數(shù)據(jù)進(jìn)行對(duì)比得到距離差值和航速差值；在橫蕩與艏搖實(shí)驗(yàn)中得到船舶運(yùn)動(dòng)距離和艏向角的仿真值，然后與實(shí)測(cè)數(shù)據(jù)進(jìn)行對(duì)比得到距離差值和艏向角差值。

仿真結(jié)果表明，在采用FPF參數(shù)辨識(shí)方法建立的船舶運(yùn)動(dòng)模型中，仿真值與實(shí)測(cè)值之間的差值穩(wěn)定在較小范圍內(nèi)，具有可靠性。圖4（a）中，由于在實(shí)驗(yàn)數(shù)據(jù)記錄等過(guò)程中產(chǎn)生的誤差峰值點(diǎn)數(shù)量較少，故忽略不計(jì)。

圖4 仿真值與實(shí)測(cè)值的差值圖Fig.4 Differential chart between simulated values and measured values

5 結(jié) 語(yǔ)

本文針對(duì)傳統(tǒng)擴(kuò)展卡爾曼濾波方法的不足，提出了一種新型的FPF算法并應(yīng)用到了船舶運(yùn)動(dòng)數(shù)學(xué)模型的參數(shù)辨識(shí)中。分離船舶運(yùn)動(dòng)模型，建立縱蕩運(yùn)動(dòng)模型及橫蕩與艏搖運(yùn)動(dòng)模型并分別進(jìn)行辨識(shí)實(shí)驗(yàn)。通過(guò)Matlab仿真，得到運(yùn)動(dòng)模型里待辨識(shí)的推進(jìn)器推力系數(shù)，發(fā)現(xiàn)辨識(shí)的收斂速度更快，穩(wěn)定性更好，證明了該算法的有效性。實(shí)驗(yàn)結(jié)果表明，本文所提出的FPF算法能夠較好地應(yīng)用到船舶運(yùn)動(dòng)數(shù)學(xué)模型的參數(shù)辨識(shí)中，可使其辨識(shí)效果與真實(shí)值更接近。該算法可靠，能夠滿足船舶模型參數(shù)辨識(shí)的需要。