本發(fā)明涉及多auv的編隊(duì)控制中的避障控制，特別涉及一種基于人工勢(shì)場(chǎng)和仿射變換的多auv編隊(duì)重構(gòu)避障控制方法。

背景技術(shù)：

1、對(duì)于陸地?zé)o人車和空中無(wú)人來(lái)說(shuō)，單機(jī)器人的避障算法和路徑規(guī)劃已經(jīng)達(dá)到了相對(duì)成熟的程度，衍生了很多經(jīng)典的避障路徑規(guī)劃和優(yōu)化算法，很多方法都能夠通過(guò)離線或在線的方式規(guī)劃一條從起始點(diǎn)到目標(biāo)點(diǎn)的安全航行路徑。但對(duì)于auv來(lái)說(shuō)，水下環(huán)境通常是復(fù)雜動(dòng)態(tài)未知的，研究auv的避障控制和航行路徑規(guī)劃，本身是比較困難的，優(yōu)勢(shì)是考慮到將算法應(yīng)用到實(shí)際中，更是一項(xiàng)不小的挑戰(zhàn)。盡管如此，學(xué)者們還是認(rèn)真研究，努力解決問(wèn)題，在auv避障和路徑規(guī)劃方面做出了相當(dāng)前瞻的研究性成果。但相對(duì)來(lái)說(shuō)，關(guān)于多auv編隊(duì)避障的研究較少。auv編隊(duì)控制中，在某些特定航行任務(wù)下，如編隊(duì)護(hù)航、協(xié)同搬運(yùn)，編隊(duì)auv需要在保持編隊(duì)得同時(shí)規(guī)避靜、動(dòng)態(tài)障礙威脅，這就要求多auv系統(tǒng)能夠應(yīng)對(duì)復(fù)雜的障礙環(huán)境，能夠在保持現(xiàn)有編隊(duì)或重構(gòu)編隊(duì)得情況下規(guī)劃可通航路徑，而目前大多數(shù)關(guān)于多auv編隊(duì)控制的研究單獨(dú)考慮了靜態(tài)障礙物或單動(dòng)態(tài)障礙情況，或者在編隊(duì)航行過(guò)程中，遇到障礙是，普遍采用分解編隊(duì)，避障后重組隊(duì)形的方式，顯然，由于水下環(huán)境的特殊性，分解重組的方法在實(shí)際應(yīng)用中將面臨很多問(wèn)題，如通信中斷，個(gè)別auv丟失等，風(fēng)險(xiǎn)更大。此外，相比于二維航路，三維航路更能充分發(fā)揮auv的機(jī)動(dòng)性能，從而適應(yīng)水下探測(cè)、目標(biāo)攔截、編隊(duì)護(hù)航等任務(wù)規(guī)劃。因此，復(fù)雜三維環(huán)境下的動(dòng)態(tài)編隊(duì)重構(gòu)和避障技術(shù)受到越來(lái)越多的關(guān)注。在大多數(shù)實(shí)際的水下工程任務(wù)中，auv航行環(huán)境存在各類障礙物(靜態(tài)和動(dòng)態(tài))，需要編隊(duì)auv避開(kāi)這些障礙物。另外，多auv編隊(duì)航行的路徑大多不是已知的，復(fù)雜水下環(huán)境地形避障與航路規(guī)劃是目前auv編隊(duì)研究的熱點(diǎn)問(wèn)題,它要求編隊(duì)auv在復(fù)雜水下環(huán)境下,能夠自主地規(guī)劃一條適合自身編隊(duì)結(jié)構(gòu)通航的避障路徑,避開(kāi)環(huán)境中的靜態(tài)障礙物或動(dòng)態(tài)障礙物,完成航行任務(wù)。然而在臺(tái)auv避障航行控制已經(jīng)很困難的情況先，多臺(tái)auv的協(xié)同，使得著多auv的控制難度，已經(jīng)不是倍數(shù)式增加，這意味著多auv編隊(duì)避障控制器的設(shè)計(jì)更加困難。

