基于PSO-NTSMC的UUV有限時(shí)間路徑跟蹤控制

于浩淼， 劉承蔚， 郭晨， 賈兆巖

(1.大連海事大學(xué) 船舶電氣工程學(xué)院， 遼寧 大連 116026；2.中國兵器工業(yè)集團(tuán) 航空彈藥研究院有限公司， 黑龍江 哈爾濱 150001)

0 引言

水下無人航行器(UUV)路徑跟蹤是近年來的熱點(diǎn)問題。路徑跟蹤控制不受時(shí)間約束，只要求欠驅(qū)動(dòng)UUV收斂至期望路徑，而對(duì)于何時(shí)到達(dá)何地沒有嚴(yán)格要求。通過引入“虛擬向?qū)А备欀茖?dǎo)策略，設(shè)計(jì)虛擬向?qū)邢蛩俣龋⑶医⒘饲夫?qū)動(dòng)UUV與期望路徑參考點(diǎn)之間的誤差方程。由于其本身的欠驅(qū)動(dòng)性和非完整性約束，欠驅(qū)動(dòng)UUV路徑跟蹤問題的研究一直是當(dāng)前的熱點(diǎn)與難點(diǎn)。Lapierre等針對(duì)模型不確定性約束下的欠驅(qū)動(dòng)UUV水平面路徑跟蹤控制問題，采用反步法和李雅普諾夫直接法設(shè)計(jì)一種魯棒非線性控制器，提高了模型不確定性約束下系統(tǒng)的魯棒性，實(shí)現(xiàn)了欠驅(qū)動(dòng)UUV的水平面路徑跟蹤控制。Moe等研究未知洋流情況下的UUV二維路徑跟蹤運(yùn)動(dòng)，提出了制導(dǎo)律和自適應(yīng)反饋線性化方法與滑模控制方法相結(jié)合組成的閉環(huán)系統(tǒng)，完成了路徑跟蹤控制。苗建明等提出一種基于改進(jìn)反步法的三維空間曲線路徑跟蹤控制器，解決了運(yùn)動(dòng)學(xué)控制器存在非因果現(xiàn)象的問題和動(dòng)力學(xué)控制器過于復(fù)雜的問題。Habib等針對(duì)由海浪和海流擾動(dòng)引起的時(shí)變干擾約束下的欠驅(qū)動(dòng)UUV路徑跟蹤控制，提出一種改進(jìn)的自抗擾控制器。張偉等針對(duì)UUV垂直面路徑跟蹤提出一種基于非線性連續(xù)模型預(yù)測(cè)控制算法的控制器，實(shí)現(xiàn)了欠驅(qū)動(dòng)UUV路徑跟蹤控制。Guo等針對(duì)參數(shù)攝動(dòng)約束下欠驅(qū)動(dòng)UUV的三維直線路徑跟蹤控制問題，提出了一種固定時(shí)間收斂的非奇異快速終端滑模三維直線路徑跟蹤控制方法。

粒子群優(yōu)化(PSO)算法是一種典型的智能優(yōu)化算法,該算法模擬鳥群覓食行為的原理，利用群體智能建立模型。PSO算法具有優(yōu)化性能好、操作簡(jiǎn)單、容易實(shí)現(xiàn)的優(yōu)點(diǎn)，因而受到了國內(nèi)外學(xué)者的廣泛關(guān)注。胡文華等設(shè)計(jì)一種針對(duì)四軸飛行器軌跡跟蹤的串級(jí)自抗擾控制器，并且采用改進(jìn)PSO算法進(jìn)行控制器參數(shù)自整定，使系統(tǒng)性能得到優(yōu)化。張?zhí)旒训忍岢鲆环N基于并行結(jié)構(gòu)的多種群PSO算法，在基本PSO算法的基礎(chǔ)上，增加了能加速和減緩粒子群移動(dòng)的兩種更新公式，既改善了PSO算法種群多樣性差的缺陷，又能保證算法的計(jì)算精度。Fakoor等針對(duì)帶有時(shí)變載荷的4自由度衛(wèi)星，設(shè)計(jì)了一種滑動(dòng)模態(tài)和線性二次調(diào)節(jié)器(LQR)相結(jié)合的控制器，減少了阻尼干擾、提高了精度，并在其基礎(chǔ)上提出了一種尋找變量最優(yōu)值和歸一化積分平方誤差的PSO算法，有效地優(yōu)化了控制器參數(shù)，提高了系統(tǒng)性能。

