改进APF与Bezier相结合的多无人机协同避碰航路规划

杨 勇, 丁 勇, 黄鑫城

(南京航空航天大学自动化学院，南京 211106)

0 引言

多无人机协同目标追踪与避障是指多架无人机协同合作对目标进行追踪并在追踪过程中规避障碍物，是无人机协同合作领域的研究热点之一，在军用和民用领域均具有重要的应用价值[1]。针对多无人机目标追踪与防碰撞问题，国内外学者做了大量的研究。文献[2]将支持向量机与人工势场法结合，采用球面对称函数构建无人机导航模型，使得多架无人机无碰撞飞行；文献[3]基于认知博弈制导算法，建立无人机与入侵机的运动学模型，将无人机机间防碰问题转化为双方博弈问题，实现多无人机自主防碰撞。卡尔曼滤波预测算法也是解决多无人机防碰撞问题的一种有效方法。文献[4]基于流体扰动计算与交互多模型相结合，完成多无人机三维环境下的无碰撞航路规划；文献[5]采用自适应卡尔曼算法预测僚机的状态进而控制僚机机动，当无人机编队过程中机间距离过近时，僚机便迅速做出规避动作，实现无人机编队互不碰撞。人工势场法是一种虚拟力法，由于算法简单，运算速度快，能够保证路径安全等特点，在路径规划中得到应用，但该算法也存在规划路径振荡等问题[6]。针对这些问题不少学者进行了研究，文献[7]模拟橡皮条的拉紧法去除机器人行驶路径的冗余节点，并用对称多项式处理得到平滑路径；文献[8]在圆锥曲线引力场函数的基础上增加一个指数项到引力场函数中，消除了奇异值点,避免了抖动现象。

总的来看，无人机碰撞规避问题大多研究多无人机在追踪目标过程中对环境障碍物的规避，从而实现无人机的目标追踪路径规划，而对于多无人机之间的机间防碰，大多单纯采用组网通信方式，很少采用路径规划算法达到多无人机之间的防碰目的；此外，基于人工势场法航路规划中的路径振荡问题多针对地面机器人展开，但由于地面机器人与无人机的运动特性有很大不同，如何避免无人机航路规划过程中的航路振荡是迫切需要解决的问题。

针对上述问题的不足，本文提出一种改进APF与Bezier相结合的多无人机协同避碰航路规划方法。该方法通过在人工势场法中增加无人机自身斥力场进行多无人机协同目标追踪航路规划，并结合分段Bezier曲线进行在线航路平滑优化，为解决多无人机机间防碰，消除航路规划过程中易出现的路径振荡现象，提供了一个有效的解决途径。

1 人工势场法简介

人工势场法是由KHATIB于1986年提出的一种虚拟力法，其基本思想是:通过构造一种抽象的势力场，无人机在追踪目标过程中受到该势力场的影响，目标对无人机产生“引力”，障碍物对无人机产生“斥力”，引力与斥力叠加求出合力控制无人机的运动[9]。图1为无人机的引力与斥力受力示意图。

图1 引力与斥力示意图Fig.1 Diagram of attraction and repulsion force

1.1 势函数

设q为无人机处于二维环境空间中的一点，则该无人机在该点的引力势函数和斥力势函数分别定义为

(1)

(2)

式中:k和m分别为引力增益和斥力增益；ρa(q)和ρr(q)分别为q点到目标a和障碍物r的距离；ρ0是障碍物的最大影响半径。

1.2 斥力函数与引力函数

无人机处在引力势场和斥力势场内，所受的合力F表示为

F=Fatt+Frep

(3)

式中:Fatt为目标对无人机的引力;Frep为障碍物对无人机的斥力。F决定了无人机的运动方向。

定义障碍物对无人机的斥力和目标点对无人机的引力分别为对应斥力场和引力场的负梯度[10]，则q点受到目标点的引力Fatt(q)和障碍物的斥力Frep(q)分别为该点引力势函数和斥力势函数的负梯度，即

Fatt(q)=-grad(Uatt(q))=-kρa(q)

(4)

>。

(5)

多个障碍物共同影响无人机运动时，根据力的叠加性理论可得出无人机所受合力F为

(6)

式中：N为障碍物的个数;Frep i为第i个障碍物对无人机的斥力。

2 改进APF的多无人机协同避碰航路规划

本文提出一种改进APF方法解决多无人机协同目标追踪过程中机间防碰撞问题。首先通过建立无人机的引力与斥力受力模型，保证每架无人机在追踪目标的过程中均可有效规避障碍物，在此基础上，将每架无人机视为移动障碍物，自身产生斥力势场来避免与其他无人机之间的相互碰撞。

2.1 模型假设

对于多无人机协同目标追踪问题，实际情况下需要考虑的因素过多将不宜于理论分析，为了重点分析问题，本文做出了一些合理的假设：

1) 忽略无人机的种类、机型、性能等因素，将无人机视为质点；

