[发明专利]一种基于航路点的无人机编队生成-切换航迹规划方法

权利要求书

1.一种基于航路点的无人机编队生成-切换航迹规划方法，其特征在于步骤如下：

步骤1：将起始点和编队样式点作为输入；所述的编队样式点包括以下五个基本参数：

1)方位角α：指相对于起飞点或上一个编队样式点的位置方向，方位角α定义为与Y轴正方向的夹角，偏向X轴正方向为正；

2)距离L：表示编队中心点在编队变换或者转弯前后沿方位角方向前进的距离长度；

3)队形：表示编队飞行过程中队形库；

4)队形间隔Δx：表示编队队形垂直于速度方向上的无人机相对位置间隔；

5)队形间隔Δy：表示编队队形沿速度方向上的无人机相对位置间隔；

根据每个编队样式点生成该编队航迹，编队航迹包括编队集结段、编队切换段和编队转弯段；

步骤2：无人机编队从起飞点按照一定时间间隔一次起飞，到作战样式点S1形成第一个队形飞行，此过程为编队集结段，作战样式点S1对应的即为编队集结点；

步骤2.1：编队集结点坐标计算

采用下式计算编队集结段：

其中，为无人机的起飞点坐标，α₁为第一个编队样式点的方位角、L₁为编队样式点距离，为编队样式点；

根据S₁的位置、队形样式及其参数Δx、Δy确定集结完成时各无人机的位置P_it(x_it,y_it)；P_it(x_it,y_it)坐标的计算方法如下：

首先根据集结时的方位角α₁，进行坐标旋转，使得S_launch-S₁段与坐标转换后的Y轴平行，新的坐标系看作将坐标系顺时针旋转-α₁后得到，对应的坐标转换关系为：

因此在转换后的OX’Y’坐标系下的坐标为：

根据的位置，无人机i在OX’Y’下的编队形成点P′_it(x′_it,y′_it)坐标为：

其中Δx_i和Δy_i为S₁样式点下无人机i相对于编队中心点的坐标；

将OX’Y’下的编队形成点P′_it(x′_it,y′_it)进行坐标逆旋转，即可得到初始OXY下无人机i的编队形成位置P_it(x_it,y_it)：

P_it(x_it,y_it)＝P_it(-x′_it×sinα₁+y′_it×cosα₁,x′_it×cosα₁+y′_it×sinα₁)

依次可以生成编队内所有无人机的编队形成位置；

步骤2.2：不考虑时间到达约束的无人机集结航迹计算

将无人机发射点位置转换到OX’Y’坐标下的坐标为根据和P′_it(x′_it,y′_it)计算编队集结航迹点；

步骤2.3：考虑时间到达约束的无人机集结航迹调整

由航迹点之间的欧拉距离计算得出航迹长度，将航迹长度相加得到规划总航迹长度C_i，根据无人机的速度V_i，计算得到无人机i的不考虑时间约束下的编队集结飞行时间为：

以第一架无人机起飞时刻为0时刻，则无人机i按照上述航迹完成编队集结的时刻：

t_i＝t_if+Δt_i

其中Δt_i为无人机i与第一架无人机之间的发射时间间隔；

在协同到达时间约束下，无人机编队需要同时以S₁点为中心形成集结队形，因此无人机在不同时刻出发，需要等时刻到达无人机的集结位置点P′_it(x′_it,y′_it)；因此取编队内无人机到达集结位置点的最大时刻为时间基准，编队内任意无人机都在此时刻到达各自的集结位置点P′_it，无人机i的预估到达时刻与基准时刻t_max的时间差t_igap为：

t_igap＝t_max-t_i

每架无人机根据各自的期望时间差t_igap做航迹调整以保证时间协同完成编队集结任务：

对无人机航迹进行逆变化，设P′_ij(x′_ij,y′_ij)为无人机i在OX’Y’坐标系下规划得到的第j个航迹点，则P′_ij(x′_ij,y′_ij)对应的OXY下的航迹点P_ij(x_ij,y_ij)为：

