CN110262543B - 多目标点同时到达约束下的集群四维轨迹规划设计方法 - Google Patents

Abstract

本发明公开一种多目标点同时到达约束下的集群四维轨迹规划设计方法，包括如下步骤：步骤1：用传统的匈牙利算法来完成任务分配的优化；步骤2：安全轨迹规划，首先通过定义两架无人机的防撞危险关系和无人机航迹上的危险区段，提出无人机防撞危险区边界点的方法；之后结合最小分离距离法提出基于贪心思想的集群防撞方法来实现无人机的速度分配。本发明方法解决了飞行器跟踪轨迹复杂性的问题，优化了飞行器的任务分配，将飞行器集群执行任务所耗能量降到最低。

Description

多目标点同时到达约束下的集群四维轨迹规划设计方法

技术领域

本发明涉及一种多目标点同时到达约束下的集群四维轨迹规划设计方法，尤其涉及一种无人机集群同时到达多目标点的时空四维轨迹设计方法。该设计方法主要分任务分配优化和安全轨迹规划两步。任务分配的优化使得无人机群能以最小能量消耗同时到达目标点，安全轨迹的规划确保无人飞行器在飞行中互不相撞，设计方法可应用于无人机，导弹等任意无人飞行器。本发明属于航迹规划领域。

背景技术

随着信息化时代的发展，集群轨迹任务规划在多个方面有着广泛的应用，多飞行器协同飞行任务技术也日渐成熟。飞行器集群飞行任务之一是同时到达多目标点。具体任务可能是多发导弹同时摧毁目标，或多架无人机同时到达战场指定位置进行侦察、攻击等。

目前已有许多算法可求解多无人机目标分配的规划方案，如带有***进化策略的蚁群算法、分解式规划算法、扩展Voronoi图等。使飞行器同时到达目标点的算法大多是等长轨迹规划法及其衍生算法，即在飞行器的力学特性约束下，将产生的路径进行平滑处理，并假设集群飞行器都以相同的速度匀速飞行，并基于将飞行轨迹看成可伸展的弹簧连接起始点与目标点的链条的思想，规划设计出等长路径，由此满足同时到达的约束和路径的动态优化，衍生算法包括平滑算法与路径缩短算法。然而实际情况中，各飞行器起始点到目标点的距离一般不同，按照等长飞行轨迹的规划方法，设计出的飞行轨迹将是曲线，这增加了飞行器跟踪轨迹的复杂性，限制了飞行器执行任务的时间，更使得飞行器的能量消耗增加。

发明内容

本发明针对现有的无人机多目标任务轨迹规划存在的难点和不足，提出了一种多目标点同时到达约束下的集群四维轨迹规划设计方法，以解决飞行器跟踪轨迹复杂性的问题，优化飞行器的任务分配，将飞行器集群执行任务所耗能量降到最低。该设计方法从全局轨迹规划角度出发，用匈牙利算法完成飞行轨迹任务分配的优化，用最小分离距离法完成了防撞算法的理论推导与实现，最终给出不同无人机在不同时刻下的三维期望航路点。

本发明设计方法是基于以下几点假设完成的：

1、所有无人机从各自起始点位置飞向目标点位置的飞行轨迹是直线。

2、每组目标点群的目标点数和无人机数量一致。

3、所有目标点的位置都是固定的，其位置坐标不随时间变化而变化。

4、无人机在不同段航迹的速度大小是突变的，即不考虑无人机变速的过渡时间。

5、将无人机看作质点模型。

本发明方法具体步骤如下：

步骤1：任务分配优化

在无人机同时到达多目标点的任务中，为各无人机分配不同的飞行任务目标是首要步骤。为使所有无人机能量消耗尽可能的少，执行任务的总代价尽可能的低，我们需将无人机飞行轨迹长度总和尽可能最小。这个问题类似于运筹学领域中的指派问题，其本质上可抽象为给定二分图的最小匹配问题。并且由之前的假设可知，此问题是一个目标平衡分配问题，因此我们用传统的匈牙利算法来完成任务分配的优化；具体如下：

S11、将无人机出发点和目标点的距离关系矩阵进行行规约和列规约的线性变换；

S12、找出变换后的距离矩阵的独立0元素，统计独立0元素的个数；

