改進A*算法的移動機器人全局路徑規(guī)劃研究

支琛博，張愛軍，杜新陽，彭 鵬

(南京理工大學(xué)機械工程學(xué)院，江蘇 南京 210094)

1 引言

路徑規(guī)劃對移動機器人的導(dǎo)航起到至關(guān)重要的作用，已經(jīng)成為當下控制領(lǐng)域研究熱點，其主要是讓目標對象在規(guī)定范圍內(nèi)的區(qū)域內(nèi)找到一條從起點到終點的無碰撞安全路徑[1]。

目前存在多種路徑規(guī)劃算法，如人工勢場法、蟻群算法、粒子群算法以及Dijkstra算法[2]等，其中A*算法是一種啟發(fā)式搜索算法，通過評估節(jié)點的代價值搜索目標點，其優(yōu)點是在實際運用過程中具有良好的魯棒性，且速度相對其他算法較快，運用較多。

針對傳統(tǒng)A*算法在實時性不高、規(guī)劃路徑的安全性得不到保障的情況下，國內(nèi)外學(xué)者從不同層面對其進行了改進與提升。文獻[6]提出了一種以雙向搜索機制為核心的改進A*算法，其通過把初始點與目標點同時以機器人當前所在點為目標進行搜索，縮短了搜索路徑的時間，但是路徑的安全性不能有保障；文獻[7]將傳統(tǒng)搜索擴展節(jié)點方法由搜索3*3領(lǐng)域改為搜索7*7領(lǐng)域，雖然在安全方面減少了運動軌跡折線，使得運動軌跡更平滑，但是不能保證機器人的路徑軌跡遠離障礙物；文獻[8]通過引入碰撞威脅因子的方法，在啟發(fā)式函數(shù)上加入與障礙物的距離和安全距離函數(shù)，使得安全性有了保障，但是在實時性方面并沒有的得到較大的改善；文獻[9]通過增設(shè)視覺傳感器等外設(shè)，實現(xiàn)障礙物的識別與定位，但是在實際應(yīng)用中往往會提高成本，不利于推廣。

以上算法都在不同程度上改善了A*算法的性能，使得A*算法得到了較快的發(fā)展，但是始終沒有一種算法可以較好程度上兼顧實時性與安全性。本文算法在選取擴展節(jié)點上融合JPS算法選取跳點的思想，將符合跳點要求的點作為A*算法的擴展節(jié)點，從而可以快速搜索節(jié)點，提高算法實時性；在融合JPS算法的基礎(chǔ)上，引入安全權(quán)重方陣思想，選取遠離障礙物的跳點，從而在提升實時性的基礎(chǔ)上提升安全性。本文最后通過一系列仿真，驗證算法的可實施性。

2 改進A*算法

傳統(tǒng)A*算法在移動機器人尋找路徑時，通過尋找當前節(jié)點附近成本代價值最小的節(jié)點來實現(xiàn)最優(yōu)路徑搜索。其在大面積地圖環(huán)境下，隨著路徑長度的增加，搜索時效性低；在擁擠的地圖環(huán)境下，規(guī)劃路徑不符合機器人運動學(xué)原理，容易造成誤碰撞。針對傳統(tǒng)A*算法的局限性，本文作出如下改進：

1)優(yōu)化選取節(jié)點過程，提高實時性；

2)結(jié)合安全權(quán)重方陣，提高安全性。

2.1 A*算法實時性改進

JPS算法[10]也是一種啟發(fā)式算法，其在移動機器人的路徑規(guī)劃過程中，通過當前節(jié)與其父節(jié)點，確定路徑方向并省去過程中無意義的節(jié)點。相較于A*算法，若檢測到有意義的節(jié)點，會忽略探路過程中所通過的各個步驟，并僅將這個有意義的節(jié)點加入Open表中，從而對路徑節(jié)點進行有針對性的選取，減少有效提升路徑搜索的速度。

2.1.1 剪枝規(guī)則

