基于無(wú)跡卡爾曼濾波的船舶運(yùn)動(dòng)數(shù)學(xué)模型辨識(shí)

秦 操

(上海交通大學(xué) 高新船舶與深海開(kāi)發(fā)裝備協(xié)同創(chuàng)新中心，上海 200240)

0 引 言

船舶運(yùn)動(dòng)數(shù)學(xué)模型是船舶運(yùn)動(dòng)仿真與控制問(wèn)題的核心[1]。運(yùn)動(dòng)數(shù)學(xué)模型可以提供船舶操縱性限制條件，輔助人或系統(tǒng)進(jìn)行路徑規(guī)劃，可以嵌入控制系統(tǒng)中以提高控制的精度，更是船舶運(yùn)動(dòng)模擬器的基礎(chǔ)。

傳統(tǒng)的建立船舶運(yùn)動(dòng)數(shù)學(xué)模型的方法是根據(jù)MMG模型結(jié)構(gòu)[2]或者Abkowitz模型結(jié)構(gòu)[3]，求解其中的水動(dòng)力導(dǎo)數(shù)。求解水動(dòng)力導(dǎo)數(shù)的方法中，經(jīng)驗(yàn)公式估算十分快速，但是準(zhǔn)確性依賴于所積累的數(shù)據(jù)和船型；約束模試驗(yàn)數(shù)據(jù)準(zhǔn)確，但是成本太高，也存在尺度效應(yīng)問(wèn)題。系統(tǒng)辨識(shí)可以從包含噪聲的數(shù)據(jù)中建立數(shù)學(xué)模型，并且可以不依賴于經(jīng)典船舶運(yùn)動(dòng)數(shù)學(xué)模型結(jié)構(gòu)，因此成為目前主流的建模方法。傳統(tǒng)的辨識(shí)方法如最小二乘法、極大似然法等由于精度過(guò)度依賴模型和初值，逐漸被以神經(jīng)網(wǎng)絡(luò)為代表的新型辨識(shí)方法取代。近年來(lái)，擴(kuò)展卡爾曼濾波器（EKF）[4]、撐向量回歸（SVR）[5]、螢火蟲(chóng)算法[6]、人工蜂群算法[7]、多目標(biāo)群智能算法[8]等都被應(yīng)用在了船舶系統(tǒng)辨識(shí)領(lǐng)域。以神經(jīng)網(wǎng)絡(luò)為代表的不依賴經(jīng)典船舶數(shù)學(xué)模型結(jié)構(gòu)的辨識(shí)方法要想得到準(zhǔn)確的辨識(shí)結(jié)果，需要輸入輸出數(shù)據(jù)中包含船舶完整的操縱性信息。以EKF為代表的對(duì)經(jīng)典船舶數(shù)學(xué)模型中的水動(dòng)力導(dǎo)數(shù)進(jìn)行辨識(shí)的方法由于已經(jīng)引入了數(shù)學(xué)模型結(jié)構(gòu)這一先驗(yàn)信息，往往需要更少的數(shù)據(jù)進(jìn)行辨識(shí)，但辨識(shí)的精度十分依賴經(jīng)典數(shù)學(xué)模型結(jié)構(gòu)，并且存在參數(shù)漂移和動(dòng)力相消[9]的問(wèn)題。

擴(kuò)展卡爾曼濾波器利用一階泰勒級(jí)數(shù)將非線性項(xiàng)線性化，因此適用于非線性系統(tǒng)，也可以進(jìn)行參數(shù)辨識(shí)，但是因?yàn)楹雎粤烁唠A項(xiàng)，在非線性較強(qiáng)時(shí)誤差大。無(wú)跡卡爾曼濾波器[10]（Unscented Kalman Filter，UKF）是無(wú)跡變換[11]（Unscented Transformation，UT）與標(biāo)準(zhǔn)卡爾曼濾波器的結(jié)合，目的是為了提高非線性系統(tǒng)濾波的精度和效率。UKF利用UT來(lái)進(jìn)行均值和協(xié)方差的非線性傳遞，其思想是近似非線性函數(shù)的概率分布而不是近似非線性函數(shù)。UKF相比EKF有著更高的精度，并且不用計(jì)算雅可比矩陣，因此被廣泛應(yīng)用于導(dǎo)航、目標(biāo)跟蹤等領(lǐng)域。

