基于雙層協(xié)調(diào)體系的多移動(dòng)機(jī)器人路徑規(guī)劃方法

鐘佩思， 徐東方， 李東民， 梁中源， 劉 梅， 陳修龍

(1.山東科技大學(xué)先進(jìn)制造技術(shù)研究中心， 青島 266590； 2.山東科技大學(xué)機(jī)電工程系， 泰安 271000)

多移動(dòng)機(jī)器人系統(tǒng)能夠勝任單機(jī)器人所不能完成的復(fù)雜任務(wù)，因此，多移動(dòng)機(jī)器人領(lǐng)域的研究受到了越來(lái)越多的關(guān)注[1-2]。其中，協(xié)調(diào)路徑規(guī)劃問題一直是多移動(dòng)機(jī)器人研究的基礎(chǔ)和重點(diǎn)[3]，一種靈活而有效的協(xié)調(diào)路徑規(guī)劃方法，能夠進(jìn)一步增強(qiáng)系統(tǒng)的可靠性和魯棒性，并提高工作效率。

用于單移動(dòng)機(jī)器人路徑規(guī)劃的方法有很多，如拓?fù)浞?、遺傳算法[4]、人工勢(shì)場(chǎng)及其改進(jìn)算法[5]等，與單機(jī)器人不同，除長(zhǎng)度、平滑度和避障能力等問題外，多移動(dòng)機(jī)器人路徑規(guī)劃還要考慮機(jī)器人之間的協(xié)調(diào)避碰問題[6]。近年來(lái)，對(duì)多移動(dòng)機(jī)器人路徑規(guī)劃的研究正在不斷深入。孫樹棟等[7]用遺傳算法規(guī)劃出能實(shí)現(xiàn)協(xié)調(diào)避碰的多機(jī)器人全局路徑，但難以適應(yīng)復(fù)雜的動(dòng)態(tài)環(huán)境；周蘭鳳等[8]提出基于知識(shí)的遺傳算法，提高了軌跡規(guī)劃的效率；Kala[9]設(shè)計(jì)一種協(xié)同進(jìn)化算法，規(guī)劃出了能夠有效協(xié)調(diào)避碰的多機(jī)器人全局路徑組合，但局部協(xié)調(diào)處理較為復(fù)雜；景興建等[10]提出用人工協(xié)調(diào)場(chǎng)進(jìn)行多機(jī)器人路徑規(guī)劃以提高實(shí)時(shí)避碰能力，但其全局性能較差；吳晉等[11]通過改進(jìn)人工協(xié)調(diào)場(chǎng)來(lái)應(yīng)對(duì)協(xié)調(diào)規(guī)劃中的運(yùn)動(dòng)抖動(dòng)和“死鎖”現(xiàn)象；Chandrasekhar等[12]采用改進(jìn)的磷蝦群算法進(jìn)行協(xié)調(diào)路徑規(guī)劃，但未能驗(yàn)證在動(dòng)態(tài)障礙物環(huán)境中算法的可行性。

可以看出，在目前的多移動(dòng)機(jī)器人路徑規(guī)劃方法中，采用全局規(guī)劃算法很難應(yīng)對(duì)局部路徑的實(shí)時(shí)協(xié)調(diào)，而傾向于局部規(guī)劃的方法又難以形成優(yōu)質(zhì)的全局路徑。針對(duì)這一問題，提出一種基于全局與局部雙層協(xié)調(diào)的多移動(dòng)機(jī)器人路徑規(guī)劃體系，結(jié)合改進(jìn)的免疫協(xié)同進(jìn)化算法和動(dòng)態(tài)窗口法，通過相互協(xié)調(diào)，力求機(jī)器人在沿全局最優(yōu)路徑行駛時(shí)，仍能實(shí)現(xiàn)靈活地局部路徑調(diào)整，從而進(jìn)一步提高多移動(dòng)機(jī)器人系統(tǒng)的全局規(guī)劃與局部協(xié)調(diào)能力。

