一種基于行為的多水下機(jī)器人路徑規(guī)劃方法與流程

本發(fā)明涉及一種基于行為的多水下機(jī)器人路徑規(guī)劃方法，是一種基于行為的多水下機(jī)器人協(xié)同信息采集的路徑規(guī)劃方法。

背景技術(shù)：

多水下機(jī)器人(multipleautonomousunderwatervehicles，簡稱mauv)是指由多個(gè)相對(duì)比較簡單的同構(gòu)或異構(gòu)的auv通過某種形式的協(xié)同合作，最終可以共同完成特定的比較復(fù)雜的作業(yè)任務(wù)的系統(tǒng)。多水下機(jī)器人的并行性可以顯著提高auv工作性能，縮短任務(wù)時(shí)間，并且通過系統(tǒng)中不同個(gè)體的協(xié)調(diào)來增加成功完成任務(wù)的可能性。此外，多水下機(jī)器人系統(tǒng)相比于單水下機(jī)器人具有效率更高、成本更低、靈活性高、魯棒性強(qiáng)等優(yōu)點(diǎn)。

發(fā)揮多水下機(jī)器人協(xié)作優(yōu)勢的關(guān)鍵是在系統(tǒng)中每個(gè)auv都被分配明確任務(wù)的前提下，通過既定的規(guī)劃和控制策略，組成一個(gè)使不同auv自身進(jìn)行作業(yè)任務(wù)的同時(shí)還能夠兼顧與其他auv合作和協(xié)調(diào)的團(tuán)隊(duì)，通過發(fā)揮團(tuán)隊(duì)意識(shí)和協(xié)作精神最終完成復(fù)雜的任務(wù)。對(duì)于數(shù)據(jù)收集任務(wù)，auv需要安全地靠近分布在海洋中的節(jié)點(diǎn)進(jìn)而通訊獲得數(shù)據(jù)。在水下海洋環(huán)境中，為了自主完成數(shù)據(jù)收集任務(wù)，對(duì)于多水下機(jī)器人系統(tǒng)來說，有效的路徑規(guī)劃策略是非常有必要的。傳統(tǒng)的多水下機(jī)器人路徑規(guī)劃方法大多只考慮auv執(zhí)行任務(wù)時(shí)，不與其它auv發(fā)生碰撞，并未考慮auv與其他成員可能失去聯(lián)系，從而不能完成協(xié)同任務(wù)。

本發(fā)明提出了一種適用于動(dòng)態(tài)未知環(huán)境下基于行為的多水下機(jī)器人路徑規(guī)劃策略，通過定義基本行為來對(duì)auv的航行路徑添加約束，之后建立行為目標(biāo)函數(shù)和全局目標(biāo)函數(shù)來解決協(xié)同信息采集過程中的安全和協(xié)作問題。本發(fā)明采用一種帶有慣性權(quán)重的粒子群優(yōu)化算法來求解全局目標(biāo)函數(shù)，通過輸出的最優(yōu)解可以生成一條免于碰撞并且是最短的路徑。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的是為了提供一種在保證多個(gè)水下機(jī)器人協(xié)同完成數(shù)據(jù)收集任務(wù)的同時(shí)，使用最少的能耗的安全的路徑規(guī)劃算法。

本發(fā)明的目的是這樣實(shí)現(xiàn)的：(1)定義3個(gè)基本行為用來約束auv的運(yùn)動(dòng)，保證在安全的前提下，auv的航行距離最短，并且實(shí)現(xiàn)與其它auv的協(xié)同；

(2)行為目標(biāo)函數(shù)與基本行為對(duì)應(yīng)，是基本行為的具體數(shù)學(xué)化，定義在auv的決策空間中，并要保證函數(shù)的收斂性；

(3)全局目標(biāo)函數(shù)考慮到了3個(gè)基本行為的綜合影響，利用帶有慣性權(quán)重的粒子群優(yōu)化算法來求解不同時(shí)刻全局目標(biāo)函數(shù)的最優(yōu)解，不同時(shí)刻全局目標(biāo)函數(shù)的最優(yōu)解組成auv的路徑軌跡。

本發(fā)明還包括這樣一些結(jié)構(gòu)特征：

1.所述3個(gè)基本行為的目標(biāo)函數(shù)具體為：

路徑規(guī)劃的環(huán)境設(shè)置為二維平面，u為t時(shí)刻auv的速度大小，θ為當(dāng)前的艏向角，(xt,yt)為該時(shí)刻的位置坐標(biāo)，δt為時(shí)間間隔，auv的決策輸出包括速度和艏向角，假設(shè)(fx,fy)為下一時(shí)刻auv的位置坐標(biāo)，得到：

