基于改进人工势场法的无人机避让航迹规划

杨 萌，王 玥

（北京理工大学机电学院，北京100081）

0 引言

近年来，随着无人机技术的不断发展，无人机被逐渐应用于航空喷洒、航空物探、航空摄影、电力巡线、石油作业等通航作业领域。为保障飞行安全，通常将无人机限制在特定空域内，与有人机隔离运行。在有限的空域资源下，隔离运行方式将难以满足无人机、有人机日益增长的应用需求，无人机进入低空空域融合运行是我国通用航空事业发展的必然趋势［1］。

在低空空域内运行的中小型无人机具有机动能力强、受天气等因素影响大、活动空间范围和类型广泛等特点，融合运行的安全问题严峻。其中，如何避免无人机间或无人机与有人机间的空中碰撞冲突，是实现融合运行的一个核心技术。美国NASA的Ames研究中心将飞行冲突解脱分为3个阶段：1）战略阶段，在20min前实现冲突解脱；2）战术阶段，在2min前实现冲突解脱；3）空中防撞系统（Traffic Alert and Collision Avoidance Sys⁃tem，TCAS）阶段，即在上一阶段解脱失败的情况下，飞行器直接进行垂直机动［2］。对于低空运行的安全保障而言，地面通信、导航和监视系统只能有效覆盖终端区等管制空域，大部分低空飞行活动并不能获得管制服务，且中小型无人机由于成本及体积限制，通常并不搭载TCAS系统。因此，在低空空域内运行的中小型无人机必须具备战术解脱能力。

1 感知与规避系统

各国进行的无人机空域安全研究主要可分为陆基感知与规避、空中感知与规避2种。

陆基感知与规避系统利用陆基系统使飞行器在规定的空域内保持一定距离。陆基系统包括雷达、显示设备、通信设备和软件。陆基感知与规避的方案将逐渐削减现有自动化系统的限制，帮助无人机实现在国家空域系统内的自由飞行，可以很好地适用于大中型无人机系统［3］。

空中感知与规避系统注重发展能够同时进行自动分离与避免碰撞的机载设备，保障一定范围内的飞行安全。该性能的目的在于使有人机飞行员和无人机操作员能够在任何空域中通过安全高效的方式避免冲突，避免与其他飞行器发生碰撞。无人机必须在目标到达危险前进行避让机动，最终恢复到原飞行轨迹并与入侵机重新保持安全间隔［4］，整个冲突的避让流程如图1所示。

本文拟针对感知与规避实际背景，对避让航迹规划模块进行算法设计与模型优化，并通过仿真验证其可靠性与优化效率。

图1 无人机感知与规避流程Fig.1 Process of sense and avoidance of UAV

2 避让航迹规划算法设计

目前，无人机在航迹规划中使用较多的智能优化算法包括A∗算法、遗传算法、蚁群算法、粒子群算法和人工势场法等。A∗算法适合进行全局最优航迹规划，理论上可以保证全局最优解，但搜索空间的增大、内部所存储的数据的增加、计算的增大、搜索时间的变长，导致其实时性无法获得保证［5］。遗传算法和粒子群算法具有较强的鲁棒性，能自适应地调整搜索方向，不受搜索空间条件的约束，但其局部搜索能力低，在进化后期收敛速度慢且容易陷入局部最优［6⁃7］。蚁群算法在对路径进行搜索时是随机的，由于信息素挥发的原因，其可能陷入局部最优情况［8］。人工势场法具有结构简单、计算量小、实时性好等优点，但如果目标距离障碍物太近或斥力与引力相等，则容易陷入局部极小值和局部震荡［9］。

考虑到感知与规避系统对高实时性的要求，放弃A∗算法、遗传算法、蚁群算法等搜索算法，采用实时性更强的人工势场法进行规避航迹的计算。

