無人水面艇自適應路徑跟蹤算法

朱 騁，莊佳園，張 磊，許建輝，蘇玉民

（哈爾濱工程大學水下機器人技術重點實驗室，哈爾濱150001）

0 引言

在2007年，美國海軍發布的《海軍無人水面艇主計劃》中，給出了無人水面艇（Unmanned Surface Vehicle，USV）定義：靜止時浮在水面上，而在運動時持續與水面接觸，且具有不同自動操控能力的一種無人航行器。無人水面艇具有巨大的軍事和工程應用前景，尤其是在維護國家海洋安全和海洋開發等方面（諸如海洋探測、環境監控、目標偵查、通行中繼等）將發揮不可替代的作用。

路徑跟蹤作為無人水面艇控制系統中最基本的模塊，要求船舶不考慮時間約束，在路徑規劃器計劃的期望路徑中自動航行。相比航跡跟蹤，路徑跟蹤忽略了時間因素，因此能夠更加光順地收斂到期望路徑，且不易出現控制輸入飽和現象。Niu等人［1］基于 “C?enduro”無人水面艇討論了4種無模型的路徑跟蹤方法，包括：胡蘿卜追逐法（Carrort Chasing Algorithm）、非線性制導律（Nonlinear Guidance Law）、向量場跟蹤法（Vector Field Algorithm）和視線法（Light?of?Sight），文中結論是 LOS 在長航時相比其他3種算法更具備抗干擾能力。B?rhaug等人［2］針對有洋流存在的情況，提出了改進的LOS制導律和自適應反饋控制器，理論分析并證明了該控制策略在恒定和無旋海流的情況下能夠保證直線路徑的全局漸近跟蹤。但是，由于動力學模型中同時存在絕對速度和相對速度，其迫使在推力和轉向控制器中引入自適應，增加了算法的復雜性，降低了穩定性。Caharjia等人［3］在 B?rhaug的基礎上，假定海流恒定且無旋，采用相對速度模型簡化了控制系統，實現了系統的全局漸近穩定性和局部指數穩定性。董早鵬等人［4］基于非對稱的無人水面艇數學模型，針對非線性反步法和LOS法之間存在的互補優化問題，給出了結合的控制策略，理論分析證明了系統的全局漸近穩定性。

目前，多數LOS導引策略的改進大多基于增強其抗干擾能力，而忽略了船體本身操縱性和規劃方案改變造成的影響。比如當無人水面艇在航行時，航行速度的改變或期望航線長度的改變，也會造成超調增加，振蕩時間變長，路線跟蹤的完成度不高（比如原本設定的安全距離為30m，但目前跟蹤路線長度為50m，造成該路徑只跟蹤了40%）等問題。因此，針對傳統LOS制導律［5］無法實時調整參數的問題，以及為了充分利用無人水面艇操作性，提高能量利用率，本文設計了基于Mamdani模塊控制的自適應LOS控制策略。

1 欠驅動水面滑行艇模型

假設以下條件成立：

1）只考慮艇體在縱蕩、橫蕩、艏搖的三自由度運動；

2）船舶前后左右對稱，并且中心和重心重合；

3）欠驅動，且只安裝1個推進泵，通過推進泵的推力和扭轉角度控制推力和轉艏力矩；

4）忽略風浪流的干擾。

可得如下欠驅動非線性方程［6］

如圖1所示，本文通過對 “天行一號”進行模型辨識，得

圖1 “天行一號”無人水面艇Fig.1 “Tianxing No.1” USV

阻力系數R的計算如下所示

式中，L為艇體水線長，Draft為艇體吃水。

2 問題描述

LOS導引策略作為經典的路徑跟蹤策略，由于其不依賴于模型，且需要設計的參數少，具備抗干擾能力強等特點，被廣泛應用于USV的路徑跟蹤。作為幾何算法應用，可將LOS導引劃分為基于收斂圈和前視距離2種［7］。本文以大地坐標系作為慣性坐標系，由圖2可知LOS制導律原理。

圖2 LOS制導律原理圖Fig.2 Principle diagram of LOS guidance law

2）LOS導引任務：通過控制船舶的方向始終對準LOS角，引導船舶位置收斂到期望航線，使得橫側偏差δ（t）和目標距離d（t）收斂至原點