本文采用精度更高的UKF，結(jié)合成本相對(duì)較低的自航試驗(yàn)對(duì)目標(biāo)三體船進(jìn)行辨識(shí)。針對(duì)三體船自身特點(diǎn)對(duì)MMG模型結(jié)構(gòu)進(jìn)行化簡(jiǎn)，去掉了作用不大的水動(dòng)力導(dǎo)數(shù)，并設(shè)計(jì)了不同的控制方式進(jìn)行分步辨識(shí)，以減輕動(dòng)力相消的問(wèn)題。通過(guò)參數(shù)的試驗(yàn)測(cè)量值與辨識(shí)值的對(duì)比，驗(yàn)證了本文辨識(shí)出參數(shù)的準(zhǔn)確性；通過(guò)模型預(yù)報(bào)數(shù)據(jù)與試驗(yàn)觀測(cè)數(shù)據(jù)的對(duì)比，驗(yàn)證了本文建立的三體船數(shù)學(xué)模型的準(zhǔn)確性，具有較高的工程意義和參考價(jià)值。

1 無(wú)跡卡爾曼濾波器介紹

1.1 無(wú)跡變換

隨機(jī)向量X的均值和協(xié)方差分別為和Pxx，對(duì)于非線性變換Y=f(X)，無(wú)跡變換是利用采樣法求取Y的均值和協(xié)方差Pyy。首先構(gòu)造Sigma點(diǎn)集 {χi}，如下式：

式中：n為系統(tǒng)的維數(shù)； λ =α2(n+κ)-n為尺度參數(shù)； α 決定了Sigma點(diǎn)圍繞的分布，通常取一個(gè)小正值，1 ＞α＞e-4； κ為第2個(gè)尺度參數(shù)，通常取為0；表示矩陣開(kāi)方運(yùn)算后選擇其中第i行并轉(zhuǎn)置后得到的n維向量。

對(duì) {χi}中的每一個(gè)點(diǎn)進(jìn)行非線性變換Yi=f(χi)，變換后的點(diǎn)集 {Yi}可以近似表示Y=f(X)的分布，對(duì){Yi}進(jìn)行加權(quán)處理，得到Y(jié)ˉ 和Pyy，如下式：

為敘述方便，將上述無(wú)跡變換過(guò)程用函數(shù)UT表示。對(duì)于隨機(jī)向量X和一個(gè)非線性變換Y=f(X)，以及另一個(gè)非線性變換Z=h(X)，互協(xié)方差Pzy的計(jì)算用函數(shù)UTc表示，如下式：

1.2 無(wú)跡卡爾曼濾波器

對(duì)n維離散的非線性系統(tǒng)，狀態(tài)空間模型如下式：

式中：Xk和Xk-1分別為k時(shí)刻和k-1時(shí)刻的系統(tǒng)狀態(tài)向量；Zk為系統(tǒng)觀測(cè)向量；f為狀態(tài)轉(zhuǎn)移函數(shù)；h為系統(tǒng)觀測(cè)函數(shù)；Uk為輸入控制；wk為服從高斯分布的過(guò)程噪聲，其均值為0，協(xié)方差矩陣為Qk；vk為服從高斯分布的觀測(cè)噪聲，其均值為0，協(xié)方差矩陣為Rk。

UKF的基本過(guò)程與標(biāo)準(zhǔn)卡爾曼濾波器一致，在誤差協(xié)方差矩陣預(yù)報(bào)和更新時(shí)都用無(wú)跡變換來(lái)計(jì)算，遞推公式如式（5）～式（8）所示。

狀態(tài)估計(jì)預(yù)報(bào)、誤差協(xié)方差矩陣預(yù)報(bào)：