S13、划最少的线覆盖所有的独立0元素；

S14、根据独立0元素的个数进行试指派，得到有与无人机数量一致的独立0元素的优化矩阵，独立0元素的行列编号便是优化后的任务分配结果。

步骤2：安全轨迹规划

步骤1的任务分配能够确定每架无人机飞行的起始点和目标点。步骤2是对无人机的速度进行分配，在时间维度上规划每架无人机的期望航路点，从而避免各无人机在飞行中相撞，并且满足同时到达目标点的约束条件。首先我们通过定义两架无人机的防撞危险关系和无人机航迹上的危险区段，提出无人机防撞危险区边界点的方法；之后我们结合最小分离距离法提出基于贪心思想的集群防撞方法来实现无人机的速度分配。

所述集群防撞方法的输入是无人机飞行参考速度、无人机相撞的危险阈值距离、及各无人机飞行轨迹的起止点坐标，输出的是各无人机在各自飞行轨迹上的速度分配结果。所述集群防撞方法的核心思想是优先分配航迹较长的无人机的速度，使得每两架无人机之间都能有足够时间差飞离各自的危险区。

输入给定的时刻，结合集群防撞方法输出的速度分配结果，即可规划出无人机四维轨迹上的安全期望航路点。

其中，所述的无人机防撞危险区边界点的方法，能够实现两架无人机危险关系的判断和危险区边界点位置的确定，具体步骤如下：

Step1计算两架无人机的轨迹直线方向向量和参考速度向量；

Step2计算出两架无人机轨迹线间的空间最短距离及相应轨迹上的最短点位置；

Step3进行危险关系判断

若两架无人机间存在危险关系，则进行Step4；

若两架无人机间不存在危险关系，不存在危险区，算法结束；

Step4进行危险区临界点验证计算

验证计算若两架无人机各自轨迹上靠近起点方向的任意点之间的距离等于危险阈值距离，且各自轨迹上靠近终点方向的任意点之间的距离等于危险阈值距离，则直接将此时各自轨迹上选取的任意点位置输出为危险区临界点位置；

若两架无人机各自轨迹上靠近起点方向的任意点之间的距离小于危险阈值距离，或各自轨迹上靠近终点方向的任意点之间的距离小于危险阈值距离，则执行Step5；

Step5更新危险区临界点位置

根据输入的迭代时间步长和无人机参考速度向量来更新危险区临界点位置，位置更新一步后，执行Step4。

其中，所述的集群防撞算法，具体步骤如下：

Step1计算各架无人机航迹长度；

Step2对各架无人机航迹长度按从长到短进行排序；

Step3定参考无人机速度并对算法参数进行初始化；

Step4判断无人机i与无人机j的危险关系；

Step5对无人机i进行速度分配；

Step6更新无人机i编号等参数，对下一架无人机进行速度分配，最终输出速度及相应速度飞行时间分配结果。

做为优选的，本发明方法进一步包括：步骤3：轨迹修正设计

为使无人机能够较好地跟踪已规划好的期望航路点，步骤3将对步骤2规划出的期望航路点再进行修正设计。具体的修正设计和无人机自身的控制器有关。修正设计后的期望轨迹航路点可直接输入进无人机控制器。

所述的修正设计主要分为两步，第一步是先将未修正的期望航路点作为指令信号输入进无人机***中，根据测得的无人机实际的响应位置来计算无人机的跟踪延时误差，并估算出无人机***的近似延迟时间常数。第二步是根据延迟时间常数对规划好的轨迹航路点进行相应的时间超前修正，使得无人机***的跟踪延时误差降低。

本发明的一种多目标点同时到达约束下的集群四维轨迹规划设计方法，其优点和功效在于：解决了飞行器跟踪轨迹复杂性的问题，优化了飞行器的任务分配，将飞行器集群执行任务所耗能量降到最低。

附图说明

图1a是5架无人机所有可能的任务轨迹图，图1b是5架无人机优化任务分配后的轨迹图，优化后的轨迹已加粗。图1c是任务分配优化的实现框图

图2是两架无人机轨迹上可能相撞的危险区示意图。

图3是防撞算法流程图

图4a，图4b，图4c和图4d分别是四维轨迹规划的步骤示例图

