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

宿 勇

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

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

宿 勇

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

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

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

1 引言

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

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

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

2 無人機的性能

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

(1)

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

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

(2)

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

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

(3)

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

(4)

對于五次PH曲線,u(ξ)和v(ξ)都是二次多項式,用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控制點,滿足如下形式：

(7)

為了形式上的對稱,在P2和P3之間引入兩個輔助點A和B：

(8)

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

圖1 五次PH曲線

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

(9)

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

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

(10)

方程一般有兩個復根,分別對應著兩個θ1的值：

(11)

從而可以得到：

(12)

則剩余的兩個控制頂點為

(13)

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

4 路徑計算流程

PH曲線長度S和彈性彎曲能量E的表達式如下：

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

(14)

(15)

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

(16)

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

(17)

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

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

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

5 結(jié)果驗證

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

表1 無人機的約束條件

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

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

圖4 PH路徑曲率

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

6 總結(jié)與展望

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

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

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

[3] 楚拉多斯.無人機協(xié)同路徑規(guī)劃[M].北京:國防工業(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] 華珊珊.基于遺傳退火算法的無人機航路規(guī)劃[J].計算機測量與控制,2013,21(3):712-715.

[7] 陳小雙,翟為剛,趙萬里.基于粒子群優(yōu)化算法的無人機航跡規(guī)劃[J].現(xiàn)代計算機,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] 陳國棟,王國瑾.五次PH曲線的Hermite插值[J].軟件學報,2001,12(10):1569-1572.

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

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

[14] 李勝軍.PH曲線的研究及其應用[D].西安:西北工業(yè)大學碩士學位論文,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日

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

TP301.6

10.3969/j.issn.1672-9730.2017.03.008