濾波增益：

狀態(tài)估計(jì)更新：

誤差協(xié)方差矩陣更新：

2 目標(biāo)船運(yùn)動(dòng)數(shù)學(xué)模型構(gòu)建與參數(shù)辨識(shí)方法

2.1 目標(biāo)船概況

安裝完成的目標(biāo)三體船如圖1所示，3個(gè)船體都采用了S標(biāo)準(zhǔn)型，其主要參數(shù)如表1所示。三體船的推進(jìn)裝置為船體兩側(cè)的1對(duì)螺旋槳，采用差分推力來(lái)控制前進(jìn)和轉(zhuǎn)向。

圖1 安裝完成的目標(biāo)三體船F(xiàn)ig.1 Installed trimaran

表1 船體主尺度信息Tab.1 Main parameters of trimaran

2.2 目標(biāo)船運(yùn)動(dòng)數(shù)學(xué)模型結(jié)構(gòu)

基于MMG模型建立目標(biāo)三體船的運(yùn)動(dòng)數(shù)學(xué)模型結(jié)構(gòu)，三自由度操縱性方程如下式：

式中：m，Izz分別為船舶的質(zhì)量和繞z軸的轉(zhuǎn)動(dòng)慣性矩；mx，my，Jzz分別為船舶縱向附加質(zhì)量，橫向附加質(zhì)量，附加轉(zhuǎn)動(dòng)慣性矩；u，v，r分別為縱向速度，橫向速度，轉(zhuǎn)首角速度；XH，YH，NH分別為船體受到的縱向力，橫向力和轉(zhuǎn)首力矩；XP，YP，NP分別為槳受到的縱向力，橫向力和轉(zhuǎn)艏力矩。

XH，YH，NH是船體受到的粘性阻力和力矩，由于三體船沒(méi)有橫向主動(dòng)力，v的產(chǎn)生靠的是坐標(biāo)變換的科里奧利力，即r和u是產(chǎn)生v的必要條件，所以與v和r相關(guān)的水動(dòng)力導(dǎo)數(shù)耦合在一起，產(chǎn)生了動(dòng)力相消的問(wèn)題，無(wú)法分開(kāi)辨識(shí)。另外，由于v很小，橫向信噪比高，XH，YH，NH中與v耦合的高階項(xiàng)作用很小。因此可以在井上模型[1]的基礎(chǔ)上對(duì)與v相關(guān)的高階項(xiàng)和耦合項(xiàng)進(jìn)行化簡(jiǎn)，如下式：

式 中 ：Xuu，Xrr，Yv，Yr，Nv，Nr，Nrr為 水 動(dòng) 力 導(dǎo) 數(shù) 。

假設(shè)推力系數(shù)為進(jìn)速系數(shù)的一次函數(shù)關(guān)系，如下式：

式中：Jp為 進(jìn)速系數(shù)；wp為伴流分?jǐn)?shù)；u為船速；n為螺旋槳轉(zhuǎn)速；D為螺旋槳直徑；a0，a1常系數(shù)。

單個(gè)螺旋槳的推力T(n,u)如下式：

式中：n表示螺旋槳轉(zhuǎn)速；t為推力減額； ρ 為液體密度；Tnn，Tun為系數(shù)；Tnn也是螺旋槳系柱推力系數(shù)。

對(duì)三體船，2個(gè)螺旋槳X方向位置與船重心X位置一致，距離船中的距離為l=0.45m，則螺旋槳的推力和力矩XP，YP，NP的計(jì)算如下式：

式中：nl，nr分別表示左右螺旋槳的轉(zhuǎn)速。

上述模型結(jié)構(gòu)中，需要確定的參數(shù)為mx，my，。

2.3 目標(biāo)船參數(shù)辨識(shí)方法