fx＝xt+u·δt·cosθ

fy＝y(tǒng)t+u·δt·sinθ

a、節(jié)能目標(biāo)函數(shù)f1(θ,u,t)為：

其中：(xs,ys)為目標(biāo)點(diǎn)的位置坐標(biāo)，d1為下一時(shí)刻auv與目標(biāo)點(diǎn)間的歐氏距離，s1和s2是放縮系數(shù)；

b、協(xié)同目標(biāo)函數(shù)f2(θ,u,t)為：

fbx＝xb+ub·δt·cosθb,fby＝y(tǒng)b+ub·δt·cosθb

其中：(fbx,fby)為與該auv距離最近的auv下一時(shí)刻的位置，d2為下一時(shí)刻的兩個(gè)auv的歐氏距離，[dmin,dmax]為d2的期望取值范圍，s1和s2是放縮系數(shù)，m2為設(shè)定的常值；

c、安全行為目標(biāo)函數(shù)f3(θ,u,t)為：

其中：(xo,yo)為障礙物點(diǎn)的位置坐標(biāo)，d3為auv與障礙物下一時(shí)刻的歐氏距離，ds為距離d3的安全閾值，m3為設(shè)定的常值。

2.所述建立并求解全局目標(biāo)函數(shù)的過程如下：

全局目標(biāo)函數(shù)具體的形式為：

f＝f(θ,u,t)＝ω1f1+ω2f2+ω3f3

式中：ω1,ω2,ω3是可變化的權(quán)重系數(shù)，將3種行為按照相對(duì)重要程度排序?yàn)榘踩袨?、協(xié)同行為和節(jié)能行為，并設(shè)置3個(gè)權(quán)重系數(shù)，全局目標(biāo)函數(shù)中具有2個(gè)空間變量θ和u、1個(gè)時(shí)間變量t；

在t時(shí)刻，利用帶有慣性權(quán)重的粒子群優(yōu)化算法，得到全局目標(biāo)函數(shù)的最優(yōu)解的具體步驟如下：

步驟1：利用帶有慣性權(quán)重的粒子群優(yōu)化算法求解生成的全局目標(biāo)函數(shù)，算法開始；

步驟2：初始化帶有慣性權(quán)重的粒子群優(yōu)化算法參數(shù)，包括設(shè)置搜索空間為二維空間，其中姿態(tài)角空間取值范圍為[θ0-θmax,θ0+θmax]，速度空間取值范圍為[0,umax]，其中：θ0為auv當(dāng)前時(shí)刻姿態(tài)角，θmax為auv最大轉(zhuǎn)角能力，umax為auv的最大航行速度；群體規(guī)模為n，設(shè)置各個(gè)粒子的初始位置和速度；

步驟3：利用全局目標(biāo)函數(shù)作為適應(yīng)度函數(shù)，計(jì)算每個(gè)粒子的適應(yīng)度；

步驟4：更新粒子的個(gè)體最優(yōu)解和全局最優(yōu)解：

每個(gè)粒子所處的位置都表示二維解空間中的一個(gè)解，解的質(zhì)量由適應(yīng)值決定，某一粒子i在二維空間的位置表示為矢量xi＝(θi,ui)，粒子通過不斷調(diào)整自己的位置來搜索新解，每個(gè)粒子都能記住自己搜索到的最優(yōu)解，記作pid；以及整個(gè)粒子群經(jīng)歷過的最好的位置，即目前搜索到的最優(yōu)解，記作pgd；

步驟5：更新粒子的位置和速度：

粒子具有飛行速度，飛行速度可以表示為矢量當(dāng)上述兩個(gè)最優(yōu)解pid和pgd都找到后，每個(gè)粒子根據(jù)下式來更新自己的位置和速度；

其中，vi^k+1表示第i個(gè)粒子在k+1次迭代中的速度，c0為慣性權(quán)重，c1和c2為加速常數(shù)，rand()為0到1之間的隨機(jī)數(shù)；

對(duì)于慣性權(quán)重c0，利用先增后減慣性權(quán)重的慣性權(quán)重確定方法為：

式中，maxnumber為算法的最大迭代次數(shù)。

步驟6：判斷是否已經(jīng)達(dá)到最大迭代次數(shù)，如果達(dá)到最大迭代次數(shù)，輸出最優(yōu)解，算法結(jié)束；否則，算法的迭代次數(shù)加1，返回步驟3。

3.所述計(jì)算每個(gè)粒子的適應(yīng)度是指得到每個(gè)粒子的適應(yīng)值，每個(gè)粒子都有一個(gè)由目標(biāo)函數(shù)決定的適應(yīng)值，適應(yīng)值直接設(shè)置為全局目標(biāo)函數(shù)f(θ,u,t)。

