基于改進(jìn)A*算法的移動(dòng)機(jī)器人路徑規(guī)劃

陸皖麟，雷景森，邵 炎

(66133部隊(duì)， 北京 100043)

路徑規(guī)劃技術(shù)是移動(dòng)機(jī)器人研究的熱點(diǎn)問題，對它的研究為控制機(jī)器人自主行駛奠定基礎(chǔ)[1]。路徑規(guī)劃的定義可以表示為：設(shè)定起始點(diǎn)、目標(biāo)點(diǎn)后，依據(jù)規(guī)劃算法搜尋從起點(diǎn)至目標(biāo)點(diǎn)同時(shí)可以避障的一條最佳路線。依照機(jī)器人的環(huán)境情況將運(yùn)動(dòng)路徑列為兩類：一類是基于地圖信息已知的全局路徑規(guī)劃，此類路徑規(guī)劃環(huán)境中的信息包括障礙物和可行駛區(qū)域全部已知；另外一類是根據(jù)相機(jī)、慣性導(dǎo)航、里程計(jì)等獲取實(shí)時(shí)信息的局部路徑規(guī)劃，局部路徑規(guī)劃是處在障礙物信息未知的環(huán)境[2]。常用的全局路徑規(guī)劃算法有[3]：A*算法、Dijkstra算法、粒子群算法、遺傳算法等。A*算法最早由Nilsson提出來的啟發(fā)式算法，它的核心是對目標(biāo)點(diǎn)進(jìn)行不斷搜尋，從而取得機(jī)器人的避障路徑。通過對狀態(tài)空間中搜索點(diǎn)進(jìn)行評價(jià)，取得最佳節(jié)點(diǎn)，然后依據(jù)此位置節(jié)點(diǎn)繼續(xù)進(jìn)行搜尋，一直到找到目標(biāo)點(diǎn)為止。該算法具有編程方法相對簡單，參數(shù)設(shè)置較少，搜索路徑效率高[4]的特點(diǎn)。

1 A*算法原理

A*算法是獲取最短路徑比較有效的搜索方法，也是典型的啟發(fā)式算法。A*算法一般用于靜態(tài)全局規(guī)劃，其核心表達(dá)式為

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

(1)

其中：f(n)表示從起點(diǎn)開始經(jīng)過當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的路徑長度；g(n)表示起點(diǎn)至當(dāng)前節(jié)點(diǎn)n的路徑長度，此時(shí)g(n)已經(jīng)是機(jī)器人從起始點(diǎn)到當(dāng)前點(diǎn)的最短距離；h(n)是從狀態(tài)節(jié)點(diǎn)n到目標(biāo)狀態(tài)的估計(jì)代價(jià)函數(shù)距離。由于g(n)是實(shí)際距離，為了在靜態(tài)地圖可以搜尋到最優(yōu)解，選擇代價(jià)h(n)類型非常重要，f(n)的值取決于h(n)的大小。下面描述算法搜索節(jié)點(diǎn)過程，如圖1所示，綠色小格表示起始點(diǎn)位置，紅色小格表示目標(biāo)點(diǎn)位置，藍(lán)色小格周圍的8個(gè)點(diǎn)表示可以設(shè)定為當(dāng)前狀態(tài)空間的子節(jié)點(diǎn)候選點(diǎn)，設(shè)小格的一條邊長度為10，式(1)簡寫為F=G+H，G表示起始點(diǎn)至當(dāng)前狀態(tài)的曼哈頓距離，H是當(dāng)前狀態(tài)節(jié)點(diǎn)至目標(biāo)節(jié)點(diǎn)估計(jì)代價(jià)函數(shù)值，這里代價(jià)函數(shù)H的計(jì)算使用的是歐式距離，起始點(diǎn)周圍8個(gè)格子F、G、H各數(shù)值見圖，由圖可知2號小格F最小，將其作為接下來要處理的點(diǎn)，然后檢查2號周圍的小格的G、H值，黑色方格是障礙物，在計(jì)算G、H值時(shí)忽略其不可通行，確定2號的待選小格是1號、3號。由于1號、3號是父節(jié)點(diǎn)(起始點(diǎn))的8個(gè)相鄰節(jié)點(diǎn)、3號節(jié)點(diǎn)周圍沒有可選新的相鄰節(jié)點(diǎn)，故從起始點(diǎn)選擇待處理節(jié)點(diǎn)是1號而不是2號、3號。按照此思路，4號節(jié)點(diǎn)是1號節(jié)點(diǎn)的子節(jié)點(diǎn)。

