一種基于人工勢(shì)場(chǎng)多AUV集群的實(shí)時(shí)避障方法

徐博，張嬌，王超

哈爾濱工程大學(xué) 自動(dòng)化學(xué)院，黑龍江 哈爾濱 150001

0 引 言

隨著無(wú)人機(jī)、無(wú)人戰(zhàn)車(chē)、機(jī)器戰(zhàn)士逐漸在戰(zhàn)場(chǎng)上顯示出越來(lái)越大的威力，無(wú)人化戰(zhàn)爭(zhēng)的發(fā)展已經(jīng)呈現(xiàn)出相對(duì)清晰的藍(lán)圖。海洋空間智能無(wú)人運(yùn)載器，如無(wú)人水面艇（USV）、自主式水下機(jī)器人（AUV）和無(wú)人水下機(jī)器人（UUV）等也得到迅速發(fā)展，開(kāi)始在未來(lái)海洋國(guó)土安全、海洋開(kāi)發(fā)方面發(fā)揮越來(lái)越大的作用。國(guó)內(nèi)外均十分重視該領(lǐng)域的研究，已取得多項(xiàng)鼓舞人心的成果，并逐漸在軍事和其他領(lǐng)域得到應(yīng)用［1］。

集群運(yùn)動(dòng)是自然界中一種非常普遍的現(xiàn)象，典型的代表有鳥(niǎo)群的集體飛翔、昆蟲(chóng)的集體遷移，甚至是生命體中蛋白質(zhì)等物質(zhì)的集體移動(dòng)［2-5］。在這些群體類(lèi)似的運(yùn)動(dòng)中，如何形成協(xié)調(diào)有序的集體運(yùn)動(dòng)模式以及如何迅速改變當(dāng)前運(yùn)動(dòng)狀態(tài)，一直是集群運(yùn)動(dòng)控制研究中的熱點(diǎn)問(wèn)題。在多AUV集群運(yùn)動(dòng)編隊(duì)方法中，有很多問(wèn)題需要考慮，如穩(wěn)定性、可控性等，為解決這些問(wèn)題，不少學(xué)者提出了自己的想法，比如領(lǐng)航跟隨者法（Leaderfollower）、基于行為的方法、圖論法以及人工勢(shì)場(chǎng)法等［6］。

Balch等［7］提出了一種基于行為的編隊(duì)方法，即將隊(duì)形控制分解為一系列基本行為，通過(guò)行為的綜合來(lái)實(shí)現(xiàn)運(yùn)動(dòng)控制。該方法有明確的隊(duì)形反饋，并實(shí)現(xiàn)了分布式控制，但對(duì)群體的行為沒(méi)有明確定義，難以進(jìn)行數(shù)學(xué)分析，不能保證隊(duì)形的穩(wěn)定性。

潘無(wú)為等［8］提出了一種虛擬結(jié)構(gòu)方法，即將AUV編隊(duì)從整體上看作是一個(gè)剛體的虛擬結(jié)構(gòu)，每個(gè)AUV是剛體上相對(duì)位置固定的一點(diǎn)。該方法可通過(guò)定義剛體的行為來(lái)控制AUV的運(yùn)動(dòng)，但不能根據(jù)環(huán)境的變化來(lái)改變隊(duì)形，因而限制了其應(yīng)用范圍。

俞輝等［9］研究了一種領(lǐng)航跟隨者法，該方法是將AUV整體拆分為兩兩一組，即一個(gè)領(lǐng)航AUV，一個(gè)跟隨AUV，通過(guò)跟隨者對(duì)領(lǐng)航者保持一定的角度和距離來(lái)實(shí)現(xiàn)隊(duì)形控制；根據(jù)領(lǐng)航者與跟隨者的相對(duì)位置關(guān)系，可以形成不同的網(wǎng)絡(luò)拓?fù)浣Y(jié)構(gòu)，當(dāng)在環(huán)境中遇到障礙物時(shí)，可以通過(guò)改變隊(duì)形來(lái)避開(kāi)障礙物。但是這種主從式策略的自適應(yīng)性以及魯棒性不強(qiáng)，無(wú)法全面反映自然界中的個(gè)體自行選擇或跟蹤目標(biāo)的能力。

Khatib［10］首次提出了人工勢(shì)場(chǎng)的概念，即假定場(chǎng)內(nèi)的個(gè)體在受到目標(biāo)和障礙的力的作用時(shí)，令其沿著勢(shì)能函數(shù)值最小的方向運(yùn)動(dòng)。在早期，該方法僅用于靜態(tài)環(huán)境，而在針對(duì)動(dòng)態(tài)環(huán)境時(shí)由于忽略了諸多動(dòng)態(tài)因素，故僅選擇相對(duì)位置作為輸入量。針對(duì)此類(lèi)問(wèn)題，Khatib對(duì)該方法進(jìn)行了改進(jìn)，使之與其他方法結(jié)合，更適合于多AUV編隊(duì)控制。