與現(xiàn)有技術(shù)相比，本發(fā)明的有益效果是：本發(fā)明定義了auv的3種基本行為，包括節(jié)能行為、安全行為和協(xié)同行為，并建立了基本行為相對(duì)應(yīng)的局部目標(biāo)函數(shù)，從而來約束auv的航行，并可以實(shí)現(xiàn)與其它auv協(xié)同航行的目的。最后建立全局目標(biāo)函數(shù)，并利用帶有慣性權(quán)重的粒子群優(yōu)化算法進(jìn)行求解，通過利用先增后減慣性權(quán)重的慣性權(quán)重確定方法，比一般的粒子群優(yōu)化算法具有更快的收斂能力和更好的局部收斂速度。

附圖說明

圖1是本發(fā)明提出的多水下機(jī)器人路徑規(guī)劃方法示意圖；

圖2是本發(fā)明中的粒子群優(yōu)化算法的流程示意圖；

圖3是本發(fā)明中auv在時(shí)間和空間運(yùn)動(dòng)示意圖；

圖4是本發(fā)明中粒子位置改變的示意圖。

具體實(shí)施方式

下面結(jié)合附圖與具體實(shí)施方式對(duì)本發(fā)明作進(jìn)一步詳細(xì)描述。

一種基于行為的多水下機(jī)器人路徑規(guī)劃方法，包括：

1、定義3個(gè)基本行為用來約束auv的運(yùn)動(dòng)，保證在安全的前提下，auv的航行距離最短，并且實(shí)現(xiàn)與其它auv的協(xié)同。

2、行為目標(biāo)函數(shù)與基本行為對(duì)應(yīng)，是基本行為的具體數(shù)學(xué)化，定義在auv的決策空間中，并要保證函數(shù)的收斂性。

3、全局目標(biāo)函數(shù)考慮到了3個(gè)基本行為的綜合影響，利用帶有慣性權(quán)重的粒子群優(yōu)化算法來求解不同時(shí)刻全局目標(biāo)函數(shù)的最優(yōu)解，不同時(shí)刻全局目標(biāo)函數(shù)的最優(yōu)解組成auv的路徑軌跡，

所述3個(gè)行為目標(biāo)函數(shù)具體為:

路徑規(guī)劃的環(huán)境設(shè)置為二維平面，u為t時(shí)刻auv的速度大小，θ為當(dāng)前的艏向角，(xt,yt)為該時(shí)刻的位置坐標(biāo)，δt為時(shí)間間隔，auv的決策輸出包括速度和艏向角。假設(shè)(fx,fy)為下一時(shí)刻auv的位置坐標(biāo)，可以得到下式：

fx＝xt+u·δt·cosθ

fy＝y(tǒng)t+u·δt·sinθ

1)節(jié)能目標(biāo)函數(shù)可以表示為：

(xs,ys)為目標(biāo)點(diǎn)的位置坐標(biāo)為，d1為下一時(shí)刻auv與目標(biāo)點(diǎn)間的歐氏距離。其中s1和s2是放縮系數(shù)。

2)協(xié)同目標(biāo)函數(shù)可以表示為：

fbx＝xb+ub·δt·cosθb,fby＝y(tǒng)b+ub·δt·cosθb

(fbx,fby)為與該auv距離最近的auv下一時(shí)刻的位置，d2為下一時(shí)刻的兩個(gè)auv的歐氏距離，[dmin,dmax]為d2的期望取值范圍，s1和s2是放縮系數(shù)，m2為設(shè)定的常值。

3)安全行為目標(biāo)函數(shù)可以表示如下:

(xo,yo)為障礙物點(diǎn)的位置坐標(biāo)，d3為auv與障礙物下一時(shí)刻的歐氏距離，ds為距離d3的安全閾值，m3為設(shè)定的常值。

所述建立并求解全局目標(biāo)函數(shù)的過程如下：

全局目標(biāo)函數(shù)具體的形式如下式所示：

f＝f(θ,u,t)＝ω1f1+ω2f2+ω3f3

式中，ω1,ω2,ω3是可變化的權(quán)重系數(shù)。將3種行為按照相對(duì)重要程度排序?yàn)榘踩袨?、協(xié)同行為和節(jié)能行為，按照這個(gè)原則來設(shè)置3個(gè)權(quán)重系數(shù)。全局目標(biāo)函數(shù)中具有2個(gè)空間變量θ和u，1個(gè)時(shí)間變量t。

在t時(shí)刻，下面利用帶有慣性權(quán)重的粒子群優(yōu)化算法，來求全局目標(biāo)函數(shù)的最優(yōu)解，具體步驟如下：

