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

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

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

0 引言

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

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

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

1 基于PH 曲線航跡規(guī)劃

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

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

2 PH 路徑篩選

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

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

圖1 PH 曲線篩選示意圖

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

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

3 多路徑合成

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

3.1 貪心算法路徑選擇

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

3.1.1 貪心選擇策略

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

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

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

圖2 貪心選擇策略示意圖

3.1.2 最優(yōu)子結構

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

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

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

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

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

2）使用動態(tài)優(yōu)化方法調整單條曲線參數(shù)，控制PH 曲線曲率與長度；

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

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

圖3 多路徑組合規(guī)劃方法流程圖

4 實驗仿真

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

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

圖4 經(jīng)典PH 路徑規(guī)劃

圖5 組合PH 路徑規(guī)劃

表1 算法結果對比

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

圖6 經(jīng)典PH 路徑二次規(guī)劃

圖7 組合PH 路徑二次規(guī)劃

表2 二次規(guī)劃算法結果對比

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

5 結論

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