在路徑搜索過程中，通過只擴展網(wǎng)格地圖上的某些節(jié)點(跳點)來加快最優(yōu)搜索，合理選取跳點需要剪枝(省去無意義節(jié)點)，其規(guī)則如下：

1)節(jié)點附近不存在障礙物

若路徑搜索方向為斜對角，則記為d1，若方向為直線，則記為d2。若從節(jié)點x按照一定方向移動可以到達節(jié)點y，則記作下式：

y=x+k1d1+k2d2

(1)

對于當前節(jié)點的任意鄰節(jié)點，若滿足以下約束條件，則進行剪枝

(2)

其中l(wèi)en表示路徑搜索長度，p(x)表示當前節(jié)點的父節(jié)點，n表示當前節(jié)點的鄰節(jié)點，a表示節(jié)點b不會出現(xiàn)在路徑a上。

圖1(a)顯示了一個示例，父節(jié)點為p(x)=1，除n=8外所有x的鄰居都被修剪掉；圖1(b)顯示了一個示例，父節(jié)點為p(x)=1，除n=6，n=7和n=8外所有x的鄰居都被修剪掉。

圖1 當前節(jié)點附近沒有障礙物示意圖

2)節(jié)點附近存在障礙物

節(jié)點n若存在以下情況下則不能被剪枝：

①n不是當前節(jié)點的自然鄰居

②

(3)

圖2(a)顯示了一個示例，父節(jié)點為p(x)=1，n=3是障礙物節(jié)點，這里n=4的節(jié)點不能剪枝；圖2當前節(jié)點附近有障礙物示意圖(b)顯示了一個示例，n=2是障礙物節(jié)點，這里n=3的節(jié)點不能剪枝。

圖2 當前節(jié)點附近有障礙物示意圖

2.1.2 篩選跳點

設(shè)初始點為x，并將其添加到Open表中，沿著x的水平和垂直方向進行路徑搜索，若遇到不可到達區(qū)域，則沿x的對角線移動一個單位距離，對新位置的水平和垂直方向搜索，往復(fù)循環(huán)此過程，若遇到強迫鄰節(jié)點，將其作為跳點加入Open表，并利用剪枝規(guī)則刪除過程中的節(jié)點。

2.1.3 算法流程

本文將JPS取點策略應(yīng)用在A*算法中，根據(jù)剪枝規(guī)則篩選出跳點，并將其放入Open表中，從而減少了Open表節(jié)點數(shù)量，提升算法速度。其大體思路如下：

1)將起始點(第一個跳點)加入Open表中；

2)在算法執(zhí)行前增加一個預(yù)處理過程，所謂預(yù)處理就是通過尋找當前節(jié)點的繼承跳點，修剪掉中間無用節(jié)點，連接跳點實現(xiàn)兩點間的長距離跳躍，并把前一個跳點移除Open表并加入至Close表中；

3)重復(fù)執(zhí)行步驟二，直到到達目標點；

4)對Close表按照時間先后順序進行路徑連線，輸出最優(yōu)路徑。

相比于傳統(tǒng)A*算法，利用JPS策略改進后的A*算法可以通過連接跳點，實現(xiàn)較長距離的“跳躍”。在尋路的過程中只需要花費很短的時間進行預(yù)處理，就可以減少大量不必要的節(jié)點，極大地提高了尋路的效率。

本文在地圖構(gòu)建方面采用柵格法，相比于拓撲地圖法等其它地圖構(gòu)建方法，柵格法可以有效地提取環(huán)境信息，并且在仿真編碼中易于實現(xiàn)，傳統(tǒng)A*算法與融合JPS的改進A*算法效果如圖3與圖4所示。

圖3 傳統(tǒng)A*算法效果圖

圖4 融合JPS的改進A*算法效果圖

2.2 A*算法安全性改進

在傳統(tǒng)A*算法中，通常存在圖5所示的路徑規(guī)劃情況，然而其忽略了實際應(yīng)用中機器人的體積，易造成誤碰撞。本文通過引入安全權(quán)重方陣思想，使得移動機器人可以有效的與障礙物保持安全距離，從而提升算法的安全性。

