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

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

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

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

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

0 引 言

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

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

1 AUV 空間運動模型

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

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

令A(yù)UV 位置姿態(tài)矢量 ηI=[pI,Θ]T（其中，位置pI=[x,y,z]T，姿 態(tài) Θ=[φ,θ,ψ]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為轉(zhuǎn)艏角速度），建立式（1）和式（2）所示AUV 六自由度空間運動模型[8]。

運動學方程：

動力學方程：

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

B 坐標系下的海流速度為

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

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

2 AUV 三維導(dǎo)引律

2.1 從I 系到參考軌跡系（P 系）的坐標轉(zhuǎn)換

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

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

式中：

其中：

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

2.2 從P 系到視線速度坐標（L 系）的坐標轉(zhuǎn)換

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

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

2.3 軌跡導(dǎo)引律

旋轉(zhuǎn)方位導(dǎo)引角 χL和俯仰導(dǎo)引角 γL可由如下LOS 法[9]得到。

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

基于軌跡跟蹤誤差 εp，設(shè)計如式（20）所示的李雅普諾夫穩(wěn)定函數(shù)。

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

展開式（21），可得

基于李雅普諾夫穩(wěn)定性判據(jù)，令

式中， κ為比例系數(shù)。

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

由式(23) 可得

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

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

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

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

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

3 控制器設(shè)計

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

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

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

這里，以 τpr為 例，設(shè)計自適應(yīng)學習算法對kpi，kdi進行調(diào)節(jié)，使用梯度下降法[11]修正控制器參數(shù)，設(shè)計代價函數(shù)G為

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

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

則有

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

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

代價函數(shù)G可迭代收斂至0。聯(lián)立式（31）、式（33）和式（35），可得

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

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

4 仿真驗證

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

4.1 控制算法的驗證

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

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

S 面控制器控制參數(shù)：

PID 控制器控制參數(shù)：

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

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

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

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

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

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

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

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

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

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

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

2） 改變控制參數(shù)，令傳統(tǒng)的S 面控制器和自適應(yīng)S 面控制器控制參數(shù)均為

PID 控制器控制參數(shù)為

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

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

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

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

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

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

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

由圖9 和圖10 可見，在改變控制參數(shù)后，PID 控制器和傳統(tǒng)的S 面控制器無法穩(wěn)定AUV的航向，且始終在45。上下振蕩，而自適應(yīng)S 面控制下的AUV 其控制參數(shù)會自適應(yīng)調(diào)節(jié)，最終使AUV航向能穩(wěn)定在45。。

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

4.2 導(dǎo)引律的驗證

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

直線參考軌跡為

式中，t表示時間，s。

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

圖15 不同導(dǎo)引法下直線軌跡跟蹤對比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 兩種導(dǎo)引法均能使AUV 跟蹤上參考軌跡，但變前視距離LOS 法的軌跡跟蹤誤差能更快收斂到0 附近，其跟蹤速度更快；在150 s 時刻加入海流干擾后，水平和垂直軌跡誤差均能在一定時間內(nèi)收斂到0 附近，AUV 在偏離預(yù)定軌道后能快速回到參考軌跡，說明導(dǎo)引律具有一定的魯棒性。

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

曲線參考軌跡為

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

圖20 不同導(dǎo)引方法下曲線軌跡跟蹤對比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

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

5 結(jié) 語

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

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