文献［10］将人工势场法和蚁群算法进行了结合，通过人工势场法构建了环境与目标之间的启发因子，并将其作为初始信息素带入蚁群算法，帮助蚁群算法快速收敛航迹。文献［11］通过搜索未知环境中的目标点，设计了一种无死锁人工势场法，有效解决了局部极小值问题。文献［12］设计了一种新的势能函数，避免了计算过程中繁琐的逆运动学计算，大大降低了算法的复杂程度。文献［13］在传统人工势场法中加入了调节因子，同时采用遍历搜索法解决了局部极小值问题。文献［14］引入了斥力偏转模型，引导机器人在路径规划时避开局部极小点，并同时通过引入斥力增益系数函数，优化了航向改变过大的问题。文献［15］提出了一种虚拟目标法，解决了局部极小值的问题。文献［16］提出了二次引力势场法，通过传统人工势场法计算出了一条预规划航迹，而后以此路径所提供的航迹点作为基础引力点，再次利用人工势场法规划航迹。

针对前文所述的人工势场法容易陷入局部极小值的问题，国内外诸多学者已进行了多项研究，并提出了多种改进方法，本文不再赘述。以下将结合感知与规避问题的实际背景，给出人工势场法局部震荡问题的解决方案，并对威胁区模型进行优化。

2.1 人工势场法的基本原理

人工势场法的基本思想是将飞行器在规划空间中的运动看作一种在虚拟力场中的受力运动，障碍物或威胁区域对其产生排斥力，目标点对其产生吸引力，飞行器在合力的作用下朝着目标点运动［17］。人工势场法航迹规划的二维示意图如图2所示。

图2 人工势场法航迹规划的二维示意图Fig.2 Two-dimensional diagram of artificial potential field method

引力势场函数与斥力势场函数可分别表示为

其中，X=（x，y，z）为飞行器的位置向量，α和β分别为引力与斥力增益系数，X、Xg、Xo分别表示当前点、 目标点、 障碍物的空间位置，ρ(X，Xg)、ρ(X，Xo)分别为当前点到目标点和障碍物的空间距离，ρ0为障碍物的斥力作用范围。

对于二元及以上函数而言，某一点方向导数的方向可以有无数个。所有方向导数中存在一个最大值，其方向为梯度的方向，方向导数最大值的大小为梯度的模。

对引力势场函数和斥力势场函数求负梯度，可得相应引力函数与斥力函数

飞行器所受合力为

2.2 局部震荡及其改进

在利用人工势场法进行航迹规划时，其规划航迹有可能出现不必要的震荡现象。主要原因是在某一航点处引力大于斥力，而在下一航点处斥力大于引力，两种情况多次循环导致航迹来回震荡。如图3所示，由式（3）和式（4）可知，距离目标点d越远的点所受引力越大，距离威胁点z越远的点所受斥力越小。在规划平面内，必然有1条斥力与引力大小相等（方向不一定相反）的平衡线L。当前点mi所受到的引力大于斥力，则其必然向上前进1个步长l，得到航点mi+1。此时，点mi+1可能越过平衡线L，使其受到的引力小于斥力。向远离威胁点方向规划，得到航点mi+2。点mi+2将再次越过平衡线，以此类推，形成局部震荡。

图3 局部震荡示意图Fig.3 Diagram of local oscillation

针对感知与规避实际背景，可在航迹规划时加入无人机最大转弯角约束。由于无人机自身动力学性能的影响，无人机飞行方向的改变受到最大转弯角（或最小转弯半径）的约束。设无人机最大转弯角为θmax，最小转弯半径为Rmin，无人机飞行的转弯角为α，转弯半径为r，要求规划出的航迹中的每一点的曲率半径都必须大于Rmin，则约束表达式为

其中，li为步长，最小转弯半径可由下式得出

其中，vmin为无人机的最小飞行速度，Nymax为无人机最大法向牵引力。

对于加入最大转弯角约束的人工势场法，当规划到图3所示的斥力与引力的平衡线附近的点mi+1时，受最大转弯角θmax的约束，其下一点不会做出类似图中点mi+2的大机动，可有效减缓或消除局部震荡影响，其改进程度受最大转弯角θmax大小的影响。

