日韩成人黄色,透逼一级毛片,狠狠躁天天躁中文字幕,久久久久久亚洲精品不卡,在线看国产美女毛片2019,黄片www.www,一级黄色毛a视频直播

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

文檔序號(hào):11250227閱讀:1073來(lái)源:國(guó)知局
一種基于行為的多水下機(jī)器人路徑規(guī)劃方法與流程

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



背景技術(shù):

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

發(fā)揮多水下機(jī)器人協(xié)作優(yōu)勢(shì)的關(guān)鍵是在系統(tǒng)中每個(gè)auv都被分配明確任務(wù)的前提下,通過(guò)既定的規(guī)劃和控制策略,組成一個(gè)使不同auv自身進(jìn)行作業(yè)任務(wù)的同時(shí)還能夠兼顧與其他auv合作和協(xié)調(diào)的團(tuán)隊(duì),通過(guò)發(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)來(lái)說(shuō),有效的路徑規(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ī)劃策略,通過(guò)定義基本行為來(lái)對(duì)auv的航行路徑添加約束,之后建立行為目標(biāo)函數(shù)和全局目標(biāo)函數(shù)來(lái)解決協(xié)同信息采集過(guò)程中的安全和協(xié)作問(wèn)題。本發(fā)明采用一種帶有慣性權(quán)重的粒子群優(yōu)化算法來(lái)求解全局目標(biāo)函數(shù),通過(guò)輸出的最優(yōu)解可以生成一條免于碰撞并且是最短的路徑。



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

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

本發(fā)明的目的是這樣實(shí)現(xiàn)的:(1)定義3個(gè)基本行為用來(lái)約束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)化算法來(lái)求解不同時(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ù)的過(guò)程如下:

全局目標(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ù),算法開(kāi)始;

步驟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),粒子通過(guò)不斷調(diào)整自己的位置來(lái)搜索新解,每個(gè)粒子都能記住自己搜索到的最優(yōu)解,記作pid;以及整個(gè)粒子群經(jīng)歷過(guò)的最好的位置,即目前搜索到的最優(yōu)解,記作pgd;

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

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

其中,vik+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ù),從而來(lái)約束auv的航行,并可以實(shí)現(xiàn)與其它auv協(xié)同航行的目的。最后建立全局目標(biāo)函數(shù),并利用帶有慣性權(quán)重的粒子群優(yōu)化算法進(jìn)行求解,通過(guò)利用先增后減慣性權(quán)重的慣性權(quán)重確定方法,比一般的粒子群優(yōu)化算法具有更快的收斂能力和更好的局部收斂速度。

附圖說(shuō)明

圖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è)基本行為用來(lái)約束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)化算法來(lái)求解不同時(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ù)的過(guò)程如下:

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

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

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

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

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

步驟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),粒子通過(guò)不斷調(diào)整自己的位置來(lái)搜索新解,每個(gè)粒子都能記住自己搜索到的最優(yōu)解,記作pid;以及整個(gè)粒子群經(jīng)歷過(guò)的最好的位置,即目前搜索到的最優(yōu)解,記作pgd;

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

其中,vik+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ù);通過(guò)3個(gè)行為目標(biāo)函數(shù)生成的全局目標(biāo)函數(shù),并對(duì)全局目標(biāo)函數(shù)進(jìn)行求解。3個(gè)基本行為用來(lái)約束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)化算法來(lái)求解不同時(shí)刻全局目標(biāo)函數(shù)的最優(yōu)解。

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

路徑規(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í)的能源有限,為了簡(jiǎn)化問(wèn)題,本發(fā)明只考慮航行距離的影響,并且認(rèn)為,航行距離越短,節(jié)能性越好。為了定義節(jié)能行為,可以對(duì)auv的航行條件添加一些約束,并把約束條件定為auv與目標(biāo)點(diǎn)的距離。

