移動(dòng)機(jī)器人多目標(biāo)搜尋的D*-蟻群融合算法

胡立坤,王帥軍,呂智林,朱文天

(廣西大學(xué) 電氣工程學(xué)院,南寧 530004)

E-mail:w8888i@foxmail.com

1 引 言

目標(biāo)搜尋是移動(dòng)機(jī)器人常見(jiàn)任務(wù)之一,同時(shí)也是機(jī)器人領(lǐng)域研究的一個(gè)熱點(diǎn)問(wèn)題,通常應(yīng)用于農(nóng)田播種撒藥、雷區(qū)探雷排雷等多個(gè)場(chǎng)所.根據(jù)目標(biāo)數(shù)量可分為單目標(biāo)和多目標(biāo)搜尋問(wèn)題,也就是路徑規(guī)劃問(wèn)題和旅行商遍歷問(wèn)題[1](TSP)的結(jié)合.目標(biāo)搜索本質(zhì)上是機(jī)器人自主導(dǎo)航的問(wèn)題,而路徑規(guī)劃是實(shí)現(xiàn)自主導(dǎo)航的關(guān)鍵技術(shù)之一[2],針對(duì)路徑規(guī)劃問(wèn)題國(guó)內(nèi)外學(xué)者做了大量研究,提出了多種算法:傳統(tǒng)的有人工勢(shì)場(chǎng)法[3],其最大的缺點(diǎn)是容易陷入局部最優(yōu)導(dǎo)致無(wú)法找到全局最優(yōu)路徑,啟發(fā)式算法A*和D*等由于其搜索高效性在路徑規(guī)劃中也得到廣泛應(yīng)用[4,5],隨著計(jì)算機(jī)技術(shù)發(fā)展,智能優(yōu)化算法也逐漸利用在路徑規(guī)劃上,遺傳算法[6]、粒子群算法[7]和蟻群算法[8]等等,也包括了傳統(tǒng)算法和智能算法結(jié)合[9]的方法.當(dāng)目標(biāo)點(diǎn)不只一個(gè),此時(shí)除了路徑規(guī)劃問(wèn)題,則還會(huì)涉及到TSP問(wèn)題,TSP求解方案有精確法和近似法,在目標(biāo)點(diǎn)較多時(shí),精確法處理時(shí)間長(zhǎng),無(wú)法勝任,而智能算法作為得到近似解的最好方案,在解決TSP問(wèn)題中最常用[10,11].由于蟻群算法的魯棒性強(qiáng)、并行性高同時(shí)容易融合其他算法,在TSP問(wèn)題以及路徑規(guī)劃問(wèn)題和指派問(wèn)題等等上有著廣泛的應(yīng)用前景[12].

D*算法是1994年Stentz提出的一種動(dòng)態(tài)搜索算法[13],是在A*算法的基礎(chǔ)上,通過(guò)將終點(diǎn)當(dāng)作擴(kuò)展節(jié)點(diǎn)的起點(diǎn)一直擴(kuò)展到原始起點(diǎn),利用反向指針沿著成本最低的路徑找到一條從原始起點(diǎn)到原始終點(diǎn)的路徑,D*算法的優(yōu)勢(shì)在于在機(jī)器人行進(jìn)圖中地圖成本更改時(shí),二次規(guī)劃可以利用之前規(guī)劃的地圖信息減少計(jì)算量.針對(duì)D*算法的一些問(wèn)題,文獻(xiàn)[14]提出一種拓展Moore型元胞結(jié)構(gòu)來(lái)改善路徑長(zhǎng)度,通過(guò)跳點(diǎn)法來(lái)找到機(jī)器人感興趣的點(diǎn),節(jié)省計(jì)算時(shí)間.文獻(xiàn)[15]通過(guò)遍歷路徑中所有節(jié)點(diǎn),當(dāng)兩個(gè)節(jié)點(diǎn)之間沒(méi)有障礙物時(shí),刪除其中間節(jié)點(diǎn)并直接連接前后節(jié)點(diǎn),建立平滑路徑模型.上述改進(jìn)一定程度上解決了原始D*算法的某些缺陷,但對(duì)于實(shí)際應(yīng)用時(shí),生成路徑十分貼近障礙物的問(wèn)題并沒(méi)有解決,同時(shí)當(dāng)搜索目標(biāo)點(diǎn)有多個(gè)的時(shí)候,單純的D*算法是無(wú)法解決的,此時(shí)還需要考慮一個(gè)最短遍歷的問(wèn)題.本文首先通過(guò)改進(jìn)D*算法,解決上述問(wèn)題,同時(shí)將改進(jìn)D*算法融合蟻群算法來(lái)解決多目標(biāo)遍歷和路徑規(guī)劃的問(wèn)題.仿真實(shí)驗(yàn)結(jié)果表明,本文提出的融合算法能有效完成多目標(biāo)搜尋任務(wù),同時(shí)在規(guī)劃速度、轉(zhuǎn)角的度數(shù)和次數(shù)、路徑安全質(zhì)量等一些移動(dòng)機(jī)器人行走時(shí)的關(guān)鍵因素有明顯改善,并且本文融合算法和單個(gè)蟻群算法以及蟻群算法與傳統(tǒng)A*、D*算法融合的效果比較而言有明顯優(yōu)勢(shì).

