改進JPS的無人機路徑規劃研究

唐嘉寧,閆搏遠,陳云浩,顏 衡,程俊濤

(1.云南民族大學 電氣信息工程學院, 昆明 650000;2.云南民族大學 無人自主系統研究院, 昆明 650000)

0 引言

路徑規劃是無人機在執行飛行任務時的關鍵問題。隨著無人機領域的發展,在災后探測、環境檢測、電力巡檢等復雜的環境飛行時,快速準確地規劃出一條飛行軌跡就尤為重要。全局規劃是無人機完成導航等任務的前提。全局規劃是指在設定好起點和終點后,根據規劃算法規劃出一條從起點到終點的與障礙物碰撞的最佳路徑[1]。常用的全局規劃算法有Dijkstra算法[2]、A*算法[3]、概率路線圖PRM方法、快速擴展隨機樹RRT算法[4]。

由于A*算法代價函數的優越性和對搜索路徑的直接性,因此在全局規劃中得到廣泛應用。但使用A*算法進行全局路徑規劃時,實時性較差,節點的計算量大,內存占用大,計算時間長。隨著節點數的增加,A*算法的規劃時間也相應的變長。因此,對于A*算法的這些問題,Harabor等[5]提出了跳點搜索算法(jump point search,JPS)。在路徑規劃時,JPS算法不僅刪除大量的對稱性中間節點和無用節點,還保留了關鍵的跳點,這樣就可以減少搜索路徑,有效提高了路徑規劃效率[6]。宋曉茹等[7]為了提高移動機器人在復雜環境下的可靠性和智能性,用一種“塊”操作方法加快搜索跳點的速度,同時刪除了代表路徑方向發生改變的中間轉折點。JPS算法定義了“強制鄰居”和“跳躍點”,去除規劃路徑時的冗余點,保留下關鍵點(即跳躍點)。將這些跳躍點連接起來,就可以形成一個完整的全局路徑。Zhao等[8]通過將JPS算法應用到移動機器人的路徑規劃上,證明了JPS算法要優于A*算法。對于全局規劃路徑的速度問題,Iakushkin等[9]提出了一種雙向JPS算法,這種方法從起點和終點同時搜索路徑,提高了路徑的查找速度,減少了規劃時間,提高了全局規劃的效率。Traish等[10]為了獲得更快的路徑規劃,提出通過記錄障礙物邊界來減少搜索過程中的迭代計算。黃智榜等[11]為了解決規劃出的路徑可能存在的碰撞問題,將跳點進行了一種危險化評估,改進了跳點的篩選方式,并同時對障礙物進行膨脹化處理,從而降低了危險碰撞度的平均值。黃健萌等[12]為了得到更多有價值的路徑,使用多段高階多項式對所得路徑進行軌跡優化,對JPS的搜索規則做了改進。邱磊[13]提出了一種雙向JPS算法,從正、反2個方向采用改進的JPS算法交替進行路徑搜索。蔡佳成等[14]為了解決路徑規劃時在大尺度復雜的場景下存在內存資源消耗過大、規劃出的路徑不夠平滑的問題,提出了一種通過融合安全勢場等級函數和優化的Floyd算法的改進JPS算法。黃智榜等[15]為了解決JPS算法規劃出的路徑距離障礙物過近的問題,提出了一種基于規劃路徑碰撞危險度的改進JPS算法,該算法通過對傳統跳點規則進行危險評估來使跳點與障礙物保持距離。周熙棟等[16]通過提出分層柵格地圖路徑規劃算法,來解決移動機器人在大范圍非結構化場景下的路徑規劃問題。張慶等[17]通過利用切比雪夫距離代替歐式距離來改進啟發函數,用來解決在場景較大的柵格地圖中冗余節點過多的情況。黃健萌等[18]為了解決在路徑規劃時存在的軌跡不夠平滑和規劃效率不高等問題,提出了一種對目標路徑序列進行優化處理并且同時對JPS搜索規則進行改進的方法。