1 雙層協(xié)調(diào)體系的構(gòu)建

基于雙層協(xié)調(diào)體系的多移動(dòng)機(jī)器人路徑規(guī)劃方法主要由全局路徑規(guī)劃層和局部路徑協(xié)調(diào)層構(gòu)成，其體系結(jié)構(gòu)如圖1所示，采用混合式控制方式實(shí)現(xiàn)整體調(diào)控。路徑規(guī)劃前，首先要完成環(huán)境地圖構(gòu)建、各機(jī)器人初始位置和目標(biāo)位置的確定以及優(yōu)先級(jí)編號(hào)等前處理工作，其中，機(jī)器人的優(yōu)先級(jí)順序由所需完成任務(wù)的重要程度來(lái)決定。

圖1 體系結(jié)構(gòu)框圖Fig.1 Architecture block diagram

全局路徑規(guī)劃層需要從各初始位置到目標(biāo)位置為每個(gè)機(jī)器人分別規(guī)劃出一條長(zhǎng)度最短、轉(zhuǎn)角最少、能有效避障并盡量減少機(jī)器人之間碰撞干涉的全局路徑，該工作由改進(jìn)的免疫協(xié)同進(jìn)化算法來(lái)實(shí)現(xiàn)。通過免疫克隆機(jī)制能加快種群進(jìn)化速度，為進(jìn)一步適應(yīng)多移動(dòng)機(jī)器人的全局路徑規(guī)劃，在原算法的基礎(chǔ)上引入路徑組的概念進(jìn)行改進(jìn)。首先為n個(gè)機(jī)器人分別規(guī)劃出多條較優(yōu)的全局路徑，并將每個(gè)機(jī)器人對(duì)應(yīng)一條全局路徑組成的集合X={X1,X2,…,Xn}稱為一個(gè)路徑組(Xi表示第i個(gè)機(jī)器人對(duì)應(yīng)的一條全局路徑)，全局路徑規(guī)劃的目標(biāo)函數(shù)作為抗原，而在種群中匹配出的所有路徑組均視為抗體，通過對(duì)每個(gè)抗體進(jìn)行親和度評(píng)價(jià)并反復(fù)迭代進(jìn)化，最終能夠規(guī)劃出整體效果最優(yōu)的全局路徑組，機(jī)器人將沿著最優(yōu)路徑組中各自的全局路徑行駛。

全局路徑規(guī)劃可以在一定程度上減少機(jī)器人之間的碰撞，但在工作過程中，難免出現(xiàn)動(dòng)態(tài)或靜態(tài)未知障礙物以及多個(gè)機(jī)器人相遇的情形，無(wú)法保證完全避免碰撞。因此僅靠全局路徑規(guī)劃難以應(yīng)對(duì)復(fù)雜的工作環(huán)境，還需要通過局部路徑協(xié)調(diào)來(lái)對(duì)機(jī)器人局部路徑進(jìn)行靈活調(diào)整以避免產(chǎn)生碰撞。

局部路徑協(xié)調(diào)層的工作采用動(dòng)態(tài)窗口法來(lái)實(shí)現(xiàn)，這是一種基于速度空間的局部避障方法，具有良好的動(dòng)態(tài)處理能力。基于雙層協(xié)調(diào)體系，可以利用全局路徑信息輔助進(jìn)行局部路徑協(xié)調(diào)，以獲得更好的規(guī)劃效果。首先根據(jù)全局路徑規(guī)劃結(jié)果對(duì)最優(yōu)路徑組中的各條路徑進(jìn)行跟蹤預(yù)測(cè)，當(dāng)發(fā)現(xiàn)機(jī)器人之間存在干涉或碰撞趨勢(shì)時(shí)，便提取碰撞點(diǎn)周圍的環(huán)境信息以及各機(jī)器人在全局路徑上的運(yùn)動(dòng)信息進(jìn)行局部路徑協(xié)調(diào)，從而避免碰撞發(fā)生；機(jī)器人個(gè)體對(duì)周圍環(huán)境也進(jìn)行實(shí)時(shí)探測(cè)和判斷，當(dāng)檢測(cè)到可能產(chǎn)生碰撞的未知障礙物時(shí)，同樣提取周圍工作環(huán)境信息并進(jìn)行局部路徑調(diào)整避障。完成局部路徑調(diào)整后，機(jī)器人重新回到各自的全局路徑上繼續(xù)行駛，直至順利到達(dá)原定目標(biāo)位置。