構(gòu)建系統(tǒng)狀態(tài)向量X是縱向速度u，橫向速度v，轉(zhuǎn)艏角速度r，大地坐標(biāo)系下正北坐標(biāo)x，大地坐標(biāo)系下正東坐標(biāo)y，艏向角 ψ 與待辨識(shí)的參數(shù)組合而成的向量；輸入控制U是左右螺旋槳轉(zhuǎn)速組成的向量；系統(tǒng)觀測(cè)向量Z是X前六維變量的測(cè)量值，如下式：

狀態(tài)轉(zhuǎn)移函數(shù)和系統(tǒng)觀測(cè)函數(shù)如下式：

其 中dt=0.2s，u˙ ，v˙ ，r˙由 式 （9） 計(jì) 算 得 到 。 無(wú) 跡 變換參數(shù)取值為 α =0.3， β =2， κ =-12。Qk為對(duì)角矩陣，u,v,r分量分別取 0.012， 0.012， 0.052，其余分量取0；Rk為對(duì)角矩陣，各分量按順序依次取 0.05542， 0.05542，0.00452，0.22，0.22，0.12。

系統(tǒng)辨識(shí)過(guò)程中，可能會(huì)出現(xiàn)參數(shù)漂移或動(dòng)力相消[9]的情況，例如在式（9）中如果m+mx，m+my，Xuu，Xrr，Tnn，Tun同時(shí)增大同樣的倍數(shù)，縱向的運(yùn)動(dòng)方程并不會(huì)發(fā)生變化。為避免參數(shù)漂移，mx，my，Jzz不參與辨識(shí)，而是由由周昭明回歸公式[12]計(jì)算得到，如表2所示。分步辨識(shí)法也可以有效地緩解動(dòng)力相消問(wèn)題，其思想是分批辨識(shí)參數(shù)，將前步辨識(shí)出的參數(shù)作為已知值再進(jìn)行后面其他參數(shù)的辨識(shí)。對(duì)于目標(biāo)船，由于對(duì)稱性，在進(jìn)行直航運(yùn)動(dòng)時(shí)v，r為0，Xrr，Yv，Yr，Nv， ,Nr，Nrr不產(chǎn)生作用，因此可以優(yōu)先辨識(shí)出Xuu，Tnn，Tun這3個(gè)參數(shù)，然后將其設(shè)為已知值，再進(jìn)行剩下參數(shù)的辨識(shí)。

表2 附加質(zhì)量、轉(zhuǎn)動(dòng)慣量值Tab.2 Added mass and moment of inertia

為獲取目標(biāo)船參數(shù)辨識(shí)所需的觀測(cè)數(shù)據(jù)序列，進(jìn)行目標(biāo)船靜水中的自航試驗(yàn)。目標(biāo)船上搭載了AHRS，GPS傳感器，可以實(shí)時(shí)地獲取位置、速度、姿態(tài)信息。目標(biāo)船由岸上的遙控器進(jìn)行操控，控制信號(hào)通過(guò)WIFI發(fā)送給船上的電機(jī)驅(qū)動(dòng)器，以控制2個(gè)螺旋槳的轉(zhuǎn)速。首先進(jìn)行多組目標(biāo)船直航試驗(yàn)，再進(jìn)行多組目標(biāo)船S型回轉(zhuǎn)試驗(yàn)，將傳感器觀測(cè)數(shù)據(jù)記錄下進(jìn)行離線辨識(shí)。

3 目標(biāo)船運(yùn)動(dòng)數(shù)學(xué)模型辨識(shí)結(jié)果

直航試驗(yàn)和S型回轉(zhuǎn)試驗(yàn)螺旋槳轉(zhuǎn)速信號(hào)如圖2所示。將多組直航試驗(yàn)觀測(cè)數(shù)據(jù)辨識(shí)的得到的參數(shù)Xuu，Tnn，Tun取均值，然后將其作為已知值，再利用S型回轉(zhuǎn)試驗(yàn)觀測(cè)數(shù)據(jù)對(duì)Xrr，Yv，Yr，Nv，Nr，Nrr進(jìn)行辨識(shí)，結(jié)果如表3所示。

