基于EKF的高速無(wú)人艇操縱響應(yīng)模型參數(shù)辨識(shí)*

楊 鑫 茅云生 董早鵬 包 濤 曾小龍 黃 鋮

(武漢理工大學(xué)高性能船舶技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室1) 武漢 430063)(武漢理工大學(xué)交通學(xué)院2) 武漢 430063) (武昌船舶重工集團(tuán)有限公司3) 武漢 430060)

0 引 言

水面無(wú)人艇(unmanned surface vessel, USV)，簡(jiǎn)稱(chēng)無(wú)人艇，是一種無(wú)人化的水面智能任務(wù)平臺(tái)，具有體積小、性?xún)r(jià)比高、高速智能等優(yōu)勢(shì)[1].隨著船用發(fā)動(dòng)機(jī)、艇型優(yōu)化設(shè)計(jì)、船舶推進(jìn)技術(shù)等的提升，無(wú)人艇的設(shè)計(jì)航速也越來(lái)越高，相應(yīng)的對(duì)其操縱性要求也越來(lái)越高[2].

為了對(duì)高速無(wú)人艇的操縱性進(jìn)行準(zhǔn)確的預(yù)報(bào)和評(píng)估，先要建立比較精確的操縱運(yùn)動(dòng)數(shù)學(xué)模型[3].操縱運(yùn)動(dòng)數(shù)學(xué)模型可分為Abkowitz模型、MMG模型和操縱響應(yīng)模型三種[4].前兩個(gè)模型主要對(duì)水動(dòng)力導(dǎo)數(shù)進(jìn)行研究，響應(yīng)模型主要是對(duì)操縱性指數(shù)K,T進(jìn)行研究.其中操縱響應(yīng)模型結(jié)構(gòu)相對(duì)簡(jiǎn)單，應(yīng)用方便廣泛，對(duì)其研究有重要意義.

無(wú)人艇操縱響應(yīng)數(shù)學(xué)模型的建立，關(guān)鍵是如何確定數(shù)學(xué)模型中的眾多參數(shù).目前無(wú)人艇操縱運(yùn)動(dòng)模型參數(shù)主要通過(guò)模型實(shí)驗(yàn)法、CFD仿真計(jì)算以及參數(shù)辨識(shí)法獲得[5]，而參數(shù)辨識(shí)法較為實(shí)用有效，它將操縱運(yùn)動(dòng)船舶視為一個(gè)動(dòng)態(tài)系統(tǒng)，通過(guò)試驗(yàn)測(cè)得系統(tǒng)輸入(舵角、螺旋槳轉(zhuǎn)速等控制量)和系統(tǒng)輸出(船速、首向角等運(yùn)動(dòng)量)，再運(yùn)用模型辨識(shí)方法，對(duì)模型中的參數(shù)進(jìn)行求解.Sutulo等[6]基于經(jīng)典的遺傳算法，進(jìn)行了操縱數(shù)學(xué)模型辨識(shí)；大連海事大學(xué)航海動(dòng)態(tài)仿真和控制交通行業(yè)重點(diǎn)實(shí)驗(yàn)室，采用自適應(yīng)遺傳算法對(duì)船舶運(yùn)動(dòng)響應(yīng)模型的各參數(shù)進(jìn)行辨識(shí)，得到該船的首搖與橫搖的響應(yīng)模型[7].秦余鋼等[8]采用迭代學(xué)習(xí)思想并引入P型學(xué)習(xí)，改進(jìn)了遞推最小二乘算法進(jìn)行參數(shù)辨識(shí)，但是迭代學(xué)習(xí)使算法變得復(fù)雜，降低效率.張心光等[9]應(yīng)用支持向量回歸機(jī)對(duì)船舶操縱運(yùn)動(dòng)響應(yīng)模型進(jìn)行了機(jī)理建模，同時(shí)進(jìn)行改進(jìn)，結(jié)合最小二乘法以及引入粒子群算法對(duì)懲罰因子C值進(jìn)行尋優(yōu)，從核函數(shù)結(jié)構(gòu)中得到了模型中的操縱性指數(shù).

擴(kuò)展卡爾曼濾波法(EKF)是一種可以實(shí)現(xiàn)線性和非線性系統(tǒng)都適用的遞推算法，即使系統(tǒng)中存在系統(tǒng)誤差和測(cè)量誤差，因其對(duì)噪聲具有強(qiáng)魯棒性，在系統(tǒng)辨識(shí)領(lǐng)域得到了廣泛的應(yīng)用[10].文中基于非線性響應(yīng)模型Z形實(shí)驗(yàn)仿真，采用擴(kuò)展卡爾曼濾波法對(duì)高速無(wú)人艇響應(yīng)模型參數(shù)進(jìn)行辨識(shí)，從而提升高速無(wú)人艇的操縱運(yùn)動(dòng)控制效果.

