固定翼无人机在线航迹规划方法

刘佳　秦小林　许洋　张力戈

摘 要：在不确定环境下，针对固定翼无人机（UAV）航迹规划问题，提出了一种基于滚动时域控制的模糊粒子群优化算法与改进人工势场法相结合的在线航迹规划方法。首先，对凸多边形障碍物进行最小外接圆拟合;然后，根据静态威胁，将规划问题转化为一系列时域窗口内的在线子问题，利用模糊粒子群算法实时优化求解以实现静态避障;当环境中存在动态威胁时，使用改进人工势场法对航迹进行调整完成动态避障。为了满足固定翼无人机的动态约束，同时提出固定翼UAV的碰撞检测法，可提前判断障碍物是否为真正威胁源，以此减少转弯频率和幅度，降低飞行代价。仿真实验结果表明，所提方法在固定翼UAV航迹规划中能有效提升规划速度、稳定性与实时避障能力，且克服了传统人工势场容易陷入局部最优的缺点。

关键词：滚动时域控制;模糊粒子群;人工势场;固定翼无人机;航迹规划

中图分类号： V249;TP301文献标志码：A

On-line path planning method of fixed-wing unmanned aerial vehicle

LIU Jia1，2， QIN Xiaolin1，2*， XU Yang1，2， ZHANG Lige1，2

（1. Chengdu Institute of Computer Application， Chinese Academy of Sciences， Chengdu Sichuan 610041， China;

2. School of Computer and Control Engineering， University of Chinese Academy of Sciences， Beijing 100049， China）

Abstract： By the combination of fuzzy particle swarm optimization algorithm based on receding horizon control and improved artificial potential field， an on-line path planning method for achieving fixed-wing Unmanned Aerial Vehicle （UAV） path planning in uncertain environment was proposed. Firstly， the minimum circumscribed circle fitting was performed on the convex polygonal obstacles. Then， aiming at the static obstacles， the path planning problem was transformed into a series of on-line sub-problems in the time domain window， and the fuzzy particle swarm algorithm was applied to optimize and solve the sub-problems in real time， realizing the static obstacle avoidance. When there were dynamic obstacles in the environment， the improved artificial potential field was used to accomplish the dynamic obstacle avoidance by adjusting the path. In order to meet the dynamic constraints of fixed-wing UAV， a collision detection method for fixed-wing UAV was proposed to judge whether the obstacles were real threat sources or not in advance and reduce the flight cost by decreasing the turning frequency and range. The simulation results show that， the proposed method can effectively improve the planning speed， stability and real-time obstacle avoidance ability of fixed-wing UAV path planning， and it overcomes the shortcoming of easy to falling into local optimum in traditional artificial potential field method.

Key words： receding horizon control; fuzzy particle swarm; artificial potential field; fixed-wing Unmanned Aerial Vehicle （UAV）; path planning

0 引言

無人机因其低成本、可自主飞行等优良特性，在军事和民用领域得到了广泛应用。无人机航迹规划是一个多目标优化问题，定义为：在一定环境和任务目标下，寻找从起始点到目标点的最优飞行路线，同时避开各种威胁源[1]。

