無(wú)人水面艇自適應(yīng)路徑跟蹤算法

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

（哈爾濱工程大學(xué)水下機(jī)器人技術(shù)重點(diǎn)實(shí)驗(yàn)室，哈爾濱150001）

0 引言

在2007年，美國(guó)海軍發(fā)布的《海軍無(wú)人水面艇主計(jì)劃》中，給出了無(wú)人水面艇（Unmanned Surface Vehicle，USV）定義：靜止時(shí)浮在水面上，而在運(yùn)動(dòng)時(shí)持續(xù)與水面接觸，且具有不同自動(dòng)操控能力的一種無(wú)人航行器。無(wú)人水面艇具有巨大的軍事和工程應(yīng)用前景，尤其是在維護(hù)國(guó)家海洋安全和海洋開(kāi)發(fā)等方面（諸如海洋探測(cè)、環(huán)境監(jiān)控、目標(biāo)偵查、通行中繼等）將發(fā)揮不可替代的作用。

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

目前，多數(shù)LOS導(dǎo)引策略的改進(jìn)大多基于增強(qiáng)其抗干擾能力，而忽略了船體本身操縱性和規(guī)劃方案改變?cè)斐傻挠绊憽１热绠?dāng)無(wú)人水面艇在航行時(shí)，航行速度的改變或期望航線長(zhǎng)度的改變，也會(huì)造成超調(diào)增加，振蕩時(shí)間變長(zhǎng)，路線跟蹤的完成度不高（比如原本設(shè)定的安全距離為30m，但目前跟蹤路線長(zhǎng)度為50m，造成該路徑只跟蹤了40%）等問(wèn)題。因此，針對(duì)傳統(tǒng)LOS制導(dǎo)律［5］無(wú)法實(shí)時(shí)調(diào)整參數(shù)的問(wèn)題，以及為了充分利用無(wú)人水面艇操作性，提高能量利用率，本文設(shè)計(jì)了基于Mamdani模塊控制的自適應(yīng)LOS控制策略。

1 欠驅(qū)動(dòng)水面滑行艇模型

假設(shè)以下條件成立：

1）只考慮艇體在縱蕩、橫蕩、艏搖的三自由度運(yùn)動(dòng)；

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

3）欠驅(qū)動(dòng)，且只安裝1個(gè)推進(jìn)泵，通過(guò)推進(jìn)泵的推力和扭轉(zhuǎn)角度控制推力和轉(zhuǎn)艏力矩；

4）忽略風(fēng)浪流的干擾。

可得如下欠驅(qū)動(dòng)非線性方程［6］

如圖1所示，本文通過(guò)對(duì) “天行一號(hào)”進(jìn)行模型辨識(shí)，得

圖1 “天行一號(hào)”無(wú)人水面艇Fig.1 “Tianxing No.1” USV

阻力系數(shù)R的計(jì)算如下所示

式中，L為艇體水線長(zhǎng)，Draft為艇體吃水。

2 問(wèn)題描述

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

圖2 LOS制導(dǎo)律原理圖Fig.2 Principle diagram of LOS guidance law

2）LOS導(dǎo)引任務(wù)：通過(guò)控制船舶的方向始終對(duì)準(zhǔn)LOS角，引導(dǎo)船舶位置收斂到期望航線，使得橫側(cè)偏差δ（t）和目標(biāo)距離d（t）收斂至原點(diǎn)

3）安全半徑RSafe：期望目標(biāo)點(diǎn)的膨脹系數(shù)。當(dāng)d（t）≤RSafe時(shí)，USV到達(dá)目標(biāo)點(diǎn)，進(jìn)而考慮是否跟蹤下一個(gè)目標(biāo)點(diǎn)。

4）LOS制導(dǎo)律：基于LOS導(dǎo)引任務(wù)的要求，輸入艇體當(dāng)前位置和期望航線，輸出當(dāng)前時(shí)刻期望艏向φd

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

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

由圖2得橫側(cè)偏差δ（t）為

對(duì)式（10）求導(dǎo)，得

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

選取V1=δ2/2，將其作為L(zhǎng)yapunov函數(shù)，對(duì)其求導(dǎo)

根據(jù)Lyapunov穩(wěn)定性定理可知，當(dāng)USV艏向?yàn)槠谕枷驎r(shí)（φ=φd），USV的位置能夠漸近收斂于期望航線上。LOS導(dǎo)引策略的控制參數(shù)：1）安全半徑RSafe，判斷USV是否跟蹤下一個(gè)目標(biāo)點(diǎn)，從而影響橫側(cè)偏差δ（t）的超調(diào)和航線的跟蹤完成度；2）收斂半徑R控制δ（t）的收斂快慢，若R越大，δ（t）收斂越慢，反之亦然。

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

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

3.1 直線路徑跟蹤控制策略

欠驅(qū)動(dòng)系統(tǒng)的內(nèi)模為自制系統(tǒng)，不受輸入控制。本文采取模糊控制器，針對(duì)不同的期望速度，通過(guò)專家辨識(shí)給出不同的LOS控制參數(shù)，從而削弱速度對(duì)橫側(cè)偏差的影響，實(shí)現(xiàn)系統(tǒng)的速度 “解耦”。

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