图5是四维轨迹规划设计的流程图

图6是仿真初始化图

图7是匈牙利算法正确性的验证仿真图

图8a是匈牙利算法进行任务分配的仿真结果图，图8b，图8c，图8d是随机任务分配的仿真结果图。

图9是防撞算法仿真结果图

图10是无人机飞行任务中无人机数量与无人机之间最小距离关系图。

图11是四维轨迹规划设计仿真图。

图12a、b是轨迹修正仿真结果对比图。

图13a、b是跟踪距离误差对比图。

具体实施方式

以下结合附图及具体实例，对本发明的技术方案做进一步的说明。

步骤1：任务分配优化

在无人机同时到达多目标点的任务中，为各无人机分配不同的飞行任务目标是首要步骤。为使所有无人机能量消耗尽可能的少，执行任务总代价尽可能的少，我们需做出最优任务分配使得无人机路径总和最短。这个问题类似于运筹学领域中的指派问题，其本质上可抽象为给定二分图的最小匹配问题。由前面的假设可知，此问题是一个目标平衡分配问题，因此我们用传统的匈牙利算法来完成任务分配的优化。

匈牙利算法是通过引用匈牙利数学家康尼格的矩阵中独立0元素数量关系定理，来有效解决一对一的优化指派问题。

我们结合一个例子来说明匈牙利算法解决无人机任务分配优化问题的步骤。

如图1a所示，现有5架无人机须从各自起点(编号1-5)同时到达目标点(编号1’-5’)。其优化任务分配的步骤如下。

S11、计算各起始点与终止点之间的距离。构成的距离矩阵如下表1.

表1

S12、进行行规约变换，即各行减去其该行最小数，得到行归约后的距离阵如表2.

表2

S13、进行列规约变换，即各列减去其该列最小数，得到列归约后的距离阵如表3.

表3

S14、对表3矩阵进行试指派。试指派操作是根据下一步划零线的结果而定，主要有以下四步操作：

S141、遍历所有未被画线的0元素，统计该0元素所在的行列的0元素个数，选取最少个数的那个0元素，对其行列进行标记。

S142、标记的行列中未被画线的0元素称为独立0元素。在此0元素的行列上画线。

S143、重复步骤S141、S142直到没有线可以画(先不看被线画掉的元素)。

S144、统计步骤S142中找到的0元素个数n，若n＝5则成功，否则失败。

S15、划盖零线，划最少的直线数覆盖所有0元素，直线数就是独立0元素的个数。

S16、更新矩阵，操作分以下三步：

S161、对没有被线划到的数中，找到最小的数。

S162、对没有被线划到的数中，减去最小的数。

S164、对被2条线划到的数中，加上最小的数。

表3的距离矩阵经过步骤S12、S13、S14和S15后各数值变成更新后的距离矩阵如表4所示：

表4

S17、重复步骤S12、S13、S14和S15，直至独立0元素个数n＝5。独立0元素对应的行列序号即是一架无人机的终止和起始点编号。若用1代替独立0元素，其他数均置为0，则该算法最后得到的优化分配矩阵如表5所示：

表5

这表明，最优分配是，第1架无人机应从1点飞到5’点，第2架从2点飞到1’点，第3架从3点飞到2’点，第4架从4点飞到4’点，第5架从5点飞到3’点。优化轨迹已经加粗显示在图1b中。

可见优化后的轨迹最短距离和为4+7+6+6+9＝32。

图1c即为匈牙利算法任务分配优化的实现框图

步骤2：安全轨迹规划

步骤1的任务分配能够确定每架无人机飞行的起始点和目标点。步骤2是对无人机的速度进行分配，在时间维度上规划每架无人机的期望航路点，从而避免各无人机在飞行中相撞，并且满足同时到达目标点的约束。

现将多目标点三维空间中分为两组点，它们相对于原点的空间向量为：

{p_I,1,…,p_I,M},{p_E,1,…,p_E,M} (1)

这里

设

表示无人机i在t时刻的三维坐标，并设M架无人机在0时刻同时出发，其中无人机i初始位置p_U,i(0)和速度v_U,i(0)满足

p_U,i(0)＝p_I,i,v_U,i(0)＝0,(i＝1,2,...,M) (2)