目前，现有航迹规划方法可分为几何方法、启发式搜索方法、势场法等。其中几何方法包括Voronoi图[2]、概率图模型[3]、可视图模型等。这类算法首先对环境进行几何建模，然后依据一定的最优策略，选择某种搜索算法得到可行解;但当任务空间发生变化，需对任务空间重新遍历，不适合动态航迹规划。文献[4]在三维环境下构建连续可微的位形空间，生成连接各个障碍物的广义可视图，并利用搜索算法搜索出一条可行路径，在障碍物发生改变时，只需更新部分信息;但仍未解决在线规划问题。文献[5]提出一种改进的Voronoi图算法，得到有效、无碰撞的可飞航迹;但未对未知环境下无人机避障问题进行讨论。势场法中典型的方法是人工势场法[6]，其优点是计算量小、速度快，但容易陷入局部最优。尽管有学者在原来基础上提出了虚拟力[7]和带干扰的流动力学系统[8]，但是局部最小的问题依旧存在。为此，文献[9]提出一种在三维环境下将改进的扰动流体动态系统与灰狼优化理论结合的航迹规划算法，使得规划出的航迹平滑可飞，能有效解决局部最优问题。文献[10]通过引入相对速度斥力势场解决了人工势场局部极小的问题，通过设计了斥力增益模糊控制器，有效地完成了无人机动态避障。启发式搜索法包括A*（A-Star）算法[11]、粒子群算法、遗传算法[12]等经典算法;但这类算法随着搜索空间的扩大，计算复杂度会呈爆炸式增长。文献[13]提出了一种稀疏A*算法，该算法在修剪搜索空间的同时使得规划路径满足约束条件，以此提高规划路径的效率。文献[14]针对机器人避障提出了一种混合粒子群算法，实验表明此算法性能和收敛速度要优于标准粒子群算法。文献[15]提出一种惯性权值依据迭代次数变化而变化的改进粒子群算法，该算法明显提高了航迹优化精度和稳定性;但因未过多考虑无人机的约束条件，不适用于实际工程应用。文献[16]提出了基于滚动时域控制的模糊粒子群优化（Receding Horizon Control-Fuzzy Particle Swarm Optimization， RHC-FPSO）算法以解决带有动力学约束的多旋翼无人机航迹规划问题，该方法是在已知全局环境信息的前提下需要计算全局代价图，当环境突然发生变化时，使用RHC-APF（Receding Horizon Control-Artificial Potential Field）作出调整，但是该方法不适用于局部环境已知的固定翼无人机航迹规划。为提高固定翼无人机的生存能力，本文中提出了静态避障和动态避障相结合的在线航迹规划算法。首先，采用基于带有约束的滚动时域控制策略的模糊粒子群优化（Restricted-RHC-FPSO， R-RHC-FPSO）方法完成已知局部环境信息下的静态避障;然后，构造分段线性人工势场法（Piecewise Linear Artificial Potential Field， PLAPF），完成动态避障。实验结果表明，本文提出的算法能够满足不确定环境下固定翼无人机实时避障的要求和无人机动力学约束，达到良好的动态航迹规划效果。

1 问题描述

1.1 滚动时域控制

滚动时域控制（Receding Horizon Control， RHC）也称为模型预测控制（Model Predictive Control， MPC），是一种基于滚动预测方式，在线求解最优的控制方案[17]。假设当前时间点为k，从当前最新状态xpk出发，预测时域为[k，k+N]，Upk定义为在时刻k预测N步的控制输入序列，即Upk=[upk|kT，upk|1 + kT，…，upk|N-1 + kT]T，系统执行第一步控制输入，并更新当前最新状态，重复此过程，直到达到目标。

1.2 模型建立

结合RHC原理以及文献[18]中提出的无人机动力学模型得到基于RHC的固定翼无人机模型为：

minu（·） JN=∑N-1i=0li（x（k+i|k），u（k+i|k），xf）+

f（x（k+N+1|k），xf）（1）

x（k+i+1|k）=p（k+i+1|k）

v（k+i+1|k）=

Ap（k+i|k）

v（k+i|k）+Bu（k+i|k）;i=0，1，…，N-1（2）

式（1）为目标函数，式（2）为无人机的状态空间模型。其中：A=I2ΔtI2

O2I2;B=12（Δt）2I2ΔtI2T;uk+i表示在第k+i时刻的控制输入;p（）、v（）、u（）分布表示无人机位置、速度和加速度;xf是目标集， f（）为终端惩罚项。 f（）表示为：

f（x（k+N+1），xf）=d（x（k+N+1），xvis）+Cif（3）

其中：d（）表示航迹端点到障碍物的代价，用航迹端点到障碍物最小外接圆的切线距离表示;xvis表示用航迹端点到障碍物最小外接圆的切点;Cif为障碍物到目标点的最小代价。

一般的输出约束满足包括转弯角约束、速度约束、加速度约束，以及障碍物约束。

change≤max

‖v（k+i|k）‖≤vmax

‖auav‖≤amax

p（k+i|k）O （4）

式中：O∈R2代表不可飞区域;max、vmax、amax分别为固定翼无人机最大转弯角、最大速度和最大加速度约束。

1.3 碰撞检测

为保证固定翼无人机满足转弯角约束条件同时减小无人机转弯频率，本文在文献[19]的碰撞检测方式上作出了改进，如图1所示，给出如下定义。

定义1 如果d-Robi≤ρo，此时视障碍物i为潜在威胁源。d表示无人机当前位置Puav与障碍物最小外接圆圆心Pobi之间的距离，ρo表示无人机检测半径。

定义2 如果vro在冲突域NPuavM中或ψor满足式（5），此时将障碍物i视为真正的威胁源，将继续向此方向飞行视为不可飞区域。

∠MPuavx′≤ψor≤∠NPuavx′

ψm-|β-|≤ψor≤ψm+|β+|（5）

其中：vro表示无人机和障碍物i的相对速度;ψor表示相对速度vro与x坐标的夹角;NPuav和MPuav为无人机当前位置和障碍物i最小外接圆的切线;β-和β+分别表示d和MPuav以及d与NPuav之间的夹角。

