一种基于一致性的多无人机集群防撞方法

1.本发明属于无人机技术领域，具体涉及一种无人机防碰撞方法。


背景技术：

2.在无人机编队领域中，一致性算法已经是一种较为成熟的算法。基于图论拓扑的思想设计编队无人机的通信范围，利用邻接矩阵来设计无人机之间通讯的拓扑权值，在此基础上设计编队控制律。然而当无人机数量过大时，传统的基于邻接矩阵的图论就出现了自身的弊端：当通讯范围很大时，飞控计算机需要处理大量的数据，当通讯范围较小时边界过渡会比较突兀，控制量会产生突变，编队的鲁棒性较低。由此就会导致无人机集群之间的距离难以保持在一定范围内，而距离的波动就会影响无人机集群的稳定性，甚至可能是无人机相互碰撞。此外，在集群飞行当中，集群避障也是不可或缺的一部分，它直接关系到无人机集群执行任务的成功率，在传统的方法中，人工势场法是一种直接有效的避障方法，在无人机障碍物周围人工布设虚拟势场，通过设计引力场和斥力场，实现无人机的引导规避行为。利用势场的原理，在无人机周围设计复合势力场，设计势能最低点，使无人机之间的距离保持在势能最低点，从而实现无人机机间防碰撞。然而传统的势场法依然存在一些弊端，虽然能够较为平滑的设计控制量，但是对于动态的障碍物没有做出明确的避障设计，并且还存在局部最小值的问题，算法不够稳定。


技术实现要素：

