未知環境下多AUV協同避障方法研究

摘要：針對多AUV（autonomous underwater vehicle）系統在未知環境中進行路徑規劃時難以兼顧避障與編隊的問題，提出了一種基于領航—跟隨者與行為的多AUV協同避障方法。首先，通過構造碰撞危險度及偏離目標評價函數，設計了AUV局部路徑規劃方法；在此基礎上，結合編隊控制方法，分別為領航者和跟隨者設計不同的行為以及行為選擇模式。半物理仿真實驗結果表明，該算法能夠實現多AUV系統在未知環境中的協同避障，且隊形偏離度與恢復隊形時間優于傳統多機器人避障算法。實驗結果證明了該算法的可行性與有效性。

關鍵詞：路徑規劃；多AUV；協同避障；領航—跟隨者；行為

中圖分類號：TP242文獻標志碼：A

文章編號：1001-3695（2022）10-006-2929-06

doi：10.19734/j.issn.1001-3695.2022.03.0115

Research on multi-AUV cooperative obstacle avoidance method in unknown environment

Liu Ya1，2，3，Zeng Junbao1，2

（1.State Key Laboratory of Robotics，Shenyang Institute of Automation，Chinese Academy of Sciences，Shenyang 110016，China；2.Institutes for Robotics amp; Intelligent Manufacturing，Chinese Academy of Sciences，Shenyang 110169，China； 3.University of Chinese Academy of Sciences，Beijing 100049，China）

Abstract：Aiming at the problem that it is difficult to take into account both obstacle avoidance and formation when multi-AUV system plans path in unknown environment，this paper proposed a multi-AUV cooperative obstacle avoidance method based on leader-follower and behavior.Firstly，this paper designed a local path planning method for AUV by constructing the evaluation function of collision risk and deviation from the target.On this basis，combined with formation control method，this paper designed different behaviors and behavior selection patterns for leader and followers respectively.The semi-physical simulation experiment results show that the algorithm can realize the cooperative obstacle avoidance of multi-AUV system in unknown environment，and the formation deviation and recovery time are better than traditional multi-robot obstacle avoidance algorithms.Experimental results verify the feasibility and effectiveness of the algorithm.

Key words：path planning；multi-AUV；cooperative obstacle avoidance；leader-follower；behavior

0引言

多機器人系統相較于單機器人而言，具有靈活性好、探索范圍廣、魯棒性強等優點，并且能夠完成單機器人難以完成的任務。AUV是探索與開發海洋的重要工具，隨著對海洋環境研究的不斷深入，多AUV系統開始逐漸應用于目標搜探、協同定位等領域。在多機器人系統中，編隊控制作為一個重要問題被廣泛研究。目前主要的編隊控制方法包括領航—跟隨者方法［1］、基于行為的方法［2］、人工勢場法［3］、虛擬結構法［4］等。

在多機器人系統執行任務的過程中，由于缺少對環境的先驗知識，提前規劃好的路徑可能會出現障礙物。機器人需要實時規劃路徑以躲避障礙物，同時還要避免因路線發生改變而與其他機器人碰撞。除此之外，機器人之間的距離不能過遠，否則可能會影響機器人之間的通信，甚至在避障結束后無法恢復編隊。因此，為了保證機器人的安全以及后續任務的正常執行，機器人之間需要通過相互協作，在躲避障礙物的同時兼顧編隊需求。由于系統中的每個機器人受到障礙物位置以及隊形的多方面約束，使得多機器人協同避障問題的處理更為困難。

目前，很多學者在編隊控制方法的基礎上對多機器人協同避障問題展開了研究。文獻［5］在領航—跟隨者隊形結構的基礎上，將編隊整體看做剛性結構，通過為領航者進行路徑規劃以及設置隊形轉向實現障礙物環境下的編隊控制。這種方法雖然將多機器人避障簡化為了單機器人的避障問題，但會使領航者路徑規劃算法的復雜度提高，并且在嚴格保持隊形的情況下，編隊整體無法通過狹窄區域；文獻［6］采用了基于行為的編隊控制方法，為每個機器人設置駛向目標點、避障、沿墻、避碰以及編隊等不同行為，實驗證明了編隊整體能夠順利通過障礙物區域，但每個機器人通過不同行為的加權進行決策，使得隊形不易保持；文獻［7］采用人工勢場法實現了多AUV系統的實時避障，但編隊效果不佳。除此之外，部分學者通過將不同的編隊控制方法相結合（比如將人工勢場法與一致性算法結合［8～10］、領航—跟隨者與人工勢場法結合［11，12］、領航—跟隨者與基于行為方法結合［13，14］）來彌補單一方法的缺陷，提升了多機器人避障的效果。

