基于改進PH 曲線的無人機路徑規劃方法*

張建東，車冰凌?，楊啟明，郭 操

（1.西北工業大學電子信息學院，西安 710072；2.沈陽飛機設計研究所，沈陽 110035）

0 引言

隨著作戰任務復雜度的提高，提升無人機自主作戰能力成為當前研究重點，對于無人機航路規劃方法的研究已經形成了不少較為成熟的方法。例如：A*搜索算法［1］、圖搜索［2］、動態規劃法、人工勢場法等。這類方法通常缺乏對無人機機動性能約束，忽略了航跡的整體平滑和曲率約束的需求。

為了能讓所規劃的路徑能夠平滑可飛，應將適用于無人機飛行的曲線直接應用于無人機的路徑規劃中。文獻［3］將Dubins 路徑應用于智能車路徑規劃問題，結合PID 控制理論對路徑曲線進行控制；文獻［4-10］將PH 曲線用于路徑規劃，分別結合模擬退火算法（SA）、遺傳算法（GA）、粒子群算法（PSO）、蟻群算法（ACO）、人工蜂群算法（ABC），調節曲線參數，獲取一條符合安全性、機動性約束的最優PH 路徑。但在實際復雜戰場環境中，障礙威脅并不總是易于發現的，僅僅將一條PH 曲線作為全局規劃航路，難以靈活應對突發危險，并且每更改一處障礙都可能導致整條航跡重新規劃，是對無人機資源的浪費。

本文針對現有無人機路徑曲線規劃的不足，在有威脅障礙的復雜環境中，基于改進的PH 曲線，提出一種組合多段PH 路徑的無人機航跡規劃方法，能夠靈活應對復雜突發威脅，實現避障的同時，保證無人機航跡的曲線、曲率連續性。

1 基于PH 曲線航跡規劃

路徑規劃的目的是得到一條安全的飛行路線。由于無人機的機動特性約束，要求規劃航跡必須滿足航跡曲線連續以及航跡曲率連續。PH 曲線是具有有理特性的參數化曲線，具有彎曲能量小，曲率和長度均有閉合解等優勢，能夠滿足曲率連續變化的約束［11］。具有連續曲率的PH 曲線［12］可以應用于無人機路徑規劃。對于一般路徑曲線，以q 為參數的路徑r（q）的長度可寫成如下形式：

PH 路徑曲線由于矢端矢量滿足勾股條件，路徑長度具有閉合解。因此，對路徑長度的求解可以簡化為對多項式的積分：

2 PH 路徑篩選

曲線能量計算過程較為繁瑣，為簡化篩選流程，本文提出一種通過比較PH 曲線與對應Dubins 路徑的形狀一致性的方法，實現對PH 路徑曲線的篩選。已知兩位姿點之間的最短路徑是Dubins 路徑［16］，因此，曲線形狀最接近Dubins 路徑，則可以認為具有最短路徑長度的優勢。Dubins 路徑可以簡單定義為：在最大曲率限制下，平面內兩個位姿點之間的最短可行路徑是CLC 路徑或者CCC 路徑，或者是它們的子集，其中C 表示弧段，L 表示與C 相切的直線段，不失一般性，本文中僅考慮CLC 路徑。

對于構造PH 曲線，任意兩個位姿點，都可以出現4 種可行解，通過解析幾何方法，可以構造出4 種Dubins 路徑曲線，從中選出最短路徑如圖1 所示：

圖1 PH 曲線篩選示意圖

其中，θs表示第1 個C 弧段夾角，θf表示第2 個C 弧段夾角，bi為PH 曲線控制點，選取PH 曲線與Dubins 路徑形狀具有一致性。

無人機執行任務時，可以認為無人機始終朝向任務目標點方向運動，路徑規劃中不會出現超過180°大的轉彎角度。在初、末轉彎半徑近似相等的情況下，要獲得最短Dubins 路徑，盡可能選擇C 弧段對應角度最小的路線。在機動條件下，可以驗證本文所提出關于PH 路徑篩選方法的正確性。

3 多路徑合成

復雜戰場環境中進行航路規劃［17-19］，既要滿足安全性、機動性約束、路徑最短，還要盡可能保證規劃航路具有靈活性、實時性，僅由一條PH 曲線作為全局規劃航路顯然是不合理的，因此，有必要針對復雜環境進行區域劃分，對多段PH 曲線進行合成，以獲得一條滿足要求的規劃航路。

3.1 貪心算法路徑選擇

貪心算法（greed algorithm），基于在每一步作出最佳選擇，獲取局部最優解，通過每一步的最優選擇可以得到全局最優解。

3.1.1 貪心選擇策略

由兩位姿點可以得到一條最短的Dubins 路徑，存在兩種情況如圖2 所示，分別對應以下兩種策略：

