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

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

0 引 言

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

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

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

1.1 半潛式無人艇運動方程建立

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

1) 只考慮縱蕩、橫蕩和艏搖這3個方向的運動,忽略垂蕩、橫搖和縱搖;

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

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

基于上述假設(shè),六自由度半潛式無人艇模型簡化成三自由度運動問題,如圖1(b)所示．

(b)三自由度模型

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

(1)

(2)

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

(3)

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

(4)

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

(5)

式中:Ai為波的幅度;λi為波的長度;wei為頻率;φi為時變的相位．

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

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

(6)

輸出方程為線性方程,

2 MPC控制器設(shè)計

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

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

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

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

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

(7)

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

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

(8)

2.3 MPC預(yù)測方程

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

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

y(k)=Csξ(k).

(9)

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

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

(10)

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

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

(11)

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

2.4 目標函數(shù)

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

(12)

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

(13)

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

(14)

其中:

2.5 系統(tǒng)約束

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

Umin≤U(k)≤Umax,

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

(15)

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

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

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

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

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

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

(16)

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

3 仿真及試驗驗證

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

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

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

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

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

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

圖3 仿真流程圖

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

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

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

圖4 無干擾軌跡跟蹤效果圖

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

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

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

圖5 外界干擾下軌跡跟蹤效果對比圖

圖6 跟蹤誤差對比圖

圖7 航向角對比圖

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

3.3 試驗驗證及分析

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

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

圖9 半潛式無人艇實物圖

圖10 半潛式無人艇試驗運行圖

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

圖11 半潛式無人艇試驗軌跡誤差圖

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

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

4 結(jié) 論

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