在T_f时刻所有无人机同时到达目标点，它们的位置和速度为

p_U,i(T_f)＝p_E,σ(i),v_U,i(T_f)＝0 (3)

其中：σ:{1,…,M}→{1,…,M}是个一一映射。

设l_i＝||p_E,σ(i)-p_I,i||为第i架无人机的航迹长度。有如下关系：

其中称无人机a为参考无人机，v_r为无人机参考速度大小，T_f为任务参考时间。

定义两架无人机间的危险阈值距离为d_th,d_i,j(t)＝||p_U,i(t)-p_U,j(t)||为t时刻无人机i和无人机j之间的距离。

若存在t^*时刻满足

d_i,j(t^*)＜d_th,t^*∈[0,T_f] (6)

则认为无人机i和无人机j在t时刻处在危险区内，它们具有危险关系，具体关系表示如图2。

若要使两架无人机能够避开各自的危险区，我们需先找到它们各自危险区的边界点位置。为此我们提出一种危险区边界点算法，该算法能够实现两架无人机危险关系的判断和危险区边界点位置的确定。

算法1：危险区边界点算法

输入：无人机i轨迹的起始点和终止点坐标{p_I,i,p_E,σ(i)}，无人机j轨迹的起始点和终止点坐标{p_I,j,p_E,σ(j)}，迭代时间步长Δt，危险阈值距离d_th

输出：无人机i和无人机j是否有危险关系，它们各自轨迹上的危险区边界点位置{p_ij(start),p_ij(end)},{p_ji(start),p_ji(end)}

算法流程伪代码：

Step1计算两架无人机的轨迹直线方向向量和参考速度向量。

Step2计算出两架无人机轨迹线间的空间最短距离及相应轨迹上的最短点位置。

设p_i(m)为无人机i轨迹上的动点，p_j(n)为无人机j轨迹上的动点。其中参数m,n描述了该动点瞬时位置相对于起始点位置的位移大小，即：

轨迹间的最短空间距离为：

其中

分别为无人机i轨迹和无人机j轨迹上的最短点位置。

的求解分以下几种情况：

情况1、若向量l_i,l_j共面，两条航迹线段所在直线相交，则设交点为p_ij

a若p_ij在无人机i和无人机j轨迹上,则

b若p_ij不在无人机i和无人机j轨迹上,

则

情况2、若向量l_i,l_j异面，则设n_ij为l_i,l_j的法向量。

用向量法求出两条航迹线异面距离

计算出异面距离线段两个端点位置分别为p_ij′,p_ji′

a若p_ij′在无人机i轨迹上且p_ji′在无人机j轨迹上,则

b若p_ij′不在无人机i轨迹上或p_ji′不在无人机j轨迹上,则

Step3进行危险关系判断

若d_min(i,j)≤d_th则输出无人机i和无人机j存在危险关系，并令

之后进行Step4；

若d_min(i,j)＞d_th则输出无人机i和无人机j不存在危险关系，不存在危险区，算法结束。

Step4进行危险区临界点验证计算

验证计算若有||p_ij(start)-p_ji(start)||＝d_th且||p_ij(end)-p_ji(end)||＝d_th，则直接输出危险区临界点位置

{p_ij(start),p_ij(end)},{p_ji(start),p_ji(end)}

若||p_ij(start)-p_ji(start)||＜d_th或||p_ij(end)-p_ji(end)||＜d_th，则执行Step5；

Step5更新危险区临界点位置

根据输入的迭代时间步长和无人机参考速度向量来更新危险区临界点位置

位置更新一步后，执行Step4；

得到以上算法输出的结果后，再进行如下分析：

若无人机i和无人机j在0-T_f时刻内均不存在危险关系，则当它们各自的飞行速度与各自的航迹长度成比例关系时，即可满足它们同时在T时刻到达目标点的约束条件。

若无人机i和无人机j存在危险关系，则结合求出的危险区边界点位置，重新分配它们各自的速度，使得无人机i在离开危险区之后，无人机j才进入危险区。即当p_U,i(t₀)＝p_ij(start)时，p_U,j(t₀)＝p_ji(end)

设n_i表示无人机i轨迹上危险区段的数量。将上述两架无人机速度分配方法拓展到多架无人机，我们可得到如下集群防撞算法。

