基于安全飛行走廊的無人機密集障礙規避算法

范力元,張浩哲,徐釗,呂明偉,胡勁文,趙春暉,劉曉斌

(1.西北工業大學 自動化學院,陜西 西安 710072;2.西北工業大學 電子信息學院,陜西 西安 710072;3.沈陽飛機設計研究所,遼寧 沈陽 110035;4.西安現代控制技術研究所,陜西 西安 710065)

近年來,隨著無人機技術的快速發展,無人機在許多方面取代了人工勞作,比如在地形勘探、災難搜尋、邊界巡檢等方面[1]。與旋翼無人機相比,固定翼無人機在載荷、飛行距離等方面有明顯優勢,在作戰、運輸等領域具有更廣泛的應用。無人機的路徑規劃是無人機自主控制中的一項關鍵技術,由于固定翼飛行約束較強,目前成熟的固定翼無人機路徑規劃通常是起飛前進行的預規劃,中途遇到突發情況往往需要人為干預,缺乏威脅規避的自主性,難以適應低空復雜多障礙環境下的高動態敏捷控制需求,例如對鳥群、樹木、建筑物等的防撞規避,以及機場附近密集空域下的無人機自主安全飛行。為了提升未來民用無人機空域集成安全與自主飛行安全能力,需要研究固定翼無人機在復雜多障礙環境下的自主“感知與規避技術”[2]。

傳統的障礙規避算法多用于無人車、無人船等智能體上。避障算法大致可以分為全局路徑規劃、局部路徑規劃以及與近年來發展迅猛的人工智能方法相結合的路徑規劃算法。全局路徑規劃方面,基于A*發展出了更加高效的迭代加深A*算法(iterative deepening A*,IDA*)、終身規劃A*算法(lifelong planning A*,LPA*)、雙向A*算法(bidirectional A*)[3-5];適用于動態障礙物環境的D*,Focused D*、D*Lite算法[6-8];基于概率的概率地圖法(probabilistic road map,PRM)、快速搜索隨機樹法(rapidly-exploring random tree,RRT)、快速搜索隨機樹優化算法(RRT*)[9-10]、雙向RRT*算法[11]。局部路徑規劃方面人工勢場法(artificial potential field,APF)[12]簡單有效,被廣泛應用于實際工程當中;速度障礙法(velocity obstacle,VO)[13]選擇速度場中合適的避障行為來保證機器人的安全性;向量場直方圖算法(vector field histogram,VFH)、VFH+[14]利用障礙物信息對移動機器人運動方向產生的影響達到避障效果;人工智能算法方面,應用較早的引力搜索樹法(gravitational search algorithm,GSA)、模擬退火法(simulated annealing,SA)、遺傳算法(genetic algorithm,GA)、粒子群優化算法(particle swarm optimization,PSO)[15-18]在路徑規劃算法方面取得了不錯的應用效果,人工神經網絡(artificial neural network,ANN)、監督學習(supervised learning,SL)、強化學習(reinforcement learning,RL)也在路徑規劃方面取得了不錯的效果[19-21]。固定翼無人機相比無人車、旋翼無人機等智能體有更多的運動學限制,上述避障算法不能簡單應用于固定翼無人機的避障。

