黃雨杰，張子唐，孫 騫，李一兵

（1. 哈爾濱工程大學(xué)信息與通信工程學(xué)院，哈爾濱 150001；2. 先進船舶通信與信息技術(shù)工業(yè)和信息化部重點實驗室，哈爾濱 150001）

1 引 言

水面無人艇（Unmanned Surface Vehicle,USV）是一種智能化的小型水面平臺，由于具有較高的智能化程度、較好的隱身性能、較強的機動能力以及較低的造價，它被應(yīng)用于完成情報收集、反潛、偵察與探測、精確打擊等作戰(zhàn)任務(wù)。面對動態(tài)復(fù)雜的環(huán)境、不可預(yù)知的潛在危險和多樣化的任務(wù)使命，單一無人艇受限于自身搭載的有限載荷與系統(tǒng)，顯得勢單力薄。同時，由多艘無人艇聯(lián)合起來構(gòu)成的協(xié)同系統(tǒng)，具有更強的魯棒性、通信能力、機動性、靈活性、更高的作業(yè)效率和更廣的作業(yè)范圍，在海洋戰(zhàn)場環(huán)境信息收集、反潛領(lǐng)海監(jiān)視、補給支援等軍事領(lǐng)域發(fā)揮著越來越重要的作用。

多無人艇編隊控制是指多個無人艇組成的編隊在向特定目標(biāo)或方向運動的過程中，相互之間保持預(yù)定的幾何形態(tài)（即隊形），同時又要適應(yīng)環(huán)境約束（例如避開障礙）的控制問題[1]。因此，多無人艇編隊運動控制技術(shù)是多無人艇水面無人艇完成協(xié)同作業(yè)任務(wù)的前提和基礎(chǔ)，無人艇編隊運動控制方法多應(yīng)用領(lǐng)航-跟隨結(jié)構(gòu)、虛擬結(jié)構(gòu)法和行為結(jié)構(gòu)法等[2]。為了保證編隊系統(tǒng)的穩(wěn)定性和魯棒性，多使用滑?？刂芠3]和模型預(yù)測控制（Model Predictive Control, MPC）[4]對無人艇編隊成員的控制器進行設(shè)計，減小了模型誤差和外界環(huán)境對無人艇運動的影響，實現(xiàn)編隊維持和編隊跟蹤。其中，分布式模型預(yù)測控制（Distributed Model Predictive Control, DMPC）能夠?qū)?fù)雜系統(tǒng)問題拆分成若干個相互關(guān)聯(lián)簡單系統(tǒng)問題并交給子系統(tǒng)單獨計算，降低了時間成本和計算復(fù)雜度，所以DMPC 在無人車和無人機編隊控制中得到了廣泛的應(yīng)用。但現(xiàn)有的大多數(shù)DMPC 算法都假設(shè)所有智能體都能獲取平衡所需的先驗信息，這對于在廣闊海面航行的無人艇而言并不現(xiàn)實。

同時，在動態(tài)和不確定的水面環(huán)境中無人艇需要進行局部路徑規(guī)劃技術(shù)完成障礙物規(guī)避。當(dāng)下，對于未知和動態(tài)障礙物的路徑規(guī)劃方法中，快速探索隨機樹（Rapidly-exploring Random Trees）不僅難以找到最優(yōu)解，而且穩(wěn)定性和實時性較差；模糊邏輯算法（Fuzzy Logic）過分依賴專家知識，且缺乏系統(tǒng)性；神經(jīng)網(wǎng)絡(luò)（Neural Network）需要大量的時間和樣本對神經(jīng)網(wǎng)絡(luò)進行訓(xùn)練；強化學(xué)習(xí)（Reinforcement Learning）需要手工標(biāo)記大量樣本，且會遇到維度災(zāi)難的問題；與強化學(xué)習(xí)相比，雖然深度強化學(xué)習(xí)（Deep Reinforcement Learning）能夠避免手工標(biāo)記數(shù)據(jù)、減少維度，但是其也需要較長的訓(xùn)練時間；人工勢場法（Artificial Potential Field，APF）結(jié)構(gòu)簡單、易于實現(xiàn)且實時性強，在無人艇的實時避障中有著廣泛的應(yīng)用[5-6]。

