基于交通約束及多元啟發(fā)函數(shù)的改進(jìn)A*算法*

張金越，侯至丞，張 弓，王衛(wèi)軍，楊文林

( 廣州中國科學(xué)院先進(jìn)技術(shù)研究所機器人與智能裝備中心,廣州 511458)

0 引言

路徑規(guī)劃是指移動機器人基于某些特定的準(zhǔn)則，在環(huán)境中存在障礙物的前提下，自主規(guī)劃一條從起始位置到目標(biāo)位置的安全、無碰撞的最短路徑。隨著自主移動機器人的快速發(fā)展，路徑規(guī)劃成為了一個熱門的研究問題。

現(xiàn)階段，國內(nèi)外眾多學(xué)者已取得了大量關(guān)于路徑規(guī)劃的研究成果：馬浩浩等[1]對傳統(tǒng)遺傳算法中的種群初始化、交叉算子以及變異算子進(jìn)行改進(jìn)，設(shè)計了機器人路徑規(guī)劃算法，有效提高了算法的效率；吳東林等[2]基于人工勢場法設(shè)計了采摘機器人的動態(tài)路徑規(guī)劃系統(tǒng)，具有很好地避障和路徑規(guī)劃的能力；陸皖麟等[3]在A*算法的基礎(chǔ)上添加閾值減少了搜索柵格的數(shù)量，并結(jié)合了Floyd算法去除了多余節(jié)點，使得規(guī)劃的路徑短且光滑；劉彩霞[4]提出了一種基于模糊推理的PSO路徑規(guī)劃算法，解決了移動機器人路徑規(guī)劃質(zhì)量不佳的問題；Jabbarpour M R等[5]針對現(xiàn)有路徑規(guī)劃算法無法降低UGV能耗的問題，將能耗預(yù)測模型與蟻群算法相結(jié)合，在減少計算時間和迭代次數(shù)的同時，規(guī)劃出低功耗的無碰撞最短路徑；Zhang H等[6]為防止RRT算法對配置空間的過度搜索，引入了回歸機制，提高了規(guī)劃的成功率和效率。在上述的路徑規(guī)劃算法中，遺傳算法、蟻群算法以及粒子群算法能否規(guī)劃出最優(yōu)路徑取決于各參數(shù)的選取；基于引力與斥力的原理的人工勢場法容易陷入受力平衡而出現(xiàn)目標(biāo)不可達(dá)的情況[7]；RRT算法存在搜索效率較低、用時較長的缺陷[8]；而A*算法在編程方式簡單的前提下能夠有效地搜索到最優(yōu)解。

對于多AGV的避碰問題，常見的避碰算法包括區(qū)域控制方法、時間窗法、交通約束法等[9]。其中，區(qū)域控制法容易在物品的傳遞環(huán)節(jié)浪費大量時間，降低工作效率；而時間窗法是利用AGV的行進(jìn)速度預(yù)測其經(jīng)過每一節(jié)點的時間，若AGV的速度發(fā)生變化，則會降低預(yù)測結(jié)果的可信度；交通約束法根據(jù)日常生活中的交通規(guī)則，通過優(yōu)先級的高低進(jìn)行減速避讓，有效地實現(xiàn)AGV間的避碰。

綜上所述，對于單一AGV，A*算法在路徑規(guī)劃的研究工作中一定程度提高了最優(yōu)路徑的搜索效率，但所規(guī)劃的路徑往往拐角數(shù)過多[10]，而AGV每次轉(zhuǎn)彎都需要經(jīng)歷加減速的過程，大量的拐角會增加AGV完成路徑行走的時間、能耗；而存在多AGV時，交通約束法能夠有效地解決AGV間的避碰問題，并且降低系統(tǒng)規(guī)劃路徑時的計算量。

因此，本文將研究采用一種基于交通約束及多元啟發(fā)函數(shù)的A*算法，對傳統(tǒng)的A*算法進(jìn)行優(yōu)化，引入交通約束，并在原有的啟發(fā)函數(shù)的基礎(chǔ)上，添加拐角數(shù)的評判指標(biāo)，構(gòu)成多元啟發(fā)函數(shù)，在保證規(guī)劃出最短路徑的前提下，實現(xiàn)拐角數(shù)最優(yōu)。

1 環(huán)境建模

環(huán)境建模即根據(jù)工作環(huán)境特點建立的抽象化地圖，是AGV進(jìn)行路徑規(guī)劃的先決條件。現(xiàn)階段常用的環(huán)境建模方法包括：柵格法、拓?fù)浞?、幾何信息法等[11]。

柵格法是將AGV的工作環(huán)境劃分為多個大小一致的柵格，采用二值信息表示自由空間和障礙物區(qū)域。拓?fù)浞╗12]是將環(huán)境劃分為多個具有一定拓?fù)涮卣鞯淖涌臻g，根據(jù)子空間的連通性構(gòu)建拓?fù)渚W(wǎng)絡(luò)并進(jìn)行搜索路徑的方法。幾何信息法利用環(huán)境信息提取出幾何特征，再使用這些幾何信息進(jìn)行環(huán)境建模。

圖1 貨架布局示意圖

由于本文所依托的項目主要執(zhí)行貨架上貨物的搬運任務(wù)，貨架的布局具體如圖1所示，其中，黑色區(qū)域為貨架，白色區(qū)域為自由空間。

出于建模方便、編程易于實現(xiàn)等因素的考慮，本文采用柵格法進(jìn)行環(huán)境建模。按照AGV實際的工作環(huán)境，建立一個12×10大小的柵格地圖，并根據(jù)通行狀態(tài)將該地圖劃分為可通行的白色自由區(qū)域以及不可通行的黑色障礙物區(qū)域。同時為了更好地描述柵格位置，本文采用坐標(biāo)的形式對柵格進(jìn)行編號，定義地圖左下角的坐標(biāo)為(1,1)，橫坐標(biāo)從左到右、縱坐標(biāo)從下到上逐個遞增，相鄰柵格坐標(biāo)值相差1。

2 A*算法設(shè)計

2.1 交通約束法

交通約束即對AGV的行進(jìn)過程添加交通規(guī)則的約束，使得AGV在某一通道上只能沿特定方向行進(jìn)。

AGV在正常行進(jìn)的過程中，常見的會遇態(tài)勢包括對遇、交叉等，具體如圖2所示。

(a)AGV對遇 (b)AGV交叉圖2 常見的AGV會遇態(tài)勢

上述的兩種情況，都會對AGV的正常行進(jìn)產(chǎn)生影響：當(dāng)兩AGV在同一通道對遇時，由于各自下一步指令均為前進(jìn)，故陷入了死鎖的現(xiàn)象；在兩AGV出現(xiàn)交叉的局面時，因兩者下一節(jié)點相同、會出現(xiàn)碰撞的情況，因此也停留在原地不動。

在引入交通約束法后，同一通道上的AGV只能沿著一個方向行進(jìn)，此時AGV交叉會遇的現(xiàn)象將不復(fù)存在；而對于交叉的情況，AGV會根據(jù)自身所賦予的優(yōu)先級大小進(jìn)行下一步動作：停車避讓或者繼續(xù)前行。

相比于時間窗法，本文所采用的交通約束法的優(yōu)勢在于不用預(yù)測每臺AGV到達(dá)每個節(jié)點的時間，無需考慮AGV在工作時受到其他因素干擾而發(fā)生速度變化的問題[13]，同時減少了會遇的類型，有效避免了AGV在通道內(nèi)的碰撞問題，減少了在工作環(huán)境中的碰撞幾率，具有更強穩(wěn)定性。

2.2 傳統(tǒng)A*算法

A*算法是一種典型的啟發(fā)式搜索算法，傳統(tǒng)的A*算法主要從垂直和水平方向進(jìn)行搜索：從起始點開始，沿著垂直和水平方向進(jìn)行搜索點的擴展，通過啟發(fā)函數(shù)f(n)估計擴展點的代價值，同時將擴展點存入open列表中，再將open列表中選取代價值最小的節(jié)點作為下一次搜索的起點,并將該點從open列表中移除，重復(fù)上述步驟直至找到目標(biāo)點。

由上述的A*算法的工作原理可知，其成功與否主要取決于啟發(fā)函數(shù)f(n)，啟發(fā)函數(shù)的表達(dá)式[14]為：

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

(1)

式中，g(n)為起始點到當(dāng)前點n的實際代價，h(n)為當(dāng)前點n到目標(biāo)點的估計代價，本文采用曼哈頓距離進(jìn)行兩點間的代價預(yù)估：

D=|xn-xg|+|yn-yg|

(2)

式中，xnyn分別為當(dāng)前點n的橫坐標(biāo)和縱坐標(biāo)，xgyg分別為目標(biāo)點g的橫坐標(biāo)和縱坐標(biāo)。

當(dāng)前進(jìn)方向為障礙物或為地圖的邊緣時，此時該方向的代價為無窮大。

由于傳統(tǒng)A*算法所采用的啟發(fā)函數(shù)主要進(jìn)行距離的評判，因此當(dāng)出現(xiàn)多個代價相同的節(jié)點時，算法將隨機選擇一個節(jié)點作為下一次搜索的起點，這導(dǎo)致了所規(guī)劃的路徑雖長度最短，但拐角數(shù)并非最優(yōu)的問題。