現在也有很多無人機路徑規劃算法,比如RRT算法、DWA算法、PRM算法等。但這些算法由于自身的特性都有一些不足之處。RRT算法是一種是樹型算法,由根節點作為起點進行隨機生長,它的優點是可以快速高效地規劃出一條路徑,但由于其隨機性采樣的原因,生成的路徑并不一定是最優路徑。由于JPS算法是通過起點與目標點之間的方向進行跳點搜索,不存在采樣的隨機性,故JPS算法可以很好地彌補RRT算法的不足。DWA算法主要通過對速度空間中的速度進行多組采樣,并通過這些采集來的速度對接下來一段時間內的軌跡進行預測,通過評價函數對得到的多組預測軌跡進行評價來選擇一條最優軌跡。DWA算法由于只考慮速度和加速度的限制,所以它的計算復雜度低。但由于DWA算法的前瞻性不足的原因,在無人機遇到“C”字形狀的障礙物時,不能很好地避障,而JPS算法在這種情況時,可以根據跳點搜索規則下的強制鄰居搜索規則來解決這一情況。PRM算法也是通過采樣進行路徑規劃的算法,與RRT算法不同的是,PRM算法在進行路徑規劃時,首先要將空間中連續的點轉換為離散的點,然后在采樣區域隨機撒點,將落在障礙物上的點剔除出去,將保留的點作為中心,在中心的一定范圍內進行鄰域搜索,再將它們連接成無數條路徑,對連接的路徑進行碰撞檢測,如果沒有碰撞那么就將該條路徑保存。但在遇到狹窄通道時,無障礙物的區域采樣點過于密集,障礙物多的地方采樣點又過少,可能會存在無法找到最優路徑的情況。而JPS算法由于自身的跳點搜索規則,在無障礙物時可以直接跳過中間的搜索,在障礙物密集區域根據自然鄰居搜索和強制鄰居搜索規則可以有效地解決PRM算法遇到的問題。由于相對于其他路徑規劃算法,JPS算法具有以上優勢,故選擇改進JPS算法作為無人機的路徑規劃算法。

在上述分析和討論的基礎上,提出了一種在三維空間進行路徑的S-JPS算法,并將此算法在三維空間下與JPS算法進行對比實驗。通過比較,證明了S-JPS算法具有較高的綜合性能。仿真實驗驗證了S-JPS具有JPS算法的快速規劃性能和域擴展性能,在特殊地圖上仍然具有靈活性。

1 JPS算法

對于A*算法運行過程中openlist列表的計算量大、運行時間長等問題,提出了JPS算法。從圖1可以看出,為了使節點的展開方向保持不變,就需要在水平方向上連續移動,但A*算法仍需要計算水平方向上的每個節點的代價值,因此JPS算法基于這種情況,省略了中間的迭代計算過程,提出了跳躍搜索方法。在JPS算法中,起始的節點考慮了各個方向上的鄰居節點。當由目標函數確定了路徑規劃的方向時,JPS算法就會繼續向這個方向擴展,但是在遇到障礙物或者由算法定義的跳躍點之前,不需要計算這個方向節點的代價值。

圖1 A*算法路徑規劃代價計算過程示意圖

1.1 跳點搜索規則

JPS算法的跳點搜索規則分為沒有障礙物時和有障礙物時2種情況。當沒有障礙物時,使用自然鄰居搜索規則,當存在障礙物時,使用強制鄰居搜索規則。

1.1.1 自然鄰居搜索規則

當沒有障礙物時,使用自然鄰居搜索規則。在圖2所示的柵格地圖中,灰色的區域表示可刪除的冗余點。當前節點a的父節點為p(a),其中擴展節點的當前方向為p(a)到a的方向。自然鄰居的搜索規則如圖2所示,主要有2種情況:直線方向和對角線方向的搜索規則。

圖2 自然鄰居搜索規則示意圖

在圖2(a)中,父節點到當前節點的方向為水平方向,所以此時當前節點a的擴展方向為直線方向。如果父節點p(a)要到達節點2,那么在直線方向搜索時,虛線和直線的路徑代價相同,也就是說父節點p(a)如果要到達節點2可以不用經過節點a并且路徑的代價不會增加。那么就稱節點2為可刪除的冗余節點。根據上面提到的規則,除了節點1以外的節點都是可以被去除的冗余點,那么就可以將白色節點1看作是節點a的自然鄰居。

