基于卡爾曼濾波離散滑模控制的明輪船直線跟蹤

洪劍青 趙德安 孫月平 張 軍 劉 波 劉曉洋

(1.江蘇大學(xué)電氣信息工程學(xué)院， 鎮(zhèn)江 212013； 2.鎮(zhèn)江高等?？茖W(xué)校現(xiàn)代裝備制造學(xué)院, 鎮(zhèn)江 212028)

0 引言

隨著人民生活水平的提高，人們對(duì)水產(chǎn)品的需求不斷增長(zhǎng)，極大地促進(jìn)了水產(chǎn)養(yǎng)殖業(yè)的飛速發(fā)展[1-3]。水產(chǎn)養(yǎng)殖業(yè)已成為國民經(jīng)濟(jì)不可缺少的組成部分，年產(chǎn)值高達(dá)幾百億元。規(guī)模化生產(chǎn)促使水產(chǎn)養(yǎng)殖業(yè)向集約化、自動(dòng)化、信息化、智能化的方向發(fā)展[4-7]。

養(yǎng)殖水域通常水情比較復(fù)雜，水下植物較多。明輪船具有吃水淺、適航性好的特點(diǎn)，適宜作為水面作業(yè)裝備的載體。水上作業(yè)裝備實(shí)現(xiàn)智能化、自動(dòng)化、信息化作業(yè)的方式，不僅可以提高作業(yè)效率、降低人力成本，克服工作時(shí)間的限制，且更加貼近養(yǎng)殖動(dòng)物的生活習(xí)性[8-9]。

明輪船的航跡跟蹤是近來研究的熱點(diǎn)。文獻(xiàn)[9]建立了明輪船的水動(dòng)力模型和控制方程。文獻(xiàn)[8，10]詳細(xì)給出明輪船航跡跟蹤系統(tǒng)的硬件方案和模糊控制航跡跟蹤方法[8,10-12]，實(shí)現(xiàn)了基本的跟蹤。文獻(xiàn)[13-14]采取魯棒預(yù)測(cè)控制、障礙李亞普諾夫函數(shù)法，取得較好的控制效果，但是由于預(yù)測(cè)控制采取在線連續(xù)時(shí)域滾動(dòng)計(jì)算，算法復(fù)雜、計(jì)算量較大、計(jì)算耗時(shí)和對(duì)系統(tǒng)計(jì)算資源需求較高、對(duì)反饋信號(hào)實(shí)時(shí)性要求較高；障礙李亞普諾夫函數(shù)法存在反步微分爆炸的可能，且控制方法未對(duì)反饋信號(hào)進(jìn)行處理，實(shí)際使用中發(fā)現(xiàn)信號(hào)擾動(dòng)較多，易造成執(zhí)行器過多的不必要主動(dòng)控制動(dòng)作，會(huì)導(dǎo)致控制效果變差，浪費(fèi)有限船載電能，縮短續(xù)航里程。文獻(xiàn)[15]采取了PD控制對(duì)航跡跟蹤，由于陸地行駛車輛無風(fēng)浪干擾，PD控制對(duì)于陸地行走的車輛跟蹤效果較好，對(duì)于受風(fēng)浪不斷干擾的明輪船而言，跟蹤效果較差。因此，克服擾動(dòng)對(duì)航跡跟蹤的影響、降低控制運(yùn)算量、提高系統(tǒng)穩(wěn)定性是明輪船航跡跟蹤研究的關(guān)鍵[16]。針對(duì)實(shí)際狀況，本文提出一種運(yùn)算量較小的基于卡爾曼濾波的離散滑?？刂品椒▽?shí)現(xiàn)明輪船的直線跟蹤。

1 航跡跟蹤系統(tǒng)建模

明輪船的航跡跟蹤屬于欠驅(qū)動(dòng)水面運(yùn)動(dòng)系統(tǒng)。采用船舶運(yùn)動(dòng)坐標(biāo)系，其運(yùn)動(dòng)方程[9-10,17]為

