基于人工势场法的可垂起无人机机间防碰撞控制方法与流程

本发明涉及一种基于人工势场法的可垂起无人机机间的防碰撞控制方法，该发明属于飞行控制领域。

背景技术：

随着微小型无人飞行器的发展，无人机的大量使用也引发了人们对于安全的担忧，影响航空秩序、闯入敏感区域等“黑飞”现象时有发生，而可垂起无人机则是近些年来微小型无人机的一个典型代表。小型区域内的无人机数量增加后，当多架无人机共同完成任务时，为了确保无人机在飞行中和工作中的安全，飞行过程中的规划决策不可避免。由于无人机都在超低空飞行，其空中飞行环境十分复杂，除了要避免无人机与一些静态障碍物比如周围的建筑物等相撞，还必须防止无人机与无人机相撞。因此，多机之间的防碰撞飞行控制很有必要。

为解决上述问题，本发明提出了一种基于人工势场法的可垂起无人机机间防碰撞控制方法，实现了多无人机之间防碰撞飞行控制。

技术实现要素：

本发明提出了一种使用的基于人工势场法的可垂起无人机机间防碰撞控制方法。该方法是基于传统的人工势场法。首先，建立可垂起无人机基本控制模型和可垂起无人机安全控制模型，然后，建立可垂起无人机周围的其他可垂起无人机的人工势场，最后，根据无人机当前的位置速度，控制无人机在不与周围的无人机发生碰撞的情况下飞行到目标点。

本发明给出了基于人工势场法的可垂起无人机机间防碰撞控制方法。这里定义如下变量：

分别是第i架无人机和第l架无人机的滤波位置，分别是第i架无人机和第l架无人机当前位置，是第i架无人机的当前速度，是控制器输出的第i架无人机的速度控制指令，分别是pi、vi、ξi一阶求导。

是无人机的飞行速度的最大值；

是安全距离，是基于滤波的安全距离，是避障机动距离，rv是距离测量的噪声误差；

是第i架无人机的目标点位置；

是第i架无人机与目标点的位置差，是第i架无人机与目标点的滤波位置差；

是第i架无人机与第l架无人机的位置差，是第i架无人机与第l架无人机的滤波位置差。

所述的控制方法如下：请参考附图6所示；

步骤1：建立可垂起无人机的基本控制模型

首先需要定义无人机的基本控制模型。这里将无人机视为一个质点模型，那么第i架无人机满足以下模型关系：

这里的lc是无人机的控制性能的参数，由无人机本身决定。由于无人机的机动性能有限，因此控制器解算的速度指令不可能无限大。这里设计了如下的饱和函数：

因此，最终控制器输出的第i架无人机的速度控制指令为：

vc,i＝sat(vc,i,vm)(3)

这里定义一个概念，称作滤波位置。滤波位置是为了将无人机的当前位置和速度用一个量表述出来。这样的好处是，我们可以将无人机的二阶控制模型用一阶形式来表示。其定义如公式(4)所示：

接下来就可以得到：

这里定义：

为了描述方便，定义如下位置差：

对应的滤波距离的位置差为：

根据滤波距离的定义，我们有：

这里i,l＝1,2,3...,i≠l。为了保证安全，任意无人机之间都必须保持一定距离。设这个距离为r，对于第i架无人机与第l架无人机，那么有：

||pi-pl||≥r,i≠l(7)

步骤2：根据可垂起无人机当前的位置和速度，根据可垂起无人机的安全距离，建立无人机的安全控制模型；

在一个多机飞行的区域内，无人机之间可以通过无线网络等方式互相报告自身的位置和姿态。由于通信存在延迟，获取的导航数据也存在噪声，因此无人机的位置信息存在不确定性。为了避免无人机与周围的物体发生碰撞，我们定义了安全距离rm，安全距离必须大于无人机的物理半径，如图1所示。对任意两架无人机来讲，它们的滤波距离应该满足：

||ξi-ξl||≥2rm(8)