2、避障問(wèn)題在多auv的編隊(duì)控制中非常關(guān)鍵，已經(jīng)成為auv完成水下作業(yè)任務(wù)過(guò)程中的一個(gè)極其重要的環(huán)節(jié)。auv的避障方法主要包括人工勢(shì)場(chǎng)法、模板匹配方法、地圖構(gòu)建與智能優(yōu)化法等。其中人工勢(shì)場(chǎng)法因其原理簡(jiǎn)單，反應(yīng)迅速，易于實(shí)現(xiàn)和擴(kuò)展被廣泛采用，是機(jī)器人路徑規(guī)劃的代表性方法。然而傳統(tǒng)的人工勢(shì)場(chǎng)法在某些情況下，存在目標(biāo)不可達(dá)的問(wèn)題，即當(dāng)auv所受目標(biāo)區(qū)域引力、障礙物斥力及鄰近auv合力為零時(shí)將致使水下機(jī)器人群組出現(xiàn)避障失敗的情況。

技術(shù)實(shí)現(xiàn)思路

1、本發(fā)明的目的在于提供一種基于人工勢(shì)場(chǎng)和仿射變換的多auv編隊(duì)重構(gòu)避障控制方法，將人工勢(shì)場(chǎng)路徑規(guī)劃與仿射變換編隊(duì)重構(gòu)方法相結(jié)合，研究基于人工勢(shì)場(chǎng)的仿射變換多auv編隊(duì)重構(gòu)避障控制方法，解決復(fù)雜環(huán)境下多auv編隊(duì)規(guī)劃與重構(gòu)避障問(wèn)題。

2、為了達(dá)到上述目的，采用的技術(shù)方案如下：

3、一種基于人工勢(shì)場(chǎng)和仿射變換的多auv編隊(duì)重構(gòu)避障控制方法，所述方法包括：

4、建立改進(jìn)人工勢(shì)場(chǎng)的勢(shì)能函數(shù)；其中，目的區(qū)域?qū)μ摂Mauv的引力勢(shì)能函數(shù)表示為：

5、

6、式中，uatt表示吸引力勢(shì)場(chǎng)，katt表示目標(biāo)區(qū)域引力場(chǎng)的引力系數(shù)，ρ(pv,pg)表示虛擬點(diǎn)與目標(biāo)區(qū)域的距離范式，pv表示虛擬點(diǎn)，pg表示目標(biāo)點(diǎn)的位置信息，dg表示改進(jìn)的吸引勢(shì)場(chǎng)的邊值；

7、引力函數(shù)表示為：

8、

9、式中，fatt(pv)表示虛擬領(lǐng)航者pv所受到的目標(biāo)點(diǎn)吸引力，表示梯度，uatt(pv)表示虛擬領(lǐng)航者pv所受到的目標(biāo)點(diǎn)吸引力勢(shì)場(chǎng)；

10、選取第i個(gè)障礙物對(duì)虛擬auv的排斥勢(shì)能函數(shù)，表示為：

11、

12、式中，表示擬領(lǐng)航者pv所受到的障礙物吸排斥勢(shì)場(chǎng)，上標(biāo)i表示第i個(gè)障礙物，krep表示排斥勢(shì)能函數(shù)系數(shù)，表示第i個(gè)障礙物所在位置信息，ρ0表示排斥勢(shì)場(chǎng)的影響范圍半徑，m表示自適應(yīng)因子，m>0；

13、以排斥勢(shì)能函數(shù)作為障礙物作用到虛擬auv的排斥力，表示為：

14、

15、式中，表示第i個(gè)障礙物作用到虛擬auv的排斥力，和分別表示第一分量和第二分量；pobs表示障礙物所在位置信息；

16、所述第一分量和第二分量表示為：

17、

18、式中，指向遠(yuǎn)離障礙物auv，指向由auv到目標(biāo)點(diǎn)。

19、根據(jù)引力函數(shù)以及排斥勢(shì)能函數(shù)確定auv在合勢(shì)場(chǎng)中所受到的合力，表示為：

20、

21、式中，ftotal為位場(chǎng)改善的合力；

22、障礙物作用到auv的排斥力隨著auv與目標(biāo)的相對(duì)距離的減小而減小，減小程度根據(jù)自適應(yīng)因子m來(lái)確定。

