基于MPC的半潛式無(wú)人艇導(dǎo)航軌跡跟蹤控制研究

張麗珍,高浩,吳迪,李衛(wèi),陸天辰

(上海海洋大學(xué) 工程學(xué)院,上海 201306)

0 引 言

智能無(wú)人艇在海洋物理環(huán)境監(jiān)控、近海安全等科學(xué)領(lǐng)域具有重要研究?jī)r(jià)值[1]．半潛式無(wú)人艇作為無(wú)人艇大家族中的一員,由于其獨(dú)特的航行方式,具有能夠有效降低海水表面浪區(qū)的沖擊和影響、隱蔽性好等優(yōu)點(diǎn)[2],我國(guó)在半潛式無(wú)人艇方面的研究較國(guó)外起步較晚[3],上海海洋大學(xué)研究團(tuán)隊(duì)開(kāi)發(fā)了一種新型半潛式無(wú)人艇．但是在半潛式無(wú)人艇導(dǎo)航過(guò)程中,使用PID控制方法對(duì)導(dǎo)航規(guī)劃軌跡進(jìn)行跟蹤時(shí),出現(xiàn)跟蹤誤差較大的問(wèn)題,最大跟蹤誤差可達(dá)1 m,影響半潛式無(wú)人艇的航行穩(wěn)定性和續(xù)航里程．

導(dǎo)航軌跡跟蹤是智能無(wú)人艇相關(guān)研究的一個(gè)重要內(nèi)容．在智能無(wú)人艇的軌跡跟蹤方面,國(guó)內(nèi)外學(xué)者對(duì)此做了很多研究[4]．目前,智能無(wú)人艇的軌跡跟蹤問(wèn)題主要控制方法有模糊控制[5-6]、模型預(yù)測(cè)控制(MPC)[7-8]等方法．但是上述研究都是水面無(wú)人艇和水下無(wú)人艇的軌跡跟蹤問(wèn)題,對(duì)半潛式無(wú)人艇的導(dǎo)航軌跡跟蹤控制研究較少．半潛式無(wú)人艇在實(shí)際導(dǎo)航航行過(guò)程中,存在環(huán)境干擾等因素導(dǎo)致的軌跡跟蹤誤差較大的問(wèn)題,目前還沒(méi)有一個(gè)實(shí)際可采用的控制方法,相關(guān)內(nèi)容的研究也非常少．因此,本文針對(duì)半潛式無(wú)人艇在導(dǎo)航軌跡跟蹤控制方面的問(wèn)題,提出了一種基于MPC的軌跡跟蹤控制方法．首先建立新型半潛式無(wú)人艇的運(yùn)動(dòng)模型,并在模型的基礎(chǔ)上設(shè)計(jì)了MPC控制器,最后通過(guò)仿真和試驗(yàn)對(duì)該控制方法的有效性進(jìn)行驗(yàn)證．

1 半潛式無(wú)人艇數(shù)學(xué)模型建立

1.1 半潛式無(wú)人艇運(yùn)動(dòng)方程建立

本文研究的半潛式無(wú)人艇主要由左、右兩個(gè)艇體和浮體組成,如圖1(a)所示,半潛式無(wú)人艇在航行時(shí),二個(gè)艇體位于水面以下,搭載通信設(shè)備的小體積浮體位于水面上．半潛式無(wú)人艇的運(yùn)動(dòng)為6自由度的運(yùn)動(dòng),考慮到研究的是水面上的平面運(yùn)動(dòng)軌跡,在不影響研究問(wèn)題的基礎(chǔ)上,進(jìn)行適當(dāng)簡(jiǎn)化,所做假設(shè)如下:

1) 只考慮縱蕩、橫蕩和艏搖這3個(gè)方向的運(yùn)動(dòng),忽略垂蕩、橫搖和縱搖;

2) 半潛式無(wú)人艇關(guān)于XOZ平面對(duì)稱;

3) 半潛式無(wú)人艇重心、浮心均在Z軸上．

基于上述假設(shè),六自由度半潛式無(wú)人艇模型簡(jiǎn)化成三自由度運(yùn)動(dòng)問(wèn)題,如圖1(b)所示．

(b)三自由度模型

由圖1(b)所示，外界干擾下的半潛式無(wú)人艇3自由度運(yùn)動(dòng)方程為[9-13]:

(1)

(2)

式中:τ11、τ12分別為半潛式無(wú)人艇左、右艇體艉部螺旋槳產(chǎn)生的推進(jìn)力;n1、n2分別為左、右兩側(cè)螺旋槳轉(zhuǎn)速;ts為推力減額系數(shù);D為螺旋槳直徑;Kt為無(wú)因次推力系數(shù);Bs為每個(gè)艇體的中心線與坐標(biāo)點(diǎn)Os之間距離;τ1為半潛式無(wú)人艇推力;τ3為半潛式無(wú)人艇艏搖力矩．