在UUV路徑跟蹤控制領(lǐng)域，目前研究成果相對(duì)成熟。文獻(xiàn)[6-7]提出了具有創(chuàng)新性的動(dòng)力學(xué)控制器，但在運(yùn)動(dòng)學(xué)制導(dǎo)律的設(shè)計(jì)上有所欠缺；文獻(xiàn)[5]對(duì)傳統(tǒng)反步法進(jìn)行改進(jìn)，但沒有解決參數(shù)攝動(dòng)對(duì)控制器的影響；文獻(xiàn)[8]提出有限時(shí)間控制策略，但控制器參數(shù)精度上有所欠缺。因此，本文對(duì)上述問題進(jìn)行分析改進(jìn)。

本文研究的是欠驅(qū)動(dòng)UUV在水平面直線上的路徑跟蹤問題。從本質(zhì)上來說，直線軌跡與曲線軌跡沒有區(qū)別，只是執(zhí)行任務(wù)的需求不同。本文針對(duì)參數(shù)攝動(dòng)和恒定未知海流對(duì)于系統(tǒng)的影響，提出了一種基于PSO的有限時(shí)間路徑跟蹤方法。首先，基于改進(jìn)的視線法(LOS)制導(dǎo)方法構(gòu)造欠驅(qū)動(dòng)UUV的誤差方程，將位置跟蹤問題轉(zhuǎn)化為艏向角誤差的鎮(zhèn)定問題；與此同時(shí)，將速度跟蹤問題轉(zhuǎn)化為縱向速度誤差和轉(zhuǎn)艏角速度誤差的跟蹤問題。其次，通過引入非奇異終端滑模面，設(shè)計(jì)路徑跟蹤控制器，用以鎮(zhèn)定縱向速度誤差、轉(zhuǎn)艏角速度誤差和艏向角誤差，并且克服了參數(shù)攝動(dòng)和恒定未知海流干擾對(duì)系統(tǒng)產(chǎn)生的負(fù)面影響。接著，應(yīng)用PSO算法，將控制器參數(shù)調(diào)節(jié)轉(zhuǎn)化為目標(biāo)函數(shù)極小值的搜索問題，克服了手動(dòng)調(diào)參不準(zhǔn)確的影響，得到更優(yōu)秀的控制效果。最后，進(jìn)行了系統(tǒng)的數(shù)值仿真，數(shù)值仿真的結(jié)果表明，該控制方法可以在有限時(shí)間內(nèi)有效地執(zhí)行水平面欠驅(qū)動(dòng)UUV路徑跟蹤任務(wù)。

1 欠驅(qū)動(dòng)UUV運(yùn)動(dòng)模型

定義固定坐標(biāo)系和隨體坐標(biāo)系兩個(gè)坐標(biāo)系：固定坐標(biāo)系是一個(gè)慣性坐標(biāo)系，其軸指向地理位置的正北方向，軸指向地理位置的正東方向，軸指向地心方向；隨體坐標(biāo)系的軸指向UUV的艏部，軸指向UUV的右舷，軸指向UUV的底部。

本文考慮的模型是欠驅(qū)動(dòng)UUV在水平面上的運(yùn)動(dòng)問題，即只考慮進(jìn)退(沿軸平移)、橫移(沿軸平移)和回轉(zhuǎn)(繞軸旋轉(zhuǎn))3個(gè)自由度。欠驅(qū)動(dòng)UUV模型在橫移方向缺少控制輸入，則在此自由度上會(huì)受到加速度約束——2階非完整性約束。