基于以上分析，本文研究了在編隊內(nèi)部分成員無法獲得編隊平衡的先驗信息條件下的多水面無人艇編隊控制問題。本文將領(lǐng)航-跟隨結(jié)構(gòu)與DMPC 算法結(jié)合，完成跟隨者控制器設(shè)計，實現(xiàn)了編隊內(nèi)部分成員無法獲取編隊平衡先驗信息情況下的無人艇編隊控制；同時，針對無人艇編隊內(nèi)部障礙物規(guī)避問題，設(shè)計了基于改進APF 和MPC 的自主避障控制器；最后，通過仿真實驗對算法的可行性和有效性進行了驗證。

2 系統(tǒng)模型

2.1 無人艇動力學(xué)模型

為了簡化無人艇的運動控制數(shù)學(xué)模型，將無人艇的運動近似為二維水平面上的三自由度運動模型，具體模型如圖1 所示，圖1 所表示的三自由度運動數(shù)學(xué)模型如式(1)所示[7]。文中所涉及的數(shù)學(xué)模型和部分狀態(tài)變量符號和含義采用國際水池會議（International Towing Tank Conference）推薦的以及造船和輪機工程學(xué)會（Society of Naval Architects and Marine Engineers）術(shù)語公報推薦的體系[8]。

圖1 雙體船模型Fig. 1 Catamaran model

式中m表示無人艇的質(zhì)量，Izz表示無人艇繞Z軸的慣性矩，Tp和Ts分別表示無人艇左右舷噴水推進器輸出推力，τx= (Tp+Ts)表示無人艇控制力，τN= (Tp+Ts)B/2表示無人艇控制力矩，B表示無人艇的寬度，xG表示無人艇重心坐標(biāo)，X(.),Y(.),N(.)是恒定的水動力導(dǎo)數(shù)，分別代表無人艇對于縱移力、橫移力和艏搖力矩的敏感程度。無人艇的平面運動學(xué)模型為

式中，η= [x,y,ψ]T代表無人艇在北東坐標(biāo)系下的位置和艏搖角大小，υ= [u,ν,r]T代表無人艇速度向量，R(ψ)是船體坐標(biāo)系到北東坐標(biāo)系下的轉(zhuǎn)換矩陣，其具體表達式為

由于無人艇運動受到外部環(huán)境的限制，假設(shè)無人艇的水平面運動和轉(zhuǎn)動之間是解耦的，進而可將復(fù)雜的動力學(xué)方程進行線性化處理，化簡為速度和轉(zhuǎn)向的動力學(xué)方程，故無人艇動力學(xué)方程為

式中，s=[u,v,r,x,y,ψ]T表示無人艇的狀態(tài)量，c=[Tp,Ts]T代表無人艇的控制變量，即左右噴水推進器推力。無人艇詳細信息和相關(guān)參數(shù)如表1所示。

表1 雙體船模型參數(shù)[7]Table 1 Parameters of catamaran model[7]

2.2 編隊模型

假設(shè)編隊中有N艘無人艇，各無人艇動力學(xué)模型均由式(4)所示。無人艇編隊控制的目標(biāo)是跟隨者（Follower）跟蹤領(lǐng)航者（Leader）的航向角和速度，同時保證任意兩艘無人艇之間的間隔與期望間隔相同，這可以表示為

式中， N = ｛1 ,2,3, … ,N｝；ψi(k)表 示第i艘無人艇在k時刻的航向角，ψL(k)表示領(lǐng)航者在k時刻的航向角；υl(k) =［ul(k),vl(k),rl(k)］T表示第i艘無人艇在k時刻的速度狀態(tài)，包括縱移速度、橫移速度和艏搖角速度，υL(k) =［u L(k),v L(k),rL(k)］T代表領(lǐng)航者在k時刻的速度狀態(tài)；Pi(k)表示第i艘無人艇在k時刻的位置，rij=(xd,ij,yd,ij)T表示第i艘無人艇和第j艘無人艇之間的期望距離向量，通過控制rij值進而得到不同的編隊隊形。

無人艇編隊中通信拓撲的具體結(jié)構(gòu)對于DMPC 算法中代價函數(shù)設(shè)計有著重要的影響[9]。無人艇編隊中通信拓撲可以用矩陣G= [gij]∈?N×N，具體定義為