(3)

式中:τdu、τdv分別為波浪在x軸和y軸方向上的干擾力;τdr為波浪干擾力矩;τd可具體表述為

(4)

式中:δs為半潛式無(wú)人艇的艏向角;ρ為海水密度;L為浮體長(zhǎng)度;B為浮體寬度;si為波頻．其中si又可表述為

(5)

式中:Ai為波的幅度;λi為波的長(zhǎng)度;wei為頻率;φi為時(shí)變的相位．

本文建立的3自由度運(yùn)動(dòng)模型適用于由水面小浮體和水下雙螺旋槳驅(qū)動(dòng)的雙艇體組成的半潛式無(wú)人艇,且風(fēng)的干擾可忽略不計(jì),只考慮波浪的干擾．

1.2 半潛式無(wú)人艇的狀態(tài)空間方程

(6)

輸出方程為線性方程,

2 MPC控制器設(shè)計(jì)

在半潛式無(wú)人艇進(jìn)行導(dǎo)航軌跡跟蹤的過(guò)程中,首先根據(jù)半潛式無(wú)人艇的初始狀態(tài)(位置,航向角,速度),結(jié)合半潛式無(wú)人艇的預(yù)測(cè)模型得出系統(tǒng)的預(yù)測(cè)輸出;然后將預(yù)測(cè)輸出和參考軌跡的誤差值和系統(tǒng)約束參數(shù)結(jié)合目標(biāo)函數(shù),求解出控制量;最后將控制量和外界干擾量共同作用到半潛式無(wú)人艇并保持一個(gè)控制周期,并將半潛式無(wú)人艇新的運(yùn)動(dòng)狀態(tài)輸入至預(yù)測(cè)模型中,完成一個(gè)周期內(nèi)的控制過(guò)程,基于MPC的半潛式無(wú)人艇導(dǎo)航軌跡跟蹤控制原理如圖2所示．為了使上述過(guò)程便于實(shí)施,首先對(duì)半潛式無(wú)人艇的狀態(tài)空間方程進(jìn)行線性化和離散化,得到新的離散化狀態(tài)空間方程;然后結(jié)合新的離散化狀態(tài)空間方程、目標(biāo)函數(shù)和系統(tǒng)約束,可將半潛式無(wú)人艇導(dǎo)航軌跡跟蹤問(wèn)題轉(zhuǎn)化為最優(yōu)值問(wèn)題進(jìn)行求解．

圖2 MPC軌跡跟蹤控制原理圖

2.1 狀態(tài)方程線性化

式(6)中狀態(tài)方程為非線性方程,不適宜用作控制器的設(shè)計(jì)．為了便于控制器的實(shí)現(xiàn),對(duì)上述非線性狀態(tài)空間方程進(jìn)行線性化后,再離散化．

在模型預(yù)測(cè)控制中,非線性系統(tǒng)近似為線性系統(tǒng)時(shí),通常采用近似線性化方法[15]．在任意參考點(diǎn)(xa,ua)處線性化后的狀態(tài)方程:

(7)

2.2 狀態(tài)空間模型離散化

用歐拉法[16]對(duì)式(7)離散化,并聯(lián)立式(6),可得半潛式無(wú)人艇離散化線性狀態(tài)空間方程為

(8)

2.3 MPC預(yù)測(cè)方程

可得到如下新的離散化狀態(tài)空間方程:

ξ(k+1)=Asξ(k)+BsΔu(k),

y(k)=Csξ(k).

(9)

為了簡(jiǎn)化運(yùn)算,假定在系統(tǒng)預(yù)測(cè)時(shí)域Np內(nèi),

Az+i=Az,Bz+i=Bz,i=1,2,…,Np.

(10)

由式(9)和(10),可得系統(tǒng)的預(yù)測(cè)方程為

Ys((k+1)|k)=Wsξ(k)+WuΔU(k),

(11)

式中:Ys((k+1)|k)表示k時(shí)刻對(duì)k+1時(shí)刻的預(yù)測(cè)輸出;ΔU(k)為控制時(shí)域Nc內(nèi)的預(yù)測(cè)輸入量;Ws和Wu為系統(tǒng)矩陣．各項(xiàng)具體定義如下:

2.4 目標(biāo)函數(shù)

以系統(tǒng)輸出量的偏差、控制量增量來(lái)構(gòu)建目標(biāo)函數(shù):

(12)

為避免目標(biāo)函數(shù)無(wú)可行性解,加入松弛因子[17],目標(biāo)函數(shù)為

(13)