未进行局部震荡改进的仿真结果如图4所示。其中，起始点为A（0，0，0），目标点为B（100，100，100），威胁点为C（50，50，50），规划步长l=1，目标点引力增益系数α=0.1，威胁点斥力增益系数β=500，威胁区势场范围P=35。加入最大转弯角约束的仿真结果如图5所示。其中，起始点为A′（0，0，0），目标点为B′（100，100，100），障碍点为C′（50，50，50），规划步长l=1，目标点引力增益系数α=0.1，威胁点斥力增益系数β=500，威胁区势场范围P=35，最大转弯角θmax=45°。

图4 原算法的仿真结果Fig.4 Simulation results of original algorithm

图5 改进后的算法仿真结果Fig.5 Simulation results of improved algorithm

2.3 威胁区模型优化

避让航迹规划过程中的威胁区域来源于航迹预测模块得出的预测航迹。结合一般飞行器机动性能，得到威胁区的结构如图6所示，类似于椎体。

图6 威胁区的结构示意图Fig.6 Diagram of threat area structure

在感知与规避实际背景下，在利用人工势场法进行航迹规划的过程中，其威胁区势场模型若采用传统的以威胁区域中心为圆心构建球体势场的方法，容易出现因规划空间过小而导致的规划航迹与实际最优解偏差过大的问题，进而造成空域和能量资源的浪费。

为此，本文采用将威胁区域结构模型进行分解的方法构建威胁区势场模型。将威胁区域Z非均匀离散为多个威胁点｛z0，z1，…，zmax｝，则越靠近航迹预测点的区域威胁点分布越密集。传感器最终探测点在XY平面的投影为z0（xz0，yz0），Kalman滤波预测Δt时刻后航点在XY平面的投影为zk（xzk，yzk）。飞行器从最终探测点开始，以最大偏航角θ′max向Y轴正向飞行Δt时刻后在XY平面的投影为zm（xzm，yzm），则有

最大偏航点zm和最终探测点z0的连线与X轴的夹角为θm0，航迹预测点zk和最大偏航点zm的连线与X轴的夹角为θkm，则有

二维威胁点集z1的分布为

其中，p为密集因子，p越大则航迹预测点zk附近的威胁点越密集。

改进后的威胁点分布如图7所示。威胁区结构类似于椎体结构，图中的各条线构成了整体威胁区结构的模型轮廓，椎体底面各线汇集于航迹预测模块给出的预测点。作为入侵飞行器的高概率出现点，其周围威胁点分布密度较高。在远离预测点的区域，威胁点分布密度越来越低，在锥顶处达到最低。

每个威胁点zi单独形成斥力势场Urepi与斥力Frepi，飞行器所受斥力合力为所有斥力的叠加，则有

图7 威胁点分布仿真图Fig.7 Simulation of threat points distribution

2.4 无人机性能约束模型

在利用人工势场法进行避让航迹规划的过程中，所规划航迹必须满足无人机自身性能的约束。除上述最大转弯角约束之外，还有最大航程约束、飞行高度约束和最大爬升/俯冲角约束，具体模型如下。

（1）最大航程约束

由于携带的动力能源有限，无人机面临着最大飞行距离的约束，即最大航程约束，这要求避让航迹长度lavoid与其余段航迹长度lbefore的和必须小于无人机能够飞行的最大航程值lmax。 设每2个航点之间的航迹段长度为li，航迹总长度的示意图如图8所示。

图8 航迹总长度示意图Fig.8 Schematic diagram of total track length

则无人机的航迹总长度ltotal应满足

（2）飞行高度约束

我国通用航空管理局对各类无人机的飞行活动制定了飞行高度限制，设无人机飞行高度的约束区间为［Hmin，Hmax］，则避让航迹中各航点的高度h需满足

（3）最大爬升/俯冲角约束

最大爬升/俯冲角是指无人机在三维空间中改变其飞行高度时可机动达到的最大角度，角度过大可能会引起失速。设最大爬升/俯冲角为γmax，无人机实际爬升/俯冲角为φ，当前航迹节点的坐标 为 (xi，yi，zi)，下 一 航 迹 节 点 的 坐 标 为(xi+1，yi+1，zi+1)，则约束满足下式

