動(dòng)態(tài)環(huán)境下機(jī)器人多目標(biāo)路徑規(guī)劃螞蟻算法

摘要：研究了一種新穎的動(dòng)態(tài)復(fù)雜不確定環(huán)境下的機(jī)器人多目標(biāo)路徑規(guī)劃螞蟻算法。該方法首先根據(jù)螞蟻覓食行為對(duì)多個(gè)目標(biāo)點(diǎn)的組合進(jìn)行優(yōu)化，規(guī)劃出一條最優(yōu)的全局導(dǎo)航路徑。在此基礎(chǔ)上，機(jī)器人按照規(guī)劃好的目標(biāo)點(diǎn)訪問順序根據(jù)多螞蟻協(xié)作局部路徑算法完成局部路徑的搜索。機(jī)器人每前進(jìn)一步都實(shí)時(shí)地進(jìn)行動(dòng)態(tài)障礙物運(yùn)動(dòng)軌跡預(yù)測(cè)以及碰撞預(yù)測(cè)，并重新進(jìn)行避碰局部路徑規(guī)劃。仿真結(jié)果表明，即使在障礙物非常復(fù)雜的地理環(huán)境，用該算法也能使機(jī)器人沿一條全局優(yōu)化的路徑安全避碰的遍歷各個(gè)目標(biāo)點(diǎn)，效果十分令人滿意。

關(guān)鍵詞：機(jī)器人；多螞蟻協(xié)作；全局導(dǎo)航路徑；局部路徑

中圖分類號(hào)：TP24文獻(xiàn)標(biāo)識(shí)碼：A文章編號(hào)：1009-3044(2008)32-1185-03

Ant Algorithm for Mobile Robot Path Plan with Multi-objects in an Unfamiliar Environment

XU Shou-jiang

