基于雙層路徑優(yōu)化算法的多機(jī)器人最優(yōu)編隊(duì)方法*

周佳加, 張 強(qiáng), 王宏健, 張洪泉, 王瑩瑩

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

0 引 言

隨著機(jī)器人技術(shù)的發(fā)展,多機(jī)器人編隊(duì)的需求與日俱增,其中最基本的行為是編隊(duì),其主要解決機(jī)器人之間協(xié)調(diào)問(wèn)題。編隊(duì)涉及到每個(gè)機(jī)器人的路徑規(guī)劃,即在一定約束下,編隊(duì)中的每個(gè)成員從規(guī)劃空間中尋找出從初始姿態(tài)到期望位置的路徑。當(dāng)編隊(duì)任務(wù)有時(shí)間約束時(shí),快速形成指向任務(wù)點(diǎn)的編隊(duì),有效節(jié)省了完成任務(wù)的時(shí)間,且增強(qiáng)了編隊(duì)任務(wù)的機(jī)動(dòng)性。1957年Dubins提出[1]在某個(gè)曲率的約束的條件下,可在同一平面內(nèi)尋找出任意兩個(gè)矢量點(diǎn)的最短路徑。Yeol J W 等人[2]采用Dubins路徑的方法求取了二維水平面兩點(diǎn)間的最短路徑,并通過(guò)微積分將這種方法給予了證明。Teng L等人[3]提出了一種多無(wú)人機(jī)攻擊多個(gè)地面目標(biāo)的任務(wù)規(guī)劃方法。戴健等人[4]采用“Z”型路徑覆蓋方法以及Dubins轉(zhuǎn)彎路徑,對(duì)各個(gè)無(wú)人機(jī)開(kāi)展覆蓋其子區(qū)域的搜索路徑規(guī)劃。Peng C等人[5]提出了一種具有姿態(tài)約束的移動(dòng)機(jī)器人路徑規(guī)劃方法規(guī)劃出一條無(wú)障礙路徑。胡永文等人[6]針對(duì)每個(gè)人和任務(wù)的最短時(shí)限指派問(wèn)題提出求解最短時(shí)限指派問(wèn)題的快速?zèng)Q策方法。Burgard W 等人[7]采用匈牙利算法求解出了最優(yōu)分配方法。宗群等人[8]通過(guò)粒子群和遺傳優(yōu)化算法雙層優(yōu)化解決大規(guī)模集群編隊(duì)中隊(duì)形選擇和站位分配問(wèn)題。

本文對(duì)多機(jī)器人編隊(duì)路徑規(guī)劃問(wèn)題進(jìn)行分析,根據(jù)編隊(duì)時(shí)間、編隊(duì)位置分配等約束建立了相應(yīng)的問(wèn)題模型,設(shè)計(jì)了將粒子群優(yōu)化(particle swarm optimization,PSO)與連續(xù)Hopfield神經(jīng)網(wǎng)絡(luò)(continuous Hopfield neural network,CHNN)的雙層路徑優(yōu)化的方法求解出最優(yōu)編隊(duì)方法。最后通過(guò)仿真實(shí)驗(yàn)驗(yàn)證了本文提出方法的有效性。

1 多機(jī)器人編隊(duì)問(wèn)題描述與建模

假設(shè)機(jī)器人初始隊(duì)形中編隊(duì)成員在地面坐標(biāo)系下第i(i=1,2,…,n)個(gè)機(jī)器人的坐標(biāo)表示為(xi,yi),ψd表示期望編隊(duì)方向,而機(jī)器人路徑規(guī)劃空間由機(jī)器人最大航程決定。

定義雙層規(guī)劃模型為

(1)

式中P和L為雙層優(yōu)化結(jié)果,f和g為優(yōu)化模型的性能指標(biāo),G為優(yōu)化模型的約束指標(biāo),a1,a2,…為優(yōu)化有關(guān)參數(shù)。

1.1 約束指標(biāo)

在多機(jī)器人編隊(duì)中,根據(jù)機(jī)器人的欠驅(qū)動(dòng)性和任務(wù),還需考慮以下四個(gè)約束[9,10]：

1)編隊(duì)到達(dá)期望隊(duì)形時(shí)的位置與航向約束

(2)

2)編隊(duì)過(guò)程的能耗約束

(3)

式中W為編隊(duì)過(guò)程中機(jī)器人總能耗,li為機(jī)器人i的路徑長(zhǎng)度,k1為常數(shù)。

3)機(jī)器人最小轉(zhuǎn)彎半徑的約束

最小轉(zhuǎn)彎半徑用如下公式表示

ρmin=f(max,U)

(4)

4)編隊(duì)時(shí)間約束

假設(shè)在編隊(duì)過(guò)程中機(jī)器人速度v沒(méi)有太大波動(dòng),則定義參數(shù)k2=1/v,編隊(duì)時(shí)間定義為