式中:ΔY((k+i)|k)為輸出量和期望量之差;Nq為輸出加權(quán)矩陣;Nr為控制加權(quán)矩陣;ε為松弛因子;ρ為權(quán)重系數(shù)．該式第一項(xiàng)反映半潛式無(wú)人艇跟蹤控制器的跟蹤能力;第二項(xiàng)反映半潛式無(wú)人艇運(yùn)動(dòng)的平穩(wěn)性,保證控制增量較小,同時(shí)較小的控制增量可減少控制系統(tǒng)對(duì)半潛式無(wú)人艇的航向角的調(diào)整幅度,可在一定程度內(nèi)減少系統(tǒng)的能源消耗．由式(8)和式(11),可得到如下標(biāo)準(zhǔn)二次型目標(biāo)函數(shù):

(14)

其中:

2.5 系統(tǒng)約束

由于半潛式無(wú)人艇運(yùn)動(dòng)情況受到其機(jī)械性能的影響,需要對(duì)控制參數(shù)進(jìn)行約束,約束條件可以表示為

Umin≤U(k)≤Umax,

ΔUmin≤PΔU(k)+U(k)≤ΔUmax.

(15)

半潛式無(wú)人艇航行速度參數(shù)如表1所示．

表1 半潛式無(wú)人艇航行速度參數(shù)

根據(jù)表1中的數(shù)據(jù)和文獻(xiàn)[18],對(duì)控制量和控制增量做如下約束:

2.6 最優(yōu)值問(wèn)題轉(zhuǎn)化與求解

綜合上述的目標(biāo)函數(shù)和系統(tǒng)約束條件,基于MPC的半潛式無(wú)人艇航跡跟蹤可以描述為最優(yōu)值問(wèn)題．由式(11)、(13)及(15),基于MPC的半潛式無(wú)人艇航跡跟蹤問(wèn)題最終轉(zhuǎn)化為標(biāo)準(zhǔn)二次型函數(shù)最優(yōu)值問(wèn)題,如式(16)所示．

ΔUmin≤PΔU(k)+U(k)≤ΔUmax.

(16)

對(duì)上述的最優(yōu)值問(wèn)題,采用QP求解器,可快速求得最優(yōu)控制序列,輸入至半潛式無(wú)人艇控制模型,如此循環(huán)迭代,實(shí)現(xiàn)對(duì)無(wú)人艇的最優(yōu)控制輸入．

3 仿真及試驗(yàn)驗(yàn)證

為了驗(yàn)證MPC控制器的有效性,搭建MATLAB仿真平臺(tái),觀察其跟蹤航跡線的誤差并分析其跟蹤性能．為了能夠全面地分析控制器的性能,分別在無(wú)外界干擾和存在外界干擾兩種情況下,對(duì)控制跟蹤性能進(jìn)行仿真．為驗(yàn)證本文設(shè)計(jì)的控制器有效性,采用MPC控制方法和原有PID方法進(jìn)行對(duì)比仿真和試驗(yàn)驗(yàn)證．

3.1 仿真參數(shù)設(shè)置

仿真中使用的無(wú)人艇的基本參數(shù)如表2和表3所示,仿真初始參數(shù)如表4所示．

表2 半潛式無(wú)人艇技術(shù)參數(shù)

表3 控制器仿真參數(shù)

表4 仿真初始參數(shù)

圖3 仿真流程圖

3.2 仿真結(jié)果及分析

3.2.1 無(wú)外界干擾下半潛式無(wú)人艇軌跡跟蹤效果

無(wú)外界干擾下,仿真結(jié)果,如圖4所示．

圖4 無(wú)干擾軌跡跟蹤效果圖

由圖4可見(jiàn),在無(wú)外界干擾的情況下,原有PID控制方法和本文設(shè)計(jì)的MPC控制方法的軌跡幾乎與參考軌跡重合,跟蹤誤差都比較小,表明二種方法都能使半潛式無(wú)人艇快速跟蹤上參考軌跡,能夠滿足設(shè)計(jì)要求,具有較好的軌跡跟蹤效果．

3.2.2 外界干擾下半潛式無(wú)人艇軌跡跟蹤效果

在外界干擾下,仿真結(jié)果如圖5～7所示．

圖5 外界干擾下軌跡跟蹤效果對(duì)比圖

圖6 跟蹤誤差對(duì)比圖

圖7 航向角對(duì)比圖

