一种多无人机编队航迹规划方法及系统与流程

本发明涉及无人机航迹规划，尤其涉及一种多无人机编队航迹规划方法及系统。

背景技术：

1、近年来，随着无人机的广泛应用，相比于单架无人机，无人机的编队飞行在完成多样化任务和能够充分收集环境信息、提高任务执行的安全性及可靠性，以及增强系统鲁棒性等方面表现出明显的优势，已成为广大学者的研究热潮，许多多无人机编队航迹规划方法受到了广泛关注，主要用于解决多同构无人机最小安全距离下多起点同步到达目标点直线路径碰撞问题。

2、例如专利公布号：cn108733074a的发明专利，该专利公开了一种基于匈牙利算法的多无人机编队航迹规划方法，无人机初始位置与目标点的距离组成距离矩阵；变换距离矩阵，选取零元素打上标记，统计零元素的个数，看是否满足指派问题的最优解，对矩阵中零元素最小覆盖；找出除去最小覆盖的所有元素中的最小值，所有未覆盖元素减去该值，而所有标记的列中元素加上该值，保证原零元素的位置不变；重复执行直到能够得出最优解对应的结果矩阵；变换得出最优解，分配结束后，进行模拟飞行，以此来判断是否有相撞的可能，否则需要避障。

3、基于对专利授权公告号的检索，结合其中的不足发现：

4、现有的多无人机编队航迹规划方法，其中包括使用匈牙利算法，将起始点与目标点的距离矩阵计算出目标任务分配最优解，但是其中利用时间差条件进行避障重匹配，相比于直接使用匈牙利算法目标任务分配效率降低，且匹配路径整体距离增加。

技术实现思路

1、本发明的目的在于克服现有技术的不足，提供一种多无人机编队航迹规划方法及系统，以解决现有技术的缺陷。

2、为实现上述目的，本发明采用以下技术方案：

3、第一方面，本发明的实施例提供了一种多无人机编队航迹规划方法，包括：

4、获取编队无人机的数量n，以及所有无人机的起始点位pstart[i]、目标点位pend[i]、分轴最大速度vmax、分轴最大加速度amax、最小安全距离dmin，其中，1≤i≤n；

5、对起始点位pstart[i]和目标点位pend[i]进行预处理，计算得到起始中转点位pstf[i]和目标中转点位petf[i]；

6、对所有无人机建立以起始中转点位pstf[i]与目标中转点位petf[i]距离的平方和为最小值的目标函数，并进行带权重的任务分配求解，得到任务分配序号indexha及每一起始中转点位pstf[i]对应的目标中转点位petf[indexha[i]]和对应的目标点位pend[indexha[i]]；

7、对无人机的多段运动轨迹进行平滑处理。

8、进一步的，起始点位pstart[i]为(xstart[i]，ystart[i]，zstart[i])，目标点位pend[i]为(xend[i]，yend[i]，zend[i])，分轴最大速度vmax为(vxmax,vymax,vzmax)、分轴最大加速度amax为(axmax，aymax，azmax)，其中，xstart[i]、ystart[i]、zstart[i]分别代表起始点位pstart[i]的x坐标、y坐标、z坐标；xend[i]、yend[i]、zend[i]分别代表目标点位pend[i]的x坐标、y坐标、z坐标；vxmax、vymax、vzmax分别代表无人机在x轴向、y轴向、z轴向上的最大速度；axmax、aymax、azmax分别代表无人机在x轴向、y轴向、z轴向上的最大加速度。

9、进一步的，所述对起始点位pstart[i]和目标点位pend[i]进行预处理，计算得到起始中转点位pstf[i]和目标中转点位petf[i]，包括：

10、执行pstf[i]＝pstart[i]；

11、判断是否成立，其中，dstf[i][j]为i号无人机的起始中转点位与j号无人机的起始中转点位的距离，i！＝j，1≤j≤n；

12、若成立，则执行并重新判断是否成立，其中，为不满足dstf[i][j]＜2*dmin的矢量和的方向单位矢量，k为自定义数值，为随机方向单位向量的0.001倍；

13、若不成立，则结束计算并输出当前起始中转点位pstf[i]。

14、进一步的，所述对起始点位pstart[i]和目标点位pend[i]进行预处理，计算得到起始中转点位pstf[i]和目标中转点位petf[i]，包括：

15、执行petf[i]＝pend[i]；

16、判断是否成立，其中，detf[i][j]为i号无人机的目标中转点位与j号无人机的目标中转点位的距离；

17、若成立，则执行并重新判断是否成立，其中，为不满足detf[i][j]＜2*dmin的矢量和的方向单位矢量；

18、若不成立，则结束计算并输出当前目标中转点位petf[i]。

