多无人机目标定位的航路规划方法与流程

本发明涉及一种无人机航路规划方法，特别涉及一种多无人机目标定位的航路规划方法。

背景技术：

随着无人机的使用越来越广泛，无人机在一些领域也发挥着重要作用。对其在航路规划上的研究能在无人机对地面目标定位的过程中，由于固定翼无人机灵活性不高，与旋翼无人机相比有更多的约束条件，所以对无人机的飞行航路规划进行研究能有效地减少无人机到达目的地的时间，提高了无人机执行任务的效率。目前无人机的航路规划已经比较成熟，但是其中大部分方法计算量较大而导致计算用时较长，不能满足无人机目标定位盘旋飞行航路规划的需要。所以如何快速规划出一条可行的无人机从出发点到目标上方盘旋飞行的路径，仍是一个难点。

文献“无威胁情况下任意两点间的无人机路径规划《系统工程与电子技术》,2009,31(9):2157-2162页)”公开了如何切入圆形轨迹的方法，但其并没有考虑多无人机定点等距盘旋以及同时到达的情况。

技术实现要素：

为了克服现有无人机航路规划方法实用性差的不足，本发明提供一种多无人机目标定位的航路规划方法。该方法首先由无人机的导航系统获取无人机的位置信息，以及搜索得到的地面固定目标的大致位置。获取无人机的飞行速度v、最大滚转角以及绕固定目标飞行的轨迹圆半径r作为初始化参数。由无人机所在位置信息得到距离目标最近的无人机，标记为uav1，计算其进入最小转弯圆c1的航迹点uav1_p1。由c1的圆心和目标位置计算三架uav进入轨迹圆的航迹点uav1_p2、uav2_p2以及uav3_p2。通过航迹点uav2_p2以及uav3_p2来计算uav2和uav3的最小转弯圆圆心位置以及uav2与uav3分别切入它们的最小轨迹圆c2与c3的切点航迹点位置。选取uav1在切入轨迹圆后绕最小转弯圆c1的飞行圈数，计算得到其uav2、uav3所绕大圆c4、c5半径及圆心位置。计算飞行路程以及飞行所需时间。本发明在多架无人机初始位置随机的情况下，能够保证多架无人机能够快速的到达目标上空，并迅速构成在轨迹圆上等距的编队构型。规划所需要的信息少，易于工程实现，计算量非常小，规划速度快。形成的规划路段只有直线段和曲线段，能达到飞行路程尽量短，飞行时间尽量少的目的，实用性好。

本发明解决其技术问题所采用的技术方案：一种多无人机目标定位的航路规划方法，其特点是包括以下步骤：

步骤一、由无人机的导航系统获取无人机的位置信息，搜索得到的地面固定目标位置。获取无人机的飞行速度、最大滚转角以及绕固定目标飞行的轨迹圆半径作为初始化参数。

步骤二、根据无人机的飞行速度v、最大滚转角由公式计算得到无人机的最小转弯半径r，轨迹圆半径r。

无人机uav1直接沿着uav1与固定目标g所在直线飞行，考虑到最小转弯半径，无人机在航迹点uav1_p1处开始沿最小转弯半径所形成的最小转弯圆c1飞行，c1与轨迹圆相切。uav1与目标所在直线与最小转弯圆c1相切，由uav1与g所在直线的法线与最小转弯半径得到c1的圆心位置。uav在一个圆上逆时针飞行，飞入与该圆外切的另一圆时，飞行方向会由逆时针变为顺时针。而无人机在飞行时向左绕最小转弯圆飞行会沿逆时针飞行。c1的圆心位置计算公式如下：

其中kuav1_g是uav1与g所在直线的斜率，xc1、yc1分别为c1圆心的横坐标和纵坐标。如果x1大于x_goal，式中进行加运算，反之进行减运算。

之后uav1一直绕c1飞行，然后进入轨迹圆。航迹点uav1_p1由无人机与固定目标所在直线及最小转弯半径以及轨迹圆半径求得：

式中kuav1_c1为uav1与c1圆心所在直线的斜率，xuav1_p1、yuav1_p1分别为航迹点uav1_p1的横坐标和纵坐标。

步骤三、由一号最小转弯圆c1的圆心位置及目标位置得到多架无人机进入轨迹圆的航迹点，公式如下：

式中n为需要规划的无人机的个数，m为当前计算的第m架无人机，kc1_g为c1圆心与目标所在直线的斜率，xuav_p2、yuav_p2分别为某架无人机进入轨迹圆的航迹点的横坐标和纵坐标。无人机通过无人机相对目标的方位来做判断以便使用两点法和反正切arctan方法求得航迹点uav1_p2与固定目标所在直线的角度，然后求从航迹点uav1_p1到航迹点uav1_p2所转过的弧度theta1，公式如下：

如果xc1大于x_goal，theta1需要加上π。如果x1小于xc1，theta1需要减去π。此时如果theta1小于0，那么theta1需要加上2π。

得到轨迹圆上的其余两架无人机进入轨迹圆的航迹点uav1_p2、uav2_p2、uav3_p2位置如下：

