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

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

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

0 引 言

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

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

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

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

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

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

(1)

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

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

(2)

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

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

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

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

(3)

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

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

1) 先驗狀態(tài)估計方程：

(4)

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

(5)

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

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

(7)

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

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

(8)

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

(9)

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

(10)

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

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

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

(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)

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

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

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

式中：

(18)

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

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

Xk+1=φk+1/kXk

(20)

令觀測矩陣：

(21)

則觀測方程為

Yt+1=Ck+1Xk+1

(22)

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

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

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

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

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

圖1 20°Z形實驗運動仿真結(jié)果

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

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

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

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

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

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

3 辨識結(jié)果分析與驗證

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

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

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

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

圖3 20°Z形實驗仿真對比

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

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

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

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

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

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

(23)

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

(24)

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

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

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

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

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

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

3.3 辨識模型泛化能力分析

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

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

圖5 正弦操舵運動仿真

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

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

4 結(jié) 論

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

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

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