1）當規劃路徑不與障礙物相交，則使用當前路徑；2）當規劃路徑與障礙物相交，將路徑拆分，得到新的兩條Dubins 路徑，組合成為新路徑。

獲取新路徑原則：由兩條路徑構成的新路徑與原始路徑需位于障礙圓的同側，由上一節關于最短Dubins 路徑篩選方法的討論，經過障礙圓的弧段部分對應的夾角最小，則路徑最短。貪心選擇策略得到的兩段路徑組合是最短路徑，滿足單步最優條件。

圖2 貪心選擇策略示意圖

3.1.2 最優子結構

采用自下而上（或者自上而下）單向尋優。假設初始路徑中不存在障礙圓，得到一條Dubins 路徑，之后按照由近及遠的順序加入障礙圓，每加入一個障礙，按照貪心選擇策略進行擇優，然后將原問題轉變具有同樣性質的子問題，從新的起始位姿點到終點的路徑尋優。不斷作出貪心選擇，最終得到全局最優組合結果。

3.2 PH 多段路徑合成改進算法

通過貪心策略可以得到一系列位姿點，從而獲得多段PH 路徑，若要得到全局路徑，需要將其組合。如果直接將曲線首尾相連，得到的組合路徑曲線連續，曲率不連續。為了解決這個問題，本文對PH 曲線方法進行改進：

對于5 次PH 多項式，由式（6）可以發現，r（q）具有對稱性，曲率κ 取決于r（q）的一、二階導數，當q=1，一階導數值只與參數b4，b5有關，二階導數值只與參數b3，b4，b5有關；當q=0，一階導數值只與參數b1，b0有關，二階導數值只與參數b2，b1，b0有關。因此，連續兩條PH 曲線，只要前一條曲線的參數b3，b4，b5與后一條曲線的參數b2，b1，b0滿足對稱關系，就可以保證多段曲線連接點處曲線、曲率連續。改進算法步驟如下：

1）給出第i 組初、末位姿點，構造PH 曲線；

2）使用動態優化方法調整單條曲線參數，控制PH 曲線曲率與長度；

3）給出第i+1 組初、末位姿點，首先構造PH 曲線，然后將控制參數b2，b1替換為第i 組位姿點曲線參數b3，b4的對稱點，即可得到修正后的曲線；

4）重復步驟1）、2）、3）直到得到完整曲線。

圖3 多路徑組合規劃方法流程圖

4 實驗仿真

設置無人機戰場環境空域大小為100 km*50 km，對本文提出的無人機航路規劃方法在MATLAB 環境下進行仿真驗證。設定無人機初始位姿點，末位姿點，轉彎半徑不小于1 km，障礙圓半徑均為5 km。

仿真實驗：設定復雜戰場環境中有5 個威脅區已知，無人機初始點坐標（0，0），終點坐標（80，40），初、末運動方向角均為0°。分別對于經典PH 路徑規劃方法與本文提出的方法進行對比。實驗結果如圖4、圖5 所示：

圖4 經典PH 路徑規劃

圖5 組合PH 路徑規劃

表1 算法結果對比

考慮到復雜戰場中會出現突發障礙，在仿真環境中添加黑色障礙圓作為突發障礙，將重規劃之后的結果進行對比。實驗結果如圖6、圖7 所示：

圖6 經典PH 路徑二次規劃

圖7 組合PH 路徑二次規劃

表2 二次規劃算法結果對比

由仿真結果可知：在已知威脅戰場環境中，采用經典PH 曲線方法進行航路規劃，需要對所有障礙進行規避；采用智能算法調節參數過程長、計算量大、耗時長，而采用本文中提出的多段PH 路徑組合方法，將每一段的PH 曲線構造過程大大簡化，每一段至多考慮一個障礙，路徑規劃時長大大縮短。在算法對環境變化的動態響應方面，當環境中出現突發威脅，傳統方法需要重新進行計算，規劃全局路線，耗時較長；基于本文方法，出現突發障礙，僅需要將與該障礙相交的路線進行拆解，其余各段曲線不需要大幅度調整，航路規劃時長大大縮短，具有很強的靈活性以應對復雜戰場要求。

5 結論

本文提出了一種基于改進PH 曲線的無人機航路組合路徑規劃算法。利用PH 曲線平滑、曲率連續以及多項式特性，實現將多條單一PH 路徑進行組合，結合貪心策略的快速尋優特性，能夠快速高效地獲取路徑參數，以較少的時間代價、計算代價生成得到一條滿足無人機運動學約束的全局路線。仿真驗證表明本文提出的多路徑組合規劃方法，可以充分發揮PH 曲線的特性，規劃效率和動態響應效果相對經典PH 算法有了很大提高。