融合改進Bi-RRT和DWA算法的無人機動態路徑規劃

羅 毅, 陳新洲

(華北電力大學控制與計算機工程學院,北京 100000)

0 引言

隨著人工智能的發展,無人機被廣泛應用于搜索救援、智慧農業、數據采集等多個領域,高效的避障功能是無人機在復雜環境中成功執行任務的關鍵。避障依賴于智能的路徑規劃,即規劃一條由起點通往目標點的滿足一定約束的無碰撞路徑[1],其核心是路徑規劃算法;根據不同的用途可分為全局路徑規劃算法和局部路徑規劃算法[2]。

全局路徑規劃要預知環境信息,如A*算法[3]、Dijkstra算法[4]、遺傳算法[5]等,但上述算法難以應用于高維復雜環境[6]。快速擴展隨機樹(Rapidly-exploring Random Trees,RRT)算法[7]是一種基于隨機采樣機制規劃全局路徑的算法,其空間搜索能力強大,常被應用于具有高維復雜約束的路徑規劃,但存在采樣過于隨機、收斂速度慢、路徑安全性低、路徑不平滑等缺陷[8],許多學者基于RRT算法提出了多種改進算法。Bi-RRT算法[9]通過構建2棵樹進行雙向搜索提升了收斂速度,但還存在原算法的其他缺陷;RRT*算法[10]通過加入鄰域搜索和重布線過程可使路徑趨于最優,但迭代次數較多;為進一步提升RRT算法的性能,文獻[11]提出了一種改進APF-RRT算法,引入目標偏置和人工勢場減少了冗余采樣點,加快了收斂速度;文獻[12]利用三次貝塞爾曲線使規劃的路徑更加平滑;文獻[13]提出了一種動態步長RRT算法,動態調整步長提升了搜索樹的擴展速度和環境適應能力。為了規避未知障礙物需要使用局部路徑規劃算法,在未知環境中根據探測到的障礙物信息實時動態調整行進路線,如人工勢場法[14]、動態窗口法(Dynamic Window Approach,DWA)[15]等。人工勢場法實時性高且計算簡單,但容易產生局部極小值和震蕩現象;動態窗口法能實時輸出機器人的速度指令,規劃的軌跡更平滑且更便于控制機器人移動,但容易陷入局部最優。文獻[16]通過修正速度窗口和優化評價函數改進DWA,提升了動態路徑規劃能力。

在復雜環境中,為使無人機避障成功,需要將全局和局部路徑規劃相融合。文獻[17]融合IRRT*和DWA能實時更新路線,但規劃的路線較長;文獻[18]融合A*和DWA能同時規避動態和靜態障礙物,但全局路徑跟蹤能力較弱;文獻[19]融合改進Informed-RRT*和人工勢場法能跳出局部規劃極小值并實現動態避障,但渡越時間較長。

綜上所述,本文通過啟發式搜索、動態調整步長提升Bi-RRT算法的收斂速度,在路徑與障礙物間設置安全距離,并對初始路徑進行修剪、插值和平滑處理,降低路徑成本,提升路徑質量。通過修正和新增子評價函數優化DWA,提升軌跡評價結果的準確性和可靠性。最后融合改進Bi-RRT和DWA,提取全局路徑的關鍵節點作為改進DWA的局部目標點,從而為無人機在復雜環境中快速規劃出一條最優的動態避障路徑。

1 改進Bi-RRT算法

Bi-RRT算法比RRT算法的收斂速度更快。該算法構建了2棵搜索樹T1和T2,它們分別以起點和目標點為根節點同時進行隨機采樣和擴展,一次迭代可生成2個新節點,當T1和T2能在一定閾值內無碰撞連接時,將它們連接為1棵樹T,回溯T的所有節點即可獲得全局路徑。然而,Bi-RRT算法還存在采樣過于隨機、路徑安全性低和路徑不平滑的缺陷,針對這些缺陷采用以下方法進行改進。