(1)

其中

ν=[vr]T

式中m——明輪船質(zhì)量，kg

xG——明輪船重心坐標(biāo)，m

u0——明輪船航行速度，m/s

Iz——明輪船質(zhì)量對(duì)z軸的慣性矩，kg·m2

v——橫漂速度，m/s

r——轉(zhuǎn)艏角速度，(°)/s

δR——明輪船虛舵角，(°)

航跡跟蹤的核心是控制明輪船的船艏航向。通過調(diào)整艏搖角使明輪船的航跡偏差不斷得到糾正。根據(jù)文獻(xiàn)[9-10]，船舶運(yùn)動(dòng)時(shí)呈現(xiàn)非常大的慣性，因提供給明輪船控制船艏運(yùn)動(dòng)方向的能量有限，通常角速度低于3(°)/s，船舶運(yùn)動(dòng)具有低頻特征，可以描述為一階微分方程。在忽略橫漂速度v的基礎(chǔ)上，采取野本一階模型(Nomoto)的形式[17-19]，給出明輪船的艏搖角速度系統(tǒng)傳遞函數(shù)為

(2)

其中

det(N(u0))=Yv(Nr-mxGu0)-Nv(Yr-mu0)

式中n11、n21——明輪船附加質(zhì)量水動(dòng)力系數(shù)

b1、b2——明輪船轉(zhuǎn)艏水動(dòng)力系數(shù)

T——穩(wěn)定性系數(shù)

K——回轉(zhuǎn)性系數(shù)

2 船艏運(yùn)動(dòng)的離散滑模控制

滑模變結(jié)構(gòu)控制策略可以使系統(tǒng)“結(jié)構(gòu)”具有隨時(shí)間變化的開關(guān)特性。迫使系統(tǒng)沿著規(guī)定軌跡作高頻、小幅運(yùn)動(dòng)實(shí)現(xiàn)滑動(dòng)模態(tài)。離散滑模控制是滑?？刂茢?shù)字化方式[20-21]?；？刂破髟O(shè)計(jì)如下。

首先將艏搖角運(yùn)動(dòng)傳遞函數(shù)轉(zhuǎn)換為狀態(tài)方程并離散化，得到離散艏搖角運(yùn)動(dòng)狀態(tài)方程

x(k+1)=Ax(k)+Bu(k)

(3)

式中A——狀態(tài)矩陣B——輸入矩陣

x(k+1)、x(k)——系統(tǒng)在k+1、k時(shí)刻的狀態(tài)

u(k)——系統(tǒng)在k時(shí)刻的輸入量

設(shè)計(jì)切換函數(shù)為

s(k)=Cex(k)

(4)

式中Ce——控制參數(shù)

s(k+1)=s(k)=Cex(k+1)=CeAx(k)+CeBu(k)

(5)

根據(jù)文獻(xiàn)[20]，設(shè)計(jì)離散滑模控制器u(k)為

u(k)=ueq(k)+FDx(k)

(6)

其中

ueq(k)=-(CeB)-1Ce(A-I)x(k)

(7)

FD=[f1f2…fn]

式中f1、f2、…、fn表示系統(tǒng)各狀態(tài)變量的增益，I表示單位矩陣。

根據(jù)文獻(xiàn)[20]，定義Lyapunov函數(shù)為

(8)

由式(5)～(7)得到

s(k+1)=CeAx(k)-Ce(A-I)x(k)+CeBFDx(k)=
Cex(k)+CeBFDx(k)=s(k)+CeBFDx(k)

s2(k+1)-s2(k)=2s(k)CeBFDx(k)+

(CeBFDx(k))2

(9)

根據(jù)Lyapunov穩(wěn)定性定理，s(k)=0為全局穩(wěn)定平衡面，任意初始位置的狀態(tài)都會(huì)趨向切換面s(k)。取到達(dá)條件為

s2(k+1)

(10)

則由式(9)可得