此种检测算法结合滚动时域预测方式能使无人机提前判断障碍物是否构成威脅，及早进行规避操作。因为转弯过程比直飞需要更大的推力，所以应避免出现转弯角度过大的情况，这是与多旋翼无人机碰撞检测算法的不同之处。

2 R-RHC-FPSO静态避障

2.1 模糊粒子群算法

PSO算法是一种基于种群搜索策略的自适应随机优化算法，各粒子速度和位置更新公式为：

Vj（t+1）=wVj（t）+c1r1（Lj（t）-Xj（t））+

c2r2（G（t）-Xj（t））

Xj（t+1）=Xj（t）+Vj（t）（6）

其中：Lj（t）是每个粒子的局部最优解;G（t）是全局最优解;w为惯性权重;t为当前迭代次数;c1、c2表示学习因子;r1和r2为区间[0，1]的随机数。因为控制时域为N，所以粒子速度Vj（t）和位置Xj（t）是N个2维向量组成的矩阵。

惯性权重具有能够平衡算法的全局搜索和局部搜索能力的作用，而惯性权重的自适应调节能够加快算法收敛到最优值[20]，本文中惯性权重更新规则如下：

w（t）=wmax， t<23T

wmax-wmax-wminKk，t≥23T （7）

其中：t为当前迭代次数;T为总迭代次数。惯性权重w（t）的值随着迭代次数的变化而变化，在迭代总次数的前2/3时期，惯性权重值设置为最大，增强全局搜索能力;在迭代后期，惯性权重值逐渐变小，增强局部搜索能力。

wmin和wmax分别为w（t）的最小和最大值，其中：wmax不是一个固定的值，而是根据粒子的适应度值作出更新，如果粒子满足约束，即为可行解，为方便起见，此时wmax设置为1;如果粒子不满足约束时，wmax（t， j）=1-Ft， j（u）maxj Ft， j（u），Ft， j（u）表示粒子j在第t次迭代的代价。

通过对wmax的动态调整，使得满足约束的粒子的搜索能力增强，不满足约束的粒子搜索能力得到弱化。实验结果表明，这种更新方式所费时间更短。

2.2 R-RHC-FPSO算法流程

在当前时刻k，依据模糊粒子群更新规则式（6）优化求解式（1），在优化过程中粒子计算得到的控制输入和式（2）不满足式（4）中任一条件时，将该粒子视为不可行粒子，在得到最优控制输入u（k+i|k）（i=0，1…，N-1）后执行第一步控制输入u（k+1|k）并更新当前状态，系统进入下一时刻k+1，循环执行此步骤，直到达到目标。具体步骤如下所示：

1）设置粒子群种群数M，并初始化每个粒子的速度Vj=（Vj，1，Vj，2，…，Vj，N），位置Xj=（Xj，1，Xj，2，…，Xj，N）。

2）判断当前迭代次数t是否小于最大迭代次数T，如果是转到步骤3）;否则转到步骤6）。

3）遍历粒子群，如果粒子j满足约束式（4），按照式（3）计算每个粒子的代价值;否则，粒子j的代价为BigM+C，其中BigM为一个很大的数，C为相应的约束违背量。

4）依据代价值最小原则更新全局最优解G（t）和每个粒子局部最优解Lj（t）。

5）按照式（6）更新粒子j的速度Vj、位置Xj，如果粒子不满足终止条件，转步骤3）;否则转到6）。

6）将全局最优解G（t）的第一个控制输入模型式（2）中，计算无人机下一时刻的状态。

7）如果无人机到达目标点，结束;否则转到步骤2）。

3 PLAPF动态避障

如果环境中存在动态障碍物时，继续使用R-RHC-FPSO静态避障算法，需实时计算代价图，计算成本大，而人工势场法因其计算量小、规划时间短的优良特性，可以和快速规避动态障碍物的目标结合，完成动态避障任务。传统人工势场法是通过构造虚拟的引力场和斥力場[21]来寻找一个无碰撞的路径。传统人工势场法在静态威胁环境下可以快速规划出一条无碰撞路径，但是如果环境中存在动态威胁物时，其规划能力就减弱了，而且会出现局部极小和目标不可达的情况。为此，通过在分段线性人工势场中引入相对速度势场来解决这两个问题。

3.1 引力势场

引力势场由无人机到目标点的势场和无人机自身速度势场构成，即：

Uatt（p，v） = εpρ2goal（p） + εv‖v‖2（8）

其中：εp、εv为引力增益因数。分段线性的引力可由该点势函数的负梯度计算得到：

Fatt=0，ρgoal（p）

