基于曲率连续曲线的无人机路径规划方法*

宿 勇

(北京西三环中路19号 北京 100071)

基于曲率连续曲线的无人机路径规划方法*

宿 勇

(北京西三环中路19号 北京 100071)

论文针对现有的路径规划算法存在的不足之处,提出了一种基于曲率连续曲线的无人机路径自动生成算法。该算法使用五次PH曲线进行初始曲线生成,再设置一个适应度函数,配以相应的参数,通过模拟退火算法循环计算适应度函数的值,使其满足设定条件,从而不断地调整PH曲线使其达到最佳状态。仿真结果表明,该方法能满足路径曲线连续以及无人机曲率约束的要求。

路径规划; 曲率连续; 五次PH曲线; 适应度函数

1 引言

在自主系统的发展中,路径规划已成为关注的问题之一。一个路径规划算法可以为无人机产生一个或多个安全的可飞行的路径。无人机应该有能力跟踪任何规划出来的路径,飞行轨迹必须满足无人机的运行学约束和符合动力学特征。因此,对于路径规划算法来说,最重要的是如何获取可飞行可机动的路径[1]。

跟踪无人机的机动特性,路径满足曲率连续应为基本要求。曲率连续又称G2连续,是指曲面或曲线点点连续,并且其曲率分析结果为连续变化(二阶导数连续),曲率连续意味着在任何曲面上的任一点中沿着边界有相同的曲率半径[2～3]。只有规划出曲率连续的路径,才能使无人机更有可能在规划好的路径上飞行而不偏离航向。具有连续曲率的可行路径才是路径规划问题的可行解。

本文利用PH曲线曲率连续、曲线平滑以及曲线长度和曲率均有有理的特征,提出了基于五次PH曲线的无人机路径规划算法。再通过模拟退火算法调整路径,使其在避开所有障碍物的基础上实现路径长度最短。

2 无人机的性能

用P(x,y,z,θ,ψ)表示无人机在特定位置的特定姿态[4～5]。其中,(x,y,z)表示无人机所在位置或航路点,(θ,ψ)分别代表无人机的水平角和垂直角。若无人机从起点PS飞往终点Pf路径规划将会产生一条或多条路径r(q)连接点Ps和Pf,数学上可以将它表示成如下形式：

(1)

式中,q为路径参数,这个参数可以是一条直线路径的长度变量(0≤q≤s)或者一条曲线路径的角度变量(0≤q≤θ),路径变量的选择取决于路径公式。

无人机路径规划的约束条件主要包括两类：路径的可飞行性和安全性。可飞行性是指路径要满足无人机运行学或运行约束,特别是机动性条件；安全性是通过回避航路中出现的静止或动态的障碍物实现。其他约束条件,如保持在通信范围内、最小时间和最短路径长度等可根据需要加入其中[6]。受到约束的路径可以表示成如下形式：

(2)

3 五次PH曲线的几何性质

设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)

系数{Pk}为曲线的Bezier控制点,满足如下形式：

(7)

为了形式上的对称,在P2和P3之间引入两个辅助点A和B：

(8)

根据上述可以得出：‖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曲线的性质：

(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,根据前述的基本性质,给定的插值条件等价于定定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)

适应度函数用于评价所有的解,在本算法中它包括三个部分[12～13]：路径的长度f1,路径的弹性弯曲能量f2以及路径与环境中障碍物是否相交的判断函数f3。其中f3的定义如下：

(16)

则适应度函数f定义为如下形式：

(17)

其中,wi为适应度函数的权值。路径长度f1由式(14)计算得到,弹性弯曲能量f2由式(15)求得,它代表了PH曲线的平滑程度,通过选择合适的权值w1和w2可以得到满足曲率约束的较短PH路径。f3相当于一个阈函数,当无人机路径与威胁体有交点时就赋予一个很大的权值w3,这样可使路径能避开环境中的威胁体。

使用遗传算法计算最优路径[14]。规定一个阈值F,每算出一组最优的个体,就将结果代入到式(17)中计算阈函数f,当f的值小于阈值F时,这组最优个体即表示了最佳路径。计算流程如图2所示。

图2 最优路径计算流程

5 结果验证

在某个已知威胁分布的环境中,对本文提出的算法进行仿真验证,其中环境中的地形威胁和雷达威胁均用圆形近似表示。表1给出了仿真中无人机的约束条件,表2给出了遗传模拟退火算法中的主要参数。仿真结果如图3、图4所示。

表1 无人机的约束条件

表2 传模拟退火算法中主要参数设置

图3 无人机路径规划

图4 PH路径曲率

由图4可以看出,路径的曲率变化是连续且光滑的,说明该路径是曲率连续的。且全程的曲率的绝对值未超过0.2,说明路径满足曲率约束。

6 总结与展望

本文提出了一种基于五次PH曲线的无人机路径规划方法,通过规定的参数设定阈函数并绘制初始路径,然后使用模拟退火算法循环优化路径,当阈函数的值小于阈值时则输出最优路径。仿真结果表示,自动生成的曲线具有曲率连续的特征,能避开障碍物,同时还能满足无人机的曲率约束。下一步可扩展本算法,将二维的路径规划算法延伸到三维,在考虑曲率约束的时候同时考虑绕率约束,从而更加符合无人机的机动要求。

[1] 孙璐璐.关于PH曲线插值若干问题的研究[D].合肥:合肥工业大学,2010.

[2] 李大林,李杰,杨东晓.基于Pythagorean+Hodograph曲线的无人机路径规划方法[J].制造业自动化,2011,33(7):50-54,68.

[3] 楚拉多斯.无人机协同路径规划[M].北京:国防工业出版社,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].计算机测量与控制,2013,21(3):712-715.

[7] 陈小双,翟为刚,赵万里.基于粒子群优化算法的无人机航迹规划[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] 陈国栋,王国瑾.五次PH曲线的Hermite插值[J].软件学报,2001,12(10):1569-1572.

[12] 龚志丹.PH曲线运动轨迹规划算法的研究与实现[D].哈尔滨:哈尔滨工业大学,2012.

[13] 雍俊海,郑文.一类五次PH曲线Hermite插值的几何方法[J].计算机辅助设计与图形学学报,2005,17(5)：990-995.

[14] 李胜军.PH曲线的研究及其应用[D].西安:西北工业大学硕士学位论文,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