田亚卓，张勇军

(北京科技大学工程技术研究院，北京,100083)

基于改进人工势场法的动态环境下无人机路径规划

田亚卓，张勇军

(北京科技大学工程技术研究院，北京,100083)

基于传统人工势场法，在相对位置引力场和斥力场函数中分别引入目标与障碍物的速度矢量，将其位置动态变化信息融入在改进后的相对速度引力场和斥力场中，引入相对位置引力、相对速度引力、相对位置斥力、相对速度斥力等比例调节因子，建立改进后的人工势场函数及多障碍物合力函数，并在模拟环境下进行仿真。结果表明，该改进人工势场法能满足无人机在目标与障碍物动态变化条件下的路径规划的安全性、实时性和可达性，并提高了动态环境中无人机的跟踪与避障速度。

无人机; 路径规划; 动态环境; 人工势场法; 仿真分析

无人机的轨迹规划是指在满足无人机性能约束指标，在当前特定的条件下，寻找一条从起始位置到任务目标完成位置的最优或次优飞行轨迹[1-2]，它不仅能够保证飞行任务的圆满完成，还能够尽可能地节约飞行时间和能耗，降低飞行器过载[3]，因此无人机的路径规划成为无人机领域中值得深入研究的热点问题之一。

近年来国内外研究者提出了许多航迹规划的方法，主要有基于图论的方法、基于单元分解的方法、基于数学规划的方法以及基于人工势场的方法等，其中基于图论的方法是用网络图来处理轨迹规划问题，主要有Voronoi图法[4]、通视图法[5]、快速搜索随机树法(RRT)[6]和随机路线图法[7]，图论对于求解最优化问题很有实用价值，但大部分基于图论的方法需要包含该地区内的所有可能的轨迹，否则规划可能会不准确；单元分解的方法是用许多单元合成的新空间来替代规划区域，具体有栅格分解法[8]和基于A*算法的轨迹规划算法[9]，单元分解法虽然可以较为准确地进行最优航迹规划，但其算法复杂度较高，难以保证实时性；基于数学规划的方法主要是用函数来模拟影响路径规划的条件，此类方法在工程实践中运用比较多，如混合整数线性规划[10]和多约束条件下的飞行器航路规划[11],此类方法综合了轨迹距离、障碍位置和轨迹规划中的其他因素，但是算法过于复杂，函数的选取也需要着重考虑；人工势场法路径规划是1986年由Khatib提出的一种基于物理力学原理对基本空间进行路径规划的方法[12]，传统人工势场法最早是用于移动机器人与机械臂的运动轨迹规划，其基本思想是将机器人在周围环境中的运动，设计成一种抽象的人造引力场中的运动，目标点对移动机器人产生“引力”，障碍物对移动机器人产生“斥力”，最后通过求合力来控制移动机器人的运动，尽管该方法存在局部最优解的难点问题，但以其结构简洁、运算步骤少、生成路径平滑等优势在机器人避障中有着广泛的应用，同时，由于它不需要对全局路径进行搜索，规划时间短，执行效率高，鲁棒性强，所以该方法便于底层的实时控制，规划出来的路径实时、高效、安全。

随着无人机路径规划研究的深入，有研究者发现人工势场法不仅能够应用于机械臂与移动机器人的轨迹规划，也能够有效应用于无人机的航迹规划，容易针对无人机执行任务时的目标点与障碍物来规划一条平滑、安全的飞行路径，实现飞行器在引力与斥力的合力作用下从起始点运动到目标点[13]，但该方法是静态路径规划方法。尽管有研究者对威胁分布建立栅格化模型和基于稀疏A*搜索算法预规划实现人工势场法的动态航迹规划[14-15]，并收到一定效果，但算法较为复杂，难以满足实时控制需求。面向目标及障碍物均发生变化的三维动态环境下无人机路径规划，传统人工势场法容易出现规划路径过长、难以收敛、无法准确到达目标点等问题。为此，本文在传统人工势场法的基础上提出一种适用于动态环境的人工势场法改进策略，在相对位置引力场和斥力场函数中分别引入目标与障碍物的速度矢量，将其位置动态变化信息融入在改进后的相对速度引力场和斥力场中，引入相对位置引力、相对速度引力、相对位置斥力、相对速度斥力等比例调节因子，建立改进后的人工势场函数及多障碍物合力函数，并在模拟环境下进行仿真分析，为无人机在目标与障碍物动态变化条件下的路径规划提供参考。