本文借助多鄰域搜索思想，針對不同安全距離的要求，設(shè)定相應(yīng)的鄰域矩陣完成對障礙物的檢測。因為本文地圖環(huán)境類型為柵格地圖，所以設(shè)與障礙物的最小安全距離為一個柵格的距離，相對應(yīng)的鄰域矩陣大小為3*3，也稱其為基礎(chǔ)比較方陣。以基礎(chǔ)比較方陣為例，將方陣中心元素設(shè)置為當前路徑節(jié)點，其值為0，其他位置值設(shè)定為1，如式所示。

(4)

在路徑搜索的過程，若矩陣范圍內(nèi)檢測到障礙物，則把相應(yīng)位置的元素值由1改為2，生成安全權(quán)重方陣，表示此處有障礙物存在。例如對于圖5中的路徑節(jié)點X，其安全權(quán)重方陣XB為

(5)

設(shè)置d為安全系數(shù)，若d越大，路徑安全程度越高。若方陣中有2，則設(shè)d為5(實際情況可自行選定)，表示當前節(jié)點附近存在障礙物，否則為0，具體表達如下

(6)

得到改進A*算法的函數(shù)代價式如式所示，其中m為統(tǒng)計基礎(chǔ)比較方陣中2的個數(shù)，也稱障礙物影響因子，其值越大表示當前節(jié)點附近的障礙物越多。如圖6所示，結(jié)合以上算法，因為原路徑節(jié)點代價值增大，所以重新選取路徑節(jié)點，使得路徑遠離障礙物。

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

(7)

圖5 傳統(tǒng)A*算法安全性改進前效果圖

圖6 傳統(tǒng)A*算法安全性改進后效果圖

若要設(shè)定其他安全距離，可以在基礎(chǔ)比較方陣的基礎(chǔ)上進行矩陣變換，通過其與變換矩陣A做式(9)的運算，從而得到中心元素值為0、其余元素值為1的k*k(k為奇數(shù))比較矩陣C。

(8)

(9)

按照式將比較方陣C生成安全權(quán)重方陣，完成重新選取路徑節(jié)點。最終的流程圖如圖7所示。

圖7 A*算法安全性改進流程圖

2.3 混合A*算法改進

為有效解決傳統(tǒng)A*算法在移動機器人尋找路徑時，其在大面積地圖環(huán)境下，隨著路徑長度的增加，搜索時效性低，在擁擠的地圖環(huán)境下，規(guī)劃路徑不符合機器人運動學(xué)原理，容易造成誤碰撞問題，本文將A*算法與JPS算法結(jié)合，采用選取跳點策略選取路徑節(jié)點，同時結(jié)合安全權(quán)重方陣思想，使得移動機器人可以有效的與障礙物保持安全距離，從而提升算法的安全性，構(gòu)建融合JPS與安全權(quán)重方陣的改進A*算法。

如圖8所示為改進算法流程圖，其具體步驟如下：

1)創(chuàng)建柵格地圖并初始化參數(shù)，選擇初始位置S(xStart，yStart)，終點位置G(xGoal，yGoal)，創(chuàng)建節(jié)點列表Open與Close表，將初始位置加入Open表中；

2)在算法執(zhí)行前增加一個預(yù)處理過程，所謂預(yù)處理就是通過尋找當前節(jié)點的繼承跳點，修剪掉中間無用跳點，連接跳點實現(xiàn)兩點間的長距離跳躍，并把前一個跳點移除Open表中，令其加入到Close表中，記錄此刻跳點的路徑代價值f(n)；

3)結(jié)合安全權(quán)重方陣算法，判斷此時路徑節(jié)點是否緊挨障礙物，若緊挨障礙物，則更新f(n)值，重新選取相應(yīng)跳點，直到達到安全距離要求，將新節(jié)點加入Open表中，并把前一個跳點移除Open表中，令其加入到Close表中；

4)重復(fù)執(zhí)行步驟二與步驟三，直到達到目標點；