2) 本文所指的障碍物分为静止障碍物和移动障碍物，其中静止障碍物一般有敌方雷达探测区域、火炮打击区域、建筑物或山脉，但其形状都视为一定大小的圆，且这些障碍物的位置认为是已知的；

3) 移动障碍物可以由无人机机载的视觉传感器和避障传感器快速精确地感知；

4) 无人机机载的是高性能飞行控制器和视觉处理器，具备快速计算能力,可进行较复杂计算。

2.2 单架无人机的人工势场

在多无人机协同目标追踪与避障过程中，每架无人机都会受到目标对它的引力作用和障碍物对它的斥力作用。

定义1设q点为第j架无人机在空间中的一点，则对于第j架无人机而言，目标a对它的引力和障碍物r对它的斥力分别为

(7)

(8)

在多无人机协同目标追踪中，通常存在多个障碍物，因此无人机受到的斥力是多个障碍物对它产生的斥力的叠加。第j架无人机所受到的合力Fj为

(9)

2.3 机间防碰人工势场

图2是无人机自身斥力场示意图。

图2 无人机自身斥力场示意图Fig.2 UAV’s own repulsive field

在多无人机协同目标追踪的过程中，为了防止多无人机机间碰撞，造成机体毁坏导致任务失败，本文将无人机视为移动障碍，对其建立一个斥力势场，使得每架无人机对其他无人机产生一定的斥力作用，这样无人机之间就能时刻保持在安全距离范围之外，避免相互碰撞。

定义2设q为空间中的一点，则第j架无人机产生的斥力势场函数为

(10)

相应的，在q点的无人机受到第j架无人机的斥力是式(10)斥力势函数的负梯度，即

(11)

2.4 APF算法规避局部极小的措施

在无人机利用人工势场法对目标进行追踪过程中，当无人机尚未到达目标，而此时其所受目标引力和障碍物斥力的总合力为零时，无人机陷入局部最优状态，本文提出如下逃离局部最优的方法。

当无人机所受引力和斥力的合力为零，但引力不为零时，引力和合斥力大小相等、方向相反，可以在无人机和目标之间假想存在一个虚拟障碍物，其对无人机产生的斥力就是此时所有实际障碍物对该无人机的合斥力，如图3所示。虚拟障碍物的具体位置由式(5)反推出ρr(q)来确定，该虚拟障碍物斥力范围仍是以ρ0为半径的圆，此时无人机逃离局部最优的飞行方向就是沿着圆的切线方向，左右切线均可，逃离局部最优后再按照原来的算法飞向目标。

图3 无人机逃离局部最优的方向示意图Fig.3 The direction of UAV escaping the local optimal

3 Bezier曲线的路径平滑

用改进人工势场法进行的无人机航路规划存在航路折线多、转折角度大等问题，尤其是无人机在障碍物的周围容易出现路径振荡，会造成无人机大角度机动动作频繁，使无人机执行任务效率低，追踪目标风险大。本文提出分段Bezier曲线对改进人工势场法规划出的航路进行平滑优化，有效地解决了人工势场法路径规划中的路径振荡问题，使得平滑后的航路适合无人机的飞行。

3.1 Bezier曲线的原理

Bezier曲线是一种参数多项式曲线，n阶Bezier曲线定义为[11]

(12)

式中：Pi(0≤i≤n)为曲线的第i个控制点，顺次连接从P0到Pn的折线被称为Bezier曲线的控制多边形；Bi,n(u)为n次Bernstein多项式，它的表达式为

(13)

3.2 分段Bezier曲线的连接

由于控制点数的增加会引起Bezier曲线次数的提高，而高次多项式又会带来计算上的困难和算法的鲁棒性差[12],所以本文采用分段Bezier曲线进行航路规划平滑优化，然后将各段路径相连接，并在接合处保持一定的连续和光滑性。

假设两条Bezier曲线P(t)和Q(t)的控制点分别为Pi(i=0,1,…,n)和Qj(j=0,1,…,m)，如图4所示,若把两条曲线连接起来，则该曲线在连接处应具备以下特点[13]：

1) 连续的充要条件是Pn=Q0；

2) 平滑的充要条件是Pn-1,Pn(Q0),Q1三点共线。

图4 两段Bezier曲线连接图Fig.4 Connection of two fragments of Bezier curve

3.3 分段Bezier曲线解决路径振荡

采用上述改进人工势场法对无人机进行航路动态规划会出现路径振荡的现象，这里提出分段Bezier曲线的方法来解决。具体思路是：假设在t0,t1,t2,t3时刻利用人工势场法计算出的飞行航点分别是P0,P1,P2,P3;V1,V2,V3分别是线段P0P1,P1P2,P2P3的中点，则由控制点V1,P1,V2和V2,P2,V3生成的Bezier曲线在连接点V2处满足连续和平滑性条件。若当前时刻是t2，此时无人机已按Bezier曲线由V1飞到V2，利用人工势场法计算出经过t3-t2后的航点P3，沿着V2,P2,V3生成的Bezier曲线飞行即为下一个时间段的航行路线。这样使得原本振荡的路径得到平滑，解决了当无人机靠近障碍物飞行时出现的路径振荡问题。示意图如图5所示。