2.3 基于多元啟發(fā)函數(shù)的A*算法

針對上述的問題，本文在原有A*算法的基礎(chǔ)上，添加了拐角數(shù)的評判指標(biāo)，構(gòu)成多元啟發(fā)函數(shù)。當(dāng)算法中出現(xiàn)多個代價相同的候選節(jié)點時，觸發(fā)拐角數(shù)評判機制，對行進(jìn)至候選節(jié)點的路徑上的拐角數(shù)進(jìn)行比較。改進(jìn)后的啟發(fā)函數(shù)表達(dá)式如下：

f(n)=g(n)+h(n)+a×N

(3)

(4)

(5)

在改進(jìn)后的啟發(fā)函數(shù)表達(dá)式中，a為拐角數(shù)評判機制的觸發(fā)函數(shù)，N為拐角數(shù)，m表示在原始啟發(fā)函數(shù)中除n點外的最小代價點。

當(dāng)存在兩個及以上的最小代價點(即g(n)+h(n)=g(m)+h(m))時，式(5)中b=1，根據(jù)式(4)可得a=1，此時觸發(fā)了啟發(fā)函數(shù)中的拐角數(shù)評判機制，滿足代價最小的點進(jìn)行式(3)的比較，最終選擇f值最小的點作為算法路徑選擇的下一個節(jié)點；當(dāng)僅存在一個最小代價點時，b≠1，此時a=0，拐角數(shù)評判機制未能觸發(fā)，故該點默認(rèn)為算法路徑選擇的下一個節(jié)點。

拐角數(shù)的統(tǒng)計步驟具體如下：獲取當(dāng)前節(jié)點A的坐標(biāo)(A1,A2)，節(jié)點A的父節(jié)點B的坐標(biāo)(B1,B2)，以及節(jié)點B的父節(jié)點C的坐標(biāo)(C1,C2)，當(dāng)A1-B1≠B1-C1且A2-B2≠B2-C2，則說明路徑出現(xiàn)了拐角，此時節(jié)點A的拐角數(shù)=節(jié)點B的拐角數(shù)+1。

改進(jìn)后的A*算法具體步驟如下[14]：

(1)設(shè)置一個open列表和close列表，并將起始節(jié)點放入open列表中；

(2)遍歷搜索open列表中存在的節(jié)點，根據(jù)啟發(fā)函數(shù)確定代價f(n)大小，將f(n)值最小的節(jié)點n作為算法路徑選擇的下一節(jié)點，將該節(jié)點從open列表中刪除并添加到close列表；若存在多個f(n)值最小的節(jié)點，則觸發(fā)拐角數(shù)評判機制，選擇拐角數(shù)最少的點作為下一個節(jié)點；

(3)判斷節(jié)點n是否為目標(biāo)節(jié)點，若為目標(biāo)節(jié)點，則結(jié)束算法；否則，則進(jìn)行下一步；

(4)搜索節(jié)點n鄰域內(nèi)所有符合搜索要求的子節(jié)點m，計算所有待搜索節(jié)點的f(m)值，選擇f(m)值最小的節(jié)點m作為算法路徑選擇的下一個節(jié)點；

(5)若節(jié)點m不在open列表和close列表中，則將其添加到open列表中，并為子節(jié)點m添加一個指針指向其父節(jié)點n；若節(jié)點m在open列表中，則對比之前存儲在open列表中的函數(shù)值，保留函數(shù)值小的數(shù)值；若節(jié)點在close列表中，則該節(jié)點已在當(dāng)前最優(yōu)路徑上，返回上一步繼續(xù)對比擴展其他子節(jié)點；

(6)循環(huán)步驟 (2)～步驟 (6)，但算法尋找到目標(biāo)節(jié)點或open列表為空時，結(jié)束算法。

當(dāng)算法尋到目標(biāo)節(jié)點時，以目標(biāo)節(jié)點為起點，沿著每個方格的父節(jié)點移動至起點，即可獲取最優(yōu)路徑。算法流程圖3所示。

圖3 基于多元啟發(fā)函數(shù)的A*算法流程

2.4 基于交通約束及多元啟發(fā)函數(shù)的A*算法

在多AGV系統(tǒng)中，為了使AGV在自主規(guī)劃路徑的前提下，減少AGV間的路徑重疊程度，降低AGV間的避碰幾率，提高系統(tǒng)的工作效率，本文將交通約束法與改進(jìn)后的基于多元啟發(fā)函數(shù)A*算法相結(jié)合，實現(xiàn)AGV的路徑規(guī)劃。

