一种自主式无人机群目标跟随避障方法与流程

本发明属于无人机群编队控制技术领域，尤其是涉及一种自主式无人机群目标跟随避障方法，旨在使得无人机群飞行过程中实现目标自主锁定，以及在保持无人机队形的情况下，顺滑的避障。

背景技术：

随着新型材料、机械电子、惯性导航系统及控制技术的发展进步，加上四轴飞行器具有价格低廉、操作灵活、控制性能稳定、适应环境能力强及可避免人员伤亡的特点，近些年其在军事、民用领域得到了广泛应用。然而单架飞行器存在功能局限、负载有限及面对复杂环境时效率低下等问题，影响了多类任务的可执行性以及完成度。近年来，国内外许多专家学者研究发现，借鉴自然界中生物群体的协调机制可以较好地解决上述问题。

群体智能以及由此发展而来的机器人学均诞生于对动物的研究，许多动物群体所使用的协调机制已被证明可以用来解决多种复杂问题，如：蚁群算法、粒子算法等智能工程的设计灵感均来源于生物。而自然界中的鱼群、鸟群、蜂群的集群行为对于无人机群的飞行控制极具启发性。

相互作用的动物，昆虫以及活性粒子等，在个体自发的限制下形成集群，在具有一定稳定性的同时能够完成集群共同的任务。那么可以认为在复数无人机形成集群时，利用每个无人机有限的感知可以侦测复杂的环境，可以通过局部信息进行个体决策以达成群体任务。根据早期基于生物媒介的模型，建立维持无碰撞稳定模型只需要设置排斥区、跟随区和吸引区，来限定无人机间距离，维持稳定的集群队形。但是通过限制集群间个体距离的方式，在集群避障时会存在局部最小点，无人机会因为周围受力平衡受困在一个区域内。

现有针对于无人机编队控制中已提出了基于速度跟随的自主式无人机群的运动模型，通过遗传算法匹配模型参数实现了有较强适应性的在有限空间内部规避障碍的稳定的无人机群模型。这种速度跟随的方式类似于粘性摩擦，使无人机群的运动类似于流体运动。

有鉴于此，本发明提出利用短程排斥力，避免无人机间碰撞；采用长程的速度跟随方式来保持无人机队形，并在障碍物处以设置速度反向虚拟无人机的方式，来实现避障。

技术实现要素：

本发明的目的在于提供一种自主式无人机群目标跟随避障方法，本发明研究了在同一集群速度下短程斥力与长程速度跟随作用下的无人机集群的避障情况及运行状态，有益效果是无人机集群在飞行过程中在更远距离做出反应以更小的速度变化避开障碍。

在无人机群的路径规划中，为达到上述目标，本发明提供了如下五个步骤的技术方案：

步骤1：按照以集群所要到达的目标点为牵引，设置集群速度方向，其中将集群牵引定以恒定的大小；通过计算每个对象无人机到终点的坐标差值来设置对象无人机的目标牵引方向；

步骤2：对于每个对象无人机，需要在每个时间帧中，对通信范围内所有其余无人机与对象无人机的斥力进行合力计算，计算结果则是对象无人机当前所受斥力总和；

步骤3：利用无人机之间的速度跟随力，来保持集群速度大小和方向的一致性；对通信范围内所有满足速度跟随条件的无人机进行速度跟随运算，获取所有跟随作用的复合作用作为此对象的当前所受的跟随力作用；

所述的满足速度跟随条件是指在通信范围内的任意两个无人机之间的速度差，保持在缓冲范围内，就能够维持集群的稳定；

所述的通信范围是指根据需要设定的范围；

步骤4：在障碍物上设置虚拟无人机，并赋予该虚拟无人机一定的虚拟速度，通过利用步骤3中的无人机之间的速度跟随力，来改变无人机群靠近障碍物时的速度方向，从而达成较好的避障效果；

步骤5：为了计算每个对象无人机所需的速度，求取步骤1～4所有作用的矢量和；将无人机速度固定为集群速度，叠加之前所有作用力，得到所需的目标速度；

所述的所有作用包括无人机的目标牵引力、无人机短距斥力、无人机之间的速度跟随力、无人机与障碍物之间的速度跟随力。

步骤1按照以集群所要到达的目标点为牵引，设置集群速度方向，具体公式如下

公式(1)中v^atti为恒定大小的集群牵引速度；将对象无人机标记为i，ri表示对象无人机的中心坐标，ra表示目标点的中心坐标，ria表示无人机i到目标点的坐标差值，获取了无人机向终点的运动方向；表示对象无人机i的目标牵引速度。

步骤2具体实现如下：

无人机间的相互排斥为短距斥力作用，只在一定范围内有效，其作用方程如下：

公式(2)中，表示两无人机短距斥力作用后的速度，p^rep是机间排斥大小的线性系数，是斥力的截止距离，将两无人机标记为i、j，ri表示无人机i的中心坐标，rj表示无人机j的中心坐标，rij表示无人机i与无人机j之间的坐标差值，获取了无人机连线反方向作为斥力作用方向，斥力大小与无人机间距离大小相关，并以单位为时间倒数的系数p^rep调整大小倍率；