因此，UUV運(yùn)動(dòng)學(xué)模型被描述為

(1)

式中：、分別表示隨體坐標(biāo)系下UUV進(jìn)退方向和橫移方向的線速度；表示UUV的艏向角；表示UUV的轉(zhuǎn)艏角速度。

UUV動(dòng)力學(xué)模型被描述為

(2)

式中：

(3)

2 欠驅(qū)動(dòng)UUV路徑跟蹤制導(dǎo)律設(shè)計(jì)

UUV在開闊海域時(shí)，可以看作沿兩個(gè)航跡點(diǎn)之間做直線運(yùn)動(dòng)。定義一系列離散航跡點(diǎn){,,…,,…,}，那么UUV就依次沿著相鄰目標(biāo)點(diǎn)做直線運(yùn)動(dòng)。這些離散航跡點(diǎn)的位置坐標(biāo)通常可以表示為[,]。相鄰航跡點(diǎn)(,+1)之間的直線段表示為，而航跡點(diǎn)為由航跡線-1到航跡線的轉(zhuǎn)向點(diǎn)。在直線段內(nèi)，UUV的期望航跡可以被描述為

(4)

(5)

UUV水平面直線航跡跟蹤示意圖如圖1所示。在圖1中，每個(gè)航跡點(diǎn)(除最后一個(gè)外)均引入一個(gè)局域Serret-Frenet坐標(biāo)系{}，且坐標(biāo)系{}的軸與重合。將慣性坐標(biāo)系的原點(diǎn)平移到航跡點(diǎn)上，然后進(jìn)行旋轉(zhuǎn)，使慣性坐標(biāo)系與局域Serret-Frenet坐標(biāo)系{}重合，其中旋轉(zhuǎn)角定義為

(6)

當(dāng)UUV運(yùn)動(dòng)時(shí)，任一時(shí)刻隨體坐標(biāo)系{}的原點(diǎn)在期望航跡上的投影在圖1中記為點(diǎn)，UUV在期望軌跡上的目標(biāo)點(diǎn)在圖1中記為點(diǎn)。當(dāng)航行器不斷運(yùn)動(dòng)時(shí)，局域坐標(biāo)系{}始終以投影點(diǎn)為原點(diǎn)，沿著軌跡不斷平移。根據(jù)幾何關(guān)系，隨體坐標(biāo)系{}的原點(diǎn)到直線航跡的距離為

(7)

(8)

定義航行器的期望艏向角為

=+Δ

(9)

圖1 UUV水平面路徑跟蹤示意圖Fig.1 UUV horizontal path tracking diagram

由(8)式可知，當(dāng)Δ→0°時(shí)，位置誤差趨近于0。那么UUV的水平面位置跟蹤問題轉(zhuǎn)化為艏向角誤差的問題，速度跟蹤問題轉(zhuǎn)化為縱向速度誤差(進(jìn)退方向)和艏向角速度誤差(回轉(zhuǎn)方向)的問題。

3 欠驅(qū)動(dòng)UUV路徑跟蹤控制器設(shè)計(jì)

為了簡(jiǎn)化設(shè)計(jì)，將UUV水平面運(yùn)動(dòng)控制分解為縱向速度控制和轉(zhuǎn)艏控制兩個(gè)子系統(tǒng)。引入非奇異終端滑模面，達(dá)到路徑跟蹤的效果，設(shè)計(jì)出全局有限時(shí)間收斂的運(yùn)動(dòng)學(xué)控制器。

3.1 縱向速度控制器設(shè)計(jì)