算法2：集群防撞算法

输入：参考速度v_r，危险阈值距离d_th，M架无人机飞行路径的起始点位置{p_I,1,…,p_I,M}和终止点位置{p_E,σ(1),…,p_E,σ(M)}

输出：M架无人机各段航线的速度及相应速度的飞行时间

算法流程伪代码：

Step1计算各架无人机航迹长度

l_i＝||p_I,i-p_E,σ(i)||,(i＝1,2,...,M)

Step2对各架无人机航迹长度按从长到短进行排序

设λ:{1,…,M}→{1,…,M}是从大到小的一一映射关系。

则序列l₁,l₂,...,l_M经从长到短排序后变为l_λ(1),l_λ(2),...,l_λ(M)

即l_λ(1)≥l_λ(2)≥,...,≥l_λ(M)

Step3定参考无人机速度并对算法参数进行初始化

由(4)式知，编号为λ(1)的无人机航迹最长，将其定为参考无人机，其速度v_λ(1)1为参考速度，飞行时间t_λ(1)1为任务时间T_f，由(5)式可得

将以下参数进行初始化

m＝2,i＝λ(m),k＝m-1,j＝λ(k)，n_i＝0,S_i＝[]

其中参数i,j,k,m均表示无人机编号循环变量，参数n_i表示无人机i轨迹上危险区段的数量，数组S_i中记录着无人机i从起点到危险区边界点的最近距离。

Step4判断无人机i与无人机j的危险关系

调用算法1判断无人机i与无人机j是否具有危险关系。

若算法1输出无危险关系则使j＝λ(k-1),n_i＝0，此时若j为0，则执行Step5,否则继续执行Step4。

若算法1输出有危险关系，则根据算法1输出的危险区边界点，计算无人机i和无人机j起始点到危险区边界点的最近距离l_ij,l_ji并更新参数n_i,S_i,j

l_ij＝||p_ij(end)-p_I,i||,l_ji＝||p_ji(start)-p_I,j||,n_i＝n_i+1,S_i＝S_i∪l_ij,j＝λ(k-1)

将无人机i从起点出发，到与无人机j关联的危险区边界点所需时间记为τ_ij，

此时若j为0，则执行Step5,否则继续执行Step4。

Step5对无人机i进行速度分配

若n_i＝0，则将无人机i各段航线的速度及相应速度的飞行时间分配如下：

若n_i≠0，将数组S_i进行从小到大排序。

记排序的一一映射关系为κ:{1,…,M}→{1,…,M}，即排序后结果为

将无人机j各段航线的速度及相应速度的飞行时间分配如下：

......

Step6更新无人机i编号等参数，对下一架无人机进行速度分配

m＝m+1,i＝λ(m),k＝m-1,j＝λ(k)

若i≤M，则执行Step4，否则输出速度及相应速度飞行时间分配结果，算法结束。

整个算法2流程框图如图3所示，得到算法2输出的速度分配结果后，输入给定的时刻，再根据以下步骤，即可规划出无人机四维轨迹上的安全期望航路点。

1、对无人机i，在其0到T_f的飞行时间轴上标注变速时间节点。设

由算法2的输出可知，无人机i的变速时刻如下：

2、给定任意时刻t,找出k满足

τ_ki≤t≤τ_(k+1)i,k＝0,1,2,...,n_i

3、设l_ik(t)为无人机i在t时刻相对于离它最近的危险区临界点的位移矢量。

则无人机i在t时刻的期望位置：

图4a到图4d为5架无人机先后同时飞抵3组目标点群的轨迹规划步骤示意图(图中仅示意性的标注了1号和3号无人机轨迹规划步骤)。

步骤1和步骤2的整体框图如图5所示。

步骤3：轨迹修正设计

实际中无人机不可能零误差地完全跟踪上期望轨迹。设p_U,i(t)为无人机的实际位置，由步骤1和步骤2的规划设计知，无人机的期望位置为

我们希望p_U,i(t)与

的误差尽可能的小，其收敛速度尽可能地快。

为使执行任务的无人机在跟踪规划轨迹上的性能更好，即使无人机输出的实际位置p_U,i(t)更好地跟踪上规划期望位置

