基于人工势场法的固定翼无人机集群避障方法及系统与流程

1.本发明涉及无人机技术领域，尤其涉及一种基于人工势场法的固定翼无人机集群避障方法及系统。


背景技术：

2.近年来随着无人机产业的不断发展，无人机得到了大量的应用，但单一个体由于受到自身视野、负载、续航等限制，很难完成大面积作业、侦察等工作。因此无人机集群得到了科研和工程领域的重点关注。无人集群即通过自组织机制，使具备有限自主能力的多架无人机能够互相通信，共享位置、速度、障碍等信息，在较高的自主程度下完成互相协作，以达到在无人控制的情况下完成预定的任务或者到达指定的区域完成飞行动作。
3.由于集群中各无人机之间的互补作用，使其具有抗毁性、协同性和拓展性，在实际应用中发挥了巨大优势，可用于大面积侦察打击、生物防治、救援搜捕等任务。目前市场上常见的无人机多为旋翼无人机，可用于商用飞行表演航拍等。而关于固定翼无人机的研究则较少，相比于旋翼无人机，固定翼无人机飞行速度更快，转弯半径需要更大，所以对集群的算法要求更高。
4.在现有的集群方法中，由于工作环境的复杂性和多变性以及集群体积较为庞大，使得集群避障成为一个无法回避的关键问题之一。针对无人机集群的避障问题，目前常用的避障算法包括人工势场法、神经网络法、最优化理论。其中人工势场法原理简单、易于理解、计算量小，所以计算速度快，实时性高，在避障控制策略中成为首选方法。但依然存在的一定的共性问题，如无人机、障碍物以及目标之间存在局部极小值，即无人机容易陷人引力与斥力的平衡点处，从而无法继续前进。此外领航者的导航引导作用力会随无人机与领航者的距离增大而增大，会导致无人机不断逼近领航者，虽然无人机受到的排斥力会随障碍物距离缩小而增大，但由于所采用的排斥力势能函数是有界的，即排斥作用力也是有界的，所以可能会出现无人机与障碍物距离过近而小于最小安全距离甚至出现碰撞现象。


技术实现要素：