T=k2lmax

(5)

式中l(wèi)max為路徑最長(zhǎng)的機(jī)器人路徑長(zhǎng)度。

1.2 優(yōu)化目標(biāo)

在三角編隊(duì)中,選取編隊(duì)完成期望點(diǎn)Pk,再計(jì)算第i個(gè)機(jī)器人到達(dá)期望編隊(duì)中第j個(gè)位置的距離lij,進(jìn)而計(jì)算出編隊(duì)過(guò)程的總耗費(fèi)(總路徑)定義優(yōu)化目標(biāo)函數(shù)

(6)

式中Wmin為編隊(duì)集結(jié)點(diǎn)k下的最少能耗,lall為所有機(jī)器人到編隊(duì)中期望位置的路徑總長(zhǎng)度,Tmin為最短編隊(duì)時(shí)間,lij為機(jī)器人i到編隊(duì)中期望位置j的路徑長(zhǎng)度。

2 雙層路徑優(yōu)化算法

2.1 基于PSO的編隊(duì)集結(jié)點(diǎn)優(yōu)化

假設(shè)由m個(gè)粒子組成的種群X=(x1,x2,…,xm)在搜索空間D中,則第i個(gè)粒子在k+1次迭代時(shí)速度和位置更新方法為

(7)

式中w為慣性權(quán)重；d=1,2,…,D；i=1,2,…,m;k為迭代次數(shù)；Vid為粒子的速度；c1和c2為非負(fù)的常數(shù),稱為加速度因子；r1和r2為分布于[0,1]之間的隨機(jī)數(shù)。將位置和速度分別限制在[-Xmax,Xmax],[-Vmax,Vmax]之間。

為了更好平衡算法全局搜索和局部搜索的能力,本文設(shè)置權(quán)重為遞減形式[11],初始權(quán)重為ws,終止權(quán)重為we,則權(quán)重的更新方式為

(8)

2.2 基于CHNN的路徑優(yōu)化

在編隊(duì)虛擬集結(jié)點(diǎn)Pk約束下,設(shè)n個(gè)機(jī)器人和編隊(duì)期望位置,第i個(gè)機(jī)器人為Ri,機(jī)器人編隊(duì)中第j個(gè)位置為R′j,j=1,2,…，n,在編隊(duì)虛擬集結(jié)點(diǎn)Pk下Ri到R′j的路徑距離為lkij。則系數(shù)矩陣為

(9)

引入變量xkij

(10)

指派每個(gè)機(jī)器人到相應(yīng)位置的最優(yōu)分配問(wèn)題的模型為

(11)

(12)

其中,在k(k=1,2,…,m)個(gè)編隊(duì)虛擬集結(jié)點(diǎn)下每個(gè)機(jī)器人在總路徑最少的約束下到達(dá)期望位置的最優(yōu)位置分配方法。式(12)表示矩陣Xk=(xkij)n×n的行列約束和取值范圍。

將每個(gè)機(jī)器人從初始位置i到編隊(duì)虛擬集結(jié)點(diǎn)Pk隊(duì)形中的位置j作為一個(gè)變量,每個(gè)神經(jīng)元處理一個(gè)變量xkij,故整個(gè)網(wǎng)絡(luò)中神經(jīng)元個(gè)數(shù)xkij應(yīng)該個(gè)數(shù)相等。

則神經(jīng)網(wǎng)絡(luò)的能量函數(shù)Ek[12]

(13)

式中λ1,λ2為拉格朗日加權(quán)系數(shù)。能量函數(shù)前兩項(xiàng)為行列約束,使得xkij每行每列有且僅有一個(gè)1,即每個(gè)機(jī)器人對(duì)應(yīng)一個(gè)期望隊(duì)形中的位置；第三項(xiàng)表示總能量耗費(fèi)約束,即整體路徑最短。

(14)

式中xkij為神經(jīng)元(i,j)的輸出,ukij為神經(jīng)元(i,j)的輸入,xkij=f(ukij),f函數(shù)為神經(jīng)元(i,j)的輸出函數(shù),可以有不同的規(guī)律,這里選取Sigmoid函數(shù)

(15)

網(wǎng)絡(luò)輸入初始化

ukij(t)=u0ln(n-1)+δkij

(16)

其中,設(shè)置u0=0.1；n為機(jī)器人數(shù)量,δkij為區(qū)間[0,1]的隨機(jī)數(shù)；導(dǎo)入優(yōu)化參數(shù)lkij。

網(wǎng)絡(luò)按照下面方式運(yùn)行

(17)

式中 Δt為單步時(shí)間；ukij(t)為t時(shí)刻的神經(jīng)元輸入。

2.3 雙層優(yōu)化算法實(shí)現(xiàn)

