人工勢(shì)場(chǎng)和虛擬結(jié)構(gòu)相結(jié)合的多水下機(jī)器人編隊(duì)控制

潘無為， 姜大鵬， 龐永杰， 李岳明， 張強(qiáng)

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

人工勢(shì)場(chǎng)和虛擬結(jié)構(gòu)相結(jié)合的多水下機(jī)器人編隊(duì)控制

潘無為， 姜大鵬， 龐永杰， 李岳明， 張強(qiáng)

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

傳統(tǒng)的多水下機(jī)器人(AUV)編隊(duì)算法，例如領(lǐng)航跟隨法、虛擬結(jié)構(gòu)法，對(duì)編隊(duì)形成過程中AUV間的避碰問題和編隊(duì)行進(jìn)過程中的避障問題，沒有進(jìn)行有效解決。人工勢(shì)場(chǎng)法可利用勢(shì)函數(shù)進(jìn)行避碰、避障，但隊(duì)形組織能力略顯不足。針對(duì)上述問題，提出了一種人工勢(shì)場(chǎng)和虛擬結(jié)構(gòu)相結(jié)合的多AUV編隊(duì)控制算法。將系統(tǒng)分為3個(gè)部分：編隊(duì)參考點(diǎn)、虛擬結(jié)構(gòu)質(zhì)點(diǎn)和AUV. 以編隊(duì)參考點(diǎn)為中心形成期望的虛擬結(jié)構(gòu)以組織隊(duì)形；虛擬結(jié)構(gòu)質(zhì)點(diǎn)以期望的虛擬結(jié)構(gòu)為運(yùn)動(dòng)目標(biāo)，并在運(yùn)動(dòng)的過程中，通過人工勢(shì)場(chǎng)斥函數(shù)，實(shí)現(xiàn)避碰和避障；AUV對(duì)虛擬結(jié)構(gòu)質(zhì)點(diǎn)進(jìn)行目標(biāo)跟蹤，從而漸進(jìn)形成AUV的隊(duì)形。通過編隊(duì)路徑跟蹤、編隊(duì)隊(duì)形變換和編隊(duì)避障等一系列仿真實(shí)驗(yàn)，對(duì)算法的可靠性和靈活性進(jìn)行充分驗(yàn)證。實(shí)驗(yàn)結(jié)果表明：在隨機(jī)的初始位置條件下，多AUV系統(tǒng)可以快速無碰撞地形成隊(duì)形；在編隊(duì)行進(jìn)過程中進(jìn)行靈活的隊(duì)形變換，并對(duì)障礙物有效避碰。

兵器科學(xué)與技術(shù)； 多水下機(jī)器人； 編隊(duì)控制； 人工勢(shì)場(chǎng)； 虛擬結(jié)構(gòu)

0 引言

多水下機(jī)器人(AUV)系統(tǒng)，是指由多個(gè)AUV組成的系統(tǒng)，通過協(xié)作完成諸如區(qū)域搜索、地形掃描、環(huán)境數(shù)據(jù)采集等任務(wù)。編隊(duì)問題作為多AUV系統(tǒng)的重要方面，日漸引起了眾多學(xué)者的關(guān)注。目前采用較多的編隊(duì)算法包括：領(lǐng)航跟隨法[1]、虛擬結(jié)構(gòu)法[2-3]、人工勢(shì)場(chǎng)法[4-5]和基于行為法[6-7]。

領(lǐng)航跟隨法采用鏈?zhǔn)降耐負(fù)浣Y(jié)構(gòu)，編隊(duì)具有一個(gè)全局領(lǐng)航者和若干子領(lǐng)航者，跟隨者以一定的位置偏移跟蹤領(lǐng)航者，從而形成隊(duì)形。例如，Cui等[1]提出了參考軌跡和虛擬AUV的概念，采用反步法使AUV的運(yùn)動(dòng)狀態(tài)收斂至虛擬AUV的狀態(tài)，從而形成隊(duì)形。虛擬結(jié)構(gòu)法要求機(jī)器人以剛體上的相應(yīng)點(diǎn)的位置和姿態(tài)信息作為各自的跟蹤目標(biāo)形成隊(duì)形。例如Ren[2]采用信息一致性算法，提出了一種分布式虛擬結(jié)構(gòu)的多智能體編隊(duì)算法；趙寧寧等[3]在Serret-Frenet坐標(biāo)系下采用AUV曲線路徑跟蹤算法，得到了多AUV編隊(duì)路徑跟蹤的算法，實(shí)際上是以曲線參考點(diǎn)形成虛擬結(jié)構(gòu)，AUV在跟蹤虛擬結(jié)構(gòu)的過程中形成編隊(duì)。領(lǐng)航跟隨法和虛擬結(jié)構(gòu)法的缺點(diǎn)在于，算法雖然對(duì)編隊(duì)的保持問題進(jìn)行了研究，但對(duì)于編隊(duì)的形成問題一般未做研究，例如在編隊(duì)形成和編隊(duì)變換過程中AUV之間的避碰問題。另外，算法針對(duì)編隊(duì)行進(jìn)過程中的避障問題，研究也很不充分。