步骤四、由航迹点uav2_p2、uav3_p2、固定目标位置轨迹圆半径r以及最小转弯半径r解算出二号最小转弯圆c2的圆心坐标，公式如下：

其中kc2_g为c2的圆心与g所在直线的斜率，由于c2圆心、目标g以及uav2切入轨迹圆的航迹点uav2_p2也就是c2与轨迹圆的交点这三点共线。

然后利用三角函数中的反余弦函数及勾股定理得到uav2、uav3进入它们的最小转弯圆的航迹点uav2_p1、uav3_p1，公式如下：

其中kuav2_c1为uav2与c1圆心所在直线的斜率。已知航迹点uav2_p1、uav3_p1、c2以及c3的圆心位置，根据反正切arctan函数的特性，需要判断uav2_p1、uav3_p1在圆上的角度是否需要加π。然后分别计算uav2从航迹点uav2_p1到航迹点uav2_p2所转过的角度theta2，以及uav3从航迹点uav3_p1到航迹点uav3_p2所转过的角度theta3。

步骤五、计算每架无人机沿直线从{[x1,y1],[x2,y2],[x3,y3]}到航迹点{uav1_p1,uav2_p1,uav3_p1}以及沿最小转弯半径从航迹点p1曲线飞行到航迹点{uav1_p2,uav2_p2,uav3_p2}的路程length1、length2、length3，其中length1的公式如下：

length2、length3的计算方法与length1相同。

假设距离固定目标最近的无人机只能绕最小转弯圆c1飞行，那么设它在到达航迹点uav1_p1时，还需要继续绕n圈，而其余两架无人机可以绕任意半径大于最小转弯半径的圆c4、c5，并且在他们绕完这个大转弯圆后进入轨迹圆。

设置一个循环语句，循环条件为另两架无人机的转弯圆半径rc2,、rc3不小于最小转弯半径r，每次循环距离固定目标最近的无人机所绕最小转弯圆加1，然后计算另外两架无人机的转弯圆半径r以及圆心位置。rc2、rc3计算公式如下：

无人机从初始位置进入轨迹圆所花费的飞行时间为：

time＝(length2+2·πrc2)/v(12)

无人机从初始位置到进入轨迹圆的总路程为：

length＝length2+2·πrc2(13)

至此，航迹规划结束。

本发明的有益效果是：该方法首先由无人机的导航系统获取无人机的位置信息，以及搜索得到的地面固定目标的大致位置。获取无人机的飞行速度v、最大滚转角以及绕固定目标飞行的轨迹圆半径r作为初始化参数。由无人机所在位置信息得到距离目标最近的无人机，标记为uav1，计算其进入最小转弯圆c1的航迹点uav1_p1。由c1的圆心和目标位置计算三架uav进入轨迹圆的航迹点uav1_p2、uav2_p2以及uav3_p2。通过航迹点uav2_p2以及uav3_p2来计算uav2和uav3的最小转弯圆圆心位置以及uav2与uav3分别切入它们的最小轨迹圆c2与c3的切点航迹点位置。选取uav1在切入轨迹圆后绕最小转弯圆c1的飞行圈数，计算得到其uav2、uav3所绕大圆c4、c5半径及圆心位置。计算飞行路程以及飞行所需时间。本发明在多架无人机初始位置随机的情况下，能够保证多架无人机能够快速的到达目标上空，并迅速构成在轨迹圆上等距的编队构型。规划所需要的信息少，易于工程实现，计算量非常小，规划速度快。形成的规划路段只有直线段和曲线段，能达到飞行路程尽量短，飞行时间尽量少的目的，实用性好。

下面结合附图和具体实施方式对本发明作详细说明。

附图说明

图1是本发明多无人机目标定位的航路规划方法的流程图。

图2是本发明方法中无人机向目标盘旋飞行过程示意图。

图3是本发明方法进行多uav对目标定点盘旋飞行的航路规划效果图。

具体实施方式

参照图1-3。

无人机在搜索到目标后，通过三架无人机本身以某种规律随机分布，且三架飞机之间呈斜线型编队构型，首先确定无人机绕目标飞行的方向，然后再规划每架无人机的航迹点。

本发明多无人机目标定位的航路规划方法具体步骤如下：

步骤1：由无人机的导航系统获取无人机的位置信息，以及搜索得到的地面固定目标的大致位置。获取无人机的飞行速度、最大滚转角，以及绕固定目标飞行的轨迹圆半径作为初始化参数。假定需要规划三架uav的航路。

步骤2：假定三架无人机编号为{uav1,uav2,uav3}，其位置坐标为{[x1,y1],[x2,y2],[x3,y3]}，其中uav1为距地面固定目标最近的无人机，uav2其次，uav3最远。地面目标编号为g，坐标为[x_goal,y_goal]，根据无人机的飞行速度v、最大滚转角由公式计算得到无人机的最小转弯半径r，轨迹圆半径为r。由于无人机所在高度不同，因此不必考虑无人机相撞这一情况。同时确定飞行约束条件，固定翼无人机只能以直线飞行或曲线飞行，而其最小转弯半径也是其最小曲率半径。