2 全局路徑規(guī)劃的實(shí)現(xiàn)

2.1 搭建工作環(huán)境

假設(shè)多移動(dòng)機(jī)器人系統(tǒng)的工作空間S中有n個(gè)移動(dòng)機(jī)器人和m個(gè)障礙物，且機(jī)器人個(gè)體在長(zhǎng)度或?qū)挾确较蛏系淖畲蟪叽鐬閐。構(gòu)建地圖時(shí)，把障礙物的邊界向外擴(kuò)展d/2，則在全局路徑規(guī)劃中機(jī)器人個(gè)體可視為質(zhì)點(diǎn)。將機(jī)器人按優(yōu)先級(jí)高低分別編號(hào)為R1, R2,…, Rn，執(zhí)行的任務(wù)越重要，機(jī)器人的優(yōu)先級(jí)就越高，相應(yīng)的下標(biāo)編號(hào)也就越小，而障礙物分別編號(hào)為O1, O2,…, Om。所有的障礙物共同組成障礙物空間SO，剩余的空間則稱為自由空間SF。

將移動(dòng)機(jī)器人Ri的起始位置定義為pi,1=(xi,1,yi,1)，目標(biāo)位置定義為pi,l=(xi,l,yi,l)，l為路徑點(diǎn)的個(gè)數(shù)，其全局路徑Xi便可通過SF中的點(diǎn)序列{pi,1,pi,2,…,pi,l}來(lái)表示，其中pi,j=(xi,j,yi,j)，表示Ri的第j個(gè)路徑點(diǎn)坐標(biāo)值，由于不同機(jī)器人的行駛路徑不同，所以路徑點(diǎn)的個(gè)數(shù)l也會(huì)各不相同。為了便于擴(kuò)展系統(tǒng)中機(jī)器人的個(gè)數(shù)，并減少后期實(shí)時(shí)路徑跟蹤和預(yù)測(cè)的計(jì)算量與計(jì)算時(shí)間，規(guī)定在全局路徑上，所有移動(dòng)機(jī)器人都以速度vq進(jìn)行勻速行駛。

2.2 改進(jìn)免疫協(xié)同進(jìn)化算法

免疫協(xié)同進(jìn)化算法流程如圖1中的全局路徑規(guī)劃層所示，為更好地適應(yīng)多移動(dòng)機(jī)器人系統(tǒng)，在傳統(tǒng)免疫協(xié)同進(jìn)化算法步驟[13]的基礎(chǔ)上進(jìn)行了改進(jìn)。

首先為每個(gè)機(jī)器人隨機(jī)生成N條全局路徑，這些路徑共同構(gòu)成一個(gè)n×N矩陣，稱為抗體種群A。之后通過適應(yīng)度函數(shù)對(duì)種群中的各條路徑進(jìn)行評(píng)價(jià)，選取適應(yīng)度較高的n×K條路徑構(gòu)造記憶集Am，而剩下的n×L條路徑則構(gòu)成一般集Ag，定義適應(yīng)度評(píng)價(jià)函數(shù)如下：

f(Xi)=α1len(Xi)+α2cor(Xi)+α3aov(Xi)

(1)

式(1)中：len(Xi)表示路徑Xi的總長(zhǎng)度，計(jì)算公式為