1 無(wú)人艇操縱響應(yīng)模型

目前對(duì)無(wú)人艇響應(yīng)模型參數(shù)辨識(shí)的研究，多數(shù)是針對(duì)相對(duì)簡(jiǎn)單的一階線性響應(yīng)模型和一階非線性響應(yīng)模型.對(duì)于大舵角操縱運(yùn)動(dòng)，非線性模型更精確，常用的一階非線性響應(yīng)模型為

(1)

式中：r為首向角速度；δ為舵角;T為應(yīng)舵性和航向穩(wěn)定性[11]；K為舵角增益；α為Norbbin系數(shù).

考慮到高速無(wú)人艇的運(yùn)動(dòng)復(fù)雜特性，一階模型無(wú)法有效的描述相應(yīng)的特性，因而本文對(duì)精度較高、更為復(fù)雜的二階非線性響應(yīng)模型中的各項(xiàng)參數(shù)進(jìn)行辨識(shí)研究；根據(jù)文獻(xiàn)[12]可知，二階非線性響應(yīng)模型為

(2)

式中：δr為壓舵角；T1，T2，T3為時(shí)間常數(shù)，并且和一階模型中的參數(shù)T滿足：T=T1+T2-T3，其他變量與式(1)相同.

2 無(wú)人艇響應(yīng)模型參數(shù)辨識(shí)

2.1 擴(kuò)展卡爾曼濾波算法(EKF)基本原理

設(shè)一個(gè)離散非線性系統(tǒng)狀態(tài)方程為

(3)

式中：狀態(tài)變量X為n維變量；f為n維非線性的狀態(tài)向量轉(zhuǎn)移函數(shù)；z為m維輸出向量；c為m維非線性向量值函數(shù)；隨機(jī)變量w,v分別為過(guò)程噪聲和測(cè)量噪聲，并且是相互獨(dú)立的均值為零的高斯白噪聲；Q和R分別為它們的協(xié)方差矩陣.

根據(jù)擴(kuò)展卡爾曼濾波法，狀態(tài)估計(jì)遞推過(guò)程為

1) 先驗(yàn)狀態(tài)估計(jì)方程：

(4)

2) 誤差協(xié)方差矩陣方程：

(5)

3) 卡爾曼增益矩陣方程：

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

(7)

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

Pk+1=Pk+1/k-Kk+1Ck+1Pk+1/k

(8)

式中：狀態(tài)轉(zhuǎn)移矩陣φk+1/k和觀測(cè)矩陣Ck+1分別由f和c的雅克比矩陣求得

(9)

濾波初值和濾波誤差方差矩陣初始值為

(10)