人工勢(shì)場(chǎng)法，通過設(shè)置勢(shì)場(chǎng)函數(shù)，AUV在勢(shì)場(chǎng)的吸引力、排斥力作用下，得到相應(yīng)的隊(duì)形。AUV之間的吸引力用于隊(duì)形的自組織，目標(biāo)點(diǎn)對(duì)AUV的吸引力可使AUV駛向目標(biāo)，排斥力則用于AUV之間的避碰和躲避障礙物。人工勢(shì)場(chǎng)法的缺點(diǎn)在于，隊(duì)形的變化需要對(duì)勢(shì)場(chǎng)函數(shù)的重新配置，因而不夠靈活。例如，Leonard等[4]采用虛擬智能體和人工勢(shì)場(chǎng)相結(jié)合的方法進(jìn)行多智能體的仿真，需要通過調(diào)整虛擬智能體的位置以改變勢(shì)場(chǎng)形狀，進(jìn)而控制智能體的隊(duì)形；Cifuentes等[5]將人工勢(shì)場(chǎng)布置成“溝槽”的形狀，機(jī)器人運(yùn)動(dòng)到“溝槽”中形成隊(duì)形，不同隊(duì)形的變化需要不同形式的“溝槽”。由此可見采用人工勢(shì)場(chǎng)進(jìn)行隊(duì)形的布置實(shí)際上并不簡(jiǎn)便。另外，人工勢(shì)場(chǎng)法需要考慮如何將勢(shì)場(chǎng)力轉(zhuǎn)化為面向具體機(jī)器人模型可用的控制量。

針對(duì)以上不足，本文提出了一種人工勢(shì)場(chǎng)和虛擬結(jié)構(gòu)相結(jié)合的多AUV編隊(duì)控制算法，并做了以下工作：

1)面向多AUV的實(shí)際應(yīng)用，將系統(tǒng)分解為3個(gè)部分：編隊(duì)參考點(diǎn)、虛擬結(jié)構(gòu)質(zhì)點(diǎn)和AUV. 編隊(duì)參考點(diǎn)負(fù)責(zé)隊(duì)形組織；虛擬結(jié)構(gòu)質(zhì)點(diǎn)負(fù)責(zé)避碰和避障，并作為AUV的跟蹤目標(biāo)；AUV則是編隊(duì)的最終實(shí)體。

2)設(shè)計(jì)人工勢(shì)場(chǎng)函數(shù)，實(shí)現(xiàn)避碰和避障。采用向障礙物邊界投影的方法，通過投影點(diǎn)設(shè)置人工勢(shì)場(chǎng)。從而將避障和避碰方法歸于統(tǒng)一。

3)采用AUV目標(biāo)跟蹤算法，跟蹤虛擬結(jié)構(gòu)質(zhì)點(diǎn)，使得多AUV系統(tǒng)最終收斂于期望的隊(duì)形。

4)通過多AUV路徑跟蹤，隊(duì)形變換和編隊(duì)避障等一系列仿真實(shí)驗(yàn)，驗(yàn)證了該算法的可靠性和穩(wěn)定性。

1 問題描述

1.1 坐標(biāo)系

為了描述問題，共建立了3個(gè)坐標(biāo)系(見圖1)，分別是大地坐標(biāo)系OnNE({n})、隨體坐標(biāo)系Obxy({b})和目標(biāo)坐標(biāo)系Otxtyt({t})。 {n}系平面為AUV所在區(qū)域地球表面的切平面，原點(diǎn)On為切點(diǎn)，OnN軸指向北，OnE軸指向東。認(rèn)為{n}系為慣性坐標(biāo)系，AUV在{n}系平面內(nèi)運(yùn)動(dòng)。{b}系原點(diǎn)Ob位于AUV的重心，Obx軸與艇體縱向軸線重合，指向艇艏，Oby軸為Obx軸順時(shí)針旋轉(zhuǎn)90°所得。{t}系原點(diǎn)Ot位于目標(biāo)點(diǎn)處，Otxt軸指向與目標(biāo)點(diǎn)速度向量相同，Otyt軸為Otxt軸順時(shí)針旋轉(zhuǎn)90°所得。