5.本发明实施例提供一种基于人工势场法的固定翼无人机集群避障方法及系统，用以解决现有技术中无人机集群避障方法不适用于固定翼无人机集群的问题。
6.根据本发明实施例的基于人工势场法的固定翼无人机集群避障方法，包括：
7.定义障碍物阻碍固定翼无人机飞行的区域为障碍区域，将所述障碍区域拟合为椭圆形；
8.以领航无人机跟随无人机模式设置固定翼无人机集群编队；
9.基于领航无人机与目标点之间的距离计算所述领航无人机所受引力，并基于椭圆形障碍区域对所述领航无人机形成的斥力以及相邻跟随无人机对所述领航无人机形成的斥力，计算所述领航无人机所受总斥力；
10.基于所述领航无人机所受引力和所述领航无人机所受总斥力，控制所述领航无人
机；
11.针对每个跟随无人机，基于椭圆形障碍区域对该跟随无人机形成的斥力以及相邻其他跟随无人机对该跟随无人机形成的斥力，计算该跟随无人机所受总斥力，并基于该跟随无人机所受总斥力，控制该跟随无人机。
12.根据本发明的一些实施例，所述基于领航无人机与目标点之间的距离计算所述领航无人机所受引力，包括：
13.判断领航无人机与目标点之间的距离是否大于最大引力距离，若是，则基于所述最大引力距离计算所述领航无人机所受引力，否则，基于领航无人机与目标点之间的距离计算所述领航无人机所受引力。
14.根据本发明的一些实施例，所述基于椭圆形障碍区域对该跟随无人机形成的斥力以及相邻其他跟随无人机对该跟随无人机形成的斥力，计算该跟随无人机所受总斥力，包括：
15.基于椭圆形障碍区域，确定多个障碍物点；
16.从所有障碍物点中筛选出与该跟随无人机的距离小于检测距离的所有目标障碍物点；
17.基于公式1计算所有目标障碍物点对该跟随无人机j形成的斥力rj：
[0018][0019]
其中，表示第l个目标障碍物点的位置，xj表示该跟随无人机的目标位置， m表示目标障碍物点的总数，α表示斥力系数。
[0020]
根据本发明的一些实施例，所述斥力系数α根据公式2或者公式3计算获得：
[0021][0022]
其中，χr表示该跟随无人机的当前实际位置，dm表示检测距离，δ为常数， d表示该跟随无人机与目标障碍物点之间的距离；
[0023][0024]
根据本发明的一些实施例，所述方法还包括：
[0025]
在对领航无人机或跟随无人机控制过程中，增加固定扰动。
[0026]
根据本发明实施例的基于人工势场法的固定翼无人机集群避障系统，包括：
[0027]
障碍物处理模块，用于定义障碍物阻碍固定翼无人机飞行的区域为障碍区域，将所述障碍区域拟合为椭圆形；
[0028]
设置模块，用于以领航无人机跟随无人机模式设置固定翼无人机集群编队；
[0029]
控制模块，用于基于领航无人机与目标点之间的距离计算所述领航无人机所受引力，并基于椭圆形障碍区域对所述领航无人机形成的斥力以及相邻跟随无人机对所述领航无人机形成的斥力，计算所述领航无人机所受总斥力；基于所述领航无人机所受引力和所述领航无人机所受总斥力，控制所述领航无人机；针对每个跟随无人机，基于椭圆形障碍区域对该跟随无人机形成的斥力以及相邻其他跟随无人机对该跟随无人机形成的斥力，计算
该跟随无人机所受总斥力，并基于该跟随无人机所受总斥力，控制该跟随无人机。
[0030]
根据本发明的一些实施例，所述控制模块，用于：
[0031]
判断领航无人机与目标点之间的距离是否大于最大引力距离，若是，则基于所述最大引力距离计算所述领航无人机所受引力，否则，基于领航无人机与目标点之间的距离计算所述领航无人机所受引力。
[0032]
根据本发明的一些实施例，所述控制模块，用于：
[0033]
基于椭圆形障碍区域，确定多个障碍物点；
[0034]
从所有障碍物点中筛选出与该跟随无人机的距离小于检测距离的所有目标障碍物点；
[0035]
基于公式1计算所有目标障碍物点对该跟随无人机j形成的斥力rj：
[0036][0037]
其中，表示第l个目标障碍物点的位置，xj表示该跟随无人机的目标位置， m表示目标障碍物点的总数，α表示斥力系数。
[0038]
根据本发明的一些实施例，所述斥力系数α根据公式2或者公式3计算获得：
[0039][0040]
其中，χr表示该跟随无人机的当前实际位置，dm表示检测距离，δ为常数， d表示该跟随无人机与目标障碍物点之间的距离；
[0041][0042]
根据本发明的一些实施例，所述控制模块，还用于：
[0043]
在对领航无人机或跟随无人机控制过程中，增加固定扰动。
[0044]
采用本发明实施例，通过将障碍区域拟合为椭圆形，相当于加入了额外的虚拟障碍物点，可以使得避障的路径更加的流畅，同时将无人机自身也算作障碍物点，避免了无人机的互相碰撞。
[0045]
上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其它目的、特征和优点能够更明显易懂，以下特举本发明的具体实施方式。
附图说明
[0046]
通过阅读下文实施方式的详细描述，各种其他的优点和益处对于本领域普通技术人员将变得清楚明了。附图仅用于示出优选实施方式的目的，而并不认为是对本发明的限制。在附图中：
[0047]
图1是人工势场法原理图；
[0048]
图2是本发明实施例中固定翼无人机集群编队原理图；
[0049]
图3是原始障碍区域示意图；
[0050]
图4是本发明实施例中拟合成椭圆形障碍区域示意图；
[0051]
图5是本发明实施例中固定翼无人机集群避障示意图；
[0052]
图6是基于原始障碍区域的避障路径示意图；
[0053]
图7是基于本发明实施例中椭圆形障碍区域的避障路径示意图；
[0054]
图8是本发明实施例基于人工势场法的固定翼无人机集群避障方法流程图。
具体实施方式
[0055]
下面将参照附图更详细地描述本发明的示例性实施例。虽然附图中显示了本发明的示例性实施例，然而应当理解，可以以各种形式实现本发明而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本发明，并且能够将本发明的范围完整的传达给本领域的技术人员。另外，在一些实例中，并未详细示出公知的方法、结构和技术，以便不模糊对本说明书的理解。
[0056]
根据本发明实施例的基于人工势场法的固定翼无人机集群避障方法，包括：
[0057]
定义障碍物阻碍固定翼无人机飞行的区域为障碍区域，将所述障碍区域拟合为椭圆形，对比图3和图4，图3中两个黑点即为两个障碍区域，图4中两个椭圆即为两个障碍区域拟合后的椭圆形区域。将障碍区域拟合为椭圆形的目的是为了使得固定翼无人机绕过障碍物的路径更顺畅。
[0058]
以领航无人机跟随无人机模式设置固定翼无人机集群编队；可以理解，固定翼无人机集群中有一个是领航无人机，其他则为跟随无人机，领航无人机相比于跟随无人机靠近目标点。固定翼无人机集群编队可以参考三角形编队、圆形编队、人字形编队，或是其他各种形状的编队。编队确定后各个跟随者无人机以及领航无人机的初始位置也就相应确定了。
[0059]
基于领航无人机与目标点之间的距离计算所述领航无人机所受引力，并基于椭圆形障碍区域对所述领航无人机形成的斥力以及相邻跟随无人机对所述领航无人机形成的斥力，计算所述领航无人机所受总斥力；
[0060]
基于所述领航无人机所受引力和所述领航无人机所受总斥力，控制所述领航无人机；
[0061]
针对每个跟随无人机，基于椭圆形障碍区域对该跟随无人机形成的斥力以及相邻其他跟随无人机对该跟随无人机形成的斥力，计算该跟随无人机所受总斥力，并基于该跟随无人机所受总斥力，控制该跟随无人机。
[0062]
参照图1，人工势场法的核心原理就是将目标点和障碍物分别看做对无人机有引力和斥力的物体，引力与斥力的合力引导运动。
[0063]
本技术就是基于人工势场法来控制领航无人机运动，改进之处在于，本技术还将无人机也作为障碍物来计算斥力。
[0064]
采用本发明实施例，通过将障碍区域拟合为椭圆形，相当于加入了额外的虚拟障碍物点，可以使得避障的路径更加的流畅，同时将无人机自身也算作障碍物点，避免了无人机的互相碰撞。
[0065]
在上述实施例的基础上，进一步提出各变型实施例，在此需要说明的是，为了使描述简要，在各变型实施例中仅描述与上述实施例的不同之处。
[0066]
根据本发明的一些实施例，椭圆形的长轴与固定翼无人机朝向目标点的方向小于
90
°
。
[0067]
根据本发明的一些实施例，所述基于领航无人机与目标点之间的距离计算所述领航无人机所受引力，包括：
[0068]
判断领航无人机与目标点之间的距离是否大于最大引力距离，若是，则基于所述最大引力距离计算所述领航无人机所受引力，否则，基于领航无人机与目标点之间的距离计算所述领航无人机所受引力。由此，可以避免当领航无人机距离目标点较远时，目标点的引力过大，造成运动异常。
[0069]
根据本发明的一些实施例，所述基于椭圆形障碍区域对该跟随无人机形成的斥力以及相邻其他跟随无人机对该跟随无人机形成的斥力，计算该跟随无人机所受总斥力，包括：
[0070]
基于椭圆形障碍区域，确定多个障碍物点；
[0071]
从所有障碍物点中筛选出与该跟随无人机的距离小于检测距离的所有目标障碍物点；
[0072]
基于公式1计算所有目标障碍物点对该跟随无人机j形成的斥力rj：
[0073][0074]
其中，表示第l个目标障碍物点的位置，xj表示该跟随无人机的目标位置， m表示目标障碍物点的总数，α表示斥力系数。
[0075]
采用上述处理方式是考虑到跟随无人机在飞行过程中障碍物点很多，假如每一个障碍物点都参与斥力计算，则会极大的增加计算负担，所以在此处设置一个“检测距离”，只有当跟随无人机与障碍物点之间的距离小于检测距离时，才会参与到斥力计算。该方式同样适用于领航无人机斥力计算中。
[0076]
根据本发明的一些实施例，所述斥力系数α根据公式2或者公式3计算获得：
[0077][0078]
其中，χr表示该跟随无人机的当前实际位置，dm表示检测距离，δ为常数， d表示该跟随无人机与目标障碍物点之间的距离；
[0079][0080]
根据本发明的一些实施例，所述方法还包括：
[0081]
在对领航无人机或跟随无人机控制过程中，增加固定扰动。由此，可以解决领航无人机或跟随无人机在人工势场法下，如果陷入力的平衡区，在现有的随机扰动下对于固定翼无人机的不适用性，所以改随机扰动为定向扰动，从而防止无人机突然随意转向。
[0082]
下面以一个具体的实施例详细描述根据本发明实施例的基于人工势场法的固定翼无人机集群避障方法。值得理解的是，下述描述仅是示例性说明，而不是对本发明的具体限制。凡是采用本发明的相似结构及其相似变化，均应列入本发明的保护范围。
[0083]
本发明实施例的基于人工势场法的固定翼无人机集群避障方法包括：
[0084]
步骤1：设置无人及总数及各无人机起始位置、设置终点位置、设置仿真步数、设置
障碍物位置、大小数量。
[0085]
步骤2：为了解决无人机的转向角度过大，经过路径不流畅等问题，将障碍物进行重新拟合为椭圆形，即人为的对障碍物的外轮廓进行改变，加入虚拟的障碍点，以保证无人机的路径流畅。
[0086]
步骤3：设置集群通信拓扑矩阵为矩阵的最后一行为领航者信息流。如图2所示，可知其出度最大，入度为0，所以为关键节点，可作为整个集群的领航者。
[0087]
步骤4：设置各个无人机的初始速度、初始加速度。设置跟随无人机与领航无人机之间的位置误差矩阵，值得注意的是领航者与自身误差为0。设置其队形为三角形，或者圆形等。
[0088]
步骤5：计算领航无人机相对于终点的距离以及相对于终点的方位角。如果距离大于设定的最大引力距离，则将设定的引力距离作为无人机与终点的距离。此举措主要是为了避免当前情况下无人机距离终点较远时，终点的引力过大，造成运动异常。
[0089]
步骤6：根据传感器回传的障碍物数据，计算出斥力大小。因为在无人机的飞行过程中障碍物很多，假如每一个障碍物都参与斥力计算，则会极大的增加计算负担，所以在此处设置一个“检测距离”，只有当无人机与障碍物之间的距离小于检测距离时，才会参与到斥力计算。根据公式：斥力计算。根据公式：或对所有的斥力进行求和即可得到无人机在当前位置的斥力大小。在此处本发明对其求和系数进行了优化，式中dm为检测距离，这让原本为常数的求和参数成为了变值，使得其斥力的求和更加合理，即距离更远的障碍物，对无人机的影响应该更小才对。
[0090]
步骤7：根据合力大小，求出下一个步长无人机的速度，根据其速度和当前位置计算出无人机的下一次位置。值得注意的是：因为人工势场法，会存在引力与斥力平衡的位置，那么计算出的速度就会很小，所以需要加入扰动来推动无人机来打破这种不合理的平衡，因为针对的是固定翼无人机，其不能突然间的随意转向，所以本发明特地在前一次的方位角上加一个合理的方位角以及速度值，来突破这个平衡点。
[0091]
步骤8：为了使集群无人机保持队形，所以加入了无人机之间的位置偏移，计算出领航者无人机的位置以后。即根据前文计算出的理想位置矩阵减去步骤4 中设置的位置误差矩阵，得到保持队形的理想位置。
[0092]
步骤9：为了解决多无人机之间的避障问题，将除自身外的无人机都当做是障碍物，计算出斥力，求出无人机的速度，这样就可以避免发生无人机的碰撞问题。
[0093]
步骤10：将求出的理想速度与无人机设置的最大飞行速度进行比较，如果超过最
大速度，则按照最大速度运动。
[0094]
步骤11：根据以上步骤即可以得到无人机的实际运动速度，结合上一次的无人机位置，既可以知道无人机下一次的运动位置。
[0095]
步骤12：循环步骤5到步骤11，直到领航者无人机与终点的位置小于设定值，即代表无人机编队完成整个运动过程。
[0096]
本发明选用领航者-跟随者编队中的分布式控制结构实现编队控制。领航者
‑ꢀ
跟随者编队是一种较为常用的集群方式，主要源于一致性的一阶编队算法思想，其核心在于如何控制好领航者无人机与跟随者无人机之间的相对位置、速度等状态量，当位置、速度等达到一定的稳定状态时，便实现了整个集群队形的保持，而领航者无人机对则是集群的控制核心。编队系统的一阶连续系统模型如下：
[0097][0098]
公式中ui则分别表示i节点下的状态量和输入量(状态表示下一次计算无人机的速度、加速度、位置等信息，输入量为无人机上一次计算的速度、加速度、位置等信息)，并且ui∈rn,n表示状态量的维度。
[0099]
在一般情况下输入的计算其中a
ij
为编队邻接矩阵元素，ni为成员i的邻居集合。
[0100]
在设定中场景为领航者带领其他机器人从起始点出发，经过一段距离以后到达设置的终点，所以在理想的情况下编队中跟随者的算法采用如下形式：
[0101][0102]
其中，χi(k)表示第k次迭代计算的第i个无人机的位置，ε为大于0的常数， r
ij
表示机器人i和机器人j之间的相对位置，与他们之间的距离相关。
[0103]
领航者算法可以写为：
[0104][0105]
p为可自行调整的常数，k为可自行调整的常数，d(k)为k次迭代领航者到目标点距离，ani为斥力系数，rni(k)为领航者与其他机器人相对位置。
[0106]
而将问题细化以后增加了斥力系数以及障碍物不唯一等其他因素。
[0107]
领航者无人机根据自身传感器对周围进行环境扫描，获得周围障碍物位置，根据公式计算出斥力，以远离障碍物，其中rj代表无人机所受到的斥力之和，代表障碍物的坐标，m为障碍物的数量，α为斥力系数，用于调节斥力大小。
[0108]
α斥力系数可以根据公式：计算而出，xr 是当前无人机位置。
[0109]
d为无人机到障碍物之间的距离，δ.d这样就可以根据需要调整斥力。
[0110]dm
为探测距离，可以根据实际需要对探测距离大小进行调整，δ为常数，根据以上公式，对无人机的速度控制产生影响。
[0111]
领航者无人机需要兼顾队伍内的跟随者无人机的受力情况，受力根据人工势场法计算得出，所以领航者的控制模型如下：
[0112][0113]
其中β为一常数，跟随者无人机的控制模型如下：
[0114][0115]
例如，设置起始点为(0,0)，终点为(1500,1500)，加入障碍物,进行仿真运算。
[0116]
图4、图5分别是无人机编队避障前以及避障后的仿真图。
[0117]
而图4中的障碍物是根据发明中的算法，对障碍物进行了原位置的重新拟合，人工的加入虚拟障碍边缘，并且对斥力系数的计算进行优化。
[0118]
对比图6和图7可以明显的看到，可以看到图6为未经过优化的无人机的飞行路径，其转角尖锐，路径并不流畅，图8为整个发明的实现流程框图。本发明相比于传统的人工势场法，更加适用于固定翼无人机集群，具有明显的优势以及创新性。
[0119]
本发明可以解决无人机在人工势场法下，如果陷入力的平衡区，在现有的随机扰动下对于固定翼无人机的不适用性，所以改随机扰动为定向扰动，从而防止无人机突然随意转向。可以解决无人机集群避障时无人机互相碰撞的问题，所以引入了无人机之间的避障模型。可以解决无人机集群避障的路径不流畅，对于固定翼而言可操作性不强等问题。所以加入额外的虚拟障碍物来平滑路径。
[0120]
从图5可以看出，本发明能让无人机在避障过程中也能尽可能的保持编队队形，这样能够维持无人机集群的工作范围。
[0121]
由于本发明加入了额外的虚拟障碍物，所以避障的路径更加的流畅，同时将无人机自身也算作障碍物，避免了无人机的互相碰撞。
[0122]
需要说明的是，以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
[0123]
根据本发明实施例的基于人工势场法的固定翼无人机集群避障系统，包括：
[0124]
障碍物处理模块，用于定义障碍物阻碍固定翼无人机飞行的区域为障碍区域，将所述障碍区域拟合为椭圆形；
[0125]
设置模块，用于以领航无人机跟随无人机模式设置固定翼无人机集群编队；
[0126]
控制模块，用于基于领航无人机与目标点之间的距离计算所述领航无人机所受引力，并基于椭圆形障碍区域对所述领航无人机形成的斥力以及相邻跟随无人机对所述领航无人机形成的斥力，计算所述领航无人机所受总斥力；基于所述领航无人机所受引力和所述领航无人机所受总斥力，控制所述领航无人机；针对每个跟随无人机，基于椭圆形障碍区域对该跟随无人机形成的斥力以及相邻其他跟随无人机对该跟随无人机形成的斥力，计算该跟随无人机所受总斥力，并基于该跟随无人机所受总斥力，控制该跟随无人机。
[0127]
根据本发明的一些实施例，所述控制模块，用于：
[0128]
判断领航无人机与目标点之间的距离是否大于最大引力距离，若是，则基于所述最大引力距离计算所述领航无人机所受引力，否则，基于领航无人机与目标点之间的距离计算所述领航无人机所受引力。
[0129]
根据本发明的一些实施例，所述控制模块，用于：
[0130]
基于椭圆形障碍区域，确定多个障碍物点；
[0131]
从所有障碍物点中筛选出与该跟随无人机的距离小于检测距离的所有目标障碍物点；
[0132]
基于公式1计算所有目标障碍物点对该跟随无人机j形成的斥力rj：
[0133][0134]
其中，表示第l个目标障碍物点的位置，xj表示该跟随无人机的目标位置， m表示目标障碍物点的总数，α表示斥力系数。
[0135]
根据本发明的一些实施例，所述斥力系数α根据公式2或者公式3计算获得：
[0136][0137]
其中，χr表示该跟随无人机的当前实际位置，dm表示检测距离，δ为常数， d表示该跟随无人机与目标障碍物点之间的距离；
[0138][0139]
根据本发明的一些实施例，所述控制模块，还用于：
[0140]
在对领航无人机或跟随无人机控制过程中，增加固定扰动。
[0141]
需要说明的是，在本说明书的描述中，上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。
[0142]
使用用于表示元件的诸如“模块”、“部件”或“单元”的后缀仅为了有利于本发明的说明，其本身没有特定的意义。因此，“模块”、“部件”或“单元”可以混合地使用。
[0143]
参考术语“一个实施例”、“一些实施例”、“示意性实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。尽管在此所述的一些实施例包括其它实施例中所包括的某些特征而不是其它特征，但是不同实施例的特征的组合意味着处于本发明的范围之内并且形成不同的实施例。描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。例如，在权利要求书中，所要求保护的实施例的任意之一都可以以任意的组合方式来使用。
[0144]
术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者装置不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者装置所固有的要素。在没有更多限制的情况下，由语句“包括一个
……”
限定的要素，并不排除在包括该要素的过程、方法、物品或者装置中还存在另外的相同要素。
[0145]
不应将位于括号之内的任何参考符号构造成对权利要求的限制。位于元件之前的单词“一”或“一个”不排除存在多个这样的元件。单词第一、第二、以及第三等的使用是用于区别类似的对象，不表示任何顺序。可将这些单词解释为名称。
[0146]“和/或”，描述关联对象的关联关系，表示可以存在三种关系，例如，a 和/或b，可以表示：单独存在a，同时存在a和b，单独存在b这三种情况。字符“/”一般表示前后关联对象是一种“或”的关系。