d1為下一時(shí)刻auv與目標(biāo)點(diǎn)間的歐氏距離,節(jié)能行為在整個(gè)任務(wù)過(guò)程中一直處于激活狀態(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ù)來(lái)說(shuō),d1的值越小,說(shuō)明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ù)的過(guò)程中,各個(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ù)建立方法和求解過(guò)程如下:

全局目標(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è)原則來(lái)設(shè)置3個(gè)權(quán)重系數(shù)。全局目標(biāo)函數(shù)中具有2個(gè)空間變量θ和u,1個(gè)時(shí)間變量t。

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

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

步驟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),粒子通過(guò)不斷調(diào)整自己的位置來(lái)搜索新解,每個(gè)粒子都能記住自己搜索到的最優(yōu)解,記作pid;以及整個(gè)粒子群經(jīng)歷過(guò)的最好的位置,即目前搜索到的最優(yōu)解,記作pgd。

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

式中,vik+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)歷過(guò)的最好的位置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í)的能源有限,為了簡(jiǎn)化問(wèn)題,本發(fā)明只考慮航行距離的影響,并且認(rèn)為,航行距離越短,節(jié)能性越好。為了定義節(jié)能行為,可以對(duì)auv的航行條件添加一些約束,并把約束條件定為auv與目標(biāo)點(diǎn)的距離。d1為下一時(shí)刻auv與目標(biāo)點(diǎn)間的歐氏距離,節(jié)能行為在整個(gè)任務(wù)過(guò)程中一直處于激活狀態(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ù)來(lái)說(shuō),d1的值越小,說(shuō)明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ù)的過(guò)程中,各個(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)榘踩袨椤f(xié)同行為和節(jié)能行為,按照這個(gè)原則來(lái)設(shè)置3個(gè)權(quán)重系數(shù)。全局目標(biāo)函數(shù)中具有2個(gè)空間變量θ和u,1個(gè)時(shí)間變量t。

下面利用帶有慣性權(quán)重的粒子群優(yōu)化算法,在t時(shí)刻,來(lái)求全局目標(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)化過(guò)程;粒子群算法是通過(guò)個(gè)體間的協(xié)作和競(jìng)爭(zhēng),實(shí)現(xiàn)復(fù)雜空間中最優(yōu)解的搜索。某一粒子i在二維空間的位置表示為矢量xi=(θi,ui),每個(gè)粒子所處的位置都表示二維解空間中的一個(gè)解。粒子通過(guò)不斷調(diào)整自己的位置來(lái)搜索新解,每個(gè)粒子都能記住自己搜索到的最優(yōu)解,記作pid;以及整個(gè)粒子群經(jīng)歷過(guò)的最好的位置,即目前搜索到的最優(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ù)下式來(lái)更新自己的位置和速度。

式中,vik+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)化過(guò)程。

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

當(dāng)算法達(dá)到最大迭代次數(shù)時(shí)便會(huì)終止;此時(shí)輸出最優(yōu)解,即整個(gè)粒子群經(jīng)歷過(guò)的最好的位置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ī)劃策略,具體包括:首先,定義基本行為來(lái)對(duì)auv的航行路徑添加約束,基本行為分別為節(jié)能行為、協(xié)同行為和安全行為;然后,建立與基本行為對(duì)應(yīng)的行為目標(biāo)函數(shù),將與auv有關(guān)的時(shí)間變量和空間變量結(jié)合起來(lái);最后,建立全局目標(biāo)函數(shù)來(lái)實(shí)現(xiàn)3種基本行為的行為融合,并采用一種帶有慣性權(quán)重的粒子群優(yōu)化算法來(lái)求解全局目標(biāo)函數(shù),通過(guò)輸出的最優(yōu)解可以生成一條免于碰撞并且是最短的路徑。

當(dāng)前第1頁(yè)1 2 
網(wǎng)友詢問(wèn)留言 已有0條留言
  • 還沒(méi)有人留言評(píng)論。精彩留言會(huì)獲得點(diǎn)贊!
1