圖1 A*算法搜索節(jié)點(diǎn)

在已構(gòu)建的地圖中，設(shè)起始點(diǎn)坐標(biāo)節(jié)點(diǎn)為S(Sx,Sy),當(dāng)前節(jié)點(diǎn)C(Cx,Cy)，目標(biāo)節(jié)點(diǎn)的坐標(biāo)T(Tx,Ty)，若使用相對簡單的歐式距離估價(jià)函數(shù)值可表示為

(2)

2 A*算法流程

下面將以偽代碼的形式描述A*算法流程，首先設(shè)置各項(xiàng)參數(shù)，start：機(jī)器人路徑規(guī)劃的出發(fā)點(diǎn)；goal：機(jī)器人路徑規(guī)劃的目標(biāo)點(diǎn)位置；g(n)：n節(jié)點(diǎn)到start點(diǎn)的實(shí)際移動(dòng)距離；h(n)：n節(jié)點(diǎn)到達(dá)目標(biāo)點(diǎn)的理論移動(dòng)距離；openlist：搜尋節(jié)點(diǎn)的過程中保存未檢測的節(jié)點(diǎn)列表；closelist：已經(jīng)被檢測的節(jié)點(diǎn)保存至該列表；comaFrom：保存子節(jié)點(diǎn)和父節(jié)點(diǎn)，最終生成的路徑在此列表中。使用偽代碼為：

把起點(diǎn)加入到openlist

do

{

搜尋openlist中f(n)值最小的節(jié)點(diǎn)，它表示當(dāng)前節(jié)點(diǎn)

加入至closelist

對當(dāng)前節(jié)點(diǎn)相鄰的所有候選節(jié)點(diǎn)

if(是障礙物點(diǎn)‖在closelist中)

{

繼續(xù)搜尋下一個(gè)相鄰候選節(jié)點(diǎn)

}

if(openlist不存在此節(jié)點(diǎn))

{

將此候選節(jié)點(diǎn)加入openlist，更新父節(jié)點(diǎn)，計(jì)算它的f(n)、g(n)、h(n)值

}

if(候選節(jié)點(diǎn)已經(jīng)在openlist內(nèi))

{

if(使用g(n)值作為評判標(biāo)準(zhǔn)，來檢測新的路徑，判斷是否有更低的g(n))

{

將它的父節(jié)點(diǎn)換為當(dāng)前節(jié)點(diǎn)，計(jì)算當(dāng)前節(jié)點(diǎn)的f(n)、h(n)的值

}}

使用comaFrom保存父節(jié)點(diǎn)

}while(目標(biāo)節(jié)點(diǎn)在openlist里面，表示路徑規(guī)劃成功)

若openlist列表是空值，表示沒有可尋找的路徑。

最后把comaFrom保存的所有父節(jié)點(diǎn)從起點(diǎn)開始以此連接起來，該路線就是A*算法所求的路徑。

傳統(tǒng)A*算法路徑規(guī)劃程序結(jié)構(gòu)如圖2所示。

3 A*算法改進(jìn)

盡管傳統(tǒng)A*算法已經(jīng)成為移動(dòng)機(jī)器人導(dǎo)航中廣泛使用的路徑規(guī)劃算法，但是算法規(guī)劃出來的路徑存在拐點(diǎn)多，轉(zhuǎn)折次數(shù)多的問題，在實(shí)際環(huán)境中不利于機(jī)器人的行駛[5]。因此在研究A*算法的基礎(chǔ)上提出一種適宜機(jī)器人行駛改進(jìn)的A*算法。

圖2 傳統(tǒng)A*算法路徑規(guī)劃程序框圖

為解決A*算法在實(shí)際路徑規(guī)劃中不能求最短路徑，Anthony Stentz提出基于插值方法的D*方法[6]，使路徑變得平滑，但采用插值的方法使計(jì)算量增大，實(shí)時(shí)性變差。遺傳算法、神經(jīng)網(wǎng)絡(luò)算法等智能算法也用于路徑規(guī)劃，但對于靜態(tài)地圖而言智能算法花費(fèi)時(shí)間較多。為優(yōu)化A*算法的規(guī)劃路徑，減少路徑的長度，針對A*算法的3個(gè)方面加以改進(jìn)：