劉明雍等［11］提出在編隊(duì)控制領(lǐng)域需要對(duì)集群的分群運(yùn)動(dòng)進(jìn)行研究，因?yàn)榉秩簭?qiáng)調(diào)了集群中的運(yùn)動(dòng)分化，可通過(guò)不同的組群來(lái)實(shí)現(xiàn)不同的目標(biāo)任務(wù)，例如避障、跟蹤等，但他們只針對(duì)該問(wèn)題進(jìn)行了理論建模，還缺少實(shí)驗(yàn)驗(yàn)證。AUV的數(shù)量對(duì)多AUV集群運(yùn)動(dòng)的導(dǎo)航以及控制算法具有重要影響，AUV的數(shù)量越多，相應(yīng)的控制難度也就越大。

綜合以上問(wèn)題，本文將提出一種基于動(dòng)態(tài)網(wǎng)絡(luò)拓?fù)涞募哼\(yùn)動(dòng)控制方法和基于人工勢(shì)場(chǎng)法的避障方法，對(duì)人工勢(shì)場(chǎng)函數(shù)進(jìn)行改進(jìn)，并通過(guò)Matlab軟件進(jìn)行仿真實(shí)驗(yàn)以驗(yàn)證算法的可行性。

1 集群運(yùn)動(dòng)基本模型

假設(shè)第i個(gè)AUV的控制律為ui，

考慮由N個(gè)AUV組成的群體，動(dòng)態(tài)方程則為

式中：qi=[xi，yi]T，為個(gè)體位置；，為速度向量；ui=[uxi，uyi]T，為控制輸入；Z為正整數(shù)集；R2為二維實(shí)數(shù)集。qij=qi-qj，為第i個(gè)與第j個(gè)AUV之間的相對(duì)距離向量。

將群體中的每個(gè)AUV都視為網(wǎng)絡(luò)有向圖中的一個(gè)節(jié)點(diǎn)。AUV在運(yùn)動(dòng)過(guò)程中網(wǎng)絡(luò)拓?fù)湟矔?huì)隨之動(dòng)態(tài)地發(fā)生改變。通過(guò)動(dòng)態(tài)網(wǎng)絡(luò)方法，可以對(duì)AUV的群體運(yùn)動(dòng)行為進(jìn)行建模，從而使得網(wǎng)絡(luò)節(jié)點(diǎn)可以維持其與鄰近個(gè)體之間的平衡距離。

定義1（鄰接圖）：設(shè)G=(v，ε)，為n個(gè)節(jié)點(diǎn)的權(quán)重有向圖，其中v={1，2，…，n}，為頂點(diǎn)的集合，ε為邊的集合。A=[aij]，為權(quán)重鄰接矩陣，其中對(duì)于?i，j∈I={1，2，…，n}，i≠j，aij≥0；對(duì)于?i∈I，aij=0。設(shè)r為任意兩臺(tái)AUV節(jié)點(diǎn)間的距離，節(jié)點(diǎn)vi的鄰接集合用Ni表示，定義為

很顯然，邊集ε(q)由q決定（q為個(gè)體位置），(G(q)，q)就是鄰接結(jié)構(gòu)。

2 基于動(dòng)態(tài)網(wǎng)絡(luò)拓?fù)涞木庩?duì)控制研究

將群體中的每一個(gè)AUV都視作是網(wǎng)絡(luò)中的一個(gè)節(jié)點(diǎn)，AUV在運(yùn)動(dòng)中形成了動(dòng)態(tài)網(wǎng)絡(luò)。為在真實(shí)的集群中捕獲明顯的空間順序，使用柵格模擬AUV集群節(jié)點(diǎn)的幾何結(jié)構(gòu)。找出一系列的q以及n個(gè)節(jié)點(diǎn)，令這些節(jié)點(diǎn)與相鄰節(jié)點(diǎn)保持相同的距離，由式（5）描述為

公式中，q的選取在很大程度上影響著AUV集群的期望隊(duì)形，故將其定義為格子型對(duì)象。

定義2：將在式（5）限制下的q幾何配置看作是一個(gè)α格子，尺寸大小為d，AUV個(gè)體稱(chēng)作α個(gè)體。多個(gè)鄰近的個(gè)體相連就形成了α格子。由α格子形成的鄰接網(wǎng)絡(luò)的邊長(zhǎng)都是相等的。

為了構(gòu)建光滑的集成勢(shì)場(chǎng)，并構(gòu)建相應(yīng)的空間鄰接矩陣，將σ范式的非負(fù)地圖做如下定義：

