CN110262543B - 多目标点同时到达约束下的集群四维轨迹规划设计方法 - Google Patents
多目标点同时到达约束下的集群四维轨迹规划设计方法 Download PDFInfo
- Publication number
- CN110262543B CN110262543B CN201910433651.2A CN201910433651A CN110262543B CN 110262543 B CN110262543 B CN 110262543B CN 201910433651 A CN201910433651 A CN 201910433651A CN 110262543 B CN110262543 B CN 110262543B
- Authority
- CN
- China
- Prior art keywords
- unmanned aerial
- aerial vehicle
- point
- track
- speed
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 43
- 238000013461 design Methods 0.000 title claims abstract description 22
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 57
- 238000005457 optimization Methods 0.000 claims abstract description 17
- 238000000926 separation method Methods 0.000 claims abstract description 4
- 239000013598 vector Substances 0.000 claims description 15
- 239000011159 matrix material Substances 0.000 claims description 13
- 238000012937 correction Methods 0.000 claims description 11
- 230000008859 change Effects 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 4
- 230000009466 transformation Effects 0.000 claims description 4
- 238000012795 verification Methods 0.000 claims description 4
- 239000002245 particle Substances 0.000 claims description 2
- 230000004044 response Effects 0.000 claims description 2
- 230000007704 transition Effects 0.000 claims description 2
- 238000005265 energy consumption Methods 0.000 abstract description 7
- 230000002265 prevention Effects 0.000 abstract description 2
- 238000004088 simulation Methods 0.000 description 20
- 238000010586 diagram Methods 0.000 description 17
- 230000000694 effects Effects 0.000 description 4
- 238000013507 mapping Methods 0.000 description 3
- 230000009467 reduction Effects 0.000 description 3
- 238000009499 grossing Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000000052 comparative effect Effects 0.000 description 1
- 125000004122 cyclic group Chemical group 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000004904 shortening Methods 0.000 description 1
- 238000010998 test method Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Traffic Control Systems (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
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是对无人机的速度进行分配,在时间维度上规划每架无人机的期望航路点,从而避免各无人机在飞行中相撞,并且满足同时到达目标点的约束。
现将多目标点三维空间中分为两组点,它们相对于原点的空间向量为:
{pI,1,…,pI,M},{pE,1,…,pE,M} (1)
pU,i(0)=pI,i,vU,i(0)=0,(i=1,2,...,M) (2)
在Tf时刻所有无人机同时到达目标点,它们的位置和速度为
pU,i(Tf)=pE,σ(i),vU,i(Tf)=0 (3)
其中:σ:{1,…,M}→{1,…,M}是个一一映射。
设li=||pE,σ(i)-pI,i||为第i架无人机的航迹长度。有如下关系:
其中称无人机a为参考无人机,vr为无人机参考速度大小,Tf为任务参考时间。
定义两架无人机间的危险阈值距离为dth,di,j(t)=||pU,i(t)-pU,j(t)||为t时刻无人机i和无人机j之间的距离。
若存在t*时刻满足
di,j(t*)<dth,t*∈[0,Tf] (6)
则认为无人机i和无人机j在t时刻处在危险区内,它们具有危险关系,具体关系表示如图2。
若要使两架无人机能够避开各自的危险区,我们需先找到它们各自危险区的边界点位置。为此我们提出一种危险区边界点算法,该算法能够实现两架无人机危险关系的判断和危险区边界点位置的确定。
算法1:危险区边界点算法
输入:无人机i轨迹的起始点和终止点坐标{pI,i,pE,σ(i)},无人机j轨迹的起始点和终止点坐标{pI,j,pE,σ(j)},迭代时间步长Δt,危险阈值距离dth
输出:无人机i和无人机j是否有危险关系,它们各自轨迹上的危险区边界点位置{pij(start),pij(end)},{pji(start),pji(end)}
算法流程伪代码:
Step1计算两架无人机的轨迹直线方向向量和参考速度向量。
Step2计算出两架无人机轨迹线间的空间最短距离及相应轨迹上的最短点位置。
设pi(m)为无人机i轨迹上的动点,pj(n)为无人机j轨迹上的动点。其中参数m,n描述了该动点瞬时位置相对于起始点位置的位移大小,即:
轨迹间的最短空间距离为:
情况1、若向量li,lj共面,两条航迹线段所在直线相交,则设交点为pij
b若pij不在无人机i和无人机j轨迹上,
情况2、若向量li,lj异面,则设nij为li,lj的法向量。
用向量法求出两条航迹线异面距离
计算出异面距离线段两个端点位置分别为pij′,pji′
a若pij′在无人机i轨迹上且pji′在无人机j轨迹上,则
b若pij′不在无人机i轨迹上或pji′不在无人机j轨迹上,则
Step3进行危险关系判断
若dmin(i,j)≤dth则输出无人机i和无人机j存在危险关系,并令
之后进行Step4;
若dmin(i,j)>dth则输出无人机i和无人机j不存在危险关系,不存在危险区,算法结束。
Step4进行危险区临界点验证计算
验证计算若有||pij(start)-pji(start)||=dth且||pij(end)-pji(end)||=dth,则直接输出危险区临界点位置
{pij(start),pij(end)},{pji(start),pji(end)}
若||pij(start)-pji(start)||<dth或||pij(end)-pji(end)||<dth,则执行Step5;
Step5更新危险区临界点位置
根据输入的迭代时间步长和无人机参考速度向量来更新危险区临界点位置
位置更新一步后,执行Step4;
得到以上算法输出的结果后,再进行如下分析:
若无人机i和无人机j在0-Tf时刻内均不存在危险关系,则当它们各自的飞行速度与各自的航迹长度成比例关系时,即可满足它们同时在T时刻到达目标点的约束条件。
若无人机i和无人机j存在危险关系,则结合求出的危险区边界点位置,重新分配它们各自的速度,使得无人机i在离开危险区之后,无人机j才进入危险区。即当pU,i(t0)=pij(start)时,pU,j(t0)=pji(end)
设ni表示无人机i轨迹上危险区段的数量。将上述两架无人机速度分配方法拓展到多架无人机,我们可得到如下集群防撞算法。
算法2:集群防撞算法
输入:参考速度vr,危险阈值距离dth,M架无人机飞行路径的起始点位置{pI,1,…,pI,M}和终止点位置{pE,σ(1),…,pE,σ(M)}
输出:M架无人机各段航线的速度及相应速度的飞行时间
Step1计算各架无人机航迹长度
li=||pI,i-pE,σ(i)||,(i=1,2,...,M)
Step2对各架无人机航迹长度按从长到短进行排序
设λ:{1,…,M}→{1,…,M}是从大到小的一一映射关系。
则序列l1,l2,...,lM经从长到短排序后变为lλ(1),lλ(2),...,lλ(M)
即lλ(1)≥lλ(2)≥,...,≥lλ(M)
Step3定参考无人机速度并对算法参数进行初始化
由(4)式知,编号为λ(1)的无人机航迹最长,将其定为参考无人机,其速度vλ(1)1为参考速度,飞行时间tλ(1)1为任务时间Tf,由(5)式可得
将以下参数进行初始化
m=2,i=λ(m),k=m-1,j=λ(k),ni=0,Si=[]
其中参数i,j,k,m均表示无人机编号循环变量,参数ni表示无人机i轨迹上危险区段的数量,数组Si中记录着无人机i从起点到危险区边界点的最近距离。
Step4判断无人机i与无人机j的危险关系
调用算法1判断无人机i与无人机j是否具有危险关系。
若算法1输出无危险关系则使j=λ(k-1),ni=0,此时若j为0,则执行Step5,否则继续执行Step4。
若算法1输出有危险关系,则根据算法1输出的危险区边界点,计算无人机i和无人机j起始点到危险区边界点的最近距离lij,lji并更新参数ni,Si,j
lij=||pij(end)-pI,i||,lji=||pji(start)-pI,j||,ni=ni+1,Si=Si∪lij,j=λ(k-1)
将无人机i从起点出发,到与无人机j关联的危险区边界点所需时间记为τij,
此时若j为0,则执行Step5,否则继续执行Step4。
Step5对无人机i进行速度分配
若ni=0,则将无人机i各段航线的速度及相应速度的飞行时间分配如下:
若ni≠0,将数组Si进行从小到大排序。
记排序的一一映射关系为κ:{1,…,M}→{1,…,M},即排序后结果为
将无人机j各段航线的速度及相应速度的飞行时间分配如下:
......
m=m+1,i=λ(m),k=m-1,j=λ(k)
若i≤M,则执行Step4,否则输出速度及相应速度飞行时间分配结果,算法结束。
整个算法2流程框图如图3所示,得到算法2输出的速度分配结果后,输入给定的时刻,再根据以下步骤,即可规划出无人机四维轨迹上的安全期望航路点。
2、给定任意时刻t,找出k满足
τki≤t≤τ(k+1)i,k=0,1,2,...,ni
3、设lik(t)为无人机i在t时刻相对于离它最近的危险区临界点的位移矢量。
图4a到图4d为5架无人机先后同时飞抵3组目标点群的轨迹规划步骤示意图(图中仅示意性的标注了1号和3号无人机轨迹规划步骤)。
步骤1和步骤2的整体框图如图5所示。
步骤3:轨迹修正设计
其中,ε是无人机***里近似惯性环节的时间常数的倒数,其数值通过以下两步测试方法得到:
2、对测试输出的实际位置与输入的期望位置数据进行对比,将输入和输出的关系近似为惯性环节,用输入和输出的数据计算出近似惯性环节下的ε
实例仿真
1仿真设置
设定无人机数量M为10架,参考速度vr为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)。
所以从出发点到第一组目标点之间最长轨迹长度为s1=29.5523m,第一组目标点与第二组目标点之间最长轨迹长度为s2=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的速度定为参考速度vr=5m/s,设无人机2的速度及相应速度飞行时间为{(v21,t21),(v22,t22),(v23,t23),(v24,t24)}
由算法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:轨迹修正设计;
所述的修正设计主要分为两步,第一步是先将未修正的期望航路点作为指令信号输入进无人机***中,根据测得的无人机实际的响应位置来计算无人机的跟踪延时误差,并估算出无人机***的近似延迟时间常数;第二步是根据延迟时间常数对规划好的轨迹航路点进行相应的时间超前修正,使得无人机***的跟踪延时误差降低。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910433651.2A CN110262543B (zh) | 2019-05-23 | 2019-05-23 | 多目标点同时到达约束下的集群四维轨迹规划设计方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910433651.2A CN110262543B (zh) | 2019-05-23 | 2019-05-23 | 多目标点同时到达约束下的集群四维轨迹规划设计方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110262543A CN110262543A (zh) | 2019-09-20 |
CN110262543B true CN110262543B (zh) | 2020-07-21 |
Family
ID=67915135
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910433651.2A Active CN110262543B (zh) | 2019-05-23 | 2019-05-23 | 多目标点同时到达约束下的集群四维轨迹规划设计方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110262543B (zh) |
Families Citing this family (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111722639B (zh) * | 2019-03-18 | 2022-06-07 | 北京京东乾石科技有限公司 | 无人机集群的起飞控制方法、装置、***和可读介质 |
CN110794874B (zh) * | 2019-10-11 | 2022-12-30 | 东南大学 | 一种定位误差约束下的飞行器航迹快速规划方法 |
CN110618702B (zh) * | 2019-10-18 | 2022-06-24 | 中国电子科技集团公司电子科学研究院 | 一种无人机集群三维曲线路径跟踪方法、装置和存储介质 |
CN112506076B (zh) * | 2020-12-04 | 2023-03-28 | 杭州电子科技大学 | 基于飞行任务的无人机集群三维空间仿真器构建方法 |
CN112666995B (zh) * | 2020-12-15 | 2023-11-21 | 广州极飞科技股份有限公司 | 无人机的速度规划方法、装置、设备及存储介质 |
CN112817330B (zh) * | 2021-01-05 | 2023-08-08 | 北京联合大学 | 一种多无人机四维航迹协同规划方法及*** |
CN114545968B (zh) * | 2022-02-18 | 2023-09-26 | 南京理工大学 | 基于纯方位定位的无人机集群多目标跟踪轨迹规划方法 |
CN114518771B (zh) * | 2022-02-23 | 2022-09-20 | 深圳大漠大智控技术有限公司 | 一种多无人机路径规划方法、装置及相关组件 |
CN115047763B (zh) * | 2022-06-08 | 2023-10-13 | 国网安徽省电力有限公司天长市供电公司 | 一种多无人机***的最小能量控制方法 |
CN114911263B (zh) * | 2022-06-10 | 2023-05-12 | 哈尔滨工业大学 | 基于同伦法的多无人机同步到达轨迹规划方法、存储介质及设备 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101477169A (zh) * | 2009-01-16 | 2009-07-08 | 华北电力大学 | 巡检飞行机器人对电力线路的检测方法 |
CN103365299A (zh) * | 2013-08-02 | 2013-10-23 | 中国科学院自动化研究所 | 一种无人机的避障方法及其装置 |
CN104298239A (zh) * | 2014-09-29 | 2015-01-21 | 湖南大学 | 一种室内移动机器人增强地图学习路径规划方法 |
CN108319291A (zh) * | 2017-12-13 | 2018-07-24 | 中国人民解放军空军工程大学 | 一种基于安全边界分析的无人机认知防碰撞控制方法 |
CN108388270A (zh) * | 2018-03-21 | 2018-08-10 | 天津大学 | 面向安全域的集群无人机轨迹姿态协同控制方法 |
CN108733074A (zh) * | 2018-05-23 | 2018-11-02 | 南京航空航天大学 | 一种基于匈牙利算法的多无人机编队航迹规划方法 |
CN109669475A (zh) * | 2019-01-22 | 2019-04-23 | 北京航空航天大学 | 基于人工蜂群算法的多无人机三维编队重构方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8705793B2 (en) * | 2009-10-08 | 2014-04-22 | University Of Southern California | Object tracking by hierarchical association of detection responses |
-
2019
- 2019-05-23 CN CN201910433651.2A patent/CN110262543B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101477169A (zh) * | 2009-01-16 | 2009-07-08 | 华北电力大学 | 巡检飞行机器人对电力线路的检测方法 |
CN103365299A (zh) * | 2013-08-02 | 2013-10-23 | 中国科学院自动化研究所 | 一种无人机的避障方法及其装置 |
CN104298239A (zh) * | 2014-09-29 | 2015-01-21 | 湖南大学 | 一种室内移动机器人增强地图学习路径规划方法 |
CN108319291A (zh) * | 2017-12-13 | 2018-07-24 | 中国人民解放军空军工程大学 | 一种基于安全边界分析的无人机认知防碰撞控制方法 |
CN108388270A (zh) * | 2018-03-21 | 2018-08-10 | 天津大学 | 面向安全域的集群无人机轨迹姿态协同控制方法 |
CN108733074A (zh) * | 2018-05-23 | 2018-11-02 | 南京航空航天大学 | 一种基于匈牙利算法的多无人机编队航迹规划方法 |
CN109669475A (zh) * | 2019-01-22 | 2019-04-23 | 北京航空航天大学 | 基于人工蜂群算法的多无人机三维编队重构方法 |
Non-Patent Citations (3)
Title |
---|
A 3D collision avoidance strategy for UAV with physical constraints;ZhuLihua,等;《Measurement》;20160131;第77卷;第40-49页 * |
基于改进蚁群算法的山区无人机路径规划方法;唐立,等;《交通运输***工程与信息》;20190228;第19卷(第1期);第158-164页 * |
飞行冲突探测与解脱技术的研究;阮健;《中国优秀硕士学位论文全文数据库工程科技Ⅱ辑》;20140415(第04(2014)期);第C031-57页 * |
Also Published As
Publication number | Publication date |
---|---|
CN110262543A (zh) | 2019-09-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110262543B (zh) | 多目标点同时到达约束下的集群四维轨迹规划设计方法 | |
Oleynikova et al. | Continuous-time trajectory optimization for online uav replanning | |
US20210287556A1 (en) | Method and apparatus for generating optimal path for an unmanned aerial vehicle | |
CN104008674B (zh) | 一种基于模式匹配的终端区进场飞行时间预测方法 | |
Cobano et al. | Path planning based on genetic algorithms and the monte-carlo method to avoid aerial vehicle collisions under uncertainties | |
CN110362115B (zh) | 一种时间约束同时到达多无人机路径规划算法 | |
Slegers et al. | Terminal guidance of autonomous parafoils in high wind-to-airspeed ratios | |
CN110413007B (zh) | 无人机飞行路径的控制方法、***、电子设备及介质 | |
CN108387232B (zh) | 基于人工势场的进化算法的飞行物航迹规划方法 | |
Liu et al. | Research on cooperative trajectory planning and tracking problem for multiple carrier aircraft on the deck | |
Wang et al. | Decentralized MPC-based trajectory generation for multiple quadrotors in cluttered environments | |
Wu et al. | Heterogeneous mission planning for multiple uav formations via metaheuristic algorithms | |
Yao et al. | Hierarchical path generation for distributed mission planning of UAVs | |
Perez-Leon et al. | Integration of a 4D-trajectory follower to improve multi-UAV conflict management within the U-space context | |
Peltzer et al. | Stt-cbs: A conflict-based search algorithm for multi-agent path finding with stochastic travel times | |
Wu et al. | Collision-aware task assignment for multi-robot systems | |
Kong et al. | Multi-UAV simultaneous target assignment and path planning based on deep reinforcement learning in dynamic multiple obstacles environments | |
RU2568161C2 (ru) | Способ адаптивно-маршрутного управления пилотируемым летательным аппаратом | |
CN110232215B (zh) | 考虑机动任务需求的三维剖面分层迭代规划方法、***及介质 | |
Jing-Lin et al. | UAV real-time route planning based on multi-optimized RRT algorithm | |
Hota et al. | Time-optimal convergence to a rectilinear path in the presence of wind | |
RU2654238C1 (ru) | Способ управления беспилотным планирующим летательным аппаратом | |
JP6217057B2 (ja) | ミサイルの飛翔方法 | |
Khachumov | Controlling the motion of a group of unmanned flight vehicles in a perturbed environment based on the rules | |
Lee et al. | Autopilot design for unmanned combat aerial vehicles (UCAVs) via learning-based approach |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |