自主水下航行器自適應S 面三維軌跡跟蹤的仿真驗證

李文魁，周鑄*,2，宦愛奇，夏宇軒

1 海軍工程大學 電氣工程學院，湖北 武漢 430033

2 中國人民解放軍 92768 部隊，廣東 汕頭 515000

3 中國人民解放軍 91206 部隊，山東 青島 266000

4 海軍工程大學 教務處，湖北 武漢 430033

0 引 言

自主水下航行器（AUV）因體積小、成本低、能夠完成多樣化任務等特點，成為了海洋探索和海底作業的重要載體，而良好的航跡跟蹤性能是AUV 進行海底作業的重要保證。目前，航跡跟蹤方式主要包括路徑跟隨和軌跡跟蹤。前者只需考慮在空間維度上沿預定路徑航行，后者則需在預定時間到達預定位置，同時滿足空間和時間維度上的跟蹤要求。Breivik 等[1]通過引入導引角的概念定義新的導引律，使導引律不受AUV 動力學的改變而改變，從而提高了通用性。陳東等[2]在文獻[1]的基礎上，利用反歩法，設計了一種基于AUV模型的非線性控制器，較好地實現了軌跡跟蹤，但控制算法對模型參數準確度要求較高。孫巧梅等[3]基于反演法和變結構模糊控制律設計了一種AUV 航跡跟蹤控制律，能較好地抑制外界干擾，實現了軌跡平穩跟蹤，但算法的模糊控制規則設置較為困難。劉學敏等[4]借鑒PID 控制器的結構，使用Sigmoid 函數設計了一種S 面控制器，其結構簡單且具有類似于模糊控制的智能性，但控制參數整定較為困難。Sahu 等[5]設計了一種靜態輸出反饋控制器，用于控制水下機器人沿著期望的軌跡運動。Oh 等[6]使用視線（line-of-sight, LOS）導引法進行導引，針對欠驅動的AUV，設計了一種狀態空間形式的模型預測控制器，獲得了比PID 控制算法更優的效果。劉麗萍等[7]通過在控制器中加入自適應律，設計了一種自適應反演滑模軌跡跟蹤控制器，該控制器能夠較好地處理水動力參數不確定和海流干擾的問題，但設計的海流觀測器在工程上較難實現。

為滿足AUV 在空間和時間維度上的跟蹤需求，本文將AUV 的軌跡跟蹤問題簡化為與時間相關的參數化軌跡參考點的方式；然后，使用文獻[1]中的LOS 導引律，將AUV 對參考點的跟蹤轉化為對指令航向、指令縱傾和指令速度的跟蹤，并改進傳統的LOS 導引律，設計三維軌跡變前視距離的LOS 導引律； 其后，在實現對航向、縱傾和速度的控制后，基于文獻[4]，設計具有控制參數自調整能力的自適應S 面控制器，以減小控制參數整定難度和增強抗干擾能力； 最后，通過數值仿真驗證所提算法的有效性。

1 AUV 空間運動模型

圖1 所示為AUV 運動的慣性坐標系（I 坐標系）和載體坐標系（B 坐標系）。I 坐標系取地球切平面坐標系，原點取地球橢球體表面某固定點，坐標軸定義為北東地（NED）。B 坐標系原點取為AUV 重心，坐標軸指向的定義為前-右 -下。

圖1 AUV 運動的慣性坐標系和載體坐標系Fig. 1 Inertial frame and body frame of AUV motion

令AUV 位置姿態矢量 ηI=[pI,Θ]T（其中，位置pI=[x,y,z]T，姿 態 Θ=[φ,θ,ψ]T，x,y,z分 別 為 縱 向、橫向和垂向位移， φ,θ,ψ分別為橫傾角、縱傾角和航向角，上標I表示I坐標系）、速度矢量νB=[vB,ω]T（其中，線速度vB=[u,v,w]T，u為縱向速度，v為橫向速度，w為垂向速度，上標B表示B 坐標系）、角速度ω=[p,q,r]T（其中，p為橫傾角速度，q為縱傾角速度，r為轉艏角速度），建立式（1）和式（2）所示AUV 六自由度空間運動模型[8]。

運動學方程：

動力學方程：

AUV 在水下航行時會受到海流干擾，因此，假設AUV 僅受到常值海流干擾且 I坐標系下的海流速度為

B 坐標系下的海流速度為

B 坐標系下的AUV 相對速度為

則在有海流干擾的情況下AUV 動力學方程為

2 AUV 三維導引律

2.1 從I 系到參考軌跡系（P 系）的坐標轉換

建立參考軌跡坐標系（P 坐標系），其原點為P，XP軸 沿參考軌跡的切線方向， χP和 γP為軌跡方位角和軌跡升角，分別表示為：

將I 坐 標 系 繞ZI軸 旋 轉 角 χP，旋 轉 矩 陣 為RZI(χP)， 再繞YI軸 旋轉角 γP， 旋轉矩陣為RYI(γP)，得到參考軌跡坐標系，對應的旋轉矩陣為

式中：

其中：

式中，S為反對稱矩陣，ST=-S。 設vP為P點在參考軌跡坐標系下表示的速度矢量vP=[UP,0,0]T，則有

2.2 從P 系到視線速度坐標（L 系）的坐標轉換

如圖2 所示，PL為視線速度坐標（L 坐標系）導引的前視點，視線速度UL由AUV 實時位置點B指向PL。 在B點處，將P 坐標系繞ZP軸旋轉方位導引角 χL，旋轉矩陣為RZP(χL)， 再繞YP軸旋轉俯仰導引角 γL， 旋轉矩陣為RYP(γL)，得到視線速度坐標系，其原點與AUV 固聯，其XL軸 沿UL方向，P 坐標系到L 坐標系旋轉的坐標轉換矩陣為

圖2 軌跡跟蹤示意圖Fig. 2 Trajectory tracking diagram

2.3 軌跡導引律

旋轉方位導引角 χL和俯仰導引角 γL可由如下LOS 法[9]得到。

式中，前 視 距 離 Δey和 Δez分 別 用 于 將AUV 導 引 到P 坐標系的X-Z平面和X-Y平面。在實際導引過程中，希望能夠在軌跡偏差較大時，AUV 可以迅速靠近期望的軌跡，此時前視距離需要得較??；在軌跡偏差變得較小時，AUV 能緩慢平滑地接近期望的軌跡，此時前視距離需要得較大?；诖藰藴?，設計如式（18）所示的變前視距離。

基于軌跡跟蹤誤差 εp，設計如式（20）所示的李雅普諾夫穩定函數。

對式(20)進行時間求導，因 (εP)TSTεP=0，故可得

展開式（21），可得

基于李雅普諾夫穩定性判據，令

式中， κ為比例系數。

將式(18)和式(23) 代入式(22)，可得到導引算法的李雅普諾夫穩定性判據如下：

由式(23) 可得

并且，得到期望的縱向速度為

此外，由式(17)可得 -π/2 ＜χL＜π/2，-π/2 ＜γL＜π/2 ，故 c osχLcosγL≠0，則式(25)為非奇異。

將I 坐標系繞ZI軸 旋轉期望的跡向角 χd，旋轉矩陣為RZI(χd) ，再繞YI軸 旋轉期望的潛浮角 γd，則旋轉矩陣為RYI(γd)，并得到L 系坐標。從I 坐標系到L 坐標系的旋轉矩陣表示為

聯立式(16)和式(27)，得到期望的跡向角χd和期望的潛浮角 γd分別如式（28）所示。

考慮到式(28)的奇異性問題，在實際計算中，可使用函數atan2 替代函數arctan 來解決式(28)的奇異值問題。另外，由于AUV 航行中會產生漂角和攻角，期望的航向角 ψd和期望的縱傾角θd可由下式計算。

3 控制器設計

S 面控制器將模糊思想與PID 控制相結合，是一種結構簡單、魯棒性能好的非線性控制器[10]。

將運動參數誤差量及其變化率歸一化，可得

式 中，kpi，kdi(i=u,θ,ψ)為 控 制 器 參 數（下 標 p 和d 分別表示PID 控制器中的比例和微分），則控制器輸出的驅動力為

這里，以 τpr為 例，設計自適應學習算法對kpi，kdi進行調節，使用梯度下降法[11]修正控制器參數，設計代價函數G為

式中， τprd為 期望的輸出推力，由ud經AUV 模型推算獲得。

對式(33) 中的kpu，kdu進行泰勒展開，忽略高階項，并記為

則有

為保證 Δkpu?Gpu＜0， Δkdu?Gdu＜0，取

式中： λpu和 λdu為學習率， 0 ＜λpu＜1,0 ＜λdu＜1，則有

代價函數G可迭代收斂至0。聯立式（31）、式（33）和式（35），可得

故m+1 時刻的控制器參數可按下式遞推得到。

此外，對 τδs， τδr的 控制參數kpθ，kdθ，kpψ，kdψ也分別按此法進行修正。

4 仿真驗證

本節以REMUS 100 AUV[12]為例，在Matlab 軟件中分別對控制算法和導引律進行仿真驗證。AUV 的初始狀態（含位置、姿態、速度及角速度）為：

4.1 控制算法的驗證

分別對控制器控制進行仿真，將PID 控制和傳統的S 面控制方法與采用自適應S 面控制方法的結果進行對比。設置指令的航向角 ψ 、縱傾角θ和 縱 向 速 度u分 別 為 ψc=45°， θc=5°，uc=2 m/s，在50 s 時刻加入海流干擾，海流速度為

1） 由試湊法得到如下較理想的控制參數。

S 面控制器控制參數：

PID 控制器控制參數：

令自適應S 面控制器控制參數與傳統S 面控制器的相同。經仿真，可得在較理想的初始控制參數情況下（工況1）的航向角 ψ、縱向速度u和縱傾角 θ對比圖及自適應S 面控制器控制參數變化圖（圖3～圖8）。

由圖3 可見，在未加入干擾及較為理想的初始控制參數下，因PID 控制和傳統的S 面控制方法航向控制效果已較理想，自適應S 面控制對航向控制效果無明顯提升； 在50 s 時刻加入海流干擾后，相比PID 控制器和傳統的S 面控制器，自適應S 面控制器更早地調整到了期望值，且振蕩次數更少。圖4 反映了此時其在航向控制過程中控制參數的變化。

圖3 較理想初始控制參數下航向控制對比（工況 1）Fig. 3 Comparison of yaw control under desirable initial parameters (Case 1)

圖4 航向自適應S 面控制器參數變化（工況1）Fig. 4 Variation of yaw for adaptive S-plane controller (Case 1)

由圖5 可見，自適應S 面控制對速度控制效果沒有明顯提升；在50 s 時刻加入海流干擾后，相比PID 控制器和S 面控制器，自適應S 面控制器更早調整到了期望值，且振蕩次數更少。圖6 反映了此時其在縱向速度控制過程中控制參數的變化。

圖5 縱向速度控制對比（工況1）Fig. 5 Longitudinal velocity control contrast (Case 1)

圖6 縱向速度自適應S 面控制器參數變化（工況1）Fig. 6 Variation of longitudinal velocity for adaptive S-plane controller (Case 1)

由圖7 可見，相比PID 控制，傳統的S 面控制和自適應S 面控制均能更快地跟蹤上指令的縱傾角，在50 s 時刻加入海流干擾后，相比PID 控制器和傳統的S 面控制，自適應S 面控制能更早地調整到期望值，且振蕩次數更少。圖8 反映了此時其在縱傾控制過程中控制參數的變化。

圖7 縱傾控制對比（工況1）Fig. 7 Trim control contrast (Case 1)

圖8 縱傾自適應S 面控制器參數變化（工況1）Fig. 8 Variation of trim for adaptive S-plane controller (Case 1)

綜上，自適應S 面控制器相比PID 控制器和傳統S 面控制器對海流干擾具有更強的魯棒性。

2） 改變控制參數，令傳統的S 面控制器和自適應S 面控制器控制參數均為

PID 控制器控制參數為

經仿真，可得到在初始控制參數發生變化（不理想）的情況（工況2）下航向角 ψ、縱向速度u和縱傾角 θ對比圖以及自適應S 面控制器控制參數變化圖（圖9～圖14）。

圖9 初始控制參數變化下航向控制對比（工況2）Fig. 9 Comparison of yaw control under undesirable initial parameters (Case 2)

圖10 航向自適應S 面控制器參數變化（工況2）Fig. 10 Variation of yaw for adaptive S-plane controller (Case 2)

圖11 縱向速度控制對比（工況2）Fig. 11 Longitudinal velocity control contrast (Case 2)

圖12 縱向速度自適應S 面控制參數變化（工況2）Fig. 12 Variation of longitudinal velocity for adaptive S-plane controller (Case 2)

圖13 縱傾控制對比（工況2）Fig. 13 Trim control contrast (Case 2)

圖14 縱傾自適應S 面控制器參數變化（工況2）Fig. 14 Variation of trim for adaptive S-plane controller (Case 2)

由圖9 和圖10 可見，在改變控制參數后，PID 控制器和傳統的S 面控制器無法穩定AUV的航向，且始終在45。上下振蕩，而自適應S 面控制下的AUV 其控制參數會自適應調節，最終使AUV航向能穩定在45。。