在本文中，ε的值保持不變。在這里建立新的范式是因?yàn)樵谌魏螘r(shí)候都可微，而在z=0（z表示函數(shù)變量）處是不可微的。

沖擊函數(shù)ρh(z)為標(biāo)量函數(shù)，在0～1之間光滑，用來(lái)構(gòu)建光滑的勢(shì)場(chǎng)函數(shù)和鄰接矩陣。沖擊函數(shù)選擇如下：

式中：h∈(0，1)；沖擊函數(shù)ρh(z)在[1，∞)內(nèi)，一致趨于z。利用該沖擊函數(shù)，可定義空間鄰接矩陣A(q)：

為了構(gòu)建光滑的成對(duì)勢(shì)場(chǎng)，又引入了行為函數(shù)?α(z)：

最后，提出了分布式控制算法，用來(lái)研究多AUV集群運(yùn)動(dòng)中的編隊(duì)控制。

3 基于人工勢(shì)場(chǎng)的多AUV集群運(yùn)動(dòng)規(guī)劃

人工勢(shì)場(chǎng)法的基本思想是：在目標(biāo)點(diǎn)位置和障礙物位置分別構(gòu)造引力場(chǎng)Uatt和斥力場(chǎng)Urep，相應(yīng)的勢(shì)場(chǎng)力會(huì)吸引AUV向目標(biāo)點(diǎn)附近運(yùn)動(dòng)，而阻止其向障礙物附近運(yùn)動(dòng)，在合力的最終作用下，指引AUV向目標(biāo)點(diǎn)運(yùn)動(dòng)。

斥力場(chǎng)函數(shù)的設(shè)計(jì)與障礙物有關(guān)，距離障礙物越近，斥力就越大；引力場(chǎng)函數(shù)與之類(lèi)似。為了對(duì)該方法進(jìn)行必要的理論研究，在既簡(jiǎn)化問(wèn)題又不失通用性的基礎(chǔ)上，構(gòu)建了二維平面、帶有靜態(tài)威脅的人工勢(shì)場(chǎng)模型［13］。

3.1 引力場(chǎng)建模

將二維平面內(nèi)的AUV與目標(biāo)點(diǎn)距離的函數(shù)定義為

其產(chǎn)生的引力場(chǎng)為

式中：r(x1，y1)指AUV的位置；g(x2，y2)指目標(biāo)點(diǎn)位置；m為正常數(shù)，m的值是決定勢(shì)場(chǎng)函數(shù)曲線形狀的因素；ξ為目標(biāo)勢(shì)場(chǎng)系數(shù)。在這里我們?nèi)=2，這是因?yàn)楫?dāng)m=1時(shí)會(huì)使引力場(chǎng)求導(dǎo)后出現(xiàn)引力無(wú)界的問(wèn)題，造成AUV在目標(biāo)點(diǎn)附近抖動(dòng)，因此，將m值取為最小值2。

目標(biāo)點(diǎn)對(duì)AUV的吸引力為

式中，nRG為AUV至目標(biāo)點(diǎn)的單位矢量。

本文中，由于AUV的目標(biāo)點(diǎn)是固定的，因此在設(shè)計(jì)引力勢(shì)場(chǎng)時(shí)不必考慮AUV與目標(biāo)點(diǎn)之間的相對(duì)運(yùn)動(dòng)（圖1）。

3.2 建立斥力場(chǎng)模型

將障礙物的位置設(shè)為o(x3，y3)，F(xiàn)rep1為AUV與障礙物間的排斥力，其使得AUV向遠(yuǎn)離障礙物的方向運(yùn)動(dòng)。與上面的分析類(lèi)似，這里假定障礙物為靜態(tài)，不考慮AUV與障礙物間相對(duì)速度對(duì)AUV運(yùn)動(dòng)的影響，F(xiàn)rep為由AUV速度產(chǎn)生的斥力，該斥力大小與速度在AUV和與障礙物連線上的速度分量大小成正比，方向?yàn)檠谹UV與障礙物的連線且遠(yuǎn)離障礙物的方向（圖2）。

斥力勢(shì)場(chǎng)為

式中，M為障礙物的個(gè)數(shù)；ρi(r，o)=，為AUV與第i個(gè)障礙物間的距離函數(shù)。因此，式（15）可以寫(xiě)作

式中，η為斥力勢(shì)場(chǎng)系數(shù)。

與二維空間S中第i個(gè)障礙物的斥力勢(shì)場(chǎng)相對(duì)應(yīng)的排斥力為

因此，總的排斥力為

3.3 空間合成勢(shì)場(chǎng)建模

將空間內(nèi)的引力勢(shì)場(chǎng)與斥力勢(shì)場(chǎng)進(jìn)行合成，得到的合成勢(shì)場(chǎng)為

