基于卡爾曼濾波的水下航行器水動(dòng)力系數(shù)辨識(shí)方法

呂幫俊 黃 斌 明廷濤 彭利坤

(海軍工程大學(xué)動(dòng)力工程學(xué)院1) 武漢 430033) (上海地區(qū)裝備修理監(jiān)修室2) 上海 200136)

0 引 言

在潛艇或水下航行器設(shè)計(jì)過(guò)程中，必須對(duì)其操縱和控制性能進(jìn)行系統(tǒng)的仿真和測(cè)試，這些都依賴于精確數(shù)學(xué)模型的建立.而水下航行器數(shù)學(xué)模型中大量的水動(dòng)力和力矩項(xiàng)，將顯著影響其性能預(yù)報(bào)結(jié)果，因此必須對(duì)描述其水動(dòng)力和力矩的系數(shù)進(jìn)行準(zhǔn)確估計(jì)[1].在設(shè)計(jì)階段，模型試驗(yàn)和計(jì)算流體動(dòng)力學(xué)(computational fluid dynamics,CFD)是獲取水動(dòng)力系數(shù)的主要手段，對(duì)于實(shí)際的水下航行器來(lái)說(shuō)，實(shí)尺度試驗(yàn)結(jié)果無(wú)疑是最為可靠與可信的，但由于其相對(duì)較高的成本而不受青睞.水下航行器運(yùn)動(dòng)模型中的水動(dòng)力系數(shù)主要可分為兩類，其中的慣性類水動(dòng)力系數(shù)，也即附加質(zhì)量通常可以采用細(xì)長(zhǎng)體理論進(jìn)行估算，且能獲得足夠的精度，因此，影響模型預(yù)報(bào)精度的核心是粘性類水動(dòng)力系數(shù)，尤其是其中的一階項(xiàng)也即水動(dòng)力導(dǎo)數(shù)對(duì)模型計(jì)算結(jié)果的影響尤為突出[1]，大量的研究工作致力于解決這一具有挑戰(zhàn)性的問(wèn)題.其中，系統(tǒng)識(shí)別(system identification,SI)理論，以相對(duì)較小的代價(jià)即可在線或離線辨識(shí)出高精度的水動(dòng)力系數(shù)，成為水下航行器設(shè)計(jì)階段獲取水動(dòng)力系數(shù)的一種強(qiáng)大且實(shí)用的方法[2-4].將基于SI理論的辨識(shí)方法與水動(dòng)力系數(shù)測(cè)試或自航模試驗(yàn)相結(jié)合，已成為利用試驗(yàn)數(shù)據(jù)建立系統(tǒng)精確數(shù)學(xué)模型的有力手段[5].

