基于自适应粒子群优化算法的无人机三维航迹规划

王 磊，赵红超，王书湖，卢仁伟

（1.海军航空大学，山东烟台264001；2.烟台南山学院，山东龙口265713）

近年来，无人机技术取得了极大进展，各种类型的无人机纷纷研制成功，性能逐步提高，并已成功应用于许多军用领域和民用领域。无人机技术已经成为世界各国军事部门、研究机构和高等院校的一个研究热点。无人机航迹规划是无人机技术领域中的一项重要课题，得到了国内外研究者的极大关注[1]。航迹规划就是在综合考虑无人机到达时间、能耗、威胁以及动力学约束条件等要素的前提下，为无人机规划出最优或者满意的飞行航迹，以保证圆满地完成飞行任务[2]。固定翼无人机的任务空间是三维环境空间，跨越的空间范围很大，环境复杂，而且无人机自身约束条件较多。因此，航迹规划算法需要在计算复杂度、搜索时间和航迹代价之间做好折中处理，以满足实际任务要求。航迹规划的组成：描述规划环境空间，选择航迹表示形式，分析约束条件，确定代价函数，选取航迹搜索算法和航迹平滑。其中，选取航迹搜索算法是解决航迹规划问题的核心部分。

航迹规划算法有很多种，每种算法都有其自身的优缺点和适用范围，按照规划决策可以分为传统经典算法和现代智能算法2大类。常用于无人机航迹规划的传统经典算法包括：Dijkstra 算法[3]、模拟退火算法[4]、Dubins 曲线法[5]、人工势场法[6]、快速扩展随机树及其改进型算法[7]等。现代智能算法包括：A*算法[8]、遗传算法（GA）[9]、蚁群优化（ACO）算法[10]、粒子群优化（PSO）算法[11]、鸽群算法[12]等。相比于传统经典算法，现代智能算法的应用更为广泛。其中，粒子群优化算法具有2 个显著特点：一是没有“优胜劣汰”的机制，所有的粒子在迭代过程中始终作为种群的成员保留；二是没有交叉、变异等进化算子，每个粒子通过追随当前搜索到的最优值寻找全局最优值。粒子群优化算法的优点是具有较强的鲁棒性，对种群大小敏感性不高，设计参数少，前期收敛速度快；缺点是后期收敛速度慢，容易早熟，陷入局部最优解。

为了克服PSO算法的缺点，近年来一些研究者进行了探索，提出了几种改进方法。文献[13]在PSO 算法中引入自适应灵敏度决策算子克服了PSO 算法易陷入局部最优和后期收敛速度慢的问题，获得了质量较好的航迹。文献[14]在PSO算法中引入空间精选投票机制找出空间中每一个航迹点的最优位置，克服了PSO 算法易早熟，陷入局部最优的问题。文献[15]研究了障碍物密集环境中的无人机航迹规划问题，在粒子群优化算法中加入了人工势场，克服局部最优解问题，并提高了算法的收敛速度。文献[16]采用模拟退火突跳概率策略，对PSO算法的粒子位置更新方式进行了改进，能够增大全局搜索能力，降低航迹规划时间。文献[17]采用PSO算法进行无人机三维局部动态航迹规划，采用分层随机初始化与粒子偷懒流放策略，改进PSO 算法以加快收敛效率。文献[18]研究了无人艇航线规划问题，采用量子粒子群优化算法，并结合人工势场法思想，解决了PSO算法局部最优解问题，收敛速度快。文献[19]设计了分段式惯性权重调整公式改进PSO 算法，在保证算法搜索速度的同时，提高了航迹规划解的精度。

前人对应用于无人机航迹规划的粒子群优化算法开展了一些研究，但仍存在一些问题须要进一步深入研究。小型固定翼无人机自身约束条件较多而且严苛，未来其执行任务的空间环境将更复杂、更危险，须要结合无人机自身约束条件和实际工程中任务环境的复杂状况，对无人机航迹规划算法进一步深入探索。受制造成本等经济因素限制，机载计算机的解算能力有限，因而要求航迹规划算法尽量简单高效，具有计算量小、精度高、容错率大等优点。本文提出了一种自适应粒子群优化算法，只需要自适应地调节惯性权重和学习因子，不需要额外加入其他算法，就能够克服局部最优问题和后期收敛速度慢的问题，满足了简单高效的要求，为固定翼无人机在复杂三维环境空间中执行任务提供了可靠保障。