圖1 {n}、{b}和{t}坐標(biāo)系Fig.1 Reference frames {n}, {b} and {t}

1.2AUV模型

對(duì)于在水平面運(yùn)動(dòng)的AUV，其運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)模型[8]可表示為

(1)

2 多AUV編隊(duì)系統(tǒng)設(shè)計(jì)

如圖2所示：黑色圓點(diǎn)表示編隊(duì)參考點(diǎn)，周圍的淺色圓點(diǎn)表示期望的虛擬結(jié)構(gòu)；深色圓點(diǎn)表示虛擬結(jié)構(gòu)質(zhì)點(diǎn)。編隊(duì)參考點(diǎn)作為編隊(duì)構(gòu)成的中心，在其周圍形成期望的虛擬結(jié)構(gòu)隊(duì)形，作為虛擬結(jié)構(gòu)質(zhì)點(diǎn)的期望位置。在初始時(shí)刻，多AUV在一定的范圍內(nèi)隨機(jī)分布，虛擬結(jié)構(gòu)質(zhì)點(diǎn)與AUV的初始位置相同，并向期望的虛擬結(jié)構(gòu)運(yùn)動(dòng)。在運(yùn)動(dòng)過程中，以虛擬結(jié)構(gòu)質(zhì)點(diǎn)為中心，設(shè)置人工勢(shì)場(chǎng)，從而實(shí)現(xiàn)避碰和避障。AUV以虛擬結(jié)構(gòu)質(zhì)點(diǎn)作為跟蹤目標(biāo)，在目標(biāo)跟蹤算法的控制下，最終實(shí)現(xiàn)自身的隊(duì)形。

圖2 多AUV編隊(duì)系統(tǒng)示意圖Fig.2 Schematic diagram of multi-AUV formation system

對(duì)于第i個(gè)AUV，其運(yùn)動(dòng)學(xué)層面上的控制目標(biāo)為，通過給定AUV的期望艏向和期望航速，使AUV的位置收斂至期望的編隊(duì)位置，使多AUV系統(tǒng)形成穩(wěn)定的編隊(duì)隊(duì)形，并在此過程中實(shí)現(xiàn)避碰和避障，保持必要的安全距離。

2.1 虛擬結(jié)構(gòu)的漸進(jìn)生成

虛擬結(jié)構(gòu)質(zhì)點(diǎn)模型為

(2)

式中：qi∈R2，pi∈R2分別為第i個(gè)質(zhì)點(diǎn)在{n}系下的位置和速度；ui∈R2為第i個(gè)質(zhì)點(diǎn)的控制輸入，

ui=c1(qri-qi)+c2(pri-pi)+fi+fi,k,

(3)

認(rèn)為質(zhì)點(diǎn)i處于鄰近其他質(zhì)點(diǎn)產(chǎn)生的勢(shì)場(chǎng)中，則第i個(gè)質(zhì)點(diǎn)所具有的總勢(shì)能可表示為

(4)

式中：φ(x)為勢(shì)能函數(shù)，‖x‖為向量模值。即人工勢(shì)場(chǎng)能為質(zhì)點(diǎn)相對(duì)距離的函數(shù)。由于只考慮將人工勢(shì)場(chǎng)用于避碰，而非組織隊(duì)形，則只需要?jiǎng)輬?chǎng)具有斥力。構(gòu)造勢(shì)函數(shù)(見圖3)為

(5)

式中：k為勢(shì)場(chǎng)強(qiáng)度系數(shù)，其選取應(yīng)考慮AUV的慣性大小和執(zhí)行機(jī)構(gòu)的控制輸出大小；k選取的過小，則不利于安全避碰；k選取的過大，則可能超出執(zhí)行機(jī)構(gòu)的輸出范圍，沒有實(shí)際意義；R為勢(shì)場(chǎng)作用范圍。

圖3 勢(shì)函數(shù)Fig.3 Potential function

為了避免人工勢(shì)場(chǎng)勢(shì)函數(shù)對(duì)編隊(duì)隊(duì)形的干擾，應(yīng)令最終隊(duì)形中質(zhì)點(diǎn)之間的距離大于勢(shì)函數(shù)的作用范圍，即