Kalman[6]基于兩步貝葉斯過(guò)程，即預(yù)測(cè)更新與測(cè)量更新，提出了卡爾曼濾波器(Kalman filter,KF)，成為線性系統(tǒng)最優(yōu)化自回歸數(shù)據(jù)處理算法.然而，大多數(shù)水下航行器的運(yùn)動(dòng)方程均存在一定程度的非線性，無(wú)法直接應(yīng)用傳統(tǒng)KF.擴(kuò)展卡爾曼濾波(extended Kalman filter,EKF)的提出使得卡爾曼濾波理論的應(yīng)用拓展到非線性領(lǐng)域，EKF的基本思想是利用泰勒展開將非線性系統(tǒng)線性化，使得原有理論繼續(xù)適用于非線性系統(tǒng)，而此時(shí)的EKF成為一種次優(yōu)濾波.Abkowitz[7]將其應(yīng)用于船舶試驗(yàn)試航中，提供了一種“測(cè)量”船舶水動(dòng)力系數(shù)的方法，并驗(yàn)證模型方程的準(zhǔn)確性.EKF的計(jì)算精度依賴于線性近似的有效性，如果線性化處理不當(dāng)或是系統(tǒng)具有強(qiáng)非線性將會(huì)引起算法發(fā)散[8].Julier 等[9]結(jié)合無(wú)跡變換與卡爾曼濾波提出了無(wú)跡卡爾曼濾波(unscented Kalman filter，UKF)方法，避免了 EKF 線性簡(jiǎn)化及求解雅克比矩陣等問(wèn)題，被廣泛運(yùn)用于非線性系統(tǒng)識(shí)別.Sabet等[10]采用UKF來(lái)估計(jì)AUV空間運(yùn)動(dòng)模型中的未知增廣狀態(tài)量(包含10個(gè)耦合水動(dòng)力系數(shù))，并與EKF進(jìn)行比較，表明UKF無(wú)論是在估算精度還是收斂速度方面均優(yōu)于EKF.Chowdhary等[11]研究了擴(kuò)展UKF、簡(jiǎn)化UKF以及EKF等三種算法對(duì)于飛行系統(tǒng)參數(shù)識(shí)別的有效性，通過(guò)對(duì)比發(fā)現(xiàn)，三種算法在估算能力上相差無(wú)幾，UKF在收斂時(shí)間和估計(jì)可靠性方面具有一定的優(yōu)勢(shì)，但要花費(fèi)更多的計(jì)算代價(jià)，而EKF算法始終可以返回高質(zhì)量的計(jì)算結(jié)果，并且在計(jì)算需求方面要求最低.褚式新等[12]采用UKF算法對(duì)USV二階非線性操縱響應(yīng)模型參數(shù)進(jìn)行了辨識(shí)，得到與真實(shí)值較為接近的結(jié)果，進(jìn)而對(duì)USV的操縱性進(jìn)行了有效預(yù)報(bào).VAN等[13]對(duì)UKF算法進(jìn)行優(yōu)化提出了SRUKF，采用QR和Cholesky分解等兩種高效的矩陣運(yùn)算，將UKF算法進(jìn)行N維未知參數(shù)向量估計(jì)時(shí)的計(jì)算復(fù)雜度由O(N3)降低到O(N2).Belanger等[14]采用SRUKF算法對(duì)潛艇標(biāo)準(zhǔn)運(yùn)動(dòng)方程中的全部120多個(gè)水動(dòng)力系數(shù)進(jìn)行了辨識(shí)，計(jì)算結(jié)果的平均誤差僅約0.2%，具有很高的辨識(shí)精度.因此，目前已開展研究集中在水下航行器空間運(yùn)動(dòng)方程水動(dòng)力系數(shù)的識(shí)別，由于模型具有強(qiáng)非線性，一般都認(rèn)為UKF或SRUKF算法比其他算法更具優(yōu)勢(shì).本文針對(duì)水下航行器垂直面機(jī)動(dòng)非線性數(shù)學(xué)模型中的水動(dòng)力導(dǎo)數(shù)辨識(shí)問(wèn)題，建立了參數(shù)辨識(shí)模型，分別采用EKF和SRUKF算法對(duì)垂直面運(yùn)動(dòng)影響最為顯著的6個(gè)參數(shù)進(jìn)行了辨識(shí)，并對(duì)辨識(shí)結(jié)果進(jìn)行了分析比較.

1 水下航行器垂直面運(yùn)動(dòng)數(shù)學(xué)模型

采用國(guó)際拖曳水池會(huì)議(ITTC)推薦以及造船與輪機(jī)工程學(xué)會(huì)(SNAME)術(shù)語(yǔ)公報(bào)的體系[15]，描述水下航行器空間運(yùn)動(dòng)的坐標(biāo)系和主要參數(shù)見圖1.

圖1 描述水下航行器運(yùn)動(dòng)的坐標(biāo)系和主要參數(shù)

對(duì)Gertler發(fā)表的潛艇標(biāo)準(zhǔn)運(yùn)動(dòng)方程進(jìn)行簡(jiǎn)化，只考慮垂直面的升沉速度w，縱傾角速度q，縱傾角θ，深度ζ，輸入控制量首舵角δb和尾舵角δs，同時(shí)假設(shè)水下航行器在垂直面內(nèi)作弱機(jī)動(dòng)，保持較低的航速u=u0不變，w、q、θ、δb、δs為小量，得到垂直面三自由度運(yùn)動(dòng)方程.

(1)

(2)

(3)

(4)

表1 模型艇參數(shù)值

表2 無(wú)因次水動(dòng)力系數(shù)及測(cè)定值

2 水動(dòng)力系數(shù)辨識(shí)模型

對(duì)于一個(gè)實(shí)際的非線性系統(tǒng)，通常采用連續(xù)-離散混合濾波方法，即采用連續(xù)的狀態(tài)估算模型與離散的測(cè)量采樣模型

(5)

y(k)=h(x(k),v(k))

(6)

式中：x∈Rn為狀態(tài)向量；u為輸入向量；y∈Rm為觀測(cè)向量；p∈Rl為未知參數(shù)向量；f(·)為系統(tǒng)動(dòng)力學(xué)模型方程；y(·)為測(cè)量方程；k為離散的時(shí)間序列；w(t)≈N(0,Q)為過(guò)程噪聲向量，v(k)≈N(0,R)為第k步的測(cè)量噪聲向量，同時(shí)假定w(t)和v(k)都為均值為零的白噪聲且彼此不相關(guān).

