李大林，李 杰，楊東曉

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

（北京理工大學(xué) 機(jī)電學(xué)院，北京 100081）

0 引言

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

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

本文提出了一種基于Pythagorean—Hodograph（PH）曲線的路徑規(guī)劃方法，此方法將產(chǎn)生具有連續(xù)曲率的無人機(jī)飛行路徑。應(yīng)用遺傳算法對PH路徑、路徑曲率標(biāo)準(zhǔn)差以及兼顧二者的情況進(jìn)行優(yōu)化，在無障礙平面內(nèi)得到最短PH路徑、曲率標(biāo)準(zhǔn)差最小路徑以及兼顧上述二者的Pareto最優(yōu)路徑。

1 PH曲線

曲線r (t)的長度為：

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

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

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

2 基于PH曲線的飛行路徑構(gòu)造

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

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

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

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

曲率計算公式如下：

給定控制點(diǎn)參數(shù)的PH曲線以及曲率變化如圖1、圖2所示?；赑H曲線構(gòu)造的PH路徑與Dubins路徑的比較如圖3所示。

圖1 PH曲線

圖2 PH曲線曲率變化

圖3 PH路徑與Dubins路徑比較

現(xiàn)在，問題轉(zhuǎn)換為找到滿足PH條件的參數(shù)θ1、θ3、n1、n2、n3和 n4值構(gòu)造適合的 PH 曲線。

3 基于遺傳算法的PH路徑優(yōu)化方法

基于以上構(gòu)造算法，下面進(jìn)行優(yōu)化處理。在起始坐標(biāo)、起始方向、終點(diǎn)坐標(biāo)以及終點(diǎn)方向已知的情況下，在曲率滿足最大曲率限制的條件下，確定6個相關(guān)參數(shù)對以下三個目標(biāo)進(jìn)行尋優(yōu)：

1） 優(yōu)化PH路徑，使其路徑長度最短；

2）優(yōu)化曲率的標(biāo)準(zhǔn)差，使其路徑曲率變化最?。?/p>

3）同時優(yōu)化以上兩個目標(biāo)，找到滿足以上兩個目標(biāo)的Pareto最優(yōu)解。

目標(biāo)函數(shù)分別為：

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

2）min E(k)

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

約束條件：

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

3.1 遺傳算法

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

本文采用二進(jìn)制編碼對優(yōu)化參數(shù)（θ1,θ3,n1,n2,n3,n4）進(jìn)行編碼，編碼長度分別為θ1,θ3為十位，精度為π/211。n1,n2,n3,n4分別是二十位，前十位為x軸方向?qū)?yīng)的參數(shù)，后十位為y軸方向?qū)?yīng)的參數(shù)。精度為500/211。本文使用的是單點(diǎn)交叉算法。對交叉概率pc和變異概率pm設(shè)計如下：

其中，ζmax為群體中個體適應(yīng)度的最大值。ζavg表示逐代適應(yīng)度的平均值。ζ'為兩個交叉?zhèn)€體的適應(yīng)度的較大值。ζ為變異個體適應(yīng)度的值。

3.2 仿真分析

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

3.2.1 最短PH路徑

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

圖4 最短路徑

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

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

圖7 最小曲率標(biāo)準(zhǔn)差情況下的PH路徑

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

3.2.2 穩(wěn)定PH路徑

考慮到控制系統(tǒng)的穩(wěn)定性，要求PH路徑的曲率變化應(yīng)相對平穩(wěn)，這里用曲率的標(biāo)準(zhǔn)差來表征曲率的變化程度，曲率標(biāo)準(zhǔn)差計算公式如式11所示。

針對曲率標(biāo)準(zhǔn)差的PH路徑仿真結(jié)果如圖7、圖8所示。

穩(wěn)定PH路徑僅僅對曲率標(biāo)準(zhǔn)差進(jìn)行了優(yōu)化，因此其路徑長度較長，僅僅滿足控制穩(wěn)定的需求。

3.2.3 穩(wěn)定最短PH路徑

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

圖8 最小曲率標(biāo)準(zhǔn)差情況下的曲率

圖9 多目標(biāo)路徑收斂曲線

圖10 多目標(biāo)曲率標(biāo)準(zhǔn)差收斂曲線

圖11 兩目標(biāo)優(yōu)化后的PH路徑

圖12 兩目標(biāo)優(yōu)化后的曲率

圖13 多目標(biāo)路徑收斂曲線2

圖14 多目標(biāo)曲率標(biāo)準(zhǔn)差收斂曲線2

圖15 兩目標(biāo)優(yōu)化后的PH路徑2

圖16 兩目標(biāo)優(yōu)化后的曲率2

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

4 結(jié)論

本文提出了一種基于PH曲線的無人機(jī)路徑優(yōu)化規(guī)劃方法，該方法使用遺傳算法對基于PH曲線的無人機(jī)路徑進(jìn)行了優(yōu)化，在無人機(jī)最小轉(zhuǎn)彎半徑（即曲線曲率的倒數(shù)）的約束下，優(yōu)化的內(nèi)容包括對最小化PH路徑的長度單目標(biāo)優(yōu)化、最小化曲率標(biāo)準(zhǔn)差單目標(biāo)優(yōu)化、同時最小化PH路徑和曲率標(biāo)準(zhǔn)差的多目標(biāo)優(yōu)化。仿真結(jié)果表明使用本方法可以為無人機(jī)提供一種曲率連續(xù)且標(biāo)準(zhǔn)差最小，且路徑相對較短的飛行路徑。

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

[2] 畢慧敏,董海鷹. 改進(jìn)遺傳算法在機(jī)器人路徑規(guī)劃中的應(yīng)用[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曲線的數(shù)控拐角過渡方法[J]. 航空學(xué)報,2010(7)： 1481-1487

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

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

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