1 无人机航迹规划问题

1.1 三维航迹规划环境建模

环境建模就是将环境中的各种物理信息转换为计算机算法能处理的数字模型，这是规划无人机飞行航迹的前提和基础。本文以无人机在山区环境中执行自然灾害勘察任务为例进行研究。山区环境存在山峰、高压电线、树木、鸟群和风切变等威胁。以东北天坐标系作为惯性参考坐标系，采用圆锥体近似模拟高耸的山峰，山区地形高度可用如下算法模拟[6]：

式（1）中：z0为当地基准地形高度；hi为第i 个山峰的高度峰值；I 为山峰的数目；x0,i、y0,i分别为第i 个山峰峰顶在水平面内的横、纵坐标；xs,i和ys,i分别为第i个山峰的横向坡度和纵向坡度。

各个山峰是飞行环境中的地形威胁。山峰附近区域通常存在风切变等极端气象，这些区域也是飞行威胁区，用椭球体近似模拟，设第i 个山峰附近椭球威胁区的中心坐标为(xc,i,yc,i,zc,i)，半轴长为(rx,i,ry,i,rz,i)。对于高压电线和树木等威胁，无人机通过自身最小飞行高度约束来规避，即无人机的最小飞行高度应比高压电线和树木的最大高度高出一定的距离。飞行的鸟群对无人机来说是突发的动态威胁，飞鸟或鸟群的运动信息比较复杂，本文暂不研究此类突发状况。

1.2 无人机自身约束条件

固定翼无人机不具备旋翼式无人机的垂直起降、空中悬停、飞行灵活等性能，因而飞行中自身约束条件要比旋翼式无人机严格得多。固定翼无人机比旋翼式无人机飞行速度快、航程远、能耗低，更适合到距离远、范围大的空间环境中去执行任务。主要考虑以下几项约束条件。

1）最大航迹距离。设无人机最大航程为Lmax，规划航迹包括K 段航迹段，第k 段航迹段的长度为Lk，k=1,2,…,K ，则最大航迹距离约束表示为：

2）最小惯性距离。最小惯性距离是指无人机突然变换飞行方向时，由于惯性作用需要按原方向飞行的最短距离。设最小惯性距离为Lmin，则最小惯性距离约束表示为：

3）最大水平转弯角。受本身硬件性能的限制，无人机在水平方向转弯时，转弯角度无法超过最大水平转弯角Δψmax，设第k 段航迹段相比于第k-1 段航迹段的水平转弯角为Δψk，则水平转弯角约束为：

4）最大高低角。高低角是指无人机向上爬升或向下俯冲的角度，由于性能限制，无人机飞行中的高低角不能超过最大高低角θmax，设第k 段航迹段与水平面的夹角为θk，则最大高低角约束为：

5）最大/小飞行高度。无人机执行勘察任务需要在低空飞行，设无人机离地面的最大高度为Δhmax，一条航迹由D 个航迹点组成，第d（d=1,2,…,D）个航迹点的海拔高度为zd，航迹点下面的地形高度为z1（由式（1）解算），则最大飞行高度约束表示为：

另外，无人机的飞行高度还要求比高压电线和树木的最大高度高出一定的安全距离。设高压电线和树木的最大高度为ht，安全距离为hsafe，则最小飞行高度约束表示为：

2 自适应粒子群优化算法设计

2.1 算法基本原理

粒子群优化算法是无人机航迹规划常采用的一种方法，其主要缺陷是：后期收敛速度慢，易陷入局部最优解。

为克服此缺陷，本文在粒子群优化算法中加入自适应调节算法，调节惯性权重和2个学习因子，从而提出了一种自适应粒子群优化（APSO）算法。

式（8）、（9）中：w 为惯性权重；c1和c2为学习因子；r1和r2为[0,1]范围内的随机数。在传统粒子群优化算法中参数w、c1和c2都取常数。