1 传统人工势场法

人工势场包括引力场与斥力场，其中目标点对物体产生引力，障碍物对物体产生斥力，避免物体与之发生碰撞，可能会存在多个斥力同时作用。物体在路径上每一点所受的合力等于这一点所有斥力和引力的和。无人机在传统人工势场中的受力分析如图1所示。

图1 无人机在传统人工势场中的受力分析

传统人工势场法的引力势场函数Uatt(X)和斥力势场函数Urepi(X)分别为：

(1)

Urepi(X)=

(2)

式中：k为引力正比例位置增益系数；ρ(X,Xg)=‖Xg-X‖，表示无人机与目标点的距离，其中X、Xg分别为无人机与目标点的位置；η为斥力正比例位置增益系数；ρi(X,X0)为无人机与第i个障碍物之间的距离；ρ0为障碍物对无人机产生影响的最大距离。

无人机所受到的引力Fatt(X)和斥力Frepi(X)分别为：

Fatt(X)=-[Uatt(X)]=-kρ(X,Xg)

(3)

Frepi(X)=-[Urepi(X)]=

(4)

总斥力为：

(5)

则无人机所受到的合力为

F(X)=Fatt(X)+Frep(X)

(6)

2 改进人工势场法

2.1 改进的引力场函数与引力函数

为了解决传统人工势场法在动态环境中能够合理追踪目标点的问题，本文对传统引力势场函数进行改进，引入相对引力速度矢量，构造相对速度引力势场函数，建立改进人工引力场。首先将式(1)中传统的引力势场函数定义为改进人工势场法的相对位置引力势场函数，即：

(7)

式中：kα为引力正比例位置增益系数。

对相对位置引力势场函数求负梯度得到相对位置引力，即：

Fattp(X)=-kαρ(X,Xg)

(8)

相对速度引力势场函数可表示为：

(9)

式中：kβ为引力正比例速度增益系数，V(X,Xg)为无人机相对于目标点的速度。

对相对速度引力场函数求负梯度得到相对速度引力，即：

Fattv(X)=-kβV(X,Xg)

(10)

在引入了相对速度引力势场函数之后，改进引力场函数变为：

Uatt(X)=Uattp(X)+Uattv(X)=

(11)

无人机在该势场函数下所受的引力为相对位置引力与相对速度引力的合力，即：

Fatt(X)=Fattp(X)+Fattv(X)

=-kαρ(X,Xg)-kβV(X,Xg)

(12)

由引力的表达式可知，相对位置引力的作用范围是全局的，在整个无人机运动区域内，均会受到相对位置引力的作用。相对速度引力使得无人机的速度与目标的速度保持一致，其大小与相对速度的大小成正比，方向为目标点相对于无人机的运动方向。kα和kβ为权重系数，调整其大小可以调整相对速度引力和相对位置引力在合引力中所占的权重，当kβ=0时，改进的引力势场函数退化为传统的引力势场函数。

2.2 改进的斥力场函数与斥力函数

相对位置斥力函数为：

(13)

式中：ηα为斥力正比例位置增益系数。

由于在动态环境下障碍物也是运动的，故同样需要引入相对速度斥力势场函数

(14)

式中：Vi(X,X0)表示无人机在飞行过程中，相对于第i个障碍物的相对速度大小；ηβ为斥力正比例速度增益系数；θori为无人机飞行过程中，相对于第i个障碍物的相对速度矢量与xOy平面的夹角；ρ1为障碍物对无人机产生相对速度影响的最大距离。本文此处假设障碍物的运动方向为沿z轴正方向，则相对速度斥力场函数可以表示为:

(15)

式中：Xori、Yori、Zori分别为无人机的X、Y、Z坐标相对于第i个障碍物的相应坐标的标量差。

当θori>0时,相对速度斥力为：

(16)

即：

(17)

当θori<0时，相对速度斥力为：

(18)

即：

(19)

而当cosθori<0时，无人机可视为正在逃离第i个障碍物，但如果在此情况下依然考虑相对速度斥力，则可能会对无人机后续避障产生相反作用，因此此处设定该障碍物对无人机的相对速度斥力为0。