(CeBFDx(k))2<-2s(k)CeBFDx(k)

即

取|fi|=f0，f0>0，則對(duì)于每一個(gè)i，有

(11)

定義

(12)

將式(12)代入式(11)得到

(13)

可得

(14)

即

(15)

只要滿足式(15)即可滿足條件式(10)。

由式(12)和式(15)得

即

(16)

滿足式(16)條件，確定f0。

綜上所述，離散滑模控制率為式(6)、(7)、(16)，即

(17)

則系統(tǒng)穩(wěn)定。

3 卡爾曼濾波

明輪船在航行過程中不斷受到風(fēng)、波浪的干擾，船艏的艏搖角度始終處于小幅波動(dòng)狀態(tài)。文獻(xiàn)[18]認(rèn)為風(fēng)浪對(duì)船舶航行的干擾主要由周期為5～20 s的長(zhǎng)波疊加組成。部分二階波引起的振蕩可以通過自動(dòng)航向糾偏抵消，而一階波形引起的振蕩和運(yùn)動(dòng)超出了自動(dòng)駕駛儀的工作頻帶，會(huì)引起自動(dòng)駕駛設(shè)備的高頻運(yùn)動(dòng)，需要通過濾波的方式降低一階波形的干擾。

船舶運(yùn)動(dòng)控制采取的濾波的方式主要有低通濾波、帶通濾波、級(jí)聯(lián)陷波濾波、基于觀測(cè)器的濾波器等。雖然前述濾波可以降低風(fēng)浪造成的干擾振蕩，但是存在“相位滯后”的問題。經(jīng)仿真、實(shí)際使用，發(fā)現(xiàn)此“相位滯后”極易引發(fā)系統(tǒng)振蕩，造成航行的不穩(wěn)定。

卡爾曼濾波能夠從噪聲污染的測(cè)量信號(hào)中，采用遞歸方式估計(jì)動(dòng)態(tài)系統(tǒng)的真實(shí)狀態(tài)，有效地避免了“相位滯后”。在雷達(dá)、通訊、導(dǎo)航、目標(biāo)跟蹤領(lǐng)域被廣泛使用。若系統(tǒng)離散狀態(tài)方程為[14,17]

(18)

式中X——系統(tǒng)狀態(tài)矩陣

Φ——系統(tǒng)矩陣

H——觀測(cè)矩陣Y——系統(tǒng)輸出

W(k)——輸入噪聲

Z(k)——觀測(cè)噪聲

?！肼曭?qū)動(dòng)矩陣

其卡爾曼濾波增益的計(jì)算過程為：

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

(k+1|k)=Φ(k|k)

(2)狀態(tài)更新

(k+1|k+1)=(k+1|k)+Ka(k+1)ε(k+1)

其中

ε(k+1)=Y(k+1)-H(k+1|k)

式中ε——系統(tǒng)實(shí)際輸出與觀測(cè)估計(jì)輸出的誤差

Ka——卡爾曼濾波增益

(3)濾波增益矩陣

Ka(k+1)=P(k+1|k)HT(HP(k+1|k)HT+R)-1

式中P——協(xié)方差矩陣R——Z(k)的方差

(4)一步預(yù)測(cè)協(xié)方差矩陣

P(k+1|k)=ΦP(k|k)ΦTΓQΓT

式中Q——W(k)的方差

(5)協(xié)方差陣更新

P(k+1|k+1)=(In-Ka(k+1)H)P(k+1|k)

式中In——單位對(duì)角陣

初始條件

式中μ0——系統(tǒng)狀態(tài)初始值

P0——協(xié)方差矩陣初始值

卡爾曼濾波將系統(tǒng)模型引入濾波過程，通過狀態(tài)估計(jì)方差計(jì)算卡爾曼增益，反復(fù)迭代得到最優(yōu)狀態(tài)估計(jì)。且由于離散卡爾曼濾波不需要存儲(chǔ)大量歷史數(shù)據(jù)，適合計(jì)算機(jī)工作。明輪船的航跡跟蹤和航向保持需使用二維卡爾曼濾波，故對(duì)艏搖角及艏搖角速度進(jìn)行二維濾波。