1.1 設置啟發式函數

基于固定概率p對目標點采樣的策略雖能有效減少非必要采樣點,但當環境中的障礙物分布密集時,p較大會導致搜索樹采樣時過度偏向目標點而陷入局部陷阱;當環境較為空曠時,p較小會導致收斂速度慢。固定采樣概率p無法使算法同時具備目標偏置性和環境適應性。于是,以T1為例,設置啟發式函數F(x)替代p,即

F(x)=1-φ/eμx

(1)

式中:φ∈(0,0.5],為概率初始值;μ∈(0,1),為導數變化率;x∈[0,50],為自變量;在此范圍內F(x)單調遞增。設x的變化規律為

(2)

式中:Δx>0,為自變量增量;Col(n)為碰撞檢測函數,Col(n)=0,表示路徑可通行,Col(n)=1,表示路徑發生碰撞。由啟發式函數可得到新的目標點采樣策略,即

(3)

式中:Qrand和Qgoal分別為采樣點和目標點;r1,r2∈[0,1],都是按均勻概率分布取的隨機值;L、W、H分別是工作空間的長、寬、高。

由式(3)可知,F(x)的輸出值即是T1對目標點采樣的概率。當T1對目標點采樣時:若Col(n)=0,則x=x+Δx,F(x)增大,T1朝目標點擴展的速度加快;若Col(n)=1,則x=x-Δx,F(x)減小,T1朝其他方向擴展的概率增大,可避免T1過度偏向目標點而陷入局部陷阱。

1.2 設置動態步長

復雜環境中的障礙物分布通常是不均勻的,與固定步長相比,動態步長能充分利用已知的環境信息將當前擴展步長調整為合適大小[20]。以T1為例,設置動態步長η,即

η=max[ηmin,min(ηmax,d/n)]

(4)

式中:ηmin和ηmax分別為步長的最小、最大值;d為T1與目標點的最短距離;n為T1當前節點數量。

迭代前期,搜索空間較大,選取ηmax擴展T1;迭代中期,T1初步成型,改選d/n進行擴展;迭代后期,搜索空間較小,使用ηmin進行擴展。

1.3 設置安全距離

Bi-RRT算法會生成緊貼障礙物的路段,導致全局路徑存在安全隱患。因此,需要在路徑與障礙物間設置安全距離以判斷路徑的安全性,如圖1所示。圖1中,Qnew為待添加節點,Qnear為Qnew的父節點,l2為待檢測路段,l1與l3同時關于l2及其所處的豎直平面對稱,l4與l5在同一豎直平面上關于l2對稱。將其他線段和l2之間的距離均設為W,W即為安全距離。對線段l1～l5同時進行碰撞檢測,若均不發生碰撞,則l2為可通行路段,Qnew為可擴展節點;若有至少一條線段發生碰撞,則l2存在安全隱患,此時需要重新采樣。

圖1 安全距離Fig.1 Safe distance

1.4 路徑插值與平滑

初始路徑中通常包含大量冗余路段,且路徑不夠平滑,通過修剪路徑[21]可有效消除冗余路段。而針對路徑不夠平滑的問題,解決方法之一是將路徑關鍵節點作為基函數控制點構建三次B樣條并替代原路徑[22],三次B樣條的表達式為

(5)

式中:Fj,3(t)為三次B樣條的基函數;Vj為基函數控制點。

下面構建三次B樣條,如圖2所示。

圖2 構建三次B樣條Fig.2 Construction of cubic B-splines

圖2中,紅色虛線為修剪后的路徑,其節點序列為,將此序列作為基函數控制點構建三次B樣條(黑色曲線)。雖然三次B樣條足夠平滑,但其曲率較小時會穿過障礙物,需要適當增大曲率。而通過移動、增加或更換控制點可以改變其曲率大小,于是選擇增加和更換控制點的方法增大曲率,使三次B樣條既平滑又安全,并基于此提出一種路徑插值法。以為例,將P2作為中間節點,分別以和為方向,以一定步長插入P4和P5并刪除P2得到新路徑(紅色實線),其節點序列為,然后將該序列作為新控制點即可構建出平滑無碰撞的三次B樣條(深藍色曲線)。