由公式(8)就可以满足公式(7)。这里rm＝rm+rv，rm是基于滤波距离定义的安全距离。为了保证无人机在发现周围的无人机时可以有充足的时间和空间进行机动避障，如图1所示。ra仅与无人机本身的动力性能和响应时间有关。要求避障机动距离ra在符合实际情况的条件下必须尽可能大，这里要求：

ra＞2rm(9)

步骤3：可垂起无人机点到点的飞行控制

无人机当前的位置与目标点的滤波距离为无人机到达目标点的条件是：

为了保证第i架无人机可以到达目标点，可以设计控制器输出量vc,i，即控制器输出的第i架无人机的速度控制指令为：

其中，k1为控制器输出的增益系数。

步骤4：基于可垂起无人机与周围其他可垂起无人机的相对位置和速度，计算可垂起无人机所受到的防碰撞的控制指令

设第i架与第l架无人机的当前位置分别为和当前速度分别为和这两架无人机的飞行目标点分别为和所接受的速度控制指令输入分别为和根据无人机的安全控制模型，它们当前的滤波位置为和无人机当前的位置与目标点的滤波距离为和这两架无人机之间的相对滤波距离为根据无人机的安全控制模型，所有的无人机的目标点之间的距离必须大于安全距离。

那么无人机在飞行至各自的目标点的途中不能发生碰撞的条件为：

其中，t为时间。

考虑如图3所示场景，这是区域内三架无人机与其目标点的俯视示意图。其中包括三架无人机uav1、uav2和uav3及它们各自对应的三个目标点对于在这个区域内飞行中的无人机，需要保证无人机之间不会发生碰撞，并且到达各自的目标点。即：

记νm,i为区域内中无人机机群中除去第i架无人机的其他无人机的集合。以图3中的飞行场景为例，共有三架无人机。νm,1＝{2,3}，νm,2＝{1,3}，νm,3＝{1,2}。基于以上性质，这里可以给出控制器输出的第i架无人机的速度控制指令，其表示形式为：

其中，为人工势场的势能函数，定义如下：

为了方便描述，我们记d1＝2rm,d2＝ra，记

rs是一个极小值。是一个平滑的截断函数，是一个平滑的饱和函数。这两个二阶平滑的基础函数，如图2(a)、图2(b)所示。引入的函数是为了更简化公式之间的运算关系，故下述公式中的参数均为中间变量，本身无含义，仅是为了方便描述。

其中，a＝-2/(d1-d2)³，b＝3(d1+d2)/(d1-d2)³，c＝-6d1d2/(d1-d2)³，d＝d2²(3d1-d2)/(d1-d2)³。函数σ(x,d1,d2)相对于x的偏导数为：

饱和函数s(y,rs)为：

其中，

y1＝y2-sin45°rs

对于任意rs∈[0,tan67.5°/(tan67.5°sin45°-1)]，都有，

其中，min(y,1)表示变量y与1其中的较小值。函数s(y,rs)相对于y的偏导数为公式(19)，显然

k2为控制器输出的增益系数。存在极小量ε，使得，

除了防碰撞之外，我们还需要控制无人机到达目标点，因此控制器输出的第i架无人机的速度控制指令：

无人机稳定到达目标点的约束条件为存在一个极小量ε，使得公式(22)成立。

因此，无人机之间防碰撞的方法包括：

输入：区域中所有无人机的实时位置速度信息和对应的目标点。

输出：每个无人机的速度控制指令vc,i。

4.1：获取区域中所有无人机的实时位置速度信息和目标点位置；

4.2：根据公式(14)和公式(15)计算无人机与周围无人机之间的防碰撞指令；

4.3：根据无人机目标点位置，按照公式(11)，计算目标点产生的控制指令；

4.4：将步骤4.2和步骤4.3中生成的指令按照公式(21)叠加，进行保方向的饱和处理；

4.5：如果满足约束条件公式(22)，说明无人机已到达目标点；否则继续执行步骤4.2。

优点及功效：本发明给出一种基于人工势场法的可垂起无人机机间防碰撞控制方法。该方法的优点是：解决了无人机多机飞行时它们之间可能发生的碰撞问题，优势如下：