空間合成勢(shì)場(chǎng)力為

常規(guī)的人工勢(shì)場(chǎng)函數(shù)的自變量是距離的二次方，這種形式的函數(shù)曲線升降過(guò)快，這也使得引力和斥力變化過(guò)快，因而在選取參數(shù)時(shí)比較嚴(yán)苛，很可能會(huì)因參數(shù)選取的問(wèn)題而無(wú)法完成路徑規(guī)劃。所以，這里采用的勢(shì)場(chǎng)函數(shù)都是經(jīng)過(guò)改進(jìn)的，采用指數(shù)函數(shù)來(lái)作為勢(shì)場(chǎng)函數(shù)，結(jié)構(gòu)如下：

對(duì)于引力場(chǎng)，指數(shù)函數(shù)自變量X的表達(dá)式為

對(duì)于斥力場(chǎng)，X的表達(dá)式為

選取了指數(shù)函數(shù)，勢(shì)場(chǎng)函數(shù)仍然為距離的函數(shù)，但函數(shù)曲線不再是二次曲線，而是指數(shù)曲線。由指數(shù)函數(shù)的特點(diǎn)可知，m和n的取值均大于1，引力勢(shì)場(chǎng)系數(shù)β和斥力勢(shì)場(chǎng)系數(shù)γ為正的常數(shù)，因此X＞0。又由指數(shù)函數(shù)曲線的特點(diǎn)可知，U(ρ(r，g))圖像取區(qū)間[0，∞)部分，該部分的變化較為緩慢，較容易控制，可達(dá)到比較穩(wěn)定的效果。

4 仿 真

為了驗(yàn)證基于人工勢(shì)場(chǎng)法的多AUV路徑規(guī)劃算法，下面對(duì)其進(jìn)行仿真驗(yàn)證：取目標(biāo)勢(shì)場(chǎng)系數(shù)ξ=5；對(duì)于引力場(chǎng)，取相鄰個(gè)體數(shù)目α=5，引力場(chǎng)系數(shù)β=0.5；對(duì)于斥力場(chǎng)，取相鄰個(gè)體數(shù)目α=1，斥力場(chǎng)系數(shù)γ=2。設(shè)置10臺(tái)AUV，在目標(biāo)點(diǎn)的引力與障礙物的斥力的共同作用下完成避障運(yùn)動(dòng)。將10個(gè)AUV的運(yùn)動(dòng)速度初值（單位：m/s）設(shè)置為：

圖3中，左下角的“x”標(biāo)志代表10個(gè)AUV，右上角的“X”標(biāo)志代表目標(biāo)點(diǎn)位置，坐標(biāo)不變；平面內(nèi)部的圓圈代表障礙物，坐標(biāo)分別為（80 m，100 m），（100 m，100 m），（120 m，100 m），（180 m，200 m），（200 m，150 m）和（240 m，200 m）；虛線為10個(gè)AUV的運(yùn)動(dòng)軌跡。圖3（b）所示為10個(gè)AUV降速0.8 m/s后的軌跡。圖4為減速前與減速后x和y方向上的多AUV集群運(yùn)動(dòng)軌跡。

由圖3和圖4的仿真結(jié)果可知，基于人工勢(shì)場(chǎng)的避障方法，所有AUV均可順利避開(kāi)障礙物，準(zhǔn)確到達(dá)目標(biāo)點(diǎn)附近的安全區(qū)域；并且速度越慢，AUV距離障礙物越遠(yuǎn)，避碰效果越好?？梢?jiàn)，基于指數(shù)函數(shù)的人工勢(shì)場(chǎng)法能準(zhǔn)確實(shí)現(xiàn)AUV的避障。

5 結(jié) 語(yǔ)

本文針對(duì)多AUV協(xié)同定位過(guò)程中出現(xiàn)的集群內(nèi)個(gè)體間保持聚集以及避免互相碰撞的問(wèn)題，采用了一種基于動(dòng)態(tài)網(wǎng)絡(luò)拓?fù)涞木庩?duì)方法，即將AUV看作網(wǎng)絡(luò)中的節(jié)點(diǎn)，通過(guò)設(shè)置勢(shì)場(chǎng)函數(shù)來(lái)滿足編隊(duì)要求；另外，針對(duì)航行中可能出現(xiàn)的障礙物，采用人工勢(shì)場(chǎng)法進(jìn)行在線規(guī)劃，將勢(shì)場(chǎng)函數(shù)作為指數(shù)函數(shù)進(jìn)行了仿真。仿真結(jié)果表明，在基于人工勢(shì)場(chǎng)函數(shù)的多AUV避碰方法中，指數(shù)函數(shù)法可實(shí)現(xiàn)多AUV對(duì)障礙物的實(shí)時(shí)規(guī)避。