我们将已规划好的轨迹航路点修正如下

其中，ε是无人机***里近似惯性环节的时间常数的倒数，其数值通过以下两步测试方法得到：

1、将未修正规划的期望位置

作为指令信号输入进无人机控制器，记录无人机作出响应的实际位置p_U,i(t)。

2、对测试输出的实际位置与输入的期望位置数据进行对比，将输入和输出的关系近似为惯性环节，用输入和输出的数据计算出近似惯性环节下的ε

微调ε参数可使得跟踪性能进一步上升。具体调整与无人机跟踪控制器有关。我们用PID无人机跟踪控制器为例进行仿真，仿真表明当调整参数ε＝0.025时，

修正后的轨迹航路点

使得

无人机跟踪性能最佳。

实例仿真

1仿真设置

设定无人机数量M为10架，参考速度v_r为5m/s，危险阈值距离为4m，这里仅设两组目标点群为例。各无人机起始点和目标点的坐标均匀随机生成，将其可视化如图6，图中星号点为起始点，方块点为第一组目标点，圆形点为第二组目标点。

2任务分配仿真

首先对匈牙利算法进行验证，如图7所示，星号为起始点，方块为第一组目标点，圆形为第二组目标点。图7表明了匈牙利算法的正确性。

接着利用匈牙利算法对仿真设置里的每架无人机的任务进行分配。由于有两组目标点群，所以需将匈牙利算法应用两次进行匹配。匈牙利匹配结果如图8a所示，为与对匈牙利算法匹配进行对比，图8b,图8c和图8d是任意匹配的结果。

先找出最长轨迹，代入参考速度值，对任务时间进行计算。

对图8a匹配进行计算知，到达第一组目标点的最长轨迹是从出发点0到第一组目标点4，到达第二组目标点的最长方块轨迹是从第一组目标点5到第二组目标点9。仿真测得起始1点坐标为(-15,0,0),第一组目标5点坐标为(6.51,-11.53,17),第二组目标9点坐标为(6.93,-4.98,34.8)。

所以从出发点到第一组目标点之间最长轨迹长度为s₁＝29.5523m,第一组目标点与第二组目标点之间最长轨迹长度为s₂＝23.1101m

由此可以得到无人机总的任务时间为：

图8a，图8b,图8c和图8d的各自轨迹长度总和，最长轨迹长度和总任务时间计算如下表6：

表6

表格的对比数据说明应用匈牙利算法的任务分配是成功的。

3防撞算法仿真

这里以其中两架无人机轨迹线为例，两架无人机的轨迹如图9所示0-4轨迹线和2-4轨迹线交点坐标A为(-1.9009,-6.7201,10.3574)。4-8轨迹线最小距离点B坐标为(0.82873147,-8.47258373,24.52429909)，9-4轨迹线最小距离点C为(2.52501745,-8.14613575,25.69413467)。

由算法1计算得出的各航迹危险临界点坐标为

A1(-4.0509,-5.6171,8.6574),A2(-3.3404,-7.3472,8.3774),

B0(1.7592,-8.8914,23.2919),C0(3.1651,-7.2669,24.5206)；

将无人机1的速度定为参考速度v_r＝5m/s，设无人机2的速度及相应速度飞行时间为{(v₂₁,t₂₁),(v₂₂,t₂₂),(v₂₃,t₂₃),(v₂₄,t₂₄)}

由算法2输出的无人机2的速度及相应速度飞行时间结果为：

{(3.6440,2.16620),(5.1818,3.74426),(3.9394,1.53329),(3.8688,3.08873)}

图10验证了无人机飞行任务中各时刻与无人机之间最小危险距离的关系。

4轨迹修正仿真

有上述防撞算法可以得到各架无人机具体的速度变化情况。下图绘制了不同时间节点时无人机的位置，同时绘制了每一节点时间段的中间时刻无人机的位置。

仿真结果如图11所示。

选取以上仿真规划好的编号0-4，4-8轨迹进行跟踪仿真，在整个规划好的轨迹上采样2000个期望航路点。按照

(ε为与无人机控制器有关的小量参数)进行规划轨迹修正。设置仿真无人机的近似一阶常数ε＝0.025，跟踪仿真结果如下图12a、b所示。(图12a为未修正的规划轨迹跟踪效果，图12b为修正后的规划轨迹的跟踪效果)