Fmaxρgoal（p）-rRdetect，r≤ρgoal（p）≤ρo

Fmax，ρgoal（p）>ρo （9）

其中Fmax=2εp ρgoal（p）ρgoal（p）+2εv‖v‖。当目标位置在无人机检测范围外时，将最大引力作用于无人机，并将其推向目标;当目标位置位于无人机检测范围内同时无人机与目标点的距离大于等于最小安全距离时，此时的引力和ρgoal（p）成正比函数，当ρgoal（p）越小时，引力越小，此种方法可防止无人机在到达目标点时由于引力过大产生目标不可达的情况;当固定翼无人机距离目标的距离小于最小安全距离r时，没有外力作用于无人机，即视为到达目标。

3.2 斥力势场

斥力势场由障碍物自身产生的势场和无人机与障碍物之间的相对速度势场构成，即：

Urepi（p，v）=ηp（1ρi（p）-1ρo）+ηvovro（10）

其中：ηp、ηvo为斥力增益因数;vro=（v-vobs）Tero表示无人机与障碍物的相对速度。分段线性的斥力可由斥力势函数的负梯度计算得到：

Frepi（p，v）=Frepi_max， ρi（p）≤r且vro>0

Frepi_max（1-（ρi（p）-r）ρo），

r<ρi（p）≤ρo且vro>0

0，ρi（p）>ρo或vro≤0（11）

其中：

Frepi_max=-p[ηp（1ρi（p）-1ρo）+ηvovro]-

o[ηp（1ρi（p）-1ρo）+ηvovro] （12）

只有当障碍物进入无人机检测范围并且无人机与障碍物i的相对速度大于0时，该障碍物才会对无人机产生斥力作用，并且障碍物i产生的斥力会随着ρi（p）的减小而增大，当ρi（p）小于等于最小安全距离r时，此时作用于无人机的斥力最大。假设无人机在检测范围内检测到障碍物数为Num，则无人机在当前位置受到的总斥力为：

Frep（p，v）=∑Numi=1Frepi（p，v） （13）

因此，通过上述改进可以保证无人机能够快速规避动环境中的动态障碍物，同时解决了APF的局部极小和目标不可达的问题。

4 仿真结果及分析

为验证所提算法的有效性，在Matlab R2016a编程环境下对固定翼无人机航迹规划进行仿真，仿真环境区域为50m×50m，按照1∶100比例缩放，仿真参数如表1所示。

表格（有表名）表1 仿真参数

Tab. 1 Simulation parameters

参数描述数值N滚动时域控制步数6K粒子群最大迭代次数100M粒子群数量30Δt/s采样时间1vmax/（km·s-1）最大速度0.6amax/（km·s-1）最大加速度0.2max/（°）最大转弯角45r/km最小安全距离0.5pinit起始位置（0，50）pgoal目标位置（50，0）

4.1 实验一

将本文所提出的碰撞检测方式（R-RHC-FPSO）和传统碰撞检测方式（RHC-FPSO）进行仿真实验对比，结果如图2所示。

图2中，（a）和（b）分别是传统的检测算法即在检测半径以内的障碍物都视为威胁源与本文所提碰撞检测算法在无动态威胁环境下的航迹和航向角变化对比。从图2（a）中Ⅰ和Ⅱ可看出，本文所提检测算法能够提前判断继续向前飞行可能会发生冲突，及时转变方向;而普通检测算法只有在无人机与障碍物距离小于安全距离时，才进行规避，明显看出普通检测算法得到的航迹图存在转弯角过大的问题。同样从图2（b）可以看出，本文所提检测方式在航向角稳定性上更具有优势，平均单步航向角转变要小于普通检测算法规划出的航迹航向角转变。

[7]DONG Z， CHEN Z， ZHOU R， et al. A hybrid approach of virtual force and A* search algorithm for UAV path re-planning [C]// Proceedings of the 6th Industrial Electronics and Applications. Piscataway： IEEE， 2011： 1140-1145.

[8]WANG H， LYU W， YAO P， et al. Three-dimensional path planning for unmanned aerial vehicle based on interfered fluid dynamical system [J]. Chinese Journal of Aeronautics， 2015， 28（1）： 229-239.

[9]姚鵬，王宏伦.基于改进流体扰动算法与灰狼优化的无人机三维航路规划[J].控制与决策，2016，31（4）：701-708.（YAO P， WANG H L. Three-dimensional path planning for UAV based on improved interfered fluid dynamical system and grey wolf optimizer [J]. Control and Decision， 2016， 31（4）： 701-708.）