針對固定翼無人機的避障問題,研究人員也做了許多相關研究。Vanegas等[22]使用與貝塞爾曲線近似的3D回旋曲線生成平滑曲線,引導無人機跟蹤實現障礙物規避。Lee等[23]基于三次貝塞爾曲線的樣條方法擴展提出的樣條RRT*算法的樹結構,生成三維曲線，實現固定翼無人機三維空間中的障礙物規避。Keller等[24]將圖搜索算法和樣條采樣算法相結合,應用于多固定翼無人機的障礙物規避。Benders等[25]在考慮固定翼無人機多飛行約束條件下,利用A*算法實現了有風干擾環境中的3D路徑規劃。Song等[26]綜合A*和Dubins曲線實現了固定翼無人機的避障,相比傳統A*算法,此方法飛行效率更高。另外智能算法在固定翼無人機的避障方面也有應用。Sanders等[27]結合無人機的動力學特性和油耗、油門推力等,利用非支配排序遺傳算法實現了固定翼無人機到目標點的最優路徑規劃。Roberge等[28]使用遺傳算法(GA)和粒子群優化算法(PSO),考慮無人機動態特性實現無人機在復雜3D環境中的準最優路徑規劃,該團隊在2018年[29]利用遺傳算法在GPU上運動生成適合于固定翼無人機的平滑路徑,實現固定翼無人機在滿足戰場高動態環境中的障礙物規避。

相比上述提到的固定翼無人機障礙規避算法,本文所提算法能夠實現復雜低空密集障礙物環境的障礙物規避。本算法考慮并滿足無人機機動性的要求,不僅基于無人機當前位置與障礙物位置,還考慮無人機未來航跡與障礙物的關系,實現障礙物規避,另外能夠通過調整障礙規避評價函數的權重向量,對避障算法進行有傾向性的調節。以上功能對于安全性要求更嚴苛的固定翼無人機而言更加安全。

1 問題描述

1.1 固定翼無人機飛行建模

(1)

pk=pk-1+uk-1

(2)

式中：pk=[xk,yk,θk]分別表示k時刻航跡的坐標點以及朝向;uk表示k時刻無人機的輸入控制量。

1.2 基于Dubins曲線的安全飛行走廊

1.2.1 Dubins曲線

對于固定翼無人機的航路規劃問題,首先要解決滿足諸如最小轉彎半徑等約束條件時,2個航路點之間的最短路徑規劃問題。航路點包含的信息有二維平面坐標點、偏航角,即Dubins曲線模型中包含的信息。Dubins[30]于1957年給出完備的Dubins路徑集,解決了有界曲率下2個點之間的最短路徑問題,最短路徑就包含在Dubins完備路徑集中,路徑集中的每條路徑由線段和圓弧組成。

圖1 Dubins軌跡示意圖(RSL)

為了減少最短路徑計算時間,Shkel等[31]對Dubins完備路徑集進行啟發式改進,根據起點和終點的運動方向將Dubins路徑集分成幾種等價組,通過構建等價組邏輯分類判斷,就能快速求得最短路徑。包含6種路徑類型的Dubins路徑集可以表示為S={LSL,LSR,LRL,RSR,RSL,RLR},最短路徑可以表示為

P(ps,pe)=

(3)

式中：ps=[xs,ys,θs]表示Dubins曲線的起始點;pe=[xe,ye,θe]表示Dubins曲線的終止點；L為左旋轉圓弧；R為右旋轉圓弧；S表示直線段。RSL類型的路徑示意圖如圖1所示。

1.2.2 固定翼無人機安全飛行走廊

在民航領域,國家統一規劃了飛機的航路圖,客機需要依據地面和機載的導航定位設備,按照預先設定的航路飛行。航路是具有上下限高度和寬度,類似于走廊的飛行通路,本文借鑒民用領域的相關規定,提出了基于Dubins曲線的飛行走廊定義方法。

如圖2所示,假設飛行軌跡起點為ps,局部終點為pe′,連接ps與pe′,定義飛行走廊的左寬度為Dl,飛行走廊的右寬度為Dr(飛行走廊的左右寬度如圖中藍色線段所示),另外根據任務需求,在飛行走廊的兩側增加安全閾值δ,飛行走廊的寬度最終可以定義為

Dw=Dl+Dr+2δ

(4)

圖2 基于Dubins曲線的飛行走廊示意圖

1.3 安全飛行評價函數

