宿 勇

(北京西三環(huán)中路19號(hào) 北京 100071)

基于曲率連續(xù)曲線的無人機(jī)路徑規(guī)劃方法*

宿 勇

(北京西三環(huán)中路19號(hào) 北京 100071)

論文針對(duì)現(xiàn)有的路徑規(guī)劃算法存在的不足之處,提出了一種基于曲率連續(xù)曲線的無人機(jī)路徑自動(dòng)生成算法。該算法使用五次PH曲線進(jìn)行初始曲線生成,再設(shè)置一個(gè)適應(yīng)度函數(shù),配以相應(yīng)的參數(shù),通過模擬退火算法循環(huán)計(jì)算適應(yīng)度函數(shù)的值,使其滿足設(shè)定條件,從而不斷地調(diào)整PH曲線使其達(dá)到最佳狀態(tài)。仿真結(jié)果表明,該方法能滿足路徑曲線連續(xù)以及無人機(jī)曲率約束的要求。

路徑規(guī)劃; 曲率連續(xù); 五次PH曲線; 適應(yīng)度函數(shù)

1 引言

在自主系統(tǒng)的發(fā)展中,路徑規(guī)劃已成為關(guān)注的問題之一。一個(gè)路徑規(guī)劃算法可以為無人機(jī)產(chǎn)生一個(gè)或多個(gè)安全的可飛行的路徑。無人機(jī)應(yīng)該有能力跟蹤任何規(guī)劃出來的路徑,飛行軌跡必須滿足無人機(jī)的運(yùn)行學(xué)約束和符合動(dòng)力學(xué)特征。因此,對(duì)于路徑規(guī)劃算法來說,最重要的是如何獲取可飛行可機(jī)動(dòng)的路徑[1]。

跟蹤無人機(jī)的機(jī)動(dòng)特性,路徑滿足曲率連續(xù)應(yīng)為基本要求。曲率連續(xù)又稱G2連續(xù),是指曲面或曲線點(diǎn)點(diǎn)連續(xù),并且其曲率分析結(jié)果為連續(xù)變化(二階導(dǎo)數(shù)連續(xù)),曲率連續(xù)意味著在任何曲面上的任一點(diǎn)中沿著邊界有相同的曲率半徑[2～3]。只有規(guī)劃出曲率連續(xù)的路徑,才能使無人機(jī)更有可能在規(guī)劃好的路徑上飛行而不偏離航向。具有連續(xù)曲率的可行路徑才是路徑規(guī)劃問題的可行解。

本文利用PH曲線曲率連續(xù)、曲線平滑以及曲線長(zhǎng)度和曲率均有有理的特征,提出了基于五次PH曲線的無人機(jī)路徑規(guī)劃算法。再通過模擬退火算法調(diào)整路徑,使其在避開所有障礙物的基礎(chǔ)上實(shí)現(xiàn)路徑長(zhǎng)度最短。

2 無人機(jī)的性能

用P(x,y,z,θ,ψ)表示無人機(jī)在特定位置的特定姿態(tài)[4～5]。其中,(x,y,z)表示無人機(jī)所在位置或航路點(diǎn),(θ,ψ)分別代表無人機(jī)的水平角和垂直角。若無人機(jī)從起點(diǎn)PS飛往終點(diǎn)Pf路徑規(guī)劃將會(huì)產(chǎn)生一條或多條路徑r(q)連接點(diǎn)Ps和Pf,數(shù)學(xué)上可以將它表示成如下形式：

(1)

式中,q為路徑參數(shù),這個(gè)參數(shù)可以是一條直線路徑的長(zhǎng)度變量(0≤q≤s)或者一條曲線路徑的角度變量(0≤q≤θ),路徑變量的選擇取決于路徑公式。

無人機(jī)路徑規(guī)劃的約束條件主要包括兩類：路徑的可飛行性和安全性?？娠w行性是指路徑要滿足無人機(jī)運(yùn)行學(xué)或運(yùn)行約束,特別是機(jī)動(dòng)性條件；安全性是通過回避航路中出現(xiàn)的靜止或動(dòng)態(tài)的障礙物實(shí)現(xiàn)。其他約束條件,如保持在通信范圍內(nèi)、最小時(shí)間和最短路徑長(zhǎng)度等可根據(jù)需要加入其中[6]。受到約束的路徑可以表示成如下形式：

(2)

3 五次PH曲線的幾何性質(zhì)

設(shè)r(ξ)=(x(ξ),y(ξ))為給定的多項(xiàng)式曲線,若存在多項(xiàng)式σ(ξ)使：

(3)

成立,則r(ξ)稱為PH曲線。

(4)

對(duì)于五次PH曲線,u(ξ)和v(ξ)都是二次多項(xiàng)式,用Bezier形式表示如下：

u(ξ)=u0(1-ξ)2+2u1(1-ξ)ξ+u2ξ2
v(ξ)=v0(1-ξ)2+2v1(1-ξ)ξ+v2ξ2