2 問(wèn)題描述與模型

2.1 地圖環(huán)境建立

格柵法建立地圖模型原理簡(jiǎn)單、容易實(shí)現(xiàn),因此在地圖模型的建立中得到廣泛的使用,基于格柵法建立地圖模型,每一個(gè)格柵對(duì)應(yīng)著實(shí)際環(huán)境的某一個(gè)位置,格柵地圖的分辨率是一個(gè)重要的參數(shù),分辨率越高,環(huán)境表示越精準(zhǔn);分辨率越低,處理速度越快,但是會(huì)影響路徑規(guī)劃的精度.本文所研究的是中小范圍環(huán)境,格柵法建立地圖模型是合理的.

如圖1所示,灰色圓柱體為機(jī)器人所在節(jié)點(diǎn),箭頭所指方向?yàn)楣?jié)點(diǎn)可擴(kuò)展方向,深灰色格柵為障礙物,在本文中將機(jī)器人考慮成質(zhì)點(diǎn),同時(shí)只涉及二維平面內(nèi)的移動(dòng).圖2所示為格柵序號(hào)和實(shí)際坐標(biāo)的對(duì)應(yīng)關(guān)系,那么機(jī)器人在格柵地圖中的實(shí)際位置和地圖坐標(biāo)的對(duì)應(yīng)關(guān)系為:

(1)

其中mod代表取余運(yùn)算,ceil代表求整運(yùn)算,根據(jù)式(1)就可以得到機(jī)器人在當(dāng)前格柵地圖中的真實(shí)坐標(biāo).將地圖信息存放在二維矩陣map(i,j)中,矩陣可以表示為:

(2)

2.2 目標(biāo)搜尋數(shù)學(xué)模型

該問(wèn)題數(shù)學(xué)模型是一個(gè)最短路徑求解問(wèn)題,用集合V={v1,v2,…,vi,…,vn}表示所需要搜尋的目標(biāo)點(diǎn)集合,vi代表所需要搜尋的第i個(gè)目標(biāo),v1代表機(jī)器人的出發(fā)點(diǎn)視作第一個(gè)需要搜索的目標(biāo),n代表目標(biāo)點(diǎn)個(gè)數(shù),最優(yōu)路徑目標(biāo)函數(shù)為:

(3)

則現(xiàn)在需要做的即找到一個(gè)解向量使得式(3)最小.

2.3 兩種算法描述

2.3.1 蟻群算法

蟻群算法數(shù)學(xué)模型如下:在t0時(shí)刻,將m只螞蟻隨機(jī)放置在n個(gè)目標(biāo)中,初始化所有路徑的信息素值,t時(shí)刻邊(i,j)上的信息素表示為τij(t),t0信息素為τij(0).在后續(xù)任意一只螞蟻移動(dòng)過(guò)程中都是根據(jù)信息素濃度來(lái)決定移動(dòng)方向,其選擇概率為:

(4)

τij(t+1)=(1-ρ)τij(t)+Δτij

(5)

(6)

(7)

2.3.2 D*算法

