基于Pythagorean Hodograph曲線的無人機路徑規劃方法

李大林，李 杰，楊東曉

LI Da-lin,LI Jie,YANG Dong-xiao

（北京理工大學 機電學院，北京 100081）

0 引言

無人機路徑規劃一直是該領域的一個研究熱點，大多數的路徑規劃方法（參考文獻[1,2]）是通過一些算法將目標區域進行劃分，找出節點，無人機沿著節點飛行至目標點。在節點處對其局部路徑進行平滑化，使其滿足無人機最小轉彎半徑的需求。此種方法僅在節點處考慮了飛行器的最小轉彎半徑的約束，沒有從整體路徑上考慮。因此，路徑的曲率變化可能相差比較大，在實際應用中則表現為真實飛行路徑與規劃路徑間的誤差較大。文獻[3,4]提出使用Dubins路徑對無人機進行路徑規劃，雖然Dubins路徑作為前向飛行器所能達到的最短路徑，其曲率同樣是不連續的，此路徑僅優化了路徑長度，并沒有控制系統需求的路徑曲率連續性。

Pythagorean—Hodograph (PH)曲線，即勾股速端曲線，是一種特殊的多項式參數曲線，1990年由Farouki等在研究等距曲線的過程中提出[5]。PH曲線各坐標分量導數的平方和恰好為一個完全平方式.由于具有特殊的代數結構，PH曲線與一般的曲線相比具有重要的計算優勢。例如，PH曲線的弧長可以表示成參數的多項式函數，因而可以被精確地計算出來。PH曲線的等距曲線是相對低階的有理曲線。PH曲線具有精確的弧長和等距線表示，即當它們是多項式或有理參數曲線時，相應的弧長函數、等距線等也是多項式或有理的。

本文提出了一種基于Pythagorean—Hodograph（PH）曲線的路徑規劃方法，此方法將產生具有連續曲率的無人機飛行路徑。應用遺傳算法對PH路徑、路徑曲率標準差以及兼顧二者的情況進行優化，在無障礙平面內得到最短PH路徑、曲率標準差最小路徑以及兼顧上述二者的Pareto最優路徑。

1 PH曲線

曲線r (t)的長度為：

公式（1）中根號內為速度的平方和，如果根號內的項可以表示為多項式的平方，記為σ (t)2，那么式（1）可轉換為：

其中，u (t)和v (t)為正素多項式，w (t)＝1，并且σ (t)為n-1次多項式。

由（3）式可知，PH曲線的參數速率的求解問題可轉換為多項式求根問題。曲率為有理形式。

2 基于PH曲線的飛行路徑構造

由于PH曲線獨特的優點，提供有效可靠的方法來構造、分析和控制PH曲線是非常必要的。因為PH曲線的定義包含多項式u(t)、v(t)等的乘積與平方，所以決定這些多項式的系數使其滿足給定的離散數據(點、切向量等等)，必然引起具有多解的非線性問題。

能體現PH行為的曲線至少為三次PH曲線。因為三次PH曲線沒有拐點，所以在設計應用中受到限制。與經典三次曲線具有類似的形狀復雜度的是五次PH曲線。因此，這里使用五次PH曲線對無人機進行路徑規劃（以下簡稱為PH路徑）。起始點和目標點位置分別為(xs，ys)和(xf，yf)，對應的方向角分別為θs和θf，這些參數可作為路徑的邊界值。

為使PH路徑具有數值穩定性，這里將其寫成Bézier形式。N階的多項式公式其Bézier形式為：

五次PH曲線能插值任意一階Hermite數據。在路徑的起始點以及目標點坐標和方向已知的前提下，使用一階Hermite插值。帶入t=0和t=1的位置坐標，形成路徑的一階微分，控制點b0，b1，b2，b3，b4，b5可由以下公式計算得出：

曲率計算公式如下：

給定控制點參數的PH曲線以及曲率變化如圖1、圖2所示。基于PH曲線構造的PH路徑與Dubins路徑的比較如圖3所示。

圖1 PH曲線

圖2 PH曲線曲率變化

圖3 PH路徑與Dubins路徑比較

現在，問題轉換為找到滿足PH條件的參數θ1、θ3、n1、n2、n3和 n4值構造適合的 PH 曲線。

3 基于遺傳算法的PH路徑優化方法

基于以上構造算法，下面進行優化處理。在起始坐標、起始方向、終點坐標以及終點方向已知的情況下，在曲率滿足最大曲率限制的條件下，確定6個相關參數對以下三個目標進行尋優：

1） 優化PH路徑，使其路徑長度最短；

2）優化曲率的標準差，使其路徑曲率變化最小；

3）同時優化以上兩個目標，找到滿足以上兩個目標的Pareto最優解。

目標函數分別為：

1）min L(r)即min L(θ1,θ3,n1,n2,n3,n4)

2）min E(k)