在實(shí)際工程中，橫向速度不可測(cè)量，且縱向速度遠(yuǎn)大于橫向速度，可以忽略橫向速度對(duì)控制器的影響。引入縱向速度誤差，則縱向速度控制問題就轉(zhuǎn)變?yōu)榱说逆?zhèn)定問題。引入如(10)式非奇異終端滑模來鎮(zhèn)定:

()=++()

(10)

(11)

(12)

(13)

選取如下李雅普諾夫函數(shù)：

(14)

對(duì)求1階導(dǎo)數(shù)，得

(15)

由有限時(shí)間定理可知，存在一個(gè)時(shí)間1，當(dāng)時(shí)間>1時(shí)滑模面收斂至0 rad/s，即=0 rad/s。

由=0 rad/s可得

(16)

(16)式等號(hào)兩邊對(duì)時(shí)間積分，可得

(17)

解上述不等式，可得

(18)

因此，存在時(shí)間常數(shù)=，當(dāng)時(shí)間>=1+時(shí)，縱向速度誤差收斂至0 m/s。

3.2 轉(zhuǎn)艏控制器設(shè)計(jì)

由第2節(jié)可知，UUV的水平面位置跟蹤問題已經(jīng)轉(zhuǎn)化為艏向角誤差的問題，定義=-。那么首先構(gòu)造李雅普諾夫函數(shù)：

(19)

(20)

由上文可知，跟蹤艏向角問題已經(jīng)轉(zhuǎn)化為鎮(zhèn)定艏向角速度誤差的問題，為完成控制目標(biāo)，引入非奇異終端滑模：

()=++()

(21)

(22)

與31節(jié)同理，轉(zhuǎn)艏控制律為

(23)

式中：

(24)

選取如下李雅普諾夫函數(shù)：

(25)

對(duì)求1階導(dǎo)數(shù)，得

(26)

由有限時(shí)間定理可知，存在一個(gè)時(shí)間常數(shù)2，當(dāng)時(shí)間>2時(shí)滑模面收斂至0 rad/s，即=0 rad/s。

4 欠驅(qū)動(dòng)UUV路徑跟蹤控制器參數(shù)優(yōu)化

4.1 PSO算法

本文采用PSO算法提升控制器的性能，利用PSO算法迭代尋優(yōu)的特性，找到定義適應(yīng)度函數(shù)的更優(yōu)解。

本文應(yīng)用的是標(biāo)準(zhǔn)PSO算法，粒子群每次迭代更新自身速度和位置的公式為

(+1)=·()+()[()-()]+
()[()-()]

(27)

(+1)=()+(+1)

(28)

式中：=1,2,…,，為粒子個(gè)數(shù)；=1,2,…,，為最大迭代次數(shù)；和為[0,1]范圍內(nèi)的均勻隨機(jī)數(shù)；和為學(xué)習(xí)因子；為慣性權(quán)重；為粒子的位置；為粒子的速度；()表示第個(gè)粒子在第次迭代時(shí)的參數(shù)；()表示所有粒子在第次迭代時(shí)的最優(yōu)參數(shù)。

4.2 控制律參數(shù)優(yōu)化方法

如32節(jié)所述，欠驅(qū)動(dòng)UUV水平面路徑跟蹤問題實(shí)際上已經(jīng)轉(zhuǎn)化為縱向速度誤差、轉(zhuǎn)艏角速度誤差和艏向角誤差的鎮(zhèn)定問題。適應(yīng)度函數(shù)選取IAT指標(biāo)，其定義為

(29)

本文中選取待優(yōu)化的參數(shù)有、、、、，故選取五維PSO算法，各參數(shù)的取值范圍見表1。設(shè)置粒子數(shù)為40，迭代次數(shù)為40。慣性權(quán)重的變化影響著PSO算法對(duì)最小值的搜索性能，當(dāng)較大時(shí)全局收斂能力較強(qiáng)，局部收斂性較弱；當(dāng)較小時(shí)局部收斂性較強(qiáng)，全局收斂性較弱。設(shè)置慣性權(quán)重初值為=1，更新公式為(+1)=099()。 隨著搜索的進(jìn)行不斷減小，這樣可以保證在算法開始時(shí)，各粒子能夠以較大的速度步長(zhǎng)在全局范圍內(nèi)探測(cè)到較好的區(qū)域；而搜索后期值較小，粒子能夠在極值點(diǎn)附近做精細(xì)的探索，從而使算法大概率向全局最優(yōu)解位置收斂。