图13a、b为每个采样航路点的跟踪误差距离结果图(图13a为未修正的规划轨迹误差结果，图13b为修正后的规划轨迹的误差结果)；

仿真结果表明，修正后的规划轨迹使得无人机跟踪期望的轨迹效果更好。

Claims (2)

1.一种多目标点同时到达约束下的集群四维轨迹规划设计方法，是基于以下几点假设完成的：

①、所有无人机从各自起始点位置飞向目标点位置的飞行轨迹是直线；

②、每组目标点群的目标点数和无人机数量一致；

③、所有目标点的位置都是固定的，其位置坐标不随时间变化而变化；

④、无人机在不同段航迹的速度大小是突变的，即不考虑无人机变速的过渡时间；

⑤、将无人机看作质点模型；

其特征在于：该方法包括如下步骤：

步骤1：任务分配优化

用传统的匈牙利算法来完成任务分配的优化；具体如下：

S11、将无人机出发点和目标点的距离关系矩阵进行行规约和列规约的线性变换；

S12、找出变换后的距离矩阵的独立0元素，统计独立0元素的个数；

S13、划最少的线覆盖所有的独立0元素；

S14、根据独立0元素的个数进行试指派，得到有与无人机数量一致的独立0元素的优化矩阵，独立0元素的行列编号便是优化后的任务分配结果；

步骤2：安全轨迹规划

首先通过定义两架无人机的防撞危险关系和无人机航迹上的危险区段，提出无人机防撞危险区边界点的方法；之后结合最小分离距离法提出基于贪心思想的集群防撞方法来实现无人机的速度分配；所述集群防撞方法的输入是无人机飞行参考速度、无人机相撞的危险阈值距离、及各无人机飞行轨迹的起止点坐标，输出的是各无人机在各自飞行轨迹上的速度分配结果；

输入给定的时刻，结合集群防撞方法输出的速度分配结果，即可规划出无人机四维轨迹上的安全期望航路点；

所述的无人机防撞危险区边界点的方法，能够实现两架无人机危险关系的判断和危险区边界点位置的确定，具体步骤如下：

Step1计算两架无人机的轨迹直线方向向量和参考速度向量；

Step2计算出两架无人机轨迹线间的空间最短距离及相应轨迹上的最短点位置；

Step3进行危险关系判断

若两架无人机间存在危险关系，则进行Step4；

若两架无人机间不存在危险关系，不存在危险区，算法结束；

Step4进行危险区临界点验证计算

验证计算若两架无人机各自轨迹上靠近起点方向的任意点之间的距离等于危险阈值距离，且各自轨迹上靠近终点方向的任意点之间的距离等于危险阈值距离，则直接将此时各自轨迹上选取的任意点位置输出为危险区临界点位置；

若两架无人机各自轨迹上靠近起点方向的任意点之间的距离小于危险阈值距离，或各自轨迹上靠近终点方向的任意点之间的距离小于危险阈值距离，则执行Step5；

Step5更新危险区临界点位置

根据输入的迭代时间步长和无人机参考速度向量来更新危险区临界点位置，位置更新一步后，执行Step4；

所述的集群防撞算法，具体步骤如下：

Step1计算各架无人机航迹长度；

Step2对各架无人机航迹长度按从长到短进行排序；

Step3设定参考无人机速度并对算法参数进行初始化；

Step4判断无人机i与无人机j的危险关系；

Step5对无人机i进行速度分配；

Step6更新无人机i编号等参数，对下一架无人机进行速度分配，最终输出速度及相应速度飞行时间分配结果。

2.根据权利要求1所述的多目标点同时到达约束下的集群四维轨迹规划设计方法，其特征在于：进一步包括：步骤3：轨迹修正设计；

所述的修正设计主要分为两步，第一步是先将未修正的期望航路点作为指令信号输入进无人机***中，根据测得的无人机实际的响应位置来计算无人机的跟踪延时误差，并估算出无人机***的近似延迟时间常数；第二步是根据延迟时间常数对规划好的轨迹航路点进行相应的时间超前修正，使得无人机***的跟踪延时误差降低。

Images