本发明属于无人机编队路径规划术领域，具体涉及一种基于补偿前瞻算法的多无人机编队路径规划方法。
背景技术：
1、无人机在协同侦察、协同打击、飞行表演、快递物流和农业喷洒等领域有着广泛的使用，这也导致了基于多无人机(multi-uav)集群的控制系统(uas)成为了目前国内外研究的重点。在无人机的研究领域中，路径规划是一项重要的研究内容，一个常见的问题是如何确定目标序列并或得最短的飞行路径或时间，这个问题与通常研究的旅行商问题(tsp)的变种多旅行商问题(mtsp)类似，但是也有着一些区别。在mtsp问题研究中，目标点之间的路径按照几何直线分布，而无人机的运动是受最大曲率约束的曲线运动，特别是固定翼无人机，它们的轨迹曲率有界，模型中的曲率设置需要更符合实际需求，因此，在研究中，科研人员通常将无人机的路径规划用dubins曲线进行替代。尽管无人机路径规划问题与mtsp问题近似，但由于曲率等运动学条件约束，往往更难求解，这就需要科研人员提出新的求解方法。
2、将dubins问题与mtsp问题结合起来称为求解dtsp问题。求解dtsp问题不仅需要确定目标序列，不同的航向角会导致不同的路径规划，因此航向角的确定必须在目标序列确定前完成。通过贪心算法暴力求解dtsp的方法是不可取的，随着目标点的增加，计算量呈指数增加，因此，求解预先确定的目标序列是解决受曲率约束的路径规划问题的常用方法。
3、ma等人(2006)在文章中提出了一种基于后退原理的算法，通过路径点序列计算光滑路径，但是作者还没有开发出一个完整的能够表征所有可能几何构型的三个路径点和初始方向的通式，从而可以唯一确定任何问题的最佳轨迹；pantelis isaiah等人(2015)结合k步预判与地平线后退原理，将目标序列打乱，提出一种k步预演算法，但是该算法仅适用于单机目标点较少时的情况；izack cohen等人(2017)提出了一类新的基于离散化的预见性算法(dlaas)，将各航路点的hoa离散化，形成整数优化规划，用整数规划求解器计算其解，但是该算法在大规模编队计算中存在计算量大、计算效率低的问题。
4、综上所述，现有技术在面对日益增长的大规模编队协同任务规划需求时，存在模型中计算量大、计算效率低的问题，如何在保证目标点集规模不变的前提下，减少了单机路径规划的计算规模，在计算量和优化路径间取得平衡是需要解决的重点。
技术实现思路
1、为了克服上述现有技术存在的不足，本发明的目的在于提供一种基于补偿前瞻算法的多无人机编队路径规划方法，该规划方法在三点曲线的基础上，加入相邻前后任一点航向角对中间点的选择影响，其次，在多无人机编队中，采用etsp算法对待分配的目标点进行初分配，形成多个目标点子集，然后再采用补偿前瞻算法对各个子集进行路径规划，在计算时间和优化路径间取得了平衡；
2、通过加入第四目标点优化航向角选择，能够更有效率对目标点路径进行规划，通过对待分配的目标点进行初分配，优化了系统模型的计算效率。为了实现上述目的，本发明采用的技术方案是：
3、一种基于补偿前瞻算法的多无人机编队路径规划方法，包括以下步骤；
4、步骤s1：获得所有任务目标点坐标，设目标点坐标集合为target＝[(x1,y1),(x2,y2),…,(xn,yn)]∈r2，无人机编队集合为u；
5、步骤s2：随机生成单个无人机对应的初始任务目标点序列；
6、步骤s3：将任务目标点坐标间的无人机轨迹设为直线(etsp模型)，即任务规划模型，采用仿生算法对任务规划模型求解计算，代价值为任务目标点坐标间的直线距离之和，得到任务目标点的路径规划结果，获得若干个无人机目标子集subi＝{(xi,yi)}∈target，获得的目标子集作为需要求解的目标点集合；
7、步骤s4：在每一个子集subi的内部，通过补偿前瞻算法对目标点序列进行大规模无人机编队协同路径规划问题求解，获得符合要求的无人机编队规划路径结果，无人机编队规划路径结果包括每一架无人机对应的任务目标点序列以及在对应目标点上的航向角。
8、所述步骤s1中，目标点坐标通过预设提供的数据获取。所述步骤s2中，随机为单个无人机生成对应的初始任务目标点序列。
9、所述步骤s3中，采用基于etsp预分组的求解方法，在简化计算量的同时保证了最短路径；
10、以无人机编队集合起始点出发到返回所有无人机的总航程作为大规模无人机协同路径规划模型优化的目标，所有目标点的坐标用target＝[(x1,y1),(x2,y2),…,(xn,yn)]表示，无人机编队集合任务规划后两点之间的距离表示为：
11、di,j＝|xi-xj|+|yi-yj| (1)
12、所述无人机为固定翼无人机，固定翼无人机的变轨属于线性变化的过程，在两点间的距离公式添加符合无人机运动轨迹的约束条件。
13、所述线性变化dubins曲线方程为：
14、di,j＝l+r+d (2)
15、式(2)中的d代表任务规划航迹中相邻两任务点dubins曲线直线部分，l、r为相邻两任务点到直线间的弧长。
16、进一步的，无人机编队dubins多旅行商模型优化目标为无人机编队最短航程，根据式(2)，目标函数用如下表示：
17、
18、上式(3)中，ki,j,q代表第q架无人机在第i个和第j个目标点间是否有规划任务，ki,j,q值为0或1。
19、所述约束条件的约束函数如下：
20、
21、式(4)为每架无人机分配到的目标数，该约束条件保证每一架无人机分配到的目标不少于k个；
22、式(5)为无人机集群分配到的目标总数，该约束条件保证所有目标点都分配到；
23、式(6)为任一目标点分配到无人机的次数，该约束条件保证每一个点只分配一次；
24、式(7)为起始点和结束点的判定式，(xw,yw)为单个无人机规划的最后一个目标点坐标，即要求每一架飞机起始点和终止都是同一个点。
25、所述步骤s4中，固定翼无人机的运动轨迹是线性变化的，其运动轨迹定义为一条曲线：
26、x(t)＝[x(t),y(t),θ(t)]t∈r2×s1 (8)
27、二维环境下无人机的运动情况，x、y代表无人机的笛卡尔坐标系位置，θ代表无人机相对于x轴的夹角，即无人机的航向角。
28、设定模型中无人机运动速率为1，最大曲率为1/γ，用如下运动学方程表达：
29、
30、其中θ∈[-1/γ1/γ]，常数u为无人机随时间变化的控制变量，|u|≤1。
31、无人机运动模型中，无人机的运动速率设定为1，dubins曲线最短路径的性能泛函用积分表示，即式(12)所示；
32、
33、上式中t0、tf代表无人机初始时刻和终端时刻，设定为最短路径的参数，结合式(9)(10)(11)无人机的运动学方程与式(12)性能泛函，根据最优控制理论中的定理构造哈密顿函数，即无人机最短路径哈密顿函数为：
34、
35、根据pontryagin极大值原理，获得dubins曲线最短路径的必要条件；
36、
37、上式表明，和在无人机运动周期内[t0，tf]上是分段常数；
38、由于控制函数u中的哈密顿量是线性的，所以最优控制有两种类型；
39、如果则如果u∈[-1，1]，第一个条件对应于半径为r的最大曲率弧，后一个条件对应于方向为的直线轨迹。
40、dubins曲线的最短路径与航向角密切相关，不同航向角的选择会导致路径细节中的不同选择，因此如何确定dubins曲线中的航向角是确定无人机最短路径的关键。
41、在dubins曲线初始点(x0,y0)和终端点(x2,y2)的中间，加入了中间点(x1,y1)，并且获得了中间点的边界关系；
42、其中红色轨迹为途径初始点、中间点和终端点的dubins最优路径，θ0为初始点(x0,y0)的方向角(相对于x轴)，θ1、θ2为所在直线的角度(相对于x轴)，θm为中间点(x1,y1)的方向角(相对于x轴)，从中间点(x1,y1)到两条直线段交点a的一条线平分了交点角，在最优路径的线段上，
43、
44、由上式推导出k1/k2＝-tan(θ1+θ2)/2，哈密顿函数在(x1,y1)点的最优轨迹上是连续的，因此有如下表示。
45、
46、由上式可知：
47、k1 cosθm+k2 sinθm＝0 (17)
48、由式(15)和式(17)可以获得式(18)；
49、
50、通过式(18)获得最短路径的中点航向角大小，求耦合三角方程的解；
51、基于四点优化的计算航向角的算法，给出了一个计算目标点航向角大小的通式：
52、首先，考虑初始点、中间点和终端点三个任务点间距离较远时的情况，红色实线为dubins轨迹，点f(x4,y4)、点d(x5,y5)为两个圆轨迹的切点；
53、通过式(18)求解中间点方向角θm，但是当初始点和中间点距离较远时，点f(x4,y4)与点d(x5,y5)所在线段的角度θ1(相对于x轴)与初始点b(x0,y0)和切点d(x5,y5)所在线段(红色虚线线段)的角度θ3(相对于x轴)之间的误差忽略不计，用初始点b的坐标(x0,y0)直接代入式(18)求解中间点的方向角θm(相对于x轴)；
54、获得任务规划后三点间的总航程l，如下所示：
55、
56、上式中，a为初始点b到切点d的距离，b为终端点c到切点e的距离，α、β为夹角，s弧de为弧线de的长度，由式可知，l的取值与α、β取值有关，即式(20)所示：
57、
58、由式可知，当α+β取值越小、α＝β时，式值最小，当圆心o在角c的角平分线上时，总航程l取得最小值，为式(19)中间点(x1,y1)的航向角
59、其次，考虑初始点、中间点和终端点三个任务点间距离较近时的情况，当初始点和中间点距离较近时，点f(x4,y4)与点d(x5,y5)所在线段的角度θ1(相对于x轴)与初始点b(x0,y0)和切点d(x5,y5)所在线段(红色虚线线段)的角度θ3(相对于x轴)之间的误差不可以忽略不计，那么就不可以用初始点b的坐标(x0,y0)直接代入式(18)求解中间点的方向角θm(相对于x轴)这种情况下初始点和终端点的航向角会影响θ1、θ2的取值。
60、当目标点距离较近时，原本的θ1与实际的θ3相差比较大，这时候如果继续用θ1计算的话，会造成较大的误差。完全耦合三角方程的解很难求出，提出一种四点优化的航向角算法，尽量减小误差项带来的偏差；
61、在初始点与种中间点距离较近的情况是，初始点(x0,y0)的航向角会影响中间点(x1,y1)的航向角，引入一个初始点(x0,y0)的相邻点(x6,y6)，以初始点距离中间点距离较远时的优化算法对目标点(x6,y6)进行预处理，获得一个切点q1(x4,y4)，以此切点q1(x4,y4)代替初始点(x0,y0)成为新的目标点，对中间点(x1,y1)进行上一步操作，这样获得了一个补偿中点的航向角θm，θ1为切点q1(x4,y4)和切点q2(x5,y5)所在线段的角度，θ2为切点q3(x7,y7)和终端点c所在线段的角度，o1、o2为两个圆轨迹的圆心点，黑色实线为第一步求解替代点q1的过程，黑色虚线为替代点q1代替初始点b求解中间点方向角θm的过程。
62、
63、本发明的有益效果：
64、本发明具备对复杂环境下的大规模无人机编队路径规划的能力。
65、本发明针对编队路径规划效率和模型计算效率的问题，设计了基于补偿前瞻算法的大规模无人机编队路径规划方法，提升了编队的协同能力。
66、本发明采用etsp预分配模型，较大程度降低了路径规划模型的计算规模，提高了规划效率。
67、本发明由于基于补偿前瞻算法的多无人机编队路径规划方法，解决了无人机在任务目标点最优航向角的选择问题。
1.一种基于补偿前瞻算法的多无人机编队路径规划方法，其特征在于，包括以下步骤；
2.根据权利要求1所述的一种基于补偿前瞻算法的多无人机编队路径规划方法，其特征在于，所述步骤s1中，目标点坐标通过预设提供的数据获取；
3.根据权利要求1所述的一种基于补偿前瞻算法的多无人机编队路径规划方法，其特征在于，所述步骤s3中，采用基于etsp预分组的求解方法，在简化计算量的同时保证了最短路径；
4.根据权利要求1所述的一种基于补偿前瞻算法的多无人机编队路径规划方法，其特征在于，所述无人机为固定翼无人机，固定翼无人机的变轨属于线性变化的过程，在两点间的距离公式添加符合无人机运动轨迹的约束条件。
5.根据权利要求4所述的一种基于补偿前瞻算法的多无人机编队路径规划方法，其特征在于，所述线性变化dubins曲线方程为：
6.根据权利要求5所述的一种基于补偿前瞻算法的多无人机编队路径规划方法，其特征在于，无人机编队dubins多旅行商模型优化目标为无人机编队最短航程，根据式(2)，目标函数用如下表示：
7.根据权利要求4所述的一种基于补偿前瞻算法的多无人机编队路径规划方法，其特征在于，所述约束条件的约束函数如下：
8.根据权利要求7所述的一种基于补偿前瞻算法的多无人机编队路径规划方法，其特征在于，所述步骤s4中，固定翼无人机的运动轨迹是线性变化的，其运动轨迹定义为一条曲线：
9.根据权利要求8所述的一种基于补偿前瞻算法的多无人机编队路径规划方法，其特征在于，无人机运动模型中，无人机的运动速率设定为1，dubins曲线最短路径的性能泛函用积分表示，即式(12)所示；
10.根据权利要求9所述的一种基于补偿前瞻算法的多无人机编队路径规划方法，其特征在于，基于四点优化的计算航向角的算法，给出了一个计算目标点航向角大小的通式：