隨著迭代次數(shù)的增加，適應(yīng)度值不斷減小，最后得到最小值點(diǎn)，該點(diǎn)值即為最優(yōu)解。最優(yōu)解對(duì)應(yīng)的參數(shù)值即為最優(yōu)參數(shù)值。

表1 參數(shù)搜索范圍

5 穩(wěn)定性分析

構(gòu)造李雅普諾夫函數(shù)

(30)

對(duì)(28)式求導(dǎo)，可得

(31)

為了分析欠驅(qū)動(dòng)UUV水平面路徑跟蹤控制系統(tǒng)穩(wěn)定性，選取李雅普諾夫函數(shù)：

=+++

(32)

對(duì)進(jìn)行求導(dǎo)，可得

(33)

6 系統(tǒng)仿真及結(jié)果

為了驗(yàn)證非奇異終端滑模控制器的控制效果以及PSO算法的優(yōu)化效果，在MATLAB軟件中進(jìn)行仿真實(shí)驗(yàn)。選取粒子種群規(guī)模=40，粒子最大迭代次數(shù)=40，學(xué)習(xí)因子=15、=20；該型欠驅(qū)動(dòng)UUV的相關(guān)運(yùn)動(dòng)參數(shù)詳見文獻(xiàn)[18]。

航行器水平面期望路徑的航跡點(diǎn)分布為(100 m,100 m)→(100 m,400 m)→(300 m,400 m)→(300 m,600 m)→(500 m,600 m)→(500 m,100 m)→(700 m,100 m)→(1000 m,600 m)。

航行器的初始狀態(tài)設(shè)置為(0)=200 m、(0)=50 m、(0)=90°、(0)=0 m/s、(0)=0 m/s、(0)=0 rad/s；航行器控制器的參數(shù)設(shè)置為=7、=5、=5、=5、=3、=3、=6、=6、=6、=6。在控制系統(tǒng)的動(dòng)力學(xué)模型中，加入10的參數(shù)攝動(dòng)和恒定未知海流，來驗(yàn)證控制系統(tǒng)的魯棒性。恒定未知海流被加入到航行器的動(dòng)力學(xué)模型中，海流初始流速和流向分別為05 m/s和30°。

在PSO算法優(yōu)化過程中，隨著迭代次數(shù)的增加，全局極值不斷減小，最后維持不變。算法找到的適應(yīng)度值最優(yōu)點(diǎn)=75636 1，對(duì)應(yīng)的參數(shù)值=0575 4、=0972 8、=16167 3、=1318 0、=0100 2。圖2為目標(biāo)函數(shù)的適應(yīng)度值變化曲線。

圖2 適應(yīng)度值曲線Fig.2 Fitness value curve

為了驗(yàn)證PSO算法的優(yōu)越性，將基于粒子群的非奇異終端滑模控制(PSO-NTSMC)算法與非奇異終端滑模控制(NTSMC)算法、傳統(tǒng)積分滑模控制(ISMC)算法進(jìn)行對(duì)比。NTSMC算法的參數(shù)為手動(dòng)調(diào)參最優(yōu)值，ISMC算法中定義的滑模面與其控制律如下：

在數(shù)值仿真中，航行器的期望縱向速度被設(shè)置為2 kn(大概為1 m/s)，仿真結(jié)果如圖3所示。從圖3中可以看出NTSMC算法和PSO-NTSMC算法很好地完成了縱向速度跟蹤任務(wù)，使縱向速度無超調(diào)、快速地追蹤期望值，并且PSO-NTSMC算法的縱向速度與NTSMC算法相比更接近期望值。ISMC算法也完成了速度跟蹤任務(wù)，但誤差較大。