图5 分段Bezier曲线的航路规划Fig.5 The route planning of a sectional Bezier curve

4 仿真验证与结果分析

本章对所提算法进行仿真验证与分析，主要分为两个步骤：

1) 采用本文改进的人工势场法进行多无人机协同目标追踪的航路规划，并与未增加无人机斥力场进行的人工势场法航路规划进行对比；

2) 采用分段Bezier曲线对改进势场法动态规划的航路进行在线平滑优化。

4.1 仿真环境与参数设置

仿真实验在Windows10操作系统中Matlab2012软件下进行，微机的处理器是Intel Core i5-3210M，主频2.5 GHz，内存为4 GiB。

在仿真中有3架无人机UAV1，UAV2，UAV3，起始位置分别为 (50 km,0 km)，(8 km,0 km)和(10 km,20 km)，目标按一定轨迹运动。共有15个障碍物按编号1～15分布在120 km×120 km的目标追踪区域内，其中,编号12为突发的移动障碍物。具体仿真参数如表1所示。

表1 仿真参数设置

4.2 仿真步骤及算法流程

无人机在tk时刻机载传感器感知周围环境，获取障碍物、目标和其他无人机的位置，然后规划出无人机下一个航点，飞行一段时间Δt，在tk+1=tk+Δt时刻到达下一个航点。仿真最终目的是使从不同起点出发的3架无人机能避开所有障碍物且机间互不碰撞并最终全部到达目标位置，即追踪到目标。

图6为仿真框图。

图6 仿真框图Fig.6 Block diagram of simulation

仿真图分为3个部分，首先构建每架无人机的受力模型，对3架无人机使用未增加无人机斥力场的人工势场法，进行多无人机目标追踪的航路动态规划，各无人机单独规划互不干扰；其次，在增加无人机斥力场后对3架无人机进行协同目标追踪的航路动态规划，无人机之间因斥力存在互不碰撞;最后，在相同环境下，使用分段Bezier曲线在改进人工势场法动态规划过程中进行在线航路平滑优化。图7为改进人工势场法与分段Bezier曲线结合进行多无人机协同目标追踪航路规划的仿真流程图。

图7 仿真流程图Fig.7 Flow chart of simulation

4.3 仿真结果及分析

4.3.1 改进人工势场法目标追踪航路规划

未增加无人机斥力场的目标追踪航路规划仿真结果如图8所示,15个障碍物按照1～15进行编号，障碍物的内圆表示障碍物的大小，无人机绝对不能与它碰撞。障碍物的外圆表示障碍物斥力场的影响范围，此范围是半径为10 km的圆。三角形代表无人机的起始位置。从3架无人机目标追踪的航线中可以看出，所有无人机均避开障碍物，最终追踪到目标。但是，无人机UAV1和UAV2的航线在障碍物5和障碍物6之间产生了重叠，仿真参数中设置UAV1,UAV2的规划步长分别为1 km和1.3 km，这2架无人机将同时到达航路重叠区，在此区域发生碰撞，致使多无人机协同目标追踪任务失败。

图8 未增加无人机斥力场的航路规划Fig.8 The route planning without UAV’s repulsive field

为了解决这个问题，采用所提出的改进人工势场法，增加无人机自身斥力场，使得每架无人机对其余无人机均产生斥力作用，即无人机之间互斥，互斥的影响范围为半径5 km的圆，仿真结果见图9所示。

图9 改进人工势场法的航路规划Fig.9 The route planning with UAV’s repulsive field

从图9中可明显看出，UAV1和UAV2的航线在障碍物5和障碍物6之间产生了航路分离，并未出现图8中的航线重叠现象，这是无人机之间相互排斥的作用导致的，解决了多无人机协同目标追踪过程中的机间防碰问题。但可以明显看出，当无人机在障碍物附近时，航路出现了严重的振荡现象，这在实际情况中无论从飞机的机动性能、能源消耗还是从实时追踪方面考虑，都是绝对不允许出现的。

4.3.2 路径振荡平滑优化

采用改进人工势场法结合分段Bezier曲线进行航路在线平滑优化，仿真结果如图10所示，可以看出平滑优化的效果显著，消除了路径振荡现象。

图10 结合分段Bezier曲线的航路平滑优化Fig.10 Path smoothing combining the sectional Bezier curve

5 结束语

本文提出了一种改进人工势场法的多无人机协同避碰航路规划方法，根据无人机协同目标追踪的飞行特点，给出了无人机自身斥力场和斥力的合理定义，使多无人机在协同目标追踪过程中互不碰撞。当人工势场法陷入局部极小时，通过构造虚拟障碍物的方法，使得无人机逃离局部最优；提出分段Bezier曲线对规划路径进行在线实时平滑优化，在两个分段Bezier曲线连接处保证了曲线的平滑性，防止了无人机飞行做大角度转弯，平滑后的航路不仅不再出现航路振荡现象，而且整条航线变得更加平滑顺畅。