(2)

cor(Xi)表示路徑Xi中連續(xù)折線間轉(zhuǎn)角的總弧度，計(jì)算公式為

(3)

aov(Xi)表示路徑Xi與工作空間中障礙物的相交次數(shù)；而α1、α2、α3則表示權(quán)重系數(shù)。

劃分完Am和Ag后，從Am中再選取適應(yīng)度較高的n×G條路徑進(jìn)行克隆以獲得克隆集Ac，其中G≤K，其大小與Am的整體適應(yīng)度成正比。為保證Am不遭到破壞，這里僅對(duì)Ac和Ag進(jìn)行變異，并對(duì)Ac進(jìn)行粒群進(jìn)化處理[13]，之后對(duì)Am、Ag、Ac中的路徑進(jìn)行疫苗接種。如圖1所示，疫苗主要分為刪除、平滑、交換和避障4類，通過接種疫苗，可以有針對(duì)性地優(yōu)化個(gè)體，從而加快收斂速度。

接種疫苗后，再次對(duì)Am、Ag、Ac中的路徑進(jìn)行適應(yīng)度評(píng)價(jià)并更新記憶集，最后在更新后的記憶集A′m中通過排列組合匹配路徑組，并通過親和度函數(shù)對(duì)不同的路徑組進(jìn)行評(píng)價(jià)以判斷是否達(dá)到規(guī)劃要求，將路徑組的親和度評(píng)價(jià)函數(shù)定義為

(4)

式(4)中：C為一較大常數(shù)；X={X1,X2,…,Xn}表示從A′m中匹配出來(lái)的路徑組；col(Xi)表示Xi與路徑組中其他路徑的相交次數(shù)，由于初始條件不確定，路徑相交便存在碰撞的可能；cro(Xi)表示路徑Xi在路徑組中的擁擠程度，用Xi兩側(cè)規(guī)定范圍D內(nèi)其他路徑的條數(shù)來(lái)表示；β1、β2為權(quán)重系數(shù)。

參照親和度評(píng)價(jià)結(jié)果，若存在滿足要求的最優(yōu)路徑組，則完成全局路徑規(guī)劃，機(jī)器人沿各自全局路徑勻速行駛，并由系統(tǒng)進(jìn)行跟蹤預(yù)測(cè)，以及時(shí)判斷可能產(chǎn)生碰撞的位置；若各路徑組均不滿足規(guī)劃要求，則消亡掉當(dāng)前種群中適應(yīng)度低的路徑，并更新種群以維護(hù)多樣度不變，重復(fù)上述步驟繼續(xù)迭代進(jìn)化，直到獲得滿足要求的路徑組為止。

2.3 全局規(guī)劃性能驗(yàn)證

在傳統(tǒng)免疫協(xié)同進(jìn)化算法中，適應(yīng)度評(píng)價(jià)被包含進(jìn)親和度函數(shù)中，直接由親和度評(píng)價(jià)來(lái)劃分Am和Ag，并且親和度評(píng)價(jià)的對(duì)象是單條路徑而非路徑組，因此每次評(píng)價(jià)時(shí)，需要將待評(píng)路徑與其他機(jī)器人所有路徑的親和度都計(jì)算一遍，計(jì)算量較大，致使迭代速度較慢，而且也不利于機(jī)器人個(gè)數(shù)的擴(kuò)展。同時(shí)也因?yàn)樵u(píng)價(jià)對(duì)象是路徑個(gè)體，所以最終獲得的每條路徑親和度可能達(dá)到最高，但放一起卻未必是整體親和度最好的一組。