4 控制系統(tǒng)結(jié)構(gòu)設(shè)計(jì)與仿真

明輪船艏搖角跟蹤控制系統(tǒng)的結(jié)構(gòu)如圖1所示。

圖1 艏搖角跟蹤控制系統(tǒng)Fig.1 Yaw angle tracking control system

電子羅經(jīng)和慣性導(dǎo)航設(shè)備給出明輪船當(dāng)前航向角ψ、艏搖角速度r，經(jīng)卡爾曼濾波后進(jìn)行負(fù)反饋，得到濾波降噪后的當(dāng)前航向角、艏搖角速度，航向控制器根據(jù)給出的目標(biāo)航向角ψd，結(jié)合負(fù)反饋信號(hào)進(jìn)行計(jì)算、經(jīng)過左右明輪轉(zhuǎn)速分配器分配后，得到左右明輪的各自轉(zhuǎn)速nl和nr。左右電動(dòng)機(jī)轉(zhuǎn)速控制器根據(jù)給定的左右明輪的各自轉(zhuǎn)速nl和nr，給出驅(qū)動(dòng)左右電動(dòng)機(jī)的信號(hào)ul和ur驅(qū)動(dòng)電動(dòng)機(jī)。電動(dòng)機(jī)的轉(zhuǎn)速信號(hào)n1、n2經(jīng)負(fù)反饋，形成轉(zhuǎn)速閉環(huán)控制。據(jù)此推轉(zhuǎn)左右明輪，驅(qū)動(dòng)船體運(yùn)動(dòng)。

為驗(yàn)證算法有效性，使用Matlab軟件進(jìn)行計(jì)算機(jī)仿真測(cè)試。根據(jù)明輪船的試驗(yàn)艇參數(shù)，建立明輪船艏搖角動(dòng)態(tài)響應(yīng)模型[10]。根據(jù)控制系統(tǒng)結(jié)構(gòu)建立明輪船艏搖角仿真系統(tǒng)，如圖2所示。

圖2 明輪船艏搖角仿真系統(tǒng)Fig.2 Paddle wheel ship yaw angle simulation system

根據(jù)上述仿真系統(tǒng)結(jié)構(gòu)，將仿真程序輸入Matlab計(jì)算機(jī)軟件系統(tǒng)進(jìn)行動(dòng)態(tài)響應(yīng)仿真，得到仿真結(jié)果。仿真過程中，使用正弦信號(hào)作為跟蹤目標(biāo)信號(hào)。對(duì)控制系統(tǒng)跟蹤信號(hào)不僅有位置誤差的要求，還有速度誤差的限制。不施加干擾時(shí)的跟蹤效果如圖3所示。

圖3 無干擾跟蹤效果Fig.3 Tracking effect without interference

圖3顯示了明輪船船艏采用滑?？刂品绞脚c采取純PD控制方式時(shí)船艏對(duì)目標(biāo)偏航角的跟蹤效果，二者差距顯著。兩種控制方式的初始狀態(tài)角度是1.5°。跟蹤目標(biāo)信號(hào)是從0°開始的正弦信號(hào)。仿真結(jié)果顯示離散滑?？刂颇軌蚩焖俑櫮繕?biāo)信號(hào)。由于滑模算法的快速跟蹤特性，使得采取滑模算法時(shí)，控制信號(hào)在初始階段存在短時(shí)間的過飽和現(xiàn)象，可以通過限制控制信號(hào)輸出幅值的方式進(jìn)行消減。PD控制對(duì)目標(biāo)的跟蹤是逐步逼近，振蕩收斂，實(shí)現(xiàn)最終的跟蹤效果慢了許多。