D*算法是一種動(dòng)態(tài)逆向搜索算法,即從目標(biāo)點(diǎn)開(kāi)始擴(kuò)展直到將起點(diǎn),其估價(jià)函數(shù)為:

f(n)=g(n)+h(n)

(8)

其中g(shù)(n)是基本項(xiàng),代表終點(diǎn)(Goal)到當(dāng)前節(jié)點(diǎn)的實(shí)際成本;h(n)為啟發(fā)項(xiàng),代表當(dāng)前節(jié)點(diǎn)到起點(diǎn)(Start)的最小成本估計(jì)值;n為當(dāng)前節(jié)點(diǎn).D*算法一般采用歐氏距離作為距離計(jì)算方式,如式所示,首先需要對(duì)每個(gè)節(jié)點(diǎn)進(jìn)行平方根運(yùn)算,降低了處理速度,其次歐氏距離不考慮角度的限制,是二維或三維空間中任意兩點(diǎn)的真實(shí)距離,而本文采用格柵法建立地圖模型,對(duì)于地圖中兩個(gè)格柵之間的距離不能準(zhǔn)確描述.總的來(lái)說(shuō),D*算法的搜索效率直接取決于估價(jià)函數(shù)的選取,可以根據(jù)實(shí)際環(huán)境選擇不同的估價(jià)函數(shù).

(9)

D*算法常用參數(shù)如下所示:

X,Y:機(jī)器人的狀態(tài).

b(X)=Y:從狀態(tài)X到狀態(tài)Y的反向指針.

c(X,Y):從Y到X的弧成本,障礙物成本為無(wú)窮大.

r(X,Y):傳感器實(shí)際采集到的從Y到X的弧成本.

t(X):狀態(tài)X的標(biāo)簽(例如OPEN、CLOSED和NEW).

h(X):實(shí)際目標(biāo)點(diǎn)到各個(gè)格柵的最短路徑成本.

k(X):當(dāng)X狀態(tài)發(fā)生變化后例如行進(jìn)過(guò)程中原有自由空間堵塞,那么成本h(X)會(huì)發(fā)生改變,k值取變化前后最小的h值即k(X)=min(hold(X),hnew(X)).

傳統(tǒng)D*算法計(jì)算流程如圖3所示,黑色為障礙物,白色為自由空間,(2,1)為實(shí)際起點(diǎn);(7,6)為實(shí)際目標(biāo)點(diǎn),不同與A*算法,D*算法將(7,6)作為節(jié)點(diǎn)擴(kuò)展起點(diǎn),直到將實(shí)際起點(diǎn)擴(kuò)展到OPEN表中,通過(guò)圖3中黑色反向指針找到最優(yōu)路徑:

圖3 D*算法

3 算法改進(jìn)與融合

3.1 D*算法的改進(jìn)

DChess(Goal,n)=max(|xGoal-xn|,|yGoal-yn|)

(10)

加之修改后的路徑成本,那么改進(jìn)后的D*算法啟發(fā)項(xiàng)為:

h(n)=10hstraight+14hdiagonal=10||yGoal-yn|-|xGoal-xn||+14min[|yGoal-yn|,|xGoal-xn|]

(11)

針對(duì)原始D*算法生成的路徑小范圍轉(zhuǎn)角過(guò)多的問(wèn)題,通過(guò)改進(jìn)其啟發(fā)函數(shù)得到一定程度的解決,但是效果還不夠理想,同時(shí)生成路徑過(guò)于靠近障礙物,對(duì)實(shí)際機(jī)器人的行進(jìn)產(chǎn)生安全隱患,因此將子節(jié)點(diǎn)的擴(kuò)展機(jī)制進(jìn)一步改進(jìn).改進(jìn)的擴(kuò)展機(jī)制分為兩部分:一是改進(jìn)子節(jié)點(diǎn)代價(jià)值計(jì)算,而是子節(jié)點(diǎn)的擴(kuò)展選取.

圖4 原始擴(kuò)展方式

由于原始D*算法長(zhǎng)度優(yōu)先的特性,若如圖4所示的3號(hào)擴(kuò)展方向的節(jié)點(diǎn)成本最低,那么生成的路徑(淺灰色區(qū)域)就會(huì)緊靠著著障礙物,在仿真環(huán)境中是可行的,但實(shí)際應(yīng)用起來(lái)往往會(huì)造成目標(biāo)點(diǎn)不可達(dá).