假設(shè)在編隊中領(lǐng)航者的通信優(yōu)先級為1，領(lǐng)航者鄰接集合AL內(nèi)成員的通信優(yōu)先級為2，能夠與AL內(nèi)成員通信而不能直接與領(lǐng)航者通信的無人艇通信優(yōu)先級為3，依此類推，通信優(yōu)先級示意具體如圖2 所示。Mi表示比第i艘無人艇通信優(yōu)先級更高的無人艇所組成的集合，O i=M i∩Ai為期望集合，表示能夠向第i艘無人艇提供有用信息的無人艇集合；此時第i艘無人艇能夠與鄰接集合Ai內(nèi)所有成員通信，但是僅對期望集合Oi中成員所廣播的假設(shè)狀態(tài)空間信息進行處理。雖然無人艇編隊的通信結(jié)構(gòu)為雙向拓撲結(jié)構(gòu)，但是其中關(guān)于狀態(tài)空間的通信拓撲結(jié)構(gòu)為單向拓撲。當(dāng)無人艇編隊的通信拓撲結(jié)構(gòu)中包含一個生成樹（Spanning Tree）時，編隊成員之間的狀態(tài)能夠趨于一致[10]。

圖2 通信優(yōu)先級示意圖Fig. 2 Schematic diagram of communication priority

3 編隊運動控制

3.1 跟隨者控制器

MPC 也稱為滾動時域控制或移動時域控制（Receding Horizon Control），是一種基于模型的開環(huán)最優(yōu)控制策略，其使用一系列控制方法，利用確定的模型預(yù)測未來時刻不同的控制變量對輸出結(jié)果的影響，并且在滿足系統(tǒng)約束條件的基礎(chǔ)上通過最小化代價函數(shù)獲得控制信號[11-12]。與傳統(tǒng)意義下的最優(yōu)控制算法不同，MPC 采用滾動優(yōu)化策略，具體實現(xiàn)過程可描述為測量當(dāng)前狀態(tài)、預(yù)測迭代、求解優(yōu)化和執(zhí)行4 個步驟[13]，其基本原理如圖3 所示。

圖3 模型預(yù)測控制原理Fig. 3 Principle of model predictive control

DMPC 是指將整個系統(tǒng)分為幾個相互關(guān)聯(lián)的子系統(tǒng)，每個子系統(tǒng)都擁有自己獨立的局部控制器，這些控制器不僅考慮子系統(tǒng)的自身狀態(tài)，還受到其他關(guān)聯(lián)子系統(tǒng)的影響[14]。DMPC 通過將一個大規(guī)模的復(fù)雜系統(tǒng)問題拆分成若干個較為簡單的問題分配給子系統(tǒng)單獨計算，降低了時間成本和計算復(fù)雜度。同時，在DMPC 中，允許各子系統(tǒng)間進行信息交互，從而能夠?qū)崿F(xiàn)子系統(tǒng)之間的相互協(xié)調(diào)，以達到系統(tǒng)的全局最優(yōu)。

由于海面區(qū)域十分廣闊，無人艇編隊在執(zhí)行任務(wù)時跟隨者與領(lǐng)航者之間的距離可能超出有效通信范圍dc，此時部分跟隨者無法獲得保持編隊平衡的先驗信息，這將增加編隊航行過程中的不穩(wěn)定因素。本文通過引入假設(shè)狀態(tài)空間和平衡狀態(tài)空間實現(xiàn)了編隊內(nèi)部分跟隨者無法獲取編隊平衡先驗信息情況下的無人艇編隊運動控制。

考慮包括N-1 個跟隨者（Follower）和1 個領(lǐng)航者（Leader）的無人艇編隊系統(tǒng)。為方便表示對以下符號進行定義：

（1）si(k) =[xi(k),yi(k),ψi(k),ui(k),vi(k),ri(k)]T表示第i艘無人艇在第k時刻的狀態(tài)；

（2）Si(k) =[si(k|k),si(k+1|k),…,si(k+Np|k)]T表示在k時刻，第i艘無人艇在[k,k+Np]時間段內(nèi)的狀態(tài)空間；

