基于改進JPS和A*算法的組合路徑規(guī)劃①

金 震 黃衛(wèi)華 李傳奇 何佳樂

(*武漢科技大學機器人與智能系統(tǒng)研究院 武漢430081）

(**武漢科技大學冶金自動化與檢測技術教育部工程研究中心 武漢430081）

(***武漢科技大學信息科學與工程學院 武漢430081）

0 引言

移動機器人的路徑規(guī)劃是指機器人遵循一些優(yōu)化指標(比如時間最短、路程最優(yōu)和能耗最低等),在具有障礙物的環(huán)境中尋找一條從起始狀態(tài)到目標狀態(tài)的無碰撞路徑[1]。在圖搜索類全局路徑搜索算法中,A*算法是一種較為穩(wěn)定的啟發(fā)式路徑算法,已廣泛應用于路徑規(guī)劃領域。然而,A*算法在每步評價過程中,均會查詢open 列表和close 列表中節(jié)點的狀態(tài),導致A*算法搜索效率較低,尤其是當環(huán)境存在對稱路徑時,A*算法耗費代價大,影響路徑規(guī)劃的實時性。2011 年,文獻[2]提出了跳點搜索(jump point search,JPS)算法。在路徑搜索中,JPS 算法刪減大量的對稱性中間節(jié)點和無用節(jié)點,保留了具有代表性的跳點,從而減少不必要的搜索路徑,由此改善A*算法小步移動的缺點,有效提高了路徑搜索效率[3]。

在應用過程中,JPS 算法簡單且高效,但其適用于具有一定規(guī)律、大開放區(qū)域的地圖進行路徑規(guī)劃,也就是通過在矩形區(qū)域內跳過大量對稱路徑,尋找關鍵跳點作為路徑搜索的節(jié)點。為進一步提高JPS算法的性能,學者們提出了改進方法。文獻[4]針對移動機器人在復雜環(huán)境下對智能性、高效性和可靠性的要求,提出了一種“塊”操作方法以加快搜索跳點的速度,并刪減了一部分僅代表路徑方向發(fā)生改變的中間轉折節(jié)點。文獻[5]針對JPS 算法規(guī)劃的路徑可能存在碰撞的問題,將跳點進行危險度評估,改進了跳點的篩選方式,并且對障礙物進行膨化處理,從而降低了平均危險碰撞度。文獻[6]改進了JPS 的搜索規(guī)則,以得到更多具有價值的路徑并使用多段高階多項式對所得路徑進行軌跡優(yōu)化。文獻[7]針對JPS 算法存在路徑搜索時間長等問題,提出了一種雙向跳點搜索算法,從正、反兩個方向采用改進的JPS 算法交替進行路徑搜索。綜合上述研究,JPS 算法主要是通過改進修剪規(guī)則和跳躍規(guī)則體現(xiàn)其優(yōu)于A*算法的路徑搜索效率。然而,相比于A*算法,JPS 算法在圖裁剪的過程中往往需要評估更多的障礙物,尤其是當其所規(guī)劃的路徑過于靠近障礙物,常規(guī)對角線的跳躍方式極易發(fā)生碰撞,搜索到的路徑存在一定的安全隱患;此外,當?shù)貓D規(guī)模較大或對稱性路徑較少的時候,JPS 算法存在搜索到的跳點多且混亂的問題,一定程度上影響了路徑搜索的效率。

考慮JPS 算法和A*算法的特點,本文提出了一種基于改進JPS 算法和A*算法的組合規(guī)劃算法。首先,針對JPS 算法的跳點篩選規(guī)則進行改進并刪除冗余的中間跳點,同時禁止機器人對角線穿越障礙物,并通過所設計的安全性評估模型保證搜索路徑的安全性。然后,考慮到環(huán)境地圖中具有對稱性和非對稱性路徑規(guī)劃的問題,設計了跳點閾值函數(shù),構成基于改進JPS 和A*的組合路徑規(guī)劃算法。最后,仿真實驗結果證明了本文所設計組合路徑規(guī)劃算法的可行性和有效性。

1 JPS 算法簡介

JPS 算法是一種啟發(fā)式路徑規(guī)劃算法,它保留了A*算法的基本結構,其不同點在于搜索路徑過程中對后繼節(jié)點拓展策略。相比于A*算法,JPS算法并非直接獲取當前節(jié)點的所有非關閉的可達鄰居節(jié)點作為后繼節(jié)點,而是根據(jù)當前節(jié)點的方向和強迫鄰居修剪規(guī)則確定后繼節(jié)點的拓展策略,通過裁減無意義的節(jié)點及保留特殊節(jié)點,提高全局路徑搜索的效率。

1.1 跳點篩選規(guī)則

根據(jù)節(jié)點周圍是否存在障礙物,JPS 算法的跳點篩選規(guī)則分為兩種情況:自然鄰居篩選規(guī)則和強制鄰居篩選規(guī)則。

1.1.1 自然鄰居篩選規(guī)則

在圖1 所示的柵格地圖中,灰色柵格表示可裁減節(jié)點。假設路徑搜索到了節(jié)點x處,該節(jié)點的父節(jié)點為p(x),p(x) 到x的方向為擴展節(jié)點的當前方向。圖1 為自然鄰居的篩選規(guī)則,主要有直線方向和對角線方向的篩選方式。

圖1 自然鄰居篩選規(guī)則

如圖1(a)所示,父節(jié)點p(x) 到x為水平方向,故此時x的擴展方向為直線方向。若p(x) 要到達節(jié)點n,虛線和實線的路徑代價相同,即p(x) 可以不經過節(jié)點x到達節(jié)點n且代價不會增加,則稱n為可裁減節(jié)點。根據(jù)以上規(guī)則,除了白色節(jié)點q之外的其他節(jié)點均可被裁減,則稱節(jié)點q為節(jié)點x的自然鄰居。

如圖1(b)所示,x的擴展方向為對角線方向,p(x) 不經過節(jié)點x到達n的路徑最短(圖中虛線所示為最短路徑)。同樣的,白色節(jié)點為自然鄰居,其他節(jié)點均可被裁減。

1.1.2 強制鄰居篩選規(guī)則

強制鄰居篩選規(guī)則如圖2 所示,黑色柵格為障礙物,其他均為可行走節(jié)點。不管當前方向為直線方向還是對角線方向,均不存在任何一條路徑使得p(x) 到n不經過節(jié)點x為最短路徑,即圖中的實線為最短路徑,則定義n為x的強制鄰居,x為n的跳點。

圖2 強制鄰居篩選規(guī)則

1.2 評價函數(shù)

JPS 算法尋路時,對每一個加入open 列表中的跳點進行評估,選擇代價最小的跳點作為下一父節(jié)點,通過不斷地迭代直到擴展到目標點。JPS 算法的評價函數(shù)如式(1)所示。

式中,f(n) 為從起始節(jié)點經過當前節(jié)點n到目標節(jié)點的評價函數(shù),g(n) 是起始節(jié)點到節(jié)點n的實際代價函數(shù),h(n) 為當前節(jié)點n到目標節(jié)點的最小代價函數(shù)。

在二維空間中,一般采用歐式距離計算g(n),即:

式中,(X(n),Y(n)) 為當前跳點n的位置,(X(n-1),Y(n -1)) 為上一跳點的位置。

對于h(n) 而言,常見最小代價函數(shù)的計算方式有曼哈頓距離、切比雪夫距離和歐式距離[8],本文采取歐氏距離計算h(n),即:

式中,(X(n +1),Y(n +1)) 為目標節(jié)點的位置。

1.3 JPS 算法尋路

圖3 為JPS 算法的尋路過程,其中S為起始點,D為目標點,陰影柵格為篩選后留下的跳點。起始點S的當前方向為空,則沿著8 個方向擴展搜索跳點。從S出發(fā)只有下、右、右下3 個方向可走,但向右搜索遇到邊界,向下搜索至障礙物,右下方向搜索到跳點M。將M作擴展節(jié)點繼續(xù)搜索,M的當前方向為對角線方向,但此時可走的方向只有向右,向右搜索到跳點W。節(jié)點W的當前方向為水平方向,沿右、右下2 個方向搜索到跳點V。節(jié)點V沿著當前方向搜索到L,L繼續(xù)搜索直至目標點D,順著父節(jié)點回溯到起始點便得到了最終路徑。

圖3 JPS 算法尋路過程

2 改進JPS 算法的設計

在路徑搜索的過程中,JPS 算法可優(yōu)化A*算法尋找后繼節(jié)點的操作,它是根據(jù)當前節(jié)點的方向和鄰居節(jié)點的特性跳過其他多余的節(jié)點,由此省略了重復多余的計算過程。然而,JPS 算法所規(guī)劃路徑往往過于靠近障礙物,易產生碰撞甚至橫穿障礙物的情況,從而影響路徑的安全性;其次,當大規(guī)模地圖中存在不對稱的情況時,JPS 算法會找到過多的跳點,同時也會不斷地操作open 列表和close 列表中的節(jié)點,導致路徑搜索效率降低。針對移動機器人在對角線穿越障礙物時,可能會發(fā)生一定的碰撞而影響尋路過程中的安全性,本文通過修改冗余節(jié)點刪減規(guī)則和跳點篩選規(guī)則,改善JPS 算法路徑尋優(yōu)的性能。

2.1 冗余跳點的刪除

JPS 算法中跳點可以分為兩種:至少有一個強迫鄰居的跳點和沒有強迫鄰居的跳點[9]。前一種類型的跳點至少相鄰一個障礙物,并且它們在兩個不相鄰的節(jié)點之間至少有一條最佳路徑。如果刪除這些跳點,很有可能導致算法不能返回最優(yōu)路徑。而后一種類型的跳點不與任何障礙物相鄰,即中間跳點,該跳點只作為轉折節(jié)點,最優(yōu)路徑選擇時可以改變方向,到達前一種跳點或者目標節(jié)點。

因此,中間跳點是可以安全修剪的,并且不會影響算法的最優(yōu)性。如圖4 所示,數(shù)字1、4、6 為中間跳點,其他黑色數(shù)字為至少有一個強迫鄰居的跳點。每個中間跳點最多有3 個后繼節(jié)點:第1 種是水平可達的跳點,第2 種是垂直可達的跳點,第3 種是不改變方向的情況下可以到達下一個中間跳點。當修剪中間跳點時,將它的后繼跳點存儲在一個列表中,并用該后繼者代替中間跳點,將這個過程不斷遞歸,直至刪除所有中間跳點。

圖4 修剪中間跳點

2.2 跳點篩選規(guī)則的改進

改進的跳點篩選規(guī)則如圖5 所示。跳點篩選分直線和對角線方向,鄰節(jié)點裁減規(guī)則不變。直線方向上改進的跳點篩選規(guī)則如圖5(a)所示,節(jié)點n是x的強制鄰居,當鄰節(jié)點存在障礙物時,改進算法是不允許對角線穿過障礙物到達強制鄰居。故從p(x) 到達節(jié)點n是必須經過節(jié)點x、y的,此時將x視作普通節(jié)點,將節(jié)點y視為跳點進行規(guī)劃。

圖5(b)是對角線方向上改進的跳點篩選規(guī)則,若不能以對角線穿過障礙物,p(x) 必須經過節(jié)點y、x、z才能到達節(jié)點n,此時將x視為普通節(jié)點,y和z當做跳點。

圖5 改進跳點篩選規(guī)則

2.3 安全性評估模型設計

為了評估所搜尋跳點的安全性,在JPS 算法中引入安全性評估函數(shù)對求解路徑的安全性進行評估。本文采用二維高斯模型構建跳點安全性評估模型,包括平均碰撞度和碰撞度函數(shù),分別如式(4)、(5)所示。

FT為平均碰撞度,O為影響范圍內障礙物的個數(shù),I為求解路徑的節(jié)點個數(shù)。T為碰撞度函數(shù),(xi,yi) 和(xo,yo) 分別是路徑節(jié)點坐標和障礙物坐標,ρ((xi,yi),(xo,yo)) 是路徑當前節(jié)點到最近障礙物的距離,do是障礙物影響范圍,與機器人本身的模型有關,α、β決定碰撞對機器人造成危險的程度,由機器人的速率以及障礙物的材質決定。

3 改進JPS 和A*組合路徑規(guī)劃算法

JPS 算法不是從相鄰節(jié)點中選擇下一個節(jié)點,而是跳過大量可能會添加到open 列表和close 列表中的中間節(jié)點,旨在消除路徑間的對稱性,提高搜索效率[10]。但是,當尋路過程中存在障礙物稠密區(qū)域且存在非對稱路徑時,相比于A*算法只需要從open 列表中選擇具有最小代價函數(shù)值的節(jié)點,JPS算法每生成一個節(jié)點就需要尋找一個跳躍點,對于下一個節(jié)點的選擇過程需要更多處理時間,此時,JPS 算法的尋路代價大,且規(guī)劃的路徑存在一定的安全隱患。因此,本文將所設計的改進JPS 算法與A*相結合,根據(jù)由地圖環(huán)境的復雜度信息確定的跳點閾值函數(shù)選擇不同的路徑搜索規(guī)則。通過組合路徑規(guī)劃算法,一方面利用改進JPS 算法裁減掉對稱路徑搜索過程中的無意義節(jié)點,減少節(jié)點擴展的數(shù)量;另一方面基于A*算法有效避免復雜路徑搜索過程中存在的安全隱患,提高路徑搜索效率。

3.1 跳點閾值函數(shù)的設計

JPS 算法尋路時,環(huán)境中的障礙物越混亂,擴展的跳點數(shù)量越多,搜索效率越低。因此,設計了直通率Pe和障礙率Po判斷地圖環(huán)境的復雜程度。

式(6)中,ne為一行或一列均為無障礙柵格的條數(shù),R和C為環(huán)境地圖的行數(shù)和列 數(shù)。Pe值越大,環(huán)境的混亂程度越低。式(7)中,no為地圖中障礙物柵格的數(shù)量,N為地圖總柵格的數(shù)量。

環(huán)境的復雜程度直接影響跳點數(shù)量,從而影響路徑搜索的效率。將路徑搜索一定區(qū)域內跳點數(shù)量進行量化,記為K,則有

式中,U表示當前節(jié)點的搜索范圍,K表示搜索范圍U內跳點數(shù)量。

跳點閾值與直通率Pe和障礙率Po有關,地圖環(huán)境越復雜,跳點就越多。因此,根據(jù)環(huán)境地圖的復雜度,由式(6)和式(7)定義跳點閾值λ為

因此,改進JPS+A*組合規(guī)劃算法的切換條件為:當K≥λ時,由改進JPS 算法切換為A*算法搜索。

3.2 改進JPS+A*組合規(guī)劃算法流程

基于式(9)所定義的跳點閾值函數(shù),將所設計的改進JPS 與A*算法相結合,構成組合規(guī)劃算法,即當跳點數(shù)量較少時,采用JPS 算法規(guī)劃以便快速穿過對稱路徑,避免不必要節(jié)點的擴展;當跳點數(shù)量過多時,便轉換為A*算法進行路徑規(guī)劃,安全且高效通過障礙物稠密的地圖環(huán)境,由此構成一條完整的規(guī)劃路徑。改進JPS +A*組合規(guī)劃算法流程圖如圖6 所示。

圖6 改進JPS+A*組合規(guī)劃算法流程圖

3.3 組合規(guī)劃算法步驟

設置搜索open 列表和close 列表,起始點為S,目標點為D。改進JPS+A*的組合規(guī)劃算法步驟如下。

步驟1初始化,判斷起點與終點是否在同一連通區(qū),若不在,搜索失敗退出。

步驟2計算地圖的直通率Pe和障礙率Po,根據(jù)式(9)得到該地圖的跳點閾值λ。

步驟3將起點S加入到open 列表中。

步驟4將open 列表中f(n) 值最小的節(jié)點i取出,并將其加入到close 列表中。若i=D,沿著父節(jié)點回溯得到路徑,轉到步驟10;若不是,擴展節(jié)點i。

步驟5判斷當前節(jié)點i搜索范圍內跳點數(shù)量K(基于式(8)),比較K、λ,若K ＜λ,轉到步驟6;否則轉到步驟7。

步驟6改進JPS 算法搜索一步,尋找不在close 列表中跳點。

步驟7A*算法搜索一步,沿8 個方向尋找當前節(jié)點不在close 列表中的可達鄰節(jié)點。

步驟8判斷該節(jié)點是否在open 列表中,若不在,將該節(jié)點加入到open 列表中;若在,計算該節(jié)點g(n) 和父節(jié)點的判斷更新。

步驟9轉到步驟4。

步驟10基于式(4)、(5),通過安全評估模型計算所得路徑的平均碰撞度,尋路成功。

4 仿真及實驗驗證

環(huán)境建模是建立一個供移動機器人自主移動的場所,對環(huán)境模型的要求是便于機器人控制器存儲、處理、更新和使用。因此本文選擇了存儲簡單且易于實現(xiàn)的柵格法構建地圖,并建立安全性評估模型對路徑安全性進行評估,式(5)中參數(shù)取α=1,β=3。

首先,建立15 m×15 m 的移動機器人工作地圖環(huán)境,將本文算法改進的JPS 算法與A*算法和JPS算法進行仿真測試。然后,擴大地圖規(guī)模,建立20 m×20 m 和30 m×30 m 的地圖模型,將改進的JPS +A*算法與JPS 算法作仿真實驗,驗證組合算法的合理性。最后,在50 m ×50 m 的地圖模型中,將改進JPS+A*算法與改進JPS 算法比較,驗證隨著地圖規(guī)模的增大,組合規(guī)劃算法在路徑搜索時間及路徑長度等方面的優(yōu)勢。

4.1 實驗1

在15 m ×15 m 的地圖環(huán)境下,基于A*算法、JPS 算法和本文所設計的改進JPS 算法的路徑規(guī)劃結果如圖7 所示,其中陰影柵格為擴展節(jié)點。3 種算法的實驗路徑參數(shù)如表1 所示。

對于規(guī)模較小且障礙物較少的地圖環(huán)境,比較圖7(a)、(b)可知,相比于A*算法,JPS 算法快速穿越對稱路徑,擴展節(jié)點相比A*算法大大減少。由表1 的實驗數(shù)據(jù)可知,當路徑長度相同的情況下,JPS 算法較A*算法,移動時間縮短了47.16 ms;而改進JPS 算法與JPS 算法相比,路徑長度縮短了5.01%,移動時間減少了16.63%,且由于對跳點篩選規(guī)則的改進,路徑的平均碰撞度降低了40.00%。綜合上述分析,所設計的改進JPS 算法提高了搜索效率和路徑安全性。

表1 路徑規(guī)劃參數(shù)表

圖7 基于15 m×15 m 柵格地圖的3 種算法路徑規(guī)劃結果

4.2 實驗2

為了驗證本文改進JPS +A*算法在復雜環(huán)境的地圖中的合理性,將地圖規(guī)模擴大為20 m ×20 m和30 m×30 m,基于JPS 算法、A*算法和改進JPS+A*算法仿真結果如圖8 和圖9 所示。

圖8 20 m×20 m 柵格地圖中不同算法的路徑規(guī)劃

圖9 30 m×30 m 柵格地圖中不同算法的路徑規(guī)劃

由圖8 和圖9 橢圓標記區(qū)域可知,在穿越障礙物稠密的環(huán)境時,改進JPS+A*算法相較于JPS 算法規(guī)劃的路徑更平滑且更合理。由表2 可得,在路徑長度和移動時間上,改進JPS+A*算法較傳統(tǒng)A*和JPS 算法具有較大的優(yōu)勢;且柵格地圖的規(guī)模越大,改進JPS +A*算法的搜索效率提升越明顯。在路徑安全性上,改進JPS+A*算法的平均碰撞度遠小于其他兩種算法且所規(guī)劃的路徑更加合理。由上述分析可知,本文改進的組合規(guī)劃算法適用于規(guī)模較大、對移動機器人的安全性要求較高的環(huán)境。

表2 不同規(guī)模地圖中各算法的路徑規(guī)劃參數(shù)表

4.3 實驗3

將地圖規(guī)模進一步擴大為50 m ×50 m。基于改進JPS +A*的組合路徑規(guī)劃算法和改進JPS 算法的規(guī)劃結果如圖10 所示。

圖10 本文算法與改進JPS 算法的路徑規(guī)劃結果

由圖10 中橢圓標記區(qū)域可得,當改進的JPS 與A*算法相結合后,利用A*算法有效處理復雜非對稱路徑。由表3 實驗數(shù)據(jù)可知,JPS +A*組合路徑規(guī)劃算法規(guī)劃出的路徑長度比改進JPS 算法減少了0.91 m,移動時間縮短了23.11%,路徑搜索效率顯著提高。

表3 路徑規(guī)劃參數(shù)表

5 結論

本文設計了一種基于改進JPS +A*的組合路徑規(guī)劃算法。該算法針對JPS 算法內部運算機制及復雜地圖環(huán)境因素對移動機器人路徑尋優(yōu)效率影響的問題,通過刪除冗余中間跳點、改進跳點篩選規(guī)則及建立安全性評估模型實現(xiàn)了一種改進的JPS 算法;在此基礎上,根據(jù)環(huán)境的復雜度設計了跳點閾值函數(shù),將所設計的改進JPS 算法與A*算法相結合,實現(xiàn)了復雜地圖環(huán)境下的路徑尋優(yōu)且提高路徑搜索的效率。仿真實驗結果表明,相比于傳統(tǒng)的JPS 算法,本文所設計的改進JPS+A*組和路徑規(guī)劃算法更適用于復雜地圖環(huán)境,尤其是存在非對稱路徑的地圖環(huán)境,有效地減少了節(jié)點數(shù)量、移動時間和平均碰撞度,提高了移動機器人的安全性和工作效率。后續(xù)工作將進一步改善算法的避障能力和求解路徑的平滑性。