一般地,為躲避障礙物而進行飛行航跡重規劃時,障礙物規避路徑偏離原始路徑的程度越大,任務執行效率越低而飛行的安全性越高;相反,任務執行效率提高而飛行安全性卻降低了。本節的目的是設計一個能在任務效率和飛行安全性之間進行權衡的多參數評價函數,在保證飛行安全的前提下盡可能地靠近原始路徑。評價函數由障礙物與飛行走廊距離、局部終點偏移距離、未來航跡與障礙物距離三部分構成。

1) 障礙物與飛行走廊距離:假設計算得到的路徑局部終點為pe′=(xe′,ye′,θe′),連接起點ps和局部終點pe′,得到一條滿足(5)式的直線。

(ye′-ys)x+(xe′-xs)+(xe′ys-ye′xs)=0

(5)

障礙物與飛行走廊之間的距離Di可以由(6)式計算得到

Di=

(6)

2) 局部終點偏移距離:飛行航跡的局部終點與原始終點的距離可以表示為

Dpe′,e=‖pe′-pe‖

(7)

3) 未來航跡與障礙物距離:未來路徑與障礙物的最小距離可以由(8)式計算得到

(8)

綜上所述,基于Dubins飛行走廊的避障優化函數可以設計為

(9)

式中：[η1η2η3]為權重向量,用戶可以根據具體的任務場景及需求設定相應的權重；Ro,i表示下標為i障礙物的半徑。

基于以上定義,無人機的障礙物規避問題就可以描述為

(10)

2 基于蒙特卡洛采樣的數值優化算法

2.1 DBSCAN聚類算法

為了生成無碰撞的路徑,要先確定感知范圍內的可飛行區域和不可飛行區域。考慮到空間中的障礙物可能很多,為了減小蒙特卡洛采樣法的搜索空間,本文采用DSBCAN算法對感知范圍內的所有障礙物進行空間聚類處理。DSBCAN算法根據障礙物之間的距離等評價指標來定義障礙物之間的密度,當感知范圍內的障礙物滿足相應的密度要求時,將相應的障礙物區域視為空間障礙簇,即為不可飛行區域。

假設無人機與障礙物之間的最小安全距離為Dmin,當障礙物的距離滿足

‖po,i-po,j‖≥2Dmin+Ro,i+Ro,j

(11)

則說明無人機能從下標為i和j的障礙物之間穿過,其中Ro,i和Ro,j表示下標為i和j障礙物的半徑。因此,DBSCAN算法的參數可以設置為：

1)Rmin=2Dmin+Ro,i+Ro,j,Rmin為最短領域半徑。

2)Pmin=1;Pmin表示領域內障礙物的最少個數。

2.2 蒙特卡洛采樣算法

局部終點的計算,即評價函數的求解問題。由2.1節可知,評價函數是不可導且不具有解析形式的,評價優化函數的解析解也就不能顯式求得,蒙特卡洛采樣法是一種具有概率完備性的、基于統計采樣的近似推理方法,它是求解近似解的有力方法,因此本文采用蒙特卡洛采樣法來近似逼近評價優化函數的最優解。

圖3 蒙特卡洛采樣區間

如圖3所示,蒙特卡洛采樣法只能在傳感器探測范圍且滿足Dubins曲線模型的限制區域內采樣(即圖中藍色虛線部分),采樣獲得一組二維坐標隨機采樣點:{(x1,y1),(x2,y2),…,(xnmc,ynmc)},其中nmc是隨機采樣點的數量。由此,優化問題轉化成

(12)

總結以上步驟,完整的避障路徑規劃算法執行步驟如表1所示。

表1 避障路徑規劃算法執行步驟

3 算法仿真驗證

3.1 密集障礙環境中的無人機避障仿真