3.为了克服现有技术的不足，本发明提供了一种基于一致性的多无人机集群防撞方法，首先基于目标无人机与邻域内无人机的状态，如速度和位置信息，设计改进的一致性集群控制算法；然后在目标无人机周围布设虚拟势场，设计好虚拟势场的分段节点，使无人机保持在势能最低的点处，基于此设计无人机机间避撞控制律；接下来对障碍物进行处理，实时选择合适的点作为最近邻障碍物点，在障碍物附近布设虚拟势场，设计单方向的斥力效果实现障碍物规避；最后为最近邻障碍物点设计虚拟速度，令目标无人机的速度与虚拟速度逐渐达到一致，实现分流效果，避免局部最小值状态。采用本发明方法能使无人机之间间距保持在期望范围，防止碰撞；使无人机速度与虚拟速度达到一致，实现障碍物附近的分流。
4.本发明解决其技术问题所采用的技术方案包括以下步骤：
5.步骤1：设计改进的一致性集群控制算法，具体如下：
6.一致性控制协议为：
[0007][0008]
其中，是位置梯度控制项，是速度匹配项；q表示无人机的位置，q
i
表示第i架无人机的位置，q
j
表示第j架无人机的位置，p
i
表示第i架无人机的速度，p
j
表示第j架无人机的速度，a
ij
(q)表示无人机系统邻接矩阵，σ表
示范数值，d表示是无人机的期望间距；
[0009]
即：
[0010][0011]
其中，r
ij
是一个单位矢量，沿着q
i
到q
j
连线；
[0012][0013]
定义的新的σ范数：
[0014][0015]
其中，k和b分别是用来调节位置控制量的变化快慢和响应速率的参数，k∈(0,1)；
[0016]
如图3所示，定义分段函数：
[0017][0018]
其中h∈(0,1)，控制着曲线的衰减位置；是sigmod函数的一种变形，能够保证该分段函数在分段处导数趋近于0，从而保证曲线能够平滑过渡；c用来控制sigmod函数的衰减速率和陡峭程度；z为分段参数；
[0019]
利用式(5)的分段函数定义无人机系统邻接矩阵：
[0020]
a
ij
(q)＝f
b
(||q
j
‑
q
i
||/r),j≠i
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
[0021]
邻接矩阵a
ij
(q)的作用范围只在半径为r的球形领域内，当两架无人机的的欧式距离超出半径为r的球形领域则邻接矩阵取零，当两架无人机的的欧式距离在半径为r的球形领域内则邻接矩阵的元素在[0,1]内；对于任意一个q，如果i＝j则a
ij
(q)＝0；
[0022]
在对无人机编队设定新任务目标之后，设计新的一致性控制协议：
[0023][0024]
其中，表示基于目标任务的协同控制量，
[0025][0026]
q
g
表示目标点的位置；
[0027]
在式(7)的控制协议中加入速度阻尼项
[0028][0029]
其中，k2表示速度阻尼的影响因子，p
pre
是预设的飞行速度；
[0030][0031]
表示无人机剩余的路程占总路程的百分比；表示无人机初始位置；
[0032]
单架无人机的运动状态使用二阶运动模型进行描述，表示为：
[0033][0034]
其中p表示无人机的速度信息；
[0035]
式(11)通过二阶微分方程的形式描述了无人机的运动状态和一致性控制策略，实现无人机的一致性编队；
[0036]
步骤2：如图4所示，在目标无人机周围布设虚拟势场，设计虚拟势场的分段节点，使无人机保持在势能最低点处，基于此设计无人机机间避撞控制律，详细过程如下：
[0037]
步骤2
‑
1：定义引力场；
[0038][0039]
其中，ξ是引力影响因子，ρ(q,q
goal
)表示无人机当前位置q和目标位置q
goal
之间的距离；
[0040]
在引力场的基础上对距离进行求导得到引力函数：
[0041][0042]
步骤2
‑
2：定义斥力场；
[0043][0044]
其中η是斥力影响因子，ρ(q,q
bos
)代表无人机当前位置q与障碍物q
bos
之间的距离；ρ0表示每个障碍物周围存在的斥力场的半径，在斥力场的半径范围内，障碍物对无人机存在斥力，超出这个范围，障碍物对无人机无斥力；
[0045]
斥力场的梯度即为斥力：
[0046][0047]
推导得到：
[0048][0049]
虚拟势场u(q)表示为引力场和斥力场的叠加：
[0050]
u(q)＝u
rep
(q)+u
att
(q)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(17)
[0051]
步骤2
‑
3：基于人工势场的机间防撞控制律设计；
[0052]
基于虚拟势场法，在无人机周围布设虚拟势场：
[0053][0054]
其中，在||q
j
‑
q
i
||≥2d的范围内不存在虚拟势场，在d≤||q
j
‑
q
i
||<2d范围内存在引力场，在||q
j
‑
q
i
||<d的范围内存在斥力场；通过虚拟势场，使无人机在势场中梯度下降，最终保持无人机之间间距为d；
[0055]
势场函数的梯度为势力函数
[0056][0057]
根据势力函数定义无人机距离保持控制律：
[0058][0059]
其中γ是无人机机间距离保持的控制参数，m是无人机的质量；
[0060]
综合上述设计无人机机间防撞控制律：
[0061][0062]
步骤3：设计单方向的斥力效果实现障碍物规避，为最近邻障碍物点设计虚拟速度，令目标无人机的速度与虚拟速度逐渐达到一致，实现分流效果，过程如下：
[0063]
步骤3
‑
1：定义最近邻障碍物点；
[0064]
对障碍物进行凸化处理，将障碍物处理为圆形的切面或者面障碍；
[0065]
从无人机向面障碍物表面做一条垂线，垂点就是最近邻障碍物点q
d
，最近邻障碍物点q
d
位于无人机和障碍物中心的连线上，处于障碍物的边界上；
[0066]
步骤3
‑
2：设计无人机避障控制律；
[0067]
假定在最近邻障碍物的周围存在有边界的斥力场，当无人机进入最近邻障碍物的斥力场范围内时，无人机受到一个向外的斥力，方向沿着二者的连线，由此获得避障控制量：
[0068][0069]
其中，r
d
是斥力场的边界范围，r
id
是最近邻障碍物指向无人机的单位向量，实现排斥效果；
[0070]
在边界处无人机所受到的斥力最小，逐渐过渡到最大值，之后一直保持在最大值；
[0071]
定义最近邻障碍物处虚拟速度p
d
的速度方向垂直于r
id
，虚拟速度方向的单位向量
为n
id
；
[0072]
最近邻障碍物处虚拟速度p
d
的速度大小为：
[0073][0074]
其中，是初始状态的最近邻障碍物的虚拟速度；
[0075]
结合一致性算法，让无人机和最近邻障碍物速度保持一致，实现无人机在障碍物处的分流：
[0076][0077]
其中，u
g
切向导引控制量，k2为切向加速度控制量影响因子。
[0078]
本发明的有益效果如下：
[0079]
1、本发明解决了一致性集群过程中，无人机之间发生碰撞的问题。本发明主要基于人工势场法，目标无人机周围布设人工势场，设置势能最低点为期望的无人机间距，从而使无人机之间间距保持在期望范围，防止碰撞。
[0080]
2、本发明解决了一致性集群过程中遭遇障碍物的碰撞问题。本发明基于人工势场法，对障碍物进行处理之后设计最近邻障碍物点，以该点为中心布设虚拟势场，通过单方向的斥力场实现障碍物规避。此外对最近邻障碍物点设计虚拟速度，通过一致性的理论使目标无人机速度与虚拟速度达到一致，实现障碍物附近的分流。
附图说明
[0081]
图1为本发明方法的流程示意图。
[0082]
图2为本发明方法的图论拓扑矩阵。
[0083]
图3为本发明方法的中分段函数的示意图。
[0084]
图4为本发明方法的人工势场法势场分布示意图。
[0085]
图5为本发明方法的障碍物凸化示意图(上图)和最近邻障碍物斥力范围(下图)。
[0086]
图6为最近邻障碍物虚拟速度设计原理。
[0087]
图7为本发明一致性集群控制算法的仿真示意图。
[0088]
图8为本发明无人机机间避撞控制律的仿真示意图。
具体实施方式
[0089]
下面结合附图和实施例对本发明进一步说明。
[0090]
如图1和图2所示，一种基于一致性的多无人机集群防撞方法，包括以下步骤：
[0091]
步骤1：设计改进的一致性集群控制算法，具体如下：
[0092]
参考基本的一致性算法，进行一致性变量的平滑设计以及限幅处理，之后加入目标点，通过二次规划获得最佳的控制参数，实现无人机的稳定控制。
[0093]
一致性控制协议为：
[0094]
[0095]
其中，是位置梯度控制项，是速度匹配项；q表示无人机的位置，q
i
表示第i架无人机的位置，q
j
表示第j架无人机的位置，p
i
表示第i架无人机的速度，p
j
表示第j架无人机的速度，a
ij
(q)表示无人机系统邻接矩阵，σ表示范数值，d表示是无人机的期望间距；
[0096]
即：
[0097][0098]
其中，r
ij
是一个单位矢量，沿着q
i
到q
j
连线；
[0099][0100]
定义的新的σ范数：
[0101][0102]
其中，k和b分别是用来调节位置控制量的变化快慢和响应速率的参数，k∈(0,1)；
[0103]
新的σ范数是向量先求范数之后进行限幅处理，使该项参数无论大小都不会出现突变，能够更好地调整位置控制量的大小和响应的稳定性；σ范数的导数在(0,+∞)的区间内都是大于0的。
[0104]
||z||
σ
用来构建多无人机编队的平滑群体结构和运动轨迹。当z＝0时，||z||
σ
＝0。在不同的位置，可能会选择不同的参数。因为这个范数由于参数的不同，可以产生不同的影响方式和大小，从而让无人机的运动方式更加平滑高效。
[0105]
定义分段函数：
[0106][0107]
其中h∈(0,1)，控制着曲线的衰减位置；是sigmod函数的一种变形，能够保证该分段函数在分段处导数趋近于0，从而保证曲线能够平滑过渡；c用来控制sigmod函数的衰减速率和陡峭程度；z为分段参数；
[0108]
该函数被定义在[0,1]的范围内，超出这个范围的值归零，这样就可以保证一致性的协同变量可以在单位圆范围内调整，不会出现极端状况。将这个分段函数用于构建一致性协议的邻接矩阵。
[0109]
当h＝1时，该分段函数就变成了一个脉冲函数，在[0,1]的区间内，函数值均为1，其余为0。
[0110]
利用式(5)的分段函数定义无人机系统邻接矩阵：
[0111]
a
ij
(q)＝f
b
(||q
j
‑
q
i
||/r),j≠i
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
[0112]
邻接矩阵a
ij
(q)的作用范围只在半径为r的球形领域内，当两架无人机的的欧式距离超出半径为r的球形领域则邻接矩阵取零，当两架无人机的的欧式距离在半径为r的球形
领域内则邻接矩阵的元素在[0,1]内；对于任意一个q，如果i＝j则a
ij
(q)＝0；最后所有的元素组成的矩阵就是该无人机系统的空间邻接矩阵，矩阵主对角线元素为0。
[0113]
无人机编队能够执行多种任务，实现多种目标指令，而最基础的编队任务就是指向目标的无人机编队。
[0114]
单纯的一致性编队只能保证多无人机能够在一致性指令的控制下，在一定时间内完成无人机的一致性编队，即位置和速度达到一致。而在对无人机编队设定新任务目标之后，设计新的一致性控制协议：
[0115][0116]
其中，表示基于目标任务的协同控制量，
[0117][0118]
q
g
表示目标点的位置；
[0119]
而在实际的仿真过程中，无人机的速度和位置基本能够达到一致，但是到达目标点附近时，无人机速度并未降到0，导致无人机不能停到目标点附近，而是继续前进。因此在式(7)的控制协议中加入速度阻尼项
[0120][0121]
其中，k2表示速度阻尼的影响因子，p
pre
是预设的飞行速度；是一个速度阻尼项，为了让无人机能够在执行任务的过程中保证一个安全速度，并且在接近目标点附近时能够迅速衰减。
[0122][0123]
表示无人机剩余的路程占总路程的百分比，当无人机还未到达指定位置，无人机编队会先达到一致按照预设的速度前进；到达指定位置后无人机会有一个减速阶段；表示无人机初始位置；
[0124]
飞机的运动是依靠发动机的推力以及飞行时产生的空气动力完成的，通过作用于机翼上的力和力矩来改变飞行的姿态，所以可以将其近似看成是一种具有二阶微分的运动模态。无人机也是飞机的一种，因此也可以使用二阶运动模型来进行描述，单架无人机的运动状态使用二阶运动模型进行描述，表示为：
[0125][0126]
其中p表示无人机的速度信息；
[0127]
式(11)通过二阶微分方程的形式描述了无人机的运动状态和一致性控制策略，实现无人机的一致性编队；
[0128]
步骤2：在目标无人机周围布设虚拟势场，设计虚拟势场的分段节点，使无人机保持在势能最低点处，基于此设计无人机机间避撞控制律，详细过程如下：
[0129]
步骤2
‑
1：定义引力场；
[0130][0131]
其中，ξ是引力影响因子，ρ(q,q
goal
)表示无人机当前位置q和目标位置q
goal
之间的距离；
[0132]
在引力场的基础上对距离进行求导得到引力函数：
[0133][0134]
步骤2
‑
2：定义斥力场；
[0135][0136]
其中η是斥力影响因子，ρ(q,q
bos
)代表无人机当前位置q与障碍物q
bos
之间的距离；ρ0表示每个障碍物周围存在的斥力场的半径，在斥力场的半径范围内，障碍物对无人机存在斥力，超出这个范围，障碍物对无人机无斥力；
[0137]
斥力场的梯度即为斥力：
[0138][0139]
推导得到：
[0140][0141]
虚拟势场u(q)表示为引力场和斥力场的叠加：
[0142]
u(q)＝u
rep
(q)+u
att
(q)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(17)
[0143]
步骤2
‑
3：基于人工势场的机间防撞控制律设计；
[0144]
基于虚拟势场法，在无人机周围布设虚拟势场：
[0145][0146]
其中，在||q
j
‑
q
i
||≥2d的范围内不存在虚拟势场，在d≤||q
j
‑
q
i
||<2d范围内存在引力场，在||q
j
‑
q
i
||<d的范围内存在斥力场；通过虚拟势场，使无人机在势场中梯度下降，最终保持无人机之间间距为d；
[0147]
势场函数的梯度为势力函数
[0148][0149]
根据势力函数定义无人机距离保持控制律：
[0150][0151]
其中γ是无人机机间距离保持的控制参数，m是无人机的质量；
[0152]
综合上述设计无人机机间防撞控制律：
[0153][0154]
步骤3：设计单方向的斥力效果实现障碍物规避，为最近邻障碍物点设计虚拟速度，令目标无人机的速度与虚拟速度逐渐达到一致，实现分流效果，过程如下：
[0155]
步骤3
‑
1：定义最近邻障碍物点；
[0156]
在真实环境下很多障碍物都是不规则的形状，无人机的避障也会受到各种形状的干扰，因此对障碍物进行凸化处理，将障碍物处理为圆形的切面或者面障碍；
[0157]
为了保证无人机避障的安全性，在图5中选取o
k
作为实时最近邻障碍物，在(上)图中，无人机向面障碍物表面做一条垂线，垂点就是最近邻障碍物位置q
d
，在(下)图中该点位于无人机和障碍物中心的连线上，在障碍物的边界。
[0158]
步骤3
‑
2：设计无人机避障控制律；
[0159]
设计单一方向的斥力场,假定在最近邻障碍物的周围存在有边界的斥力场，当无人机进入最近邻障碍物的斥力场范围内时，无人机受到一个向外的斥力，方向沿着二者的连线，由此获得避障控制量：
[0160][0161]
其中，分段函数f
b
是一个平滑曲线，当无人机在斥力场范围外时控制量为0，之后避障控制量平滑的从0过渡到最大；q
d
是最近邻障碍物的实时坐标；r
d
是斥力场的边界范围；r
id
是最近邻障碍物指向无人机的单位向量，由于其他项均大于0，所以r
id
的方向指向无人机实现排斥效果；
[0162]
在边界处无人机所受到的斥力最小，逐渐过渡到最大值，之后一直保持在最大值。
[0163]
之后由于单纯的斥力场可能会导致无人机偏离原来航线，超调过大。对无人机在最近邻障碍物附近的路径进行导引，加入了一个沿着障碍物表面切线的虚拟速度。
[0164]
定义最近邻障碍物处虚拟速度p
d
的速度方向垂直于r
id
，虚拟速度方向的单位向量为n
id
；如图6所示，当无人机速度与连线l夹角为正(以顺时针方向为正)，虚拟速度p
d
指向正方向，反之指向反方向；虚拟速度方向的单位向量为n
id
。
[0165]
最近邻障碍物处虚拟速度p
d
的速度大小为：
[0166][0167]
其中，是初始状态的最近邻障碍物的虚拟速度；
[0168]
结合一致性算法，让无人机和最近邻障碍物速度保持一致，实现无人机在障碍物处的分流：
[0169][0170]
其中，u
g
为切向导引控制量，k2为切向加速度控制量影响因子。
[0171]
具体实施例：
[0172]
在仿真环境中，搭建集群协同控制模型，设计具体参数如下：：k＝0.5,b＝1,c＝15，d＝1，h＝0.7,k1＝8.2,k2＝2.5,r
d
＝2。
[0173]
在集群仿真验证中，设计了两种实验场景，一种是由散乱状态到有序状态之后集结到目标点，期间无人机距离保持在期望间距，不发生碰撞；另一种是集群无人机规避障碍。
[0174]
进行了十架无人机的集群仿真验证。无人机初始位置和初始速度随机生成，速度在[
‑
1,1]的区间内，目标点坐标为[5,5]。仿真步长为0.01，仿真时间6s。
[0175]
箭头尾部表示无人机当前位置，箭头方向为无人机速度方向。在图7中可以看出，无人机从不同起点出发，集结到目标点。速度保持一致，形成规则的网状结构。
[0176]
之后验证集群避障算法，仍是十架无人机进行试验。障碍物坐标为[15,15]，半径为2。如图8可知，在2.0s是无人机开始到达障碍物附近，之后从障碍物两侧分流。
[0177]
在第10s时完全避过障碍，之后重新组成编队。由此可以看出无人机避障算法效果达到预期效果，且快速性也有一定保证。