3）安全半徑RSafe：期望目標點的膨脹系數。當d（t）≤RSafe時，USV到達目標點，進而考慮是否跟蹤下一個目標點。

4）LOS制導律：基于LOS導引任務的要求，輸入艇體當前位置和期望航線，輸出當前時刻期望艏向φd

式中，φpk表示期望航線與X軸形成的夾角；β表示橫漂角，β=tanh-1（v/u）。

定理1：若航向控制器完美跟蹤LOS制導律輸出的期望艏向，則橫側偏差δ（t）和目標距離d（t）收斂至原點［8］。

由圖2得橫側偏差δ（t）為

對式（10）求導，得

若航向控制器能夠完美跟蹤期望艏向，即φ=φd，則有

選取V1=δ2/2，將其作為Lyapunov函數，對其求導

根據Lyapunov穩定性定理可知，當USV艏向為期望艏向時（φ=φd），USV的位置能夠漸近收斂于期望航線上。LOS導引策略的控制參數：1）安全半徑RSafe，判斷USV是否跟蹤下一個目標點，從而影響橫側偏差δ（t）的超調和航線的跟蹤完成度；2）收斂半徑R控制δ（t）的收斂快慢，若R越大，δ（t）收斂越慢，反之亦然。

從式（12）可以看出，USV的前向速度u與δ（t）之間存在耦合。如果安全半徑固定，當期望航線的距離小于安全半徑，則此期望航線被跳過。為避免上述問題，本文采用模糊控制器對LOS的控制參數進行修正，削減了u與δ（t）之間存在耦合的影響并且獲得較為優良的航線跟蹤完成度。

3 基于Mamdani的無人水面艇直線路徑跟蹤方法

3.1 直線路徑跟蹤控制策略

欠驅動系統的內模為自制系統，不受輸入控制。本文采取模糊控制器，針對不同的期望速度，通過專家辨識給出不同的LOS控制參數，從而削弱速度對橫側偏差的影響，實現系統的速度 “解耦”。

基于Mamdani的LOS路徑跟蹤控制器原理框圖如圖3所示。

LOS制導律模塊采用上述提及的制導律。

航速控制器和艏向控制器分別采用累積式PD和PD控制器，產生推進器推力P，以及推進器扭轉角度α。

圖3 基于Mamdani的LOS路徑跟蹤原理框架圖Fig.3 Block diagram of LOS path following principle based on Mamdani

3.2 模糊控制器設計

本文所采用的模糊控制器為基于Mamdani的MIMO模糊控制器［9］，根據給定的期望速度ud和目標路徑（Pk，Pk+1）求出的目標路徑長度L，將其作為兩個輸入變量。ud的基本論域為［0，4］，L的基本論域為［0，500］，兩個輸出變量分別為LOS導引策略的控制參數安全半徑RSafe和收斂半徑R。其中，RSafe的基本論域為［0，30］，R的基本論域為[0，100]。 將ud、L、RSafe、R模糊化，模糊集合分別為EU、EL、ES、ER。

R影響δ的收斂速度，當R越大則δ越快收斂，RSafe決定何時進入下一個目標點跟蹤。本文給出模糊規則設計的基本原則為：L越大，RSafe越大，R越大；ud越大，RSafe越小，R越大。具體如表1所示。

表1 模糊集合Table 1 Fuzzy set

建立推理規則庫，如表2所示。表格內，行代表EL，列代表 EU； （x，y）中，x代表ES，y代表ER。

表2 模糊推理規則庫Table 2 Rule base of fuzzy reasoning

4 仿真實驗

為驗證本文算法的優越性和有效性，實驗采用不規則且分段速度不同的閉合曲線作為期望航線，同時與ud∈（1.0，2.5）、路線長度 Length∈（100，300）之內具有優良跟蹤精度的LOS控制參數RSafe及R（RSafe=20、R=30）進行對比。

USV從原點出發，初始速度u=0，初始艏向φ=0，期望路徑點pn（x，y）分別為p1（55.7，45.6）、p2（55.7，228.0）、p3（167.0，319.2）、p4（612.3，319.1）、p5（55.7，45.6）。