‖qj-qi‖≥R?i,j∈N且j≠i，

(6)

從而在最終隊(duì)形的某個(gè)鄰域內(nèi)，(3)式右側(cè)的人工勢(shì)場(chǎng)力項(xiàng)可以消去。(2)式、(3)式構(gòu)成一個(gè)線性2階系統(tǒng)：

(7)

由Hurwite判據(jù)可知，應(yīng)令c1>0，c2>0使系統(tǒng)穩(wěn)定。此時(shí)，系統(tǒng)的自然頻率和相對(duì)阻尼系數(shù)分別為

(8)

虛擬結(jié)構(gòu)質(zhì)點(diǎn)作為AUV的跟蹤目標(biāo)，應(yīng)避免劇烈的運(yùn)動(dòng)，以減少AUV的頻繁機(jī)動(dòng)。選擇c1、c2使此2階系統(tǒng)成為一個(gè)過阻尼系統(tǒng)，可令c1較小，c2較大。由于初始時(shí)刻AUV的位置為一定范圍內(nèi)的隨機(jī)分布，個(gè)別AUV的位置可能遠(yuǎn)離編隊(duì)參考點(diǎn)，c1較小可以避免虛擬結(jié)構(gòu)質(zhì)點(diǎn)過快向編隊(duì)參考點(diǎn)運(yùn)動(dòng)，使AUV速度飽和；c2較大則可以令A(yù)UV進(jìn)行速度匹配，便于隊(duì)形的穩(wěn)定形成。

對(duì)于隊(duì)形形成過程中的人工勢(shì)場(chǎng)作用，可以認(rèn)為是對(duì)該2階系統(tǒng)的干擾。人工勢(shì)場(chǎng)負(fù)梯度項(xiàng)的存在，令質(zhì)點(diǎn)向著人工勢(shì)場(chǎng)能減小的方向運(yùn)動(dòng)，從而避免碰撞。

2.2 基于虛擬結(jié)構(gòu)的隊(duì)形變換

相比于人工勢(shì)場(chǎng)法，虛擬結(jié)構(gòu)法更利于組織隊(duì)形。設(shè)編隊(duì)參考點(diǎn)在{n}系下的位置和速度表示為qr∈R2，pr∈R2. 則有如下關(guān)系成立：

(9)

2.3 編隊(duì)避障

對(duì)于編隊(duì)行進(jìn)過程中的避障問題，同樣采用人工勢(shì)場(chǎng)的方法解決。將障礙物以一個(gè)圓形區(qū)域包圍，作為AUV編隊(duì)的避障區(qū)域。當(dāng)編隊(duì)靠近障礙物時(shí)，會(huì)在圓上產(chǎn)生一個(gè)投影點(diǎn)，以投影點(diǎn)為基礎(chǔ)產(chǎn)生人工勢(shì)場(chǎng)，保證編隊(duì)與障礙物具有一定的安全距離。如圖4所示，Ok為第k個(gè)障礙物包圍圓圓心的位置，Rk為包圍圓的半徑。則有投影點(diǎn)的坐標(biāo)qi,k為

(10)

令避障函數(shù)的人工勢(shì)場(chǎng)和2.1節(jié)具有相同的形式，表示為

(11)

圖4 避障示意圖Fig.4 Schematic diagram of obstacle avoidance

2.4AUV目標(biāo)跟蹤算法

在實(shí)際應(yīng)用中，AUV的控制系統(tǒng)多采用解耦的形式，分為規(guī)劃部分和運(yùn)動(dòng)控制部分[9]。規(guī)劃部分根據(jù)作業(yè)任務(wù)的要求，提出期望的路徑點(diǎn)，從而給出AUV的期望艏向和期望速度。運(yùn)動(dòng)控制器則根據(jù)期望的艏向和速度，分別控制舵角和螺旋槳轉(zhuǎn)速。本文在運(yùn)動(dòng)層面給出AUV的目標(biāo)跟蹤算法，而AUV的動(dòng)力學(xué)控制采用文獻(xiàn)[10]中智能AUV的控制算法。

qa=q-qt.

(12)

選取Lyapunov函數(shù)

(13)

對(duì)V求導(dǎo)可得

(14)

選取期望的相對(duì)速度

(15)

則有

(16)