(1)使用的多旋翼无人机模型是具有速度指令输入的双积分模型，适合大多数无人机。该模型简单易行，更重要的是，该模型是连接底层控制与顶层应用算法的纽带。我们可以基于商用半自主自驾仪，为各种任务完成相应的设计开发。

(2)速度控制指令有饱和量保护。在设计的控制器中的最大速度指令是有限制的。当无人机与障碍物接近时，防碰撞产生的速度指令会远大于无人机到达目标点的速度控制指令，在使用保方向的饱和控制后，可以提高无人机防碰撞控制的优先级，保证无人机的安全。

(3)根据该控制器，无人机无论是否遇到障碍物，都是最终收敛到目标点。也就是说，该控制器不仅能使无人机完成飞行任务，还能保证完成任务过程中的安全性。

(4)当两架无人机距离小于安全距离时，整个控制算法依然能快速使其分离。

附图说明

图1：安全距离二维投影模型。

图2(a)：是平滑的截断函数。

图2(b)：是平滑的饱和函数。

图3：多架无人机区域内飞行示意图。

图4：三架无人机防碰撞的实验设置示意图。

图5(a)：是三架四旋翼的三维轨迹。

图5(b)：是三架四旋翼的轨迹水平投影。

图6：是本发明的流程图。

图中符号说明

图1：安全距离rm，避障机动距离ra。

图2(b)：rs∈[0,tan67.5°/(tan67.5°sin45°-1)]，是一个极小值。

图3：三架无人机uav1、uav2和uav3及它们各自对应的三个目标点

图4：三架无人机，分别记为u1,u2,u3，它们的目标点分别为的pd,1,pd,2,pd,3

具体实施方式

本发明提供了一种基于人工势场法的可垂起无人机机间防碰撞控制方法，以多个无人机在区域内飞行为例，对本发明的具体实施方式进行进一步说明。下面介绍根据本发明中提到的方法对无人机的多机机间防碰撞飞行控制。请参考附图1-6所示。

(1)方法具体实施步骤：

步骤一：建立可垂起无人机基本控制模型

基本控制模型的建立需要两个参数：l＝10，vm＝2.0m/s。我们的滤波位置为：

带有饱和的控制器输出的第i架无人机的速度控制指令为：

步骤二：建立可垂起无人机的安全控制模型

安全距离模型的核心参数为：rm＝0.6m,ra＝1.8m，rm＝0.8m。

步骤三：可垂起无人机点到点的飞行控制

这一步需要给出的参数为k1＝0.5，带有饱和的控制器输出的第i架无人机的速度控制指令为：

步骤四：基于可垂起无人机与周围其他可垂起无人机的相对位置和速度，计算可垂起无人机所受到的防碰撞的控制指令。

该实验中，共使用了三架无人机，分别记为u1,u2,u3，见图4。它们各自的起始位置分别为：

pu1＝[-2.0-0.51.2]^t

pu2＝[0.00.01.2]^t(26)

pu3＝[2.01.01.2]^t

它们的目标点为对应的pd,1,pd,2,pd,3，如图4，它们的坐标分别是：

pd,1＝[2.20.51.2]^t

pd,2＝[-0.10.61.2]^t(27)

pd,3＝[-1.8-0.61.2]^t

无人机的起始点与其各自的目标点的直线连线如图4中虚线所示，可以看出来这些直线是相交的。

无人机多机飞行防碰撞实验的参数为：

1)防碰撞速度指令增益系数：k2＝0.5

2)收敛极小量：ε＝0.01

3)饱和基础函数参数：rs＝0.01

无人机的防碰撞飞行的控制器输出的第i架无人机的速度控制指令为：

其中，

假如无人机之间没有防碰撞的保护措施，无人机在往目标点飞行的过程中就会发生碰撞。但加入防碰撞保护算法就可以避免无人机在空中相撞。

由于每架无人机都会受到另外两架无人机所产生的排斥力、区域边界产生的排斥力和目标点对无人机的吸引力，所以无人机不可能朝着目标点完全按照直线前进，并且每时每刻无人机的位置和速度都是在变化的，如图5(a)、图5(b)中三条无人机的轨迹所示。