本節在MATLAB R2017b中總計進行了100次無人機避障仿真,其中前50次均采用同一環境和無人機參數進行重復實仿真,后50次采用另外一組環境和無人機參數進行對照,驗證該算法在不同環境中的避障效果。在2個仿真環境中隨機生成多個障礙物,并且在2個環境中無人機可飛行區域內隨機確定無人機的初始位置與目標位置。首先人為設置無人機任務路徑的點集,再基于任務路徑點集,通過Dubins算法生成一條滿足固定翼無人機飛行約束的光滑曲線,獲得的曲線作為無人機的任務路徑。接下來設置本文所提避障算法的參數,前50次仿真為一組,后50次仿真為另一組,參數如表2所示。

表2 仿真參數設置

每進行一次仿真就會進行若干次蒙特卡洛采樣,每次蒙特卡洛采樣會采集到若干個位于采樣區域的點,然后根據這些采樣點得到最優局部終點。將前50次仿真中所有相對應的局部最優點進行均值化,得到第一組數據;同理,另一組環境下將得到第二組數據。下面對這2組不同環境中的數據繪制圖像進行具體分析。

第一組的仿真結果如圖4所示。圖中隨機散落著一些障礙物點,紅色曲線代表預設的任務路徑,無人機在跟隨任務路徑飛行的過程當中,傳感器不斷對環境進行感知,若感知到障礙物,無人機則會依據本文提出的算法進行路徑規劃。規劃的路徑如圖4a)中黑色曲線所示,原始任務路徑中被替代的曲線段由圖中綠色曲線所示。從圖4a)中還可以看出,當規劃的避障路線中仍有障礙物時,算法對路徑進行了二次重規劃。最終無人機完成飛行任務時的飛行路線如圖4b)中藍色曲線所示。

圖4 第一組仿真結果

第二組仿真結果如圖5所示。和第一組仿真一樣,無人機在隨機散落障礙物的地圖中成功地規劃出一條從起點到終點,規避任務路徑中所有障礙物的避障路線。以上仿真結果都驗證了本文所提出算法的有效性。

圖5 第二組仿真結果

在上面2組實驗中,無人機都能避開規劃航跡上的障礙,若無人機的任務路徑點上沒有障礙物,則得到一個經過所有無人機任務路徑點集的最短路徑;若有個別路徑點與障礙物重疊,則根據上述算法繞開障礙再向下一路徑點飛行。

3.2 基于MPC軌跡跟蹤的固定翼無人機障礙規避仿真

考慮到無人機在實際飛行中很難實現對航路的零誤差跟蹤,本節以模型預測控制為軌跡跟蹤控制器,模擬無人機對目標路徑的跟蹤效果,以進一步驗證本文所提出算法的實用性。

設置起始狀態為:ps=[1 200 m,200 m,235°],目標狀態為:pe=[800 m,2 700 m,235°],利用3.1節中描述的任務路徑生成方法生成任務路徑。設置無人機最小轉彎半徑ρ=250 m,傳感器探測角度θ=30°,傳感器探測距離為d=1 000 m,障礙威脅評價函數權重η=[η1,η2,η3],分別為航線偏離程度、局部終點偏移距離、未來航跡與障礙物距離的權重。

無人機在按任務路徑飛行時,如果沒有探測到障礙物,則無人機通過模型預測控制器以任務路徑為參考路徑實行跟蹤控制;如無人機探測到障礙物,則由本文提出的算法進行路徑規劃以躲避障礙物,此時無人機通過模型預測控制器以規劃的避障路徑為參考路徑進行跟蹤。仿真結果如圖6所示,圖中藍色路徑為無人機的任務路徑,黑色路徑為無人機實時參考的路徑,紅色路徑為無人機最終飛行的路徑。

圖6 無人機跟蹤軌跡