23、進(jìn)一步地，障礙物作用到auv的排斥力隨著auv與目標(biāo)的相對(duì)距離的減小而減小，減小程度根據(jù)自適應(yīng)因子m來(lái)確定，包括：

24、當(dāng)0<m<1時(shí)，auv順利到達(dá)目標(biāo)點(diǎn)，ρ(pv,pg)→0時(shí)第一個(gè)分量frep1(pv)→0，第二個(gè)分量frep2(pv)→∞，auv所受的合力大于零，auv的方向從auv指向目標(biāo)點(diǎn)；

25、當(dāng)m＝1，并且ρ(pv,pg)→0時(shí)，第一分量frep1(pv)→0，第二分量frep2(pv)→c，c是一個(gè)常數(shù)，auv所受的合力大于零，auv的方向從auv指向目標(biāo)點(diǎn)；

26、當(dāng)m>1，并且frep1(pv)→0時(shí)，frep(pv)→0逐漸減小，由于存在吸引勢(shì)場(chǎng)，auv在產(chǎn)生的勢(shì)場(chǎng)下順利達(dá)到目標(biāo)點(diǎn)。

27、進(jìn)一步地，所述方法還包括：

28、在確定x軸逆時(shí)針為正的情況下，在第j個(gè)路徑點(diǎn)通過(guò)如下公式計(jì)算auv與目標(biāo)點(diǎn)的連線和三軸的夾角以及auv與障礙物的連線及三軸的夾角：

29、

30、式中，αgi表示auv在第j個(gè)路徑點(diǎn)所受目標(biāo)點(diǎn)吸引力與x軸之間的夾角，βgi表示auv在第j個(gè)路徑點(diǎn)所受目標(biāo)點(diǎn)吸引力與y軸之間的夾角，γgi表示auv在第j個(gè)路徑點(diǎn)所受目標(biāo)點(diǎn)吸引力與z軸之間的夾角，pvj表示auv在第j個(gè)路徑點(diǎn)所在的位置，xg表示目標(biāo)點(diǎn)所在位置的x軸坐標(biāo)，xvj表示auv在第j個(gè)路徑點(diǎn)所在的位置的x軸坐標(biāo)，yg表示目標(biāo)點(diǎn)所在位置的y軸坐標(biāo)，yvj表示auv在第j個(gè)路徑點(diǎn)所在的位置的y軸坐標(biāo)，zg表示目標(biāo)點(diǎn)所在位置的z軸坐標(biāo)，zvj表示auv在第j個(gè)路徑點(diǎn)所在的位置的z軸坐標(biāo)，αoj表示auv在第j個(gè)路徑點(diǎn)所受障礙物排斥力與x軸之間的夾角，βoj表示auv在第j個(gè)路徑點(diǎn)所受障礙物排斥力與y軸之間的夾角，γoj表示auv在第j個(gè)路徑點(diǎn)所受障礙物排斥力與z軸之間的夾角，pobs表示障礙物所在的位置，xobs表示障礙物所在位置的x軸坐標(biāo)，yobs表示障礙物所在位置的y軸坐標(biāo)，zobs表示障礙物所在位置的x軸坐標(biāo)。；

31、基于計(jì)算得到的吸引力和斥力，通過(guò)如下公式計(jì)算出當(dāng)前路徑點(diǎn)下auv三軸上的吸引力和斥力分量：

32、(fattx(pv),fatty(pv),fattz(pv))＝fatt(pv)(αgi,βgi,γgi)

33、(frep1x(pv),frep1y(pv),frep1z(pv))＝frep1(pv)(αoj,βoj,γoj)

34、(frep2x(pv),frep2y(pv),frep2z(pv))＝frep2(pv)(αgi,βgi,γgi)

35、式中，(fattx(pv),fatty(pv),fattz(pv))表示目標(biāo)點(diǎn)對(duì)auv吸引力在三個(gè)坐標(biāo)軸方向上的分力，fatt(pv)表示目標(biāo)點(diǎn)對(duì)auv吸引力，(frep1x(pv),frep1y(pv),frep1z(pv))表示障礙物對(duì)auv排斥力1在三個(gè)坐標(biāo)軸方向上的分力，frep1(pv)表示障礙物對(duì)auv的排斥力1，(frep2x(pv),frep2y(pv),frep2z(pv))表示目標(biāo)點(diǎn)對(duì)auv排斥力2在三個(gè)坐標(biāo)軸方向上的分力，frep2(pv)表示目標(biāo)點(diǎn)對(duì)auv的排斥力2；