（3）Ci(k) =[ci(k|k),ci(k+1|k),…,ci(k+Np-1|k)]T表示第i艘無人艇在［k,k+Np］時間段內(nèi)實現(xiàn)S i(k)的預(yù)測控制序列；

（4）sF(k) =[s1(k),s2(k), …,sN(k)]T表示在k時刻編隊系統(tǒng)的狀態(tài)集合；

（5）cF(k) =[ci(k),c2(k),…cN(k)]T表 示 在k時刻編隊系統(tǒng)的輸入控制量集合。其中，Np代表預(yù)測步長，同時對符號上標(biāo)進行定義，p表示預(yù)測狀態(tài)，a表示假設(shè)狀態(tài)，b表示平衡狀態(tài)。

進而，整個無人艇編隊系統(tǒng)的離散時間動力學(xué)模型可表示為

DMPC 算法將整個系統(tǒng)的全局最優(yōu)控制問題轉(zhuǎn)化為單個無人艇i的局部最優(yōu)控制問題。對第i艘無人艇而言，其局部最優(yōu)控制問題的求解僅使用期望集合Oi中的無人艇提供的信息，故(k)的定義為

式中，poi,j代表選取某個鄰接集合成員的偏好，一般而言，第j艘無人艇通信優(yōu)先級越高poi,j越大。進而，第i艘跟隨者無人艇的代價函數(shù)可以表示為式(10)所示形式。

其中，Qd、Qa、Qb、R均為對角矩陣，0 為零矩陣，Qd,i，Qa,i和Ri為第i艘無人艇代價函數(shù)的權(quán) 重 矩 陣，Qd,i=diag{qd,i1,qd,i2, …,qd,i6}表 示 對 預(yù) 測狀態(tài)與期望狀態(tài)之間誤差的懲罰強度，Qb,i=diag{qb,i1,qb,i2, …,qb,i6}表 示 對 預(yù) 測 狀 態(tài) 與 平 衡 狀 態(tài)之 間 誤 差 的 懲 罰 強 度，Qa,i=diag{qa,i1,qa,i2,…,qa,i6}表示對預(yù)測狀態(tài)與假設(shè)狀態(tài)之間誤差的懲罰強度，Ri=diag{ri1,ri2}表示對預(yù)測控制序列與假設(shè)控制序列之間誤差的懲罰強度。

第i艘無人艇的本地開環(huán)優(yōu)化控制問題可以表示為式中，C 表示輸入允許集合，S表示狀態(tài)允許集合。進而，第i艘跟隨者無人艇編隊控制的DMPC控制算法內(nèi)容如下：

（1）接收鄰接集合內(nèi)其他成員的假設(shè)狀態(tài)空間Sia(k)，按照式(9)得出期望狀態(tài)空間Sid(k)；

（2）獲取k時刻的期望狀態(tài)空間Sid(k)和平衡狀態(tài)信息Sib(k)，使用順序二次規(guī)劃算法(Sequential Quadratic Programming, SQP)求解式(11)所表示的優(yōu)化問題，得到最優(yōu)控制序列Ci*(k)；

（3）使用最優(yōu)控制序列Ci*(k)中的第一個控制量，即令ci(k) =ci*(k|k)，并將ci(k)作用于無人艇動力學(xué)模型，得到k+ 1時刻的狀態(tài)si(k+ 1)；

（4）由最優(yōu)控制序列C*(k)和最優(yōu)控制狀態(tài)空間Si*(k)得到假設(shè)控制序列Cia(k+1)，具體如式(12)所示。其中，c(k+Np|k)為保持最優(yōu)預(yù)測狀態(tài)空間Si*(k)中第Np個速度狀態(tài)的控制量；

（5）將Cia(k+1)作用于動力學(xué)方程得到假設(shè)狀態(tài)空間Sia(k+1)，并向外廣播假設(shè)狀態(tài)空間Sia(k+1)；

（6）求解平衡狀態(tài)空間Sb(k+1)，其表示無人艇保持當(dāng)前速度狀態(tài)移動Np步生成的狀態(tài)空間；

（7）令k=k+ 1，回到步驟（1）。

3.2 自主避障控制