1)初始化粒子群位置和速度,包括權(quán)重wk,加速度因子c1和c2等。2)第一層優(yōu)化計(jì)算,以當(dāng)前粒子位置為編隊(duì)集合點(diǎn),為每個(gè)機(jī)器人選擇合適的Dubins路徑,并計(jì)算系數(shù)矩陣。3)第二層優(yōu)化計(jì)算,采用CHNN算法求解在此系數(shù)矩陣下,每組機(jī)器人的最優(yōu)位置分配方法和適應(yīng)度,第二層優(yōu)化結(jié)束。4)根據(jù)適應(yīng)度更新局部和全局最優(yōu)集結(jié)點(diǎn),初始優(yōu)化完成。5)根據(jù)迭代次數(shù)更新位置、速度和權(quán)重w(k)。6)以當(dāng)前粒子位置為編隊(duì)集合點(diǎn),為每個(gè)機(jī)器人選擇合適的Dubins路徑,并計(jì)算系數(shù)矩陣。7)第二層優(yōu)化計(jì)算,采用CHNN算法求解每組機(jī)器人的最優(yōu)位置和適應(yīng)度,第二層優(yōu)化結(jié)束。8)根據(jù)PSO迭代次數(shù)判斷迭代是否完成,完成則算法結(jié)束,否則返回步驟(4)。

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

本文采用MATLAB 2016a進(jìn)行仿真實(shí)驗(yàn),規(guī)劃空間為{(xi,yi)|-100≤xi≤500,-100≤yi≤400},3個(gè)欠驅(qū)動(dòng)機(jī)器人組成的編隊(duì),轉(zhuǎn)彎半徑ρ=60 m,初始坐標(biāo)隨機(jī)分布在[-100,100],初始方向角為130°,60°,270°,期望編隊(duì)能在B=(400,230)前完成編隊(duì)。

采用式(7)PSO算法公式更新位置和速度,取100個(gè)粒子,進(jìn)化次數(shù)為200,式(8)中取慣性權(quán)值參數(shù)ws=0.95,we=0.35,學(xué)習(xí)因子c1=c2=1.5。式(13)、式(14)中,λ1=150,λ2=80,采樣時(shí)間取0.001,迭代次數(shù)取1 000。

圖1(a)是靜態(tài)權(quán)重PSO算法求解最優(yōu)編隊(duì)集結(jié)點(diǎn)的適應(yīng)度曲線,在迭代180次時(shí)接近最優(yōu)值276,圖1(b)改進(jìn)PSO算法在迭代20次時(shí)接近最優(yōu)值276,求出最優(yōu)編隊(duì)集結(jié)點(diǎn)(276,156)。固定慣性權(quán)值全局搜索和局部搜索無(wú)法兼顧,時(shí)變的慣性權(quán)重則前期有較強(qiáng)的全局搜索能力,后期更有利于精確的局部搜索。

圖1 靜態(tài)/動(dòng)態(tài)權(quán)重PSO算法適應(yīng)度曲線

圖2是最優(yōu)編隊(duì)集結(jié)點(diǎn)下采用CHNN分配方法的能量函數(shù),從圖中可知CHNN的能量隨著迭代次數(shù)逐漸降低。在能量變化率趨于0時(shí),整個(gè)網(wǎng)絡(luò)逐漸接近平衡態(tài),則求得的各航行器最優(yōu)分配路徑。

圖2 CHNN能量函數(shù)

圖3為機(jī)器人完整的編隊(duì)路徑圖,編隊(duì)初始位置在圖左下角,指向設(shè)定編隊(duì)集結(jié)B點(diǎn)前行,并在A點(diǎn)完成編隊(duì)。

圖3 改進(jìn)粒子群算法路徑仿真

4 結(jié) 論

針對(duì)多機(jī)器人編隊(duì)集結(jié)點(diǎn)的優(yōu)化問(wèn)題,本文提出了一種多機(jī)器人編隊(duì)雙層規(guī)劃模型。第一層解決了最優(yōu)隊(duì)形編隊(duì)集結(jié)點(diǎn)規(guī)劃問(wèn)題,在時(shí)間約束上采用改進(jìn)PSO算法求取在編隊(duì)時(shí)間最短的最優(yōu)編隊(duì)集合點(diǎn),并利用動(dòng)態(tài)遞減的權(quán)重平衡了PSO算法全局與局部的搜索能力。第二層解決了編隊(duì)中成員位置分配問(wèn)題,基于Dubins路徑采用CHNN算法將編隊(duì)中的機(jī)器人成員以最小能耗的條件下分配到最優(yōu)位置。最后,通過(guò)欠驅(qū)動(dòng)機(jī)器人的編隊(duì)仿真實(shí)驗(yàn)表明所提算法正確、有效。