進(jìn)行變異、進(jìn)化和疫苗接種前，種群中各條路徑的適應(yīng)度都相對(duì)較低，此時(shí)進(jìn)行親和度評(píng)價(jià)不僅計(jì)算量大，且效果不佳，因此在改進(jìn)的算法中，先通過適應(yīng)度函數(shù)對(duì)Am和Ag進(jìn)行劃分，之后進(jìn)行克隆、變異、進(jìn)化和免疫處理，并再次利用適應(yīng)度函數(shù)對(duì)路徑進(jìn)行評(píng)價(jià)，更新出適應(yīng)度相對(duì)較好的記憶集A′m后，再對(duì)A′m中的路徑進(jìn)行親和度評(píng)價(jià)。

在進(jìn)行親和度評(píng)價(jià)時(shí)，先通過排列組合將A′m中的路徑匹配為不同的路徑組，再利用親和度函數(shù)分別評(píng)價(jià)，并通過反復(fù)迭代，尋找親和度最高的路徑組。親和度評(píng)價(jià)對(duì)象是路徑組這個(gè)整體概念而不是單條路徑，所以進(jìn)行評(píng)價(jià)時(shí)只考慮該路徑組中n條路徑間的親和度即可，而不需要將待評(píng)路徑與其他機(jī)器人所有路徑的協(xié)作行為都計(jì)算一遍。

通過改進(jìn)算法，能夠?qū)⒍鄼C(jī)器人全局路徑規(guī)劃先簡(jiǎn)化為單個(gè)機(jī)器人的路徑規(guī)劃問題，當(dāng)種群中的路徑都達(dá)到較好的適應(yīng)度后，再匹配路徑組進(jìn)行多機(jī)器人的路徑親和度評(píng)價(jià)，減少了很多不必要的協(xié)作度計(jì)算，從而加快了迭代速度。并且由于親和度評(píng)價(jià)對(duì)象是路徑組而非單條路徑，所以能夠保證最終規(guī)劃出的n條路徑可以取得最好的整體效果。在相同的軟硬件環(huán)境和初始條件下，改進(jìn)前后算法的迭代速度和迭代效果對(duì)比如圖2所示。

從圖2(a)中可以看出，改進(jìn)后算法的迭代速度要優(yōu)于原算法，且隨著迭代次數(shù)的增加，這種優(yōu)勢(shì)會(huì)越來(lái)越明顯，而圖2(b)則顯示改進(jìn)后的算法在迭代至30次左右便逐漸趨于穩(wěn)定，比改進(jìn)前算法的收斂速度快，而且最終所獲得的路徑組親和度也更高。這說明對(duì)算法的改進(jìn)可行且有效，能實(shí)現(xiàn)更加快速、高質(zhì)量的多移動(dòng)機(jī)器人全局路徑規(guī)劃。

圖2 算法性能對(duì)比Fig.2 Comparison of algorithm performance

3 局部路徑協(xié)調(diào)規(guī)劃

3.1 動(dòng)態(tài)窗口法的構(gòu)建

機(jī)器人沿各自全局路徑以vq勻速行駛，根據(jù)初始位置和行駛時(shí)間可以預(yù)測(cè)出存在碰撞或干涉的路徑點(diǎn)，當(dāng)需要進(jìn)行局部路徑協(xié)調(diào)時(shí)，便啟用動(dòng)態(tài)窗口法進(jìn)行調(diào)整。動(dòng)態(tài)窗口法的主要思想[14]是在速度空間中采樣多組速度并模擬機(jī)器人在這些速度下一定時(shí)間Δt內(nèi)的軌跡，得到多組軌跡后，通過對(duì)這些軌跡進(jìn)行評(píng)價(jià)，選取最優(yōu)軌跡所對(duì)應(yīng)的速度來(lái)驅(qū)動(dòng)機(jī)器人進(jìn)行路徑協(xié)調(diào)。

(5)

進(jìn)行速度采樣時(shí)，雖然速度空間中存在無(wú)窮多組速度，但并不是每組速度都可取，需要根據(jù)機(jī)器人自身?xiàng)l件和工作環(huán)境的限制，將速度采樣控制在一個(gè)速度動(dòng)態(tài)窗口V內(nèi)，該動(dòng)態(tài)窗口可以表示為機(jī)器人最值速度窗口Vm、電機(jī)加速度性能窗口Ve和安全速度窗口Vs的交集：