步驟1：利用帶有慣性權(quán)重的粒子群優(yōu)化算法求解生成的全局目標(biāo)函數(shù)，算法開始；

步驟2：初始化帶有慣性權(quán)重的粒子群優(yōu)化算法參數(shù)，包括設(shè)置搜索空間為二維空間，其中姿態(tài)角空間取值范圍為[θ0-θmax,θ0+θmax]，速度空間取值范圍為[0,umax]，這里θ0為auv當(dāng)前時(shí)刻姿態(tài)角，θmax為auv最大轉(zhuǎn)角能力，umax為auv的最大航行速度；群體規(guī)模為n，設(shè)置各個(gè)粒子的初始位置和速度；

步驟3：利用全局目標(biāo)函數(shù)作為適應(yīng)度函數(shù)，計(jì)算每個(gè)粒子的適應(yīng)度。每個(gè)粒子都有一個(gè)由目標(biāo)函數(shù)決定的適應(yīng)值(fitnessvalue)，這里適應(yīng)值可以直接設(shè)置為全局目標(biāo)函數(shù)f(θ,u,t)；

步驟4：更新粒子的個(gè)體最優(yōu)解和全局最優(yōu)解。每個(gè)粒子所處的位置都表示二維解空間中的一個(gè)解，解的質(zhì)量由適應(yīng)值決定，適應(yīng)度越大，解的質(zhì)量越好。某一粒子i在二維空間的位置表示為矢量xi＝(θi,ui)，粒子通過不斷調(diào)整自己的位置來搜索新解，每個(gè)粒子都能記住自己搜索到的最優(yōu)解，記作pid；以及整個(gè)粒子群經(jīng)歷過的最好的位置，即目前搜索到的最優(yōu)解，記作pgd；

步驟5：更新粒子的位置和速度。粒子具有飛行速度，從而可以調(diào)整自己的位置。飛行速度可以表示為矢量當(dāng)上述兩個(gè)最優(yōu)解pid和pgd都找到后，每個(gè)粒子根據(jù)下式來更新自己的位置和速度；

其中，vi^k+1表示第i個(gè)粒子在k+1次迭代中的速度，c0為慣性權(quán)重，c1和c2為加速常數(shù)，rand()為0到1之間的隨機(jī)數(shù)；

對(duì)于慣性權(quán)重c0，利用先增后減慣性權(quán)重的慣性權(quán)重確定方法，該方法在迭代的初期，具有較快的收斂速度；在迭代的后期，具有較好的局部搜索能力，具體方法如下式所示：

式中，maxnumber為算法的最大迭代次數(shù)。

步驟6：判斷是否已經(jīng)達(dá)到最大迭代次數(shù)，如果達(dá)到最大迭代次數(shù)，輸出最優(yōu)解，算法結(jié)束。否則，算法的迭代次數(shù)加1，返回步驟3。

下面結(jié)合附圖對(duì)本發(fā)明進(jìn)行詳細(xì)描述：

如圖1所示，本發(fā)明提出的一種基于行為的多水下機(jī)器人路徑規(guī)劃方法，主要包含以下部分：3個(gè)基本行為，分別是節(jié)能行為、協(xié)同行為和安全行為；與3個(gè)基本行為對(duì)應(yīng)的行為目標(biāo)函數(shù)；通過3個(gè)行為目標(biāo)函數(shù)生成的全局目標(biāo)函數(shù)，并對(duì)全局目標(biāo)函數(shù)進(jìn)行求解。3個(gè)基本行為用來約束auv的運(yùn)動(dòng)，保證在安全的前提下，auv的航行距離最短，并且實(shí)現(xiàn)與其它auv的協(xié)同。行為目標(biāo)函數(shù)分別是對(duì)應(yīng)3個(gè)基本行為的具體數(shù)學(xué)化，行為目標(biāo)函數(shù)需要定義在auv的決策空間中，并要保證函數(shù)的收斂性。全局目標(biāo)函數(shù)考慮到了3個(gè)基本行為的綜合影響，不同時(shí)刻全局目標(biāo)函數(shù)的最優(yōu)解組成auv的路徑軌跡，本發(fā)明利用帶有慣性權(quán)重的粒子群優(yōu)化算法來求解不同時(shí)刻全局目標(biāo)函數(shù)的最優(yōu)解。

(1)所述的基本行為通過以下方式定義：

