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

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

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

0 引言

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

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

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

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

1 欠驅(qū)動UUV運動模型

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

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

因此，UUV運動學(xué)模型被描述為

(1)

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

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

(2)

式中：

(3)

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

UUV在開闊海域時，可以看作沿兩個航跡點之間做直線運動。定義一系列離散航跡點{,,…,,…,}，那么UUV就依次沿著相鄰目標點做直線運動。這些離散航跡點的位置坐標通?？梢员硎緸閇,]。相鄰航跡點(,+1)之間的直線段表示為，而航跡點為由航跡線-1到航跡線的轉(zhuǎn)向點。在直線段內(nèi)，UUV的期望航跡可以被描述為

(4)

(5)

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

(6)

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

(7)

(8)

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

=+Δ

(9)

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

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

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

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

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

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

()=++()

(10)

(11)

(12)

(13)

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

(14)

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

(15)

由有限時間定理可知，存在一個時間1，當時間>1時滑模面收斂至0 rad/s，即=0 rad/s。

由=0 rad/s可得

(16)

(16)式等號兩邊對時間積分，可得

(17)

解上述不等式，可得

(18)

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

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

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

(19)

(20)

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

()=++()

(21)

(22)

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

(23)

式中：

(24)

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

(25)

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

(26)

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

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

4.1 PSO算法

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

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

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

(27)

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

(28)

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

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

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

(29)

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

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

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

5 穩(wěn)定性分析

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

(30)

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

(31)

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

=+++

(32)

對進行求導(dǎo)，可得

(33)

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

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

航行器水平面期望路徑的航跡點分布為(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)的動力學(xué)模型中，加入10的參數(shù)攝動和恒定未知海流，來驗證控制系統(tǒng)的魯棒性。恒定未知海流被加入到航行器的動力學(xué)模型中，海流初始流速和流向分別為05 m/s和30°。

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

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

為了驗證PSO算法的優(yōu)越性，將基于粒子群的非奇異終端滑?？刂?PSO-NTSMC)算法與非奇異終端滑模控制(NTSMC)算法、傳統(tǒng)積分滑?？刂?ISMC)算法進行對比。NTSMC算法的參數(shù)為手動調(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。由于靠近航跡點時需要轉(zhuǎn)彎，誤差迅速增大，但在短時間內(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算法的艏向角誤差更小，在橫坐標為452 s的轉(zhuǎn)彎處為0.15°；NTSMC和ISMC算法分別為-3.4°和-20.1°。 圖7給出了航行器跟蹤控制器的輸出。由圖7可見：盡管相比NTSMC算法，PSO-NTSMC算法進退方向的力有些許抖動，但在開始階段NTSMC算法的力為320 N遠遠大于PSO-NTSMC算法的160 N。 而ISMC算法開始階段的力較小，但是抖動過大。故多方面考慮，PSO-NTSMC算法所需要的能源少、抖動小，故控制策略更佳。

圖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é)論

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