CN112684807A - 无人机集群三维编队方法 - Google Patents

Abstract

本发明公开的无人机集群三维编队方法，旨在提供一种能够实现动态环境下避碰操作，解决无人机陷入局部极小点而无法移动问题的编队方法。本发明通过以下技术方案实现：基于改进型人工势场法，在地面站上设置每架无人机的目标点坐标，获取每架无人机的初始位置和无人机间的距离，规划出每架无人机单元的最优无障碍路径；在势场函数中加入动态调节因子,利用势场函数计算每架无人机单元所受的合力大小、方向，根据合力计算下一步无人机单元位置；然后判断是否陷入局部极小点，得出无人机编队主路径规划最优路径；生成目标与障碍物动态变化条件下协同性和执行高效性的预规划路径，将规划出的无障碍最优路径写入无人机飞控***，按照指令飞往目标点。

Description

无人机集群三维编队方法

技术领域

本发明涉及无人机的编队飞行技术领域，具体来说涉及基于改进型人工势场法的多无人机三维编队方法。

背景技术

人工势场法(Artificial Potential Field，APF)是一种虚拟力法。它的基本思想是引入一个时变的人工势场，利用基于视觉传感器、声纳传感器等获取信息，然后应用人工势场函数实现移动机器人对静态障碍物(Obstacle)的避碰撞。它将机器人在周围环境中的运动，设计成一种抽象的人造引力场中的运动，目标点(Goal)对移动机器人产生“引力”，障碍物对移动机器人产生“斥力”，最后通过求合力来控制移动机器人的运动。人工势场法的思想类似于电子在正负电荷产生的电场中运动。障碍物对移动机器人产生斥力，目标点产生引力，引力和斥力的合力作为移动机器人的加速力来控制机器人的运动方向和计算机器人的位置。该方法构造一个叫势函数的标量函数，使得目标点对应于其最小值，而障碍物区域对应势函数的一些较大的值。在任何其它位置势函数都是向目标点单调递减的。这样无论起始点在自由空间何处，只要路径存在，移动机器人都能通过势能值的负梯度方向找到目标点。由于移动机器人对周围环境信息的感知具有局限性,容易出现机器人陷入局部极小点的问题。当机器人处于局部极值点的时候容易产生锁死现象，容易陷入局部极小点，障碍物附近目标不可达问题、路径不是最优等问题，使机器人无法移动。因此传统人工势场法由于局部极小点问题而导致规划失败。

人工势场法在机器人的路径规划算法中已经有大量的应用，并常被用于解决平面路径规划问题。由于无人机集群广泛应用在搜索救援、交通监测中，无人机必须能够成功地感知并避开障碍物，避免与障碍物或其他无人机碰撞。因此近年来国内外研究者提出了许多航迹规划的方法，主要有单元分解法、基于数学规划的方法以及基于人工势场的方法。单元分解的方法是用许多单元合成的新空间来替代规划区域，单元分解法虽然可以较为准确地进行最优航迹规划，但其算法复杂度较高，难以保证实时性；基于数学规划的方法主要是用函数来模拟影响路径规划的条件，此类方法综合了轨迹距离、障碍位置和轨迹规划中的其他因素，但是算法过于复杂，函数的选取也需要着重考虑；传统的人工势场法在编队控制中，控制精度较差。传统人工势场方法在生成覆盖航路的时候，有一些现实中好用的模式无法生成。