(5)

代入PH曲線的定義式(2)并積分得到五次PH曲線：

(6)

系數(shù){Pk}為曲線的Bezier控制點(diǎn),滿足如下形式：

(7)

為了形式上的對(duì)稱,在P2和P3之間引入兩個(gè)輔助點(diǎn)A和B：

(8)

根據(jù)上述可以得出：‖P2A‖=‖P3B‖且P2A‖P3B。該曲線的控制多邊形如圖1所示。引入兩個(gè)輔助點(diǎn)C和D,其中C是BA延長(zhǎng)線和P5P4延長(zhǎng)線的交點(diǎn),D是AB延長(zhǎng)線和P5P4所在直線的交點(diǎn)(如圖1所示)[7～9]。

圖1 五次PH曲線

設(shè)L1=‖P0P1‖,L2=‖P1P2‖,L3′=‖P2A‖=‖P3B‖,L3″=‖AB‖,L4=‖P3P4‖,L5=‖P4P5‖,可以將五次Bezier的控制頂點(diǎn)以及兩個(gè)輔助點(diǎn)A和B分為兩組：P0、P1、P2、A為一組,B、P3、P4、P5為另一組,由五次PH Bezier曲線與三次PH Bezier曲線的相似性,可以推出五次PH Bezier曲線的性質(zhì)：

(9)

給定五次PH曲線的兩個(gè)端點(diǎn)P0和P5以及在這兩個(gè)端點(diǎn)處的切向量,可以確定五次Bezier曲線的四個(gè)控制點(diǎn)P0、P1、P4、P5。以P0P5為x軸建立局部坐標(biāo)系,令α為x軸沿逆時(shí)針方向到向量(P1-P0)的有向角,β為向量(P4-P5)沿逆時(shí)針到x軸負(fù)方向的有向角,∠P0P1P2=∠P1P2A=θ1,∠P3P4P5=∠BP3P4=θ2,∠P0CD=∠CDP5=θ2,根據(jù)前述的基本性質(zhì),給定的插值條件等價(jià)于定定P0,P5,α,β,L1,L5。

ω2Z2+ω1Z+ω0=0

(10)

方程一般有兩個(gè)復(fù)根,分別對(duì)應(yīng)著兩個(gè)θ1的值：

(11)

從而可以得到：

(12)

則剩余的兩個(gè)控制頂點(diǎn)為

(13)

由式(10)～式(13)可以看出,當(dāng)無人機(jī)的起始點(diǎn)位置和方向已知時(shí),只有改變?cè)谄瘘c(diǎn)和終點(diǎn)方向向量的長(zhǎng)度才能改變PH曲線的形狀[10～11]。對(duì)起點(diǎn)和終點(diǎn)方向向量進(jìn)行二進(jìn)制編碼,對(duì)它們進(jìn)行遺傳操作。在本文的算法中,將起點(diǎn)和終點(diǎn)方向向量的范圍設(shè)為[0.1,1024],二進(jìn)制編碼精度設(shè)為0.05。

4 路徑計(jì)算流程

PH曲線長(zhǎng)度S和彈性彎曲能量E的表達(dá)式如下：

