基于改進反步法的AUV直線路徑跟隨*

李 旻 周 鑄 呂志彪 夏宇軒 宦愛奇

（1.海軍裝備部駐武漢地區第二軍事代表室 武漢 430000）（2.海軍工程大學 武漢 430000）（3.中國人民解放軍92768部隊 汕頭 515000）（4.中國人民解放軍91206部隊 青島 266000）

1 引言

無人水下航行器（Autonomous Underwater Vehicle，AUV）以其體積小、成本低、能完成多樣化任務等特點，在科學研究、商業開發和軍事應用中都具有重要的作用，這些應用包括近海油田勘探、海洋測繪、水下結構檢查、反潛戰、海洋調查等［1～4］。而良好的操縱控制能力是AUV 進行海底作業的重要保證。Kokotovic 和Kanellakopoulos［5］等于1991年提出了反步法（backstepping）控制方法，它能有效處理非線性控制問題。1995年，Krstic［6］等對該方法進行系統地整理和歸納，建立了反步設計的基本框架。LaPierre 和Jouvencel［7］設計了一種運動學控制器，用于處理基于反步法和李雅普諾夫理論的動力學問題。Bouadi 等［8］在反步法中加入動態面控制技術，將反步法中微分運算轉化為簡單的代數運算，避免了“微分爆炸”問題并且降低了算法的復雜性。Xu 等［9］使用二階濾波器替代微分，在避免“微分爆炸”的同時，克服了動態面代替微分時噪聲信號放大的問題。針對AUV 的路徑跟隨和軌跡跟蹤問題，馬艷彤等［10］提出一種不依賴模型的改進型PID 控制算法，，通過對增設的左右水平推進器進行控制，實現了AUV 低速時水平面的精確航行，von Ellenrieder 等［11～12］提出了自適應切換監督控制與基于非線性李雅普諾夫的跟蹤控制律相結合的方法，用來解決欠驅動AUV 在可能存在較大建模參數不確定性的情況下的路徑跟隨和軌跡跟蹤問題。

本文在文獻［7］的基礎上，使用二階濾波器替代直接微分項，設計海流觀測器估計未知海流干擾，同時設計改進反步控制器解決AUV 強非線性問題，實現AUV 的速度、航向和縱傾控制，最后采用變前視距離LOS 法設計了AUV 直線路徑跟隨算法。

2 AUV空間運動模型

本論文采用國際水池會議（ITTC）推薦及造船與輪機工程學會（SNAME）術語公報的體系［13］。AUV 運動的慣性坐標系（I系）和載體坐標系（B 系）如圖1 所示。I 系坐標系取地球切平面坐標系，原點取地球橢球體表面某固定點，坐標軸定義為北東地（NED）。B系原點取為AUV浮心，坐標軸指向定義為前-右-下。

圖1 慣性坐標系和載體坐標系

1）運動學方程

AUV 的運動學方程不涉及對象的受力分析，主要描述載體位置和姿態隨時間變化的規律，AUV的運動學方程為

2）動力學方程

AUV與海流的相對速度為

其中，為載體系下的海流速度，則海流干擾下AUV動力學方程為［14］

其中，M為慣性及附加質量陣，CRB(v)為剛體科里奧利力陣，CA(v)為附加質量科里奧利力陣，D(v)為阻力陣，g(η)為恢復力陣，τ為驅動力矢量。

3 改進反步控制器設計

3.1 控制器設計

本文基于李亞普洛夫第二法，采用反步法設計AUV 的速度/航向/縱搖/橫搖控制器。令控制力

其中eu、eφ、eθ、eψ均為控制變量，eu=ud-u，eφ=φd-φ，eθ=θd-θ，eψ=ψd-ψ，Kc為控制系數矩 陣，且Kc∈R4×4，Kc＞0。設控制誤差變量為

其中，α為虛擬控制量，由式（2）可得：

其中：

其中：