當(dāng)無人艇在編隊航行遇到障礙物時，無人艇需要進行自主避障。在此情況下，無人艇集群控制與無人艇自主避障在控制目標(biāo)層面是相互沖突的。編隊集結(jié)需要無人艇盡可能保持相對穩(wěn)定的隊形，而自主避障則要求無人艇根據(jù)環(huán)境信息和自身情況和環(huán)境信息，進行實時路徑規(guī)劃。本文采用任務(wù)分時切換的方法[15]解決這一問題，即令自主避障任務(wù)的優(yōu)先級高于編隊集結(jié)任務(wù)，當(dāng)無人艇完成自主避障后，重新執(zhí)行編隊集結(jié)任務(wù)，進而達到任務(wù)分時切換的目的。同時，無人艇在編隊航行過程中不僅需要解決環(huán)境中障礙物的規(guī)避，還需要考慮編隊內(nèi)部成員的避碰，這要求無人艇需要擁有動態(tài)障礙物規(guī)避的能力。本文設(shè)計了一個基于人工勢場的MPC 控制器實現(xiàn)無人艇自主避障。

3.2.1 引力勢場

設(shè)無人艇在海面的位置坐標(biāo)為P=(x,y)T，目標(biāo)點的坐標(biāo)為Pg=(xg,yg)T，在到達目標(biāo)點之前，無人艇將一直受到引力的作用，與目標(biāo)點之間的距離越遠，無人艇所受到的引力就越強。所以，目標(biāo)點產(chǎn)生的引力勢場函數(shù)Uatt可表示為

式中，katt表示引力勢場常數(shù)，d(P,Pg)=P-Pg表示無人艇與目標(biāo)點之間的歐幾里得距離。在引力勢場中，無人艇受到的引力Fatt等于Uatt的負梯度，故Fatt可表示為

引力Fatt的方向沿著無人艇和目標(biāo)點之間連線的方向指向目標(biāo)點。

3.2.2 斥力勢場

APF 中的斥力勢場存在一定作用范圍，只有當(dāng)無人艇進入斥力場范圍內(nèi)才會受到斥力作用。為了使勢場空間更符合無人艇的運動特性，本文對斥力勢場的作用范圍和具體形式進行了修改，具體過程如下。

式中，PΩ= (m,n)表示北東坐標(biāo)系下的點坐標(biāo)。為方便表達，將斥力場作用范圍記為Ω，此時可以得到

其 中，F(xiàn)1= [x+ccos(ψ),y+csin(ψ)]T和F2=[x-ccos(ψ),y-csin(ψ)]T分別為斥力勢場的兩個焦點。

由于斥力勢場需要擁有更大的下降梯度才能提升無人艇航行的安全性，故使用指數(shù)函數(shù)構(gòu)造斥力場，改進后的斥力勢場函數(shù)具體形式為

式中，krep表示斥力勢場常數(shù)。在斥力勢場中，無人艇受到的引力Frep等于Urep的負梯度，故Frep可表示為

斥力Frep方向沿著無人艇和障礙物之間連線的方向指向無人艇。

3.2.3 合力勢場

假設(shè)空間中存在M個障礙物，即 {Po1,Po2,…PoM}?Ωo，其中Ωo表示障礙物區(qū)域。無人艇在向目標(biāo)點移動的過程中會受到引力勢場和斥力勢場共同的作用，所以合力勢場函數(shù)U(P,Ωo)可表示為

進而，無人艇在勢場中受到的合力可表示為

3.2.4 自主避障控制器

無人艇自主避障的MPC 控制器與3.1 節(jié)中提到的DMPC 控制器類似，也可以用式(11)表示。但是，自主避障控制器中Qa=0，即在計算過程中不需要考慮假設(shè)狀態(tài)空間Sa(k)。同時，期望狀態(tài)空間Sd(k)由APF 算法得出的路徑規(guī)劃結(jié)果轉(zhuǎn)換而成，即對規(guī)劃路徑進行采樣，根據(jù)采樣點的坐標(biāo)生成對應(yīng)的狀態(tài)空間信息。單無人艇自主避障控制流程如下：

（1）當(dāng)障礙物進入第i艘無人艇斥力場范圍內(nèi)時，第i艘無人艇與編隊解耦，不再向鄰接集合Ai內(nèi)無人艇發(fā)送Sia(k)；