1) 設(shè)定openlist訪問時(shí)間閾值

傳統(tǒng)A*算法在當(dāng)前節(jié)點(diǎn)擴(kuò)展子節(jié)點(diǎn)時(shí)，每次都要把所有候選節(jié)點(diǎn)擴(kuò)展完畢，這種方法在復(fù)雜環(huán)境下可能存在一定的無效搜索，將會(huì)耗費(fèi)一定的時(shí)間，因此，在傳統(tǒng)A*算法的基礎(chǔ)上，每次檢測openlist表中最先插入的節(jié)點(diǎn)在經(jīng)過一定時(shí)間閾值N后，判斷是否得到擴(kuò)展，若未擴(kuò)展，則將此節(jié)點(diǎn)作為最高級優(yōu)先擴(kuò)展，在一定程度可以避免陷入局部最小值，解決無法規(guī)劃出全局路徑問題。

2) Floyd算法優(yōu)化

Floyd算法借鑒動(dòng)態(tài)規(guī)劃的思路搜尋最短路徑，是用來解決兩點(diǎn)之間的最優(yōu)距離問題的算法[7]。使用Floyd算法和A*算法結(jié)合的方法以減少路徑長度，符合實(shí)際應(yīng)用需求。Floyd算法原理如圖3所示。

圖3 Floyd算法原理

L(A,D)為A、D兩點(diǎn)間的距離，如圖3，A、D存在障礙物，可設(shè)L(A,D)=+∞，R(A,D)=A->D

B點(diǎn)是A點(diǎn)和D之間已規(guī)劃出的節(jié)點(diǎn)：

若

L(A,B)+L(B,D)

(3)

則

L(A,D)=L(A,B)+L(B,D)

(4)

R(A,D)=A->B->D

(5)

在B、D線段上插入點(diǎn)C：

若

L(A,C)+L(C,D)

(6)

則

L(A,D)=L(A,C)+L(C,D)

(7)

R(A,D)=A->C->D

(8)

去除C點(diǎn)，A到D的優(yōu)化路徑表示為一段優(yōu)化后的弧形路徑R(A,D)。采取Floyd算法優(yōu)化A*算法規(guī)劃的路徑能夠刪除多余的點(diǎn)，更加適合移動(dòng)機(jī)器人行駛。

3) 路徑光滑處理

A*算法規(guī)劃的路徑存在許多尖銳的拐點(diǎn)和折點(diǎn)，這種拐點(diǎn)不符合移動(dòng)機(jī)器人的平穩(wěn)運(yùn)動(dòng)。為了滿足移動(dòng)機(jī)器人在規(guī)劃的路徑上能夠平穩(wěn)移動(dòng)，需要對規(guī)劃的路徑進(jìn)行平滑處理[8]，用平滑的曲線來代替尖銳點(diǎn)。其原理框圖如圖4。

圖4 平滑處理原理框圖

(9)

過直線BC方程為

(10)

由圖可知

(11)

設(shè)圓弧半徑

|p1r|=|p2r|=R

(12)

由式可得

|p1B|=|p2B|=R·cot(δ/2)

(13)

設(shè)

(14)

由A，B點(diǎn)坐標(biāo)和式得p1和p2點(diǎn)坐標(biāo):

p1·x=ε1·x1+(1-ε1)·x2

p1·y=ε1·y1+(1-ε1)·y2

p2·x=(1-ε2)·x2+ε2·x3

p2·y=(1-ε2)·y2+ε2·y3

(15)

過p1點(diǎn)和點(diǎn)p2分別做直線AB和BC的垂線，其交點(diǎn)為。則圓弧方程為：

(x-ry)2+(y-ry)2=R2

(16)

那么軌跡A->P1->P2->C的方程為:

(17)

通過以上方法可以在拐點(diǎn)、折點(diǎn)處，采取Floyd算法去除多余的點(diǎn)，縮短機(jī)器人行駛路徑，然后再平滑算法處理路徑，從而減少拐點(diǎn)的彎折程度，有利于機(jī)器人轉(zhuǎn)向行駛以及避障操作[9]。A*算法改進(jìn)后的程序流程框圖如圖5所示。