考慮到障礙物環境的復雜與多樣性以及機器人的能耗問題，一般不將多機器人系統看做剛性結構。在這個前提下，由于每個機器人需要同時考慮編隊約束以及障礙物約束，多機器人系統難以保持原有隊形，需要局部調整位置進行局部隊形變換，或者根據障礙物環境選擇合適的隊形進行整體隊形變換。目前對于整體隊形變換的研究包括障礙物約束下隊形選擇方法［15］、隊形變換的策略［16，17］。文獻［18］引入障礙物密度的概念，機器人通過感知到的障礙物密度局部調整位置，這種方法使多機器人系統呈現出自適應隊形變換的特點，但障礙物密度這一指標實際上難以計算。

基于以上分析，本文將兩種編隊控制方法相結合，提出一種未知環境下基于領航—跟隨者與行為的多AUV協同避障方法。領航者負責隊形選擇以及規劃編隊整體前進路線，每臺AUV通過行為的選擇進行決策，使多AUV系統在避障的同時能夠自適應調整隊形，并且根據環境的需要變換合適的隊形。仿真實驗證明了水下不同場景下該算法的有效性。

1基于前視聲吶的AUV局部路徑規劃方法

機器人的路徑規劃一般分為環境建模與路徑搜索兩部分［19］。目前主要的環境建模方法包括柵格法、幾何表示法以及拓撲表示法等［20］，本文采用幾何學表示的方法。同時，考慮到相較于圓形，多邊形可擬合的輪廓形狀更為廣泛，因此采用障礙物輪廓的最小凸多邊形進行環境建模。前視聲吶模型如圖1所示。

AUV使用前視聲吶對環境進行感知，返回的聲吶圖像中包含探測到的前方環境信息。實驗中采用的前視聲吶水平開角為130°，作用距離L=100 m。將前視聲吶的探測區域平均分成N部分，則每部分Si（i=1，2，…，N）的探測范圍為［-1336π+13π（i-1）18N，-1336π+13πi18N）。

規定劃分的每部分探測區域的中間航向ψi（i=1，2，…，N）為此部分的代表航向，通過建立評價函數對ψi進行評估，以確定AUV下一時刻的航行方向。

當目標點航向上不存在障礙物時，AUV會直接朝著目標點航行；當存在障礙物時，AUV需要改變航向以避免碰撞。某一航向上AUV與障礙物的距離越近，說明沿著該航向航行碰撞危險性越高。因此，評價函數包含兩部分：第一部分為與目標點航向的偏離程度，第二部分為與障礙物碰撞的危險程度。

Goal（ψi）=180-|ψi-ψgoal|（1）

Dist（ψi）=DiDilt;Ds

LDi≥Ds （2）

其中：ψgoal為目標點航向；Di為ψi方向上與障礙物的距離；Ds為設置的距離閾值。

在計算總評價函數之前，需要對評價函數中的兩部分分別進行歸一化處理：

Goaln（ψi）=Goal（ψi）∑Nj=1Goal（ψj）（3）

Distn（ψi）=Dist（ψi）∑Nj=1Dist（ψj）（4）

根據式（5）對待評估航向進行評分，分值最高的航向為當前時刻的決策航向。

Evaluate（ψi）=α×Goaln（ψi）+β×Distn（ψi）（5）

2多AUV協同避障方法

2.1基于領航—跟隨者的編隊控制方法

領航—跟隨者是一種常見的編隊控制方法，主要包括l-l控制及l-φ控制［21］。本文采用l-φ控制方法，即跟隨者需要與領航者保持一定的距離和角度。領航—跟隨者模型如圖2所示。

AUV的位姿采用（x，y，ψ）表示，其中（x，y）為AUV在固定坐標系下的坐標，ψ為AUV的航向角。

如圖2所示，領航者的位姿為（xL，yL，ψL），根據領航者信息及隊形參數可以計算出跟隨者的期望位姿：

xir（t）