改进后的人工势场法与其它三维航路规划算法相比具有显著的优点：首先，结合当前无人机运动状态进行避障规划，人工势场法在规划航路时，根据势力场计算当前位置受到的合力，将环境中运动的物体看作处于虚拟力场中的一个质点，虚拟力场由目标的吸引力场和障碍物的排斥力场组成，通过搜索沿着势函数下降的路线规划出避撞的航路。其最显著的特点即为计算量小，运算速度快。其次，利用人工势场法可以得到平滑而安全的航路，而其它航路规划算法不仅需要对航路进行平滑操作，可能还需要重新进行最小直飞距离、最大爬升角度等飞行安全性能检测。目前传统的人工势场法主要用于二维地面机器人编队。为了有效实现无人机三维编队飞行，必须对无人机三维坐标精准定位，而一般的无人机高度定位是不准确的。传统的航路规划算法是以固定的任务目标、稳定不变的飞行环境为假设前提而提出的，是静态的规划，而无人机在实际的勘探、商业等应用中，其任务目标与飞行环境都可能是变化和不确定的，由于传统航路规划算法存在在线规划能力不足。因此传统的航路规划算法无法满足无人机在动态变化的任务执行环境中快速运行期间对航路变化的要求。针对传统人工势场法的障碍物附近目标不可达、存在局部极小点和振荡的问题，现有技术对势场函数改进,保证目标点为势场的全局最小点。在动态势场中,引入是否陷入局部极小点的判断机制, 并结合一种“沿目标方向90°移动”的方法来跳出局部极小点,实现多无人机编队的路径规划、协同避障和防碰撞。此改进方法有效地弥补了传统人工势场法的不足,提高了人工势场法的实用性。然而当飞行环境地形精度要求较高时，地形栅格数量急剧增加，从而搜索空间变大，这些算法的规划用时会大幅增加，大大降低了无人机的反应速度，使其动态规划能力下降。其次这些方法规划出来的航路没有充分考虑无人机实际的飞行航迹与飞行性能，大多航路是以关键坐标点间直线连接组成不平滑的航路，这种连接方式必须考虑无人机安全指标，例如最大转弯半径、最小直飞距离等。因此，这些常用于全局航路规划的算法在针对动态环境的在线航路实时规划方面仍存在一定不足。所以本发明提出了一种回归搜索算法对最后的路径进行全局优化。

发明内容

为了提高无人机的避障性能,本发明针对传统的人工势场(APF)法无法适应复杂环境而陷入局部停滞状态、路径不够平滑的不足之处，根据改进的人工势场法，提供一种基于改进型人工势场法的无人机集群三维编队方法，通过该方法能够实现动态环境下避碰操作，解决无人机陷入局部极小点而无法移动的问题。

本发明的上述目的可以通过以下措施来达到：一种无人机集群三维编队方法，具有如下技术特征：基于改进型人工势场法，在地面站上设置每架无人机的目标点坐标，通过GPS、气压计、激光测距、超声波测距获取每架无人机的初始位置和无人机间的距离；在势场函数中加入动态调节因子,利用势场函数计算每架无人机单元所受的合力大小、方向，根据合力计算下一步无人机单元位置；然后判断是否陷入局部极小点，如果陷入局部极小点，采用局部极小点逃离策略，跳出局部极小点，否则，根据计算合力的大小、方向，计算下一步位置；基于回归搜索法进行路径优化，得到一条从起始位置到任务目标完成位置的最优或次优飞行轨迹，指定完成从起始点到目标点间的自主路径规划空域，得出无人机编队主路径规划最优路径；生成目标与障碍物动态变化条件下协同性和执行高效性的预规划路径，将规划出的无障碍最优路径写入无人机飞控***，无人机编队单元按照指令飞往目标点坐标。

本发明相比于现有技术具有如下有益效果：

本发明基于改进型人工势场法，在地面站上设置每架无人机的目标点坐标，结合GPS、气压计、激光测距、超声波测距计算准确的高度和距离，规划出每架无人机的最优无障碍路径，生成预规划路径弱化了目标点对无人机的吸引作用，增加了路径的连贯性；在势场函数中加入动态调节因子,规划全局最优路径，实现无人机编队在指定空域完成从起始点到目标点间的自主路径规划,且机群在该过程中具有良好的协同性和执行高效性,提高了编队的效率，具有较强的路径规划能力，满足无人机在目标与障碍物动态变化条件下的路径规划的安全性、实时性和可达性，并提高了动态环境中无人机的跟踪与避障速度。通过人工势场法的改进版本，避免了无人机编队的碰撞问题，解决了无人机陷入局部极小点而无法移动的问题。

本发明通过改进势场函数，从根本上解决了目标不可达问题；采用局部极小点逃离策略，跳出局部极小点，弥补了人工势场法易陷入局部极小而无法找到可行路径的不足。解决了传统人工势场法局部极小点问题、障碍物附近目标不可达问题，避免了无人机编队的碰撞问题，并且规划了每架无人机的全局最优路径，提高了编队的效率。

本发明借鉴人工势场的思想,将人工势场法的规划结果作为先验知识,针对飞行器的轨迹规划问题,采用基于回归搜索法进行路径优化，在当前特定的条件下，寻找一条从起始位置到任务目标完成位置的最优路径，不仅能够保证飞行任务的圆满完成，还能节约飞行时间和能耗降低飞行器过载。克服了传统人工势场法的缺点，具有较强的实用性。