3 仿真验证

根据上述威胁区模型的优化方法，对感知与规避过程中的避让航迹规划进行仿真验证，并将其与改进之前的算法进行对比。其中，起始点、目标点、威胁点、引力增益系数、斥力增益系数固定不变，仅对威胁区势场范围进行修改，并比较两种算法的仿真结果。

假设我方无人机于t时刻发现入侵飞行器，以此时我方无人机的位置作为原点建立直角坐标系。 威胁区域中心点的坐标为（xobs，yobs，zobs），威胁区势场范围为P1，以此构造威胁区模型，进行基于传统人工势场的避让航迹规划。入侵飞行器航迹预测点的坐标为（xforecast，yforecast，zforecast），威胁点zi的自坐标为（xobsi，yobsi，zobsi），威胁点的势场范围为P1，以此构造改进威胁区模型，并进行改进后的避让航迹规划。

为清晰观察传统算法的规划结果是否成功绕开了威胁区域，需在传统算法避让航迹规划的仿真结果中加入威胁点，以显示威胁区域，且不将其作为障碍点代入计算。传统避让航迹规划仿真参数的设置为：规划步长l=1，目标点引力增益系数α=0.1，威胁点斥力增益系数β=500，威胁区域中心（79，50，70）。当威胁区势场范围P1=15时，得出的仿真结果如图9、图10所示。航迹从威胁区穿过，未成功规避威胁区域。

保持其他条件不变，扩大威胁区势场范围，使规划航点提前受斥力作用。当威胁区势场范围P1=30时，得出的仿真结果如图11、图12所示。航迹从威胁区穿过，未成功规避威胁区域。

图9 传统算法的仿真结果（P1＝15）Fig.9 Simulation results of original algorithm （P1＝15）

图10 传统算法的仿真结果（图9曲线右转一定角度）Fig.10 Simulation results of original algorithm（curve in Fig.9 turns right at a certain angle）

图11 传统算法的仿真结果（P1＝30）Fig.11 Simulation results of original algorithm （P1＝30）

图12 传统算法的仿真结果（图11曲线右转一定角度）Fig.12 Simulation results of original algorithm（curve in Fig.11 turns right at a certain angle）

保持其他条件不变，再次扩大威胁区势场范围，使规划航点提前受斥力作用。当威胁区势场范围P1=40时，得出的仿真结果如图13、图14所示。航迹未从威胁区穿过，成功规避威胁区域，此时总航点数为263。

改进后的避让航迹规划仿真参数的设置为：规划步长l=1，威胁区势场范围P2=15，目标点引力增益系数α=0.1，威胁点斥力增益系数β=500。 得出的仿真结果如图15、图16所示，成功规避威胁区域，总航点数为219个。

图13 传统算法的仿真结果（P1＝40）Fig.13 Simulation results of original algorithm （P1＝40）

图14 传统算法的仿真结果（图13曲线右转一定角度）Fig.14 Simulation results of original algorithm（curve in Fig.13 turns right at a certain angle）

图15 改进算法的仿真结果Fig.15 Simulation results of improved algorithm

图16 改进算法的仿真结果（图15曲线右转一定角度）Fig.16 Simulation results of improved algorithm（curve in Fig.15 turns right at a certain angle）

4 结论

在相同规划步长的条件下，避让航迹总航点数可直接反映航迹总长度，影响无人机动力能源的消耗。由上文可知，在感知与规避实际背景下，本文提出的改进人工势场法成功避让航迹的总航点数较传统人工势场法减少了16.7%。在节约了动力能源的同时，无人机的避让飞行耗时也有所减少，提高了任务执行效率。

虽然基于人工势场法的航迹规划实时性高，但本文采用的威胁区模型优化的改进人工势场方法扩大了威胁势场的数量，增加了计算量，其实时性也会成为未来实物实验中的隐患，日后需在算法效率方面继续改进。