yir（t）

ψir（t）=xL（t）+licos（φi+ψL（t））

yL（t）+lisin（φi+ψL（t））

ψL（t）（6）

2.2多AUV協同避障控制架構

本文在領航—跟隨者結構的基礎上，結合基于行為的編隊控制方法，分別為領航者和跟隨者設置不同的行為。領航者的目標點為多AUV系統避障最終目標點，跟隨者的目標點為滿足期望隊形的虛擬目標點。領航者在每一時刻通過環境感知及避障目標點進行行為選擇與決策，規劃編隊整體前進路線，同時進行隊形選擇，并將當前位姿與隊形參數發送給跟隨者；跟隨者實時計算虛擬目標點，并結合自身的感知信息進行行為選擇與決策。多AUV協同避障控制架構如圖3所示。

2.3領航者行為設置與隊形選擇

根據提出的多AUV協同避障控制架構，本節分別對領航者的行為設置、行為選擇及隊形選擇方法進行說明。

為保證AUV的平穩運行，規定領航者在避障過程中的速度大小u保持不變。為領航者設置駛向目標點以及避障兩種行為。設領航者的決策速度為VL。

VL=J1J2Vmove_to_goalVavoid_obstacle（7）

其中：J為行為選擇矩陣，每一時刻其中的元素只有一個為1，其余為0；Vmove_to_goal與Vavoid_obstacle分別為駛向目標點行為和避障行為的決策速度。

1）駛向目標點行為駛向目標點行為的決策速度可以表示為

Vmove_to_goal=uρ（X，Xg）xg-xyg-y（8）

其中：（xg，yg）為目標點坐標；（x，y）為當前位置坐標；ρ（X，Xg）為當前位置與目標點的歐氏距離。

2）避障行為

領航者的避障方法已在本文第1章中詳細闡述。為避免在避障過程中跟隨者之間發生碰撞，增大領航者與障礙物之間的安全距離dL。設跟隨者與障礙物之間的安全距離為d，跟隨者之間允許的最小距離為dmin，則設置

dL=d+dmin2（9）

避障行為的決策可以表示為

Vavoid_obstacle=ucos ψsin ψ（10）

其中：ψ為局部路徑規劃方法的決策航向。

3）行為選擇

領航者行為選擇方法如下：

a）判斷前視聲吶視野內是否存在障礙物，若不存在，執行駛向目標點行為；否則，轉向步驟b）；

b）計算目標點航向與障礙物的距離D，若小于所設定的距離閾值Ds，執行避障行為；否則，執行駛向目標點行為。

領航者從兩障礙物間通行如圖4所示。

4）隊形選擇

共設置三角形（原隊形）以及一字形兩種隊形。若領航者的決策航向在當前位置與兩障礙物切線之間，認為領航者決定從兩障礙物之間通行（圖4），此時計算兩個障礙物之間的最近距離Dmin。若Dmin≥dmin，選擇三角形隊形；否則選擇一字隊形。

Dmin=mindist{obstacle1obstacle2}（11）

2.4跟隨者行為設置與行為選擇

根據提出的多AUV協同避障控制架構，跟隨者需根據領航者發送的位姿信息、隊形參數計算當前時刻的虛擬目標點，并結合感知信息進行行為的選擇與決策。因此，本節對跟隨者的行為設置及行為選擇進行說明。

為跟隨者設置四種不同的行為，分別為駛向目標點行為、避障行為、保持行為以及跟隨行為。不同于領航者，跟隨者具有編隊以及隊形變換需求，因此在避障過程中速度大小發生改變。設跟隨者的決策速度為VF。

VF=J1J2J3J4Vmove_to_goal

Vavoid_obstacle

Vkeep

Vfollow（12）

其中：J為行為選擇矩陣；Vmove_to_goal、Vavoid_obstacle、Vkeep、Vfollow分別對應四種行為的決策速度。

1）駛向目標點行為

駛向目標點行為的決策可以表示為

Vmove_to_goal=u+ερ（X，Xvg）ρ（X，Xvg）xvg-xyvg-y（13）

其中：（xvg，yvg）為虛擬目標點坐標；（x，y）為當前位置坐標；ρ（X，Xvg）為當前位置與虛擬目標點的歐氏距離；u為領航者速度大小；ε為速度增益。

2）避障行為