式中：Ua,max>0為最大的相對(duì)跟蹤速度；Δpa>0保證當(dāng)目標(biāo)跟蹤誤差為0時(shí)，分母不為0. 可知在期望的相對(duì)速度下，跟蹤誤差將收斂至0.

圖5 AUV目標(biāo)跟蹤Fig.5 AUV target tracking

根據(jù)向量加法，AUV的期望速度向量為

pd=pt+pa.

(17)

在坐標(biāo)系{t}中跟蹤的縱向誤差s和橫向誤差e可表示為

(18)

式中：χt表示質(zhì)點(diǎn)i的艏向，即速度向量的方向，如圖1所示。

采用等方位角算法進(jìn)行目標(biāo)跟蹤，AUV的期望航向角χd、目標(biāo)點(diǎn)航向角χt和等方位航向角χr，三者的關(guān)系可表示為

χd=χt+χr.

(19)

等方位航向角可表示為

χr=arctan(-e/Δe),

(20)

式中：Δe>0表示前視距離。

由AUV的艏向角ψ、航向角χ和漂角β三者的關(guān)系(見圖1)，可得AUV的期望艏向角可表示為

ψd=χd-β,

(21)

式中：β=arcsin(v/U)；U為AUV的合速度，即速度向量的模值。

由(15)式、(18)式推導(dǎo)，AUV的期望速度Ud可表示為

(22)

式中：Δs>0為控制跟蹤匯合的參數(shù)。可知當(dāng)AUV落后于質(zhì)點(diǎn)時(shí)Ud>Ut，AUV超前于質(zhì)點(diǎn)時(shí)Ud

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

[11]，選取AUV的模型系數(shù)如下：m11=25.80，m22=33.80，m33=2.76，m23=m32=6.20，d11=27.0，d22=17.0，d33=0.5，d23=0.2，d32=0.5. 控制參數(shù)c1=0.005，c2=0.500，k=4，Δe=10，Δs=10. AUV之間的避碰距離為10 m，實(shí)際應(yīng)用中可根據(jù)AUV的主尺度和編隊(duì)間距調(diào)整。AUV的初始位置在300 m×300 m的矩形區(qū)域內(nèi)隨機(jī)產(chǎn)生，艏向在[-π/2,π/2]的范圍內(nèi)隨機(jī)產(chǎn)生。編隊(duì)參考點(diǎn)的初始位置為(150 m,150 m)。

仿真實(shí)驗(yàn)1：3個(gè)AUV進(jìn)行編隊(duì)協(xié)同路徑跟蹤的仿真實(shí)驗(yàn)。編隊(duì)參考點(diǎn)以1 m/s的速度沿OnN軸正方向運(yùn)動(dòng)，之后做圓形軌跡運(yùn)動(dòng)。AUV以編隊(duì)參考點(diǎn)為圓心，30 m為半徑的圓上分布，將圓3等分。AUV編隊(duì)效果見圖6，其中◇表示AUV的位置，其上的箭頭表示AUV的艏向，*表示編隊(duì)參考點(diǎn)的位置，實(shí)線表示AUV的運(yùn)動(dòng)軌跡，加粗虛線表示編隊(duì)參考點(diǎn)的運(yùn)動(dòng)軌跡。圖7為虛擬結(jié)構(gòu)質(zhì)點(diǎn)收斂至期望編隊(duì)位置的誤差曲線，以及AUV對(duì)虛擬結(jié)構(gòu)質(zhì)點(diǎn)進(jìn)行目標(biāo)跟蹤的誤差曲線。圖8為AUV之間的相對(duì)距離曲線。

圖6 AUV編隊(duì)路徑跟蹤 Fig.6 AUV formation path following

圖7 AUV編隊(duì)誤差Fig.7 AUV formation error

圖8 AUV間距Fig.8 Distance between AUVs

由圖6～圖8可知，多AUV系統(tǒng)在隨機(jī)分布的初始位置下，虛擬結(jié)構(gòu)位置偏差和AUV目標(biāo)跟蹤誤差，逐漸收斂至0，形成期望的編隊(duì)隊(duì)形，表明編隊(duì)形成過程中算法的有效性。多AUV編隊(duì)跟隨參考點(diǎn)沿期望的編隊(duì)軌跡運(yùn)動(dòng)，并在編隊(duì)行進(jìn)和轉(zhuǎn)向的過程中均保持了良好的隊(duì)形。表明算法對(duì)于多AUV直線編隊(duì)路徑跟蹤和曲線編隊(duì)路徑跟蹤，均具有良好的適用性。