圖3 縱向速度響應(yīng)曲線Fig.3 Longitudinal velocity response curve

圖4為路徑跟蹤結(jié)果，從中可以看出，控制器很好地完成了位置跟蹤任務(wù)，并且在左側(cè)放大圖中可以看出，PSO-NTSMC算法在(127.6 m，400.0 m)處完成轉(zhuǎn)彎，NTSMC和ISMC算法分別在(161.4 m，400.0 m)和(175.8 m，400.0 m)處完成轉(zhuǎn)彎。因此PSO-NTSMC算法的效果要優(yōu)于NTSMC和ISMC算法。

圖4 水平面路徑跟蹤效果圖Fig.4 Map of Horizontal path-following effect

圖5給出了路徑跟蹤誤差曲線，航行器誤差在控制器的作用下收斂到0 m。由于靠近航跡點(diǎn)時(shí)需要轉(zhuǎn)彎，誤差迅速增大，但在短時(shí)間內(nèi)重新收斂為0 m。在局部放大圖中可以看出ISMC、NTSMC和PSO-NTSMC算法分別在(595.9 m，0 m)、(598 m，0 m)和(598.8 m，0 m)處誤差增大，并于(688.7 m，-0.02 m)、(672.3 m，-0.02 m)、(658.2 m，-0.02 m)處誤差基本減小為0 m，故PSO-NTSMC控制效果更佳。圖6給出了艏向角及艏向角誤差響應(yīng)曲線。由圖6可知，PSO-NTSMC算法在轉(zhuǎn)彎處期望漂角更小，故期望艏向角更平穩(wěn)；PSO-NTSMC算法的艏向角誤差更小，在橫坐標(biāo)為452 s的轉(zhuǎn)彎處為0.15°；NTSMC和ISMC算法分別為-3.4°和-20.1°。 圖7給出了航行器跟蹤控制器的輸出。由圖7可見：盡管相比NTSMC算法，PSO-NTSMC算法進(jìn)退方向的力有些許抖動(dòng)，但在開始階段NTSMC算法的力為320 N遠(yuǎn)遠(yuǎn)大于PSO-NTSMC算法的160 N。 而ISMC算法開始階段的力較小，但是抖動(dòng)過大。故多方面考慮，PSO-NTSMC算法所需要的能源少、抖動(dòng)小，故控制策略更佳。

圖5 路徑跟蹤誤差曲線Fig.5 Path-following error curve

圖6 艏向角和艏向角誤差響應(yīng)曲線Fig.6 Response curve of yaw angle and yaw angle errors

圖7 控制器輸出Fig.7 Controller output

7 結(jié)論

本文針對(duì)欠驅(qū)動(dòng)UUV水平面直線路徑跟蹤問題，結(jié)合非奇異終端滑模控制和PSO算法提出控制器，克服了恒定未知海流、參數(shù)攝動(dòng)以及控制器參數(shù)不精確的缺點(diǎn)。利用LOS制導(dǎo)法思想和Serret-Frenet坐標(biāo)系結(jié)合，得到跟蹤誤差方程，進(jìn)而將路徑跟蹤問題轉(zhuǎn)化為鎮(zhèn)定縱向速度誤差、艏向角速度誤差和艏向角誤差的問題，設(shè)計(jì)非奇異終端滑模控制器達(dá)到有限時(shí)間內(nèi)完成路徑跟蹤的目的和效果。引入PSO算法，搜索更優(yōu)的控制器參數(shù)，提高路徑跟蹤的精度。最后，仿真結(jié)果表明該控制器可以克服參數(shù)攝動(dòng)和恒定未知海流干擾的影響，精確地執(zhí)行路徑跟蹤任務(wù)。