基于上述讨论，将第i个障碍物的相对速度斥力势场函数表示为:

(20)

相对速度斥力函数表示为:

Frepvi(X)=

(21)

在引入了相对速度斥力场之后，无人机在人工势场中所受到的斥力为：

(22)

式中：N为障碍物的个数。

斥力场的作用范围是局部的，引入的相对速度斥力垂直于无人机与障碍物的相对位置方向，当夹角θori较小时，速度斥力几乎垂直于相对速度的方向，能够使无人机更快地脱离障碍物威胁。无人机与障碍物之间的相对距离越小，或相对速度越大，则相对速度斥力越大。ηα和ηβ为权重系数，调整其大小可以调整相对位置斥力和相对速度斥力在斥力中的权重。

在改进人工势场法中无人机受到的力，为其在改进后的人工势场中所受到的引力与斥力的合力。

3 仿真分析

3.1 引力场的仿真及分析

设无人机的初始位置为P(0,0,0)，初始速度为V(0,0,0)，最大速度为20 m/s，最大转角为120°，目标初始位置为(4000,4000,0)，以速度为6 m/s沿z轴方向做匀速直线运动，分别在传统人工引力场和改进人工引力场中以该初始条件做仿真，kα和kβ取值分别为0.4和0.2，无人机在传统人工引力场和改进人工引力场中的运动轨迹仿真结果如图2所示，其中，Sgoal为目标点运动轨迹，箭头所指为运动方向，Stra为无人机在传统人工引力场中的运动轨迹，Simp为无人机在改进人工引力场中的运动轨迹。从图2中可以看出，无人机在改进引力势场中可以更加平滑和高效地达到目标点，由于目标点初始位置为(4000,4000,0)，速度为6 m/s沿z轴正方向做匀速直线运动，故在改进人工势场法中，无人机受到一个沿z轴正方向的相对速度引力，改进引力势场中，无人机的整体路径相对于传统引力势场有一个z轴正方向的提升，就是因为相对速度引力的作用。

图2无人机在传统人工引力场和改进人工引力场中的运动轨迹

Fig.2TrajectoryofUAVbytraditionalAPFandimprovedAPF

图3为在传统引力势场和改进引力势场作用下无人机到目标点的距离，其中，Dtra、Dimp分别为无人机在传统人工引力场和改进人工引力场中运动时到目标点的距离。从图3中可以看出，在改进引力势场函数下，无人机能更快地到达目标点，表明改进后的引力势场函数更高效、运动轨迹更平滑。

图3无人机在传统人工引力场和改进人工引力场中到目标点的距离

Fig.3ThedistanceofUAVtothetargetpointbytraditionalAPFandimprovedAPF

3.2 斥力场的仿真及分析

(a)无人机在传统斥力场中的运动轨迹

(b)无人机在改进斥力场中的运动轨迹

图5 无人机与目标点之间的动态距离

在无人机、目标点与第一个运动障碍物的运动情况保持不变的情况下，加入了另一运动的障碍物，其初始点的位置为(3000,3000,2500)，以速度为2 m/s沿z轴负方向做匀速直线运动，当无人机脱离第一个障碍物时，该障碍物开始运动，当无人机脱离该障碍物之后，该障碍物停止运动。无人机在两个运动障碍物下的运动轨迹仿真结果如图6所示，Sobs1和Sobs2分别表示第一个障碍物和第二个障碍物的运动轨迹，运动方向分别为其对应箭头所指方向。由图6可知，无人机在传统人工斥力场中遭遇多个动态障碍物时，规划的避障路径非常复杂且路径过长，但是在改进人工斥力场中，无人机能够更加高效地避开多个运动的障碍物、高速有效地到达目标点。

(a)无人机在传统斥力场中的运动轨迹

(b)无人机在改进斥力场中的运动轨迹

上述仿真结果表明，在单个与多个运动障碍物和运动目标点的环境中，改进人工势场法能够较好地解决传统人工势场法轨迹规划过长的问题，实现了无人机在三维动态环境下的路径规划。

4 结论

(1)基于传统人工势场法，在相对位置引力场和斥力场函数中分别引入目标与障碍物的速度矢量，将其位置动态变化信息融入在改进后的相对速度引力场和斥力场中，引入相对位置引力、相对速度引力、相对位置斥力、相对速度斥力等比例调节因子，建立改进后的人工势场函数及多障碍物合力函数，并在模拟环境下进行仿真。