跟隨者的避障方法已在本文第1章中詳細闡述。當跟隨者執行避障行為時，由于偏離了虛擬目標點方向，所以無法保持編隊隊形。這種情況下，出于以下兩點的考慮，跟隨者與領航者之間保持距離不變，只改變角度是較為理想的情況：a）保證領航者與跟隨者的通信距離；b）保證跟隨者與領航者之間不會發生碰撞。

如圖5所示，建立以領航者為中心，半徑為l的圓，l為跟隨者與領航者的編隊距離。當決策航向所在的直線與圓有交點時，與跟隨者距離較近的交點為跟隨者的當前目標；當決策航向所在的直線與圓無交點時，擴大圓的半徑直至相切，切點為跟隨者的當前目標。

避障行為的決策可以表示為

Vavoid_obstacle=（u+ερ（X，Xc））cos ψsin ψ|a（X，Xc）-ψ|=0

（u-ερ（X，Xc））cos ψsin ψ|a（X，Xc）-ψ|=π（14）

其中：ρ（X，Xc）為當前位置與當前目標點的歐氏距離；ψ為局部路徑規劃算法決策航向；a（X，Xc）為從當前位置指向當前目標點的射線與x軸的夾角。

3）保持行為

在l-φ的編隊控制結構下，當領航者航向角發生改變時，跟隨者的虛擬目標點位置會發生突變。對位于領航者轉彎內側的跟隨者來說，目標點可能會位于其后方。此時使跟隨者轉彎以跟隨后方目標點顯然是不合理的，保持行為設置其維持當前航向，并減速等待目標點。保持行為的決策可以表示為

Vkeep=（u-ερ（X，Xvg））cos ψcsin ψc（15）

其中：ρ（X，Xvg）為當前位置與虛擬目標點的歐氏距離；ψc為AUV當前航向。

4）跟隨行為

當領航者發布隊形為一字隊形時，說明前方可通行的寬度較小。此時，在形成編隊后，跟隨者直接跟蹤領航者的軌跡，即跟隨者將領航者發送的位置信息保存，作為自己的目標點序列。

5）行為選擇

跟隨者行為選擇如下：

a）若當前隊形為一字形且已經形成編隊，執行跟隨行為；否則轉向步驟b）。

b）判斷虛擬目標點位于當前位置的前方或后方，若為前方，轉向步驟c）；否則轉向步驟d）。

c）計算目標航向與障礙物的最近距離D，若小于設置的閾值Ds1，執行避障行為；否則執行駛向目標點行為。

d）計算當前航向與障礙物的最近距離D′，若小于設置的閾值D′s，執行避障行為（當前航向作為避障算法的目標航向）；否則執行保持行為。

需要說明的是，在領航者發布變換隊形的指令后，為了快速形成編隊，跟隨者的行為選擇方式有所不同。此時，若當前位置與目標點之間無障礙物，選擇駛向目標點行為，等待新的編隊形成后，恢復上述的行為選擇方式。

3仿真實驗

為證明算法的可行性，本文采用視景平臺以及真實的水下機器人控制軟件進行了仿真實驗，包括單臺AUV的避障實驗以及三臺AUV協同避障實驗。如圖6、7所示，視景平臺在實際的多波束測深數據、海圖數據等的基礎上構建了逼真的三維場景，可以設置多臺AUV并接收顯示AUV的實時位姿。同時，視景平臺還可以設置前視聲吶的作用距離，并根據AUV在前視聲吶探測范圍內不同角度距障礙物的距離實時模擬并返回前視聲納圖像，這與真實前視聲吶的成像原理類似。平臺可以設置不規則形狀、長方體、圓柱體三種不同類型的障礙物。水下機器人控制軟件能夠按照一定的頻率模擬AUV之間的通信。實驗中每臺AUV均在100 m深度定深航行。視景平臺如圖6所示。前視聲吶圖像如圖7所示。

每臺AUV的仿真框架如圖8所示。在視景平臺上設置障礙物，平臺會回傳每個AUV的前視聲吶信息；每個AUV根據感知信息以及目標點按照本文算法進行決策，得到速度指令V；分別將速度大小及速度方向（航向）傳給對應的PID控制器，得到相應的控制量；將控制量傳入AUV模型，從而得到更新后的位姿；最后將位姿傳給視景平臺進行實時更新顯示。這種仿真方式從AUV的環境感知、運動控制及通信等方面較大程度地模擬了真實AUV的情況。