(Jiangsu Food Science College， Huai'an 223003， China)

Abstract: Based on Ant Colony Optimization (ACO)， this thesis presents a novel algorithm underlying the robot multi-objects path planning and dynamic obstacle avoidance in a complex and unfamiliar environment， and the algorithm optimizes the combination of all objects and can get a globally optimal navigation path. The locally optimal path between the position of robot and the present object which is gotten from the globally optimal navigation path is accomplished by local path planning with multi-ants in cooperation. Collision prediction proceeds timely after ever step of robot and local dynamic planning for obstacle avoidance is executed. Our computer experiments demonstrate that the algorithms are robust and stable.

Key words: robot; multi-ants cooperation; global navigation path; local path

1 引言

移動(dòng)機(jī)器人運(yùn)動(dòng)導(dǎo)航或路徑規(guī)劃屬于研究機(jī)器人控制系統(tǒng)的重要應(yīng)用基礎(chǔ)問題，也是機(jī)器人研究領(lǐng)域中的一個(gè)重要分支。它是指在有障礙物的工作環(huán)境中，如何尋找一條從給定起始點(diǎn)到終止點(diǎn)的較優(yōu)的運(yùn)動(dòng)路徑，使機(jī)器人在運(yùn)動(dòng)過程中能安全無碰撞地繞過障礙物，且所走路線最短。對(duì)這一問題，各國(guó)學(xué)者已經(jīng)做了大量的研究，其中包括Dijkstra算法及其改進(jìn)[1]、啟發(fā)式搜索算法[2]、模糊算法[3]、神經(jīng)網(wǎng)絡(luò)[4]、遺傳算法[5]，滾動(dòng)窗口規(guī)劃方法[6]等等，這些方法都不同程度上提高了機(jī)器人路徑規(guī)劃的效率，但其研究的對(duì)象大多局限于機(jī)器人單目標(biāo)搜索，對(duì)單機(jī)器人進(jìn)行多目標(biāo)搜索鮮有報(bào)道。

隨著機(jī)器人技術(shù)的發(fā)展，機(jī)器人將被應(yīng)用于多目標(biāo)搜索，如機(jī)器人郵遞員需要將每一份郵件遞送到指定的地點(diǎn)等。即如何保證機(jī)器人在有障礙物的工作環(huán)境中可以快速地沿一條較優(yōu)化避碰路徑安全無碰地訪問多個(gè)目標(biāo)點(diǎn)。在這類應(yīng)用中機(jī)器人需要遍歷多個(gè)目標(biāo)，當(dāng)目標(biāo)數(shù)增多時(shí)，算法的復(fù)雜度呈指數(shù)級(jí)增長(zhǎng)[7]，將嚴(yán)重影響規(guī)劃的時(shí)效性。根據(jù)目前的研究現(xiàn)狀和不足并受意大利學(xué)者M(jìn).Dorigo等人于上世紀(jì)90年代提出的蟻群優(yōu)化算法[8-9]的啟示。為此，本文研究了一種動(dòng)態(tài)復(fù)雜環(huán)境下移動(dòng)機(jī)器人多目標(biāo)路徑規(guī)劃螞蟻算法，該算法首先根據(jù)Ant Colony System（簡(jiǎn)稱ACS）算法[8]對(duì)多個(gè)目標(biāo)點(diǎn)的組合進(jìn)行優(yōu)化，規(guī)劃出一條最優(yōu)的全局導(dǎo)航路徑，然后機(jī)器人依賴全局導(dǎo)航路徑信息進(jìn)行各個(gè)目標(biāo)點(diǎn)的遍歷，機(jī)器人在當(dāng)前點(diǎn)再根據(jù)已知的靜態(tài)環(huán)境信息利用多螞蟻局部路徑規(guī)劃算法[10]規(guī)劃出一條局部路徑，機(jī)器人根據(jù)該局部路徑每走一步都進(jìn)行動(dòng)態(tài)環(huán)境信息探測(cè)，根據(jù)該局部路徑預(yù)測(cè)與動(dòng)態(tài)物碰撞點(diǎn)并進(jìn)行局部動(dòng)態(tài)避障規(guī)劃。仿真實(shí)驗(yàn)結(jié)果表明，即使在障礙物非常復(fù)雜的地理環(huán)境，用本算法也能使機(jī)器人沿一條全局較優(yōu)化的路徑安全避碰的遍歷各個(gè)目標(biāo)點(diǎn)，效果十分令人滿意。

2 環(huán)境描述及問題描述

記AS為機(jī)器人Rob在二維平面上的凸多邊形有限運(yùn)動(dòng)區(qū)域，其內(nèi)部分布著有限個(gè)已知的靜態(tài)障礙物Sb1，Sb2，…，Sbn和有限個(gè)未知的動(dòng)態(tài)障礙物Db1，Db2，…，Dbq；在任意時(shí)刻，機(jī)器人Rob能探測(cè)到以當(dāng)前位置為中心，r為半徑區(qū)域內(nèi)的環(huán)境信息，稱該區(qū)域?yàn)镽ob視野域View ；Rob的運(yùn)動(dòng)速率記為VR；環(huán)境探測(cè)及路徑規(guī)劃所需的時(shí)間忽略不計(jì)；各動(dòng)態(tài)障礙物之間的最近距離>2r；t時(shí)刻Dbi(i=1，2，…，q)的運(yùn)動(dòng)速率記為vid(t)， vid(t)∈[0，vdh]，其中，vdh為一有限速率。Dbi的運(yùn)動(dòng)軌跡和方向?qū)傥粗溲夭婚]合、不自相交的光滑平緩軌跡單向運(yùn)動(dòng)，且在機(jī)器人傳感器探測(cè)范圍內(nèi)近似為方向不變的直線運(yùn)動(dòng)。規(guī)劃的目的是使機(jī)器人由起點(diǎn)Gbegin出發(fā)，經(jīng)過一系列目標(biāo)點(diǎn)G2，G3，……，Gn，安全避碰地回到出發(fā)點(diǎn)Gbegin，從而構(gòu)成一條回路。由于隨著目標(biāo)點(diǎn)的增多，解的規(guī)模呈指數(shù)級(jí)增，屬于NP-Hard問題，并且各個(gè)目標(biāo)點(diǎn)之間的局部信息未知。假設(shè)機(jī)器人的訪問目標(biāo)點(diǎn)的順序?yàn)閜1，p2，p3，……，pn，p1，規(guī)劃的目的就是使得相鄰兩個(gè)目標(biāo)點(diǎn)之間的局部避碰路徑之和最短或者較短，能夠保證機(jī)器人快速安全地訪問各個(gè)目標(biāo)點(diǎn)，其中p1=Gbegin，p2，p3，……，pn為G2，G3，……，Gn的一個(gè)全排列。其中Gbegin∈A，Gbegin?埸OS ， Gi∈A，Gi?埸OS ， i={2，…，n}，其它約束條件為：Gbegin與任意動(dòng)態(tài)障礙物的距離≥r。