(2)该改进人工势场法能满足无人机在目标与障碍物动态变化条件下的路径规划的安全性、实时性和可达性，并提高了动态环境中无人机的跟踪与避障速度。

[1] 唐强，张翔伦，左玲. 无人机航迹规划算法的初步研究[J]. 航空计算技术，2003，33(1)：125-128，132.

[2] 屈耀红，潘泉，闫建国，等. 无人机全局航路规划技术仿真[J].系统仿真学报, 2006，18(2)：278-281.

[3] 殷春武，侯明善，李明翔. 基于区间灰度的无人机航迹择优评价方法研究[J].系统工程理论与实践，2016，36(4)：1025-1032.

[4] 赵文婷，彭俊毅. 基于VORONOI图的无人机航迹规划[J].系统仿真学报，2006，18(S2)：159-162，165.

[5] Asano T, Guibas L, Hershberger J, et al. Visibility-polygon search and Euclidean shortest paths[C]∥Foundations of Computer Science 1985,26th Annual Symposium on IEEE, 1985:155-164.

[6] LavalleSM. Rapidly-exploringrandomtrees:A new tool for path planning[R].Ames:Computer Science Dept., Iowa State University,Oct,1998.

[7] KavrakiL, LatombeJC. Randomizedpreprocessingof configuration space for fast path planning[C]//Proceedings of IEEE International Conference on Robotics and Automation, San Diego: IEEE Computer Society Press, 1994:2138-2145.

[8] Parsons D, Canny J. A motion planner for multiple mobile robots[C]∥Proceedings of IEEE International Conference on Robotics and Automation,Cincinnati,Ohio,USA, 1990:8-13.

[9] 刘新，周成平，俞琪,等. 基于分层策略的三维航迹快速规划方法[J].宇航学报，2010,31(11)：2524-2529.

[10] Hauser J, Jadbabaie A. Aggressive maneuvering of a thrust vectored flying wing: A receding horizon approach[C]//Proceedings of the 39th IEEE Conference on Decision and Control,Sydney,Australia,2000:3582-3587.

[11] 熊丹君，蔡满意，刘宇坤，等. 多约束条件下飞行器航路规划[J].弹箭与制导学报，2009，29(2)：289-292.

[12] Khatib O.Real-time obstacle avoidance for manipulators and mobile robots[J].International Journal of Robotics Research,1986,5(1):396-404.

[13] 王强，张安，吴忠杰. 改进人工势场法与模拟退火算法的无人机航路规划[J].火力与指挥控制，2014，39(8):70-73.

[14] 丁家如，杜昌平，赵耀，等. 基于改进人工势场法的无人机路径规划算法[J].计算机应用，2016，36(1)：287-290.

[15] 姚远，周兴社，张凯龙，等. 基于稀疏A*搜索和改进人工势场的无人机动态航迹规划[J].控制理论与应用，2010，27(7)：953-959.

[责任编辑张惠芳]

UAVpathplanningbasedonimprovedartificialpotentialfieldindynamicenvironment

TianYazhuo,ZhangYongjun

(Institute of Engineering and Technology, University of Science and Technology Beijing, Beijing 100083, China)

Based on the traditional artificial potential field method, the velocity vector of the targets and obstacles were introduced into the relative position gravitational field and repulsion field function. The information of dynamic changing position was integrated into the improved relative velocity gravitational field and repulsion field. The relative position of gravity, the relative position and speed of repulsion, and other proportional adjustment factors were introduced. The improved artificial potential field function and multi-obstacle force function were established to simulate in the simulation environment. The results show that the improved artificial potential field method can not only meet the requirements of safety, real-time capability and reachability of UAV path planning under the dynamic changes of targets and obstacles, but also improves the tracking and obstacle avoidance speed of the UAV in a dynamic environment.

UAV; path planing; dynamic environment; artifical potential field; simulation analysis

V249

A

1674-3644(2017)06-0451-06

2017-09-09

田亚卓(1991-)，男，北京科技大学硕士生.E-mail:tian19911116@126.com

张勇军(1973-)，男，北京科技大学研究员，博士生导师.E-mail:zhangyj@ustb.edu.cn

10.3969/j.issn.1674-3644.2017.06.009