分析这些参数的作用，提出相应的改进策略。w反映了粒子维持自己先前运动速度的趋势，取较大值能够加快搜索速度，但是影响搜索精度。c1反映了粒子的认知能力，取较大值能够提高个体搜索速度，增强全局搜索能力，但是影响局部搜索能力和搜索精度。c2反映了粒子的社会能力，取较大值能够增强局部搜索能力，加快收敛速度，但是容易早熟，陷入局部最优解。基于上述分析，动态调整参数的数值则能够实现搜索速度与搜索精度、全局搜索与局部搜索的协调，克服局部最优解。

自适应调节算法设计如下：

式（10）～（12）中：wmax和wmin分别为w 的最大值和最小值；c1max和c1min分别为c1的最大值和最小值；c2max和c2min分别为c2的最大值和最小值；t 为当前迭代次数；tmax为最大迭代次数。

此改进策略的原理是：在迭代前期w 和c1的取值较大、而c2的取值较小；利用w 和c1较大加快搜索速度、增强全局搜索能力，对于搜索精度较差的问题，在迭代后期再弥补；利用c2较小来避免早熟陷入局部最优解。在迭代后期w 和c1的取值较小、而c2的取值较大；后期粒子群基本上运动到了最优解的附近区域，通过减慢搜索速度来提高搜索精度，降低全局搜索能力而提高局部搜索能力，从而加快收敛到最优解的速度；实现了搜索速度与搜索精度、全局搜索与局部搜索的协调配合。

2.2 适应度函数设计

适应度函数就是无人机航迹规划的优化函数，它是衡量一个粒子优劣的指标函数。结合前面分析的各种环境威胁、无人机自身约束条件等限制，适应度函数中应包含航迹长度、环境威胁和约束条件等指标，表达式如下：

式（13）中：FL表示航迹长度代价；FT表示环境威胁代价；FS表示约束条件代价；wL、wT和wS分别表示3项代价的权重，wL+wT+wS=1。

对于无人机三维航迹规划问题，一个粒子代表了一条航迹，航迹由一组三维航迹点组成。粒子的位置向量中有D 个元素，即一条航迹由D 个航迹点组成，由K（K=D-1）段航迹段组成，则航迹长度适应度函数为：

环境威胁主要包括山峰威胁和风切变区域威胁。飞行环境中的山峰位置及其附近的风切变区域都是已知的。高压电线和树木等威胁通过约束无人机的最小飞行高度来规避；由高度约束式（7）可知，无人机飞行高度须高于包括山峰在内的地形高度。此处考虑的山峰威胁是要求无人机航迹在某一高度的水平面内也要远离山峰以提高飞行安全性；避免仅依靠增大飞行高度来规避山峰威胁，无人机飞的过高则不利于勘察工作。

设第n 条航迹上的第d（d=1,2,…,D）个航迹点的位置坐标为snd=(xnd,ynd,znd)。山峰威胁适应度函数为：

式（15）、（16）中：fT1表示山峰威胁代价常数因子；R1i表示在水平面内第n 条航迹上的所有航迹点到第i 个山峰中心点的最小距离。

风切变区域也是飞行威胁区，用椭球体近似模拟，无人机航迹也要远离威胁区。威胁区的数目为I ，第i 个威胁区的中心坐标为(xc,i,yc,i,zc,i)。适应度函数为：

式（17）、（18）中：fT2表示风切变区域威胁代价常数因子；R2i表示第n 条航迹上的所有航迹点到第i 个威胁区中心的最小距离。

概括起来，环境威胁适应度函数为：

建立约束条件的代价函数。为了简便起见，采用惩罚常数因子来描述代价函数[9]，设惩罚常数因子为C0，不妨取较大的正数。最大航迹距离的适应度函数为：

其他约束条件的适应度函数也是同理，若不满足约束条件则适应度值为C0，若满足约束条件则适应度值为0。

2.3 航迹规划流程

采用APSO 算法开展无人机三维航迹规划，通过粒子群的迭代搜索寻优，最终找到的全局极值就是规划出的最优参考航迹。所规划出的航迹是从起始点经由K 段航迹段联通到目标点的一条路径。航迹规划的流程如图1所示。

图1 航迹规划流程图Fig.1 Flowchart of path planning