(6)

圖3 動(dòng)態(tài)窗口采樣示意圖Fig.3 Dynamic window sampling diagram

基于動(dòng)態(tài)窗口進(jìn)行速度采樣，并根據(jù)機(jī)器人運(yùn)動(dòng)學(xué)模型推算每組速度在時(shí)間Δt內(nèi)對(duì)應(yīng)的軌跡，進(jìn)而完成對(duì)各條軌跡的評(píng)價(jià)，構(gòu)造評(píng)價(jià)函數(shù)為

h(v,ω)=γ1hea(v,ω)+γ2dis(v,ω)+

γ3vel(v,ω)

(7)

式(7)中：hea(v,ω)用來(lái)評(píng)價(jià)在當(dāng)前采樣速度下，機(jī)器人到達(dá)模擬軌跡末端時(shí)的前進(jìn)方向與目標(biāo)方向之間的夾角δ，用π-δ表示，即δ越小評(píng)分越高；dis(v,ω)表示機(jī)器人在當(dāng)前模擬軌跡上與障礙物之間的最小距離；vel(v,ω)表示當(dāng)前模擬軌跡對(duì)應(yīng)的線速度大??；γ1、γ2、γ3表示權(quán)重系數(shù)。

最后基于上述函數(shù)的評(píng)價(jià)結(jié)果，選擇評(píng)分最高的軌跡對(duì)應(yīng)的采樣速度來(lái)驅(qū)動(dòng)移動(dòng)機(jī)器人運(yùn)動(dòng)。

3.2 局部路徑協(xié)調(diào)處理

局部路徑協(xié)調(diào)過程中，規(guī)定機(jī)器人的速度方向與大小均由動(dòng)態(tài)窗口法的評(píng)價(jià)結(jié)果來(lái)確定，但協(xié)調(diào)的初始速度和終止速度仍然是全局路徑上對(duì)應(yīng)的速度，從而確保完成局部路徑調(diào)整后，機(jī)器人回到全局路徑上仍然能按原定速度勻速行駛。

當(dāng)機(jī)器人之間存在碰撞可能時(shí)，需要比較這些機(jī)器人的優(yōu)先級(jí)，由優(yōu)先級(jí)低的機(jī)器人啟用動(dòng)態(tài)窗口法進(jìn)行局部協(xié)調(diào)，而優(yōu)先級(jí)最高的機(jī)器人將不受影響，繼續(xù)沿自身路徑行駛。相比之下，機(jī)器人的優(yōu)先級(jí)越低，路徑調(diào)整的次序就越靠前，相應(yīng)的協(xié)調(diào)范圍也越大，依次類推。位于全局路徑或局部調(diào)整中的機(jī)器人，均可按此方法進(jìn)行協(xié)調(diào)。而對(duì)于未知障礙物，則需要機(jī)器人自行檢測(cè)并判斷，若存在碰撞可能，便及時(shí)進(jìn)行局部路徑調(diào)整避障。

當(dāng)協(xié)調(diào)避碰的對(duì)象為未知障礙物或處于局部調(diào)整中的機(jī)器人時(shí)，由于其運(yùn)動(dòng)信息不可預(yù)知，直接利用路徑評(píng)價(jià)函數(shù)選擇最合理的(v,ω)來(lái)驅(qū)動(dòng)機(jī)器人進(jìn)行協(xié)調(diào)避碰即可；而當(dāng)避碰對(duì)象為全局路徑上的機(jī)器人時(shí)，則可利用機(jī)器人在全局路徑上的運(yùn)動(dòng)信息來(lái)輔助局部路徑協(xié)調(diào)，從而進(jìn)一步提高整體規(guī)劃效果，仍以Rb的局部協(xié)調(diào)避碰為例，可通過Ra的全局路徑運(yùn)動(dòng)信息完善對(duì)Rb的路徑評(píng)價(jià)，在二者行駛軌跡相交前，引入新的路徑評(píng)價(jià)項(xiàng)：