選取虛擬控制量αp為

式（13）代入式（12）可得：

其中：

將式（7）代入動力學模型式（4），可得：

選取李雅普諾夫函數V2為

對式（18）求導可得：

令：

設計六自由度上的控制力τ為

顯然0，由李雅普諾夫第二法可知，控制系統穩定。縱向、橫搖、縱搖和轉艏維度上的控制力為

3.2 濾波器設計

上述設計的反步法控制算法包含對虛擬信號的導數，為避免直接對信號解析求導導致“微分爆炸”問題，本文使用二階濾波器代替直接微分，定義如下濾波器：

其中，xc0為輸入信號，xc為輸出信號，q1，q2為中間變量，ζ和ωf為濾波器參數，由上式可得xc0到xc的傳遞函數為

式中，ωf為自然頻率，ζ為阻尼。濾波器的結構如圖2所示。

圖2 濾波器結構圖

由式（25）可知是通過積分而非微分得到的，從而可以較大的減少噪聲的影響。

3.3 抗積分飽和設計

考慮到積分項可能出現的積分飽和問題，使用反計算的方法對控制回路進行抗積分飽和設計［15］，這一方法的優點是，當系統發生飽和溢出時抗積分飽和項對系統進行反饋補償，當系統未飽和時，則不產生影響。設計抗積分飽和如下：

In=In-1+KiTseu

其中，eu為誤差量，In為當前時刻誤差積分量，In-1為上一時刻誤差積分量，Ki為積分系數，Ts為采樣時間，vn為控制器輸出值，umax飽和積分上限，umin為飽和積分下限，Kt為抗飽和增益。抗積分飽和結構圖如圖3所示。

圖3 抗積分飽和結構圖

3.4 海流觀測器設計

本文采用基于模型的方法進行海流觀測器設計，如前文所述，水下航行器速度vB和姿態角Θ 通過導航設備測量得到，控制力τ由舵角和螺旋槳轉速計算得到。由AUV 運動學和動力學方程，設計觀測器，實現常值海流速度的估計。

當AUV 姿態產生變化時，載體坐標系下的海流速度向量隨之變化，其值為

令R( Θ )簡寫為R，對式（27）等號兩邊同時求導，由于可得

又由于

可得：

其中S(ω)為反對稱矩陣。

令：

結合式（3）和式（31）可得：

根據式（4）和式（32），設計海流觀測器如下：

其中：

K1、K2為觀測器增益矩陣，其元素均為常數。，K2=diag{k21，k22，k23，k24，k25，k26}。K2=＞0 ，當K1、K2滿足式（35）時，海流觀測器誤差可全局漸進穩定。

4 二維直線路徑跟隨導引

視線導引［16］（Line of Sight，LOS）法最大的優點就在于其不依賴于被控對象的模型，能夠在模型參數不確定的情形下，或是在外界擾動對AUV 影響較大的環境中來設計控制器，實現對目標模型的控制作用。此外，LOS 法的設計簡單，抗干擾能力強，控制效果出色也是其經常被應用的主要原因。

LOS 法的原理圖如圖4 所示，其基本思想是在兩個航路點Pk(xk，yk)和Pk+1(xk+1，yk+1) 之間的直線上定義一個前視點，從被控對象當前位置到前視點的矢量被稱為LOS矢量，該矢量的方向即為被控對象的期望航向。期望航跡向角為

圖4 LOS原理圖

其中：

χp為路徑切向角，χr(e)為矢量LOS 方向和軌跡誤差e之間的夾角，Δ 為前視距離。Δ 的取值大小會影響跟蹤速度，Δ 越小跟蹤航線越迅速，但Δ太小又會影響整個導引系統的穩定性。在實際導引過程中，我們希望軌跡偏差較大時，AUV 能迅速靠近期望軌跡，此時需要較小的前視距離；在軌跡偏差較小時，AUV 能緩慢平滑地接近期望軌跡，此時需要較大的前視距離。由此思想，設計以下變前視距離：