公式(3)表示在每个时间帧中，无人机需要对斥力截止范围内所有无人机单位进行斥力计算，并取其合力为当前对象无人机所受集群其它无人机的总斥力作用，为此作用力后的速度。

步骤3具体实现如下：

设定在一定距离的无人机间速度差只要保持在缓冲范围内就能够维持集群的稳定，速度一致性要求由制动函数d(r,a,p)决定：

公式(4)中r是目标点与跟随的对象无人机之间的距离，p是线性增益，a是制动加速度；d(r,a,p)是以距离变化改变速度缓冲上限的分段函数，速度缓冲上限的表达式如下：

公式(5)中，是速度缓冲项，将两无人机标记为i、j，vij表示无人机i与无人机j之间的速度差值，是无人机i与无人机j理想相对静止点的距离，a^frict和p^frict是跟随加速度参数以及跟随线性增益，在短程设置的速度缓冲上限内，将无人机的当前速度向参考对象无人机的速度变化，仅当超出速度缓冲上限后，无人机跟随作用使无人机速度向目标速度调整，具体调整公式如下：

公式(6)中c^frict为速度跟随作用强度的线性系数，越大表现为更强的阻尼作用以及更小的震荡，但是会使运动缓慢；是速度缓冲项，vi表示无人机i的当前速度，vj表示无人机j的当前速度，vij表示无人机i与无人机j之间的速度差值，为速度缓冲上限；

公式(7)表示在每个时间帧中，无人机需要对通信范围范围内所有无人机单位进行速度跟随计算，并取其合力为当前对象无人机所受集群其它无人机的总速度跟随作用力，为此作用力后的速度。

步骤4具体实现如下：

通过在障碍物上设置虚拟无人机单位，并赋予其一定的虚拟速度，通过利用无人机间速度跟随力，来实时改变无人机群靠近障碍物时的速度方向：

首先获取距离无人机最近的障碍物点，并在该位置设置虚拟shill单位，对多边障碍物或是多个障碍物的的每一边都要进行虚拟shill单位的设置；为无人机与障碍间的跟随速度缓冲上限，ris＝|ri-rs|,其中rs是多边形障碍物上获取的距离最近的点，即是获取最近点到无人机的距离，是无人机与障碍物之间理想相对静止点的距离；a^shill是制动加速度，p^shill是跟随线性增益；

由于跟随作用由对象无人机间的速度差产生，相对于其他牵引力，斥力等较弱，为了使其确实能在强作用产生效果，取消了由虚拟shill单位速度产生的跟随作用的速度缓冲，并将对齐强度系数c^shill保持在1以获取最强的对齐作用；

将对象无人机标记为i，障碍物离无人机距离最近的一点标记为s，vi表示无人机i的当前速度，vs是为虚拟shill单位设定的虚拟速度，其方向为垂直于障碍物墙体或是沿无人机至障碍物方向向障碍物外部延伸；速度vis＝|vi-vs|，表示无人机i与障碍物之间的速度差值，为对象无人机与障碍物之间的跟随作用力后的速度。

步骤5具体实现如下：

为了得到每个对象的所有作用力之合力，对步骤1～4所有速度的矢量求和：

假定无人机以集群速度v^flock运动，vi为对象无人机的原始速度，通过运算保留其自身的运动方向，将其速度固定为集群速度，叠加之前所有作用力，得到为所需的每个对象的目标速度；

在期望集群中所有无人机按固定集群速度运动，且无人机本身有速度极限，故将所得目标速度方向保留，大小限制为最大集群速度截止值v^max。

本发明有益效果如下：

本发明将无人机群的运动分为一个个时间帧叠加的形式，在每个时刻每个无人机单位对当前环境进行分析，并通过步骤5将各个作用力以产生作用后的合力所得速度表示，最终获得当前时刻所需的加速度，已达到为无人机群规划出良好的自主协同避障路线。

附图说明

图1为本发明的实现总体框图；

图2为目标牵引力作用示意图；

图3为无人机间斥力作用示意图；

图4为无人机间跟随作用示意图；

图5为障碍物虚拟无人机与无人机间跟随作用避障示意图；

图6为每个无人机对象最终速度矢量图。

具体实施方式

以下结合附图对本发明作进一步详细描述。

本发明提出一种新的自主式无人机群目标跟随避障算法，其实现总体框图如图1所示，具体包括如下5个步骤：

步骤1：按照以集群所要到达的目标点为牵引，设置集群速度方向：

公式(1)中v^atti为恒定大小的集群牵引速度；将对象无人机标记为i，ri表示对象无人机的中心坐标，ra表示目标点的中心坐标，ria表示无人机i到目标点的坐标差值，获取了无人机向终点的运动方向；表示对象无人机i的目标牵引速度。

图2中坐标单位为m，表示无人机无论在何处都受到相同大小且方向指向终点的牵引力。按上述牵引力，设置集群速度方向，即每个无人机的目标牵引方向。