本发明采用指定完成从起始点到目标点间的自主路径规划空域，得出无人机编队主路径规划最优路径，生成目标与障碍物动态变化条件下协同性和执行高效性的预规划路径，将规划出的无障碍最优路径写入无人机飞控***，无人机按照指令飞往目标点坐标。可以通过中央操控***对多架无人机进行分组，在组内进行无人机的位置、高度、速度、偏角等信息实时共享，小组内无人机数量可根据无人机密集程度进行调整，将单无人机局部行为构成多无人机全局行为的协同方式，集群内的各无人机单元将位置信息传递给中央操控***，可以根据无人机集群的实时位置信息进行调整，使无人机之间进行充分通信，从而降低增益系数的不利影响，解决机间防碰撞的问题。仿真结果表明：本发明能够保持无人机的稳定编队，并同时能够躲避飞行过程中的障碍以及避免飞机之间的碰撞。

本发明主要用于无人机三维编队飞行时，稳定编队。

附图说明

图1是本发明无人机集群三维编队的程序流程图；

图2是人工势场法的原理图；

图3是局部极小点的逃离示意图；

图4为回归搜索法的原理图；

图5为本发明方法的仿真效果图。

下面结合附图进一步详细描述本发明的技术方案。

具体实施方式

参阅图1。根据本发明，基于改进型人工势场法，在地面站上设置每架无人机的目标点(Goal)坐标，通过GPS、气压计、激光测距、超声波测距获取每架无人机的初始位置和无人机间的距离，结合相应信号分析处理算法、滤波算法计算得到准确的高度和距离，利用改进人工势场法规划出每架无人机单元的最优无障碍路径；在势场函数中加入动态调节因子, 利用势场函数计算每架无人机单元所受的合力大小、方向，根据合力计算下一步无人机单元位置；然后判断是否陷入局部极小点，寻找一条从起始位置到任务目标完成位置的最优或次优飞行轨迹，指定完成从起始点到目标点间的自主路径规划空域，得出无人机编队主路径规划最优路径；生成目标与障碍物(Obstacle)动态变化条件下协同性和执行高效性的预规划路径，将规划出的无障碍最优路径写入无人机飞控***，无人机编队单元按照指令飞往目标点。

基于改进人工势场法，编队算法根据障碍物和目标点，计算每架无人机所受的合力大小、方向，根据改进后的势场函数计算每架无人机所受的合力大小、方向，计算无人机飞往下一步位置；接下来判断无人机是否陷入局部极小点，如果陷入局部极小点，执行逃离局部极小点策略跳出局部极小点，如果没有，再根据合力计算下一步位置；最后判断是否到达目标点，如果没有，则重复上述步骤，则继续计算合力大小、方向；如果到达目标点，则进入下一步，基于回归搜索法进行路径优化，得出最优路径；最后无人机飞控根据规划出的无障碍最优路径发出指令信号，使无人机飞往目标。

本发明硬件***包括：包括地面站***，无人机机载飞控***，GPS定位***，超声波测距，气压计，激光测距等。无人机单元通过感知***获得环境信息进一步获得是否有障碍信息，根据获得的信息进行编队控制、避障控制以及避撞控制，得出相应控制量，将得出的控制量传到飞行控制***也就是自动驾驶仪，然后通过飞控***完成对无人机的控制。

参阅图2。根据人工势场法构造虚拟势场，利用改进人工势场函数、引入局部极小点判断机制、构建无人机(UAV)位置的路径直角坐标系XYZ，无人机受到目标点(Goal)的吸引力 F_atti，同时受到障碍物(Obstacle)的排斥力F_repi，无人机UAV在合力F_i的作用下向目标点移动，利用无人机合力F_i分解形成的受力图，采用局部极小点逃离法、回归搜索法优化势场函数，在传统人工势场函数的基础上引入各架无人机与其所对应目标之间相对距离l_gi(i＝1,2,3,...,n)，计算无人机第j个障碍物对第i架无人机所产生的斥力场U_rep1ij和第k架无人机UAV对第i架无人机所产生的斥力场U_rep2ik(i≠k)，计算公式为:

(i＝1,2,3,...,n；j＝1,2,3,...,m；k＝1,2,3,...,n)

式中，k₁、k₂为斥力增益系数，i≠k，

表示第i架无人机到对应目标点的距离，p是一个大于零的实数；l_oij表示第i架无人机到第j个障碍物的距离；l_max是人为设定的障碍物影响距离，l_ik表示第i架无人机与第k架无人机之间的距离；n表示无人机架数；m表示障碍物个数。

