CN115840465A - 一种基于粒子群算法的无人机集群航迹规划方法 - Google Patents

一种基于粒子群算法的无人机集群航迹规划方法 Download PDF

Info

Publication number
CN115840465A
CN115840465A CN202211542932.XA CN202211542932A CN115840465A CN 115840465 A CN115840465 A CN 115840465A CN 202211542932 A CN202211542932 A CN 202211542932A CN 115840465 A CN115840465 A CN 115840465A
Authority
CN
China
Prior art keywords
unmanned aerial
aerial vehicle
particle
flight path
track
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.)
Pending
Application number
CN202211542932.XA
Other languages
English (en)
Inventor
张文博
刘君兰
姜超颖
常超
文强
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Xidian University
Original Assignee
Xidian University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Xidian University filed Critical Xidian University
Priority to CN202211542932.XA priority Critical patent/CN115840465A/zh
Publication of CN115840465A publication Critical patent/CN115840465A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种基于粒子群算法的无人机集群协同航迹规划方法，包括以下步骤：无人机集群获取街道场景下的一组起点，终点以及障碍物信息，建立二维栅格图；采用粒子群算法生成无人机集群中各无人机的全局初始航迹；检测各无人机初始航迹中存在的时空冲突点，采用在速度圆上取特定方向获得冲突避让点进行冲突消解；得到无时空冲突的无人机集群航迹，将航迹以网络通信udp方式传送给unity3D仿真平台进行无人机集群飞行验证；本发明能提供安全系数高，平滑性好，满足无人机集群协同性，有效完成冲突消解的航迹解决方案。

Description