3.1單AUV避障仿真實驗

為驗證本文AUV局部路徑規劃算法，設置單AUV避障仿真實驗。實驗中AUV的速度大小保持不變，設置u=1.5 m/s。分別在離散障礙物場景、不同類型障礙物場景、復雜場景以及海山場景中采用本文算法進行實驗，規劃結果如圖9所示。以距障礙物的最小距離、運行時間、路徑長度以及路徑平滑度作為衡量規劃結果的指標（表1），其中路徑平滑度使用避障過程中航向總變化量計算。實驗結果顯示，在不同場景下AUV均順利通過障礙物區域，并在避障過程中與障礙物保持足夠的安全距離。

為進一步驗證本文算法的有效性，在離散障礙物場景下采用人工勢場法及RRT算法進行規劃，并與本文算法規劃結果進行對比，結果如圖10、表2所示。實驗結果表明，在安全距離相當的情況下，本文算法相較于人工勢場法路徑更為平滑，運行時間更短。RRT算法由于具有較大的隨機性，規劃的路徑較為曲折。

3.2多AUV協同避障仿真實驗

為驗證本文提出的多AUV協同避障算法，采用三臺AUV進行仿真實驗。算法參數的選取如表3所示。

如圖11所示，實驗中三臺AUV的期望隊形為三角形隊形，變換隊形為一字隊形。隊形參數的選取如表4所示。

3.2.1算法仿真

根據上述的算法參數及隊形參數設置，在三種不同的場景下進行仿真實驗，分別為離散障礙物場景、簡單場景以及復雜場景。場景設置及實驗結果分別如圖12～14所示。采用平均隊形偏離度及恢復隊形時間作為衡量規劃結果的指標。其中隊形偏離度δ由當前隊形與期望隊形計算確定：

δ=∑ni=1di-li（16）

其中：di為當前時刻AUV間距離；li為期望隊形AUV間距離。恢復隊形時間定義為從最后一臺AUV避障結束開始，到隊形偏離度小于7.5時所用時間。

從實驗結果可以看出，領航者規劃編隊整體前進路線，跟隨者通過與領航者進行信息交互及環境感知躲避障礙物，從而在避障過程中兼顧編隊需求。從隊形變化中可以看出，多AUV系統能夠根據障礙物情況自主壓縮隊形，呈現出自適應隊形變換的特點，且AUV之間能夠保持一定的距離，保證AUV的安全。多AUV系統在避障結束后能夠快速恢復原始隊形，繼續在原定航線上執行任務。不同場景下協同避障統計如表5所示。

為驗證算法提出的隊形變換以及跟隨行為，設置一條狹窄水道場景，水道中五部分的寬度分別為60 m、40 m、28 m、23 m、28 m，多AUV協同避障實驗結果如圖15所示。

實驗結果顯示，多AUV通過狹窄水道可分為以下幾個階段：

a）在60 m寬的水道中，編隊保持原隊形即可安全通過，當跟隨者發現航行前方存在障礙物且距離小于設定的閾值時，開始躲避障礙物。

b）在40 m寬的水道中，兩個跟隨者均需要避障，隊形寬度壓縮。當領航者計算前方可通行寬度小于編隊可壓縮最小寬度時，發布變換一字隊形命令，并將自身與障礙物之間的安全距離調整為10 m。

c）當一字隊形形成后，跟隨者執行跟隨行為，將領航者的軌跡點作為自己的目標點，進行軌跡跟蹤。

d）當領航者聲吶視野內不存在障礙物時，開始駛向避障目標點。考慮到跟隨者的安全，領航者在40 s后發布恢復隊形的命令，跟隨者變換為三角形隊形。

e）到達避障目標點后，多AUV繼續在原定航線上編隊執行任務。

跟隨者執行跟隨行為如圖16所示。

3.2.2對比實驗

為進一步驗證本文提出的多AUV協同避障算法的有效性，將其與目前常用于障礙物環境下編隊控制的人工勢場法進行對比，結果如圖17所示。

表6顯示，在多AUV避障過程中，本文算法相較于人工勢場法具有更小的平均隊形偏離度，并且在避障結束后能夠更快速恢復隊形，有利于多AUV系統后續任務的執行。

