一种基于人工势场的无人机编队防撞方法

一种基于人工势场的无人机编队防撞方法
1.本发明属于无人机技术领域，具体涉及一种基于人工势场的无人机编队防撞方法。


背景技术：

2.无人机编队飞行，旨在将一定数量的无人机按一定队形排列，在飞行途中保持队形不变。无人机机间防撞是无人机编队安全飞行的前提，在无人机集结以及队形变化的过程中无人机位置的不确定性导致无人机存在相互碰撞风险，现有的机间防撞主要通过路径规划、人工势场法、几何法实现，对于大规模无人机编队，事先进行路径规划存在较大困难且缺乏灵活性，几何法与人工势场法可以在线实现，但几何法所需计算量较大，因此人工势场更广泛的应用于大规模无人机编队中，现有的基于人工势场的无人机间防撞方法主要分为高度方向以及水平方向防撞，防撞效果不理想。
3.结合现有技术成果本技术提出了一种基于人工势场的无人机防撞方法。


技术实现要素：

4.本发明的目的在于提供一种基于人工势场的无人机编队防撞方法，以解决上述背景技术中提出的问题。
5.一种基于人工势场的无人机编队防撞方法，具体包括如下步骤：
6.步骤一、构建人工势场模型，人工势场分为防撞区域和碰撞区域，当另一架无人机进入防撞区域时人工势场开始计算相应的人工力，当另一架无人机进入碰撞区域认为无人机发生碰撞；
7.步骤二、计算无人机间相对距离，相对角度：
[0008][0009][0010][0011]
其中xi,yi,zi为无人机i在惯性坐标系下的坐标，v
x
,vy,vz为无人机i在惯性坐标系下的速度，dxyz为无人机间相对距离矩阵，yaw为无人机间相对俯仰角矩阵，表示无人机i与无人机j的相对俯仰角，projection为无人机j在无人机i速度方向的投影矩阵；
[0012]
步骤三、计算人工力，对于进入防撞区域的无人机执行以下计算步骤：
[0013]
s1：计算yaw每一行的和记作yaw
sum
(i)；
[0014]
s2：判断每架无人机的位置信息是否满足条件 (dxyz(i,j)＜rd)&&(projection(i,j)＜0)do；
[0015]
s3：对于满足条件的无人机，使用相应的人工势场函数计算人工力 u
x
(i),uy(i),uz(i)；
[0016]
s4：对满足第二步条件的无人机判断是否满足如下条件：yaw
sum
(i)≥π/4 满足条件的无人机输出控制指令uz(i)＝sum(uz)、不满足条件的输出控制指令 u
xy
(i)＝atan(sum(uy)/sum(u
x
))；
[0017]
上述算法输入为无人机间相对距离dxyz、相对俯仰角dxyz、投影值 projection；输出uz(i)为第i架无人机在竖直方向的过载指令，输出u
xy
(i)为第i 架无人机在水平方向的航向角指令。
[0018]
与现有技术相比，本发明的有益效果是：改善现有基于人工势场防撞方案，通过计算无人机间相对投影，实现了只有速度前向有无人机时启动防撞步骤，减小了人工势场触发次数，利用无人机间相对俯仰角，实现防撞指令分配，不再单一选择高度防撞或者航向角防撞，进一步提高了防撞效果。
附图说明
[0019]
图1为本发明人工势场示意图；
[0020]
图2为本发明无人机相对投影示意图；
[0021]
图3为实验初始条件数据表；
[0022]
图4为本发明无人机机间距离图；
[0023]
图5为本发明编队飞行结果图；
[0024]
图6为本发明计算人工力的伪代码运算。
具体实施方式
[0025]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。
[0026]
本发明提供了一种基于人工势场的无人机编队防撞方法，具体包括如下步骤：
[0027]
步骤一、构建人工势场模型，人工势场即划定一个空间区域，进入该区域的无人机将受到一种抽象人工力场的作用，力场对无人机的作用通过施加在无人机上的排斥人工力体现，排斥人工力通过计算人工势场势函数梯度得到。人工势场分为防撞区域和碰撞区域，当另一架无人机进入防撞区域时人工势场开始计算相应的人工力，当另一架无人机进入碰撞区域认为无人机发生碰撞；
[0028]
步骤二、计算无人机间相对距离，相对角度：
[0029][0030]
[0031][0032]
其中xi,yi,zi为无人机i在惯性坐标系下的坐标，v
x
,vy,vz为无人机i在惯性坐标系下的速度，dxyz为无人机间相对距离矩阵，yaw为无人机间相对俯仰角矩阵，表示无人机i与无人机j的相对俯仰角，projection为无人机j在无人机i速度方向的投影矩阵；
[0033]
步骤三、计算人工力，对于进入防撞区域的无人机执行以下计算步骤(该步骤的伪代码如附图6所示)：
[0034]
s1：计算yaw每一行的和记作yaw
sum
(i)；
[0035]
s2：判断每架无人机的位置信息是否满足条件 (dxyz(i,j)＜rd)&&(projection(i,j)＜0)do；
[0036]
s3：对于满足条件的无人机，使用相应的人工势场函数计算人工力 u
x
(i),uy(i),uz(i)；
[0037]
s4：对满足第二步条件的无人机判断是否满足如下条件：yaw
sum
(i)≥π/4 满足条件的无人机输出控制指令uz(i)＝sum(uz)、不满足条件的输出控制指令 u
xy
(i)＝atan(sum(uy)/sum(u
x
))；
[0038]
上述算法输入为无人机间相对距离dxyz、相对俯仰角dxyz、投影值 projection；输出uz(i)为第i架无人机在竖直方向的过载指令，输出u
xy
(i)为第i 架无人机在水平方向的航向角指令。
[0039]
上述算法第1步计算相对俯仰角的和，第2步开始计算无人机j与无人机i间的人工力，第3步给出了判断条件，第一个判断条件比较无人机i与无人机j间的距离，第二个判断条件比较无人机ij位置向量在无人机i速度方向的投影，当投影值小于零时无人机j在无人机i速度坐标系x正半轴方向的空间内，当条件同时满足时认为无人机j在无人机i前方且距离小于防撞距离此时无人机i计算人工力；第6步判断无人机i与无人机j相对俯仰角之和，当相对俯仰角之和大于45度时在通过调整高度实现防撞，在相对俯仰角之和小于45度时通过调整航向角实现防撞，指令航向角根据第9步计算。
[0040]
针对上述的实施例做如下的验证试验：
[0041]
一、实验前期准备
[0042]
实验人工势场函数如下：
[0043][0044]
其中p代表无人机位置向量[xiyizi]、rd＝10m,rc＝5m。
[0045]
实验编队无人机运动学模型如下：
[0046][0047]
公式(5)中，v
xi
，v
yi
，v
zi
分别代表无人机沿x,y,z三个方向的速度，a
xi
， a
xi
，a
zi
分别代表无人机沿x,y,z三个方向的加速度。
[0048]
利用如下控制律实现编队稳定
[0049][0050]
公式(6)中，u
xi
，u
yi
，u
zi
为无人机在x、y、z方向的加速度指令，a
ij
代表无人机间通讯拓扑邻接拓扑矩阵中对应的元素，如果个体i可以接受到个体j 的信息，则a
ij
＝1,i,j＝1,2,l,n,i≠j，反之a
ij
＝0，r
x
,ry.rz为无人机机间的期望距离。期望无人机队形沿y轴排列相邻无人机间距离10m，期望速度为10m/s，期望航向角为0度，期望高度30m。
[0051]
二、将发明内容与人工势场函数结合进行实验，实验初始条件见附图3：
[0052]
三、实验时长20s，实验步长0.01s，图4，图5分别给出了无人机间距离情况以及飞行轨迹，图4中水平的虚线表示rc＝5m即碰撞距离，可以看到无人机间距离均大于碰撞距离，验证了本专利提出的防撞算法的有效性，结合图4 图5可以看到无人机间距离以及飞行轨迹很快达到期望状态，展示了本发明提出的防撞算法对编队影响较小的特点。
[0053]
最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换,凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。