P_ij(x_ij,y_ij)＝P_ij(-x′_ij×sinα₁+y′_ij×cosα₁,x′_ij×cosα₁+y′_ij×sinα₁)

对所有航迹点进行坐标逆变换后得到无人机的集结航迹点；

步骤3：从第二个编队样式点开始遍历编队样式点，根据当前队形样式点S_k参数和上一个编队样式点S_k-1参数的不同判断无人机编队做队形切换动作或是转弯动作，当队形、队形间隔Δx和队形间隔Δy中任一参数发生改变时，判断此时编队为队形切换动作，进入到步骤4；

若队形、队形间隔Δx和队形间隔Δy参数均保持不变，方位角α和距离L参数发生改变时，判断此时编队为编队转弯动作，进入到步骤5；

步骤4：无人机编队队形切换，根据当前队形样式点S_k参数和上一个编队样式点S_k-1的参数按照编队切换方案完成编队切换过程中航迹点的设计与计算：

步骤4.1：编队切换完成点坐标计算

对于样式点S_k编队切换段，将上一样式点S_k-1段规划得到的最后一个航迹点看作该阶段规划的起点，根据上一阶段计算的S_k-1的位置、样式点S_k的方位角α_k和距离L_k按照下式计算得到样式点S_k的位置；

根据S_k的位置、队形样式及其参数Δx_k、Δy_k确定编队切换完成时各无人机的位置P_it(x_it,y_it)；P_it坐标的计算方法如下：

首先根据编队切换S_k的方位角α₁，进行坐标旋转，使得S_k-1-S_k段与坐标转换后的Y_k'轴平行，新的坐标系可以看作将坐标系顺时针旋转-α_k后得到，对应的坐标转换关系为：

在OX_k’Y_k’坐标系下S_k的坐标为：

根据的位置，无人机i在OX’Y’下的编队形成点P′_it(x′_it,y′_it)坐标为：

其中Δx_i和Δy_i为S_k样式点下无人机i相对于编队中心点的坐标；

将OX_k’Y_k’下的编队形成点P′_it(x′_it,y′_it)进行坐标逆旋转，即可得到初始OXY下无人机i的编队形成位置P_it(x_it,y_it)：

P_it(x_it,y_it)＝P_it(-x′_it×sinα_k+y′_it×cosα_k,x′_it×cosα_k+y′_it×sinα_k)

对编队中每架无人机均采用以上方法求得编队切换完成后无人机坐标；

步骤4.2：不考虑时间到达约束的无人机编队切换航迹计算

无人机集群编队无人机起始位置为S_k-1样式生成的最后一个航迹点，以无人机i为例，其编队切换起始航迹点为且编队切换形成位置按照步骤4.1进行计算为P_it(x_it,y_it)；编队切换前的初始速度为V_start，方向与S_k-1的方位角α_k-1相同，编队切换后的速度为V_end，方向与S_k的方位角α_k相同，对应的相对速度角为：

β＝α_k-α_k-1

无人机在不考虑时间到达约束下的编队切换航迹计算方法如下：

首先将坐标系OXY沿无人机初始速度V_start方向α_k-1进行坐标旋转形成新坐标系OX_k-1’Y_k-1’，使得无人机初始速度方向沿着OY_k-1’方向，即沿着S_k-1方位角α_k-1进行坐标旋转，对应的坐标转换关系为：

将无人机i编队切换起始航迹点为和编队切换形成位置点P_it(x_it,y_it)按照上式进行坐标转换后得到坐标为和P′_it(x′_it,y′_it)；无人机i的切换起始点点坐标对应编队切换方案中编队起始点P0(x_i0,y_i0)，集结完成位置P′_it(x′_it,y′_it)对应编队切换方案中编队生成点Pt(x_it,y_it)，根据β的大小，分几种情况进行无人机编队切换航迹的设计；根据β的大小分情况生成编队内所有无人机的编队切换段航迹，即生成在满足转弯半径约束下的由编队切换起始航迹点坐标指向每架无人机对应的编队切换完成位置P′_it(x′_it,y′_it)的集结段航迹；