其中，Δmax、Δmin分別為前視距離的最大值和最小值，k為調節系數。

式（38）同樣可以表示為

考慮側滑角，對期望航向角進行修正，期望航向可由下式表示：

其中，β=arctan(v/u)。

2）直航線段切換

當有多個航路點時，需考慮航路點的切換。常用的方法是為每個航路點設計一個轉向切換圓，切換圓半徑為R，R根據航行任務需要和船的轉向能力確定。如圖5所示，當前位置P(x，y)可測量得到，為下一航路點，計算當前位置和下一航路點Pk+1之間的距離L，當L滿足式（42）時，表示AUV 進入切換圓范圍，則切換到下一個航路點。

圖5 航路點切換示意圖

5 仿真驗證

1）單直線航段路徑跟隨仿真

初始航路點坐標為（0，0），目標航路點坐標為（200，-100），指令速度ud=1.5 m/s。在100s時加入海流干擾仿真結果如圖6～10 所示。

圖6 跟蹤軌跡

由圖6和圖7可以看出，直線路徑跟隨情況下，變前視距離和固定前視距離LOS 兩種導引法均能使AUV 跟蹤上參考路徑，但變前視距離LOS 法的軌跡跟蹤誤差能更快收斂到0 附近，其跟蹤速度更快，在接近參考路徑后，變前視距離LOS 導引法跟蹤更為平緩；在150s 加入海流干擾后，軌跡誤差均能在一定時間內收斂到0附近，AUV 僅較小的偏離參考路徑后就快速回到參考軌跡，兩種導引方法對海流干擾均具有一定的魯棒性，且兩種導引法的抗干擾能力并無較大差異，究其原因，變前視距離LOS 導引法僅在軌跡誤差較大時會有明顯優勢。由圖8 和圖10 可以看出，在100s 加入海流干擾后，AUV 會產生穩定的橫向速度使其產生漂移，加入側滑角項之后，導引律會對指令航向角進行修正，產生新的指令航向角以消除側向速度產生的影響，最終可使AUV跟蹤上參考路徑。

圖7 軌跡誤差

圖8 航向角

圖9 縱向速度

圖10 橫向速度

2）多直線航段路徑跟隨

圖11 軌跡對比

圖12 軌跡誤差對比

圖13 航向跟蹤圖

圖14 縱向速度跟蹤圖

由圖11～14 可以看出，多航路點路徑跟隨情況下，變前視距離和固定前視距離LOS兩種導引法均能使AUV 跟蹤上參考路徑，且海流干擾對導引過程影響不大，航路點的跟蹤能進行良好的切換。在航路點處進行變向時，變前視距離LOS法的軌跡跟蹤誤差能更快的跟蹤上參考路徑，在接近參考路徑后，變前視距離LOS 導引法跟蹤更為平緩。 由圖15可以看出，在100s加入海流干擾后，因AUV航向方向在不斷變化，其受到的海流干擾在AUV 橫向的分量在不斷變化，使其產生變化的橫向速度v，但由于加入側滑角進行修正，使AUV 產生了隨橫向速度v同步變化的指令航向角，最終使AUV 跟蹤上參考路徑。

圖15 橫向速度

6 結語

本文通過分析AUV 運動學和動力學特性，構建了六自由度AUV 的數學模型，使用反步法設計了控制器，并且使用二階濾波器和抗積分飽和對控制器進行了改進，設計海流觀測器成功估計了未知海流干擾并將其反饋給反步控制器，采用變前視距離LOS 導引法設計了直線路徑跟隨算法。通過Matlab仿真分析，所設計的改進反步法控制器能較好地實現AUV 的操縱控制，所設計的直線路徑跟隨算法能較好地實現AUV 的二維路徑跟隨。為下一步實現AUV 的三維軌跡跟蹤打下了良好的基礎。