一種基于幾何關系的無人機航跡規劃方法

張 敬,李 彤,楊 鈞,朱得糠,張士峰

(1.國防科技創新研究院 無人系統技術研究中心, 北京 100071; 2.國防科技大學 空天科學學院, 長沙 410073)

0 引言

近年來,隨著商用無人機產業的崛起,無人機技術得到了快速發展與迭代,無人機制造成本不斷下降,應用范圍也更加廣泛,涉及了偵查探測、植保安防和巡檢測繪等大量軍用及民用領域,并逐漸向智能化應用方向拓展[1]。

無人機飛控計算機通?；贏RM或PowerPC架構進行開發,并且伴隨芯片技術的不斷提升,飛控計算機算力已非常強大,飛控相關技術由于開源化也相對成熟。然而面臨復雜任務,無人機機載計算機一般采用任務計算機和飛控計算機組合的形式,任務計算機負責上層任務規劃、深度感知、策略制定和集群通信等計算復雜度高而實時性要求低的工作,飛控計算機僅面向傳統導航制導控制以及底層硬件驅動等計算復雜度低但實時性要求高的功能。這種形式使得飛控計算機算力在一定程度上有所浪費,并且考慮到無人機未來小型化和低成本化的發展趨勢,輕小型無人機從載重和體積上都難以搭載任務計算機,因此,針對常規任務算法的優化對于拓展輕小型無人機任務功能十分必要。

無人機在飛行任務執行過程中,通常會面臨較多地形地勢障礙或拒止威脅區域,因而無人機航跡規劃成為任務安全和飛行可靠的重要保障。航跡規劃作為無人機規避障礙和威脅的必要技術,得到廣泛的研究與關注,并且形成了較多成熟的算法,主要包括基于全局動態搜索的方法如Dijkstra算法[2-3]、A*算法[4-6]、Voronoi圖法[7-9]和快速擴展隨機樹[10-12]等,基于啟發式學習的方法如遺傳算法[13-14]和粒子群算法[15-16]等,以及基于數理關系的方法如人工勢場法等[17-20]。其中,A*算法和人工勢場法在工程中應用較為廣泛。A*算法優化性能較好,廣泛應用于全局路徑規劃,但是類似于其他全局動態搜索算法和啟發式學習算法,規劃空間均需要通過地圖柵格化、離散化或者重建路徑的方式進行全局化建模,因此存在內存占用大、搜索時間長等問題,僅適于靜態規劃。人工勢場法是一種動態連續算法,它的基本思想是把環境根據目標和障礙建模為虛擬力場,目標點產生引力,障礙點產生斥力,從而引導對象遠離障礙點向目標點運動,但該算法也存在一些缺點,如當目標點附近有障礙點時,斥力遠遠大于引力,規劃對象將很難到達目標點,又或當對象在某一點的引力和斥力剛好大小相等時,將陷入局部最優點。

由此,為充分利用飛控計算機算力,同時提高算法效率和保證算法性能,本研究中提出了一種基于幾何關系的航跡規劃與區域避障算法,能夠避免規劃空間全局離散化表示,大幅降低算法計算時間,有效規避障礙區域,并且支持動態實時上傳障礙區域,在采用航跡點通用表示方法基礎上考慮了無人機轉彎半徑,無需對航跡表示進行平滑處理。

1 問題描述與空間表示

航跡規劃問題是一個綜合考慮多目標多約束的非線性規劃問題[21],其求解復雜度很大程度依賴于問題建模。為提高算法計算效率,本研究中航跡規劃問題主要考慮二維情形下,無人機針對給定障礙和威脅區域,從初始點至目標點的飛行航跡規劃和區域避障。障礙區域采用多邊形點集形式,規劃航跡采用當前普遍應用的航跡點表示方法,同時,為適應無人機飛控計算機算力,實時在線規劃,犧牲一定程度的路徑最優性,以保證算法計算效率。

1.1 坐標表示與變換

由于本研究中無人機航跡規劃算法涉及點間距離角度以及向量運算,因此選擇無人機導航制導常用的北東地坐標系(north-east-down,NED)作為當地坐標系進行表示和計算。在二維坐標條件下,不考慮高程,需將點的經緯度坐標轉換為北向和東向坐標。為提高計算效率,本研究中采用等距方位投影[22]進行坐標變換。方位投影是地圖投影的一種,其以平面作為投影面與地球表面相切或相割,并將球面上的經緯線投影至該平面所形成的一類投影。等距方位投影則是投影中心點至地圖各點的距離和方位角不變,但存在角度變形和面積變形,介于等角和等積投影之間。由于等距方位投影中各點與中心點的距離和方位多是精確的,因此常被用作交通地圖指導航空航海。

在等距方位投影中,選擇NED坐標系原點作為投影中心點,令其經度和緯度分別為λ0和φ0,則對于緯度和經度分別為φ和λ任意一點在NED坐標系中投影坐標(x,y)為

(1)

其中,RE=6371 km為地球平均半徑。

(2)

cosc=sinφ0sinφ+cosφ0cosφcos(λ-λ0)

(3)

相反,將NED坐標系中的點變換至緯度和經度坐標則分別為

(4)

(5)

其中

(6)

坐標變換計算中應注意經緯度單位的轉換。

相比三維坐標變換,投影計算方法顯然計算速度快效率高,同時也能將精度損失保持在可接受范圍內,適于機載飛控算力。

1.2 障礙區域表示與處理

本研究中考慮的二維平面障礙區域采用多邊形點集的形式,按順時針或逆時針順序進行表示,能夠有效擬合不規則區域,并且易于以鏈表或數組等現有數據結構進行存儲或通信。對于障礙區域的多邊形點集表示需作如下假設。

假設1：區域間不存在重疊,即多個區域重疊則按一個連通障礙區域處理。

假設2：不同區域間的間隔距離應超過2倍于無人機轉彎半徑,小于該距離則按一個連通障礙區域處理。

2個假設是為了保證數據合理性和無人機飛行動力學條件下存在有效解。由于算法是基于幾何關系設計,因此在數據預處理方面,對于多邊形障礙區域,還需對凹多邊形區域進行凸化處理,以保證規劃航跡點滿足要求,后文將進一步說明。

障礙區域的凸化處理采用凹點剔除方式。首先,對于多邊形區域的點集中任意點,判斷其是否在其余點所組成的多邊形內,若該點位于其內,則該點為凹點須剔除,或者該點位于該多邊形頂點或邊上時,由于該點為重復或者多余的點也應剔除,反之則為凸點,應保留。遍歷多邊形障礙區域所有點重復該過程,直至該區域每個點均滿足位于其余點所組成的多邊形外,即均為凸點,從而使多邊形障礙區域為凸多邊形,完成區域的凸化處理。

由此,將多邊形凸化處理過程轉變為判斷點是否在多邊形內的問題。該判斷方法采用射線法[23],由判斷點引出一條任意方向射線,若與該區域其余點所形成多邊形相交,且與各邊交點總個數為奇數,則該點位于多邊形內,否則,若射線與該多邊形各邊的交點總個數為偶數,則該點位于多邊形外。射線法的優勢在于可以采用垂直或水平射線,使判斷過程簡化直觀。

凸化處理示意圖如圖1所示,選取多邊形區域某個頂點P作為判斷點,其垂直射線與其余各點所組成多邊形區域相交情況如圖1所示。

圖1 凸化處理示意圖

(7)

障礙區域凸化處理算法偽代碼如下。

算法1：障礙區域凸化處理

輸入:障礙區域點集pointsblock

輸出:凸化處理后的障礙區域點集pointsblock

參數:點在內部標志位flagin,判斷點pointP,其余點所組成多邊形任意連續兩頂點pointA和pointB,假想焦點xC

1:forpointPinpointsblockdo

2:flagin←False

3:foradjacent pointspointA,pointBexceptpointPinpointsblockdo

4:ifpointP==pointA||pointP==pointB||

5: (pointP.y==pointA.y&&pointP.y==pointB.y&&

6: (pointP.x-pointA.x)(pointP.x-pointB.x)≤0)then

7:flagin← True

8:break

9:endif

10:if(pointP.y>pointA.y&&pointP.y≤pointB.y) ||

11: (pointP.y>pointB.y&&pointP.y≤pointA.y&&

12: (pointP.x-pointA.x)(pointP.x-pointB.x) ≤ 0)then

14:ifxC==pointP.xthen

15:flagin←True

16:break

17:endif

18:ifxC>pointP.xthen

19:flagin← !flagin

20:endif

21:endif

22:endfor

23:ifflaginthen

24: removepointPfrompointsblock

25:endif

26:endfor

27:returnpointsblock

注意,經過凸化處理,障礙區域可能一定程度被擴大,對于極端情況下凹度較大的區域可能并不適用。

2 基于幾何關系的航跡規劃與避障

本研究中航跡點采用鏈表或數組數據形式進行存儲,在給定起點和終點的條件下,航跡點初始包含起點和終點。由于任意2個連續航跡點間能夠形成向量,因此基于幾何關系在起點和終點間規劃航跡點,從而對障礙區域進行規避。

2.1 避障判斷與航跡點添加

對于經過凸化處理的多邊形障礙區域,不考慮特殊情況,假設起點和終點均位于障礙區域外,且根據假設2以及設計算法能夠保證規劃添加的航跡點不位于任一障礙區域內,則若已存在兩航跡點間向量被障礙區域阻截,則該障礙區域有且僅有2條邊與該向量相交(不包括與向量重合的邊)。由此,通過對阻截的障礙區域所相交的2條邊進行延伸,延伸方向為所相交向量與多邊形區域距離最遠的頂點所在側的相反方向,延伸距離為2倍規劃對象機動半徑,由此形成2個新航跡點,按照與該相交向量起始航跡點的距離從小至大順序添加至向量2個航跡點之間,從而形成新的規劃航跡,達到避障目的。

根據上文分析,障礙區域的避障問題轉化為向量相交判斷和延伸問題。由于主流編程語言對于向量操作均有較為豐富的方法庫,且運行效率較高,因此,向量相交判斷和延伸能夠通過庫函數進行實現,極大提高計算速度,由此提出基于幾何關系的判斷方法。

(8)

(9)

圖2 避障航跡點添加示意圖

(10)

反之,則向點A和點D方向延伸形成

(11)

其中,rT為規劃對象機動半徑。而后,根據點P和點Q到點S的距離遠近將2個新航跡點依次添加至航跡點S和點T中間,即S→Q→P→T。

避障判斷與航跡點添加算法偽代碼如下:

算法2：避障判斷與航跡點添加

輸入:2個連續航跡點pointS和pointT(鏈表形式),障礙區域點集pointblock

輸出:處理后點S至點T的航跡點集(鏈表形式),航跡點添加標志位flagadd

參數:多邊形任意連續兩頂點point1和point2以及2點距航跡點向量的距離dist1、dist2和與航跡點向量的夾角θS、θT,多邊形兩條相交邊的4個頂點pointA、pointB、pointC和pointD以及A點距航跡點向量距離distA,航跡添加點pointP和pointQ,二次相交標志位flagfirst,最大距離distmax,機動半徑rT

1:flagadd←False

2:flagfirst←True

3:distmax← 0

4:foradjacent pointspointspoint1,point2inpointsblockdo

5:dist1← cross(vector(pointS,pointT),vector(pointS,point1))

6:dist2← cross(vector(pointS,pointT),vector(pointS,point2))

9:ifabs(distmax)

10:distmax←dist1

11:endif

12:ifabs(distmax)

13:distmax←dist2

14:endif

15:ifdist1·dist2≤ 0&&(dist1| |dist2)&&θSθT< 0then

16:ifflagfirstthen

17:flagadd← True

18:flagfirst← False

19:pointA←point1

20:pointB←point2

21:distA←dist1

28:else

22:pointC←point1

23:pointD←point2

24:endif

25:endif

26:endfor

27:ifflagaddthen

28:ifdistmax·distA≥ 0then

29:pointP←pointB+ 2rT· normalize(vector(pointA,pointB))

30:pointQ←pointC+ 2rT· normalize(vector(pointD,pointC))

29:else

31:pointP←pointA+ 2rT· normalize(vector(pointB,pointA))

32:pointQ←pointD+ 2rT· normalize(vector(pointC,pointD))

33:endif

34:ifnorm(vector(pointP,pointS))>norm(vector(pointQ,pointS))then

35:pointP?pointQ

36:endif

37:pointS.next←pointP

38:pointP.next←pointQ

39:pointQ.next←pointT

40:endif

41:returnpointS,flagadd

由此,針對該障礙區域的避障航跡點添加完畢。由于起點和終點位于障礙區域外,且根據假設2,在給定延伸距離為轉彎半徑的條件下,保證了規劃航跡點的可行性和有效性,避免了避障航跡點添加在障礙區域內或無人機在該點機動范圍觸碰臨近障礙區域,因此無需對航跡點進行平滑處理。同時,通過判斷障礙區域所有點與相交航跡向量的最大距離,進行避障航跡點反向延伸添加,實現了無人機針對該障礙區域的最短路徑避障。

2.2 全局航跡生成

根據上文算法設計,通過幾何關系添加避障航跡點的方式完成了對單個多邊形障礙區域避障。因而,針對所有需考慮的障礙區域,按照上述方法對每一個區域遍歷處理,對于與已規劃航跡點相交的任何一個障礙區域,添加避障航跡點后,需對所有區域重新遍歷,以確保區域的遍歷處理順序不對結果產生影響,即所規劃航跡點能夠避開所有區域,從而完成對所有障礙區域的避障。算法整體流程如圖3所示。

根據上述算法流程,針對所有障礙區域生成全局航跡點。值得注意的是,對于所有障礙區域的重復遍歷判斷,雖然在一定程度上降低了計算效率,但相比基于離散地圖的搜索方式,基于幾何關系的計算方法仍能極大減小算法計算時間,同時,重復遍歷也確保了障礙區域能夠實時添加,在前序航跡規劃基礎上動態計算。相對不足的是,由于沒有在全局進行優化,僅針對單個區域考慮最優避障路徑,該方法僅能夠得到次優解,若從最優化方面而言,對該航跡規劃方法選擇應有所取舍。

圖3 算法整體流程圖

3 仿真與驗證

3.1 數值仿真

為驗證算法性能,首先通過數值仿真對算法進行測試分析與對比。本研究中選擇A*算法、快速擴展隨機樹法(rapidly exploring random tree,RRT)、人工勢場法(artificial potential field,APF)作為對比方法,A*算法應用廣泛,且對于靜態規劃效果較好,RRT基于全局地圖采樣,能夠很好處理帶有非完整約束的路徑規劃問題,APF作為動態規劃方法,工程應用廣泛,因此3類對比方法具有普遍性,能夠作為參考驗證算法性能。A*算法采用8朝向移動,為避免與障礙區域碰撞,同時保證可操作性,直線移動距離同樣定為2倍轉彎半徑,斜線移動按對角距離處理,啟發函數采用歐式距離;RRT算法對全局地圖對東西南北各向拓展0.5倍起點至終點距離,并采用2倍轉彎半徑進行采樣;APF算法引力勢場Vs和斥力勢場Vc設計如下

(12)

(13)

通常在無人機飛控系統中,速度和高度控制采用TECS方法,水平航跡跟蹤采用L1算法,因此無法直接接收速度指令,APF算法通過向期望速度方向延伸2倍轉彎半徑形成航跡點方式進行航跡規劃計算為

(14)

數值仿真選擇當地坐標系即NED系下進行,起點和終點的水平坐標分別為(0,0)和(2 900,0),障礙區域設置如表3所示,無人機轉彎半徑設為10 m。數值仿真計算的硬件環境基于Intel i9-9880H的CPU和16 G的DDR4內存,軟件語言基于C++編寫運行。

表3 障礙區域設置

數值仿真對比結果如圖4所示,4種方法的航跡規劃都成功避開了障礙區域,但路徑選擇有區別。算法計算結果如表4所示,包括運行時長和規劃航跡距離。根據圖4和表4,所提出算法大幅降低了計算時間,達到了毫秒級,與A*算法相比雖然不能保證最優性,但基本處于同等優化水平,并且不需要地圖柵格化表示。相較而言,A*算法計算時間較長,RRT由于隨機采樣不僅計算時間過長也無法保證規劃航跡最優性,每次計算時間和航跡均有較大差異,APF算法計算時間同樣達到了毫秒級,但由于規劃航跡在障礙區域邊界振蕩,顯示的航跡距離較大,因此需要對規劃航跡進行平滑。由此可見,所提出算法既達到了較高的計算效率,同時也保證了優化水平,不需要平滑規劃航跡,并且完全能夠適用于無人機飛控算力,具備可行性。

圖4 數值仿真對比結果

表4 算法計算結果

3.2 飛行試驗

根據數值仿真結果,在此基礎上開展飛行試驗,以進一步驗證算法性能。由于RRT算法運行時間和規劃航跡均具有不確定性,APF算法對參數較為敏感,且需要航跡平滑,不具有可靠性,因此飛行試驗僅采用A*算法作為對比。飛行試驗平臺選型如圖5所示,選擇FMS塞斯納EPO泡沫機作為算法測試無人機,硬件選擇HolybroPixhawk4開源飛控,軟件選用PX4v1.11.0版本。該開源飛控平臺基于ARMCortex-M7體系架構處理器STM32F765進行運算,算力充足,并且PX4飛控軟件采用Nuttx操作系統進行任務調度,而航跡規劃算法在實際飛行中運行頻率較低,因此所提出算法能夠移植于該飛控系統。

將所提出算法按照PX4中標準任務模塊改寫,在頭文件中聲明所需變量和類的實現方法,并在系統CMakeLists中進行添加。障礙區域坐標信息通過自定義MAVLink消息實現,每個障礙區域獨立打包,在地面站實時上傳,飛控系統通過任務程序進行接收。該任務程序選擇位置控制模式作為控制變量,由地面站更改該變量值進行激活,激活后無人機按照當前坐標定點盤旋,當地面站更改位置控制模式為自動尋跡模式后,任務程序根據接收到的障礙區域運行更新函數規劃航跡,并通過PX4系統uORB發布,無人機將按發布航跡點進行飛行,尋跡飛行過程中仍可通過更改控制變量激活航跡規劃任務程序,上傳新的障礙區域。

圖5 飛行試驗平臺選型

飛行試驗的障礙區域設置與數值仿真保持一致,以保證天地一致性。飛行開始前僅設定起始點和目標點,障礙區域點均采用經緯高形式上傳。飛行開始后,通過地面站激活飛控系統任務程序,實時上傳4個障礙區域后,更改控制變量為自動尋跡模式,無人機將在航跡規劃完成后由盤旋轉為按規劃航跡點飛行。由于A*算法為靜態規劃算法,計算時間較長,無法在飛控系統中移植,A*算法的飛行試驗僅將離線規劃的航跡點在飛行開始前完成上傳。飛行試驗中,測試無人機飛行速度設定為15 m/s,飛行高度為70 m,航跡點接受半徑為50 m。

飛行試驗對比結果如圖6所示。測試無人機按照所提出算法與A*算法的規劃航跡飛行,均能夠實現避障,滿足性能要求。由于所提出算法的規劃航跡轉彎方向較大,飛行軌跡離障礙區域較近,但由于考慮了無人機轉彎半徑,因而仍能夠避開障礙區域。

圖6 飛行試驗對比結果

從起始點至目標點,所提出算法飛行總用時為217.4 s,A*算法飛行總用時為210.4 s?？梢?由于無人機轉彎影響,所提出算法規劃航跡的最優性在實現上有一定損失,而A*算法的8朝向則更易實現。但是,所提出算法基于實時上傳的障礙區域在線規劃動態計算,達到了實時性和快速性的目的。由此,通過飛行試驗對所提出算法性能進行了驗證,算法滿足可行性和有效性要求,具有較快運行速度和較高計算效率,能夠移植于現有飛控系統。

4 結論

針對無人機二維區域避障提出了一種基于幾何關系的航跡規劃方法,并通過數值仿真和飛行試驗對其性能進行了對比分析和驗證。研究表明:

1) 所提出方法基于幾何關系進行航跡規劃,避免了全局地圖離散化表示,規劃時間遠小于其他類型規劃算法,并且具有較高規劃水平,能夠適用于現有飛控算力,并可靠移植于現有飛控系統;

2) 所提出方法相比靜態規劃類型算法,能夠根據實時上傳的障礙區域在線規劃動態計算,滿足了實時性和快速性要求;

3) 所提出方法規劃航跡考慮了無人機轉彎半徑,經過飛行試驗驗證,能夠避免無人機機動范圍觸碰障礙區域,滿足了有效性和可行性要求。