（2）以當(dāng)前無人艇位置為起點，以編隊目標(biāo)為終點，進行實時路徑規(guī)劃，并且每次規(guī)劃的步長與MPC 算法預(yù)測步長相同；

（3）將規(guī)劃路徑轉(zhuǎn)化為期望狀態(tài)信息Sid(k)，執(zhí)行MPC 控制器，得到下一時刻狀態(tài)Si(k+1)；

（4）令k=k+ 1，返回步驟（2），直到第i艘無人艇周圍不存在障礙物；

（5）第i艘無人艇重新執(zhí)行編隊集結(jié)任務(wù)；

（6）當(dāng)?shù)趇艘跟隨者的期望集合Oi中的所有成員執(zhí)行障礙物規(guī)避任務(wù)時，第i艘跟隨者無法獲取到有價值的期望信息，即Sid(k)=?，此時第i艘跟隨者使用進行自主巡航，當(dāng)再次夠獲取Sid(k)時，第i艘跟隨者回歸編隊。

綜上所述，無人艇編隊運動控制流程如圖4所示。

圖4 無人艇編隊運動控制流程Fig. 4 Process of USV formation motion control

4 仿真結(jié)果及分析

4.1 實驗背景

假設(shè)編隊由9 艘無人艇組成，其中1 艘無人艇為領(lǐng)航者，其他為跟隨者，編隊處于理想環(huán)境下，不受海風(fēng)、海浪、海流的影響；各無人艇以Pc,0=(0,0)T為中心組成圓形編隊，編隊中各無人艇初始狀態(tài)如表 2 所示；有效通信距離dc=100 m，則無人艇編隊通信拓撲結(jié)構(gòu)如圖5 所示，紅色虛線表示無人艇能夠接收領(lǐng)航者（優(yōu)先級為1）的信息，藍色虛線表示能夠接收優(yōu)先級為2的跟隨者的信息，其中跟隨者5～8 無法獲取領(lǐng)航者的假設(shè)狀態(tài)空間(k)。無人艇穩(wěn)定狀態(tài)下最大縱移速度為ustable= 1.2 m/s，預(yù)測步長Np= 10；區(qū)域海圖信息已知，在區(qū)域中存在一個半徑r=110 m 的島嶼；同時，區(qū)域中存在4 個個半徑r=5 m 的圓形障礙物，由于障礙物尺寸過小，無人艇編隊在航行前沒有關(guān)于這4 個障礙物的先驗信息，障礙物坐標(biāo)如表3 所示。同時，APF 中斥力場相關(guān)參數(shù)設(shè)置如下：a=20 m，b=12 m。

表2 無人艇初始狀態(tài)Table 2 Initial state of UAVs

表3 障礙物坐標(biāo)Table 3 Coordinate obstacles

圖5 無人艇編隊單向拓撲結(jié)構(gòu)Fig. 5 Unidirectional topologies structure of USV formation

假設(shè)領(lǐng)航者在考慮編隊隊形和島嶼對編隊的航行的影響后，完成自身航行的全局路徑規(guī)劃。所以，本次實驗要求無人艇編隊中心從Pc,0=(0,0)T開始，在順利規(guī)避小型障礙物的基礎(chǔ)上，保持編隊隊形使得編隊中心順利到達Pc,g=(50 m, 450 m)T。

4.2 實驗結(jié)果分析

圖6 展示了無人艇在保證自身安全航行的前提下，保持編隊隊形，使編隊中心從Pc,0=(0,0)T移動至Pc,g=(50 m,450 m)T的軌跡。從圖中可以發(fā)現(xiàn)，無人艇編隊從初始狀態(tài)出發(fā)，在領(lǐng)航者的帶領(lǐng)下向目的地移動。在編隊航行過程中，當(dāng)障礙物進入斥力場范圍內(nèi)時，跟隨者與編隊解耦，執(zhí)行自主避障任務(wù)；當(dāng)避障結(jié)束后，無人艇再次回歸編隊，保持編隊穩(wěn)定航行。

圖6 無人艇編隊運動軌跡Fig. 6 Track of USV formation