路徑規(guī)劃的環(huán)境設(shè)置為二維平面，建立環(huán)境地圖的全局坐標(biāo)系o-xy。如圖1所示，當(dāng)auv獲取需要的周圍環(huán)境信息后，這些信息包括目標(biāo)點(diǎn)的位置，其它auv的位置速度大小和艏向角，以及障礙物位置信息。在這里，u為t時(shí)刻auv的速度大小，θ為當(dāng)前的艏向角，(xt,yt)為該時(shí)刻的位置坐標(biāo)，δt為時(shí)間間隔，auv的決策輸出包括速度和艏向角，如圖3所示為auv在時(shí)間和空間上的運(yùn)動(dòng)示意圖。

假設(shè)(fx,fy)為下一時(shí)刻auv的位置坐標(biāo)，可以表示為下式：

fx＝xt+u·δt·cosθ

fy＝y(tǒng)t+u·δt·sinθ

1.1)節(jié)能行為

考慮到auv工作時(shí)的能源有限，為了簡化問題，本發(fā)明只考慮航行距離的影響，并且認(rèn)為，航行距離越短，節(jié)能性越好。為了定義節(jié)能行為，可以對(duì)auv的航行條件添加一些約束，并把約束條件定為auv與目標(biāo)點(diǎn)的距離。

d1為下一時(shí)刻auv與目標(biāo)點(diǎn)間的歐氏距離，節(jié)能行為在整個(gè)任務(wù)過程中一直處于激活狀態(tài)。將auv與目標(biāo)點(diǎn)的距離對(duì)應(yīng)到具體的決策空間，利用auv的姿態(tài)角、速度以及時(shí)間變量可以將d1表示如下:

d1＝d1(θ,u,t)

1.2)協(xié)同行為

考慮到實(shí)際水下傳感器以及通訊手段的限制，在保證安全的前提下，協(xié)同行為要求的數(shù)學(xué)形式可以表示如下：

dmin＜d2＜dmax

d2為下一時(shí)刻的兩個(gè)auv的歐氏距離，[dmin,dmax]為d2的期望取值范圍。以上述公式為協(xié)同行為處于激活狀態(tài)的條件，利用auv的姿態(tài)角、速度以及時(shí)間變量可以將d2表示如下:

d2＝d2(θ,u,t)

1.3)安全行為

安全行為的數(shù)學(xué)形式可以表示如下式：

d3＞dmin

d3為auv與障礙物下一時(shí)刻的歐氏距離，ds為距離d3的安全閾值。利用auv的姿態(tài)角、速度以及時(shí)間變量可以將d3表示如下式:

d3＝d3(θ,u,t)

(2)建立局部目標(biāo)函數(shù):

2.1)節(jié)能目標(biāo)函數(shù)

與節(jié)能行為相對(duì)應(yīng)，節(jié)能目標(biāo)函數(shù)的變量是auv與目標(biāo)點(diǎn)的距離d1。對(duì)于節(jié)能目標(biāo)函數(shù)來說，d1的值越小，說明auv越接近目標(biāo)點(diǎn)。在時(shí)間間隔δt確定時(shí)，距離d1與auv速度和艏向角有關(guān)。對(duì)d1進(jìn)行度量處理，得到節(jié)能性的目標(biāo)函數(shù)具體形式如下式:

式中s1和s2是放縮系數(shù)。

2.2)協(xié)同目標(biāo)函數(shù)

與協(xié)同行為對(duì)應(yīng)，協(xié)同目標(biāo)函數(shù)的變量值是auv與其他成員的距離，在執(zhí)行任務(wù)的過程中，各個(gè)auv之間應(yīng)該保持協(xié)同作業(yè)，這里表現(xiàn)為auv與系統(tǒng)中最近的成員距離要處于一個(gè)閾值[dmin,dmax]范圍內(nèi)。最終的協(xié)同目標(biāo)函數(shù)如下式所示：

fbx＝xb+ub·δt·cosθb,fby＝y(tǒng)b+ub·δt·cosθb

(fbx,fby)為與該auv距離最近的auv下一時(shí)刻的位置，d2為下一時(shí)刻的兩個(gè)auv的歐氏距離，[dmin,dmax]為d2的期望取值范圍，s1和s2是放縮系數(shù)，m2為設(shè)定的常值。

2.3)安全目標(biāo)函數(shù)

安全行為的激活需要滿足一定的限制條件，具體體現(xiàn)為auv與障礙物的距離閾值ds。最終的安全行為目標(biāo)函數(shù)如下式所示：

(xo,yo)為障礙物點(diǎn)的位置坐標(biāo)，d3為auv與障礙物下一時(shí)刻的歐氏距離，ds為距離d3的安全閾值，m3為設(shè)定的常值。

(3)所述的全局目標(biāo)函數(shù)建立方法和求解過程如下：