多AUV系統(tǒng)的初始位置不利于隊(duì)形的形成，在隊(duì)形形成的過程中AUV發(fā)生交匯，位置在(260 m,170 m)附近，見圖6的局部放大圖。期望編隊(duì)位置在左側(cè)的AUV，初始位置在初始化區(qū)域的右側(cè)；而期望編隊(duì)位置在右側(cè)的AUV，初始位置與之相反。在AUV駛向編隊(duì)期望位置的過程中二者發(fā)生交匯，因而改變了原軌跡，并行行駛一段距離后，前后交錯(cuò)避免了碰撞，繼而駛向期望位置。由圖8可知， 80～150 s的時(shí)間段內(nèi)AUV之間的距離較小，發(fā)生交匯，左右兩側(cè)的AUV在較長(zhǎng)的時(shí)間段內(nèi)保持了10 m左右的距離，此時(shí)人工勢(shì)場(chǎng)勢(shì)函數(shù)已經(jīng)產(chǎn)生作用，避免了AUV之間的碰撞。之后AUV逐漸收斂至期望的隊(duì)形，AUV的間距穩(wěn)定在52 m.

分析圖7中的AUV目標(biāo)跟蹤誤差可知，在初始時(shí)刻目標(biāo)跟蹤誤差為0，因?yàn)锳UV和虛擬結(jié)構(gòu)質(zhì)點(diǎn)具有相同的初始位置。誤差隨之變大，原因在于AUV的初始艏向是隨機(jī)分布的，質(zhì)點(diǎn)開始運(yùn)動(dòng)后，AUV進(jìn)行跟蹤需要調(diào)整艏向，由于回轉(zhuǎn)半徑的限制，使得AUV落后于質(zhì)點(diǎn)。當(dāng)AUV艏向調(diào)整完畢，跟蹤誤差迅速收斂至0. 圖7局部放大圖對(duì)應(yīng)AUV發(fā)生交匯的過程，由于勢(shì)函數(shù)的干擾，導(dǎo)致質(zhì)點(diǎn)偏離原來軌跡，從而使得跟蹤誤差變大。

圖9 AUV螺旋運(yùn)動(dòng)Fig.9 AUV helical motion

仿真實(shí)驗(yàn)2：12個(gè)AUV進(jìn)行螺旋擴(kuò)張運(yùn)動(dòng)的仿真實(shí)驗(yàn)。期望的隊(duì)形為圓環(huán)隊(duì)形，圓的半徑為80 m，并以0.1 m/s的速度擴(kuò)張，繞參考點(diǎn)旋轉(zhuǎn)的相對(duì)線速度為0.3 m/s. 如圖9所示，AUV由初始的隨機(jī)位置收斂至圓環(huán)上，并均勻分布。多AUV編隊(duì)跟隨參考點(diǎn)沿OnN軸正方向運(yùn)動(dòng)的同時(shí)，繞參考點(diǎn)勻速轉(zhuǎn)動(dòng)，從而形成了螺旋運(yùn)動(dòng)的運(yùn)動(dòng)形式。

仿真實(shí)驗(yàn)3：10個(gè)AUV進(jìn)行隊(duì)形變換的仿真實(shí)驗(yàn)。AUV之間的期望距離為50 m，第1個(gè)隊(duì)形為矩形隊(duì)形，第2個(gè)隊(duì)形為三角隊(duì)形。仿真結(jié)果如圖10，AUV群體由初始時(shí)刻的隨機(jī)位置，逐漸收斂至矩形隊(duì)形，當(dāng)隊(duì)形穩(wěn)定后，逐步變換為三角隊(duì)形。在隊(duì)形變化過程中，AUV較有秩序地進(jìn)行，避免了個(gè)體之間的碰撞。最終，在新隊(duì)形下達(dá)到了穩(wěn)定。

圖10 AUV隊(duì)形變換Fig.10 AUV formation changing

由仿真實(shí)驗(yàn)2、實(shí)驗(yàn)3可知，提出的編隊(duì)控制算法可以靈活實(shí)現(xiàn)編隊(duì)的隊(duì)形控制，進(jìn)行復(fù)雜的隊(duì)形變化。并在編隊(duì)變化的過程中，保證AUV之間的安全距離。