3）min (L(r)∪E(k)

約束條件：

由于參數較多，在一定精度范圍內使用窮舉法對此問題進行求解，運算量巨大。遺傳算法則可大大提升求解此問題的效率。

3.1 遺傳算法

Holland創建的遺傳算法（GA）是一種概率搜索算法，在許多領域中得到了大量的應用[6,7]。GA可以在可行解決方案的決策狀態空間進行有效的搜索。該方法包括可行解種群的迭代操作，稱為染色體，來獲得更好解的種群。GA染色體編碼是優化過程的重要組成部分。該算法包括以下步驟：1）初始化——產生初始種群；2）適應度——評估種群中每個染色體的適應度；3）解算——滿足停止條件則停止，否則繼續；4）得到新的解——通過遺傳算子生成新的候選染色體，從而模擬進化過程；5）替代——用新的染色體替換就得染色體；6）循環——回到步驟2）。

本文采用二進制編碼對優化參數（θ1,θ3,n1,n2,n3,n4）進行編碼，編碼長度分別為θ1,θ3為十位，精度為π/211。n1,n2,n3,n4分別是二十位，前十位為x軸方向對應的參數，后十位為y軸方向對應的參數。精度為500/211。本文使用的是單點交叉算法。對交叉概率pc和變異概率pm設計如下：

其中，ζmax為群體中個體適應度的最大值。ζavg表示逐代適應度的平均值。ζ'為兩個交叉個體的適應度的較大值。ζ為變異個體適應度的值。

3.2 仿真分析

本文針對初始位置為（0，0）、終點位置為（100km，20km）、初始方向和終點方向均為-90度的情況進行仿真。

3.2.1 最短PH路徑

不考慮曲率方差最短PH路徑仿真結果如圖4、圖5、圖6所示：

圖4 最短路徑

圖5 最短PH路徑的曲率變化

圖6 計算PH最短路徑的遺傳算法收斂曲線

圖7 最小曲率標準差情況下的PH路徑

圖5表明了構造的最短PH路徑符合無人機最小轉彎半徑的約束，即此路徑為可飛路徑。圖6表明了構造的最小PH路徑長度收斂在102.8km，具有最短路徑特征的Dubins路徑長度為102.77km。根據多種情況下的仿真，這里就不一一列舉，最短PH路徑的長度相比最短Dubins路徑長度長出2%。

3.2.2 穩定PH路徑

考慮到控制系統的穩定性，要求PH路徑的曲率變化應相對平穩，這里用曲率的標準差來表征曲率的變化程度，曲率標準差計算公式如式11所示。

針對曲率標準差的PH路徑仿真結果如圖7、圖8所示。

穩定PH路徑僅僅對曲率標準差進行了優化，因此其路徑長度較長，僅僅滿足控制穩定的需求。

3.2.3 穩定最短PH路徑

同時對路徑長度以及曲率的標準差進行尋優，屬多目標優化問題。對于求解多目標優化問題的Pareto最優解，本文主要使用并列選擇法進行求解。先將種群中的全部個體按子目標函數的數目均等地劃分為一些子種群，各自選擇出一些適應度高的個體組成一個新種群，然后分別進行交叉和變異運算，然后將子種群進行隨機合并成一個完整的新種群，如此不斷地進行“分割—并列選擇—合并”操作，最終可求出多目標優化問題的Pareto最優解[8]。

圖8 最小曲率標準差情況下的曲率

圖9 多目標路徑收斂曲線

圖10 多目標曲率標準差收斂曲線

圖11 兩目標優化后的PH路徑

圖12 兩目標優化后的曲率

圖13 多目標路徑收斂曲線2

圖14 多目標曲率標準差收斂曲線2

圖15 兩目標優化后的PH路徑2

圖16 兩目標優化后的曲率2

如圖9～圖16所示，穩定最短PH曲線存在多種情況，Pareto最優解是一個最優解集它是由那些任一個目標函數值的提高都必須以犧牲其他目標函數值為代價的解組成的集合。

4 結論

本文提出了一種基于PH曲線的無人機路徑優化規劃方法，該方法使用遺傳算法對基于PH曲線的無人機路徑進行了優化，在無人機最小轉彎半徑（即曲線曲率的倒數）的約束下，優化的內容包括對最小化PH路徑的長度單目標優化、最小化曲率標準差單目標優化、同時最小化PH路徑和曲率標準差的多目標優化。仿真結果表明使用本方法可以為無人機提供一種曲率連續且標準差最小，且路徑相對較短的飛行路徑。

[1] 溫瑞,王航宇,謝君,一種移動機器人路徑規劃方法[J]. 兵工自動化,2009(12)： 60-63.

[2] 畢慧敏,董海鷹. 改進遺傳算法在機器人路徑規劃中的應用[J]. 兵工自動化,2006(4)： 53-54+66.

[3] Savla,K.,F. Bullo and E.Frazzoli. The coverage problem for loitering Dubins vehicles[C].Decision and Control,200746th IEEE Conference.2007. p. 1398-1403.

[4] Chao,Y. and E.J. Barth,Real-time Dynamic Path Planning for Dubins'Nonholonomic Robot[C].Decision and Control,200645th IEEE Conference.2006. p. 2418-2423.

[5] 王琦魁,陳友東,李偉等. 基于PH曲線的數控拐角過渡方法[J]. 航空學報,2010(7)： 1481-1487

[6] 嚴浙平,黃宇峰,李鋒. 遺傳算法在AUV局部路徑規劃中的應用研究[J]. 應用科技,2009(2)： 46-51.

[7] 徐劍,周德云,黃鶴. 基于改進遺傳算法的多無人機路徑規劃[J]. 航空計算技術,2009(4)： 43-46.

[8] 雷英杰,張善文,李續武等. MATLAB遺傳算法工具箱及應用[M]. 西安： 西安電子科技大學出版社. 2005.