5)對Close表按照時間先后順序進行路徑連線，輸出最優(yōu)路徑，可得到可得出本文全局路徑規(guī)劃的最優(yōu)路徑。

圖8 改進算法流程圖

3 改進 A* 算法仿真

為了驗證本文算法在移動機器人在實際路徑規(guī)劃過程的有效性，本文采用仿真形式，在多組不同柵格地圖環(huán)境下進行三種算法的仿真，并將結(jié)果進行比對。仿真環(huán)境的計算機參數(shù)為：系統(tǒng)Windows 10，CPUIntel Core i5，內(nèi)存8GB，編譯環(huán)境MATLAB 2016b。

在實驗過程中，本文分別將傳統(tǒng)A*算法(算法1)、融合JPS的改進A*算法(算法2)、融合JPS與安全權(quán)重方陣的改進A*算法(算法3)三種方法做仿真，其中對于算法3設(shè)定安全距離為一個柵格的距離，從評估節(jié)點數(shù)量、路徑搜索時間、路徑長度三個因素來評估算法的有效性，同時在每組對比試驗中設(shè)定起始點、目標點以及障礙物信息均相同，以此來達到控制變量對比算法效果。

圖9、圖10與圖11分別為算法1、算法2、算法3在地圖大小為20*20的柵格地圖上的算法效果圖，從圖中可以明顯看出，算法3無論是安全性還是拐點數(shù)量相比去前兩者都有一定的效果。

圖9 傳統(tǒng)A*算法效果圖 圖10 融合JPS的改A*算法效果圖

圖11 融合JPS與安全權(quán)重方陣的改進A*算法效果圖

如圖12所示，為了驗證算法的普適性，本文分別在10*10、50*50以及100*100大小的柵格地圖上仿真本文算法，并將其與前兩種進行效果比對。

圖12 不同大小柵格地圖改進算法前后的定型效果對比

表1傳統(tǒng)A*算法與本文改進算法的對比是本文算法在改進前后算法效果對比表，分別通過評估節(jié)點數(shù)量、路徑搜索時間、路徑長度三個因素來評估算法的有效性。

表1 傳統(tǒng)A*算法與本文改進算法的對比

將表格中的搜索時間與路徑長度作成線條圖形式，如圖13所示，隨著地圖空間的增加，本文算法相對于傳統(tǒng)A*算法在搜索時間方面有很大的提升，耗時長短與融合JPS的改進A*算法相當；如圖14所示，相比于傳統(tǒng)A*算法與融合JPS的改進A*算法相比，本文算法的路徑長度平均增加了11.3%，這主要體現(xiàn)在為了提高安全性需要增加其他節(jié)點，從而可以避開障礙物。此外在評估節(jié)點數(shù)量方面，本文算法體現(xiàn)出搜索方向的引導(dǎo)效果好，減少了需要比較計算的節(jié)點數(shù)。

圖13 搜索時間對比圖

圖14 路徑長度對比圖

4 結(jié)束語

本文針對傳統(tǒng)A*算法在移動機器人路徑規(guī)劃的過程中存在的一些問題進行了針對性的改進：首先為有效解決傳統(tǒng)A*算法在移動機器人尋找路徑時，其在大面積地圖環(huán)境下，隨著路徑長度的增加，搜索時效性低，本文結(jié)合了JPS算法的選取跳點思想，對路徑節(jié)點進行有針對性的選??；其次為了解決在擁擠的地圖環(huán)境下，規(guī)劃路徑不符合機器人運動學(xué)原理，容易造成誤碰撞問題，本文將傳統(tǒng)A*算法結(jié)合安全權(quán)重方陣，使得移動機器人可以有效的與障礙物保持安全距離，從而提升算法的安全性。最終融合以上兩種方法構(gòu)建融合JPS與安全權(quán)重方陣的改進A*算法，使得移動機器人可以在復(fù)雜環(huán)境下提高實時性，同時也可以增加路徑軌跡的安全性。下一步工作將會考慮如何應(yīng)對動態(tài)環(huán)境中的路徑規(guī)劃，以此來提高算法的魯棒性。