一种基于改进型人工势场法的多无人机编队避障方法与流程

1.本发明属于多无人机编队避障方法领域，具体涉及一种基于改进型人工势场法的多无人机编队避障方法。


背景技术：

2.编队避障是多无人机集群实现智能化的重要技术，其基本思想是通过障碍物的斥力场和目标位置的引力场共同作用形成一个虚拟的人工势场，再搜索一条势函数下降的方向，形成一条无碰撞的轨迹。传统人工势场法将障碍物视为一定大小的圆，模型过于简化，不符合实际情况，且障碍物周围设置影响范围，没有考虑移动障碍物的速度因素，可能导致碰撞事故。


技术实现要素：

3.发明目的：本发明所要解决的技术问题是针对现有技术的不足，提供一种基于改进型人工势场法的多无人机编队避障方法，能够实现多无人机编队避障，同时考虑了移动障碍物的问题，增强了无人机对于移动障碍物的敏感度，使得无人机不会进入任何一个障碍物的影响范围，提高了飞行安全性提高。
4.本发明方法具体包括如下内：在多无人机避障过程中，执行如下步骤：
5.步骤1：对引力增益系数ka、斥力增益系数kr、无人机安全距离区间[d
min
，d
max
]进行初始化；d
min
表示无人机安全距离最小值，d
max
表示无人机安全距离最大值；
[0006]
步骤2：初始化无人机当前位置坐标p
current
，目标点位置为q0，障碍物数量为n，障碍物的影响范围为ρ0；
[0007]
步骤3：无人机获取与编队内其他无人机之间距离d，判断d是否处于无人机安全距离区间[d
min
，d
max
]，计算无人机间的作用力f
uav
(p)；
[0008]
步骤4：通过无人机上的双目视觉模块获得障碍物的点云数据，对点云数据进行椭球拟合，拟合出障碍物的椭球体方程(利用通用的椭球拟合算法，对获取的点云数据进行处理，得到方程)；
[0009]
步骤5：无人机p的坐标为(x
p
，y
p
，z
p
)，无人机p与第i个障碍物之间的距离d
poi
(p，oi)修正为点到椭圆的最短距离，其中椭圆的方程可由椭球体得到；
[0010]
步骤6：计算无人机与障碍物的相对速度，引入斥力函数，得到改进后的斥力场函数f
rep
(p)；
[0011]
步骤7：获得无人机所受的合力；
[0012]
步骤8：计算下一个航迹点p
next
；
[0013]
步骤9：按无人机所受合力的方向运动到下一个航迹点p
next
；
[0014]
步骤10：保存无人机当前的位置坐标，并将当前无人机的位置设置为起点坐标，p
current
＝p
next
；
[0015]
步骤11：通过无人机与目标点的位置来判断无人机是否已经到达目标点，如果无
人机没有到达目标点，则返回步骤3继续执行，直到运动至目标点。
[0016]
步骤3包括：
[0017]
无人机间的作用力f
uav
(p)为：
[0018][0019]
其中ka为引力增益系数，kr为斥力增益系数，为梯度。
[0020]
步骤5包括：
[0021]
设定由障碍物椭球体得到的椭圆方程为：
[0022][0023]
其中，(x0，y0)为椭圆的中心点坐标，a，b为椭圆的焦距，(x，y)为椭圆上任意一点的坐标。
[0024]
步骤5还包括：离无人机p最近的椭圆上的点的坐标为(x2，y2)，通过求解如下方程组得到：
[0025][0026]
无人机p与第i个障碍物之间的距离d
poi
(p，oi)为：
[0027][0028]
步骤6包括：
[0029]
以障碍物与无人机的相对速度v
poc
在无人机前进方向分量v
poc
作为判断依据，只有当v
poc
＜0时，才考虑相对速度的影响；改进后的斥力势能函数u
rep
(p，v
po
)如下：
[0030][0031][0032]
其中，ξ1，ξ2为斥力系数，ρ0为障碍物影响范围。
[0033]
斥力函数f
rep
(p)为：
[0034][0035]
[0036]
x
pro
表示无人机p相对于障碍物o的位置的单位向量。
[0037]
步骤7包括：
[0038]
目标点对无人机产生的引力为f
att
(p)，按如下公式获得无人机所受的引力：
[0039]
引力势能函数u
att
(p)如下：
[0040][0041]
目标点对无人机产生的引力f
att
(p)为：
[0042][0043]
其中，ka表示引力势能场的增益系数，d
pt
(p，t)表示无人机所在的p点到目标t点的欧氏距离；
[0044]
无人机所受的合力f为：
[0045][0046]
其中f为无人机所受的合力，f
att
(p)表示目标点对无人机产生的引力，f
rep
(p)表示障碍物对无人机产生的斥力，f
uav
(p)表示编队其他无人机对无人机产生的作用力。
[0047]
本发明的有益效果：提供了优化斥力函数，增强了无人机对于移动障碍物的敏感度，使得无人机不会进入任何一个障碍物的影响范围，飞行安全性提高，同时路径曲线振荡的现象消除，速度不会高频波动，减少了能耗。
附图说明
[0048]
下面结合附图和具体实施方式对本发明做更进一步的具体说明。
[0049]
图1本发明无人机在虚拟人工势场中的受力图。
[0050]
图2是本发明方法整体流程图。
具体实施方式
[0051]
如图1所示，无人机1在虚拟的势场环境中收到多种力的作用，包括障碍物1、障碍物2对其的斥力，无人机2对其的斥力及目标点产生的引力在合力的作用下，无人机1按照本发明流程进行避障运动。
[0052]
如图2所示，本发明所要解决的技术问题是提供一种基于改进型人工势场法的多无人机编队避障方法，具体包括：
[0053]
步骤1：确定无人机的引力增益系数ka、斥力增益系数kr、无人机的安全距离[d
min
，d
max
]；
[0054]
步骤2：确定无人机的当前位置坐标p
current
，目标点的位置q0，障碍物数量为n，障碍物的影响范围为ρ0；
[0055]
步骤3：确定无人机与编队内其他无人机之间距离d，如图1所示，无人机2对无人机1表现为斥力，计算无人机间的作用力f
uav
(p)，按如下公式获得无人机间的作用力
[0056][0057]
其中ka为引力增益系数、kr为斥力增益系数，当无人机间的距离d大于无人机安全距离最大值d
max
时，无人机间表现出吸引力，当无人机间的距离d小于无人机安全距离最大值d
min
时，无人机间表现出排斥力，当无人机间的距离d位于[d
min
，d
max
]时，无人机间无作用力；
[0058]
步骤4：确定障碍物的椭球体方程，通过无人机上的双目视觉模块获得障碍物的点云数据，再对点云数据进行椭球拟合得到，设定得到的椭球体方程如下：
[0059][0060]
其中，(x0，y0)为椭圆的中心点坐标，a，b为椭圆的焦距。
[0061]
步骤5：确定无人机与第i个障碍物之间的距离d
poi
(p，oi)，按如下公式获得：
[0062][0063]
无人机与第i个障碍物之间的距离，其中无人机p的坐标为(x
p
，y
p
，z
p
)，(x2，y2)为椭圆上离无人机p最近的点的坐标，(x2，y2)按如下公式获得：
[0064][0065]
步骤6：确定引入速度场后改进的斥力函数，斥力函数f
rep
(p，v
po
)按如下公式获得：
[0066][0067][0068]urep
(p，v
po
)为改进后的斥力势能函数，为梯度，x
pro
表示无人机p相对于障碍物o的位置的单位向量。
[0069]
步骤7：确定无人机所受的合力，按如下公式获得：
[0070][0071]
其中目标点对无人机产生的引力f
att
(p)，障碍物对无人机产生的斥力为f
rep
(p)，f
uav
(p)表示编队其他无人机对无人机产生的作用力；
[0072]
步骤8：确定下一个航迹点p
next
；
[0073]
步骤9：按无人机所受合力的方向运动到下一个航迹点p
next
；
[0074]
步骤10：将当前无人机的位置设置为起点坐标，p
current
＝p
next
；
[0075]
步骤11：通过无人机与目标点的位置来判断无人机是否已经到达目标点，如果无人机没有到达目标点，则需要返回步骤3继续执行，直到运动至目标点。
[0076]
发明提供了一种基于改进型人工势场法的多无人机编队避障方法，具体实现该技术方案的方法和途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。本实施例中未明确的各组成部分均可用现有技术加以实现。