參考式(5)～(6)給出的狀態(tài)估算和測(cè)量采樣模型，將式(1)～(4)所描述的水下航行器垂直面三自由度運(yùn)動(dòng)方程改寫成矩陣形式

(7)

y=[θ,w,q]T

(8)

E=T-1S2，F(xiàn)=T-1S3

式(7)給出的運(yùn)動(dòng)模型是連續(xù)的，要通過(guò)計(jì)算機(jī)實(shí)現(xiàn)未知參數(shù)的在線估計(jì)，或是根據(jù)試驗(yàn)數(shù)據(jù)進(jìn)行未知參數(shù)的迭代估計(jì)時(shí)，必須對(duì)模型進(jìn)行離散化處理

(9)

式(9)中數(shù)值積分可以采用四階龍格庫(kù)塔積分算法

式中：b1=f(xk,uk,p)Δt；b2=F(xk+0.5b1,uk,p)Δt；b3=F(xk+0.5b2,uk,p)Δt；b4=F(xk+b3,uk,p)Δt；積分步長(zhǎng)Δt=t(k+1)-t(k).

2.1 擴(kuò)展卡爾曼濾波(EKF)參數(shù)辨識(shí)模型

當(dāng)系統(tǒng)存在隨機(jī)擾動(dòng)和傳感器噪聲的情況下，EKF可以最優(yōu)地估計(jì)非線性系統(tǒng)中的狀態(tài)變量.為了對(duì)未知參數(shù)進(jìn)行估計(jì)，通常采用的方法是將狀態(tài)參數(shù)向量進(jìn)行擴(kuò)展，即把待識(shí)別參數(shù)作為額外的狀態(tài)變量來(lái)進(jìn)行估計(jì)

(11)

(12)

式中：擴(kuò)展?fàn)顟B(tài)向量x*=[xT,pT]T∈Rn+l；0和I分別為零矩陣和單位陣；F(·)為采用擴(kuò)展向量時(shí)的系統(tǒng)動(dòng)力學(xué)模型方程.

通過(guò)EKF進(jìn)行參數(shù)估計(jì)時(shí)分為兩個(gè)步驟：第一步為預(yù)報(bào)，主要計(jì)算狀態(tài)量以及狀態(tài)誤差協(xié)方差；第二步為測(cè)量更新，計(jì)算出所構(gòu)造的擴(kuò)展卡爾曼濾波器的增益，進(jìn)行狀態(tài)誤差協(xié)方差矩陣的更新，以及對(duì)所預(yù)報(bào)的狀態(tài)值進(jìn)行更新.

1) 狀態(tài)預(yù)測(cè)

(13)

(14)

2) 測(cè)量更新

(17)

2.2 平方根無(wú)跡卡爾曼濾波(SRUKF)參數(shù)辨識(shí)模型

采用SRUKF進(jìn)行參數(shù)估計(jì)時(shí)，狀態(tài)預(yù)測(cè)與測(cè)量更新等基本步驟與EKF類似，但具體實(shí)施細(xì)節(jié)有一定差異.與EKF相比，SRUKF不需要對(duì)非線性系統(tǒng)方程進(jìn)行任何線性化處理，而是利用概率分布構(gòu)造Sigma點(diǎn)采樣的方式，保持了原系統(tǒng)的非線性特征.此外，相對(duì)于UKF算法，SRUKF采用了高效的QR分解和Cholesky分解矩陣運(yùn)算，極大的降低了計(jì)算復(fù)雜度.

1) Sigma點(diǎn)采樣和狀態(tài)預(yù)測(cè)

(18)

(19)

Dk|k-1=f(xk,uk,Pk|k-1)

(21)

(22)

2) 測(cè)量更新

(23)

(24)

(25)

(26)

(27)

U=κkSdk

(28)

(29)

3 仿真算例分析

為了比較EKF和SRUKF兩種算法進(jìn)行水下航行器水動(dòng)力系數(shù)辨識(shí)時(shí)的性能差異，采用模型艇作為仿真算例，相關(guān)參數(shù)見表1.進(jìn)行參數(shù)辨識(shí)時(shí)所需的測(cè)量數(shù)據(jù)由水下航行器垂直面運(yùn)動(dòng)模型仿真產(chǎn)生，式(1)～(4)中的水動(dòng)力系數(shù)取表2所列模型艇的無(wú)因次水動(dòng)力系數(shù)測(cè)定值.