在AS中以AS左上角為坐標(biāo)原點(diǎn)0，以橫向?yàn)閄軸，縱向?yàn)閅軸建立系統(tǒng)直角坐標(biāo)系，如圖1。假設(shè)機(jī)器人在水平方向上的行走步長(zhǎng)為δ，并且AS在X、Y方向的最大值分別為Xmax和Ymax。以δ為步長(zhǎng)形成一個(gè)柵格。則每行的柵格數(shù)Nx=Xmax/δ，每列的柵格數(shù)y=Ymax/δ。其中bi(i=1，2…，n)占一個(gè)或多個(gè)柵格，當(dāng)不滿一個(gè)柵格時(shí)算一個(gè)柵格。

記g∈AS為任意柵格，A為AS中g(shù)的集合，記OS＝{b1，b2，…，bn} A為靜態(tài)障礙柵格集。 g∈A在坐標(biāo)系中都有確定的坐標(biāo)(x，y)，記做g(x，y)，x為g所在的行號(hào)，y為g所在的列號(hào)。令C={1，2，3，…，，M}為柵格序號(hào)集，g(1，1)的序號(hào)為1，g(1，2)序號(hào)為2，g(2，1)序號(hào)為Nx+1…，如圖1所示。gi∈A的坐標(biāo)（xi，yi）與序號(hào)i∈C構(gòu)成互為映射關(guān)系，序號(hào)i的坐標(biāo)可由（1）式確定：

xi=((i-1) mod Nx)+1，

yi=(int)((i-1)/Nx)+1 （1）

式中，int為取取整運(yùn)算，mod為求余運(yùn)算。

Rob在AS中的位置記為PR，?坌 PR在∑0都有確定的坐標(biāo)(x，y)，t時(shí)刻的位置表示為PR(t)，其坐標(biāo)為(xR(t)，yR(t))。

對(duì)于任意二維地形，規(guī)劃的目的是使機(jī)器人由任意起點(diǎn)Gbegin，安全地沿一條較短路徑到達(dá)任意終點(diǎn)Gend。且Gbegin、Gend OS，其它約束條件為：begin ，end∈C， begin≠end。

3算法的總體思路及算法描述

算法首先根據(jù)螞蟻覓食行為對(duì)多個(gè)目標(biāo)點(diǎn)的組合進(jìn)行優(yōu)化，規(guī)劃出一條最優(yōu)的全局導(dǎo)航路徑。在此基礎(chǔ)上，機(jī)器人按照規(guī)劃好的目標(biāo)點(diǎn)訪問順序根據(jù)多螞蟻協(xié)作局部路徑算法完成局部路徑的規(guī)劃。規(guī)劃過程分兩步完成，第一步，暫時(shí)不考慮動(dòng)態(tài)物，以Rob當(dāng)前位置PR(t)為蟻穴，Gi為食物源或反之，螞蟻從PR(t)或Gi出發(fā)，進(jìn)行尋找食物的過程，經(jīng)過螞蟻群體的反復(fù)尋食，由于螞蟻留下的信息素的正反饋?zhàn)饔?，最終繞開所有障礙物找到一條最短局部路徑。根據(jù)該靜態(tài)局部?jī)?yōu)化路徑和動(dòng)態(tài)物的軌跡進(jìn)行碰撞預(yù)測(cè)，當(dāng)碰撞點(diǎn)無法回避時(shí)，則將其列為禁入域，由螞蟻重新快速規(guī)劃出一條動(dòng)態(tài)避障局部?jī)?yōu)化路徑。Rob每行進(jìn)一步，都進(jìn)行上述局部避碰規(guī)劃過程，因此，其規(guī)劃出的局部路徑是實(shí)時(shí)動(dòng)態(tài)修改的。