一种基于粒子群算法的无人机集群航迹规划方法
技术领域
本发明属于多无人机协同规划技术领域，特别涉及一种基于粒子群算法的无人机集群航迹规划方法。
背景技术
近年来，无人机由于体积小，隐蔽性好，易于维护和部署成本低廉等特点而被广泛应用。随着芯片研发，材料制造，大数据等技术的不断进步，无人机逐步向智能化、集群化发展，无人机集群具有抗毁重构强，分布协同性高，经济效益显著等优势。无人机集群化以后，主要研究方向包括协同任务分配，协同航迹规划和集群编队等。其中协同航迹规划是无人机集群执行任务的基础和前提，多无人机航迹规划是根据各无人机的具体任务，在满足多类约束条件(平台性能约束、环境约束、任务协同约束)的前提下，为每架无人机规划初始点到目标点的飞行航迹，使其在指定的性能指标下达到最优或者较优。
目前复杂二维动态场景下的无人机集群航迹规划依旧是研究难点。基于传统算法，如A*算法的航迹规划，主要是将场景进行栅格化，然后采用点搜索算法，取各个离散点，规划出来的航迹较短，规划时间短，但是这是以牺牲平滑性和安全性为代价，规划出来的航迹并不适用于无人机飞行，并且存在与障碍物相切的情况。粒子群算法作为群优化算法分类中的重要算法，具有实现简单，可调参数少等优势，在各个优化场景下被广泛应用。在路径规划上也有能很好的应用，并且由于粒子种群的多样性，使得最终的解也呈现多样性，能找到更多的可行路径，但是在面向无人机集群时，粒子群算法并不能很好的规划出平滑的、避碰的集群轨迹。
发明内容
有鉴于此，为了克服上述现有技术的缺点，本发明的目的在于提供一种基于粒子群算法的无人机集群航迹规划方法，以期得到平滑且安全的航迹。
为了实现上述目的，本发明采用的技术方案是：
一种基于粒子群算法的无人机集群航迹规划方法，包括以下步骤：
步骤1：无人机集群获取目标场景下的障碍物位置与体积信息，以及无人机集群中各无人机对应起点与终点信息，建立对应的环境二维栅格图，并将无人机建模为一个可移动的圆形，该圆形即无人机的外切圆；
步骤2：采用粒子群算法生成无人机集群中各无人机的全局航迹；每个无人机对应一个粒子种群，粒子种群中每个粒子规划出一条航迹，无人机取其对应粒子种群中的最优航迹作为其初始航迹；
步骤3：采用分段贝塞尔曲线法对初始航迹进行分段平滑处理，依照整条航迹的曲线长度设定无人机集群中各无人机的优先级，长度越短，设置的优先级越低；
步骤4：检测多条无人机航迹中存在的冲突位置，在避让无人机的速度圆上寻找避让位置，取代检测得到的冲突位置，完成冲突消解；所述冲突是指在同一时刻下，两个无人机质心之间的距离小于安全距离，所述冲突位置即两个无人机外切圆相交的区域；所述安全距离为两个无人机对应的外切圆的半径和；
步骤5：得到新的无人机集群航迹，在仿真平台上验证航迹的安全性与时间效率。
在一个实施例中，所述步骤1，将目标场景建模为k*k栅格图，白色区域表示无人机集群的可行区域，黑色区域表示障碍物区域；从而用k*k大小的二维矩阵表示该场景信息，1表示障碍物区域，0表示可行区域；无人机集群由多架无人机组成，对应有一组起点和一组终点。
在一个实施例中，所述步骤2，初始化粒子种群数量为M，再设置粒子的速度和位置范围，分别为vmin≤vid≤vmax，xmin≤xid≤xmax，ymin≤yid≤ymax，其中vmin表示速度下限，取值为0，vmax表示速度上限，(xmin,ymin)表示粒子位置对应的横纵坐标的下限，xmin和ymin取值均为0，(xmax,ymax)表示粒子位置对应的横纵坐标的上限；vid表示粒子id的当前速度，(xid,yid)表示粒子id的当前位置；
标准粒子群算法的速度更新公式为：
vid(t+1)＝w×vid(t)+c1r1(pbestid(t)-xid(t))+c2r2(gbestid(t)-xid(t))
其中w表示惯性权重因子，将其设置为迭代期间线性递减的变量；vid(t)表示粒子id的t时刻速度，xid(t)表示粒子id的t时刻位置，c1表示自身认知部分的学习因子，c2表示社会认知部分的学习因子，r1，r2表示0-1之间的随机数；
标准粒子群算法的位置更新公式为：
xid(t+1)＝vid(t+1)+xid(t)
依照速度进行位置更新，然后设置对应的适应度函数fitness(t)，适应度函数应包括三部分：无人机当前位置到达目标点的距离函数f1(t)，无人机当前位置与障碍物的距离函数f2(t)以及无人机轨迹的平滑度对应的函数f3(t)，公式如下：
fitness(t)＝αf1(t)+βf2(t)+ηf3(t)
Figure BDA0003978549340000031
Figure BDA0003978549340000032
Figure BDA0003978549340000033
f1(t)的取值代表了粒子当前位置到达目标点的距离，f2(t)的取值代表了粒子当前位置距离障碍物的大小，f3(t)的取值代表了无人机上一段航迹到下一段航迹的偏角，α，β，η是取值为[0,1]之间的比例因子；obstacle表示障碍物的质心位置，xt表示粒子的t时刻位置，xt-1表示粒子的t-1时刻位置，xt-2表示粒子的t-2时刻位置。
在一个实施例中，所述步骤3，贝塞尔曲线的表达式如下所示：
Figure BDA0003978549340000041
Figure BDA0003978549340000042
/>
其中Pi为无人机航迹对应的贝塞尔曲线的控制点，
Figure BDA0003978549340000043
为伯恩斯坦多项式；i表示0-n之间的阶次变量、n表示控制点个数，即贝塞尔曲线的阶数，t表示0-1之间的时间变量；
所述分段贝塞尔曲线法，对于每个无人机，以N个航迹点作为一组，构成一段航迹，对该段航迹进行平滑处理，得到每个无人机平滑的航迹。
在一个实施例中，所述步骤4，当检测到下一时刻位置为冲突位置时，优先级低的一方为避让方，优先级高的一方保持原航迹，在集群任务时间内，优先级低的无人机保证在规定时间内到达目标点，并完成冲突避让。
在一个实施例中，所述避让方的速度圆，是以避让方当前时刻的位置为圆心，以该位置到下一时刻位置的距离为半径r的圆，在圆周上寻找避让位置，即避让冲突的航迹点；依照的方向为与其发生冲突的无人机的速度方向的反方向向量和自身速度方向向量的和所指向的方向，对应新的位置的横纵坐标(xnew,ynew)的计算公式具体如下：
Figure BDA0003978549340000044
Figure BDA0003978549340000045
其中(xcur,ycur)表示当前无人机的横纵坐标，angle表示速度方向需要调整的角度，通过如下公式求得，
vector1＝(xnext-xcur,ynext-ycur)
vector2＝-(xo,next-xo,cur,yo,next-yo,cur)
Figure BDA0003978549340000046
式中，(xcur,ycur)表示避让方的当前位置，(xnext,ynext)表示避让方下一时刻的位置，即发生碰撞的位置，vector1表示避让方的速度矢量，vector2表示发生冲突的另一架无人机的速度矢量的反方向向量，(xo,next,yo,next)和(xo,cur,yo,cur)分别表示发生冲突的另一架无人机的下一时刻的位置和当前时刻位置，最终得到无人机的避让位置，用避让位置代替原位置实现冲突消解。
与现有技术相比，本发明的有益效果是：
1.采用粒子群算法规划无人机集群初始化航迹，在充分考虑了无人机本身体积大小，航迹长度，动力学约束等诸多因素，在密集障碍物场景下，得到一条绝对安全航迹。
2.分段式贝塞尔曲线保留了无人机航迹的安全性，同时对无人机航迹进行平滑处理，得到平滑且安全的航迹。
3.提出的冲突消解方式，能有效处理该场景下出现的冲突问题，完成了无人机集群协同航迹规划。
附图说明
图1是本发明一种基于粒子群算法的无人机集群协同航迹规划方法的整体流程图。
图2是本发明一种基于粒子群算法的无人机集群协同航迹规划方法对应的Unity三维仿真场景。
图3是本发明一种基于粒子群算法的无人机集群协同航迹规划方法的建模的二维栅格环境图。
图4是本发明一种基于粒子群算法的无人机集群协同航迹规划方法的步骤2关于粒子群算法规划无人机集群的初始航迹的流程图。
图5a是本发明一种基于粒子群算法的无人机集群协同航迹规划方法的采用贝塞尔曲线优化整体航迹的效果图。
图5b是本发明一种基于粒子群算法的无人机集群协同航迹规划方法的采用贝塞尔曲线分段平滑航迹的效果图。
图6是本发明一种基于粒子群算法的无人机集群协同航迹规划方法的发生冲突的示意图。
图7是本发明一种基于粒子群算法的无人机集群协同航迹规划方法的冲突避让原理图。
图8a是本发明一种基于粒子群算法的无人机集群协同航迹规划的二维仿真图。
图8b是本发明一种基于粒子群算法的无人机集群协同航迹规划方法的三维仿真实验验证俯视图。
图8c是本发明一种基于粒子群算法的无人机集群协同航迹规划方法的三维仿真实验验证侧视图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步描述。
如图1所示，本发明基于粒子群算法的无人机集群航迹规划方法，包括以下步骤：
步骤1：无人机集群获取目标场景下的障碍物位置、体积等信息，以及无人机集群中各无人机对应起点与终点坐标等信息，建立对应的环境二维栅格图。并将无人机建模为一个可移动的圆形，该圆形即无人机的外切圆。
本发明中，无人机集群由多架无人机组成，因此对应有一组起点和一组终点。本发明将目标场景建模为k*k栅格图，从而用k*k大小的二维矩阵表示该场景信息，并以1表示障碍物区域，0表示可行区域。
在本发明的实施例中，选取目标场景为街道，Unity街道场景的面积大小为50m×50m，并且分布着一些建筑物，障碍物以类似于建筑物的四方体障碍物为主。无人机的可行区域为那些没有覆盖障碍物的区域，具体见图2所示。从俯视图来看，各建筑物可以看作是由一个个方格组成，存在很多不被障碍物覆盖的可行街道，所以可以将场景建模为50*50栅格图，在二维栅格图中，将障碍物区域建模为黑色方格，无人机集群的可行区域为白色区域，具体见图3。数学上，可用50*50大小的二维矩阵表示该场景信息，1表示障碍物区域，0表示可行区域。无人机具有一定大小和体积，因此本实施例将无人机建模为一个半径为r的可移动圆形区域，示例地r＝0.6。
步骤2：采用粒子群算法生成无人机集群中各无人机的全局航迹。每个无人机对应一个粒子种群，粒子种群中每个粒子规划出一条航迹，无人机取其对应粒子种群中的最优航迹作为其航迹。
在本发明的实施例中，随机初始粒子种群中每个粒子的速度，将种群粒子的初始位置设在起点处，进入循环；计算每个粒子的适应度函数值，根据对比获得每个粒子的历史最优值pbestid(t)，然后在本代中寻找最小适应度函数值作为种群本代的最优值gbestid(t)，根据速度和位置更新公式更新粒子的速度和位置，并保存每代的粒子的位置，该位置即一个航迹点，判断位置是否到达目标点并且适应度函数值是否小于阈值或者迭代次数是否达到最大迭代次数，满足两个条件中的任意一个，退出循环；将一个粒子的航迹点连起来组成一条折线，即一个粒子的航迹；一个粒子种群中，含有多个粒子，由此对应多条航迹，各粒子种群中的最优航迹即各无人机的全局航迹。
本发明利用粒子群算法进行全局航迹规划的流程可参考图4，粒子群算法以步骤1建模的二维栅格图作为粒子搜索点的环境。首先需要限制粒子选取航迹点的范围，初始化粒子种群数量为M，设置粒子的位置范围，对应约束为：
xmin≤xid≤xmax
ymin≤yid≤ymax
其中(xmin,ymin)表示粒子位置对应的横纵坐标的下限，(xmax,ymax)表示粒子位置对应的横纵坐标的上限；vid表示粒子id的当前速度，(xid,yid)表示粒子id的当前位置。
由于本实施例是在50m×50m的栅格地图上寻找航迹点，所以xmin为0，xmax为50，ymin为0，ymax为50。
对于粒子的速度也要设置相应的上下限，避免前后两代的位置相差太大，对应约束为：
vmin≤vid≤vmax
其中vmin表示速度下限，vmax表示速度上限，在本实施例中，vmin设为0，vmax设为4。
标准粒子群算法的速度和位置更新公式为：
vid(t+1)＝w×vid(t)+c1r1(pbestid(t)-xid(t))+c2r2(gbestid(t)-xid(t))
xid(t+1)＝vid(t+1)+xid(t)
其中w表示惯性权重因子，其值越大，表明粒子的全局寻优能力越好，收敛越快，但是也存在搜索粗糙，不易找到精确解等问题，w值越小，粒子的局部寻优能力越好，能找到精确解，但是收敛速度慢。考虑到粒子在迭代初期需要大范围探索，应具有较大的全局寻优能力，在迭代后期需要局部寻优找到精确解，即需要较好的局部寻优能力。所以将惯性权重因子w设置为迭代期间线性递减的变量，即随着迭代次数的增加而线性减小的量，保证初期大范围搜索的全局寻优能力和后期确定精确解的局部寻优能力，具体公式如下：
Figure BDA0003978549340000081
其中，wmax表示惯性权重因子的上限，一般取0.9，wmin表示惯性权重因子的下限，一般取0.4，Gmax表示最大迭代次数，g表示当前迭代次数。在速度更新公式中，c1表示自身认知部分的学习因子，c2表示社会认知部分的学习因子，研究表明当c1+c2≤4通常能取得较好的收敛结果；r1和r2为0-1之间的任意随机数；vid(t)表示粒子id的t时刻速度，xid(t)表示粒子id的t时刻位置。
速度更新公式包括三部分：针对之前速度的惯性项，自身认知项以及群体认知项。表明粒子的当代的速度不仅受上一代的影响，自身历史最优值的影响，而且受整个种群最优值的影响。
本发明依照速度进行位置更新，并设置对应的适应度函数fitness(t)，本发明设置的适应度函数fitness(t)应包括三部分：无人机当前位置到达目标点的距离函数f1(t)，无人机当前位置与障碍物的距离函数f2(t)以及无人机轨迹的平滑度对应的函数f3(t)。粒子群算法的优化目的是使得适应度函数值达到最小值。
fitness(t)＝αf1(t)+βf2(t)+ηf3(t)
α，β和η为比例因子。
其中f1(t)是和目标相关的距离函数，为粒子提供指引，向目标点不断靠近，从而使得函数值变小，其取值代表了粒子当前位置到达目标点的距离。具体公式为：
Figure BDA0003978549340000091
f2(t)是和避障相关的函数，保证粒子选取的航迹点是远离障碍物的质心的，其取值代表了粒子当前位置距离障碍物的大小。具体公式为：
Figure BDA0003978549340000092
f3(t)是和航迹的偏转角相关的函数，保证粒子在选择航迹点时尽量不要有太大的偏转角，从而尽量保证航迹总体是向着一个方向的，其取值代表了无人机上一段航迹到下一段航迹的偏角。具体公式为：
Figure BDA0003978549340000093
式中，α，β，η是取值为[0,1]之间的比例因子；obstacle表示障碍物的质心位置，xt表示粒子的t时刻位置，xt-1表示粒子的t-1时刻位置，xt-2表示粒子的t-2时刻位置。
经实验表明初始化种群数量为20，已经能找到较多可行航迹。本发明实施例中步骤2总的流程为：首先设置种群数量为20，再在[0,vmax]范围内随机初始化粒子种群每个粒子的初始速度，将粒子种群的位置设在起点处，每个粒子的历史最优值pbestid(t)以起点位置计算出来适应度函数值作为初始值，种群本代的最优值gbestid(t)设为无穷大。初始化后，进入循环，计算每个粒子的适应度函数值，如果粒子当前适应度函数值比历史最优值pbestid(t)小，则更新每个粒子的历史最优值pbestid(t)，在种群本代粒子中找到适应度函数最小值，作为全局最优值gbestid(t)，然后将pbestid(t)和gbestid(t)带入速度公式更新粒子的速度，位置，并保存每代的位置，每一代一个粒子的位置就对应一条航迹的航迹点。每一代都去判断位置是否到达目标点或者迭代次数是否达到，满足两个条件中的任意一个，退出循环。然后进行判断，如果迭代次数不等于最大迭代次数，并且粒子最后一代的坐标位置离目标点的距离小于1，说明找到了目标点，即找到了一条航迹，如果迭代次数等于最大迭代次数并且粒子最后一代的坐标位置离目标点较远，说明没有找到目标点，需要重新运行算法。经实验表明，当种群数量大于20个时，并让学习因子c1，c2满足c1≤c2，c1+c2≤4等条件，总能找到一条从起点到终点的无碰航迹。c1≤c2是因为在障碍物密集场景下，需要粒子接受更多来自粒子种群的指引，避免它跟随自己的速度和自身认知，陷入局部最优，即粒子卡在场景中某些障碍物死角处。
步骤3：采用分段贝塞尔曲线法对航迹进行分段平滑处理。经过步骤2，可以得到一系列的航迹点，将这些点连起来可以组成一条折线，这条折线就是一个粒子的航迹，这样的航迹不够平滑，偏角过大，不符合无人机动力学约束。所以在得到步骤2粒子群算法规划的初始航迹后，采用贝塞尔曲线对航迹进行平滑处理。并依照整条航迹的曲线长度设定无人机集群中各无人机的优先级，长度越短，设置的优先级越低。
贝塞尔曲线由伯恩斯坦公式推导而来。其n阶贝塞尔曲线对应公式如下：
Figure BDA0003978549340000101
Figure BDA0003978549340000111
其中Pi为无人机航迹对应的贝塞尔曲线的控制点，
Figure BDA0003978549340000112
为伯恩斯坦多项式。i表示0-n之间的阶次变量、n表示控制点个数，即贝塞尔曲线的阶数，t表示0-1之间的时间变量。
贝塞尔曲线是由伯恩斯坦多项式推导而来，是一条面向全局优化的曲线，除了起点和终点与第一个控制点和最后一个控制点重合外，中间的控制点一般不在曲线上。贝塞尔曲线的阶数与其控制点的个数有关，由两个点控制的是一阶贝塞尔曲线，为一条直线；由三个点控制的是二阶贝塞尔曲线为一条抛物线。所以贝塞尔曲线的阶数由这段航迹拥有的航迹点数量决定。
经实验表明，在本发明提供的密集障碍物场景下，无人机起点终点相隔较远的情况下，如果将整条航迹的航迹点作为贝塞尔曲线的控制点，得到一条拟合整体航迹的贝塞尔曲线，会存在与障碍物某个角相切的情况，具体见图5a。图5a中圈标出来的区域为无人机飞行危险区域，在仿真实验下，无人机的机翼会与障碍物发送碰撞，安全性不能保证。分析原因，粒子群算法规划出来的初始航迹是考虑了无人机的体积大小的，但是贝塞尔曲线在做平滑处理时，是要保证曲线包含所有顶点的最小多边形，并且不是按照控制点的顺序包围，所以特别是在转角处，贝塞尔曲线可能为了包含所有航迹点，而越过障碍物。
为了保证无人机飞行航迹的安全性，本发明采用分段贝塞尔曲线法进行分段优化，将整条航迹分为多段，分别采用贝塞尔曲线进行拟合。这样做的好处是每段航迹的跨度比较小，转角少，并且控制点的减少，也伴随着贝塞尔曲线阶数的降低，会相对于高阶的贝塞尔曲线更稳定，出现与障碍物相切的情况减少，当然也会带来平滑度的降低。
即，本发明的分段贝塞尔曲线法，对于每个无人机，以N个航迹点作为一组，构成一段航迹，对该段航迹进行平滑处理，得到每个无人机平滑的航迹。折中考虑，本实施例中以20个航迹点作为一组，对该段航迹进行平滑处理，能有效解决图5a出现的与墙面相撞的情况，最终保证了无人机航迹的安全性和平滑性，图5b是用贝塞尔曲线分段处理航迹的结果，可以看出每条航迹都不存在与障碍物相切的点。
同时，由于贝塞尔曲线本身也是时间的函数，可以将位置点与时间参数进行有效结合，规定无人机在固定时间内飞完整条航迹。在得到每个无人机平滑的航迹后，依照整条航迹曲线的长度来设定集群中各无人机的优先级，航迹长度越短，完成飞行任务的时间越短，有冗余时间去进行冲突避让，所以设置较低的优先级。
步骤4：检测多条无人机航迹中存在的冲突点，在避让无人机的速度圆上寻找避让位置，取代检测得到的冲突位置即原来发送冲突的位置坐标，完成冲突消解。
经过步骤3和步骤4的处理，得到的初始航迹满足安全性和平滑性，但是在整个区域内，随着无人机数量的增加，无法保障各无人机飞行过程中不相撞。所以需要进行冲突检测与消解。在本发明中，冲突是指在同一时刻下，两个无人机质心之间的距离小于安全距离，所以冲突位置并不是一个特定点，而是两个无人机外切圆相交的区域，见图6示意。同理，由于本发明将无人机建模为一个圆形，安全距离为两个无人机对应的外切圆的半径和。
在进行冲突消解前，需要规定集群中无人机的优先级，根据优先级高低去选择需要调整的无人机航迹点。设置无人机优先级的依据是，总航迹长度越短，优先级越低。在集群任务时间内，优先级低的无人机由于航迹短，能保证完成冲突避让后，在规定时间内到达目标点。即，当检测到下一时刻位置为冲突位置时，优先级低的一方为避让方，优先级高的一方保持原航迹，在集群任务时间内，优先级低的无人机保证在规定时间内到达目标点，并完成冲突避让。
冲突消解具体操作如图7所示，假设避让无人机的优先级低于另一架无人机的优先级，两机预测到下一时刻无人机会发生碰撞，避让无人机需要提前做出避让动作。以避让无人机当前时刻的位置为圆心，以该位置到下一时刻位置的距离为半径r画圆，这个圆也就是避让无人机的速度圆，在圆周上寻找避让位置，即避让冲突的航迹点，依照的方向为与其发生冲突的无人机的速度方向的反方向向量和自身速度方向向量的和所指向的方向，对应新的位置的横纵坐标(xnew,ynew)的计算公式具体如下：
Figure BDA0003978549340000131
Figure BDA0003978549340000132
其中(xcur,ycur)表示要完成避让动作的无人机的当前位置，r表示该无人机下一时刻位置到当前位置的距离，即速度圆。angle表示该无人机速度方向需要调整的角度，由表示避让方的速度矢量(即自身速度方向向量的和对应的方向向量)vector1和发生冲突的另一架无人机的速度矢量的反方向向量vector2的夹角决定，其对应公式为：
vector1＝(xnext-xcur,ynext-ycur)
vector2＝-(xo,next-xo,cur,yo,next-yo,cur)
Figure BDA0003978549340000133
其中(xcur,ycur)表示避让无人机的当前位置，(xnext,ynext)表示避让无人机下一时刻的位置，vector1表示避让无人机的速度向量。(xo,next,yo,next)和(xo,cur,yo,cur)分别表示发生冲突的另一架无人机的下一时刻位置和当前时刻位置，vector2表示避让无人机的速度向量的反方向。最终通过公式计算，可以得到无人机的避让位置坐标(xnew,ynew)，用避让位置代替原位置实现冲突消解。
步骤5：得到新的无人机集群航迹，在仿真平台上验证航迹的安全性与时间效率。
本发明的实施例中，在仿真平台unity3D上进行无人机仿真实验验证能否完成躲避静态障碍物和其他飞行中的无人机，并在有限时间内到达目标点。经过上千次无人机飞行实验验证，本发明能有效指导无人机集群完成多目标任务，并且中间过程能顺利避开障碍物，不与其他无人机相撞。
具体地，本发明首先在二维平台上规划出无人机集群完成步骤2-步骤4的航迹，然后将航迹以mavlink2.0格式通过udp网络通信方式发送到unity3D仿真平台上，无人机集群飞行验证是否会发生碰撞。图8a、图8b和图8c列出某次实验中，规划出来的航迹在二维平面显示结果以及无人机集群在三维平面的飞行俯视图和侧视图。经上千次仿真飞行实验验证，无人机集群飞行过程中不会发生碰撞，并且能在平均1分钟内完成50m*50m范围内的多目标任务。表明本发明提出的基于粒子群算法的无人机集群协同航迹规划方法能有效地为无人机集群规划出安全且平滑的航迹，并能有效的完成冲突消解。
综上，本发明在考虑了安全性，航迹长度，机间距离，无人机建模大小等诸多因素的前提下，保证了粒子群算法规划出来的初始航迹是绝对安全的。同时使用贝塞尔曲线对整条航迹进行分段平滑处理，使其更符合无人机的动力学约束，并对无人机集群的航迹进行冲突检测，针对冲突位置进行冲突消解。使得无人机集群各无人机都能在飞行过程中不发生碰撞，安全到达指定的目标，在规定时间内完成多目标任务。本发明能提供安全系数高，平滑性好，满足无人机集群协同性，有效完成冲突消解的航迹解决方案。
以上所述的具体实施方式仅是对本发明的优选方式进行描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。