无人机uav1直接沿着uav1与固定目标g所在直线飞行，考虑到最小转弯半径，无人机在航迹点uav1_p1处开始沿最小转弯半径所形成的最小转弯圆c1飞行，c1与轨迹圆相切。uav1与目标所在直线与最小转弯圆c1相切，所以可以由uav1与g所在直线的法线与最小转弯半径得到c1的圆心位置。由圆与圆相切的特性可知，uav在一个圆上逆时针飞行，飞入与该圆外切的另一圆时，飞行方向会由逆时针变为顺时针。而无人机在飞行时向左绕最小转弯圆飞行会沿逆时针飞行。c1的圆心位置计算公式如下：

其中kuav1_g是uav1与g所在直线的斜率，xc1、yc1分别为c1圆心的横坐标和纵坐标。如果x1大于x_goal，式中进行加运算，反之进行减运算。

之后uav1一直绕c1飞行，然后进入轨迹圆。航迹点uav1_p1由无人机与固定目标所在直线及最小转弯半径以及轨迹圆半径求得：

式中kuav1_c1为uav1与c1圆心所在直线的斜率，xuav1_p1、yuav1_p1分别为航迹点uav1_p1的横坐标和纵坐标。

步骤3：由一号最小转弯圆c1的圆心位置及目标位置可以得到多架无人机进入轨迹圆的航迹点:，公式如下：

式中n为需要规划的无人机的个数，m为当前计算的第m架无人机，kc1_g为c1圆心与目标所在直线的斜率，xuav_p2、yuav_p2分别为某架无人机进入轨迹圆的航迹点的横坐标和纵坐标。无人机通过无人机相对目标的方位来做判断以便使用两点法和反正切arctan方法求得航迹点uav1_p2与固定目标所在直线的角度，然后求从航迹点uav1_p1到航迹点uav1_p2所转过的弧度theta1，公式如下：

如果xc1大于x_goal，theta1需要加上π。如果x1小于xc1，theta1需要再减去π。此时如果theta1小于0，那么theta1需要加上2π。

这里由于假定三架无人机，那么得到轨迹圆上的其余两个无人机进入轨迹圆的航迹点uav1_p2、uav2_p2、uav3_p2位置如下：

步骤4：由航迹点uav2_p2、uav3_p2、固定目标位置轨迹圆半径r以及最小转弯半径r可以解算出二号最小转弯圆c2的圆心坐标，公式如下：

其中kc2_g为c2的圆心与g所在直线的斜率，由于c2圆心、目标g以及uav2切入轨迹圆的航迹点uav2_p2也就是c2与轨迹圆的交点这三点共线，kc2_g也可以使用uav2_p2与g使用两点法计算出来。c3的圆心坐标的计算方法与c2类似，只不过用在arctan中的是c3与目标所在直线的斜率。

然后利用三角函数中的反余弦函数及勾股定理可以得到uav2、uav3进入它们的最小转弯圆的航迹点uav2_p1、uav3_p1，公式如下：

其中kuav2_c1为uav2与c1圆心所在直线的斜率。uav3_p1的计算方法与上面相同。已知航迹点uav2_p1、uav3_p1、c2以及c3的圆心位置，根据反正切arctan函数的特性，需要判断uav2_p1、uav3_p1在圆上的角度是否需要加π。然后分别计算uav2从航迹点uav2_p1到航迹点uav2_p2所转过的角度theta2，以及uav3从航迹点uav3_p1到航迹点uav3_p2所转过的角度theta3，方法同步骤3中计算theta1相同。

步骤5：计算每架无人机沿直线从{[x1,y1],[x2,y2],[x3,y3]}到航迹点{uav1_p1,uav2_p1,uav3_p1}以及沿最小转弯半径从航迹点p1曲线飞行到航迹点{uav1_p2,uav2_p2,uav3_p2}的路程length1、length2、length3，其中length1的公式如下：

length2、length3的计算方法与length1相同。

对三架无人机路程的约束条件为三架无人机所飞行的路程长度一样，假设距离固定目标最近的无人机只能绕最小转弯圆c1飞行，那么设它在到达航迹点uav1_p1时，还需要继续绕n圈，而其余两架无人机可以绕任意半径大于最小转弯半径的圆c4、c5，并且在他们绕完这个大转弯圆后进入轨迹圆。

设置一个循环语句，循环条件为另两架无人机的转弯圆半径rc2、rc3不小于最小转弯半径r，每次循环距离固定目标最近的无人机所绕最小转弯圆加1，然后计算另外两架无人机的转弯圆半径r以及圆心位置。rc2,、rc3计算公式如下：

无人机从初始位置进入轨迹圆所花费的飞行时间为：

time＝(length2+2·πrc2)/v(12)

无人机从初始位置到进入轨迹圆的总路程为：

length＝length2+2·πrc2(13)

至此，航迹规划结束。