為了敘述方便，進(jìn)一步作出如下約定和定義：

定義1：View(PR(t))={P|P∈A，d(P，PR(t))≤r}稱為Rob在位置PR(t)處的視野域。

機(jī)器人路徑滾動(dòng)規(guī)劃的螞蟻算法步驟描述如下：

Step 1：利用ACS算法根據(jù)螞蟻覓食行為對(duì)多個(gè)目標(biāo)點(diǎn)的組合進(jìn)行優(yōu)化，規(guī)劃出一條最優(yōu)的全局導(dǎo)航路徑{G'1，G'2，…，G'i，…Gin， G'n+1}，其中G'1=G'n+1=Gbegin，G'2，…，G'i，…Gin為G2，…，Gi，…Gn的一個(gè)全排列；并設(shè)N=1；

Step 2：若N>n+1，規(guī)劃結(jié)束，否則轉(zhuǎn)Step 3；

Step 3：Rob根據(jù)環(huán)境已知的Sbi， i=(1，2，…h(huán))， 用多螞蟻協(xié)作局部路徑規(guī)劃算法迅速規(guī)劃出不考慮Dbi時(shí)從G'N到G'N+1的局部靜態(tài)優(yōu)化路徑；

Step 4：Rob在當(dāng)前位置PR(t)進(jìn)行環(huán)境探測(cè)，識(shí)別出動(dòng)態(tài)障礙物Dbi。其中PR(t)～gR。

Step 5: 若無Dbi，轉(zhuǎn)Step7，否則，則測(cè)出其方向、速度、運(yùn)動(dòng)軌跡并預(yù)測(cè)出Rob沿上一次規(guī)劃出的局部靜態(tài)優(yōu)化路徑向gsub前進(jìn)時(shí)與Dbi的碰撞點(diǎn)，碰撞點(diǎn)的集合記作CO；

Step 6：根據(jù)CO情況和避碰策略進(jìn)行避碰處理。當(dāng)碰撞點(diǎn)不能回避時(shí)，將CO作為靜態(tài)障礙物，再用多螞蟻協(xié)作算法重新規(guī)劃當(dāng)前位置PR(t)到G'N+1的一條避碰局部?jī)?yōu)化路徑。

Step 7：Rob沿著規(guī)劃出的局部最優(yōu)路徑向G'N+1前進(jìn)一步，若PR(t)不等于G'N+1，轉(zhuǎn)到Step 4；

Step 8：N=N+1，返回Step 2。

4 仿真實(shí)驗(yàn)

為了驗(yàn)證算法的效果，作者進(jìn)行了大量的仿真實(shí)驗(yàn)，結(jié)果都十分令人滿意。假設(shè)環(huán)境地圖大小為30*30，靜態(tài)障礙物信息已知。為了實(shí)驗(yàn)算法的效果，設(shè)計(jì)了圖2所示的復(fù)雜地形。本文算法首先根據(jù)ACS算法可以迅速規(guī)劃出一條全局導(dǎo)航路徑引導(dǎo)機(jī)器人按照該順序遍歷各個(gè)目標(biāo)點(diǎn)，如圖2所示。圖中紅色圓心節(jié)點(diǎn)為機(jī)器人起始位置Gbegin，方形紅色填充區(qū)域?yàn)闄C(jī)器人需要訪問的目標(biāo)點(diǎn)，黑色柵格為障礙物。Rob到達(dá)某個(gè)目標(biāo)點(diǎn)位置時(shí)（開始時(shí)為Gbegin）再根據(jù)多螞蟻協(xié)作局部路徑規(guī)劃算法規(guī)劃出到達(dá)下一個(gè)目標(biāo)點(diǎn)的局部避障路徑，Rob每前進(jìn)一步，都進(jìn)行碰撞預(yù)測(cè)和動(dòng)態(tài)避碰局部規(guī)劃，如圖3所示，圖中粗線為機(jī)器人根據(jù)探測(cè)到的動(dòng)態(tài)障礙物再次規(guī)劃出的局部路徑。