3 仿真分析

采用Matlab R2010b 软件编程构建无人机三维航迹规划仿真模型，进行仿真实验检验APSO 算法的有效性。假设山区环境为100 km×100 km×4 km 的区域，无人机的起始点S 坐标为(0,0,0.06)，第1 个任务点T1坐标为(43,81,1.1) ，第2 个任务点T2坐标为(80,23,1.2)。无人机从起始点飞行到T1点上空执行自然灾害（如山体滑坡、泥石流等）勘察任务，然后再飞行到T2点上空勘察，最后返回起始点。本文重点研究航迹规划问题。为了简便起见，将上述各段航迹分别进行规划，即，第1段规划航迹为：S →T1；第2段规划航迹为：T1→T2；第3段规划航迹为：T2→S。无人机在T1点上空盘旋一周执行勘察任务，并完成转弯，然后飞行到T2点上空勘察；盘旋勘察工作这里不进行研究。

设山区基准地形高度为z0=0.05 km，并设置8 座山峰和8个风切变椭球区，山峰参数见表1，风切变椭球区参数见表2。

表1 山峰参数Tab.1 Parameters of the peaks

表2 风切变椭球区参数Tab.2 Parameters of the wind shear ellipsoid areas

然后，利用式（1）计算出x ∈[0,100] 、y ∈[0,100]范围内的山区地形高度z1。设置无人机的约束条件分别为：Lmax=500 km ，Lmin=1.0 km ，Δψmax=1 rad ，θmax=0.8 rad ， Δhmax=0.5 km ， ht=0.02 km ，hsafe=0.05 km。为了降低航迹规划的复杂度，提高规划效率，对规划航迹高度采用直接设置方式，即，根据山区地形高度和无人机最大/小飞行高度约束，设置无人机的实时航迹高度高于其下方地形高度0.2 km，满足高度约束。

采用APSO算法进行航迹规划实际上是对全部航迹点的x 和y 坐标值进行迭代寻优。

应用仿真试凑法进行反复尝试，确定算法参数如下：N=300，D=25，wmax=0.8，wmin=0.2，c1max=3.0，c1min=1.0，c2max=3.0，c2min=1.0，tmax=300，wL=0.4，wT=0.3，wS=0.3，fT1=300，fT2=300，C0=900。

为了与传统PSO 算法进行对比，也采用了传统PSO算法进行无人机航迹规划仿真实验。

在传统PSO算法中,w、c1和c2都为常数，其取值偏大或者偏小都会影响航迹规划效果，通常是根据设计经验和仿真试凑法进行选取。不失一般性，选取其值为前面APSO 算法的最大值和最小值之间的平均值，即w=0.5，c1=2.0，c2=2.0，其他参数与APSO算法相同。仿真实验共进行了50次，统计出各段规划航迹的平均适应度值，2种算法的适应度值对比如表3所示。由表3可以看出，对于3段规划航迹，本文提出的APSO 算法比传统PSO 算法均获得了更小的适应度值，表明本文算法具有更高的全局搜索能力和搜索精度。

表3 2种算法的适应度值对比Tab.3 Contrast of fitness values of two algorithms

给出最后一次的仿真结果，基于APSO 算法的最优航迹及其适应度值收敛曲线如图2 所示，基于传统PSO算法的最优航迹及其适应度值收敛曲线如图3所示。

图3 基于PSO算法的无人机航迹规划Fig.3 UAV path planning based on the PSO algorithm

4 结束语

本文提出了一种自适应粒子群优化算法，用来解决固定翼无人机在三维环境空间中的航迹规划问题。以无人机飞行到山区环境执行自然灾害勘察任务为例，开展了研究工作。考虑了山区地形环境威胁和无人机自身约束条件。在此基础上，设计了自适应粒子群优化算法的适应度函数和航迹规划算法流程。为了检验APSO 算法的有效性，分别采用APSO算法和传统PSO算法，开展了无人机三维航迹规划仿真实验。仿真结果对比表明，所提出的APSO 算法比传统PSO 算法具有更高的全局搜索能力和搜索精度。本文研究主要考虑了环境中的静态威胁，下一步将研究无人机规避突发动态威胁的航迹规划方法。