(8)

式(8)中：t′表示在采樣速度(v,ω)驅(qū)使下，Rb的行駛軌跡與Ra全局路徑相交時(shí)所需要的時(shí)間；(xa,t′，ya,t′)和(xb,t′，yb,t′)分別為行駛軌跡相交時(shí)，Ra和Rb所到達(dá)的位置坐標(biāo)，該項(xiàng)權(quán)重系數(shù)取為γ4。

通過對(duì)行駛軌跡相交時(shí)機(jī)器人之間的位置關(guān)系進(jìn)行評(píng)價(jià)，能夠挑選出更加合理的運(yùn)動(dòng)軌跡，從而提高局部路徑協(xié)調(diào)效率，待行駛軌跡相交后，再重新啟用原路徑評(píng)價(jià)函數(shù)進(jìn)行評(píng)價(jià)調(diào)整。通過增加距離評(píng)價(jià)項(xiàng)，也可使該方法擴(kuò)展至全局路徑上多個(gè)機(jī)器人之間協(xié)調(diào)避碰的情況中。

4 仿真試驗(yàn)分析

利用MATLAB編程構(gòu)建多機(jī)器人雙層協(xié)調(diào)路徑規(guī)劃體系，隨機(jī)生成二維柵格環(huán)境地圖，柵格邊長(zhǎng)設(shè)置為1 m，系統(tǒng)中使用Pioneer3-DX款差分驅(qū)動(dòng)機(jī)器人模型，d取為0.5 m，機(jī)器人個(gè)數(shù)設(shè)置為4，分別進(jìn)行優(yōu)先級(jí)編號(hào)并分配初始位置與目標(biāo)位置坐標(biāo)，在柵格地圖上起點(diǎn)位置和目標(biāo)位置分別用實(shí)心的圓形和方形來(lái)表示，由此進(jìn)行仿真試驗(yàn)。路徑規(guī)劃過程中，各權(quán)重系數(shù)的取值如表1所示。

表1 權(quán)重系數(shù)取值Table 1 Calculation results of power

進(jìn)行全局路徑規(guī)劃時(shí)，親和度評(píng)價(jià)函數(shù)中常數(shù)C取為100，經(jīng)過迭代進(jìn)化，最終得到的規(guī)劃結(jié)果如圖4所示。規(guī)劃過程中，種群內(nèi)各路徑的適應(yīng)度值變化情況如圖5所示，從A′m中匹配出的各路徑組親和度值變化則如圖6所示?？梢钥闯?，迭代次數(shù)達(dá)到30次以后，便可獲得趨于穩(wěn)定規(guī)劃結(jié)果，最終所獲得的各路徑適應(yīng)度值較好，且路徑組的親和度也很高。改進(jìn)后算法的收斂速度快，且全局路徑規(guī)劃的結(jié)果較為理想。

圖4 全局路徑規(guī)劃結(jié)果Fig.4 Results of global path planning

圖5 全局路徑適應(yīng)度值Fig.5 Fitness of global path

圖6 全局路徑親和度Fig.6 Affinity of global path