從實(shí)驗(yàn)結(jié)果可以看出，只要各個(gè)目標(biāo)點(diǎn)之間存在路徑，總可以規(guī)劃出有效路徑，且Rob從Gbegin逐一訪問各個(gè)目標(biāo)點(diǎn)后返回到Gbegin的路徑為一條優(yōu)化路徑。

5 小結(jié)

算法首先快速規(guī)劃出各個(gè)目標(biāo)點(diǎn)的訪問次序，給出了Rob的一種引導(dǎo)趨勢(shì)。Rob行進(jìn)過程中采用兩組螞蟻協(xié)作完成基于靜態(tài)全局信息的局部路徑優(yōu)化規(guī)劃，其中，一組螞蟻置于機(jī)器人當(dāng)前位置，另一組螞蟻置于目標(biāo)點(diǎn)，兩組螞蟻相向并行搜索，由兩組螞蟻工程完成最優(yōu)路徑的搜索，加上設(shè)有隨機(jī)搜索策略，保證了搜索的多樣性，使搜索不易限于停滯。只要有可行通道客觀存在，即使在非常復(fù)雜的地形環(huán)境，也能迅速找到一條優(yōu)化路徑。由于采用的啟發(fā)函數(shù)具有趨近導(dǎo)向作用和螞蟻信息素的正反饋，加上兩組螞蟻的并行性，因而局部路徑規(guī)劃效率較高。在此基礎(chǔ)上，實(shí)時(shí)地進(jìn)行動(dòng)態(tài)障礙物運(yùn)動(dòng)軌跡預(yù)測(cè)以及碰撞預(yù)測(cè)，在機(jī)器人視野域范圍，將該動(dòng)態(tài)物軌跡上預(yù)測(cè)碰撞點(diǎn)及附近的柵格視為靜態(tài)障礙物，從而可以再次實(shí)時(shí)地作出動(dòng)態(tài)避障局部規(guī)劃，使得不僅安全避碰，而且使機(jī)器人行走路徑基本達(dá)最優(yōu)。仿真結(jié)果表明，該算法具有簡(jiǎn)單、快速、高效、安全等特點(diǎn)，可以有效解決多個(gè)目標(biāo)點(diǎn)的機(jī)器人路徑規(guī)劃問題。

參考文獻(xiàn)：

[1] Hwang J Y， Kim J S. A fast path planning by path graph optimization Systems[J].Man and Cybernetics，Part A，IEEE Transactions，2003，33(1):121-129.

[2] Ikeda T， Hsu M Y， Imai M. A Fast Algorithm for Finding Better Routes by AI Search Techniques[C].IEEE Vehicle Navigation Information Systems Conference Proceedings，1994:291-296.

[3] Kim B N， Kwon O S. A study on path planning for mobile robot based on fuzzy logic controller[C].Proceedings of the IEEE Region 10 Conference，1999，2(15-17):1002-1005.

[4] Araújo F， Ribeiro B， Rodrigues L.A Neural Network for Shortest Path Computation[J].IEEE Transactions on Neural Networks， Sep. 2001，12(5):1067-1073.

[5] Gen M， Cheng R W， Wang D W. Genetic Algorithms for Solving Shortest Path Problems[J].IEEE，1997:401-406.

[6] 張純剛，席裕庚.全局環(huán)境未知時(shí)基于滾動(dòng)窗口的機(jī)器人路徑規(guī)劃[J].中國(guó)科學(xué)（E輯），2001，31(1):51-58.

[7] Lawler E L， Lenstra J K， Rinnooy A H G， et al. The Traveling Salesman Problem[M]. New York:John Wiley Sons，1985.

[8] Colorni A， Dorigo M， Maniezzo V， Distributed optimization by ant colonies[C]. Paris:Proceeding of ECAL91 European Conference of Artificial Life，1991:134-144.

[9] Dorigo M. Di Caro G. Ant colony system: a cooperative learning approach to the traveling salesman problem[C].IEEE Transactions on Evolutionary Computation， 1997，1(1):53-66.

[10] 朱慶保.全局未知環(huán)境下多機(jī)器人運(yùn)動(dòng)螞蟻導(dǎo)航算法[J].軟件學(xué)報(bào)，2006，17(9):1890-1898.