為了準(zhǔn)確、迅速的進(jìn)行水動(dòng)力參數(shù)辨識(shí)，需要使水下航行器以一定的形式進(jìn)行機(jī)動(dòng)，盡可能激發(fā)垂直面的全部水動(dòng)力分量，通常采用z型機(jī)動(dòng)或深度、縱傾周期變化的振蕩模式.本文仿真時(shí)取無(wú)人航行器航速u0=1.1 m/s，采用周期性操舵來(lái)實(shí)現(xiàn)上述運(yùn)動(dòng)，首、尾升降舵按照正弦規(guī)律操舵δb=δsin(0.03t)，δs=δsin(0.04t)，分別取舵角幅值δ=5°和10°，仿真時(shí)長(zhǎng)t=3 000 s，步長(zhǎng)Δt=0.1 s，水下航行器在垂直面的運(yùn)動(dòng)軌跡見圖2，為了便于比較，兩種算法采用同一組測(cè)量數(shù)據(jù)作為水動(dòng)力系數(shù)辨識(shí)算法的輸入.

圖2 分別采用5°/10°幅值正弦規(guī)律操舵時(shí)無(wú)人水下航行器狀態(tài)參數(shù)變化情況

圖3 無(wú)因次水動(dòng)力系數(shù)隨時(shí)間收斂情況

此外，從算法的平均收斂時(shí)間比較來(lái)看，文獻(xiàn)[15]應(yīng)用SRUKF算法辨識(shí)潛艇標(biāo)準(zhǔn)運(yùn)動(dòng)方程水動(dòng)力系數(shù)所需時(shí)間約2 500 s，由圖3可知：當(dāng)操舵幅值分別為5°/10°時(shí)，采用SRUKF算法，全部水動(dòng)力系數(shù)收斂到固定值所需時(shí)間約2 500 s/3 000 s，而EKF算法僅需要約100 s/600 s即可收斂到固定值.因此，EKF算法相較于SRUKF具有更快的辨識(shí)速度，且兩種算法都受到操舵幅值的影響，也即水下航行器機(jī)動(dòng)強(qiáng)度越大，水動(dòng)力系數(shù)收斂速度越快

從理論上講，SRUKF算法采用無(wú)跡變換，采樣點(diǎn)沒有經(jīng)過(guò)線性化，也沒有忽略高階項(xiàng)，在求解非線性系統(tǒng)時(shí)具有更高的精度.但由仿真結(jié)果來(lái)看，對(duì)于本算例所涉及的水下航行器垂直面水動(dòng)力導(dǎo)數(shù)辨識(shí)，無(wú)論是辨識(shí)誤差還是收斂速度，EKF都優(yōu)于SRUKF.分析其主要原因，是水下航行器采用低航速在垂直面正常機(jī)動(dòng)時(shí)，采用非線性方程和線性方程的求解結(jié)果誤差在5%之內(nèi)，可以認(rèn)為其垂直面運(yùn)動(dòng)模型具有較好的線性度，因此采用EKF算法對(duì)于其水動(dòng)力系數(shù)辨識(shí)具有很好的效果.盡管采用SRUKF與EKF算法用于參數(shù)估計(jì)時(shí)，算法復(fù)雜度都為O(N2)，但是EKF算法的收斂速度要明顯優(yōu)于SRUKF算法.

表3 無(wú)因次水動(dòng)力系數(shù)辨識(shí)誤差

4 結(jié) 束 語(yǔ)

文中分別采用基于卡爾曼濾波原理的EKF和SRUKF算法，以模型艇在垂直面正弦操舵機(jī)動(dòng)時(shí)的縱傾角、垂速和縱傾角速度變化作為測(cè)量輸入，對(duì)水下航行器垂直面運(yùn)動(dòng)模型中的6個(gè)核心水動(dòng)力導(dǎo)數(shù)進(jìn)行了系統(tǒng)辨識(shí)，取得了理想的辨識(shí)效果.盡管SRUKF算法理論上具有更高的計(jì)算精度，但通過(guò)仿真研究發(fā)現(xiàn)，SRUKF算法的辨識(shí)精度受模型艇機(jī)動(dòng)強(qiáng)度的影響較大，對(duì)于線性度較高的水下航行器垂直面正常機(jī)動(dòng)模型，無(wú)論是辨識(shí)精度還是收斂速度，EKF算法都要優(yōu)于SRUKF，更適用于線性水動(dòng)力系數(shù)的在線識(shí)別.