在目前的研究中，一部分學者認為當多機器人系統遇到障礙物時，避障的優先級相較于編隊更高，多機器人系統應當解除編隊，在躲避障礙物后恢復隊形。基于此，將本文算法與編隊拆分避障方法進行對比，結果如圖18所示。

由于編隊拆分避障不具有隊形偏離度的概念，采用AUV間距離進行對比（表7）。在實驗場景中，采用編隊拆分避障方法AUV各自進行避障，選擇從障礙物兩側繞行，導致AUV距離過遠，甚至造成失聯。由于AUV距離過遠，避障結束后要花費較長時間恢復隊形。相較于編隊拆分避障，本文算法在保證AUV之間保持一定距離的前提下，將AUV之間的最遠距離維持在編隊距離附近，能夠在避障結束后快速恢復編隊。

3.2.3算法可擴展性實驗

上述實驗對不同場景下三臺AUV協同避障進行了算法驗證，為驗證算法的可擴展性，將AUV數量增加至五臺，分別在簡單場景及復雜場景中進行實驗。五臺AUV的編隊隊形及隊形參數分別如圖19、表8所示。

算法參數的選取與三臺AUV系統相同，實驗結果如圖20、21所示。實驗結果顯示，本文算法在不同場景下應用于五臺AUV系統均取得了良好的效果。

4結束語

本文基于前視聲吶提出了一種AUV局部路徑規劃方法，并在此基礎上，結合編隊控制方法，提出了一種未知環境下多AUV協同避障方法。行為的設置不僅考慮了AUV與障礙物的碰撞，還考慮了AUV之間的相互碰撞。設計了避障行為下目標點的計算方法，使跟隨者在避障過程中與領航者的距離基本維持在編隊距離，以保證AUV的通信與安全。水下多種場景的仿真實驗均取得了良好的效果，證明本文方法能夠實現多AUV在未知環境下的協同避障，兼顧避障過程中的編隊需求，在避障結束后能夠快速恢復編隊，并具有可擴展性。未來的工作重點是依托實驗室的AUV進行實物驗證。

參考文獻：

［1］Cruz J.Leader-follower strategies for multilevel systems［J］.IEEE Trans on Automatic Control，1978，23（2）：244-255.

［2］Brooks R.A robust layered control system for a mobile robot［J］.IEEE Journal on Robotics and Automation，1986，2（1）：14-23.

［3］Khatib O.Real-time obstacle avoidance for manipulators and mobile robots［J］.International Journal of Robotics Research，1986，5（1）：90-98.

［4］Tan K H，Lewis M A.Virtual structures for high-precision cooperative mobile robotic control［C］//Proc of IEEE/RSJ International Confe-rence on Intelligent Robots and Systems.Piscataway，NJ：IEEE Press，1996：132-139.

［5］王樂樂，眭澤智，蒲志強，等.一種改進RRT的多機器人編隊路徑規劃算法［J］.電子學報，2020，48（11）：2138-2145.（Wang Lele，Sui Zezhi，Pu Zhiqiang，et al.An improved RRT algorithm for multi-robot formation path planning［J］.Acta Electronica Sinica，2020，48（11）：2138-2145.）

［6］Xu Dongdong，Zhang Xingnan，Zhu Zhangqing，et al.Behavior-based formation control of swarm robots［J］.Mathematical Problems in Engineering，2014，2014：1-13.

［7］徐博，張嬌，王超.一種基于人工勢場多AUV集群的實時避障方法［J］.中國艦船研究，2018，13（6）：66-71.（Xu Bo，Zhang Jiao，Wang Chao.A real-time obstacle avoidance method for multi-AUV cluster based on artificial potential field［J］.Chinese Journal of Ship Research，2018，13（6）：66-71.）

［8］Wang Ning，Dai Jiyang，Ying Jin.UAV formation obstacle avoidance control algorithm based on improved artificial potential field and consensus［J］.International Journal of Aeronautical and Space Sciences，2021，22（6）：1413-1427.

［9］張佳龍，閆建國，張普，等.基于一致性算法的無人機協同編隊避障研究［J］.西安交通大學學報，2018，52（9）：168-174.（Zhang Jialong，Yan Jianguo，Zhang Pu，et al.Collision avoidance of unmanned aerial vehicle formation based on consensus control algorithm［J］.Journal of Xi’an Jiaotong University，2018，52（9）：168-174.）