跟蹤時的期望速度如下：up0，1=1.5、up1，2=2.5、up2，3=4.0、up3，4=3.5、up4，5=2.5。

水動力參數如下：Yv=-0.4715、Yr=0.07156、Nv=-0.1459、Nr=-0.0557；Xvr=-0.0025、Xvv=0.0682、Xrr=0.0039、Yvr=-0.29、Yvv=-0.4757、Yrr=-0.035、Nvv=0.0148、Nrr=-0.04。

船型參數如下所示：L=5.2、Draft=0.4、Mass=500.5、IZ=8000、Mxx=184.57、Myy=15.3384、Mrr=1206。

控制器參數如下：Ku=1、kPu=10、kDu=3、kPα=9、kDα=6。

路徑跟蹤結果對比如圖4所示。

從圖4（a）可知，模糊LOS的全局跟蹤精度更高。從圖4（b）和圖4（c）可知，在跟蹤第一、第二段路徑時，模糊LOS的最大超調為0.7，沒有發生振蕩，傳統LOS的最大超調為1.1，發生了一次振蕩，兩種算法跟蹤效果均十分良好。從圖4（d）可知，在第三段路徑初始階段，即在第二、第三段路徑拐角處，模糊LOS的最大超調為2.1，發生一次短暫的振蕩，傳統LOS的最大超調為4.9，發生兩次振蕩。從圖4（e）可知，在第四段路徑初始階段，即在第三、第四段路徑拐角處，模糊LOS的最大超調為10.5，不發生振蕩，傳統LOS的最大超調為14.4，發生了一次振蕩但收斂時間較長。

圖4 基于Mamdani的LOS和傳統LOS閉合曲線跟蹤圖Fig.4 Closed curve path following of different LOS

對比可得，基于Mamdani的模糊LOS導引策略能夠更快地收斂于期望航線，且能夠更加精確地跟蹤期望航線。

具體跟蹤精度如圖5所示。

圖5 控制參數變化及橫側偏差Fig.5 Varying of parameters and cross-track error

從圖5可知，在航線或者航速變化時，模糊LOS通過自適應的調節參數，使得橫側偏差的震蕩時間減小，最大超調減小（排除由于切換目標點而產生的躍變），平均誤差減小，整體跟蹤精度更高，并且模糊LOS能夠更快地完成路徑跟蹤。具體數值如表3所示。

表3 跟蹤精度Table 3 Path following accuracy

推進器使用及控制跟蹤曲線如圖6所示。

從圖6可知，兩種算法的艏向跟蹤相差不大，但是跟蹤時間為600s～700s時，傳統LOS產生的超調及振蕩時長遠大于模糊LOS；在1200s左右，傳統LOS的響應速度遠慢于模糊LOS，從而導致后續跟蹤完成時間慢于模糊LOS。

圖6 控制器性能圖Fig.6 Diagram of controller performance

通過分析可知，由于模糊LOS削減了縱向速度u和LOS導引策略之間的耦合影響，因此其速度控制振蕩時間較短，收斂速度更快。從圖6（c）可知，無論是推進器推力還是扭轉角度，模糊LOS的超調均更小，振蕩時間更短。因此，其更有效地利用了艇體操縱性能，能量利用率更高。

5 結論

本文研究了欠驅動無人水面艇的路徑跟蹤問題。首先給出了水面滑行艇的數學模型，然后通過Lyapunov定理證明了LOS算法的穩定性，最后采用模糊控制的方法修正了LOS控制參數。

通過仿真實驗對比傳統LOS算法，驗證了基于Mamdani的LOS導引策略的優越性和可行性。仿真數據表明，該算法能夠精確地跟蹤不規則閉合曲線，相比傳統LOS具有優良的自適應能力、更好的魯棒性，推進器效率更高，并且將模糊控制作為智能算法，使其相比傳統算法更具備先進性。同時，本算法作為無模型算法，在應用上具備普適性。

為了真實地反應USV的實際操縱情況，并讓算法具備更加優秀的泛化能力，下一步將基于假設船前后不對稱、且受到海洋環境干擾的情況下，研究將智能控制理論及多種控制方法結合的算法。