移動機器人多目標搜尋的D*-蟻群融合算法

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

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

E-mail:w8888i@foxmail.com

1 引 言

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

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

2 問題描述與模型

2.1 地圖環境建立

格柵法建立地圖模型原理簡單、容易實現,因此在地圖模型的建立中得到廣泛的使用,基于格柵法建立地圖模型,每一個格柵對應著實際環境的某一個位置,格柵地圖的分辨率是一個重要的參數,分辨率越高,環境表示越精準;分辨率越低,處理速度越快,但是會影響路徑規劃的精度.本文所研究的是中小范圍環境,格柵法建立地圖模型是合理的.

如圖1所示,灰色圓柱體為機器人所在節點,箭頭所指方向為節點可擴展方向,深灰色格柵為障礙物,在本文中將機器人考慮成質點,同時只涉及二維平面內的移動.圖2所示為格柵序號和實際坐標的對應關系,那么機器人在格柵地圖中的實際位置和地圖坐標的對應關系為:

(1)

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

(2)

2.2 目標搜尋數學模型

該問題數學模型是一個最短路徑求解問題,用集合V={v1,v2,…,vi,…,vn}表示所需要搜尋的目標點集合,vi代表所需要搜尋的第i個目標,v1代表機器人的出發點視作第一個需要搜索的目標,n代表目標點個數,最優路徑目標函數為:

(3)

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

2.3 兩種算法描述

2.3.1 蟻群算法

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

(4)

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

(5)

(6)

(7)

2.3.2 D*算法

D*算法是一種動態逆向搜索算法,即從目標點開始擴展直到將起點,其估價函數為:

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

(8)

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

(9)

D*算法常用參數如下所示:

X,Y:機器人的狀態.

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

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

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

t(X):狀態X的標簽(例如OPEN、CLOSED和NEW).

h(X):實際目標點到各個格柵的最短路徑成本.

k(X):當X狀態發生變化后例如行進過程中原有自由空間堵塞,那么成本h(X)會發生改變,k值取變化前后最小的h值即k(X)=min(hold(X),hnew(X)).

傳統D*算法計算流程如圖3所示,黑色為障礙物,白色為自由空間,(2,1)為實際起點;(7,6)為實際目標點,不同與A*算法,D*算法將(7,6)作為節點擴展起點,直到將實際起點擴展到OPEN表中,通過圖3中黑色反向指針找到最優路徑:

圖3 D*算法

3 算法改進與融合

3.1 D*算法的改進

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

(10)

加之修改后的路徑成本,那么改進后的D*算法啟發項為:

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

(11)

針對原始D*算法生成的路徑小范圍轉角過多的問題,通過改進其啟發函數得到一定程度的解決,但是效果還不夠理想,同時生成路徑過于靠近障礙物,對實際機器人的行進產生安全隱患,因此將子節點的擴展機制進一步改進.改進的擴展機制分為兩部分:一是改進子節點代價值計算,而是子節點的擴展選取.

圖4 原始擴展方式

由于原始D*算法長度優先的特性,若如圖4所示的3號擴展方向的節點成本最低,那么生成的路徑(淺灰色區域)就會緊靠著著障礙物,在仿真環境中是可行的,但實際應用起來往往會造成目標點不可達.

圖5 子節點擴展方向

這里再將子節點的基本項進行改進,添加角度偏移產生的額外代價,那么新的基本項如式(12)所示:

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

(12)

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

Δ(n)=Δφ*σ

(13)

其中,σ為角度偏移系數.

最后將圖4中橫豎方向節點當作一級節點(2、4、6、8),斜向節點當作二級節點(1、3、5、7),那么現在子節點擴展選取改變為:若節點2不可達,則將1、3節點也標記為不可達,因此不會生成0-1、0-3路徑.其它的一級節點和二級節點選取以此類推.

3.2 兩種算法融合

本文的核心是解決多目標搜尋問題,在3.1節中,已經對原始D*算法進行改進,則此時還需考慮TSP問題,若單獨使用改進D*算法,找到任意兩個目標點之間的最短距離,會導致路徑結果不一定是全局范圍中的最優路徑.本文引入蟻群算法融合改進D*算法來解決該問題,蟻群算法的目標函數根據式表示為:

(14)

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

圖6 蟻群算法路徑規劃

4 算法流程

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

圖7 算法融合流程

具體步驟如下:

步驟1.采用格柵圖對機器人工作環境建模,建立n階地圖,同時建立一個map矩陣對格柵狀態進行存儲,初始化螞蟻數量NP=80,目標數m,信息素重要權值alpha=1,啟發項重要權值beta=5,最大迭代次數Maxgen=200,形成初始信息素矩陣Tau,將各代最佳路徑長度存放在bestLength,各代最佳路徑平均長度meanLength.

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

步驟3.如果Open表不為空,選擇Open表中成本最小的格柵,將訪問的格柵放入Closed表中同時更新Open表,評估領域空間中所有節點,如果節點在Closed中且成本更小,則進行更新,成本無窮大,說明是障礙物,跳過該點.若節點既不在Open表中,也不在Close表中,將其插入Open表,如果節點位于Open中,判斷新路徑是否更優如果節點位于Close中,判斷新路徑是否更優,如果更優解,則更新.

步驟4.回溯所有父節點得到距離函數D.

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

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

步驟7.循環執行步驟6直到最大迭代數或找到最優解.

步驟8.輸出最優結果:

Shortest_Length=min(bestLength).

5 仿真結果與分析

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

圖8 矩陣地圖

在100階地圖中,選取5個目標為node=[34 20;90 50;14 71;10 90;30 50],12個目標為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].仿真結果如圖9、圖10所示.

表1 實驗數據對比

Table 1 Comparison of experimental data

地圖一算法路徑長度/像素轉角次數/次處理時間/秒5個目標ACO-A?1577.691253.429ACO-D?1541.965303.511ACO-ID?1603.514123.10912個目標ACO-A?2127.6753310.137ACO-D?2115.0944610.954ACO-ID?2112.366148.324

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

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

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

圖9 5個目標

圖10 12個目標

圖11 離線先驗路徑

表2 實驗數據對比

Table 2 Comparison of experimental data

地圖二算法路徑長度/像素轉角次數/次處理時間/秒8個目標ACO-A?5006.25975671.751ACO-D?4921.18873682.358ACO-MD?5013.14542438.465

圖12 復雜地圖搜索8個目標

6 結 論

本文對多目標搜尋問題提出一種有效的解決方案,針對原始D*算法和原始蟻群算法在解決路徑規劃問題和TSP問題上均有缺陷,通過估計函數、子節點擴展機制等來改良D*算法路徑規劃能力,然后通過改進D*算法來計算蟻群算法中的啟發因子.仿真實驗得出,改進D*融合蟻群算法在多目標搜尋中相比于單個或其他融合算法有明顯優勢,證明該算法的有效性和實用性.