LOS制導(dǎo)律模塊采用上述提及的制導(dǎo)律。

航速控制器和艏向控制器分別采用累積式PD和PD控制器，產(chǎn)生推進(jìn)器推力P，以及推進(jìn)器扭轉(zhuǎn)角度α。

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

3.2 模糊控制器設(shè)計(jì)

本文所采用的模糊控制器為基于Mamdani的MIMO模糊控制器［9］，根據(jù)給定的期望速度ud和目標(biāo)路徑（Pk，Pk+1）求出的目標(biāo)路徑長(zhǎng)度L，將其作為兩個(gè)輸入變量。ud的基本論域?yàn)椋?，4］，L的基本論域?yàn)椋?，500］，兩個(gè)輸出變量分別為L(zhǎng)OS導(dǎo)引策略的控制參數(shù)安全半徑RSafe和收斂半徑R。其中，RSafe的基本論域?yàn)椋?，30］，R的基本論域?yàn)閇0，100]。 將ud、L、RSafe、R模糊化，模糊集合分別為EU、EL、ES、ER。

R影響δ的收斂速度，當(dāng)R越大則δ越快收斂，RSafe決定何時(shí)進(jìn)入下一個(gè)目標(biāo)點(diǎn)跟蹤。本文給出模糊規(guī)則設(shè)計(jì)的基本原則為：L越大，RSafe越大，R越大；ud越大，RSafe越小，R越大。具體如表1所示。

表1 模糊集合Table 1 Fuzzy set

建立推理規(guī)則庫(kù)，如表2所示。表格內(nèi)，行代表EL，列代表 EU； （x，y）中，x代表ES，y代表ER。

表2 模糊推理規(guī)則庫(kù)Table 2 Rule base of fuzzy reasoning

4 仿真實(shí)驗(yàn)

為驗(yàn)證本文算法的優(yōu)越性和有效性，實(shí)驗(yàn)采用不規(guī)則且分段速度不同的閉合曲線作為期望航線，同時(shí)與ud∈（1.0，2.5）、路線長(zhǎng)度 Length∈（100，300）之內(nèi)具有優(yōu)良跟蹤精度的LOS控制參數(shù)RSafe及R（RSafe=20、R=30）進(jìn)行對(duì)比。

USV從原點(diǎn)出發(fā)，初始速度u=0，初始艏向φ=0，期望路徑點(diǎn)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）。

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

水動(dòng)力參數(shù)如下：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。

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

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

路徑跟蹤結(jié)果對(duì)比如圖4所示。

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

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

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

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

圖5 控制參數(shù)變化及橫側(cè)偏差Fig.5 Varying of parameters and cross-track error

從圖5可知，在航線或者航速變化時(shí)，模糊LOS通過(guò)自適應(yīng)的調(diào)節(jié)參數(shù)，使得橫側(cè)偏差的震蕩時(shí)間減小，最大超調(diào)減?。ㄅ懦捎谇袚Q目標(biāo)點(diǎn)而產(chǎn)生的躍變），平均誤差減小，整體跟蹤精度更高，并且模糊LOS能夠更快地完成路徑跟蹤。具體數(shù)值如表3所示。

表3 跟蹤精度Table 3 Path following accuracy

推進(jìn)器使用及控制跟蹤曲線如圖6所示。

從圖6可知，兩種算法的艏向跟蹤相差不大，但是跟蹤時(shí)間為600s～700s時(shí)，傳統(tǒng)LOS產(chǎn)生的超調(diào)及振蕩時(shí)長(zhǎng)遠(yuǎn)大于模糊LOS；在1200s左右，傳統(tǒng)LOS的響應(yīng)速度遠(yuǎn)慢于模糊LOS，從而導(dǎo)致后續(xù)跟蹤完成時(shí)間慢于模糊LOS。

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

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

5 結(jié)論

本文研究了欠驅(qū)動(dòng)無(wú)人水面艇的路徑跟蹤問(wèn)題。首先給出了水面滑行艇的數(shù)學(xué)模型，然后通過(guò)Lyapunov定理證明了LOS算法的穩(wěn)定性，最后采用模糊控制的方法修正了LOS控制參數(shù)。

通過(guò)仿真實(shí)驗(yàn)對(duì)比傳統(tǒng)LOS算法，驗(yàn)證了基于Mamdani的LOS導(dǎo)引策略的優(yōu)越性和可行性。仿真數(shù)據(jù)表明，該算法能夠精確地跟蹤不規(guī)則閉合曲線，相比傳統(tǒng)LOS具有優(yōu)良的自適應(yīng)能力、更好的魯棒性，推進(jìn)器效率更高，并且將模糊控制作為智能算法，使其相比傳統(tǒng)算法更具備先進(jìn)性。同時(shí)，本算法作為無(wú)模型算法，在應(yīng)用上具備普適性。

為了真實(shí)地反應(yīng)USV的實(shí)際操縱情況，并讓算法具備更加優(yōu)秀的泛化能力，下一步將基于假設(shè)船前后不對(duì)稱、且受到海洋環(huán)境干擾的情況下，研究將智能控制理論及多種控制方法結(jié)合的算法。