圖5 子節(jié)點(diǎn)擴(kuò)展方向

這里再將子節(jié)點(diǎn)的基本項(xiàng)進(jìn)行改進(jìn),添加角度偏移產(chǎn)生的額外代價(jià),那么新的基本項(xiàng)如式(12)所示:

gnew(n)=gold(n)+Δ(n)

(12)

Δ(n)為角度偏移代價(jià),根據(jù)Δφ的值,定義Δ(n)為:

Δ(n)=Δφ*σ

(13)

其中,σ為角度偏移系數(shù).

最后將圖4中橫豎方向節(jié)點(diǎn)當(dāng)作一級(jí)節(jié)點(diǎn)(2、4、6、8),斜向節(jié)點(diǎn)當(dāng)作二級(jí)節(jié)點(diǎn)(1、3、5、7),那么現(xiàn)在子節(jié)點(diǎn)擴(kuò)展選取改變?yōu)?若節(jié)點(diǎn)2不可達(dá),則將1、3節(jié)點(diǎn)也標(biāo)記為不可達(dá),因此不會(huì)生成0-1、0-3路徑.其它的一級(jí)節(jié)點(diǎn)和二級(jí)節(jié)點(diǎn)選取以此類(lèi)推.

3.2 兩種算法融合

本文的核心是解決多目標(biāo)搜尋問(wèn)題,在3.1節(jié)中,已經(jīng)對(duì)原始D*算法進(jìn)行改進(jìn),則此時(shí)還需考慮TSP問(wèn)題,若單獨(dú)使用改進(jìn)D*算法,找到任意兩個(gè)目標(biāo)點(diǎn)之間的最短距離,會(huì)導(dǎo)致路徑結(jié)果不一定是全局范圍中的最優(yōu)路徑.本文引入蟻群算法融合改進(jìn)D*算法來(lái)解決該問(wèn)題,蟻群算法的目標(biāo)函數(shù)根據(jù)式表示為:

(14)

其中minL表示最優(yōu)路徑的長(zhǎng)度,d(vi,vi+1)表示任意兩個(gè)目標(biāo)點(diǎn)之間的長(zhǎng)度即為式中的啟發(fā)因子,傳統(tǒng)蟻群算法啟發(fā)因子是根據(jù)目標(biāo)點(diǎn)的坐標(biāo)來(lái)計(jì)算,也就是Dijkstra算法,這種距離啟發(fā)因子對(duì)下個(gè)節(jié)點(diǎn)尋找的啟發(fā)性不強(qiáng),尤其是當(dāng)兩個(gè)目標(biāo)點(diǎn)距離較遠(yuǎn)時(shí),存在著計(jì)算時(shí)間長(zhǎng)、啟發(fā)效果差和易陷入局部最優(yōu)的缺點(diǎn),如圖6所示,利用蟻群算法路徑規(guī)劃,仿真環(huán)境為20階,規(guī)劃時(shí)間t=59.034s,若在更加精密環(huán)境例如100階、600階的地圖矩陣中計(jì)算時(shí)間將會(huì)更久,這樣的規(guī)劃速度是難以接受的.因此將原有的啟發(fā)因子用改進(jìn)D*算法來(lái)計(jì)算,即式中分母部分采用改進(jìn)D*算法來(lái)求解.

圖6 蟻群算法路徑規(guī)劃

4 算法流程

算法融合流程如圖7所示.

圖7 算法融合流程

具體步驟如下:

步驟1.采用格柵圖對(duì)機(jī)器人工作環(huán)境建模,建立n階地圖,同時(shí)建立一個(gè)map矩陣對(duì)格柵狀態(tài)進(jìn)行存儲(chǔ),初始化螞蟻數(shù)量NP=80,目標(biāo)數(shù)m,信息素重要權(quán)值alpha=1,啟發(fā)項(xiàng)重要權(quán)值beta=5,最大迭代次數(shù)Maxgen=200,形成初始信息素矩陣Tau,將各代最佳路徑長(zhǎng)度存放在bestLength,各代最佳路徑平均長(zhǎng)度meanLength.

