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

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

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

0 引言

水下無人航行器(UUV)路徑跟蹤是近年來的熱點問題。路徑跟蹤控制不受時間約束，只要求欠驅動UUV收斂至期望路徑，而對于何時到達何地沒有嚴格要求。通過引入“虛擬向導”跟蹤制導策略，設計虛擬向導切向速度，并且建立了欠驅動UUV與期望路徑參考點之間的誤差方程。由于其本身的欠驅動性和非完整性約束，欠驅動UUV路徑跟蹤問題的研究一直是當前的熱點與難點。Lapierre等針對模型不確定性約束下的欠驅動UUV水平面路徑跟蹤控制問題，采用反步法和李雅普諾夫直接法設計一種魯棒非線性控制器，提高了模型不確定性約束下系統的魯棒性，實現了欠驅動UUV的水平面路徑跟蹤控制。Moe等研究未知洋流情況下的UUV二維路徑跟蹤運動，提出了制導律和自適應反饋線性化方法與滑模控制方法相結合組成的閉環系統，完成了路徑跟蹤控制。苗建明等提出一種基于改進反步法的三維空間曲線路徑跟蹤控制器，解決了運動學控制器存在非因果現象的問題和動力學控制器過于復雜的問題。Habib等針對由海浪和海流擾動引起的時變干擾約束下的欠驅動UUV路徑跟蹤控制，提出一種改進的自抗擾控制器。張偉等針對UUV垂直面路徑跟蹤提出一種基于非線性連續模型預測控制算法的控制器，實現了欠驅動UUV路徑跟蹤控制。Guo等針對參數攝動約束下欠驅動UUV的三維直線路徑跟蹤控制問題，提出了一種固定時間收斂的非奇異快速終端滑模三維直線路徑跟蹤控制方法。

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

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

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

1 欠驅動UUV運動模型

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

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

因此，UUV運動學模型被描述為

(1)

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

UUV動力學模型被描述為

(2)

式中：

(3)

2 欠驅動UUV路徑跟蹤制導律設計

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

(4)

(5)

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

(6)

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

(7)

(8)

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

=+Δ

(9)

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

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

3 欠驅動UUV路徑跟蹤控制器設計

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

3.1 縱向速度控制器設計

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

()=++()

(10)

(11)

(12)

(13)

選取如下李雅普諾夫函數：

(14)

對求1階導數，得

(15)

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

由=0 rad/s可得

(16)

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

(17)

解上述不等式，可得

(18)

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

3.2 轉艏控制器設計

由第2節可知，UUV的水平面位置跟蹤問題已經轉化為艏向角誤差的問題，定義=-。那么首先構造李雅普諾夫函數：

(19)

(20)

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

()=++()

(21)

(22)

與31節同理，轉艏控制律為

(23)

式中：

(24)

選取如下李雅普諾夫函數：

(25)

對求1階導數，得

(26)

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

4 欠驅動UUV路徑跟蹤控制器參數優化

4.1 PSO算法

本文采用PSO算法提升控制器的性能，利用PSO算法迭代尋優的特性，找到定義適應度函數的更優解。

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

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

(27)

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

(28)

式中：=1,2,…,，為粒子個數；=1,2,…,，為最大迭代次數；和為[0,1]范圍內的均勻隨機數；和為學習因子；為慣性權重；為粒子的位置；為粒子的速度；()表示第個粒子在第次迭代時的參數；()表示所有粒子在第次迭代時的最優參數。

4.2 控制律參數優化方法

如32節所述，欠驅動UUV水平面路徑跟蹤問題實際上已經轉化為縱向速度誤差、轉艏角速度誤差和艏向角誤差的鎮定問題。適應度函數選取IAT指標，其定義為

(29)

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

隨著迭代次數的增加，適應度值不斷減小，最后得到最小值點，該點值即為最優解。最優解對應的參數值即為最優參數值。

表1 參數搜索范圍

5 穩定性分析

構造李雅普諾夫函數

(30)

對(28)式求導，可得

(31)

為了分析欠驅動UUV水平面路徑跟蹤控制系統穩定性，選取李雅普諾夫函數：

=+++

(32)

對進行求導，可得

(33)

6 系統仿真及結果

為了驗證非奇異終端滑模控制器的控制效果以及PSO算法的優化效果，在MATLAB軟件中進行仿真實驗。選取粒子種群規模=40，粒子最大迭代次數=40，學習因子=15、=20；該型欠驅動UUV的相關運動參數詳見文獻[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)。

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

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

圖2 適應度值曲線Fig.2 Fitness value curve

為了驗證PSO算法的優越性，將基于粒子群的非奇異終端滑模控制(PSO-NTSMC)算法與非奇異終端滑模控制(NTSMC)算法、傳統積分滑模控制(ISMC)算法進行對比。NTSMC算法的參數為手動調參最優值，ISMC算法中定義的滑模面與其控制律如下：

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

圖3 縱向速度響應曲線Fig.3 Longitudinal velocity response curve

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

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

圖5給出了路徑跟蹤誤差曲線，航行器誤差在控制器的作用下收斂到0 m。由于靠近航跡點時需要轉彎，誤差迅速增大，但在短時間內重新收斂為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給出了艏向角及艏向角誤差響應曲線。由圖6可知，PSO-NTSMC算法在轉彎處期望漂角更小，故期望艏向角更平穩；PSO-NTSMC算法的艏向角誤差更小，在橫坐標為452 s的轉彎處為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 艏向角和艏向角誤差響應曲線Fig.6 Response curve of yaw angle and yaw angle errors

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

7 結論

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