Claims (9)

1.一种基于粒子群算法的无人机集群航迹规划方法，其特征在于，包括以下步骤：
步骤1：无人机集群获取目标场景下的障碍物位置与体积信息，以及无人机集群中各无人机对应起点与终点信息，建立对应的环境二维栅格图，并将无人机建模为一个可移动的圆形，该圆形即无人机的外切圆；
步骤2：采用粒子群算法生成无人机集群中各无人机的全局航迹；每个无人机对应一个粒子种群，粒子种群中每个粒子规划出一条航迹，无人机取其对应粒子种群中的最优航迹作为其初始航迹；
步骤3：采用分段贝塞尔曲线法对初始航迹进行分段平滑处理，依照整条航迹的曲线长度设定无人机集群中各无人机的优先级，长度越短，设置的优先级越低；
步骤4：检测多条无人机航迹中存在的冲突位置，在避让无人机的速度圆上寻找避让位置，取代检测得到的冲突位置，完成冲突消解；所述冲突是指在同一时刻下，两个无人机质心之间的距离小于安全距离，所述冲突位置即两个无人机外切圆相交的区域；所述安全距离为两个无人机对应的外切圆的半径和；
步骤5：得到新的无人机集群航迹，在仿真平台上验证航迹的安全性与时间效率。
2.根据权利要求1所述基于粒子群算法的无人机集群航迹规划方法，其特征在于，所述步骤1，将目标场景建模为k*k栅格图，白色区域表示无人机集群的可行区域，黑色区域表示障碍物区域；从而用k*k大小的二维矩阵表示该场景信息，1表示障碍物区域，0表示可行区域；无人机集群由多架无人机组成，对应有一组起点和一组终点。
3.根据权利要求1所述基于粒子群算法的无人机集群航迹规划方法，其特征在于，所述步骤2，随机初始粒子种群中每个粒子的速度，将种群粒子的初始位置设在起点处，进入循环；计算每个粒子的适应度函数值，根据对比获得每个粒子的历史最优值pbestid(t)，然后在本代中寻找最小适应度函数值作为种群本代的最优值gbestid(t)，根据速度和位置更新公式更新粒子的速度和位置，并保存每代的粒子的位置，该位置即一个航迹点，判断位置是否到达目标点并且适应度函数值是否小于阈值或者迭代次数是否达到最大迭代次数，满足两个条件中的任意一个，退出循环；将一个粒子的航迹点连起来组成一条折线，即一个粒子的航迹；一个粒子种群中，含有多个粒子，由此对应多条航迹，各粒子种群中的最优航迹即各无人机的全局航迹。
4.根据权利要求3所述基于粒子群算法的无人机集群航迹规划方法，其特征在于，所述步骤2，初始化粒子种群数量为M，再设置粒子的速度和位置范围，分别为vmin≤vid≤vmax，xmin≤xid≤xmax，ymin≤yid≤ymax，其中vmin表示速度下限，取值为0，vmax表示速度上限，(xmin,ymin)表示粒子位置对应的横纵坐标的下限，xmin和ymin取值均为0，(xmax,ymax)表示粒子位置对应的横纵坐标的上限；vid表示粒子id的当前速度，(xid,yid)表示粒子id的当前位置；
标准粒子群算法的速度更新公式为：
vid(t+1)＝w×vid(t)+c1r1(pbestid(t)-xid(t))+c2r2(gbestid(t)-xid(t))
其中w表示惯性权重因子，将其设置为迭代期间线性递减的变量；vid(t)表示粒子id的t时刻速度，xid(t)表示粒子id的t时刻位置，c1表示自身认知部分的学习因子，c2表示社会认知部分的学习因子，r1，r2表示0-1之间的随机数；
标准粒子群算法的位置更新公式为：
xid(t+1)＝vid(t+1)+xid(t)
依照速度进行位置更新，然后设置对应的适应度函数fitness(t)，适应度函数应包括三部分：无人机当前位置到达目标点的距离函数f1(t)，无人机当前位置与障碍物的距离函数f2(t)以及无人机轨迹的平滑度对应的函数f3(t)，公式如下：
fitness(t)＝αf1(t)+βf2(t)+ηf3(t)
Figure FDA0003978549330000031
Figure FDA0003978549330000032
Figure FDA0003978549330000033
f1(t)的取值代表了粒子当前位置到达目标点的距离，f2(t)的取值代表了粒子当前位置距离障碍物的大小，f3(t)的取值代表了无人机上一段航迹到下一段航迹的偏角，α，β，η是取值为[0,1]之间的比例因子；obstacle表示障碍物的质心位置，xt表示粒子的t时刻位置，xt-1表示粒子的t-1时刻位置，xt-2表示粒子的t-2时刻位置。
5.根据权利要求4所述基于粒子群算法的无人机集群航迹规划方法，其特征在于，所述惯性权重因子w的公式如下：
Figure FDA0003978549330000034
其中，wmax表示惯性权重因子的上限，wmin表示惯性权重因子的下限，Gmax表示最大迭代次数，g表示当前迭代次数。
6.根据权利要求1所述基于粒子群算法的无人机集群航迹规划方法，其特征在于，所述步骤3，贝塞尔曲线的表达式如下所示：
Figure FDA0003978549330000035
Figure FDA0003978549330000036
其中Pi为无人机航迹对应的贝塞尔曲线的控制点，
Figure FDA0003978549330000037
为伯恩斯坦多项式；i表示0-n之间的阶次变量、n表示控制点个数，即贝塞尔曲线的阶数，t表示0-1之间的时间变量；
所述分段贝塞尔曲线法，对于每个无人机，以N个航迹点作为一组，构成一段航迹，对该段航迹进行平滑处理，得到每个无人机平滑的航迹。
7.根据权利要求1所述基于粒子群算法的无人机集群航迹规划方法，其特征在于，所述步骤4，当检测到下一时刻位置为冲突位置时，优先级低的一方为避让方，优先级高的一方保持原航迹，在集群任务时间内，优先级低的无人机保证在规定时间内到达目标点，并完成冲突避让。
8.根据权利要求1所述基于粒子群算法的无人机集群航迹规划方法，其特征在于，所述避让方的速度圆，是以避让方当前时刻的位置为圆心，以该位置到下一时刻位置的距离为半径r的圆，在圆周上寻找避让位置，即避让冲突的航迹点；依照的方向为与其发生冲突的无人机的速度方向的反方向向量和自身速度方向向量的和所指向的方向，对应新的位置的横纵坐标(xnew,ynew)的计算公式具体如下：
Figure FDA0003978549330000041
Figure FDA0003978549330000042
其中(xcur,ycur)表示当前无人机的横纵坐标，angle表示速度方向需要调整的角度，通过如下公式求得，
vector1＝(xnext-xcur,ynext-ycur)
vector2＝-(xo,next-xo,cur,yo,next-yo,cur)
Figure FDA0003978549330000043
式中，(xcur,ycur)表示避让方的当前位置，(xnext,ynext)表示避让方下一时刻的位置，即发生碰撞的位置，vector1表示避让方的速度矢量，vector2表示发生冲突的另一架无人机的速度矢量的反方向向量，(xo,next,yo,next)和(xo,cur,yo,cur)分别表示发生冲突的另一架无人机的下一时刻的位置和当前时刻位置，最终得到无人机的避让位置，用避让位置代替原位置实现冲突消解。
9.根据权利要求1所述基于粒子群算法的无人机集群航迹规划方法，其特征在于，所述步骤5，在仿真平台unity3D上进行无人机仿真实验验证能否完成躲避静态障碍物和其他飞行中的无人机，并在有限时间内到达目标点。
CN202211542932.XA 2022-12-02 2022-12-02 一种基于粒子群算法的无人机集群航迹规划方法 Pending CN115840465A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211542932.XA CN115840465A (zh) 2022-12-02 2022-12-02 一种基于粒子群算法的无人机集群航迹规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211542932.XA CN115840465A (zh) 2022-12-02 2022-12-02 一种基于粒子群算法的无人机集群航迹规划方法