S= |k|2(|a|2|b|2-|a|2Re(a)

(14)

(15)

適應(yīng)度函數(shù)用于評(píng)價(jià)所有的解,在本算法中它包括三個(gè)部分[12～13]：路徑的長(zhǎng)度f1,路徑的彈性彎曲能量f2以及路徑與環(huán)境中障礙物是否相交的判斷函數(shù)f3。其中f3的定義如下：

(16)

則適應(yīng)度函數(shù)f定義為如下形式：

(17)

其中,wi為適應(yīng)度函數(shù)的權(quán)值。路徑長(zhǎng)度f1由式(14)計(jì)算得到,彈性彎曲能量f2由式(15)求得,它代表了PH曲線的平滑程度,通過選擇合適的權(quán)值w1和w2可以得到滿足曲率約束的較短PH路徑。f3相當(dāng)于一個(gè)閾函數(shù),當(dāng)無人機(jī)路徑與威脅體有交點(diǎn)時(shí)就賦予一個(gè)很大的權(quán)值w3,這樣可使路徑能避開環(huán)境中的威脅體。

使用遺傳算法計(jì)算最優(yōu)路徑[14]。規(guī)定一個(gè)閾值F,每算出一組最優(yōu)的個(gè)體,就將結(jié)果代入到式(17)中計(jì)算閾函數(shù)f,當(dāng)f的值小于閾值F時(shí),這組最優(yōu)個(gè)體即表示了最佳路徑。計(jì)算流程如圖2所示。

圖2 最優(yōu)路徑計(jì)算流程

5 結(jié)果驗(yàn)證

在某個(gè)已知威脅分布的環(huán)境中,對(duì)本文提出的算法進(jìn)行仿真驗(yàn)證,其中環(huán)境中的地形威脅和雷達(dá)威脅均用圓形近似表示。表1給出了仿真中無人機(jī)的約束條件,表2給出了遺傳模擬退火算法中的主要參數(shù)。仿真結(jié)果如圖3、圖4所示。

表1 無人機(jī)的約束條件

表2 傳模擬退火算法中主要參數(shù)設(shè)置

圖3 無人機(jī)路徑規(guī)劃

圖4 PH路徑曲率

由圖4可以看出,路徑的曲率變化是連續(xù)且光滑的,說明該路徑是曲率連續(xù)的。且全程的曲率的絕對(duì)值未超過0.2,說明路徑滿足曲率約束。

6 總結(jié)與展望

本文提出了一種基于五次PH曲線的無人機(jī)路徑規(guī)劃方法,通過規(guī)定的參數(shù)設(shè)定閾函數(shù)并繪制初始路徑,然后使用模擬退火算法循環(huán)優(yōu)化路徑,當(dāng)閾函數(shù)的值小于閾值時(shí)則輸出最優(yōu)路徑。仿真結(jié)果表示,自動(dòng)生成的曲線具有曲率連續(xù)的特征,能避開障礙物,同時(shí)還能滿足無人機(jī)的曲率約束。下一步可擴(kuò)展本算法,將二維的路徑規(guī)劃算法延伸到三維,在考慮曲率約束的時(shí)候同時(shí)考慮繞率約束,從而更加符合無人機(jī)的機(jī)動(dòng)要求。

[1] 孫璐璐.關(guān)于PH曲線插值若干問題的研究[D].合肥:合肥工業(yè)大學(xué),2010.

[2] 李大林,李杰,楊東曉.基于Pythagorean+Hodograph曲線的無人機(jī)路徑規(guī)劃方法[J].制造業(yè)自動(dòng)化,2011,33(7):50-54,68.

[3] 楚拉多斯.無人機(jī)協(xié)同路徑規(guī)劃[M].北京:國(guó)防工業(yè)出版社,2013.

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

[5] MadhavanShanmugavel, et al.Co-operative path planning of multiple UAVs using Dubins paths with clothoid arcs[J]. Control engineering practice, 2010, 18：1084-1092.

[6] 華珊珊.基于遺傳退火算法的無人機(jī)航路規(guī)劃[J].計(jì)算機(jī)測(cè)量與控制,2013,21(3):712-715.

[7] 陳小雙,翟為剛,趙萬里.基于粒子群優(yōu)化算法的無人機(jī)航跡規(guī)劃[J].現(xiàn)代計(jì)算機(jī),2011,10(25):8-11.

[8] Shi Y, Eberhart R, Empirical C. Study of particle swarm optimization[C]//Proceeding of the World Multi-conference on Systemics, Cybernetics and Informatics. Orlando, FL: International Institute of Informatics and Systmics,2000:1945-1950.

[9] Ueda K.Pythagorean-Hodograph space curves by quaternion calculus[J]. Advances in Computational Mathematics.2004,2:41-52.

[10] Pelosi F, Farouki R T, Manni C. Geometric Hermite interpolation by spatial Pythagorean-Hodograph cubics[J]. Advances in Computational Mathematics.2005,22：325-352.

[11] 陳國(guó)棟,王國(guó)瑾.五次PH曲線的Hermite插值[J].軟件學(xué)報(bào),2001,12(10):1569-1572.

[12] 龔志丹.PH曲線運(yùn)動(dòng)軌跡規(guī)劃算法的研究與實(shí)現(xiàn)[D].哈爾濱:哈爾濱工業(yè)大學(xué),2012.

[13] 雍俊海,鄭文.一類五次PH曲線Hermite插值的幾何方法[J].計(jì)算機(jī)輔助設(shè)計(jì)與圖形學(xué)學(xué)報(bào),2005,17(5)：990-995.

[14] 李勝軍.PH曲線的研究及其應(yīng)用[D].西安:西北工業(yè)大學(xué)碩士學(xué)位論文,2001.

Path Planning Based on Curvature Continuous Curve for Unmanned Aerial Vehicles

SU Yong

(No. 19 Central Xisanhuan Road, Beijing 100071)

An algorithm for automatic path generation of UAV based on curvature continuous curve is proposed for the shortcomings of existing path planning algorithms. The PH curve is used to generate the initial curve, and then a fitness function is set up. Corresponding parameters are calculated and the value of the fitness function is cycled through the simulated annealing algorithm to meet the setting conditions. The PH curve is adjusted continuously to achieve the best condition. Simulation results show that the proposed method can meet the requirements of curvilinear path continuity and UAV curvature constraint.

path planning, curvature continuous, quintic PH curve, fitness function

TP301.6

2016年9月3日,

2016年10月25日

宿勇,男,高級(jí)工程師,研究方向:信息工程。

TP301.6

10.3969/j.issn.1672-9730.2017.03.008