全局目標(biāo)函數(shù)的作用是實(shí)現(xiàn)之前一系列基本行為的協(xié)調(diào)，全局目標(biāo)函數(shù)具體的形式如下式所示：

f＝f(θ,u,t)＝ω1f1+ω2f2+ω3f3

式中，ω1,ω2,ω3是可變化的權(quán)重系數(shù)。將3種行為按照相對(duì)重要程度排序?yàn)榘踩袨?、協(xié)同行為和節(jié)能行為，按照這個(gè)原則來設(shè)置3個(gè)權(quán)重系數(shù)。全局目標(biāo)函數(shù)中具有2個(gè)空間變量θ和u，1個(gè)時(shí)間變量t。

在t時(shí)刻，利用帶有慣性權(quán)重的粒子群優(yōu)化算法，來求全局目標(biāo)函數(shù)的最優(yōu)解，函數(shù)輸出為空間變量θ和u。如圖2所示，具體步驟如下：

步驟1：利用帶有慣性權(quán)重的粒子群優(yōu)化算法求解生成的全局目標(biāo)函數(shù)，算法開始；

步驟2：初始化帶有慣性權(quán)重的粒子群優(yōu)化算法參數(shù)，包括設(shè)置搜索空間為二維空間，其中姿態(tài)角空間取值范圍為[θ0-θmax,θ0+θmax]，速度空間取值范圍為[0,umax]，這里θ0為auv當(dāng)前時(shí)刻姿態(tài)角，θmax為auv最大轉(zhuǎn)角能力，umax為auv的最大航行速度；群體規(guī)模為n，設(shè)置各個(gè)粒子的初始位置和速度。

步驟3：利用全局目標(biāo)函數(shù)作為適應(yīng)度函數(shù)，計(jì)算每個(gè)粒子的適應(yīng)度。每個(gè)粒子都有一個(gè)由目標(biāo)函數(shù)決定的適應(yīng)值(fitnessvalue)，這里適應(yīng)值設(shè)置為全局目標(biāo)函數(shù)f(θ,u,t)。

步驟4：更新粒子的個(gè)體最優(yōu)解和全局最優(yōu)解。每個(gè)粒子所處的位置都表示二維解空間中的一個(gè)解，解的質(zhì)量由適應(yīng)值決定，適應(yīng)度越大，解的質(zhì)量越好。某一粒子i在二維空間的位置表示為矢量xi＝(θi,ui)，粒子通過不斷調(diào)整自己的位置來搜索新解，每個(gè)粒子都能記住自己搜索到的最優(yōu)解，記作pid；以及整個(gè)粒子群經(jīng)歷過的最好的位置，即目前搜索到的最優(yōu)解，記作pgd。

步驟5：更新粒子的位置和速度。粒子具有飛行速度，從而可以調(diào)整自己的位置。飛行速度可以表示為矢量當(dāng)上述兩個(gè)最優(yōu)解pid和pgd都找到后，每個(gè)粒子根據(jù)下式來更新自己的位置和速度，粒子的位置和速度更新過程具體如圖4所示。

式中，vi^k+1表示第i個(gè)粒子在k+1次迭代中的速度，c0為慣性權(quán)重，c1和c2為加速常數(shù)，rand()為0到1之間的隨機(jī)數(shù)。

對(duì)于慣性權(quán)重c0，利用先增后減慣性權(quán)重的慣性權(quán)重確定方法，該方法在迭代的初期，具有較快的收斂速度；在迭代的后期，具有較好的局部搜索能力，具體方法如下式所示：

式中，maxnumber為算法的最大迭代次數(shù)。

步驟6：判斷是否已經(jīng)達(dá)到最大迭代次數(shù)，如果達(dá)到最大迭代次數(shù)，輸出最優(yōu)解，算法結(jié)束。否則，算法的迭代次數(shù)加1，返回步驟3。

當(dāng)算法達(dá)到最大迭代次數(shù)時(shí)便會(huì)終止；此時(shí)輸出最優(yōu)解，即整個(gè)粒子群經(jīng)歷過的最好的位置pgd，該位置對(duì)應(yīng)的矢量xi＝(θi,ui)代表auv在時(shí)刻t應(yīng)該輸出的姿態(tài)角和速度。最后，利用不同時(shí)刻粒子群優(yōu)化算法輸出的最優(yōu)解對(duì)應(yīng)的姿態(tài)角和速度，可以生成auv最后的路徑。

(1)定義基本行為：