2 改進DWA

DWA的執行步驟為速度采樣、軌跡預測和軌跡評分。首先在無人機的速度空間中預測出所有可行軌跡,再根據評價函數選取最優軌跡并輸出與之對應的速度指令。在現實中需要對速度空間進行約束,即

(6)

(7)

式中:(x(t+Δt),y(t+Δt),z(t+Δt))為無人機在(t+Δt)時刻的坐標;θxy(t)和θz(t)分別為無人機在t時刻的偏航角和俯仰角。最后根據評價函數從中選取最優軌跡,傳統評價函數的表達式為

G(v,w)=α×head(v,w)+β×obsdis(v,w)+
γ×vel(v,w)

(8)

式中:G(v,w)為總評價函數;head(v,w)用于衡量無人機當前航向與目標點方向之間的偏差,偏差越小,評分越高;obsdis(v,w)用于衡量當前預測軌跡與障礙物之間的最短距離,距離越大,評分越高;vel(v,w)用于調整速度大小;α,β和γ分別為各項子函數的系數。

下面對傳統評價函數進行優化。

2.1 修正障礙物距離評價函數

首先將障礙物分為已知障礙物和未知障礙物,將已知障礙物的信息作為碰撞檢測函數的輸入,未知障礙物的信息則作為障礙物距離評價函數obsdis(v,w)的輸入,同時設置無人機威脅空間對障礙物進行篩選,見圖3。

圖3 無人機威脅空間Fig.3 Threat space to the UAV

圖3中,設點U為無人機,點X1～X3為未知障礙物,以U為中心,以r為半徑的球體空間即為威脅空間,空間內的障礙物X2被判定為存在威脅的障礙物,空間外的障礙物X1和X3則直接忽略。于是得到新的障礙物距離評價函數,即

(9)

2.2 引入目標點距離評價函數

傳統評價函數中,無人機由head(v,w)導航,通常導致無人機實際到達位置與目標點位置有偏差。goaldis(v,w)是目標點距離評價函數,可用于衡量無人機當前預測軌跡與目標點的最短距離,從而增強無人機的目標跟蹤能力,即

goaldis(v,w)=[min(disg(v,w)-L),0]2

(10)

式中:disg(v,w)為無人機在速度(v,w)下的預測軌跡與目標點的最短距離;L為距離閾值。

當disg(v,w)>L時,goaldis(v,w)=0,不參與導航,無人機仍由head(v,w)導航;當disg(v,w)≤L時,無人機到達目標點附近,此時goaldis(v,w)>0,并且距離目標點越近goaldis(v,w)的值越大,在導航中發揮的作用也越大,目標導向性則越強,從而能引導無人機準確到達目標點位置。

由式(9)和式(10)得到新的評價函數,即

G(v,w)=α×head(v,w)+β×obsdis(v,w)+
γ×vel(v,w)+ε×goaldis(v,w)

(11)

式中,ε為goaldis(v,w)的系數。

3 融合算法

融合改進Bi-RRT和DWA,提出一種無人機動態路徑規劃算法,具體流程如圖4所示。

圖4 融合算法流程圖Fig.4 Flow chart of the fusion algorithm

4 仿真驗證

下面設計2組仿真實驗,分別驗證所提融合算法在全局路徑規劃和局部路徑規劃上相較于傳統算法的優越性和高效性。

4.1 全局路徑規劃

建立仿真實驗所需的工作空間,大小為800×800×100,并設置起點(20,20,20)和目標點(760,760,20),單位均為m。首先將改進Bi-RRT算法和普通Bi-RRT算法、Bi-RRT*算法生成路徑的質量進行對比,并根據環境信息將改進Bi-RRT算法的安全距離設為2 m,如圖5所示,圖中的灰色區域均為靜態障礙物,高度均為100 m。