圖5 改進(jìn)A*算法路徑規(guī)劃程序流程框圖

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

為驗(yàn)證改進(jìn)后的A*算法效果，仿真使用MATLAB2016b進(jìn)行編程。在MATLAB中采用GUI設(shè)計(jì)A*算法圖形化驗(yàn)證界面，如圖6所示，在guide里面設(shè)置兩個(gè)按鈕，一個(gè)命名為A*算法路徑規(guī)劃，另外一個(gè)命名為生成障礙物，它們會(huì)自動(dòng)生成.m文件，使用GUI可編輯文本設(shè)置起點(diǎn)坐標(biāo)、目標(biāo)點(diǎn)坐標(biāo)，分別在A*算法路徑規(guī)劃和生成障礙物按鈕生成的.m文件導(dǎo)入相關(guān)的代碼，修改對應(yīng)Function函數(shù)的參數(shù)，生成圖7的GUI界面，點(diǎn)擊生成障礙物按鈕，會(huì)出現(xiàn)長和寬為32×32，且周圍有一定障礙物的地圖，地圖上一個(gè)柵格單位長為1。

圖6 仿真界面GUI設(shè)計(jì)

圖7 A*算法驗(yàn)證界面

構(gòu)建仿真環(huán)境地圖，地圖中障礙物隨機(jī)設(shè)置，同時(shí)在GUI編輯文本中隨機(jī)設(shè)置起點(diǎn)坐標(biāo)為(3,3)，目標(biāo)點(diǎn)隨機(jī)設(shè)置為(29,22)。仿真功能就是實(shí)現(xiàn)從起點(diǎn)(3,3)至(29,22)尋找一條最優(yōu)路徑。實(shí)驗(yàn)一采用傳統(tǒng)A*算法進(jìn)行路徑規(guī)劃，路徑仿真結(jié)果見圖8(a)；實(shí)驗(yàn)二僅設(shè)定openlist訪問時(shí)間閾值，進(jìn)行路徑規(guī)劃，路徑仿真結(jié)果見圖8(b)；實(shí)驗(yàn)三在實(shí)驗(yàn)二基礎(chǔ)上增加Floyd和平滑處理算法，路徑仿真結(jié)果見圖8(c)。

圖8 仿真路徑規(guī)劃效果

分析對比實(shí)驗(yàn)結(jié)果可知改進(jìn)后的算法路徑總長度縮短了，拐點(diǎn)和折點(diǎn)數(shù)量有所降低，提高了路徑平滑度，更加適合移動(dòng)機(jī)器人的運(yùn)動(dòng)。統(tǒng)計(jì)實(shí)驗(yàn)數(shù)據(jù)如表1：改進(jìn)后規(guī)劃時(shí)間略有增加，這是因?yàn)樵黾恿薋loyd算法和平滑算法。但規(guī)劃時(shí)間相差不是很大，在可以接受范圍內(nèi)，這是由于對算法做了改進(jìn)，在開啟列表(openlist)增加了一個(gè)閾值N，提高了尋找子節(jié)點(diǎn)的效率。

表1 仿真實(shí)驗(yàn)數(shù)據(jù)

5 結(jié)論

1) 基于靜態(tài)環(huán)境下A*算法，為了提高路徑規(guī)劃的實(shí)時(shí)性并且減少搜索節(jié)點(diǎn)數(shù)量，提出了在A*算法的openlist加入判斷閾值N，若搜索節(jié)點(diǎn)的次數(shù)超過N，判斷最初加入的節(jié)點(diǎn)是否擴(kuò)展，未擴(kuò)展設(shè)它為最高級擴(kuò)展節(jié)點(diǎn)，該方法能夠縮短規(guī)劃時(shí)間。

2) 針對傳統(tǒng)A*算法搜索路徑存在的拐點(diǎn)多、路徑彎折程度大等問題利用Floyd算法進(jìn)行優(yōu)化，然后進(jìn)行路徑平滑處理：用圓弧曲線代替折點(diǎn)使其適合機(jī)器人運(yùn)動(dòng)。

3) 改進(jìn)后的A*算法與傳統(tǒng)A*算法的MATLAB仿真結(jié)果對比分析可知，兩者規(guī)劃時(shí)間差別不大，但改進(jìn)后的路徑更加平滑，路徑長度更短，更利于機(jī)器人的行駛。