［10］付雷，秦一杰，何頂新，等.基于改進人工勢場法的多機器人編隊避障［J］.控制工程，2022，29（3）：388-396.（Fu Lei，Qin Yijie，He Dingxin，et al.Obstacle avoidance in multi-robot formation based on improved artificial potential field［J］.Control Engineering of China，2022，29（3）：388-396.）

［11］Feng Yunduo，Wu Yanxuan，Cao Haozhe，et al.UAV formation and obstacle avoidance based on improved APF［C］//Proc of the 10th International Conference on Modelling，Identification and Control.Piscataway，NJ：IEEE Press，2018：1-6.

［12］陳駿嶺，秦小麟，李星羅，等.基于人工勢場法的多機器人協同避障［J］.計算機科學，2020，47（11）：220-225.（Chen Junling，Qin Xiaolin，Li Xingluo，et al.Multi-robot collaborative obstacle avoidance based on artificial potential field method［J］.Computer Science，2020，47（11）：220-225.）

［13］Lee G，Chwa D.Decentralized behavior-based formation control of multiple robots considering obstacle avoidance［J］.Intelligent Ser-vice Robotics，2018，11（1）：127-138.

［14］常路，單梁，戴躍偉，等.未知環境下基于改進DWA的多機器人編隊控制［J/OL］.控制與決策.（2021-08-02）［2022-01-20］.http：//doi.org/10.13195/j.kzyjc.2020.1817.（Chang Lu，Shan Liang，Dai Yuewei，et al.Multi-robot formation control in unknown environment based on improved DWA［J/OL］.Control and Decision.（2021-08-02）［2022-01-20］.http：//doi.org/10.13195/j.kzyjc.2020.1817.）

［15］任立敏，王偉東，杜志江，等.障礙環境下多移動機器人動態優化隊形變換［J］.機器人，2013，35（5）：535-543.（Ren Limin，Wang Weidong，Du Zhijiang，et al.Dynamic and optimized formation swi-tching for multiple mobile robots in obstacle environments［J］.Robot，2013，35（5）：535-543.）

［16］Vilca J M，Adouane L，Mezouar Y.Adaptive leader-follower formation in cluttered environment using dynamic target reconfiguration［C］//Proc of the 12th International Symposium on Distributed Autonomous Robotic Systems.Berlin：Springer，2016：237-254.

［17］吳軍成，肖宇峰，霍建文.不確定環境下多機器人編隊控制研究［J］.計算機應用研究，2021，38（4）：1123-1127.（Wu Juncheng，Xiao Yufeng，Huo Jianwen.Research on multi-robot formation control in uncertain environment［J］.Application Research of Compu-ters，2021，38（4）：1123-1127.）

［18］Wasik A，Pereira J N，Ventura R，et al.Graph-based distributed control for adaptive multi-robot patrolling through local formation transformation［C］//Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems.Piscataway，NJ：IEEE Press，2016：1721-1728.

［19］郭銀景，孟慶良，孔芳，等.AUV路徑規劃算法研究現狀與展望［J］.計算機科學與探索，2020，14（12）：1981-1994.（Guo Yinjing，Meng Qingliang，Kong Fang，et al.Research status and prospect of AUV path planning algorithms［J］.Journal of Frontiers of Computer Science and Technology，2020，14（12）：1981-1994.）

［20］隋玲玲.復雜未知環境下機器人路徑規劃算法研究［D］.上海：復旦大學，2011.（Sui Lingling.Research on robot path planning algorithm in complex and unknown environment［D］.Shanghai：Fudan University，2011.）

［21］Desai J P，Ostrowski J P，Kumar V.Controlling formations of multiple mobile robots［C］//Proc of IEEE International Conference on Robo-tics and Automation.Piscataway，NJ：IEEE Press，1998：2864-2869.

收稿日期：2022-03-04；修回日期：2022-05-18基金項目：國家自然科學基金資助項目（42176194）

作者簡介：劉亞（1999-），女，山東菏澤人，碩士研究生，主要研究方向為水下機器人控制；曾俊寶（1983-），男（通信作者），甘肅白銀人，研究員，碩導，博士，主要研究方向為水下機器人控制（zengjb@sia.cn）．