由圖5可知,在有外界干擾的情況下,原有PID控制方法和MPC控制方法都完成了對(duì)軌跡的跟蹤,MPC控制方法的軌跡跟蹤效果比原有PID控制方法更好．綜合分析圖5～6可看出,原有PID控制方法比MPC控制方法的航跡偏差較大,誤差波動(dòng)也比較大,原有PID控制方法的誤差在0.4 ～0.6 m波動(dòng),誤差最大值可達(dá)0.65 m;而MPC控制方法誤差波動(dòng)范圍為0.2～0.45 m,誤差最大值僅為0.5 m,相比之下,MPC控制方法的軌跡誤差更小,軌跡誤差可以減小40%～50%．由圖7可知,原有PID控制方法的航向角變化幅度比MPC控制方法要大,不僅影響了半潛式無(wú)人艇的航行平穩(wěn)性,同時(shí)控制系統(tǒng)還需要耗費(fèi)更多的能源來(lái)調(diào)整半潛式無(wú)人艇的航向角．綜合圖5～7,可得:MPC控制方法的軌跡跟蹤誤差小于原有PID控制方法．

3.3 試驗(yàn)驗(yàn)證及分析

為了進(jìn)一步驗(yàn)證該控制方法的有效性,對(duì)控制方法進(jìn)行實(shí)際環(huán)境驗(yàn)證,試驗(yàn)場(chǎng)所選擇在上海海洋大學(xué)圖書(shū)館旁小湖,如圖8所示．為了保證仿真與試驗(yàn)的一致性,在試驗(yàn)時(shí),MPC控制器的參數(shù)與仿真控制器的參數(shù)相同．由于實(shí)際環(huán)境外界干擾的復(fù)雜性,本次試驗(yàn)僅對(duì)半潛式無(wú)人艇直線導(dǎo)航軌跡跟蹤效果進(jìn)行驗(yàn)證;采用原有PID控制方法和MPC控制方法分別對(duì)半潛式無(wú)人艇進(jìn)行直線跟蹤試驗(yàn)．試驗(yàn)驗(yàn)證的無(wú)人艇采用上海海洋大學(xué)研究團(tuán)隊(duì)的半潛式無(wú)人艇,半潛式無(wú)人艇實(shí)物如圖9所示．試驗(yàn)前,利用半潛式無(wú)人艇導(dǎo)航設(shè)備設(shè)置一條直線參考軌跡,觀察并記錄半潛式無(wú)人艇運(yùn)行狀況,如圖10為半潛式無(wú)人艇在運(yùn)行中．

圖8 上海海洋大學(xué)圖書(shū)館旁小湖實(shí)拍圖

圖9 半潛式無(wú)人艇實(shí)物圖

圖10 半潛式無(wú)人艇試驗(yàn)運(yùn)行圖

在試驗(yàn)過(guò)程中,利用半潛式無(wú)人艇GPS/BDS定位設(shè)備和實(shí)時(shí)動(dòng)態(tài)(RTK)輔助定位設(shè)備讀取半潛式無(wú)人艇的位置信息,半潛式無(wú)人艇的位置誤差在20 cm以內(nèi)．對(duì)此次試驗(yàn)采集到的位置信息進(jìn)行統(tǒng)計(jì)后,得到半潛式無(wú)人艇的軌跡跟蹤誤差圖,如圖11所示．

圖11 半潛式無(wú)人艇試驗(yàn)軌跡誤差圖

由圖11可知,在實(shí)驗(yàn)過(guò)程中,MPC控制方法和原有PID控制方法的跟蹤誤差一直存在,且都在一定范圍內(nèi)波動(dòng),但原有PID控制方法的誤差值在0.5～1 m波動(dòng),MPC控制方法的誤差值在0.4 ～0.6 m波動(dòng),軌跡跟蹤誤差減小了50%左右．本文提出的MPC控制方法,相比原有PID控制方法而言,導(dǎo)航軌跡跟蹤誤差的數(shù)值和變動(dòng)范圍更小,較小的跟蹤誤差,使半潛式無(wú)人艇以更短的路徑完成對(duì)導(dǎo)航參考軌跡的跟蹤,可在一定范圍內(nèi)節(jié)省能源．

綜上所述,本文設(shè)計(jì)的MPC控制器在滿足導(dǎo)航軌跡跟蹤效果的同時(shí),可以提高半潛式無(wú)人艇航行穩(wěn)定性,能夠減少能源消耗,有效提升半潛式無(wú)人艇的續(xù)航能力．

4 結(jié) 論

為了解決半潛式無(wú)人艇在導(dǎo)航航行中軌跡跟蹤誤差較大的問(wèn)題,使用了基于MPC的軌跡跟蹤控制方法解決這一問(wèn)題．由仿真和試驗(yàn)結(jié)果可得:本文提出的MPC控制方法,相較于原有PID控制方法,能夠?qū)?dǎo)航軌跡跟蹤誤差減小50%左右,提高軌跡跟蹤的精度;同時(shí)MPC控制方法能夠以更短的路徑完成對(duì)參考軌跡的跟蹤過(guò)程,在一定程度上減少能源消耗,有利于實(shí)際使用．