在圖2(b)中,節點a為對角線方向搜索,父節點p(a)不經過節點a到達節點2路徑最短,在圖中虛線所表示的路徑為最短路徑。和直線方向搜索的情況一樣,白色的節點看作是自然鄰居,其他節點作為冗余節點刪除。

1.1.2 強制鄰居搜索規則

當有障礙物時,使用強制鄰居搜索規則。在圖3所示的柵格地圖中,障礙物用黑色柵格表示,其他區域為可通過的路徑。當前節點a的父節點為p(a),當有障礙物存在時,無論是按照直線方向搜索還是按照對角線方向搜索,都無法避免節點a直接到達節點1。強制鄰居的搜索規則如圖3所示,主要有2種情況:直線方向和對角線方向的搜索規則。

圖3 強制鄰居搜索規則示意圖

1.2 代價函數

假設集合A表示展開點的集合,集合A={a1,…,ai,…,an},其中ai為展開的第i個節點,n是展開的節點數,astart是起點,agoal是終點。D表示路徑的距離,d(astart,a1)表示從起點到第一個節點的距離,d(ai,ai+1)表示在尋路過程擴展的任意2個節點之間的距離,d(an,agoal)表示從最后一個擴展點到目標點的距離。路徑長度的目標函數為:

(1)

代價函數為:

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

(2)

在式(2)中,f(n)表示起點到目標點的代價函數,g(n)表示當前節點到起點的代價函數,h(n)表示當前節點到目標點的預計代價函數,其中:

(3)

常用的預計代價函數公式有:曼哈頓距離(式(4))、歐幾里得距離(式(5))、切比雪夫距離(式(6))、八位數距離(式(7))。

h(n)=|x2-x1|+|y2-y1|

(4)

(5)

h(n)=max(|x2-x1|,|y2-y1|)

(6)

(7)

式中:(x1,y1)為當前節點的坐標,(x2,y2)為目標點的坐標。

采用歐式距離作為預計代價函數。

JPS算法尋路過程如圖4所示。

圖4 JPS算法尋路過程示意圖

在圖4中,綠色柵格是起點,紅色柵格是終點,黃色柵格是搜索的跳躍點。從圖4可知,JPS算法丟棄了大量不需要擴展的節點,相對于A*算法,大大提高了搜索效率。

1.3 算法流程

JPS算法流程如圖5所示。JPS算法的流程與傳統A*算法的流程相似。都需要對openlist和closelist進行操作,當前節點的下一節點由代價函數f=g+h更新。JPS算法在節點運動的方向為:直線和對角線之間尋找跳點。

圖5 JPS算法流程

2 改進JPS算法

在傳統的JPS算法里,前端的路徑規劃里規劃出的節點較多,效率低。在后端的軌跡優化里只考慮了距離信息,沒有考慮方向信息,軌跡優化比較困難。針對這2個問題,提出基于JPS算法改進的S-JPS算法。

2.1 引入新的啟發函數

在S-JPS算法中,提出一個新的啟發函數f(n)來解決前面提到的2個問題,如下:

f(n)=aL-bcosα

(8)

式中:a、b表示距離和方向的權重,L表示距離信息,如式(9)所示;cosα是方向信息,表示父節點和擴展節點之間的角度以及擴展節點和目標點之間的角度,它表示根據這個節點擴展所需要的方向成本。

當2個方向相同時,方向代價最小,cosα的值為+1;當2個方向相反時,方向代價最大,cosα的值為-1。

(9)

式中:dx1和dx2分別表示當前節點S和目標節點G的橫坐標;dy1和dy2分別表示當前節點S和目標節點G的縱坐標。

2.2 節點更新規則