抗干擾檢驗(yàn)時(shí)，在仿真系統(tǒng)的輸出信號(hào)上施加無量綱高斯噪聲信號(hào)，得到在干擾下的艏搖角跟蹤仿真結(jié)果，如圖4、5所示。

圖4 有干擾跟蹤仿真結(jié)果Fig.4 Tracking effect with interference

圖5 有干擾跟蹤仿真方差Fig.5 Interference tracking simulation variance

仿真結(jié)果表明：離散滑?？刂圃诟咚乖肼暩蓴_作用下，依然可以控制船艏進(jìn)行航向跟蹤，但是跟蹤過程諧波較大，有干擾無濾波跟蹤結(jié)果與目標(biāo)信號(hào)的無量綱方差達(dá)到0.4。可以預(yù)見干擾會(huì)導(dǎo)致欠驅(qū)動(dòng)系統(tǒng)明輪船艏做出幅度較大、頻率較高的頻繁控制動(dòng)作。這對(duì)明輪船驅(qū)動(dòng)設(shè)備、續(xù)航時(shí)間都會(huì)產(chǎn)生不利影響。

引入卡爾曼濾波降噪后的仿真結(jié)果表明，施加相同量級(jí)的高斯噪聲干擾，跟蹤信號(hào)的諧波幅值大幅度降低，如圖6、7所示。

圖6 有干擾濾波跟蹤仿真結(jié)果Fig.6 Interference filter tracking simulation results

圖7 有干擾濾波跟蹤仿真方差Fig.7 Interference filtering tracking simulation variance

經(jīng)數(shù)據(jù)分析，在加入卡爾曼濾波之后，跟蹤信號(hào)與目標(biāo)信號(hào)的無量綱方差降低到0.18。顯然，卡爾曼濾波的引入，對(duì)干擾起到了抑制與降低。這對(duì)于明輪船延長(zhǎng)續(xù)航時(shí)間、降低控制系統(tǒng)不必要的高頻動(dòng)作作用明顯。

5 試驗(yàn)

為驗(yàn)證算法的可行性，采用實(shí)際樣船進(jìn)行算法適航性驗(yàn)證。經(jīng)測(cè)量獲取明輪船參數(shù)，計(jì)算相關(guān)水動(dòng)力參數(shù)。試驗(yàn)明輪船見圖8。

圖8 明輪船樣船F(xiàn)ig.8 Paddle wheel ship

將離散滑模算法的控制代碼輸入控制系統(tǒng)，并反復(fù)進(jìn)行調(diào)試、試驗(yàn)。試驗(yàn)當(dāng)天多云，風(fēng)力3～4級(jí)，氣溫15～24℃。得到轉(zhuǎn)艏運(yùn)動(dòng)試驗(yàn)動(dòng)態(tài)響應(yīng)結(jié)果如圖9所示。

圖9 轉(zhuǎn)艏動(dòng)態(tài)響應(yīng)曲線Fig.9 Dynamic response curves