設(shè)定系統(tǒng)的初始條件為機(jī)器人從各自起點(diǎn)位置同時(shí)出發(fā)，在全局路徑上以vq=0.3 m/s勻速行駛，由此進(jìn)行路徑跟蹤和預(yù)測(cè)?；诔跏紬l件推算，機(jī)器人R1和R2在19.893 s會(huì)產(chǎn)生碰撞，此時(shí)R2處于圖4中的a點(diǎn)位置，需要在a點(diǎn)附近對(duì)R2進(jìn)行局部路徑協(xié)調(diào)，R2的優(yōu)先級(jí)比R1低一級(jí)，取協(xié)調(diào)范圍半徑為4vq，并將R1視為以d為半徑的圓形動(dòng)態(tài)障礙物，處理結(jié)果如圖7(a)所示；路徑調(diào)整致使R2的行駛時(shí)間延長(zhǎng)，進(jìn)而導(dǎo)致R2和R3在39.476 s時(shí)存在碰撞可能，此時(shí)R3處于圖4中的b點(diǎn)位置，則在b點(diǎn)附近同樣需要對(duì)R3進(jìn)行局部路徑協(xié)調(diào)，處理結(jié)果如圖7(b)所示。

圖7 局部路徑協(xié)調(diào)處理Fig.7 Coordination of local paths

可以看出圖4中R3和R4的全局路徑也存在交點(diǎn)，但是根據(jù)初始條件推算，兩個(gè)機(jī)器人之間不會(huì)發(fā)生干涉和碰撞，故不需要進(jìn)行局部路徑協(xié)調(diào)，對(duì)于地圖中的未知障礙物c，需要由機(jī)器人自行探測(cè)并判斷，以及時(shí)進(jìn)行調(diào)整避讓。

完成局部路徑協(xié)調(diào)處理后，機(jī)器人重新回到原全局路徑，以vq繼續(xù)向前勻速行駛，若再次出現(xiàn)碰撞趨勢(shì)，便重新開啟動(dòng)態(tài)窗口法進(jìn)行局部路徑調(diào)整，直至順利到達(dá)目標(biāo)位置以執(zhí)行原定任務(wù)，基于雙層協(xié)調(diào)體系的路徑規(guī)劃結(jié)果如圖8所示。

從圖8中可以看出，基于雙層協(xié)調(diào)體系的多移動(dòng)機(jī)器人路徑規(guī)劃方法所規(guī)劃出的各條路徑均具有較好的全局效果，并且可以實(shí)現(xiàn)機(jī)器人之間靈活有序的協(xié)調(diào)避碰，對(duì)于突然出現(xiàn)的未知障礙物也能做出及時(shí)的避障處理，能夠?qū)崿F(xiàn)預(yù)期目的。

圖8 雙層協(xié)調(diào)路徑規(guī)劃結(jié)果Fig.8 Results of bilevel coordinated path planning

5 結(jié)論

針對(duì)多移動(dòng)機(jī)器人的路徑規(guī)劃問題，提出一種基于全局與局部雙層協(xié)調(diào)的路徑規(guī)劃方法，通過仿真分析，得到以下結(jié)論。

(1)引入路徑組親和度評(píng)價(jià)方法來(lái)改進(jìn)免疫協(xié)同進(jìn)化算法，提高了全局路徑規(guī)劃的效率，為多移動(dòng)機(jī)器人系統(tǒng)規(guī)劃出了質(zhì)量更高并能盡量減少機(jī)器人之間干涉碰撞的全局路徑。

(2)利用動(dòng)態(tài)窗口法并結(jié)合機(jī)器人的優(yōu)先級(jí)機(jī)制和全局路徑信息，及時(shí)對(duì)機(jī)器人的局部路徑進(jìn)行協(xié)調(diào)處理，有效避免了復(fù)雜環(huán)境下各機(jī)器人之間以及機(jī)器人與未知障礙物之間的碰撞。

基于雙層協(xié)調(diào)體系的多移動(dòng)機(jī)器人路徑規(guī)劃方法可以實(shí)現(xiàn)機(jī)器人在沿全局最優(yōu)路徑行駛時(shí)，仍能進(jìn)行靈活有序的局部路徑協(xié)調(diào)，能夠應(yīng)對(duì)動(dòng)態(tài)復(fù)雜的工作環(huán)境，進(jìn)一步提高了多移動(dòng)機(jī)器人系統(tǒng)的作業(yè)能力。