上述遞推過(guò)程中，濾波器增益Kk+1為觀測(cè)誤差在修正過(guò)程中的權(quán)重參數(shù)，當(dāng)預(yù)測(cè)得到的狀態(tài)變量與真實(shí)值相差較大時(shí)，Kk+1也較大，可以較快地對(duì)狀態(tài)變量進(jìn)行修正，加速初始狀態(tài)變量的收斂；隨著狀態(tài)變量可信度的提高，觀測(cè)誤差就會(huì)隨之減小，Kk+1就逐漸減小，觀測(cè)誤差修正預(yù)測(cè)值只會(huì)進(jìn)行適當(dāng)?shù)奈⒄{(diào)，最終達(dá)到狀態(tài)估計(jì)的效果，由此可以用于參數(shù)辨識(shí).

2.2 響應(yīng)模型離散化處理

通過(guò)前向差分法，將無(wú)人艇的二階非線性響應(yīng)模型進(jìn)行離散化處理，對(duì)艏向角速度進(jìn)行差分可得

(11)

式中：yt=ψt+1-ψt；ψ為艏向角；h為數(shù)據(jù)采樣周期.

(13)

將式(11)～(13)代入二階非線性響應(yīng)模型式(2)，得

yt+2-2yt+1+yt=

(14)

令：

(15)

從而可將式(14)表示為

Yt=Htλ

(16)

由此，將高速無(wú)人艇的二階非線性響應(yīng)模型轉(zhuǎn)化成離散型系統(tǒng)，從而基于擴(kuò)展卡爾曼濾波法進(jìn)行下一步求解.

2.3 基于EKF的參數(shù)辨識(shí)

基于擴(kuò)展卡爾曼濾波算法對(duì)狀態(tài)向量進(jìn)行估計(jì)的特性.將式(15)中λ的待辨識(shí)元素與式 (15) 中的Yt，一同組成一個(gè)新的系統(tǒng)狀態(tài)量Xk.

式中：

(18)

令系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣：

得到狀態(tài)方程：

Xk+1=φk+1/kXk

(20)

令觀測(cè)矩陣：

(21)

則觀測(cè)方程為

Yt+1=Ck+1Xk+1

(22)

由此將二階非線性響應(yīng)模型，轉(zhuǎn)化成擴(kuò)展卡爾曼濾波法對(duì)應(yīng)的狀態(tài)向量形式.

2.4 響應(yīng)模型仿真采集數(shù)據(jù)

從文獻(xiàn)[10]中選取一組二階非線性響應(yīng)模型參數(shù)，見(jiàn)表1.

表1 二階非線性響應(yīng)模型參數(shù)

采用工程上廣泛應(yīng)用的四階龍格-庫(kù)塔法(Runge-Kutta)進(jìn)行無(wú)人艇操縱運(yùn)動(dòng)的仿真預(yù)報(bào)：基于表1中的響應(yīng)模型開(kāi)展20°的Z形實(shí)驗(yàn)仿真，得到包括艏向角、舵角，以及角速度等數(shù)據(jù)的變化曲線見(jiàn)圖1.

圖1 20°Z形實(shí)驗(yàn)運(yùn)動(dòng)仿真結(jié)果

將圖1中采集的仿真數(shù)據(jù)帶入式(15)中的Yt和Ht，進(jìn)一步得到狀態(tài)向量Xk和狀態(tài)轉(zhuǎn)移矩陣φk+1/k，最后即可基于擴(kuò)展卡爾曼濾波對(duì)系統(tǒng)狀態(tài)向量Xk進(jìn)行狀態(tài)估計(jì).

其中，狀態(tài)向量初始值x1～x7都取0.01，由于狀態(tài)估計(jì)的數(shù)據(jù)是通過(guò)響應(yīng)模型操縱運(yùn)動(dòng)仿真得到的，令噪聲w,v都為0.

最后得到狀態(tài)向量Xk中x2～x7的狀態(tài)估計(jì)值，其辨識(shí)結(jié)果見(jiàn)圖2.

圖2 x2～x7的辨識(shí)結(jié)果

x2～x7的最終辨識(shí)結(jié)果見(jiàn)表2.

表2 x2～x7辨識(shí)結(jié)果

3 辨識(shí)結(jié)果分析與驗(yàn)證

3.1 模型參數(shù)辨識(shí)結(jié)果及誤差分析

基于表2中x2～x7的辨識(shí)結(jié)果，通過(guò)進(jìn)一步解方程得到二階非線性響應(yīng)模型參數(shù)辨識(shí)結(jié)果，其與原始參數(shù)對(duì)比見(jiàn)表3.

表3 響應(yīng)模型參數(shù)辨識(shí)結(jié)果對(duì)比

由表3可知，T1、K、α的辨識(shí)結(jié)果較好，T2、T3、δr的誤差較大.分別對(duì)原始模型和辨識(shí)模型進(jìn)行20°Z形實(shí)驗(yàn)仿真，其艏向角的變化曲線見(jiàn)圖3.

圖3 20°Z形實(shí)驗(yàn)仿真對(duì)比

由圖3可知，辨識(shí)模型所對(duì)應(yīng)的艏向角變化曲線在前40 s，幾乎與原始模型的艏向角變化曲線重合，誤差很小.隨著時(shí)間的推移，兩條曲線的間距逐漸增大.這主要是由于辨識(shí)結(jié)果與原始模型參數(shù)不完全相同，必然會(huì)引起誤差，而隨著時(shí)間的增加，誤差也不斷累積，最終導(dǎo)致二者的偏差逐漸擴(kuò)大.

結(jié)合表3與圖3可知，雖然T2、T3和δr的辨識(shí)結(jié)果誤差較大，但是最終對(duì)操縱運(yùn)動(dòng)的影響卻較小.

3.2 模型參數(shù)敏感性分析

考慮到可能是由于各個(gè)參數(shù)對(duì)模型輸出結(jié)果的影響權(quán)重不同，對(duì)此進(jìn)一步對(duì)模型參數(shù)的敏感性進(jìn)行分析研究.分別給原始模型中的各個(gè)參數(shù)一定的偏差，計(jì)算其對(duì)最終響應(yīng)模型輸出結(jié)果的影響程度[13-14].

對(duì)于操縱響應(yīng)模型，其最終輸出結(jié)果是艏向角，而且艏向角誤差會(huì)隨著時(shí)間的推移而不斷累積，因此將20°Z形實(shí)驗(yàn)在100 s時(shí)的艏向角誤差作為目標(biāo)函數(shù)：

Δψ=ψ(100)-ψ0(100)

(23)

同時(shí)，令艏向角對(duì)響應(yīng)模型中各個(gè)參數(shù)變化的敏感系數(shù)為

(24)

式中：ψ為辨識(shí)模型的艏向角；ψ0為原始模型的艏向角；Δε為各參數(shù)對(duì)應(yīng)的相對(duì)偏差.

分別給予原始模型的各個(gè)參數(shù)15%，10%，5%，-5%，-10%，-15%的偏差，其它參數(shù)不變，進(jìn)行20°Z形實(shí)驗(yàn)操縱運(yùn)動(dòng)仿真，得到其與原始模型艏向角的誤差.圖4中a～f分別為各參數(shù)15%，10%，5%，-5%，-10%，-15%偏差對(duì)應(yīng)的20°Z形實(shí)驗(yàn)0～100s內(nèi)艏向角誤差變化曲線.

圖4 0～100 s內(nèi)首向角誤差變化

最后，分別將響應(yīng)模型各參數(shù)的6個(gè)偏差所對(duì)應(yīng)的敏感系數(shù)ω的平均值作為最終艏向角對(duì)響應(yīng)模型中各個(gè)參數(shù)變化的敏感系數(shù)，見(jiàn)表4.

表4 原始模型各參數(shù)敏感系數(shù)

結(jié)合圖4和表4可知，當(dāng)T1和K發(fā)生偏差，最終導(dǎo)致艏向角誤差明顯大于其他參數(shù)，并且二者對(duì)應(yīng)的敏感系數(shù)的絕對(duì)值也較高.可以看出，響應(yīng)模型輸出的艏向角對(duì)模型參數(shù)T1和K的敏感性較高，而其他參數(shù)對(duì)模型輸出的影響遠(yuǎn)低于T1和K，所以雖然T2，T3，δr的辨識(shí)結(jié)果誤差較大，但對(duì)最終操縱運(yùn)動(dòng)的影響較小.

3.3 辨識(shí)模型泛化能力分析

為了驗(yàn)證辨識(shí)模型的泛化能力，同時(shí)考慮到高速無(wú)人艇，要求航速高、操縱靈活，在實(shí)際工作中存在各種復(fù)雜的操縱運(yùn)動(dòng)，舵角不能只服從這種標(biāo)準(zhǔn)Z形實(shí)驗(yàn)的變化規(guī)律，本文采用正弦操舵運(yùn)動(dòng)進(jìn)行仿真對(duì)比分析，讓舵角按正弦規(guī)律操縱：δt=W·sin(k·t)，其中W為正弦舵角最大值，周期為360°/ks.

分別對(duì)原始模型和辨識(shí)模型進(jìn)行10°，20°以及30°正弦操舵仿真，對(duì)應(yīng)的艏向角的變化曲線對(duì)比結(jié)果見(jiàn)圖5.

圖5 正弦操舵運(yùn)動(dòng)仿真

由圖5可知，對(duì)辨識(shí)模型進(jìn)行多個(gè)舵角下的正弦操舵運(yùn)動(dòng)仿真，其艏向角的變化曲線與原始模型的誤差較小，說(shuō)明基于擴(kuò)展卡爾曼濾波法對(duì)高速無(wú)人艇的二階非線性響應(yīng)模型參數(shù)辨識(shí)結(jié)果具有良好的泛化能力，能夠較好的適應(yīng)不同舵角下的操縱運(yùn)動(dòng)，從而更好地實(shí)現(xiàn)高速無(wú)人艇的操縱運(yùn)動(dòng).

考慮到在無(wú)人艇的實(shí)際運(yùn)動(dòng)控制過(guò)程中，舵角會(huì)實(shí)時(shí)根據(jù)需要進(jìn)行改變，從而就不會(huì)受到累計(jì)誤差的影響，這就說(shuō)明基于擴(kuò)展卡爾曼濾波法的響應(yīng)模型辨識(shí)結(jié)果能夠較好的為高速無(wú)人艇運(yùn)動(dòng)控制服務(wù).

4 結(jié) 論

1) 對(duì)于二階非線性響應(yīng)模型，模型輸出的艏向角對(duì)模型參數(shù)T1和K的敏感性較高，其他參數(shù)對(duì)模型輸出的影響遠(yuǎn)低于T1和K.

2) 基于模型參數(shù)的敏感性分析，采用EKF對(duì)響應(yīng)模型參數(shù)辨識(shí)得到的結(jié)果存在一定誤差，但是對(duì)整體模型輸出影響不大，可以滿足對(duì)無(wú)人艇運(yùn)動(dòng)控制精確度要求.

3) 基于擴(kuò)展卡爾曼濾波法辨識(shí)得到的響應(yīng)模型具有較強(qiáng)的泛化能力，對(duì)不同舵角以及不同操舵形式下的操縱運(yùn)動(dòng)，都能夠保證足夠的準(zhǔn)確性，從而更好的對(duì)高速無(wú)人艇進(jìn)行操縱運(yùn)動(dòng)控制和預(yù)報(bào).