步驟2.初始化Open和Closed列表,n階地圖矩陣map(map==1)=inf,map(map==0)=1,確保起點(diǎn)終點(diǎn)不是障礙物,map(S)=0;map(T)=0,確定成本矩陣f=NaN*ones(n,n).

步驟3.如果Open表不為空,選擇Open表中成本最小的格柵,將訪問(wèn)的格柵放入Closed表中同時(shí)更新Open表,評(píng)估領(lǐng)域空間中所有節(jié)點(diǎn),如果節(jié)點(diǎn)在Closed中且成本更小,則進(jìn)行更新,成本無(wú)窮大,說(shuō)明是障礙物,跳過(guò)該點(diǎn).若節(jié)點(diǎn)既不在Open表中,也不在Close表中,將其插入Open表,如果節(jié)點(diǎn)位于Open中,判斷新路徑是否更優(yōu)如果節(jié)點(diǎn)位于Close中,判斷新路徑是否更優(yōu),如果更優(yōu)解,則更新.

步驟4.回溯所有父節(jié)點(diǎn)得到距離函數(shù)D.

步驟5.將NP只螞蟻隨機(jī)放置在地圖中,初始化信息素矩陣Tau,路徑記錄表Table=zeros(NP,m)和新的啟發(fā)函數(shù)Eta=1./D,將Eta帶入式建立的概率分布進(jìn)行螞蟻下一步節(jié)點(diǎn)選擇,已訪問(wèn)節(jié)點(diǎn)放入禁忌表tabu中.

步驟6.由各個(gè)螞蟻的路徑距離計(jì)算最短路徑bestLength及平均距離meanLength,偽代碼如下,更新信息素dTau=zeros(m,m),進(jìn)行下一次迭代,清空Table.

步驟7.循環(huán)執(zhí)行步驟6直到最大迭代數(shù)或找到最優(yōu)解.

步驟8.輸出最優(yōu)結(jié)果:

Shortest_Length=min(bestLength).

5 仿真結(jié)果與分析

為了驗(yàn)證算法的質(zhì)量和效率,在MATLAB中進(jìn)行以下仿真實(shí)驗(yàn),分別采用蟻群-A*算法(ACO-A*)、蟻群-D*算法(ACO-D*)和本文提出的蟻群-改進(jìn)D*算法(ACO-MD*),在地圖1(100階)中搜索5個(gè)和12個(gè)目標(biāo),在地圖2(600階)中搜索8個(gè)目標(biāo),然后分別對(duì)比其實(shí)驗(yàn)結(jié)果.參數(shù)初始化前所述,兩份仿真地圖如圖8所示.

圖8 矩陣地圖

在100階地圖中,選取5個(gè)目標(biāo)為node=[34 20;90 50;14 71;10 90;30 50],12個(gè)目標(biāo)為node=[10 90;20 95;15 50;50 50;30 30;40 10;70 20;80 30;90 50;75 55;80 85;40 80].仿真結(jié)果如圖9、圖10所示.

表1 實(shí)驗(yàn)數(shù)據(jù)對(duì)比

Table 1 Comparison of experimental data

地圖一算法路徑長(zhǎng)度/像素轉(zhuǎn)角次數(shù)/次處理時(shí)間/秒5個(gè)目標(biāo)ACO-A?1577.691253.429ACO-D?1541.965303.511ACO-ID?1603.514123.10912個(gè)目標(biāo)ACO-A?2127.6753310.137ACO-D?2115.0944610.954ACO-ID?2112.366148.324