步骤2：本发明将每两个对象无人机间的短距排斥力用来避免每两个无人机间相碰撞。

无人机间的相互排斥为短距斥力作用，只在一定范围内有效。其作用方程如下：

公式(2)中，表示两无人机短距斥力作用后的速度，p^rep是机间排斥大小的线性系数，是斥力的截止距离，将两无人机标记为i、j，ri表示无人机i的中心坐标，rj表示无人机j的中心坐标，rij表示无人机i与无人机j之间的坐标差值，获取了无人机连线反方向作为斥力作用方向，斥力大小与无人机间距离大小相关，并以单位为时间倒数的系数p^rep调整大小倍率；

公式(3)表示在每个时间帧中，无人机需要对斥力截止范围内所有无人机单位进行斥力计算，并取其合力为当前对象无人机所受集群其它无人机的总斥力作用，为此作用力后的速度。

图3表示无人机间斥力方向永远是两个无人机连线相反的方向。

步骤3：本发明利用无人机之间的跟随力，来保持集群速度大小和方向的一致性，以及集群的队形不会因为步骤4中的避障而散掉。考虑到复杂的环境因素，设定在一定距离的无人机间速度差只要保持在缓冲范围内就能够维持集群的稳定，速度一致性要求由制动函数d(r,a,p)决定：

公式(4)中r是目标点与跟随的对象无人机之间的距离，p是线性增益，a是制动加速度；d(r,a,p)是以距离变化改变速度缓冲上限的分段函数，速度缓冲上限的表达式如下：

公式(5)中，是速度缓冲项，将两无人机标记为i、j，vij表示无人机i与无人机j之间的速度差值，是无人机i与无人机j理想相对静止点的距离，a^frict和p^frict是跟随加速度参数以及跟随线性增益，在短程设置的速度缓冲上限内，将无人机的当前速度向参考对象无人机的速度变化，仅当超出速度缓冲上限后，无人机跟随作用使无人机速度向目标速度调整，具体调整公式如下：

公式(6)中c^frict为速度跟随作用强度的线性系数，越大表现为更强的阻尼作用以及更小的震荡，但是会使运动缓慢；是速度缓冲项，vi表示无人机i的当前速度，vj表示无人机j的当前速度，vij表示无人机i与无人机j之间的速度差值，为速度缓冲上限；

公式(7)表示在每个时间帧中，无人机需要对通信范围范围内所有无人机单位进行速度跟随计算，并取其合力为当前对象无人机所受集群其它无人机的总速度跟随作用力，为此作用力后的速度。图4表示无人机速度跟随作用于无人机间速度差，使无人机在一定的速度缓冲范围内达成速度的一致性。

步骤4：本发明通过在障碍物上设置虚拟无人机单位，并赋予其一定的虚拟速度，通过利用无人机间速度跟随力，来实时改变无人机群靠近障碍物时的速度方向，从而达成避障的效果。

首先获取距离无人机最近的障碍物点，并在该位置设置虚拟shill单位，对多边障碍物或是多个障碍物的的每一边都要进行虚拟shill单位的设置；为无人机与障碍间的跟随速度缓冲上限，ris＝|ri-rs|,其中rs是多边形障碍物上获取的距离最近的点，即是获取最近点到无人机的距离，是无人机与障碍物之间理想相对静止点的距离；a^shill是制动加速度，p^shill是跟随线性增益；

将对象无人机标记为i，障碍物离无人机距离最近的一点标记为s，vi表示无人机i的当前速度，vs是为虚拟shill单位设定的虚拟速度，其方向为垂直于障碍物墙体或是沿无人机至障碍物方向向障碍物外部延伸；速度vis＝|vi-vs|，表示无人机i与障碍物之间的速度差值，为对象无人机与障碍物之间的跟随作用力后的速度。

由于跟随作用由对象无人机间的速度差产生，相对于其他牵引力，斥力等较弱，为了使其确实能在强作用产生效果，取消了由虚拟shill单位速度产生的跟随作用的速度缓冲，并将对齐强度系数c^shill保持在1以获取最强的对齐作用。

图5表示利用设置障碍物虚拟无人机单位，达到无人机对于障碍物较好的跟随避障效果。

步骤5：本发明为了得到每个对象的所有作用力之合力，我们取用上述步骤1～4所有速度的矢量和：

假定无人机以集群速度v^flock运动，vi为对象无人机的原始速度，通过运算保留其自身的运动方向，将其速度固定为集群速度，叠加之前所有作用力，得到为所需的每个对象的目标速度；

图6为每个对象无人机在所定范围内，合成所有作用力后，最终的速度矢量关系。

在期望集群中所有无人机按固定集群速度运动，且无人机本身有速度极限，故将所得目标速度方向保留，大小限制为最大集群速度截止值v^max。基于上述速度计算方法，得出在单位时间δt内每个无人机对象的运动速度与方向，从而达到集群自主协同且以以更小的速度变化避开障碍。