根据第j个障碍物对第i架无人机所产生的斥力场U_rep1ij、第k架无人机对第i架无人机所产生的斥力场U_rep2ik和引力增益系数η，得到第i架无人机与其对应目标点之间的引力场 U_atti，

当无人机与障碍物之间的距离大于影响距离，则无人机所受障碍物的斥力为零。以一个大于0的任意实数p，用

乘以原来的势场函数，这样就会使得无人机在目标点受到的斥力为0，确保目标点为整个势场的全局最小点，从而解决了障碍物附近目标不可达问题。

参阅图3。根据第i架无人机受到的障碍物(Obstacle)和其他无人机的斥力F_repi、第i架无人机的位置坐标P_i和第j个障碍物的位置坐标P_oj，可以得到斥力为和引力势场函数的负梯度：

式中，

和

分别是第j个障碍物的斥力方向的单位向量和第k架无人机的斥力方向的单位向量；F_repi1和F_repi2是F_repi的两个分力。则F_repi1指向无人机，F_repi2指向目标点(Goal)，

式中，k₁、k₂为斥力增益系数，p是一个大于0的任意实数，l_oij是第i架无人机与第j个障碍物间的距离；l_max是人为设定的障碍物影响距离；l_gi(i＝1,2,3,...,n)是在传统人工势场函数的基础上引进的各架无人机(UAV)与其所对应目标之间相对距离。

当无人机单元判断到陷入局部极小点后，沿着目标方向的法平面上任意方向移动，跳出局部极小点，如果无人机单元在前一时刻的位置与当前位置的距离小于某个设定值，则判断无人机陷入了局部极小点，此时，添加一个附加势场，无人机沿着与目标方向垂直的法平面上任意方向移动一段距离，从而逃离局部极小点。当F_repi＝F_atti，即无人机受到的引力恰好等于斥力，且方向相反时，无人机处于局部极小点中，无法移动。附加的势场函数为：

其中，a是一个大于零的常数，P_local代表局部极小点的位置，l_amax是附加势场的最大影响距离。陷入局部极小点的无人机在附加势场力的作用下逃离局部极小点。

参阅图4。在利用改进后的人工势场法规划处无障碍路径后，采用回归搜索法对路径进行全局优化。本实施例选用回归搜索法的路径优化，是一种全局优化方案，在改进人工势场法所规划出的路径上选取r个目标点{P₁,P₂,...,P_t,...,P_r}(若选取的点数越多，则迭代时间越长，但精度越高，所以需要折中考虑)，连接目标点P₁、P₂，若线段

与障碍物(Obstacle)安全距离l₀范围的边界圆不存在交点，则连接目标点P₁、P₃，依次重复该步骤。若

与障碍物安全距离l₀范围的边界圆不存在交点，但

与障碍物安全距离l₀范围的边界圆存在交点，则将

选作优化后的路径线段。再从P_k点开始，重复上述过程，直至到达对应目标点(Goal)坐标结束运算。本实施例所做的研究处在一个以时间变量为参考的动态势场中，若步长设置为常数，则无人机的运动是匀速的，目标点 {P₁,P₂,...,P_t,...,P_r}的选取实际上是根据无人机在等时间间隔下的不同坐标来确定的。

从图5仿真效果图可以看出回归搜索法最后规划的路径是全局最优的。

以上所述为本发明较佳实施例，应该注意的是上述实施例对本发明进行说明，然而本发明并不局限于此，并且本领域技术人员在脱离所附权利要求的范围情况下可设计出替换实施例。对于本领域内的普通技术人员而言，在不脱离本发明的精神和实质的情况下，可以做出各种变型和改进，这些变型和改进也视为本发明的保护范围。

Claims (10)

1.一种无人机集群三维编队方法，具有如下技术特征：基于改进型人工势场法，在地面站上设置每架无人机的目标点(Goal)坐标，通过GPS、气压计、激光测距、超声波测距获取每架无人机的初始位置和无人机间的距离，结合GPS、气压计、激光测距、超声波测距计算准确的高度和距离，规划出每架无人机单元的最优无障碍路径；在势场函数中加入动态调节因子,利用势场函数计算每架无人机单元所受的合力大小、方向，根据合力计算下一步无人机单元位置；然后判断是否陷入局部极小点，寻找一条从起始位置到任务目标完成位置的最优或次优飞行轨迹，指定完成从起始点到目标点间的自主路径规划空域，得出无人机编队主路径规划最优路径；生成目标与障碍物(Obstacle)动态变化条件下协同性和执行高效性的预规划路径，将规划出的无障碍最优路径写入无人机飞控***，无人机编队单元按照指令飞往目标点。