仿真實(shí)驗(yàn)4：10個(gè)AUV進(jìn)行避碰的仿真實(shí)驗(yàn)。障礙物勢(shì)函數(shù)的作用距離為15 m. 圖11(a)中，障礙物的位置為(800 m,150 m)，半徑為60 m；圖11(b)中，兩個(gè)障礙物的位置為(800 m,50 m)，(800 m,250 m)，半徑為40 m. 由圖11可知，AUV編隊(duì)在行進(jìn)過程中，遇到編隊(duì)路徑上的障礙物。AUV個(gè)體在障礙物邊界上的投影點(diǎn)產(chǎn)生人工勢(shì)場(chǎng)，對(duì)AUV的運(yùn)動(dòng)造成影響。AUV在勢(shì)場(chǎng)力的作用下，改變?cè)瓉淼倪\(yùn)動(dòng)軌跡，沿障礙物的邊界劃過障礙物。在成功完成避障機(jī)動(dòng)之后，多AUV系統(tǒng)又恢復(fù)了原期望的隊(duì)形。

圖11 AUV隊(duì)形避碰Fig.11 AUV formation with obstacle avoidance

圖12 AUV間距和AUV與障礙物距離曲線Fig.12 Distance between AUVs and distance between AUV and obstacle

圖12為在避碰過程中，AUV與障礙物邊界的距離，以及AUV之間的相互距離。由圖12可見，在500～700 s的時(shí)間段內(nèi)，AUV編隊(duì)進(jìn)行避障機(jī)動(dòng)。圖12(a)為AUV與單個(gè)障礙物邊界的距離，分析數(shù)據(jù)可知，最小值為13 m. 此時(shí)，AUV處于避碰勢(shì)函數(shù)的作用之下，與障礙物保持了一定的安全距離。圖12(b)為AUV躲避單個(gè)障礙物時(shí)，AUV之間的相互距離。由于障礙物的干擾，AUV編隊(duì)隊(duì)形被破壞，處于中間位置處的AUV向兩側(cè)運(yùn)動(dòng)，“擠壓”兩側(cè)的AUV，使得AUV之間的距離變小。但是由于避碰函數(shù)的作用，AUV之間的距離始終大于0，從而避免了AUV之間的碰撞。當(dāng)完成避障機(jī)動(dòng)后，AUV恢復(fù)編隊(duì)隊(duì)形，AUV之間的距離恢復(fù)到一個(gè)固定值。圖12(c)為AUV分別與兩個(gè)障礙物邊界的距離，圖12(d)為AUV穿過兩個(gè)障礙物間隙時(shí)，AUV之間的距離。比較圖12(b)、圖12(d)兩圖可知，AUV穿過兩個(gè)障礙物間隙，隊(duì)形的破壞較小。原因在于處于編隊(duì)中央的AUV，并不具有碰撞障礙物的危險(xiǎn)，前方的AUV通過間隙后，只有后方兩側(cè)的AUV受到了障礙物的影響，向內(nèi)側(cè)運(yùn)動(dòng)，使得AUV間距變小。這一分析結(jié)果可與圖11中AUV編隊(duì)軌跡相印證。

4 結(jié)論

本文通過人工勢(shì)場(chǎng)和虛擬結(jié)構(gòu)相結(jié)合，提出了一種新型多AUV編隊(duì)控制算法。采用虛擬結(jié)構(gòu)法進(jìn)行編隊(duì)隊(duì)形的組織，并利用人工勢(shì)場(chǎng)勢(shì)函數(shù)，實(shí)現(xiàn)了AUV之間的避碰和編隊(duì)的避障。進(jìn)行了編隊(duì)協(xié)同路徑跟蹤仿真實(shí)驗(yàn)，編隊(duì)變換仿真實(shí)驗(yàn)和編隊(duì)避障仿真實(shí)驗(yàn)。仿真實(shí)驗(yàn)表明，在隨機(jī)分布的初始條件下，算法不僅可以有效地組織隊(duì)形，靈活地進(jìn)行編隊(duì)變換，而且在編隊(duì)形成和編隊(duì)行進(jìn)的過程中，有效地避碰和避障，保障AUV之間、AUV和障礙物之間具有足夠的安全距離。

參考文獻(xiàn)(References)

[1] Cui R, Ge S S, How B V E, et al. Leader-follower formation control of underactuated autonomous underwater vehicles[J]. Ocean Engineering, 2010, 37(17/18):1491-1502．

[2] Ren W. Decentralization of virtual structures in formation control of multiple vehicle systems via, consensus strategies[J]. European Journal of Control, 2008, 14(2):93-103.