步骤4.3：考虑时间到达约束的无人机切换航迹调整

对步骤4.2计算得到的每架无人机的航迹，分情况计算得到每架无人机规划得到的航迹总长度C_i，根据无人机的速度V_i，计算得到无人机i的不考虑时间约束下的编队切换飞行时间：

无人机i的预估到达时刻与基准时刻t_max的时间差t_igap为：

t_igap＝t_max-t_i

根据β角的大小，进行协同到达时间约束下无人机编队切换航迹调整的设计；将无人机的航迹进行坐标逆变换，得到坐标系OXY下的结果，对无人机航迹进行逆变化，设P′_ij(x′_ij,y′_ij)为无人机i在OX_k-1’Y_k-1’坐标系下规划得到的第j个航迹点，则P′_ij(x′_ij,y′_ij)对应的OXY下的航迹点P_ij(x_ij,y_ij)为：

P_ij(x_ij,y_ij)＝P_ij(-x′_ij×sinα_k-1+y′_ij×cosα_k-1,x′_ij×cosα_k-1+y′_ij×sinα_k-1)

对所有航迹点进行坐标逆变换后得到无人机的编队切换航迹点；

步骤5：无人机编队转弯，无人机集群编队转弯过程中初始位置为S_k-1样式生成的最后一个航迹点，以无人机i为例，其编队转弯初始航迹点为编队转弯前的初始速度为V_start，方向与S_k-1的方位角α_k-1相同，编队转弯后的速度为V_end，方向与S_k的方位角α_k相同，对应的相对速度角为：

β＝α_k-α_k-1

无人机的编队转弯航迹计算方法如下：

首先将坐标系OXY沿无人机初始速度V_start方向α_k-1进行坐标旋转形成新坐标系OX_k-1’Y_k-1’，使得无人机初始速度方向沿着OY_k-1’方向，即沿着S_k-1方位角α_k-1进行坐标旋转，对应的坐标转换关系为：

将无人机i编队切换起始航迹点为按照上式进行坐标转换后得到坐标为无人机i的切换起始点点坐标对应编队切换方案中编队起始点按照S_k的队形参数Δx_k和Δy_k分别计算得到L₁和L₂的取值范围，取L₁和L₂分别为其取值范围的下限；如果L₂小于编队样式点S_k距离L_k，则L₂＝L_k；

其中Δy_ij为无人机i、j沿速度方向的间隔，Δx_ij为垂直于速度方向上的编队间隔；

根据L₁和L₂的大小计算得到无人机i的平行于OY_k-1’轴的第①段航迹长度

再计算得到无人机i沿β方向的第②段航迹长度

根据无人机编队转弯前初始位置和无人机编队转弯的两段航迹长度计算得到编队内无人机M_j的转弯关键点和编队生成坐标P_t^j：

和P_t^j(x_jt,y_jt)对应无人机M_j在坐标系OX_k-1’Y_k-1’下生成的航迹点P′_j1(x'_j1,y'_j1)和P′_j2(x'_j2,y'_j2)；

此时无人机的航迹均在转换后的坐标系OX_k-1’Y_k-1’下进行设计，进行坐标逆变换，得到坐标系OXY下的结果，按照下式对无人机航迹进行逆变化，设P′_ij(x′_ij,y′_ij)为无人机i在OX_k-1’Y_k-1’坐标系下规划得到的第j个航迹点，则P′_ij(x′_ij,y′_ij)对应的OXY下的航迹点P_ij(x_ij,y_ij)为：

P_ij(x_ij,y_ij)＝P_ij(-x′_ij×sinα_k-1+y′_ij×cosα_k-1,x′_ij×cosα_k-1+y′_ij×sinα_k-1)

步骤6：根据步骤3的判断重复步骤4和步骤5遍历所有编队样式点即可生成无人机编队在满足无人机性能约束和时间约束下的按照编队样式点飞行的整个过程的航迹结果。