36、根據(jù)合力計(jì)算下一路徑點(diǎn)的位置pj+1(xj+1,yj+1,zj+1)，表示為：

37、

38、式中，xv(j+1)、yv(j+1)和zv(j+1)分別表示，αtotj、βtotj和γtotj分別表示合力與x,y,z軸之間的夾角。

39、進(jìn)一步地，所述方法還包括：

40、構(gòu)建變形因子；其中，所述變形因子為多auv的編隊(duì)隊(duì)形在避障過(guò)程中保持標(biāo)稱構(gòu)型的拓?fù)浣Y(jié)構(gòu)不變，利用仿射變換的方式，在編隊(duì)結(jié)構(gòu)大小和形狀上變化通過(guò)障礙區(qū)的參數(shù)；

41、根據(jù)變形因子確定切換指令；其中，所述切換指令代表不同的隊(duì)形切換模式。

42、進(jìn)一步地，所述變形因子的表達(dá)式為：

43、

44、式中，χ表示變形因子，χ>χm,χm為編隊(duì)重構(gòu)因子的閾值，是指保證編隊(duì)中auv與障礙物之間不碰撞的變形因子的最小值；d為多auv編隊(duì)標(biāo)稱構(gòu)型的最大寬度，dmin為編隊(duì)系統(tǒng)前進(jìn)方向上，可通行障礙區(qū)的最小寬度。

45、進(jìn)一步地，切換指令表示為σ，σ∈{σi|i＝0,1,2,3}，根據(jù)變形因子確定切換指令，包括：

46、若χ≥1，dmin≥d，則σ＝0，代表多auv編隊(duì)結(jié)構(gòu)保持零變換模式，表示多auv編隊(duì)在不改變當(dāng)前隊(duì)形或旋轉(zhuǎn)當(dāng)前隊(duì)形的情況下，能夠直接通過(guò)障礙區(qū)；

47、若χmin＜χ＜1，則σ＝1，代表多auv編隊(duì)結(jié)構(gòu)同構(gòu)變換模式，表示編隊(duì)系統(tǒng)不能以當(dāng)前隊(duì)形直接通過(guò)障礙區(qū)，通過(guò)將隊(duì)形按比例縮小或進(jìn)行剪切或位移變形，進(jìn)而無(wú)碰通過(guò)障礙區(qū)；

48、若χ＜χm，則σ＝2，代表多auv編隊(duì)結(jié)構(gòu)異構(gòu)變換模式，表示編隊(duì)系統(tǒng)只能通過(guò)剪切和位移的綜合操作來(lái)重構(gòu)變換當(dāng)前隊(duì)形，才能通過(guò)障礙區(qū)；

49、三維空間中，當(dāng)編隊(duì)auv航行至障礙水域時(shí)，σ＝3，代表多auv編隊(duì)結(jié)構(gòu)平面化模式，通過(guò)混合編隊(duì)重構(gòu)方式，將編隊(duì)構(gòu)型變換到同一平面，以通過(guò)障礙水域。

50、進(jìn)一步地，所述方法還包括構(gòu)建多auv三維編隊(duì)重構(gòu)避障控制器；其中，構(gòu)建多auv三維編隊(duì)重構(gòu)避障控制器具體包括：

51、利用pi＝(xi,yi,zi)表示auvi在三維空間中的位置，其中i＝(1,2.....n)，vi＝(vxi,vyi,vzi)表示第i個(gè)auv的速度向量，ui＝(uxi,uyi,uzi)表示第i個(gè)auv的控制輸入，則有：

52、

53、式中，表示auvi位置pi的一階導(dǎo)數(shù)，即速度，表示auvi速度vi的一階導(dǎo)數(shù)，即加速度；

54、確定任意兩臺(tái)auv之間的相對(duì)位置向量如下:

55、

56、式中，表示auvi位置和auvj位置之間的相對(duì)位置，pi表示auvi位置，pj表示auvj位置；