[3] 趙寧寧, 徐德民, 高劍, 等. 基于Serret-Frenet坐標(biāo)系的多AUV編隊(duì)路徑跟蹤控制[J]. 魚雷技術(shù), 2015, 23(1): 35-39. ZHAO Ning-ning, XU De-min, GAO Jian, et al. Formation path following control of multiple AUVs based on Serret-Frenet coordinate system[J]. Torpedo Technology, 2015, 23(1): 35-39. (in Chinese)

[4] Leonard N E, Fiorelli E. Virtual leaders, artificial potentials and coordinated control of groups[C]∥IEEE Conference on Decision and Control.Orlando, America：IEEE Control Systems Society, 2001: 2963-2973.

[5] Cifuentes S, Girón-Sierra J M, Jiménez J. Virtual fields and behaviour blending for the coordinated navigation of robot teams: Some experimental results.[J]. Expert Systems with Applications, 2015, 42(10):4778-4796.

[6] Djapic V. Unifying behavior based control design and hybrid stability theory for AUV application[D]. Riverside: University of California, 2009: 72-105.

[7] Jiang D P, Pang Y J, Qin Z B. Coordinated control of multiple autonomous underwater vehicle system[C]∥Proceedings of 2010 8th World Congress on Intelligent Control and Automation. Jinan, China:IEEE Robotics and Automation Society, IEEE Control System Society, 2010: 4901-4906．

[8] Fossen T I. Handbook of marine craft hydrodynamics and motion control, first edition[M]. United Kingdom: John Wiley & Sons Ltd, 2011.

[9] 甘永. 水下機(jī)器人運(yùn)動(dòng)控制系統(tǒng)體系結(jié)構(gòu)的研究[D].哈爾濱: 哈爾濱工程大學(xué), 2007: 10-18. GAN Yong. Research on motion control system architecture of underwater vehicle[D]. Harbin: Harbin Engineering University, 2007: 10-18. (in Chinese)

[10] 李岳明, 龐永杰, 萬磊. 水下機(jī)器人自適應(yīng)S面控制[J]. 上海交通大學(xué)學(xué)報(bào), 2012, 46(2): 195-200. LI Yue-ming, PANG Yong-jie, WAN Lei. Adaptive S plane control for autonomous underwater vehicle[J]. Journal of Shanghai Jiao Tong University, 2012, 46(2): 195-200. (in Chinese)

[11] Do K D, Pan J. Global robust adaptive path following of underactuated ships[J]. Autonomous, 2006, 42(10): 1713-1722.

A Multi-AUV Formation Algorithm Combining Artificial Potential Field and Virtual Structure

PAN Wu-wei， JIANG Da-peng， PANG Yong-jie， LI Yue-ming， ZHANG Qiang

(Science and Technology on Underwater Vehicle Laboratory，Harbin Engineering University, Harbin 150001, Heilongjiang， China)

Collision avoidance in the forming process of formation and obstacle avoidance on the formation proceeding are not solved using traditional multi-AUV formation algorithms, such as leader-follower and virtual structure. Artificial potential field algorithm can be used to solve the above problems, while its ability of formation organization is not satisfied. A multi-AUV formation control algorithm combing artificial potential field and virtual structure is proposed to overcome the problems. The system is divided into three parts: formation reference point, virtual structure particle and AUV. The desired virtual structure, which is the moving target of virtual structure particles, is organized to surround the formation reference point. Repulsive artificial potential is used to avoid collision and obstacle while the particles move. The AUVs track the particles to form a certain formation asymptotically. The formation path following, formation changing and obstacle avoidance are simulated to verify the availability and flexibility of the proposed algorithm. The results indicate that the multi-AUV system can form a formation without collision from random initial positions, and the fleet can change formation flexibly and avoid obstacles effectively while proceeding.

ordnance science and technology; multi-AUV；formation control；artificial potential field；virtual structure

2016-05-18

國(guó)家自然科學(xué)基金項(xiàng)目(51209051、51509057、51309066)； 中央高校基本科研項(xiàng)目(HEUCFD1514)； 海洋工程國(guó)家重點(diǎn)實(shí)驗(yàn)室開放課題項(xiàng)目(1415)

潘無為(1988—)， 男， 博士研究生， E-mail： panwuwei@hrbeu.edu.cn

姜大鵬(1981—)， 男， 副教授， 碩士生導(dǎo)師， E-mail： jdp103@hrbeu.edu.cn

TP273+.5

A

1000-1093(2017)02-0326-09

10.3969/j.issn.1000-1093.2017.02.017