圖7 展示了在本次仿真實驗中，無人艇實際位置與保持編隊隊形的期望位置之間的相對距離。雖然各無人艇的初始位置已經(jīng)滿足編隊隊形要求，但是無人艇之間的航向角和速度狀態(tài)與領(lǐng)航者不同。所以，各跟隨者在[0,30 s]內(nèi)調(diào)整自身狀態(tài)，直到各跟隨者狀態(tài)si(k)與領(lǐng)航者的狀態(tài)sL(k)趨于一致。在[31s,500 s]內(nèi)，編隊內(nèi)部的跟隨者未受到小型障礙物的影響，無人艇編隊能夠保持穩(wěn)定的編隊隊形，按照領(lǐng)航者所規(guī)劃的軌跡穩(wěn)定航行。在[501s,720 s]內(nèi)，無人艇編隊內(nèi)部成員受到障礙物1、2、4 的影響，所以編隊中部分跟隨者與期望位置之間的相對距離發(fā)生改變。在[721s,823 s]內(nèi)，無人艇編隊保持編隊隊形穩(wěn)定航行，在k= 824 s 時，無人艇編隊中心到達Pc,g=(50 m,450 m)T。

圖7 跟隨者與期望位置之間的相對距離Fig. 7 Relative distance between follower and desired position

圖8～13 展示了本次仿真實驗中各無人艇航向角、速度狀態(tài)（縱移速度、橫移速度、艏搖角速度）以及控制量（左、右噴水推進器輸出推力）隨時間變化的曲線。從圖中可知，跟隨者在[0,30s]內(nèi)調(diào)整各狀態(tài)量，使得狀態(tài)空間si(k)不斷趨近于領(lǐng)航者的狀態(tài)空間sL(k)；[30 s, 500 s]內(nèi)無人艇編隊保持穩(wěn)定航行，編隊內(nèi)成員的航向角和速度狀態(tài)保持一致；[510 s,720 s]內(nèi)無人艇編隊受到小型障礙物的影響，在這段時間內(nèi)，解耦無人艇的狀態(tài)量變化較為劇烈，編隊內(nèi)其他跟隨者狀態(tài)量基本與領(lǐng)航者保持一致。在[721s,824 s]內(nèi)，無人艇編隊保持編隊隊形穩(wěn)定航行，編隊的內(nèi)部成員的狀態(tài)量再次趨于一致。

圖8 無人艇航向角Fig. 8 Heading angle of USVs

圖9 無人艇縱移速度Fig. 9 Surge velocity of USVs

圖10 無人艇橫移速度Fig. 10 Sway velocity of USVs

圖11 無人艇艏搖角速度Fig. 11 Yaw angular velocity of USVs

圖12 無人艇左推進器輸出推力Fig. 12 Left propeller output thrust of USVs

圖13 無人艇右推進器輸出推力Fig. 13 Right propeller output thrust of USVs

5 結(jié) 論

本文針對編隊內(nèi)部分成員無法獲得保持編隊平衡的先驗信息條件下，多水面無人艇編隊運動控制問題展開了研究。設(shè)計提出了一種基于APF和MPC 算法的無人艇自主避障和多無人艇編隊控制方案。在本方案中，首先通過設(shè)定通信優(yōu)先級和單向拓撲結(jié)構(gòu)，將DMPC 算法引入假設(shè)狀態(tài)空間和平衡狀態(tài)空間，使用SQP 算法求解預(yù)測狀態(tài)空間與假設(shè)狀態(tài)空間和平衡狀態(tài)空間之間的最小代價以完成跟隨者無人艇控制器設(shè)計。其次為保證水面無人艇避障安全性，通過將改進APF 和MPC 算法相結(jié)合，實現(xiàn)了無人艇自主避障控制。數(shù)值仿真結(jié)果證明了本方案的有效性。

然而，本文假設(shè)編隊處于理想環(huán)境下，不受海風(fēng)、海浪、海流的影響，因此在未來的研究中，將重點關(guān)注面向時變洋流和動態(tài)威脅的多水面無人艇編隊運動控制技術(shù)；此外，未來將進一步完善仿真驗證場景，并計劃利用水面無人艇編隊模擬器等仿真平臺對相關(guān)算法進行仿真驗證。