S-JSP算法可以更高效、準確地規劃出路徑。雖然JPS算法可以顯著地減少擴展節點的數量,但它會使障礙物區域的擴展節點數目增加,這樣將會增加后端軌跡優化的難度。在這個基礎上,對拐點進行了修整,使路徑的平滑度得到了進一步提高。對于所有的擴展節點(a0,a1,…,aN),如果相鄰節點形成的直線an-1an和anan+1的斜率不同,則連接節點an-1和an+1。如果直線an-1an+1沒有穿過障礙物,則舍棄原來的an-1an和anan+1,保留線段an-1an+1。然后比較an-1an+1和an+1an+2,以此類推。

如圖6所示,藍色線段為JPS算法規劃出的路徑,一共得到5個規劃點。在JPS算法的基礎上改進得到S-JPS算法,使用S-JPS算法進行規劃,得到的新路徑,用紅色線段表示。S-JPS算法將點a0和a2連接起來,因為路徑a0a2沒有通過障礙物,所以舍棄了原來的路徑a0a1和a1a2,保留了路徑a0a2。繼續連接點a0和a3,由于路徑a0a3穿越障礙物,所以舍棄a0a3,保留路徑a0a2。再次從點a2開始,連接a2a3和a3a4,依次類推,最后將路徑a2a3、a3a4、a4a5、a5aN舍棄,保留路徑a2aN。從圖6中可以看出,擴容點a1、a3、a4、a5被舍棄,只保留節點a2。雖然算法的擴展節點保持不變,但改進后的規劃路徑更高效。

圖6 基于JPS改進算法示意圖

S-JPS算法的偽代碼如下:

Algorithm1S-JPS

Input:initialastart,targetagoal,priority queue

Output:optimal path

1g(astral)=0,g(n)=infinite

2 while

3 if the queue is empty,return FALSE break

4 calculate theh(n)=aL-bcosα

5 remove the nodenwith the lowestf(n)=g(n)+h(n) from the priority queue

6 mark nodenas expanded

7 if the nodenis the goal stateagoal,return TRUE; break

8 for all unexpanded jumping neighborsmof the noden

9 ifg(m)=infinite

10g(m)=g(n)+Cnm

11 push nodeminto the queue

12 ifg(m)>g(n)+Cnm

13g(m)=g(n)+Cnm

14 end

15 fori=1 ton-2(number of points,up date every cycle)

16 whilen-i>0

17 ifanan+2does not pass through the obstacles

18 discardan+1

19 else

20 break

21 end

22 end

3 基于Bezier曲線和直線混合的軌跡優化

基于JPS算法提出的S-JPS算法雖然能夠生成最優路徑,但是沒有考慮無人機的運動學模型,得到的路徑不是光滑的,路徑的曲率也不是連續的。針對這個問題,提出一種基于Bezier曲線和直線混合的軌跡優化算法對軌跡進行平滑處理,提高全局路徑的平滑度。其中Bezier曲線的一般形式如下:

(10)

式中

(11)

B5(t)=(1-t)5a0+5(1-t)4ta1+
10(1-t)3t2a2+10(1-t)2t3a3+
5(1-t)t4a4+t5a5

(12)

利用5次Bezier將規劃出的路徑優化成平滑、曲率連續的路徑,實現無人機導航的軌跡生成。5次Bezier曲線的表達式如式(12)所示,曲線本身需要6個控制點,除起點和終點以外還有4個中間控制點。對于本文算法,如果路徑有N個拐點,則整個路徑有N+2個路徑點:a0,a1,a2,…,aN,aN+1,N+1條折線。因此,軌跡優化后的N條Bezier曲線和N+1條直線段采用5次Bezier曲線直線段,它們的路徑和曲率都是連續的。

由于該算法需要Bezier曲線的曲率與直線段連續,因此Bezier曲線與直線段連接處的曲率為0。如圖7所示,由于Bezier曲線的凹凸性,優化出的軌跡在線段內[19-20]。其中紅色軌跡為Bezier曲線處理后的軌跡。用Bezier曲線代替原直線段的最重要的部分為控制點的選取。

圖7 Bezier曲線的凹凸性

如圖8所示,圖中ai-1、ai、ai+1為相鄰的3個關鍵拐點,這3個點的坐標分別為(xai-1,yai-1)、(xai,yai)、(xai+1,yai+1),其中拐點的平滑突變區又有4個關鍵控制點Pi、Pi+1、Pi+2、Pi+3。式(13)為控制點Pi的坐標:

圖8 Bezier曲線平滑策略

將上述求解過程中控制點Pi的xai-1和yai-1用xai+1和yai+1替換,就是控制點Pi+3的坐標。Pi+1和Pi+2為中間的2個控制點,坐標為:

(15)

4 仿真分析與實驗

4.1 仿真分析

為了對改進的算法進行驗證,對傳統JPS算法和S-JPS算法進行仿真分析。在同一個地圖環境下,起點相同,分別選取6個不同的目標點,進行三維路徑規劃。實驗所用的硬件平臺為Intel(R) Core(TM) i7-8750H CPU@ 2.20 GHz,運行內存為16 GB,操作系統為Ubuntu18.04。在ROS中隨機生成一個三維地圖,三維地圖的3個不同視角的視圖如圖9所示。

圖9 隨機生成三維環境示意圖

將本文算法與JPS算法同時進行仿真測試。白色柵格為JPS算法規劃出的節點,黑色柵格本文算法規劃出的節點。在生成的三維環境中依次進行選取目標點進行規劃,仿真結果如圖10所示。

圖10 JPS算法與S-JPS算法仿真結果

為便于觀察,在圖10中規劃出節點的地方已用黑色圓圈圈出,并用紅色箭頭指出。通過仿真結果可以看出,在目標點和起點相同時,本文算法規劃出的節點要比JPS算法規劃出的節點少。表1為JPS算法與S-JPS算法在同一地圖中的實驗數據對比情況。S-JPS算法相比于JPS算法,時間代價減少98.6%,路徑代價減少81.1%,拜訪節點個數減少99.7%。

表1 JPS算法與S-JPS算法實驗數據對比

4.2 實驗

為了驗證S-JPS算法在無人機中實際運行的有效性,將JPS算法與S-JPS算法同時在同一地圖進行規劃,將算法移植到自主搭建的無人機上,如圖11(a)所示。無人機采用D435I雙目相機獲取二維點云數據,結合陀螺儀和里程計數據進行三維地圖的構建,最后通過S-JPS算法對無人機進行路徑規劃。

圖11 自主搭建的無人機與實驗環境

如圖12所示,在無人機實際起飛后,可以在Rviz工具中顯示出越過障礙物時經過Bezier曲線和直線混合的優化軌跡。其中藍色軌跡為無Bezier曲線和直線混合的軌跡優化,紅色軌跡為基于Bezier曲線和直線混合的軌跡優化。從實驗結果可以看出,藍色軌跡會出現穿越障礙物或者距離障礙較近的問題,而紅色軌跡則不會穿越障礙物并且距離障礙物會保持一定的安全距離。

圖12 無人機起飛過程中的Rivz界面

5 結論

路徑規劃作為無人機完成給定任務的最重要的環節,在路徑規劃當中引入了改進的JPS和Bezier曲線算法。首先,利用基于距離和方向的啟發式函數,更準確地描述了從當前點到目標點的估計代價,以降低時間代價、路徑代價和拜訪的節點數。最后通過Bezier曲線和直線混合的軌跡優化算法將規劃出的路徑進行平滑處理,使它更滿足無人機的動力學約束。實驗結果表明,相比于傳統JPS算法,S-JPS算法在時間代價、路徑代價和拜訪節點數目上都遠小于JPS算法。

對于提出的算法,還存在以下改進空間:由于JPS是在A*算法的基礎上提出的,在較為空曠的區域時A*算法的效率要高于JPS,在障礙物密集區域時JPS算法效率又遠高于A*算法。所以在S-JPS算法中,在啟發函數階段設置一個閾值k,當障礙物密度小于閾值k時使用A*算法的啟發函數;當障礙物密度大于閾值k時,使用S-JPS算法的啟發函數,動態啟發函數公式如式(16)所示。通過定性的分析可以得出改進后算法的規劃時間要小于S-JPS算法,規劃效率要高于S-JPS算法。

(16)

式中:H為當前障礙物的密度,k為設置的障礙物密度閾值。