由圖6可以看出無人機在任務起初階段未探測到障礙物,以目標路徑為參考路徑進行跟蹤飛行,當探測到障礙物時對障礙物進行聚類歸并(歸并的障礙物由相同的顏色表示),并由上文提出的路徑規劃算法計算出一條避障路徑,此時無人機以避障路徑為參考路徑進行實時跟蹤,當無人機實現對障礙物的規避之后,無人機又以任務路徑為參考路徑進行跟蹤飛行。由仿真結果還可以看出在跟蹤目標路徑時,模型預測控制器的跟蹤誤差較小,但是在跟蹤避障路徑時,由于無人機機動性的約束,存在較大的跟蹤誤差。從飛行過程整體上看,無人機實現了對任務路徑的跟隨,并且成功規避了任務路徑上的障礙物。仿真結果進一步證明了本文所提出算法的有效性。

在圖7的仿真結果中展現了蒙特卡洛采樣的過程。蒙特卡洛采樣次數設置為100(采樣點在圖中用黑色圓圈表示),其余參數設置與本節仿真參數設置類似。

圖7 蒙特卡洛采樣示意圖

上述2次仿真設置了不同的障礙威脅評價函數權重向量,仿真結果圖7a)中權重向量η=[0.3,0.3,0.4],仿真結果圖7b)中權重向量η=[0.2,0.6,0.2],可以看出蒙特卡洛采樣點通常位于采樣區域邊緣的位置,通過改變權重向量,可以改變最優路徑點在采樣區域中的分布趨勢。多次仿真實驗后,設置不同的權重向量影響無人機避障過程中對于航線偏離程度、局部終點偏移距離、未來航跡與障礙物距離3個部分。取場景1中的一個避障過程,分析不同權值對局部終點偏移距離、未來航跡與障礙物距離的影響，結果如表3所示。

表3 權值向量設置

上述仿真中,航線偏離程度隨著局部終點偏移距離的減小而減小,未來航跡與障礙物距離的變化與之相同。即當權重值η2越大,局部最優路徑點傾向于靠近障礙物,這與局部終點偏移距離越小,飛行成本越低的事實相吻合;當η1和η3參數占比越大,局部最優路徑點傾向于遠離障礙物,這與障礙物與飛行走廊距離、未來航跡與障礙物距離越遠,飛行安全越高的事實相吻合。通過以上仿真結果與分析說明了障礙威脅評價函數設置的有效性。針對危險系數較高的障礙物,需要盡可能地遠離,并且要提前進行規避,這種情況下通常將權重值η2設置小一點,使得無人機避障軌跡的局部最優路徑點遠離障礙物;對于危險系數低或者半徑較小的障礙物,出于減少飛行航程節省燃料等因素,趨向于將權值η2設置大一點,使得無人機盡可能靠近障礙物繞行。另外仿真中蒙特卡洛的采樣細節如圖8所示。

圖8 蒙特卡洛采樣細節圖

4 結 論

針對固定翼無人機避障問題,提出了一種基于安全飛行走廊的密集障礙避障算法。首先建立了固定翼無人機的運動學模型與軌跡模型、給出了無人機安全飛行走廊的定義、構建了考慮無人機飛行成本、飛行安全,針對安全飛行走廊的障礙威脅評價函數,并將無人機的避障問題轉化為目標點的求解問題;然后針對障礙物密集造成計算復雜的問題,通過DBSCAN算法將滿足條件的障礙物聚類,減小計算的復雜程度,再通過蒙特卡洛采樣法逼近無人機避障路徑局部最優終點,基于無人機的當前位置與避障路徑的局部最優終點通過Dubins曲線規劃出固定翼無人機障礙規避路徑,最終使得避障路徑盡可能靠近任務路徑,將飛行軌跡限制在一個飛行走廊當中。

本文所設計的算法能夠讓無人機在復雜密集多障礙環境中規劃出一條滿足無人機機動性約束的、綜合考慮無人機飛行安全與飛行效率的飛行路徑,對于安全性要求嚴苛的固定翼無人機而言更加安全。另外能夠通過調節障礙威脅評價函數中的權重,對無人機避障路徑進行有偏向性的調節,使規劃的避障路徑更好地與任務目的相匹配。