57、距離誤差ei表示為：

58、

59、式中，dij表示期望編隊(duì)中，auvi和auvj之間的期望編隊(duì)距離；

60、確定距離誤差動(dòng)力學(xué)為：

61、

62、式中，表示距離誤差eij的一階導(dǎo)數(shù)，t表示時(shí)間，表示期望距離dij的一階導(dǎo)數(shù)，t表示矩陣呢個(gè)的轉(zhuǎn)置；

63、引入如下替代誤差變量zij：

64、

65、利用距離誤差驗(yàn)證替代誤差變量，表示為：

66、

67、通過(guò)如下公式計(jì)算zij的開(kāi)環(huán)動(dòng)力學(xué)為：

68、

69、式中，表示參數(shù)變量zij的一階導(dǎo)數(shù)；

70、引入lyapunov函數(shù)，表示為

71、

72、式中，wij表示引入的lyapunov函數(shù)，z表示引入的參數(shù)變量；

73、定義如下第一函數(shù):

74、

75、式中，w(e)表示引入的lyapunov函數(shù)，e*表示編隊(duì)的邊集，eij表示示距離誤差；

76、第一函數(shù)沿距離誤差動(dòng)力學(xué)的時(shí)間導(dǎo)數(shù)由如下公式給出：

77、

78、式中，表示w(e)的一階導(dǎo)數(shù)；

79、給出剛性矩陣r(p)：

80、

81、其中f(p)＝(...,||pi-pj||2,...)，表示關(guān)于多臺(tái)auv的距離矩陣,p＝(p1,p2,...,pn)，表示編隊(duì)中各auv的位置，。

82、將第一函數(shù)沿距離誤差動(dòng)力學(xué)的時(shí)間導(dǎo)數(shù)寫(xiě)成：

83、

84、其中表示編隊(duì)中各auv的速度，表示關(guān)于距離的一個(gè)變量。

85、引入如下變量s：

86、s＝v-vd

87、式中，表示虛擬速度輸入；表示自然數(shù)集，n表示編隊(duì)中auv的數(shù)量；

88、引入第二函數(shù)：

89、

90、式中，wd(e,s)表示引入的lyapunov函數(shù)；

91、對(duì)第二函數(shù)求導(dǎo)后得到：

92、

93、其中為編隊(duì)中各auv的控制輸入，表示wd(e,s)的一階導(dǎo)數(shù)，表示期望速度vd的一階導(dǎo)數(shù)；

94、確定控制律：

95、

96、ua＝r+(p)(-kvz+dv)

97、其中ka,kv>0，ka,kv分別表示定常系數(shù)；r+(p)表示r(p)的摩爾-彭羅斯偽逆，vm表示編隊(duì)航行速度，vd表示編隊(duì)期望運(yùn)動(dòng)速度，ua表示編隊(duì)形成控制輸入；

98、確定多auv三維編隊(duì)重構(gòu)避障控制器表示為：

99、u＝-kas+hd-rt(p)z

100、

101、ua＝r+(p)(-kvz+dv)

102、式中，kt為正常數(shù)，et＝pv-p1為虛擬領(lǐng)航auv與auv1之間的位置誤差，vv為為虛擬auv的航行速度，即規(guī)劃路徑步長(zhǎng)，ks為正常數(shù)且hd表示，表示vd的一階導(dǎo)數(shù)，表示ua的一階導(dǎo)數(shù)。

103、本發(fā)明的有益效果是：

104、本發(fā)明通過(guò)改進(jìn)人工勢(shì)場(chǎng)，以有效的解決在傳統(tǒng)人工勢(shì)場(chǎng)方法中，存的在目標(biāo)區(qū)域不可達(dá)等問(wèn)題。首次將仿射變換強(qiáng)大的變形能力與iapf高效的避障效率有效地結(jié)合起來(lái)，提出基于仿射變換的三維編隊(duì)隊(duì)形收縮，隊(duì)形位移，隊(duì)形剪切和隊(duì)形轉(zhuǎn)載等隊(duì)形時(shí)變換避障方式，為多auv編隊(duì)整體避障提供隊(duì)形變換選擇。