[10]姚远，周兴社，张凯龙，等.基于稀疏A* 搜索和改进人工势场的无人机动态航迹规划[J].控制理论与应用，2010，27（7）：953-959.（YAO Y， ZHOU X S， ZHANG K L， et al. Dynamic trajectory planning for unmanned aerial vehicle based on sparse A* search and improved artificial potential field [J]. Control Theory and Applications， 2010， 27（7）： 953-959.）

[11]FAN K C， LUI P C. Solving find path problem in mapped environments using modified A* algorithm [J]. IEEE Transactions on Systems， Man， and Cybernetics， 1994， 24（9）： 1390-1396.

[12]CHEN G， CRUZ J B. Genetic algorithm for task allocation in UAV cooperative control [C]// Proceedings of the 2003 AIAA Guidance， Navigation， and Control Conference and Exhibit. Reston： AIAA， 2003： Article No. 5582.

[13]SZCZERBA R J， GALKOWSKI P， GLICKTEIN I S， et al. Robust algorithm for real-time route planning [J]. IEEE Transactions on Aerospace and Electronic Systems， 2000， 36（3）： 869-878.

[14]KUO P H， LI T H S， CHEN G Y， et al. A migrant-inspired path planning algorithm for obstacle run using particle swarm optimization， potential field navigation， and fuzzy logic controller [J]. The Knowledge Engineering Review， 2017， 32： Article No. e5.

[15]方群，徐青.基于改进粒子群算法的无人机三维航迹规划[J].西北工业大学学报，2017，35（1）：66-73.（FANG Q， XU Q. 3D route planning for UAV based on improved PSO algorithm [J]. Journal of Northwestern Polytechnical University， 2017， 35（1）： 66-73.）

[16]王文彬，秦小林，张力戈，等.基于滚动时域的无人机动态航迹规[J].智能系统学报，2018，13（4）：524-533.（WANG W B， QIN X L， ZHANG L G， et al. Dynamic UAV trajectory planning based on receding horizon [J]. CAAI Transactions on Intelligent Systems， 2018， 13（4）： 524-533.）

[17]MAYNE D Q， RAWLINGS J B， RAO C V， et al. Constrained model predictive control： Stability and optimality [J]. Automatica， 2000， 36（6）： 789-814.

[18]KUWATA Y， RICHARDS A， SCHOUWENAARS T， et al. Decentralized robust receding horizon control for multi-vehicle guidance [C]// Proceedings of the 2006 American Control Conference. Piscataway： IEEE， 2006： 2047-2052.

[19]THANH H L N N， PHI N N， HONG S K， et al. Simple nonlinear control of quadcopter for collision avoidance based on geometric approach in static environment [J]. International Journal of Advanced Robotic Systems， 2018， 15（2）： 1-7.

[20]張长胜，孙吉贵，欧阳丹彤.一种自适应离散粒子群算法及其应用研究[J].电子学报，2009，37（2）：299-304.（ZHANG C S， SUN J G， OUYANG D T. A self-adaptive discrete particle swarm optimization algorithm [J]. Acta Electronica Sinica， 2009， 37（2）： 299-304.）

[21]LATOMBE J C. Robot Motion Planning [M]. Berlin： Kluwer Academic Publishers， 1991： 19-20.

This work is partially supported by the National Natural Science Foundation of China （61402537）， the Light of West China Program of Chinese Academy of Sciences， the Sichuan Science and Technology Program （2018GZDZX0041）.

LIU Jia， born in 1995， M. S. candidate. Her research interests include path planning of unmanned aerial vehicle， machine learning.

QIN Xiaolin， born in 1980， Ph. D， research follow. His research interests include automatic reasoning， cluster intelligence.

XU Yang， born in 1994， M. S. candidate. His research interests include machine learning， optimization algorithm.

ZHAGN Lige， born in 1995， Ph. D. candidate. His research interests include machine learning， optimization algorithm.

收稿日期：2019-05-22;修回日期：2019-07-23;录用日期：2019-07-23。

基金项目：国家自然科学基金资助项目（61402537）;中国科学院“西部青年学者”项目;四川省科技计划项目（2018GZDZX0041）。

作者简介：刘佳（1995—），女，宁夏银川人，硕士研究生，主要研究方向：无人机航迹规划、机器学习; 秦小林（1980—），男，重庆人，研究员，博士生导师，博士，主要研究方向：自动推理、集群智能; 许洋（1994—），男，重庆人，硕士研究生，主要研究方向：机器学习、优化算法;张力戈（1995—），男，山西原平人，博士研究生，主要研究方向：机器学习、优化算法。

文章编号：1001-9081（2019）12-3522-06DOI：10.11772/j.issn.1001-9081.2019050863