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

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

1 引言

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

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

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

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

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

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

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

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

2.1 A*算法實(shí)時性改進(jìn)

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

2.1.1 剪枝規(guī)則

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

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

若路徑搜索方向?yàn)樾睂?，則記為d1，若方向?yàn)橹本€，則記為d2。若從節(jié)點(diǎn)x按照一定方向移動可以到達(dá)節(jié)點(diǎn)y，則記作下式：

y=x+k1d1+k2d2

(1)

對于當(dāng)前節(jié)點(diǎn)的任意鄰節(jié)點(diǎn)，若滿足以下約束條件，則進(jìn)行剪枝

(2)

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

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

圖1 當(dāng)前節(jié)點(diǎn)附近沒有障礙物示意圖

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

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

①n不是當(dāng)前節(jié)點(diǎn)的自然鄰居

②

(3)

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

圖2 當(dāng)前節(jié)點(diǎn)附近有障礙物示意圖

2.1.2 篩選跳點(diǎn)

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

2.1.3 算法流程

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

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

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

3)重復(fù)執(zhí)行步驟二，直到到達(dá)目標(biāo)點(diǎn)；

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

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

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

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

圖4 融合JPS的改進(jìn)A*算法效果圖

2.2 A*算法安全性改進(jìn)

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

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

(4)

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

(5)

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

(6)

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

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

(7)

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

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

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

(8)

(9)

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

圖7 A*算法安全性改進(jìn)流程圖

2.3 混合A*算法改進(jìn)

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

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

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

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

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

4)重復(fù)執(zhí)行步驟二與步驟三，直到達(dá)到目標(biāo)點(diǎn)；

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

圖8 改進(jìn)算法流程圖

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

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

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

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

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

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

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

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

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

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

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

圖13 搜索時間對比圖

圖14 路徑長度對比圖

4 結(jié)束語

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