自主水下航行器自適應(yīng)S 面三維軌跡跟蹤的仿真驗(yàn)證

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

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

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

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

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

0 引 言

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

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

1 AUV 空間運(yùn)動(dòng)模型

圖1 所示為AUV 運(yùn)動(dòng)的慣性坐標(biāo)系（I 坐標(biāo)系）和載體坐標(biāo)系（B 坐標(biāo)系）。I 坐標(biāo)系取地球切平面坐標(biāo)系，原點(diǎn)取地球橢球體表面某固定點(diǎn)，坐標(biāo)軸定義為北東地（NED）。B 坐標(biāo)系原點(diǎn)取為AUV 重心，坐標(biāo)軸指向的定義為前-右 -下。

圖1 AUV 運(yùn)動(dòng)的慣性坐標(biāo)系和載體坐標(biāo)系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分 別 為 縱 向、橫向和垂向位移， φ,θ,ψ分別為橫傾角、縱傾角和航向角，上標(biāo)I表示I坐標(biāo)系）、速度矢量νB=[vB,ω]T（其中，線速度vB=[u,v,w]T，u為縱向速度，v為橫向速度，w為垂向速度，上標(biāo)B表示B 坐標(biāo)系）、角速度ω=[p,q,r]T（其中，p為橫傾角速度，q為縱傾角速度，r為轉(zhuǎn)艏角速度），建立式（1）和式（2）所示AUV 六自由度空間運(yùn)動(dòng)模型[8]。

運(yùn)動(dòng)學(xué)方程：

動(dòng)力學(xué)方程：

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

B 坐標(biāo)系下的海流速度為

B 坐標(biāo)系下的AUV 相對(duì)速度為

則在有海流干擾的情況下AUV 動(dòng)力學(xué)方程為

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

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

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

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

式中：

其中：

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

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

如圖2 所示，PL為視線速度坐標(biāo)（L 坐標(biāo)系）導(dǎo)引的前視點(diǎn)，視線速度UL由AUV 實(shí)時(shí)位置點(diǎn)B指向PL。 在B點(diǎn)處，將P 坐標(biāo)系繞ZP軸旋轉(zhuǎn)方位導(dǎo)引角 χL，旋轉(zhuǎn)矩陣為RZP(χL)， 再繞YP軸旋轉(zhuǎn)俯仰導(dǎo)引角 γL， 旋轉(zhuǎn)矩陣為RYP(γL)，得到視線速度坐標(biāo)系，其原點(diǎn)與AUV 固聯(lián)，其XL軸 沿UL方向，P 坐標(biāo)系到L 坐標(biāo)系旋轉(zhuǎn)的坐標(biāo)轉(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 坐標(biāo)系的X-Z平面和X-Y平面。在實(shí)際導(dǎo)引過程中，希望能夠在軌跡偏差較大時(shí)，AUV 可以迅速靠近期望的軌跡，此時(shí)前視距離需要得較小；在軌跡偏差變得較小時(shí)，AUV 能緩慢平滑地接近期望的軌跡，此時(shí)前視距離需要得較大?；诖藰?biāo)準(zhǔn)，設(shè)計(jì)如式（18）所示的變前視距離。

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

對(duì)式(20)進(jìn)行時(shí)間求導(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 坐標(biāo)系繞ZI軸 旋轉(zhuǎn)期望的跡向角 χd，旋轉(zhuǎn)矩陣為RZI(χd) ，再繞YI軸 旋轉(zhuǎn)期望的潛浮角 γd，則旋轉(zhuǎn)矩陣為RYI(γd)，并得到L 系坐標(biāo)。從I 坐標(biāo)系到L 坐標(biāo)系的旋轉(zhuǎn)矩陣表示為

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

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

3 控制器設(shè)計(jì)

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

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

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

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

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

對(duì)式(33) 中的kpu，kdu進(jìn)行泰勒展開，忽略高階項(xiàng)，并記為

則有

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

式中： λpu和 λdu為學(xué)習(xí)率， 0 ＜λpu＜1,0 ＜λdu＜1，則有

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

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

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

4 仿真驗(yàn)證

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

4.1 控制算法的驗(yàn)證

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

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

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

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

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

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

圖3 較理想初始控制參數(shù)下航向控制對(duì)比（工況 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 面控制對(duì)速度控制效果沒有明顯提升；在50 s 時(shí)刻加入海流干擾后，相比PID 控制器和S 面控制器，自適應(yīng)S 面控制器更早調(diào)整到了期望值，且振蕩次數(shù)更少。圖6 反映了此時(shí)其在縱向速度控制過程中控制參數(shù)的變化。

圖5 縱向速度控制對(duì)比（工況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 時(shí)刻加入海流干擾后，相比PID 控制器和傳統(tǒng)的S 面控制，自適應(yīng)S 面控制能更早地調(diào)整到期望值，且振蕩次數(shù)更少。圖8 反映了此時(shí)其在縱傾控制過程中控制參數(shù)的變化。

圖7 縱傾控制對(duì)比（工況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 面控制器對(duì)海流干擾具有更強(qiáng)的魯棒性。

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

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

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

圖9 初始控制參數(shù)變化下航向控制對(duì)比（工況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 縱向速度控制對(duì)比（工況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 縱傾控制對(duì)比（工況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ù)會(huì)自適應(yīng)調(diào)節(jié)，最終使AUV航向能穩(wěn)定在45。。

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

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

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

直線參考軌跡為

式中，t表示時(shí)間，s。

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

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

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

曲線參考軌跡為

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

圖20 不同導(dǎo)引方法下曲線軌跡跟蹤對(duì)比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 具有一定的空間軌跡跟蹤能力，對(duì)海流干擾有一定的魯棒性，跟蹤收斂速度也比傳統(tǒng)LOS 導(dǎo)引法更快。

5 結(jié) 語

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

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