路徑規(guī)劃的環(huán)境設(shè)置為二維平面，建立環(huán)境地圖的全局坐標(biāo)系o-xy。當(dāng)auv獲取需要的周圍環(huán)境信息后，這些信息包括目標(biāo)點(diǎn)的位置，其它auv的位置速度大小和艏向角，以及障礙物位置信息。在這里，u為auv的速度大小，θ為艏向角，(xt,yt)為t時(shí)刻的位置坐標(biāo)，δt為時(shí)間間隔，auv的決策輸出包括速度和艏向角。

假設(shè)(fx,fy)為下一時(shí)刻auv的位置坐標(biāo)，可以表示為下式：

fx＝xt+u·δt·cosθ

fy＝y(tǒng)t+u·δt·sinθ

1.1)節(jié)能行為

考慮到auv工作時(shí)的能源有限，為了簡化問題，本發(fā)明只考慮航行距離的影響，并且認(rèn)為，航行距離越短，節(jié)能性越好。為了定義節(jié)能行為，可以對(duì)auv的航行條件添加一些約束，并把約束條件定為auv與目標(biāo)點(diǎn)的距離。d1為下一時(shí)刻auv與目標(biāo)點(diǎn)間的歐氏距離，節(jié)能行為在整個(gè)任務(wù)過程中一直處于激活狀態(tài)。將auv與目標(biāo)點(diǎn)的距離對(duì)應(yīng)到具體的決策空間，利用auv的姿態(tài)角、速度以及時(shí)間變量可以將d1表示如下:

d1＝d1(θ,u,t)

1.2)考慮到實(shí)際水下傳感器以及通訊手段的限制，在保證安全的前提下，協(xié)同行為要求的數(shù)學(xué)形式可以表示如下：

dmin＜d2＜dmax

d2為下一時(shí)刻的兩個(gè)auv的歐氏距離，[dmin,dmax]為d2的期望取值范圍。以上述公式為協(xié)同行為處于激活狀態(tài)的條件，利用auv的姿態(tài)角、速度以及時(shí)間變量可以將d2表示如下:

d2＝d2(θ,u,t)

1.3)安全行為數(shù)學(xué)形式可以表示如下：

d3＞dmin

d3為auv與障礙物下一時(shí)刻的歐氏距離，ds為距離d3的安全閾值。利用auv的姿態(tài)角、速度以及時(shí)間變量可以將d3表示如下:

d3＝d3(θ,u,t)

(2)建立局部目標(biāo)函數(shù):

2.1)節(jié)能目標(biāo)函數(shù)

與節(jié)能行為相對(duì)應(yīng)，節(jié)能目標(biāo)函數(shù)的變量是auv與目標(biāo)點(diǎn)的距離。假設(shè)目標(biāo)點(diǎn)的位置坐標(biāo)為(xs,ys)，d1可以表示為式：

對(duì)于節(jié)能目標(biāo)函數(shù)來說，d1的值越小，說明auv越接近目標(biāo)點(diǎn)。在時(shí)間間隔δt確定時(shí)，這個(gè)距離與auv速度和艏向角有關(guān)。對(duì)d1進(jìn)行度量處理，得到節(jié)能性的目標(biāo)函數(shù)具體形式如下式:

其中s1和s2是放縮系數(shù)。

2.2)協(xié)同目標(biāo)函數(shù)

與協(xié)同行為對(duì)應(yīng)，協(xié)同目標(biāo)函數(shù)的變量值是auv與其他成員的距離，在執(zhí)行任務(wù)的過程中，各個(gè)auv之間應(yīng)該保持協(xié)同作業(yè)，這里體現(xiàn)為auv與系統(tǒng)中最近的成員距離要處于一個(gè)閾值[dmin,dmax]范圍內(nèi)。最終的協(xié)同目標(biāo)函數(shù)如下式所示：

fbx＝xb+ub·δt·cosθb,fby＝y(tǒng)b+ub·δt·cosθb

(fbx,fby)為與該auv距離最近的auv下一時(shí)刻的位置，d2為下一時(shí)刻的兩個(gè)auv的歐氏距離，[dmin,dmax]為d2的期望取值范圍，s1和s2是放縮系數(shù)，m2為設(shè)定的常值。

2.3)安全目標(biāo)函數(shù)

安全行為的激活需要滿足一定的限制條件，具體體現(xiàn)為auv與障礙物的距離閾值ds。最終的安全行為目標(biāo)函數(shù)如下式所示：

(xo,yo)為障礙物點(diǎn)的位置坐標(biāo)，d3為auv與障礙物下一時(shí)刻的歐氏距離，ds為距離d3的安全閾值，s1和s2是放縮系數(shù)，m3為設(shè)定的常值。

(3)建立并求解全局目標(biāo)函數(shù):

全局目標(biāo)函數(shù)的作用是實(shí)現(xiàn)之前一系列基本行為的協(xié)調(diào)，具體的形式如下式所示：