圖11～圖14 也具有如圖9 和圖10 相同的現象?？梢?，在控制參數產生較大變化的情況下，PID 控制和傳統的S 面控制方法已經無法實現對AUV 的穩定控制，但自適應S 面控制方法則能夠通過改變控制參數的大小來實現對AUV 的航向角、縱向速度、縱傾角的控制。對于自適應S 面控制而言，在控制參數整定時，不需要反復試湊來尋找最佳參數，而只需找到一定范圍內的參數，即可實現對AUV 的控制，并減小了參數整定的難度。

4.2 導引律的驗證

在采用自適應S 面控制控制方法的基礎上，為驗證變前視距離LOS 法的優點，增加LOS[9]導引方法與之對比。為此，在150 s 時刻加入海流干擾，海流速度=[0.5,0.2,0,0,0,0]T，分別進行直線和曲線軌跡跟蹤仿真。

直線參考軌跡為

式中，t表示時間，s。

通過直線軌跡跟蹤仿真，得到上述兩種導引方法下的軌跡對比圖（圖15）和軌跡跟蹤誤差對比圖（圖16），以及變前視距離LOS 法下的航向角ψ、縱傾角 θ 和縱向速度u跟蹤圖（圖17～圖19）。

圖15 不同導引法下直線軌跡跟蹤對比Fig. 15 Comparison of straight line trajectory tracking under different guidance laws

圖16 直線軌跡跟蹤誤差Fig. 16 Erros of straight line trajectory tracking

圖17 直線軌跡跟蹤的航向Fig. 17 Yaw of straight line trajectory tracking

圖18 直線軌跡跟蹤的縱傾Fig. 18 Trim of straight line trajectory tracking

圖19 直線軌跡跟蹤的縱向速度Fig. 19 Longitudinal velocity of straight line trajectory tracking

由圖15 和圖16 可見，在直線軌跡跟蹤情況下，變前視距離和固定前視距離LOS 兩種導引法均能使AUV 跟蹤上參考軌跡，但變前視距離LOS 法的軌跡跟蹤誤差能更快收斂到0 附近，其跟蹤速度更快；在150 s 時刻加入海流干擾后，水平和垂直軌跡誤差均能在一定時間內收斂到0 附近，AUV 在偏離預定軌道后能快速回到參考軌跡，說明導引律具有一定的魯棒性。

結合圖15～圖19 可見，在AUV 偏離參考軌跡時，變前視距離LOS 導引法會不斷調整指令的航向角 ψ、縱傾角 θ 和 縱向速度u，以使AUV 跟蹤上參考軌跡。而且，軌跡跟蹤誤差越大，指令的航向角、縱傾角和縱向速度的變化越大，且實際的航向角、縱傾角和縱向速度經短暫調整后可跟蹤上指令信號。

曲線參考軌跡為

通過曲線軌跡跟蹤仿真，得到兩種導引方法下的軌跡對比圖（圖20）和軌跡跟蹤誤差對比圖（圖21） 及變前視距離LOS 導引法下的航向角、縱傾角和縱向速度跟蹤圖（圖22～圖24）。

圖20 不同導引方法下曲線軌跡跟蹤對比Fig. 20 Comprison of curve trajectory tracking under differentguidance laws

圖21 曲線軌跡跟蹤誤差Fig. 21 Errors of curve trajectory tracking

圖22 曲線軌跡跟蹤的航向Fig. 22 Yaw of curve trajectory tracking

圖23 曲線軌跡跟蹤的縱傾Fig. 23 Trim of curve trajectory tracking

圖24 曲線軌跡跟蹤的縱向速度Fig. 24 Longitudinal velocity of curve trajectory tracking

結合圖20～圖24 可見，在曲線軌跡跟蹤情況下能夠得出與直線軌跡跟蹤相同的結論。由此可知，變前視距離LOS 導引法下的AUV 具有一定的空間軌跡跟蹤能力，對海流干擾有一定的魯棒性，跟蹤收斂速度也比傳統LOS 導引法更快。

5 結 語

本文通過分析AUV 的運動學和動力學特性，構建了六自由度AUV 軌跡跟蹤數學模型，采用設計的三維軌跡變前視距離LOS 導引律和自適應S 面控制器控制。以REMUS 100 AUV 為對象進行仿真分析，驗證了所設計的導引律的有效性。結果證明，本文所設計的軌跡跟蹤控制算法具備較好的直線和曲線軌跡跟蹤特性，對海流干擾也具有一定的魯棒性，且設計的自適應S 面控制器能夠降低控制參數整定的難度，增強AUV的抗干擾能力。

然而，本文設計的控制算法僅考慮了常值海流干擾下的AUV 的軌跡跟蹤問題，未來的研究工作將考慮變海流干擾因素，并采用更加智能的控制算法。