將辨識(shí)出的參數(shù)代入運(yùn)動(dòng)數(shù)學(xué)模型，并將模型預(yù)報(bào)的結(jié)果與實(shí)際觀測(cè)數(shù)據(jù)進(jìn)行比較，直航試驗(yàn)對(duì)比結(jié)果如圖3所示，S型回轉(zhuǎn)試驗(yàn)對(duì)比結(jié)果如圖4所示。由對(duì)比結(jié)果可以得出，直航試驗(yàn)觀測(cè)的u和模型預(yù)報(bào)值比較吻合；S型回轉(zhuǎn)試驗(yàn)觀測(cè)的u和r與模型預(yù)報(bào)值十分吻合，v存在一定的誤差，原因是三體船沒(méi)有橫向主動(dòng)力，橫向速度比較小，因此信噪比較大。

圖2 直航試驗(yàn)和S型回轉(zhuǎn)試驗(yàn)螺旋槳轉(zhuǎn)速信號(hào)Fig.2 Propeller speed on straight sailing test and S rotation test

表3 參數(shù)辨識(shí)結(jié)果Tab.3 Results of parameter identification

辨識(shí)的參數(shù)中，Tnn是可以由螺旋槳系柱推力試驗(yàn)獲得精確值的，為進(jìn)一步驗(yàn)證參數(shù)辨識(shí)的準(zhǔn)確性，在上海交通大學(xué)循環(huán)水槽進(jìn)行了目標(biāo)船螺旋槳的系柱推力標(biāo)定試驗(yàn)。將左右螺旋槳轉(zhuǎn)速設(shè)為一樣，轉(zhuǎn)速依次為 5，10，15，20，25，30 r/min，測(cè)量對(duì)應(yīng)的系柱推力，將系柱推力對(duì)轉(zhuǎn)速平方進(jìn)行線性擬合，擬合直線的斜率即為2Tnn，如圖5所示。Tnn由系統(tǒng)辨識(shí)得到的結(jié)果與系柱推力測(cè)量的結(jié)果對(duì)比如表4所示，結(jié)果驗(yàn)證了辨識(shí)參數(shù)的準(zhǔn)確性。

4 結(jié) 語(yǔ)

本文針對(duì)目標(biāo)三體船，基于MMG模型建立了其運(yùn)動(dòng)數(shù)學(xué)模型結(jié)構(gòu)，利用無(wú)跡卡爾曼濾波器結(jié)合自航試驗(yàn)數(shù)據(jù)對(duì)模型中的參數(shù)進(jìn)行辨識(shí)。為減輕動(dòng)力相消帶來(lái)的影響，結(jié)合三體船特點(diǎn)對(duì)模型結(jié)構(gòu)進(jìn)行了化簡(jiǎn)，并設(shè)計(jì)了不同的航行方式和分步辨識(shí)方法。模型預(yù)報(bào)的速度、轉(zhuǎn)首角速度與實(shí)際觀測(cè)值十分吻合，對(duì)其中可以由試驗(yàn)測(cè)量的系柱推力系數(shù)，試驗(yàn)測(cè)量值與辨識(shí)值誤差很小，驗(yàn)證了本文辨識(shí)方法的有效性和優(yōu)越性。

圖3 直航試驗(yàn)?zāi)Ｐ皖A(yù)報(bào)的結(jié)果與實(shí)際觀測(cè)數(shù)據(jù)對(duì)比Fig.3 Comparison between model prediction result and observed data on straight sailing test

圖4 S型回轉(zhuǎn)試驗(yàn)?zāi)Ｐ皖A(yù)報(bào)的結(jié)果與實(shí)際觀測(cè)數(shù)據(jù)對(duì)比Fig.4 Comparison between model prediction result and observed data on S rotation test

圖5 系柱推力測(cè)量結(jié)果線性擬合Fig.5 Linear regression of propeller bollard thrust

表4 試驗(yàn)結(jié)果與系統(tǒng)辨識(shí)結(jié)果比較Tab.4 Comparison between test result and identification result