從圖9、圖10看出,三種組合算法都能順利完成任務(wù),但前兩種算法存在明顯缺陷,首先是兩種算法都太過(guò)于靠近障礙物,將地圖一的標(biāo)度尺寸設(shè)置為671×621像素,通過(guò)表1三種對(duì)比參數(shù)知,三種算法在路徑長(zhǎng)度上基本一致,而在轉(zhuǎn)角次數(shù)上ACO-A*、ACO-D*這兩種算法轉(zhuǎn)角次數(shù)相比于ACO-ID*在搜索5個(gè)目標(biāo)時(shí)多了13和18次、搜索12個(gè)目標(biāo)時(shí)多了19和32次,若實(shí)際應(yīng)用在機(jī)器人上會(huì)產(chǎn)生更多能耗.將ACO算法中的啟發(fā)項(xiàng)替換為本文提出的改進(jìn)D*算法,在搜索5個(gè)目標(biāo)時(shí)處理時(shí)間分別為前兩種算法的90.6%和88.5%,12個(gè)目標(biāo)時(shí)分別為82.1%和86%,因?yàn)楫?dāng)前處理環(huán)境為100階矩陣,而且是一種開(kāi)放環(huán)境,無(wú)法將地圖進(jìn)行局部分解,所以性能有所提升但不夠明顯.

地圖2模擬的是一個(gè)室內(nèi)環(huán)境,地圖階數(shù)n為600,對(duì)障礙物的外形以及自由空間的描述會(huì)更加精細(xì),由于室內(nèi)環(huán)境邊界是完全封閉的,這里利用Voronoi圖[16]來(lái)得到一個(gè)離線先驗(yàn)路徑,如圖11粗黑色曲線所示.

仿真路線如圖12所示,A*和D*算法基于地圖成本進(jìn)行計(jì)算,為追求最短路徑,很多路徑都是緊貼著障礙物生成,在600階地圖下障礙物輪廓更加清晰導(dǎo)致該情況尤為明顯,因此ACO-A*和ACO-D*除了算法本身產(chǎn)生了很多轉(zhuǎn)角,緊貼障礙物的路徑也增加了相當(dāng)數(shù)量的轉(zhuǎn)角,由表2知ACO-MD*相較于前兩種算法而言轉(zhuǎn)交次數(shù)分別減少了33次、31次,地圖2標(biāo)度尺寸設(shè)置為1000×1000像素,在減少轉(zhuǎn)角次數(shù)的基礎(chǔ)上路徑長(zhǎng)度比前兩種算法僅僅增加6.886和91.957個(gè)像素.由于將地圖2的Voronoi路線圖構(gòu)造出來(lái),選取距離各個(gè)目標(biāo)點(diǎn)最近的三叉點(diǎn)作為臨時(shí)節(jié)點(diǎn),規(guī)劃出連接各個(gè)節(jié)點(diǎn)一個(gè)無(wú)向圖,這個(gè)圖可以當(dāng)作是一條離線先驗(yàn)路徑如圖11所示,關(guān)閉圖中灰色區(qū)域節(jié)點(diǎn),在ACO-MD*算法規(guī)劃時(shí)使用先驗(yàn)路徑進(jìn)行預(yù)處理,算法規(guī)劃時(shí)間大幅度較低,為前兩種算法的65.27%和64.26%.

圖9 5個(gè)目標(biāo)

圖10 12個(gè)目標(biāo)

圖11 離線先驗(yàn)路徑

表2 實(shí)驗(yàn)數(shù)據(jù)對(duì)比

Table 2 Comparison of experimental data

地圖二算法路徑長(zhǎng)度/像素轉(zhuǎn)角次數(shù)/次處理時(shí)間/秒8個(gè)目標(biāo)ACO-A?5006.25975671.751ACO-D?4921.18873682.358ACO-MD?5013.14542438.465

圖12 復(fù)雜地圖搜索8個(gè)目標(biāo)

6 結(jié) 論

本文對(duì)多目標(biāo)搜尋問(wèn)題提出一種有效的解決方案,針對(duì)原始D*算法和原始蟻群算法在解決路徑規(guī)劃問(wèn)題和TSP問(wèn)題上均有缺陷,通過(guò)估計(jì)函數(shù)、子節(jié)點(diǎn)擴(kuò)展機(jī)制等來(lái)改良D*算法路徑規(guī)劃能力,然后通過(guò)改進(jìn)D*算法來(lái)計(jì)算蟻群算法中的啟發(fā)因子.仿真實(shí)驗(yàn)得出,改進(jìn)D*融合蟻群算法在多目標(biāo)搜尋中相比于單個(gè)或其他融合算法有明顯優(yōu)勢(shì),證明該算法的有效性和實(shí)用性.