2.如权利要求1所述的无人机集群三维编队方法，其特征在于：基于改进人工势场法，编队算法根据障碍物和目标点，计算每架无人机所受的合力大小、方向，根据改进后的势场函数计算每架无人机所受的合力大小、方向，计算无人机飞往下一步位置；接下来判断无人机是否陷入局部极小点，如果陷入局部极小点，执行逃离局部极小点策略跳出局部极小点，如果没有，再根据合力计算下一步位置；最后判断是否到达目标点，如果没有，则重复上述步骤，则继续计算合力大小、方向；如果到达目标点，则进入下一步，基于回归搜索法进行路径优化，得出最优路径；最后无人机飞控根据规划出的无障碍最优路径发出指令信号，使无人机飞往目标。

3.如权利要求1所述的无人机集群三维编队方法，其特征在于：无人机单元通过感知***获得环境信息进一步获得是否有障碍信息，根据获得的信息进行编队控制、避障控制以及避撞控制，得出相应控制量，将得出的控制量传到飞行控制***也就是自动驾驶仪，然后通过飞控***完成对无人机的控制。

4.如权利要求1所述的无人机集群三维编队方法，其特征在于：根据人工势场法构造虚拟势场，利用改进人工势场函数、引入局部极小点判断机制、构建无人机(UAV)位置的路径直角坐标系XYZ。

5.如权利要求1所述的无人机集群三维编队方法，其特征在于：无人机UAV在合力F_i的作用下向目标点移动，利用无人机合力F_i分解形成的受力图，采用局部极小点逃离法、回归搜索法优化势场函数，在传统人工势场函数的基础上引入各架无人机与其所对应目标之间相对距离l_gi(i＝1,2,3,...,n)，计算无人机第j个障碍物对第i架无人机所产生的斥力场U_rep1ij和第k架无人机UAV对第i架无人机所产生的斥力场U_rep2ik(i≠k)，计算公式为:

式中，k₁、k₂为斥力增益系数，i≠k，

表示第i架无人机到对应目标点的距离，p是一个大于零的实数；l_oij表示第i架无人机到第j个障碍物的距离；l_max是人为设定的障碍物影响距离，l_ik表示第i架无人机与第k架无人机之间的距离；n表示无人机架数；m表示障碍物个数。

6.如权利要求5所述的无人机集群三维编队方法，其特征在于：根据第j个障碍物对第i架无人机所产生的斥力场U_rep1ij、第k架无人机对第i架无人机所产生的斥力场U_rep2ik和引力增益系数η，得到第i架无人机与其对应目标点之间的引力场U_atti，

7.如权利要求1所述的无人机集群三维编队方法，其特征在于：根据第i架无人机受到的障碍物(Obstacle)和其他无人机的斥力F_repi、第i架无人机的位置坐标P_i和第j个障碍物的位置坐标P_oj，可以得到斥力为和引力势场函数的负梯度：

式中，

和

分别是第j个障碍物的斥力方向的单位向量和第k架无人机的斥力方向的单位向量；F_repi1和F_repi2是F_repi的两个分力。

8.如权利要求7所述的无人机集群三维编队方法，其特征在于：F_repi1指向无人机，F_repi2指向目标点(Goal)，其中，

式中，k₁、k₂为斥力增益系数，p是一个大于0的任意实数，l_oij是第i架无人机与第j个障碍物间的距离；l_max是人为设定的障碍物影响距离；l_gi(i＝1,2,3,...,n)是在传统人工势场函数的基础上引进的各架无人机(UAV)与其所对应目标之间相对距离。

9.如权利要求1所述的无人机集群三维编队方法，其特征在于：当无人机单元判断到陷入局部极小点后，沿着目标方向的法平面上任意方向移动，跳出局部极小点，如果无人机单元在前一时刻的位置与当前位置的距离小于某个设定值，则判断无人机陷入了局部极小点，此时，添加一个附加势场，无人机沿着与目标方向垂直的法平面上任意方向移动一段距离，从而逃离局部极小点。

10.如权利要求1所述的无人机集群三维编队方法，其特征在于：当F_repi＝F_atti，即无人机受到的引力恰好等于斥力，且方向相反时，无人机处于局部极小点中，无法移动，附加的势场函数为：

其中，a是一个大于零的常数，P_local代表局部极小点的位置，l_amax是附加势场的最大影响距离。陷入局部极小点的无人机在附加势场力的作用下逃离局部极小点。

Images