19、进一步的，所述对所有无人机建立以起始中转点位pstf[i]与目标中转点位petf[i]距离的平方和为最小值的目标函数，并进行带权重的任务分配求解，得到任务分配序号indexha及每一起始中转点位pstf[i]对应的目标中转点位petf[indexha[i]]和对应的目标点位pend[indexha[i]]，包括：

20、使用匈牙利算法进行带权重的任务分配求解，或使用霍普克罗夫特-卡普算法进行带权重的任务分配求解。

21、进一步的，所述对无人机的多段运动轨迹进行平滑处理，包括：

22、计算无人机由pstart[i]运动至pstf[i]的过程中各轴向的位移预估时间，将其中各轴向的位移预估时间最大的数值对应无人机序号i1；

23、计算无人机由pstf[i]运动至petf[indexha[i]]的过程中各轴向的位移预估时间，将其中各轴向的位移预估时间最大的数值对应无人机序号i2；

24、计算无人机由petf[indexha[i]]运动至pend[indexha[i]]的过程中各轴向的位移预估时间，将其中各轴向的位移预估时间最大的数值对应无人机序号i3。

25、进一步的，所述对无人机的多段运动轨迹进行平滑处理，还包括：

26、根据s型速度规划方式计算得出无人机由pstart[i1]运动至pstf[i1]关于该段路径运动总时间t1的位移序列s01，并将其归一化为序列u1(t1)；

27、根据s型速度规划方式计算得出无人机由pstf[i2]运动至petf[indexha[i2]]关于该段路径运动总时间t2的位移序列s02，并将其归一化为序列u2(t2)；

28、根据s型速度规划方式计算得出无人机由petf[indexha[i3]]运动至pend[indexha[i3]]关于该段路径运动总时间t3的位移序列s03，并将其归一化为序列u3(t3)；

29、得到无人机的三段直线路径公式：

30、p1[i]＝(pstf[i]-pstart[i])*u1(t1)

31、p2[i]＝(petf[indexha[i]]-pstf[i])*u2(t2)

32、p3[i]＝(pend[indexha[i]]-petf[indexha[i]])*u3(t3)

33、其中，0≤u1(t1)≤1，0≤u2(t2)≤1，0≤u2(t2)≤1，0≤t1≤t1，0≤t2≤t2，0≤t3≤t3，p1[i]代表t1时刻时第一段直线路径i号无人机位移增量，p2[i]代表t2时刻时第二段直线路径i号无人机位移增量，p3[i]代表t3时刻时第三段直线路径i号无人机位移增量。

34、进一步的，所述对无人机的多段运动轨迹进行平滑处理，还包括：

35、利用是否任意时刻满足机间距离大于最小安全距离dmin、行驶速度小于分轴最大速度vmax、行驶加速度小于分轴最大加速度amax作为无人机组的约束条件；

36、在满足约束条件的前提下，利用u1(t1)、u2(t2)、u3(t3)进行同步时间的叠加计算，从而得出一条平滑的曲线轨迹。

37、进一步的，还包括：

38、将起始点位pstart[i]、起始中转点位pstf[i]、目标中转点位petf[indexha[i]]、目标点位pend[indexha[i]]、u1(t1)、u2(t2)、u3(t3)记录并储存。

39、第二方面，本发明的实施例还提供了一种多无人机编队航迹规划系统，包括：

40、数据获取模块，用于获取编队无人机的数量n，以及所有无人机的起始点位pstart[i]、目标点位pend[i]、分轴最大速度vmax、分轴最大加速度amax、最小安全距离dmin，其中，1≤i≤n；

41、点位推算模块，用于对起始点位pstart[i]和目标点位pend[i]进行预处理，计算得到起始中转点位pstf[i]和目标中转点位petf[i]；

42、任务分配模块，用于对所有无人机建立以起始中转点位pstf[f]与目标中转点位petf[i]距离的平方和为最小值的目标函数，并进行带权重的任务分配求解，得到任务分配序号indexha及每一起始中转点位pstf[i]对应的目标中转点位petf[indexha[i]]和对应的目标点位pend[indexha[i]]；

43、路径规划模块，用于对无人机的多段运动轨迹进行平滑处理。

44、与现有技术相比，本发明的多无人机编队航迹规划方法，采用预处理点位方式，结合目标任务分配以及同步协同起飞降落，对多无人机进行路径规划，充分利用无人机最小安全距离，从而缩短无人机路径总行程，不需要进行时间差避障选择，减少无人机姿态变换频率，减少了计算量，提升了计算效率。

45、上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明技术手段，可依照说明书的内容予以实施，并且为了让本发明的上述和其它目的、特征及优点能够更明显易懂，以下特举较佳实施例，详细说明如下。