Publications (1)

Publication Number Publication Date
CN115840465A true CN115840465A (zh) 2023-03-24

Family

ID=85577935

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211542932.XA Pending CN115840465A (zh) 2022-12-02 2022-12-02 一种基于粒子群算法的无人机集群航迹规划方法

Country Status (1)

Country Link
CN (1) CN115840465A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116822362A (zh) * 2023-06-29 2023-09-29 中国人民解放军93209部队 一种基于粒子群算法的无人机无冲突四维航迹规划方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116822362A (zh) * 2023-06-29 2023-09-29 中国人民解放军93209部队 一种基于粒子群算法的无人机无冲突四维航迹规划方法
CN116822362B (zh) * 2023-06-29 2024-01-30 中国人民解放军93209部队 一种基于粒子群算法的无人机无冲突四维航迹规划方法

Similar Documents

Publication Publication Date Title
Wen et al. UAV online path planning algorithm in a low altitude dangerous environment
CN108664022B (zh) 一种基于拓扑地图的机器人路径规划方法及系统
CN111780777A (zh) 一种基于改进a*算法和深度强化学习的无人车路径规划方法
CN109144102A (zh) 一种基于改进蝙蝠算法的无人机航路规划方法
CN107168305A (zh) 路口场景下基于Bezier和VFH的无人车轨迹规划方法
CN109520507A (zh) 一种基于改进rrt的无人机实时路径规划方法
CN110413005A (zh) 一种基于逆推法的多无人机协同航迹规划方法
CN112414405A (zh) 一种顾及dsm的应急任务的无人机集群航迹规划方法
CN114740846A (zh) 面向拓扑-栅格-度量混合地图的分层路径规划方法
CN113313957A (zh) 基于增强型Dijkstra算法的无信号灯交叉路口车辆调度方法
CN115840465A (zh) 一种基于粒子群算法的无人机集群航迹规划方法
CN114967744A (zh) 一种多无人机协同避障的规划方法
CN111882047A (zh) 一种基于强化学习与线性规划的快速空管防冲突方法
CN115903888A (zh) 一种基于天牛群算法的旋翼无人机自主路径规划方法
CN113359808A (zh) 一种无人机电力巡检多级路径规划方法及相关装置
CN113759935B (zh) 基于模糊逻辑的智能群体编队移动控制方法
CN112327853B (zh) 一种可保证远离障碍物的基于硬约束优化问题的机器人平滑轨迹规划方法
CN115237928B (zh) 一种基于轨迹基元分割的高效碰撞检测方法及系统
CN114995384A (zh) 融合神经网络的改进hho算法agv路径规划
Wang et al. UAV online path planning based on improved genetic algorithm with optimized search region
CN115357050A (zh) 一种基于分层粒子群算法的无人机路径规划方法及其应用
CN115494866A (zh) 一种多无人机全局及局部路径智能规划方法及系统
Schroder et al. Path planning for cognitive vehicles using risk maps
Khachumov The problem of target pursuit by a group of unmanned flight vehicles
Liu et al. Multiple UAV formations delivery task planning based on a distributed adaptive algorithm

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