圖5 3種全局路徑規劃算法生成的路徑Fig.5 Paths generated by three global path planning algorithms

由圖5可知:普通Bi-RRT算法會生成大量冗余路段,且路徑曲折;Bi-RRT*算法雖然通過重布線過程使得冗余路段顯著減少,但路徑仍不夠平滑;改進Bi-RRT算法生成的路徑既無冗余路段,也能始終保持平滑,滿足無人機的動態特性要求。

3種全局路徑規劃算法的實驗數據見表1。

表1 3種全局路徑規劃算法的性能

由表1可知,與普通Bi-RRT算法相比,改進Bi-RRT算法通過啟發式搜索和動態調整步長加快了收斂速度,提升了環境適應能力,將規劃時間減少了87.2%,迭代次數也顯著減少,并通過優化路徑使路徑長度縮短了24.77%。Bi-RRT*算法雖然在一定程度上提升了路徑質量,且路徑長度縮短了12.43%,但迭代次數和時間成本明顯增加,利小于弊。3種全局路徑規劃算法中,僅有改進Bi-RRT算法通過設置安全距離使路徑始終保持安全性。綜上所述,改進Bi-RRT算法規劃的全局路徑最優。

4.2 局部路徑規劃

在4.1節工作空間中隨機加入未知動、靜態障礙物,設動態障礙物的半徑,在25～30 m之間,靜態障礙物的高度為100 m。將圖5中改進Bi-RRT算法生成的關鍵節點作為DWA的局部目標點進行仿真實驗,將改進DWA與傳統DAW生成航跡的質量進行對比,結果如圖6和圖7所示。圖中,綠色與藍色球體分別表示各動態障礙物的移動起點與終點,紅色物體表示靜態障礙物。為了便于觀察航跡,在側視圖和主視圖中僅顯示未知障礙物模型。

圖6 傳統DWA

圖7 改進DWA

由圖6可知,在規避未知障礙物時,傳統DWA通常需要規劃出更長的避障軌跡,整體生成的路徑較為曲折,與全局最優路徑的貼合度較低,航跡不具備最優性,導致無人機需要規避的動態障礙物較多。

由圖7可知,改進DWA生成的航跡不僅平滑,與全局最優路徑的貼合度也更高,可靠性更高,滿足無人機的動態特性要求。

對傳統DWA和改進DWA的性能進行分析,由實驗得到的數據見表2。結合表2的數據可知:與傳統DWA相比,改進DWA對未知障礙物進行了篩選,使得obsdis(v,w)的輸出不受無關障礙物信息的影響,結果更準確;在引入goaldis(v,w)后,無人機航行至目標點附近時開始由head(v,w)和goaldis(v,w)共同導航,目標跟蹤能力明顯增強;改進DWA在優化了評價函數后,對預測軌跡的評價結果更加準確,使無人機從起點航行至終點的時間減少了25.48%,航跡長度縮短了11.90%,迭代次數減少了27.34%,動態避障效率更高,效果更佳。

表2 傳統DWA和改進DWA的性能對比

綜上所述,所提融合算法在全局和局部路徑規劃方面較傳統算法都更具優勢,具備控制無人機在復雜環境中高效規劃動態避障路徑的能力。

5 結論

本文融合改進Bi-RRT和DWA提出了一種無人機動態路徑規劃算法。在全局規劃方面,設置啟發式函數和動態步長加快了Bi-RRT算法的搜索速度;設置安全距離提升了路徑安全性;采取修剪、插值和平滑路徑的措施獲得了全局最優路徑。在局部規劃方面,優化評價函數提升了DWA預測軌跡評價結果的準確性,增強了動態避障能力。所提融合算法為無人機在復雜環境中的高效導航創造了有利條件。