多AGV系統(tǒng)的運行流程圖如圖4所示，單臺AGV在獲取地圖信息后，根據(jù)地圖上各點的交通約束條件，調(diào)用基于多元啟發(fā)函數(shù)的A*算法進(jìn)行路徑規(guī)劃；當(dāng)AGV的制定的路徑遇到其他AGV時，根據(jù)兩者的優(yōu)先級進(jìn)行避讓行為。

圖4 多AGV系統(tǒng)運行流程圖

3 仿真與分析

為了驗證改進(jìn)后的基于交通約束及多元啟發(fā)函數(shù)的A*算法的效果，本文在MATLAB軟件上進(jìn)行算法仿真，其中起始點和目標(biāo)點的位置是隨機生成的。柵格的大小為12×10，代表AGV路徑規(guī)劃區(qū)域的大小，黑色的方格代表障礙物的位置。

本文通過實驗對傳統(tǒng)的A*算法(記為算法1)、基于多元啟發(fā)函數(shù)A*算法(記為算法2)和基于交通約束及多元啟發(fā)函數(shù)的A*算法(記為算法3)所規(guī)劃的路徑進(jìn)行對比、分析。

實驗1：單臺AGV的路徑規(guī)劃。起點為(2，11)，目標(biāo)點為(9，2)，具體的仿真結(jié)果如圖5所示，圖中圓點為起點，方點為目標(biāo)點。

(a) 算法1 (b) 算法2(c) 算法3

為了更好地驗證改進(jìn)A*算法的優(yōu)越性，本文將實驗1中3種算法在路徑長度和拐角數(shù)量兩項主要指標(biāo)進(jìn)行了比較，如表1所示。

表1 三種算法在實驗1中的結(jié)果比較

通過對仿真圖的觀察以及對路徑長度、拐角數(shù)量這兩項指標(biāo)的對比結(jié)果的分析可知，算法1雖能夠規(guī)劃出長度最短的路徑，但是該路徑拐角數(shù)多，路徑復(fù)雜程度較高；而算法2和算法3可在規(guī)劃出長度最短的路徑的同時，盡可能地減少拐角數(shù)，以降低路徑的復(fù)雜程度。

由于在實驗1中，無法有效地分辨出算法2和算法3的優(yōu)劣勢，因此，本文再次進(jìn)行實驗，對算法2和算法3進(jìn)行對比分析。

實驗2：多臺AGV的路徑規(guī)劃。1號AGV的起點為(9，2)、目標(biāo)點為(2，11)，2號AGV的起點為(2，11)、目標(biāo)點為(9，2)，具體的仿真結(jié)果如圖6所示，其中圖6a、圖6b分別為算法2和算法3所規(guī)劃的路徑，圖中圓點為起點，方點為目標(biāo)點，1號AGV的路徑為黑色，2號AGV的路徑為灰色。

(a)算法2(b)算法3 圖6 不同算法在實驗2的規(guī)劃路線

通過對仿真圖的觀察可知，算法2為兩臺AGV規(guī)劃的路徑存在大量相同的節(jié)點，此時兩臺AGV在行進(jìn)的過程會產(chǎn)生對遇的局面，進(jìn)而出現(xiàn)死鎖現(xiàn)象；而算法3在交通規(guī)則的約束下，為兩臺AGV規(guī)劃了無相同的節(jié)點的路徑，使得兩臺AGV無避碰、高效地達(dá)到目標(biāo)點。

4 結(jié)論

在移動機器人得到廣泛應(yīng)用的時代，路徑規(guī)劃是一個重要的研究領(lǐng)域。本文針對多AGV同時運行容易出現(xiàn)碰撞的情況以及傳統(tǒng)A*算法所規(guī)劃的路徑存在拐角數(shù)多的問題，在傳統(tǒng)A*算法的基礎(chǔ)上引入了交通約束條件，并在原有啟發(fā)函數(shù)的基礎(chǔ)上，添加了拐角數(shù)評判機制，構(gòu)成多元啟發(fā)函數(shù)。采用柵格法進(jìn)行環(huán)境建模，并在MATLAB實驗環(huán)境下進(jìn)行兩組的實驗，實驗結(jié)果表明，在多AGV的環(huán)境下，相比于傳統(tǒng)的A*算法，基于交通約束及多元啟發(fā)函數(shù)的A*算法所規(guī)劃路徑合理，拐角數(shù)量優(yōu)于傳統(tǒng)A*算法，且能有效地降低AGV間地碰撞幾率，降低AGV完成路徑的能耗，增強系統(tǒng)的穩(wěn)定性，具有一定的應(yīng)用價值。