f＝f(θ,u,t)＝ω1f1+ω2f2+ω3f3

式中，ω1,ω2,ω3是可變化的權(quán)重系數(shù)。將3種行為按照相對(duì)重要程度排序?yàn)榘踩袨?、協(xié)同行為和節(jié)能行為，按照這個(gè)原則來設(shè)置3個(gè)權(quán)重系數(shù)。全局目標(biāo)函數(shù)中具有2個(gè)空間變量θ和u，1個(gè)時(shí)間變量t。

下面利用帶有慣性權(quán)重的粒子群優(yōu)化算法，在t時(shí)刻，來求全局目標(biāo)函數(shù)的最優(yōu)解。

3.1)路徑規(guī)劃參數(shù)、帶有慣性權(quán)重的粒子群優(yōu)化算法參數(shù)初始化

設(shè)置搜索空間為二維空間，其中姿態(tài)角空間取值范圍為[θ0-θmax,θ0+θmax]，速度空間取值范圍為[0,umax]，這里θ0為auv當(dāng)前時(shí)刻姿態(tài)角，θmax為auv最大轉(zhuǎn)角能力，umax為auv的最大航行速度。群體規(guī)模為n，設(shè)置各個(gè)粒子的初始位置和速度；

3.2)利用粒子對(duì)路徑進(jìn)行優(yōu)化：

算法初始化之后，進(jìn)入算法的主體迭代優(yōu)化過程；粒子群算法是通過個(gè)體間的協(xié)作和競爭，實(shí)現(xiàn)復(fù)雜空間中最優(yōu)解的搜索。某一粒子i在二維空間的位置表示為矢量xi＝(θi,ui)，每個(gè)粒子所處的位置都表示二維解空間中的一個(gè)解。粒子通過不斷調(diào)整自己的位置來搜索新解，每個(gè)粒子都能記住自己搜索到的最優(yōu)解，記作pid；以及整個(gè)粒子群經(jīng)歷過的最好的位置，即目前搜索到的最優(yōu)解，記作pgd。

粒子具有飛行速度，從而可以調(diào)整自己的位置。飛行速度可以表示為矢量每個(gè)粒子都有一個(gè)由目標(biāo)函數(shù)決定的適應(yīng)值(fitnessvalue)，這里適應(yīng)值可以直接設(shè)置為全局目標(biāo)函數(shù)f(θ,u,t)；當(dāng)上述兩個(gè)最優(yōu)解pid和pgd都找到后，每個(gè)粒子根據(jù)下式來更新自己的位置和速度。

式中，vi^k+1表示第i個(gè)粒子在k+1次迭代中的速度，c0為慣性權(quán)重，c1和c2為加速常數(shù)，rand()為0到1之間的隨機(jī)數(shù)。

之后判斷是否已經(jīng)達(dá)到最大迭代次數(shù)，如果達(dá)到最大迭代次數(shù)則進(jìn)入下一步；否則，算法的迭代次數(shù)加1，繼續(xù)進(jìn)行下一步優(yōu)化過程。

3.3)輸出最優(yōu)解

當(dāng)算法達(dá)到最大迭代次數(shù)時(shí)便會(huì)終止；此時(shí)輸出最優(yōu)解，即整個(gè)粒子群經(jīng)歷過的最好的位置pgd，該位置對(duì)應(yīng)的矢量xi＝(θi,ui)代表auv在時(shí)刻t應(yīng)該輸出的姿態(tài)角和速度。

最后，利用不同時(shí)刻粒子群優(yōu)化算法輸出的最優(yōu)解對(duì)應(yīng)的姿態(tài)角和速度，可以形成auv最后的路徑。

綜上，本發(fā)明提出一種基于行為的多水下機(jī)器人路徑規(guī)劃方法，屬于路徑規(guī)劃技術(shù)領(lǐng)域。

本發(fā)明提出了一種適用于動(dòng)態(tài)未知環(huán)境下的多水下機(jī)器人路徑規(guī)劃策略，具體包括：首先，定義基本行為來對(duì)auv的航行路徑添加約束，基本行為分別為節(jié)能行為、協(xié)同行為和安全行為；然后，建立與基本行為對(duì)應(yīng)的行為目標(biāo)函數(shù)，將與auv有關(guān)的時(shí)間變量和空間變量結(jié)合起來；最后，建立全局目標(biāo)函數(shù)來實(shí)現(xiàn)3種基本行為的行為融合，并采用一種帶有慣性權(quán)重的粒子群優(yōu)化算法來求解全局目標(biāo)函數(shù)，通過輸出的最優(yōu)解可以生成一條免于碰撞并且是最短的路徑。