試驗(yàn)曲線顯示了明輪船轉(zhuǎn)艏運(yùn)動(dòng)、直線運(yùn)動(dòng)的艏搖角動(dòng)態(tài)變化過程。開始階段是轉(zhuǎn)艏運(yùn)動(dòng)，艏搖角急速拉升。經(jīng)圖9的試驗(yàn)結(jié)果分析，離散滑?？刂品绞较缕浇堑淖畲蟪{(diào)量較PD控制方式減小25%，調(diào)整時(shí)間減少50%。受制于明輪船轉(zhuǎn)艏運(yùn)動(dòng)過程的艏搖角速度限幅和執(zhí)行電機(jī)輸出飽和限制的影響，轉(zhuǎn)艏運(yùn)動(dòng)的快速性、超調(diào)量不及仿真效果，但是明輪船的直線跟蹤效果達(dá)到預(yù)期，直線跟蹤偏差效果見圖10。

圖10 明輪船直線跟蹤偏差Fig.10 Paddle wheel ship linear tracking deviation

從圖10可見，明輪船進(jìn)入直線軌跡跟蹤時(shí)，由于轉(zhuǎn)艏運(yùn)動(dòng)，跟蹤軌跡切換造成跟蹤目標(biāo)不定，致使明輪船剛進(jìn)入直線段航跡點(diǎn)時(shí)出現(xiàn)較大偏差。隨后，明輪船完全進(jìn)入直線段目標(biāo)軌跡區(qū)域航行，由于引入卡爾曼濾波滑模算法，直線跟蹤偏差均在10 cm以內(nèi)，達(dá)到直線跟蹤目標(biāo)要求。由于跟蹤過程存在陣風(fēng)干擾，明輪船被風(fēng)力推向偏航。設(shè)計(jì)的濾波滑模控制算法進(jìn)行了糾偏操作，使明輪船沿著目標(biāo)軌跡航行。分析圖10的試驗(yàn)數(shù)據(jù)，離散滑?？刂茖?shí)現(xiàn)的明輪船直線航跡在剛進(jìn)入直線段時(shí)，快速逼近目標(biāo)軌跡，在風(fēng)浪干擾的作用下，最大偏差只是瞬間超過10 cm，其他時(shí)間段均低于10 cm，遠(yuǎn)優(yōu)于PD控制實(shí)現(xiàn)的效果。

無風(fēng)浪干擾時(shí)，無卡爾曼濾波PD控制算法的跟蹤精度雖然滿足要求，但是收斂速度較慢。當(dāng)出現(xiàn)風(fēng)浪干擾時(shí)，明輪船出現(xiàn)較大的偏航，在無濾波的PD控制算法作用下，最終也能糾偏，但是航行過程的波動(dòng)較大，最大偏差達(dá)到0.35 m，收斂到滿足偏差要求的航跡時(shí)間也較長(zhǎng)。實(shí)際航行試驗(yàn)表現(xiàn)為在目標(biāo)航跡附近多次振蕩。

有卡爾曼濾波的滑模航跡跟蹤控制算法，在跟蹤精度滿足要求時(shí)，跟蹤過程更加平穩(wěn)，收斂速度更快。這對(duì)于明輪船工作過程的穩(wěn)定性、節(jié)能降耗、延長(zhǎng)續(xù)航時(shí)間起到重要作用。

6 結(jié)論

(1)采用離散滑?？刂品绞綄?duì)明輪船的轉(zhuǎn)艏運(yùn)動(dòng)進(jìn)行控制。通過建立轉(zhuǎn)艏運(yùn)動(dòng)模型，進(jìn)行了離散滑模控制仿真，證明離散滑模算法可以對(duì)明輪船的轉(zhuǎn)艏運(yùn)動(dòng)、直線跟蹤進(jìn)行有效控制。

(2)對(duì)干擾采取了卡爾曼濾波進(jìn)行消減。仿真結(jié)果說明，卡爾曼濾波可以有效抑制航行過程中風(fēng)力、波浪對(duì)航跡跟蹤造成的干擾，對(duì)提升系統(tǒng)控制過程的平穩(wěn)、節(jié)能降耗、延長(zhǎng)續(xù)航起到重要作用。

(3)對(duì)算法進(jìn)行了試驗(yàn)驗(yàn)證。轉(zhuǎn)艏運(yùn)動(dòng)在算法控制下，均能實(shí)現(xiàn)直線跟蹤。結(jié)果表明，采取基于卡爾曼濾波的滑?？刂品绞捷^PD控制，艏搖角的最大超調(diào)量比PD控制方式減小25%，調(diào)整時(shí)間減少50%，航跡偏差低于10 cm。由于轉(zhuǎn)艏運(yùn)動(dòng)的角速度限幅、執(zhí)行電機(jī)的飽和特性限制，明輪船的大角度轉(zhuǎn)艏動(dòng)態(tài)響應(yīng)特性不及仿真效果，但目標(biāo)軌跡跟蹤效果達(dá)到預(yù)期。