CN115357050A - 一种基于分层粒子群算法的无人机路径规划方法及其应用 - Google Patents

一种基于分层粒子群算法的无人机路径规划方法及其应用 Download PDF

Info

Publication number
CN115357050A
CN115357050A CN202211138201.9A CN202211138201A CN115357050A CN 115357050 A CN115357050 A CN 115357050A CN 202211138201 A CN202211138201 A CN 202211138201A CN 115357050 A CN115357050 A CN 115357050A
Authority
CN
China
Prior art keywords
point
path
generation
generation population
formula
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
CN202211138201.9A
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.)
Anhui University
Original Assignee
Anhui 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 Anhui University filed Critical Anhui University
Priority to CN202211138201.9A priority Critical patent/CN115357050A/zh
Publication of CN115357050A publication Critical patent/CN115357050A/zh
Pending legal-status Critical Current

Links

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)
  • Management, Administration, Business Operations System, And Electronic Commerce (AREA)

Abstract

本发明公开了一种基于分层粒子群算法的无人机路径规划方法及其应用，该方法包括：1)构建无人机的路径规划模型；2)利用障碍物中点信息去初始化种群，使种群中的个体在初始化时找到多条可行的路径；3)改进粒子群算法的实现，按照适应度值对种群个体分成三层，每层按照不同的更新公式去更新。本发明能够适用于复杂场景，同时能找到近似最优的路径规划方案。

Description

一种基于分层粒子群算法的无人机路径规划方法及其应用
技术领域
本发明涉及到无人机路径规划领域，具体的说是一种基于障碍物中点信息初始化的分层粒子群算法解决无人机路径规划问题。
背景技术
在过去的几十年里，无人机从过去的军事领域拓展到了农业、快递业、电力系、通信、图像识别等领域，比如利用无人机喷洒农药，配送包裹，电力巡检等。而无人机路径规划成为无人机任务中的关键技术之一，同时作为一种现实的优化问题，得到了国内外学者的广泛研究。无人机路径规划可以看作是一个复杂的优化问题，主要目标是得到最小的综合成本，同时满足无人机性能要求。无人机需要在不碰撞到障碍物的前提下，从给定的起始位置到目标位置，设计出一条平滑的、满足任务需要的近似最优的路径。因为无人机要躲避山脉、树木、建筑物等障碍物，使得无人机很难高效完成任务，因此，无人机路径规划问题需要高效的算法来解决。已有的解决路径规划问题的方法有Voronoi图，概率路线图(PRM)，A*算法，人工势场法(APF)，快速探索随机树(RRT)。但这些方法大多数存在容易陷入局部最优、时间复杂度高等问题。
近年来，元启发式算法以其更灵活、更有效的特点受到了研究者的广泛研究。为了解决无人机的路径规划问题，人们提出了一系列改进的元启发式算法，如遗传算法(GA)、差分进化算法(DE)、粒子群算法(PSO)、蚁群算法(ACO)、人工蜂群算法(ABC)、灰狼优化算法(GWO)等。这些方法虽然已经取得了一些路径规划的结果，但是在障碍物较多的复杂场景下找到更高质量的近似最优解仍然需要进一步改进。
发明内容
本发明为克服现有技术的不足之处，提供一种新基于分层粒子群算法的无人机路径规划方法及其应用，以期能解决较复杂环境下的无人机路径规划问题，从而能够提供最优的路径飞行方案。
本发明为达到上述发明目的，采用如下技术方案：
本发明一种基于分层粒子群算法的无人机路径规划方法，其特征在于，是按如下步骤进行：
步骤1、获取一目标区域，并以所述目标区域的外接矩形的一个顶点为原点，以与原点相连的两条边分别作为x轴和y轴，以垂直于oxy水平面的方向为z轴，从而建立三维坐标系；令Pj表示无人机的第j个路径点，Pj的坐标为(xj,yj,zj)，令起点S的坐标为(x1,y1,z1)，令终点E的坐标为(xN,yN,zN)；
步骤2、构建目标函数：
步骤2.1、最小化路径长度：
利用式(1)构建使无人机从起点S到终点E的飞行距离最小的路径长度f1，并作为第1个目标函数：
Figure BDA0003852252380000021
式(1)中，lj表示从第j个路径点Pj到第j+1个路径点Pj+1的欧几里得距离，N是路径点的个数，N-1是路径段的个数；
步骤2.2、最小化威胁代价函数：
利用式(2)和式(3)构建无人机飞行的威胁代价f2，并作为第2个目标函数：
Figure BDA0003852252380000022
Figure BDA0003852252380000023
式(2)和式(3)中，K为障碍物的数目，Rk为第k个障碍物的半径，S为碰撞区的危险距离，D为无人机的直径，
Figure BDA0003852252380000024
表示从第j个路径点Pij到第j+1个路径点Pj+1构成的向量，djk表示路径段
Figure BDA0003852252380000025
到第k个障碍物的圆心之间的距离；Tk(·)表示第k个障碍物的威胁代价；
步骤2.3、最小化高度代价：
利用式(4)和式(5)构建无人机在飞行过程中的高度代价f3，并作为第3个目标函数：
Figure BDA0003852252380000026
Figure BDA0003852252380000027
式(4)和式(5)中，hj表示无人机在第j个路径点Pj相对于地面的飞行高度，hmin和hmax分别为无人机飞行的最小高度和最大高度，Hj表示无人机在第j个路径点Pj的高度代价；
步骤2.4、最小化平滑代价：
利用式(6)计算两个连续路径段
Figure BDA0003852252380000028
和
Figure BDA0003852252380000029
之间的转角Φj：
Figure BDA0003852252380000031
式(6)中，P'j表示在第j个路径点Pj在oxy水平面的投影点，P'j+1表示在第j+1个路径点Pj+1在oxy水平面的投影点，
Figure BDA0003852252380000032
表示在第j个路径点Pj指向第j+1个路径点Pj+1构成的向量在oxy水平面的投影，
Figure BDA0003852252380000033
表示在第j+1个路径点Pj+1指向第j+2个路径点Pj+2构成的向量在oxy水平面的投影；
利用式(7)计算无人机在路径段
Figure BDA0003852252380000034
与其在oxy水平面上的投影
Figure BDA0003852252380000035
之间的爬升角ψj：
Figure BDA0003852252380000036
式(7)中，zj表示在第j个路径点Pj的z轴坐标，zj+1表示在第j+1个路径点Pj+1的z轴坐标；
利用式(8)构建无人机的平滑成本f4，并作为第4个目标函数：
Figure BDA0003852252380000037
式(8)中，a1和a2分别为转角Φj和爬升角ψj的惩罚系数；
Figure BDA0003852252380000038
表示从第j-1个路径点Pij到第j个路径点Pj+1构成的向量，
Figure BDA0003852252380000039
表示在第j-1个路径点Pj指向第j个路径点Pj+1构成的向量在oxy水平面的投影，ψj-1表示无人机在路径段
Figure BDA00038522523800000310
与其在oxy水平面上的投影
Figure BDA00038522523800000311
之间的爬升角；
利用式(9)构建无人机路径规划模型：
Figure BDA00038522523800000312
式(9)中，J是无人机路径的目标函数，bs是第s个目标函数fs的权重系数；
步骤3、基于改进的分层粒子群算法求解无人机路径规划模型：
步骤3.1、根据障碍物信息初始化种群：
步骤3.1.1、定义障碍物的数目为K，定义k1、k2、k3为障碍物的编号，并初始化k1＝1、k2＝k1+1、k3＝1；
步骤3.1.2、定义第k1个障碍物的圆心为
Figure BDA00038522523800000313
定义第k2个障碍物的圆心为
Figure BDA00038522523800000314
定义第k3个障碍物的圆心为
Figure BDA0003852252380000041
定义第k3个障碍物的半径为
Figure BDA00038522523800000418
步骤3.1.3、判断k3＝k1或k3＝k2是否成立，若成立，则执行步骤3.1.5；否则，执行步骤3.1.4；
步骤3.1.4、根据第k1个障碍物的圆心到第k2个障碍物的圆心所构成的直线
Figure BDA00038522523800000419
判断第k3个障碍物圆心到直线
Figure BDA00038522523800000425
的垂直距离是否小于第k3个障碍物的半径
Figure BDA00038522523800000421
若是，则执行步骤3.1.7；否则，执行步骤3.1.5；
步骤3.1.5、将k3+1赋值给k3，判断k3≤K是否成立，若成立，则执行步骤3.1.3；否则，执行步骤3.1.6；
步骤3.1.6、计算第k1个障碍物的圆心
Figure BDA0003852252380000042
与其相邻第k2个障碍物的圆心
Figure BDA0003852252380000043
的中点坐标为
Figure BDA0003852252380000044
步骤3.1.7、将k2+1赋值给k2后，初始化k3＝1，并判断k2≤K是否成立，若成立，则执行步骤3.1.3；否则，执行步骤3.1.8；
步骤3.1.8、将k1+1赋值给k1后，将k1+1赋值给k2，判断k1≤K是否成立，若成立，则执行步骤3.1.3；否则，表示得到所有相邻障碍物之间的中点并形成中点列表，并将中点列表中所有相邻障碍物之间的中点的个数记为G，执行步骤3.1.9；
步骤3.1.9、将终点E加入所述中点列表中，定义g1、g2为中点的编号，并初始化g1＝0，g2＝1；
步骤3.1.10、定义k为障碍物的编号，并初始化k＝1；
步骤3.1.11、定义第k个障碍物的圆心为
Figure BDA0003852252380000045
定义第k个障碍物的半径为rk；
步骤3.1.12、当g1＝0时，建立起点S与中点坐标
Figure BDA0003852252380000046
之间的直线
Figure BDA00038522523800000422
否则，建立中点
Figure BDA0003852252380000047
与中点
Figure BDA0003852252380000048
之间的直线
Figure BDA00038522523800000423
步骤3.1.13、判断
Figure BDA0003852252380000049
或
Figure BDA00038522523800000410
是否成立，若成立，表示中点
Figure BDA00038522523800000411
的x轴坐标相对中点
Figure BDA00038522523800000412
的x轴坐标是增加，执行步骤3.1.14；否则，执行步骤3.1.18；
步骤3.1.14、判断
Figure BDA00038522523800000413
和
Figure BDA00038522523800000414
是否成立，若成立，表示中点
Figure BDA00038522523800000415
的y轴坐标相对中点
Figure BDA00038522523800000416
的y轴坐标是增加的，执行步骤3.1.15；否则，执行步骤3.1.18；其中，ε表示一个固定值；
步骤3.1.15、判断第k个障碍物的圆心
Figure BDA00038522523800000417
到直线
Figure BDA00038522523800000424
的垂直距离是否小于第k个障碍物的半径rk，若是，则执行步骤3.1.18；否则，执行步骤3.1.16；
步骤3.1.16、将k+1赋值给k，判断k≤K是否成立，若成立，则执行步骤3.1.12；否则，执行步骤3.1.17；
步骤3.1.17、当g1＝0时，记中点坐标
Figure BDA0003852252380000051
为起点S的可达点，否则，记中点
Figure BDA0003852252380000052
为中点
Figure BDA0003852252380000053
的下一个可达点；
步骤3.1.18、将g2+1赋值给g2，初始化k＝1，判断g2≤G+1是否成立，若成立，则执行步骤3.1.12；否则，执行步骤3.1.19；
步骤3.1.19、将g1+1赋值给g1，初始化g2＝1，判断g1≤G是否成立，若成立，则执行步骤3.1.12；否则，表示得到起点S以及每个中点的可达点列表；
步骤3.1.20、定义t为当前迭代次数，令tmax为最大迭代次数，并初始化t＝1；
步骤3.1.21、定义并初始化种群大小为M，定义i为粒子的编号，并初始化i＝1；
步骤3.1.22、定义并初始化路径点的总个数为N，并初始化j＝1；
定义第t-1代种群第i个粒子的速度为
Figure BDA0003852252380000054
并初始化为全零向量；其中，
Figure BDA0003852252380000055
表示第t-1代第i个粒子的第j个路径点的速度；
定义第t-1代种群第i个粒子的位置为
Figure BDA0003852252380000056
Figure BDA0003852252380000057
其中，
Figure BDA0003852252380000058
表示第t-1代种群第i个粒子的第j个路径点的位置；
定义第t-1代种群第i个粒子的局部最优解为
Figure BDA0003852252380000059
其中，
Figure BDA00038522523800000510
表示第t-1代种群第i个粒子的第j个路径点的局部最优解；
定义第t-1代种群的全局最优解为
Figure BDA00038522523800000511
其中，
Figure BDA00038522523800000512
表示第t-1代种群的第j个路径点的全局最优解；
步骤3.1.23、当j＝1时，令起点S为第t-1代种群第i个粒子的第j个路径点
Figure BDA00038522523800000513
假设存在路径点
Figure BDA00038522523800000514
存在L个可达点，则第t-1代第i个粒子的第j个路径点的第l个可达点记为
Figure BDA00038522523800000515
其中，l＝1,2,...,L；
步骤3.1.24、计算第t-1代种群第i个粒子的第j个路径点
Figure BDA00038522523800000516
与其第l个可达点的距离为
Figure BDA00038522523800000517
从而利用式(10)计算第t-1代第i个粒子的第j个路径点的第l个可达点的选择概率
Figure BDA00038522523800000518
Figure BDA00038522523800000519
步骤3.1.25、令随机选择的概率为(0,1)之间的随机数r，当
Figure BDA00038522523800000520
时，选择中点
Figure BDA00038522523800000521
作为第t-1代种群第i个粒子的第j个路径点
Figure BDA00038522523800000522
的下一个可达点；
当2≤l≤L-1且
Figure BDA00038522523800000523
时，选择中点
Figure BDA00038522523800000524
作为第t-1代种群第i个粒子的第j个路径点
Figure BDA00038522523800000525
的下一个可达点；
当
Figure BDA0003852252380000061
时，选择中点
Figure BDA0003852252380000062
作为第t-1代种群第i个粒子的第j个路径点
Figure BDA0003852252380000063
的下一个可达点；从而得到第t-1代种群第i个粒子的第j+1个路径点
Figure BDA0003852252380000064
步骤3.1.26、将j+1赋值给j，判断第t-1代种群第i个粒子的第j个路径点
Figure BDA0003852252380000065
是否为终点，若是，则表示找到一条从起点S到终点E的路径，执行步骤3.1.28；否则，执行步骤3.1.27；
步骤3.1.27、判断j<N是否成立，若成立，执行步骤3.1.23；否则，未找到一条从起点S到终点E的路径，令j＝1，重新执行步骤3.1.23；
步骤3.1.28、判断j<N是否成立，若成立，则表示找到的路径点的数目小于N，执行步骤3.1.29；否则，表示完成对第t-1代种群第i个粒子的迭代，并获得第t-1代种群第i个粒子的初始位置
Figure BDA0003852252380000066
并执行步骤3.1.30；
步骤3.1.29、将最后一个路径段
Figure BDA0003852252380000067
均分成N-j+1段，以获得N-j个点，并作为剩下的N-j个路径点；从而完成对第t-1代种群第i个粒子的迭代，并获得第t-1代种群第i个粒子的初始位置
Figure BDA0003852252380000068
步骤3.1.30、利用式(9)计算第t-1代第i个粒子的适应度的值；
步骤3.1.31、将第t-1代第i个粒子的局部最优解
Figure BDA00038522523800000617
初始化为第t-1代种群第i个粒子的初始位置
Figure BDA00038522523800000618
步骤3.1.32、将i+1赋值给i，初始化j＝1，判断i≤M是否成立，若成立，则执行步骤3.1.23；否则，表示完成对第t-1代种群的迭代，并获得第t-1代种群的初始全局最优解为
Figure BDA0003852252380000069
步骤3.2、利用分层粒子群算法更新种群；
步骤3.2.1、根据式(11)得到第t次迭代的惯性权重为wt：
Figure BDA00038522523800000610
式(11)中，w0表示惯性权重的初始值；
步骤3.2.2、定义第t次迭代的第一个学习因子为
Figure BDA00038522523800000611
并初始化为固定值，根据式(12)-式(14)计算第t次迭代的四个学习因子
Figure BDA00038522523800000612
和
Figure BDA00038522523800000613
Figure BDA00038522523800000614
Figure BDA00038522523800000615
Figure BDA00038522523800000616
式(12)和(13)中，cmin为学习因子的最小值，cmax为学习因子的最大值；
步骤3.2.3、利用式(9)计算第t代种群的适应度值，并根据适应度值对第t代种群中的粒子进行升序排序，得到排序后的第t代种群；
步骤3.2.4、根据适应度值将排序后的第t代种群划分为三层，并将第t代种群的粒子数目M划分为
Figure BDA0003852252380000071
和
Figure BDA0003852252380000072
从而根据式(15)-式(17)分别计算排序后的第t代种群的第一层的粒子数目
Figure BDA0003852252380000073
第二层的粒子数目
Figure BDA0003852252380000074
和第三层的粒子数目
Figure BDA0003852252380000075
Figure BDA0003852252380000076
Figure BDA0003852252380000077
Figure BDA0003852252380000078
式(15)-式(17)中，floor()为向下取整，ceil()为向上取整，参数a1、a2、a3和a4取固定的参数，且满足a1+a2+a3＝1；
步骤3.2.5、初始化i＝1；
步骤3.2.6、初始化j＝1；
步骤3.2.7、当i<n1时，利用式(18)更新排序后的第t代种群的第一层粒子i的速度
Figure BDA00038522523800000722
当i<n1+n2时，利用式(19)更新排序后的第t代种群的第二层粒子i的速度
Figure BDA00038522523800000723
否则，利用式(20)更新排序后的第t代种群的第三层粒子i的速度
Figure BDA00038522523800000724
Figure BDA0003852252380000079
Figure BDA00038522523800000710
Figure BDA00038522523800000711
式(18)-式(20)中，
Figure BDA00038522523800000712
表示第t-1代第i个粒子的第j个路径点的位置，α1表示从第一层随机选取的粒子的序号，
Figure BDA00038522523800000713
表示第t代第α1个粒子的第j个路径点的位置，α2表示从第二层随机选取的粒子的序号，
Figure BDA00038522523800000714
表示第t代第α2个粒子的第j个路径点的位置，
Figure BDA00038522523800000715
表示第t-1代第i个粒子的第j个路径点的速度，
Figure BDA00038522523800000716
表示第t代第i个粒子的第j个路径点的速度，
Figure BDA00038522523800000717
表示第t-1代第i个粒子的第j个路径点的局部最优解，
Figure BDA00038522523800000718
表示第t-1代的第j个路径点的全局最优解，wt表示第t代种群的惯性权重，
Figure BDA00038522523800000719
和
Figure BDA00038522523800000720
为第t次迭代(0,1)之间的随机数；
步骤3.2.8、利用式(21)计算第t代第i个粒子的新位置
Figure BDA00038522523800000721
Figure BDA0003852252380000081
式(21)中，
Figure BDA0003852252380000082
表示排序后的第t代种群中第i个粒子的第j个路径点的位置；
步骤3.2.9、将j+1赋值给j，判断j≤N是否成立，若成立，则执行步骤3.2.7；否则，表示完成对排序后的第t代种群中第i个粒子的迭代；
步骤3.2.10、利用式(9)计算第t代种群中第i个粒子的适应度值；
步骤3.2.11、比较第t代种群和第t-1代种群中第i个粒子的适应度值，选择具有较小适应度值的第i个粒子的位置作为第t代种群第i个粒子的局部最优解
Figure BDA0003852252380000083
步骤3.2.12、将i+1赋值给i，判断i≤M是否成立，若成立，则执行步骤3.2.6；否则，表示完成对排序后的第t代种群的迭代，并获得第t代种群的全局最优解为
Figure BDA0003852252380000084
步骤3.2.13、将t+1赋值给t，判断t≤tmax是否成立，若成立，则执行步骤3.2.1；否则，表示获得第tmax代种群的全局最优解
Figure BDA0003852252380000085
并以全局最优解
Figure BDA0003852252380000086
对应的路径为最佳路径。
本发明一种电子设备，包括存储器以及处理器，其特点在于，所述存储器用于存储支持处理器执行所述的无人机路径规划方法的程序，所述处理器被配置为用于执行所述存储器中存储的程序。
本发明一种计算机可读存储介质，计算机可读存储介质上存储有计算机程序，其特点在于，所述计算机程序被处理器运行时执行所述的无人机路径规划方法的步骤。
与已有技术相比，本发明的有益效果体现在：
1、本发明提出了一种路径规划问题初始化种群的方法，利用障碍物中点信息去初始化种群，使种群中的个体在初始化时找到多条可行的路径，提高了种群的多样性，解决了随机初始化方法中只能找到一条或找不到可行路径的弊端。
2、本发明提出了一种分层粒子群算法的无人机路径规划方法，可以提高算法的收敛速度，具有较好的鲁棒性，提高了种群搜索的精度和多样性，使无人机能快速规划出较短路径，该方法在解决无人机三维路径规划问题上具有更显著的性能，同时适用于较复杂的无人机路径规划问题。
附图说明
图1为本发明分层粒子群算法的流程图；
图2为本发明无人机路径规划示意图；
图3为本发明威胁成本示意图；
图4为本发明转角和爬升角的计算示意图。
具体实施方式
在实施例中，一种基于分层粒子群算法的无人机路径规划方法是针对无人机的飞行场景，找到使路径长度、威胁成本和高度成本和平滑成本最小的路径规划方案，使无人机避开所有的障碍物，生成从起点到终点的高质量的接近最优的飞行路径。具体地说，如图1所示，按该方法是按如下步骤进行：
步骤1、获取一目标区域，并以目标区域的外接矩形的一个顶点为原点，以与原点相连的两条边分别作为x轴和y轴，以垂直于oxy水平面的方向为z轴，从而建立三维坐标系，具体来说，如图2所示。令Pj表示无人机的第j个路径点，Pj的坐标为(xj,yj,zj)，令起点S的坐标为(x1,y1,z1)，令终点E的坐标为(xN,yN,zN)；
步骤2、构建目标函数：
步骤2.1、最小化路径长度：
第一个目标函数是使无人机从起点到终点的飞行距离最小。一般来说，无人机路径越短，飞行所需的时间和燃料消耗越少，利用式(1)构建使无人机从起点S到终点E的飞行距离最小的路径长度f1，并作为第1个目标函数：
Figure BDA0003852252380000091
式(1)中，lj表示从第j个路径点Pj到第j+1个路径点Pj+1的欧几里得距离，N是路径点的个数，N-1是路径段的个数；
步骤2.2、最小化威胁代价函数：
除了优化路径长度之外，无人机在飞行过程中还需要躲避障碍物，来确保无人机的安全飞行。假设每个威胁用圆柱体表示。具体来说，如图3所示，利用式(2)和式(3)构建无人机飞行的威胁代价f2，并作为第2个目标函数：
Figure BDA0003852252380000092
Figure BDA0003852252380000093
式(2)和式(3)中，K为障碍物的数目，Rk为第k个障碍物的半径，S为碰撞区的危险距离，D为无人机的直径，直径D由无人机大小决定，距离S取决于几个因素，如应用、操作环境和定位精度；
Figure BDA0003852252380000094
表示从第j个路径点Pij到第j+1个路径点Pj+1构成的向量，djk表示路径段
Figure BDA0003852252380000095
到第k个障碍物的圆心之间的距离；Tk(·)表示第k个障碍物的威胁代价；
步骤2.3、最小化高度代价：
无人机在飞行过程中，飞行高度经常被限制在给定的最小高度和最大高度之间。利用式(4)和式(5)构建无人机在飞行过程中的高度代价f3，并作为第3个目标函数：
Figure BDA0003852252380000101
Figure BDA0003852252380000102
式(4)和式(5)中，hj表示无人机在第j个路径点Pj相对于地面的飞行高度，hmin和hmax分别为无人机飞行的最小高度和最大高度，Hj表示无人机在第j个路径点Pj的高度代价。该目标使无人机保持平均高度，并惩罚超出范围的值。
步骤2.4、最小化平滑代价：
无人机的角度由于其物理限制而受到限制，它不应该超过设定的最大角度。平滑成本用来评估转弯和爬升角，这是生成可行路径所必须的。具体来说，如图4所示，利用式(6)计算两个连续路径段
Figure BDA0003852252380000103
和
Figure BDA0003852252380000104
之间的转角Φj：
Figure BDA0003852252380000105
式(6)中，P'j表示在第j个路径点Pj在oxy水平面的投影点，P'j+1表示在第j+1个路径点Pj+1在oxy水平面的投影点，
Figure BDA0003852252380000106
表示在第j个路径点Pj指向第j+1个路径点Pj+1构成的向量在oxy水平面的投影，
Figure BDA0003852252380000107
表示在第j+1个路径点Pj+1指向第j+2个路径点Pj+2构成的向量在oxy水平面的投影；
利用式(7)计算无人机在路径段
Figure BDA0003852252380000108
与其在oxy水平面上的投影
Figure BDA0003852252380000109
之间的爬升角ψj：
Figure BDA00038522523800001010
式(7)中，zj表示在第j个路径点Pj的z轴坐标，zj+1表示在第j+1个路径点Pj+1的z轴坐标；
利用式(8)构建无人机的平滑成本f4，并作为第4个目标函数：
Figure BDA0003852252380000111
式(8)中，a1和a2分别为转角Φj和爬升角ψj的惩罚系数；
Figure BDA0003852252380000112
表示从第j-1个路径点Pij到第j个路径点Pj+1构成的向量，
Figure BDA0003852252380000113
表示在第j-1个路径点Pj指向第j个路径点Pj+1构成的向量在oxy水平面的投影，ψj-1表示无人机在路径段
Figure BDA0003852252380000114
与其在oxy水平面上的投影
Figure BDA0003852252380000115
之间的爬升角；
将无人机路径规划问题建模为由一组优化准则和约束条件定义的最小化问题，无人机路径的成本函数被定义为四个目标函数的加权和，利用式(9)构建无人机路径规划模型：
Figure BDA0003852252380000116
式(9)中，J是无人机路径的目标函数，bs是第s个目标函数fs的权重系数，f1到f4分别是与路径长度、威胁、飞行高度和平滑相关的代价；
步骤3、基于改进的分层粒子群算法求解无人机路径规划模型，粒子群算法是一种智能优化算法，本发明主要对将粒子分为三层，提高搜索精度和多样性，从而使得分层粒子群算法能够更好地求解无人机路径规划问题，分层粒子群算法按如下步骤进行：
步骤3.1、根据障碍物信息初始化种群：
步骤3.1.1、定义障碍物的数目为K，定义k1、k2、k3为障碍物的编号，并初始化k1＝1、k2＝k1+1、k3＝1；
步骤3.1.2、定义第k1个障碍物的圆心为
Figure BDA0003852252380000117
定义第k2个障碍物的圆心为
Figure BDA0003852252380000118
定义第k3个障碍物的圆心为
Figure BDA0003852252380000119
定义第k3个障碍物的半径为
Figure BDA00038522523800001110
步骤3.1.3、判断k3＝k1或k3＝k2是否成立，若成立，则执行步骤3.1.5；否则，执行步骤3.1.4；
步骤3.1.4、根据第k1个障碍物的圆心到第k2个障碍物的圆心所构成的直线
Figure BDA00038522523800001111
判断第k3个障碍物圆心到直线
Figure BDA00038522523800001112
的垂直距离是否小于第k3个障碍物的半径
Figure BDA00038522523800001113
若是，则执行步骤3.1.7；否则，执行步骤3.1.5；
步骤3.1.5、将k3+1赋值给k3，判断k3≤K是否成立，若成立，则执行步骤3.1.3；否则，执行步骤3.1.6；
步骤3.1.6、计算第k1个障碍物的圆心
Figure BDA00038522523800001114
与其相邻第k2个障碍物的圆心
Figure BDA00038522523800001115
的中点坐标为
Figure BDA00038522523800001116
步骤3.1.7、将k2+1赋值给k2后，初始化k3＝1，并判断k2≤K是否成立，若成立，则执行步骤3.1.3；否则，执行步骤3.1.8；
步骤3.1.8、将k1+1赋值给k1后，将k1+1赋值给k2，判断k1≤K是否成立，若成立，则执行步骤3.1.3；否则，表示得到所有相邻障碍物之间的中点并形成中点列表，并将中点列表中所有相邻障碍物之间的中点的个数记为G，执行步骤3.1.9；
步骤3.1.9、将终点E加入中点列表尾部，定义g1、g2为中点的编号，并初始化g1＝0，g2＝1；
步骤3.1.10、定义k为障碍物的编号，并初始化k＝1；
步骤3.1.11、定义第k个障碍物的圆心为
Figure BDA0003852252380000121
定义第k个障碍物的半径为rk；
步骤3.1.12、当g1＝0时，建立起点S与中点坐标
Figure BDA0003852252380000122
之间的直线
Figure BDA0003852252380000123
否则，建立中点坐标
Figure BDA0003852252380000124
与中点坐标
Figure BDA0003852252380000125
之间的直线
Figure BDA0003852252380000126
步骤3.1.13、判断
Figure BDA0003852252380000127
或
Figure BDA0003852252380000128
是否成立，若成立，表示中点
Figure BDA0003852252380000129
的x轴坐标相对中点
Figure BDA00038522523800001210
的x轴坐标是增加的或者很小范围的减少，执行步骤3.1.14；否则，执行步骤3.1.18；
步骤3.1.14、判断
Figure BDA00038522523800001211
和
Figure BDA00038522523800001212
是否成立，若成立，表示中点
Figure BDA00038522523800001213
的y轴坐标相对中点
Figure BDA00038522523800001214
的y轴坐标是增加的或者很小范围的减少，执行步骤3.1.15；否则，执行步骤3.1.18；其中，ε表示一个比较小的固定值；
步骤3.1.15、判断第k个障碍物的圆心
Figure BDA00038522523800001215
到直线
Figure BDA00038522523800001223
的垂直距离是否小于第k个障碍物的半径rk，若是，则执行步骤3.1.18；否则，执行步骤3.1.16；
步骤3.1.16、将k+1赋值给k，判断k≤K是否成立，若成立，则执行步骤3.1.12；否则，执行步骤3.1.17；
步骤3.1.17、当g1＝0时，记中点坐标
Figure BDA00038522523800001216
为起点S的可达点，否则，记中点
Figure BDA00038522523800001217
为中点
Figure BDA00038522523800001218
的下一个可达点；即中点
Figure BDA00038522523800001219
和中点
Figure BDA00038522523800001220
的连线没有碰到障碍物，则称中点
Figure BDA00038522523800001221
为
Figure BDA00038522523800001222
的可达点。
步骤3.1.18、将g2+1赋值给g2，初始化k＝1，判断g2≤G+1是否成立，若成立，则执行步骤3.1.12；否则，执行步骤3.1.19；
步骤3.1.19、将g1+1赋值给g1，初始化g2＝1，判断g1≤G是否成立，若成立，则执行步骤3.1.12；否则，表示得到起点S以及每个中点的可达点列表；
步骤3.1.20、定义t为当前迭代次数，令tmax为最大迭代次数，并初始化t＝1；
步骤3.1.21、定义并初始化种群大小为M，定义i为粒子的编号，并初始化i＝1；
步骤3.1.22、定义并初始化路径点的总个数为N，并初始化j＝1；
定义第t-1代种群第i个粒子的速度为
Figure BDA0003852252380000131
并初始化为全零向量；其中，
Figure BDA0003852252380000132
表示第t-1代第i个粒子的第j个路径点的速度；
定义第t-1代种群第i个粒子的位置为
Figure BDA0003852252380000133
Figure BDA0003852252380000134
其中，
Figure BDA0003852252380000135
表示第t-1代种群第i个粒子的第j个路径点的位置；
定义第t-1代种群第i个粒子的局部最优解为
Figure BDA0003852252380000136
其中，
Figure BDA0003852252380000137
表示第t-1代种群第i个粒子的第j个路径点的局部最优解；
定义第t-1代种群的全局最优解为
Figure BDA0003852252380000138
其中，
Figure BDA0003852252380000139
表示第t-1代种群的第j个路径点的全局最优解；
步骤3.1.23、当j＝1时，起点S为第t-1代种群第i个粒子的第j个路径点
Figure BDA00038522523800001310
假设存在路径点
Figure BDA00038522523800001311
存在L个可达点，则第t-1代第i个粒子的第j个路径点的第l个可达点记为
Figure BDA00038522523800001312
其中，l＝1,2,...,L；
步骤3.1.24、计算第t-1代种群第i个粒子的第j个路径点
Figure BDA00038522523800001313
与其第l个可达点的距离为
Figure BDA00038522523800001314
多个可达点，尽量选择距离短的点，距离越短被选择的概率越大。利用式(10)计算第t-1代第i个粒子的第j个路径点的第l个可达点的选择概率
Figure BDA00038522523800001315
Figure BDA00038522523800001316
步骤3.1.25、令随机选择的概率为(0,1)之间的随机数r，当
Figure BDA00038522523800001317
时，选择中点
Figure BDA00038522523800001318
作为第t-1代种群第i个粒子的第j个路径点
Figure BDA00038522523800001319
的下一个可达点；
当2≤l≤L-1且
Figure BDA00038522523800001320
时，选择中点
Figure BDA00038522523800001321
作为第t-1代种群第i个粒子的第j个路径点
Figure BDA00038522523800001322
的下一个可达点；
当
Figure BDA00038522523800001323
时，选择中点
Figure BDA00038522523800001324
作为第t-1代种群第i个粒子的第j个路径点
Figure BDA00038522523800001325
的下一个可达点；从而得到第t-1代种群第i个粒子的第j+1个路径点
Figure BDA00038522523800001326
步骤3.1.26、将j+1赋值给j，判断第t-1代种群第i个粒子的第j个路径点
Figure BDA00038522523800001327
是否为终点，若是，则表示找到一条从起点到终点的路径，执行步骤3.1.28；否则，执行步骤3.1.27；
步骤3.1.27、判断j<N是否成立，若成立，执行步骤3.1.23；否则，表示找到的路径点的数目达到N，没有找到一条从起点到终点的路径，令j＝1，重新执行步骤3.1.23；
步骤3.1.28、判断j<N是否成立，若成立，则表示找到的路径点的数目小于N，执行步骤3.1.29；否则，表示完成对第t-1代种群第i个粒子的迭代，并获得第t-1代种群第i个粒子的初始位置
Figure BDA0003852252380000141
并执行步骤3.1.30；
步骤3.1.29、将最后一个路径段
Figure BDA0003852252380000142
均分成N-j+1段，以获得N-j个点，并作为剩下的N-j个路径点；从而完成对第t-1代种群第i个粒子的迭代，并获得第t-1代种群第i个粒子的初始位置
Figure BDA0003852252380000143
步骤3.1.30、利用式(9)计算第t-1代第i个粒子的适应度的值；
步骤3.1.31、将第t-1代第i个粒子的局部最优解
Figure BDA0003852252380000144
初始化为第t-1代种群第i个粒子的初始位置
Figure BDA0003852252380000145
步骤3.1.32、将i+1赋值给i，初始化j＝1，判断i≤M是否成立，若成立，则执行步骤3.1.23；否则，表示完成对第t-1代种群的迭代，并获得第t-1代种群的初始全局最优解为
Figure BDA0003852252380000146
步骤3.2、利用分层粒子群算法更新种群；
步骤3.2.1、wt采用指数形式,动态非线性变化。根据式(11)得到第t次迭代的惯性权重为wt：
Figure BDA0003852252380000147
式(11)中，w0表示惯性权重的初始值；
步骤3.2.2、定义第t次迭代的第一个学习因子为
Figure BDA0003852252380000148
并初始化为固定值，根据式(12)-式(14)计算第t次迭代的四个学习因子
Figure BDA0003852252380000149
和
Figure BDA00038522523800001410
Figure BDA00038522523800001411
Figure BDA00038522523800001412
Figure BDA00038522523800001413
式(12)和(13)中，cmin为学习因子的最小值，cmax为学习因子的最大值；
Figure BDA00038522523800001414
在最小值cmin和最大值cmax之间随着迭代次数的增加而减少，
Figure BDA00038522523800001415
和
Figure BDA00038522523800001416
随着迭代次数在最小值cmin和最大值cmax之间增加；
步骤3.2.3、利用式(9)计算第t代种群的适应度值，并根据适应度值对第t代种群中的粒子进行升序排序，得到排序后的第t代种群；
步骤3.2.4、根据适应度值将排序后的第t代种群划分为三层，并将第t代种群的粒子数目M划分为
Figure BDA00038522523800001417
和
Figure BDA00038522523800001418
从而根据式(15)-式(17)分别计算排序后的第t代种群的第一层的粒子数目
Figure BDA00038522523800001419
第二层的粒子数目
Figure BDA00038522523800001420
和第三层的粒子数目
Figure BDA00038522523800001421
Figure BDA0003852252380000151
Figure BDA0003852252380000152
Figure BDA0003852252380000153
式(15)-式(17)中，floor()为向下取整，ceil()为向上取整，参数a1、a2、a3和a4取固定的参数，且满足a1+a2+a3＝1；n1和n3随着迭代次数变化，n2在迭代过程中固定不变。在迭代初期，需要提高粒子的多样性，所以n1设置的比较小，n3比较大。在迭代后期，粒子需要提高搜索精度，所以n1比较大，n3比较小。n1随着迭代次数慢慢增加，n3随着迭代次数慢慢减少。
步骤3.2.5、初始化i＝1；
步骤3.2.6、初始化j＝1；
步骤3.2.7、当i<n1时，利用式(18)更新排序后的第t代种群的第一层粒子i的速度
Figure BDA0003852252380000154
当i<n1+n2时，利用式(19)更新排序后的第t代种群的第二层粒子i的速度
Figure BDA0003852252380000155
否则，利用式(20)更新排序后的第t代种群的第三层粒子i的速度
Figure BDA0003852252380000156
Figure BDA0003852252380000157
Figure BDA0003852252380000158
Figure BDA0003852252380000159
式(18)-式(20)中，
Figure BDA00038522523800001510
表示第t-1代第i个粒子的第j个路径点的位置，α1表示从第一层随机选取的粒子的序号，
Figure BDA00038522523800001511
表示第t代第α1个粒子的第j个路径点的位置，α2表示从第二层随机选取的粒子的序号，
Figure BDA00038522523800001512
表示第t代第α2个粒子的第j个路径点的位置，
Figure BDA00038522523800001513
表示第t-1代第i个粒子的第j个路径点的速度，
Figure BDA00038522523800001514
表示第t代第i个粒子的第j个路径点的速度，
Figure BDA00038522523800001515
表示第t-1代第i个粒子的第j个路径点的局部最优解，
Figure BDA00038522523800001516
表示第t-1代的第j个路径点的全局最优解，wt表示第t代种群的惯性权重，
Figure BDA00038522523800001517
和
Figure BDA00038522523800001518
为第t次迭代(0,1)之间的随机数；
第一层为适应度较好的粒子，该层的粒子主要进行全局搜索，向全局最优解学习，提高了搜索精度，第二层粒子适应度值适中，同时向全局最优解和局部最优解学习，第三层的粒子适应度值较差，该层的粒子随机选择两个第一层和第二层的个体对当前个体进行引导，主要为了增加粒子的多样性。
步骤3.2.8、利用式(21)计算第t代第i个粒子的新位置
Figure BDA0003852252380000161
Figure BDA0003852252380000162
式(21)中，
Figure BDA0003852252380000163
表示排序后的第t代种群中第i个粒子的第j个路径点的位置；
步骤3.2.9、将j+1赋值给j，判断j≤N是否成立，若成立，则执行步骤3.2.7；否则，表示完成对排序后的第t代种群中第i个粒子的迭代；
步骤3.2.10、利用式(9)计算第t代种群中第i个粒子的适应度值；
步骤3.2.11、比较第t代种群和第t-1代种群中第i个粒子的适应度值，选择具有较小适应度值的第i个粒子的位置作为第t代种群第i个粒子的局部最优解
Figure BDA0003852252380000164
步骤3.2.12、将i+1赋值给i，判断i≤M是否成立，若成立，则执行步骤3.2.6；否则，表示完成对排序后的第t代种群的迭代，并获得第t代种群的全局最优解为
Figure BDA0003852252380000165
步骤3.2.13、将t+1赋值给t，判断t≤tmax是否成立，若成立，则执行步骤3.2.1；否则，表示获得第tmax代种群的全局最优解
Figure BDA0003852252380000166
并以全局最优解
Figure BDA0003852252380000167
对应的路径为最佳路径。
本实施例中，一种电子设备，包括存储器以及处理器，该存储器用于存储支持处理器执行该无人机路径规划方法的程序，该处理器被配置为用于执行该存储器中存储的程序。
本实施例中，一种计算机可读存储介质，是在计算机可读存储介质上存储有计算机程序，该计算机程序被处理器运行时执行该无人机路径规划方法的步骤。

Claims (3)

1.一种基于分层粒子群算法的无人机路径规划方法，其特征在于，是按如下步骤进行：
步骤1、获取一目标区域，并以所述目标区域的外接矩形的一个顶点为原点，以与原点相连的两条边分别作为x轴和y轴，以垂直于oxy水平面的方向为z轴，从而建立三维坐标系；令Pj表示无人机的第j个路径点，Pj的坐标为(xj,yj,zj)，令起点S的坐标为(x1,y1,z1)，令终点E的坐标为(xN,yN,zN)；
步骤2、构建目标函数：
步骤2.1、最小化路径长度：
利用式(1)构建使无人机从起点S到终点E的飞行距离最小的路径长度f1，并作为第1个目标函数：
Figure FDA0003852252370000011
式(1)中，lj表示从第j个路径点Pj到第j+1个路径点Pj+1的欧几里得距离，N是路径点的个数，N-1是路径段的个数；
步骤2.2、最小化威胁代价函数：
利用式(2)和式(3)构建无人机飞行的威胁代价f2，并作为第2个目标函数：
Figure FDA0003852252370000012
Figure FDA0003852252370000013
式(2)和式(3)中，K为障碍物的数目，Rk为第k个障碍物的半径，S为碰撞区的危险距离，D为无人机的直径，
Figure FDA0003852252370000014
表示从第j个路径点Pij到第j+1个路径点Pj+1构成的向量，djk表示路径段
Figure FDA0003852252370000015
到第k个障碍物的圆心之间的距离；Tk(·)表示第k个障碍物的威胁代价；
步骤2.3、最小化高度代价：
利用式(4)和式(5)构建无人机在飞行过程中的高度代价f3，并作为第3个目标函数：
Figure FDA0003852252370000016
Figure FDA0003852252370000017
式(4)和式(5)中，hj表示无人机在第j个路径点Pj相对于地面的飞行高度，hmin和hmax分别为无人机飞行的最小高度和最大高度，Hj表示无人机在第j个路径点Pj的高度代价；
步骤2.4、最小化平滑代价：
利用式(6)计算两个连续路径段
Figure FDA0003852252370000021
和
Figure FDA0003852252370000022
之间的转角Φj：
Figure FDA0003852252370000023
式(6)中，P'j表示在第j个路径点Pj在oxy水平面的投影点，P'j+1表示在第j+1个路径点Pj+1在oxy水平面的投影点，
Figure FDA0003852252370000024
表示在第j个路径点Pj指向第j+1个路径点Pj+1构成的向量在oxy水平面的投影，
Figure FDA0003852252370000025
表示在第j+1个路径点Pj+1指向第j+2个路径点Pj+2构成的向量在oxy水平面的投影；
利用式(7)计算无人机在路径段
Figure FDA0003852252370000026
与其在oxy水平面上的投影
Figure FDA0003852252370000027
之间的爬升角ψj：
Figure FDA0003852252370000028
式(7)中，zj表示在第j个路径点Pj的z轴坐标，zj+1表示在第j+1个路径点Pj+1的z轴坐标；
利用式(8)构建无人机的平滑成本f4，并作为第4个目标函数：
Figure FDA0003852252370000029
式(8)中，a1和a2分别为转角Φj和爬升角ψj的惩罚系数；
Figure FDA00038522523700000210
表示从第j-1个路径点Pij到第j个路径点Pj+1构成的向量，
Figure FDA00038522523700000211
表示在第j-1个路径点Pj指向第j个路径点Pj+1构成的向量在oxy水平面的投影，ψj-1表示无人机在路径段
Figure FDA00038522523700000212
与其在oxy水平面上的投影
Figure FDA00038522523700000213
之间的爬升角；
利用式(9)构建无人机路径规划模型：
Figure FDA00038522523700000214
式(9)中，J是无人机路径的目标函数，bs是第s个目标函数fs的权重系数；
步骤3、基于改进的分层粒子群算法求解无人机路径规划模型：
步骤3.1、根据障碍物信息初始化种群：
步骤3.1.1、定义障碍物的数目为K，定义k1、k2、k3为障碍物的编号，并初始化k1＝1、k2＝k1+1、k3＝1；
步骤3.1.2、定义第k1个障碍物的圆心为
Figure FDA0003852252370000031
定义第k2个障碍物的圆心为
Figure FDA0003852252370000032
定义第k3个障碍物的圆心为
Figure FDA0003852252370000033
定义第k3个障碍物的半径为
Figure FDA0003852252370000034
步骤3.1.3、判断k3＝k1或k3＝k2是否成立，若成立，则执行步骤3.1.5；否则，执行步骤3.1.4；
步骤3.1.4、根据第k1个障碍物的圆心到第k2个障碍物的圆心所构成的直线
Figure FDA0003852252370000035
判断第k3个障碍物圆心到直线
Figure FDA0003852252370000036
的垂直距离是否小于第k3个障碍物的半径
Figure FDA0003852252370000037
若是，则执行步骤3.1.7；否则，执行步骤3.1.5；
步骤3.1.5、将k3+1赋值给k3，判断k3≤K是否成立，若成立，则执行步骤3.1.3；否则，执行步骤3.1.6；
步骤3.1.6、计算第k1个障碍物的圆心
Figure FDA0003852252370000038
与其相邻第k2个障碍物的圆心
Figure FDA0003852252370000039
的中点坐标为
Figure FDA00038522523700000310
步骤3.1.7、将k2+1赋值给k2后，初始化k3＝1，并判断k2≤K是否成立，若成立，则执行步骤3.1.3；否则，执行步骤3.1.8；
步骤3.1.8、将k1+1赋值给k1后，将k1+1赋值给k2，判断k1≤K是否成立，若成立，则执行步骤3.1.3；否则，表示得到所有相邻障碍物之间的中点并形成中点列表，并将中点列表中所有相邻障碍物之间的中点的个数记为G，执行步骤3.1.9；
步骤3.1.9、将终点E加入所述中点列表中，定义g1、g2为中点的编号，并初始化g1＝0，g2＝1；
步骤3.1.10、定义k为障碍物的编号，并初始化k＝1；
步骤3.1.11、定义第k个障碍物的圆心为
Figure FDA00038522523700000311
定义第k个障碍物的半径为rk；
步骤3.1.12、当g1＝0时，建立起点S与中点坐标
Figure FDA00038522523700000312
之间的直线lg2，否则，建立中点
Figure FDA00038522523700000313
与中点
Figure FDA00038522523700000314
之间的直线
Figure FDA00038522523700000323
步骤3.1.13、判断
Figure FDA00038522523700000315
或
Figure FDA00038522523700000316
是否成立，若成立，表示中点
Figure FDA00038522523700000317
的x轴坐标相对中点
Figure FDA00038522523700000318
的x轴坐标是增加，执行步骤3.1.14；否则，执行步骤3.1.18；
步骤3.1.14、判断
Figure FDA00038522523700000319
和
Figure FDA00038522523700000320
是否成立，若成立，表示中点
Figure FDA00038522523700000321
的y轴坐标相对中点
Figure FDA00038522523700000322
的y轴坐标是增加的，执行步骤3.1.15；否则，执行步骤3.1.18；其中，ε表示一个固定值；
步骤3.1.15、判断第k个障碍物的圆心
Figure FDA0003852252370000041
到直线
Figure FDA0003852252370000042
的垂直距离是否小于第k个障碍物的半径rk，若是，则执行步骤3.1.18；否则，执行步骤3.1.16；
步骤3.1.16、将k+1赋值给k，判断k≤K是否成立，若成立，则执行步骤3.1.12；否则，执行步骤3.1.17；
步骤3.1.17、当g1＝0时，记中点坐标
Figure FDA0003852252370000043
为起点S的可达点，否则，记中点
Figure FDA0003852252370000044
为中点
Figure FDA0003852252370000045
的下一个可达点；
步骤3.1.18、将g2+1赋值给g2，初始化k＝1，判断g2≤G+1是否成立，若成立，则执行步骤3.1.12；否则，执行步骤3.1.19；
步骤3.1.19、将g1+1赋值给g1，初始化g2＝1，判断g1≤G是否成立，若成立，则执行步骤3.1.12；否则，表示得到起点S以及每个中点的可达点列表；
步骤3.1.20、定义t为当前迭代次数，令tmax为最大迭代次数，并初始化t＝1；
步骤3.1.21、定义并初始化种群大小为M，定义i为粒子的编号，并初始化i＝1；
步骤3.1.22、定义并初始化路径点的总个数为N，并初始化j＝1；
定义第t-1代种群第i个粒子的速度为
Figure FDA0003852252370000046
并初始化为全零向量；其中，
Figure FDA0003852252370000047
表示第t-1代第i个粒子的第j个路径点的速度；
定义第t-1代种群第i个粒子的位置为
Figure FDA0003852252370000048
Figure FDA0003852252370000049
其中，
Figure FDA00038522523700000410
表示第t-1代种群第i个粒子的第j个路径点的位置；
定义第t-1代种群第i个粒子的局部最优解为
Figure FDA00038522523700000411
其中，
Figure FDA00038522523700000412
表示第t-1代种群第i个粒子的第j个路径点的局部最优解；
定义第t-1代种群的全局最优解为
Figure FDA00038522523700000413
其中，
Figure FDA00038522523700000414
表示第t-1代种群的第j个路径点的全局最优解；
步骤3.1.23、当j＝1时，令起点S为第t-1代种群第i个粒子的第j个路径点
Figure FDA00038522523700000415
假设存在路径点
Figure FDA00038522523700000416
存在L个可达点，则第t-1代第i个粒子的第j个路径点的第l个可达点记为
Figure FDA00038522523700000417
其中，l＝1,2,...,L；
步骤3.1.24、计算第t-1代种群第i个粒子的第j个路径点
Figure FDA00038522523700000418
与其第l个可达点的距离为
Figure FDA00038522523700000419
从而利用式(10)计算第t-1代第i个粒子的第j个路径点的第l个可达点的选择概率
Figure FDA00038522523700000420
Figure FDA0003852252370000051
步骤3.1.25、令随机选择的概率为(0,1)之间的随机数r，当
Figure FDA0003852252370000052
时，选择中点
Figure FDA0003852252370000053
作为第t-1代种群第i个粒子的第j个路径点
Figure FDA0003852252370000054
的下一个可达点；
当2≤l≤L-1且
Figure FDA0003852252370000055
时，选择中点
Figure FDA0003852252370000056
作为第t-1代种群第i个粒子的第j个路径点
Figure FDA0003852252370000057
的下一个可达点；
当
Figure FDA0003852252370000058
时，选择中点
Figure FDA0003852252370000059
作为第t-1代种群第i个粒子的第j个路径点
Figure FDA00038522523700000510
的下一个可达点；从而得到第t-1代种群第i个粒子的第j+1个路径点
Figure FDA00038522523700000511
步骤3.1.26、将j+1赋值给j，判断第t-1代种群第i个粒子的第j个路径点
Figure FDA00038522523700000512
是否为终点，若是，则表示找到一条从起点S到终点E的路径，执行步骤3.1.28；否则，执行步骤3.1.27；
步骤3.1.27、判断j<N是否成立，若成立，执行步骤3.1.23；否则，未找到一条从起点S到终点E的路径，令j＝1，重新执行步骤3.1.23；
步骤3.1.28、判断j<N是否成立，若成立，则表示找到的路径点的数目小于N，执行步骤3.1.29；否则，表示完成对第t-1代种群第i个粒子的迭代，并获得第t-1代种群第i个粒子的初始位置
Figure FDA00038522523700000513
并执行步骤3.1.30；
步骤3.1.29、将最后一个路径段
Figure FDA00038522523700000514
均分成N-j+1段，以获得N-j个点，并作为剩下的N-j个路径点；从而完成对第t-1代种群第i个粒子的迭代，并获得第t-1代种群第i个粒子的初始位置
Figure FDA00038522523700000515
步骤3.1.30、利用式(9)计算第t-1代第i个粒子的适应度的值；
步骤3.1.31、将第t-1代第i个粒子的局部最优解
Figure FDA00038522523700000516
初始化为第t-1代种群第i个粒子的初始位置
Figure FDA00038522523700000517
步骤3.1.32、将i+1赋值给i，初始化j＝1，判断i≤M是否成立，若成立，则执行步骤3.1.23；否则，表示完成对第t-1代种群的迭代，并获得第t-1代种群的初始全局最优解为
Figure FDA00038522523700000518
步骤3.2、利用分层粒子群算法更新种群；
步骤3.2.1、根据式(11)得到第t次迭代的惯性权重为wt：
Figure FDA00038522523700000519
式(11)中，w0表示惯性权重的初始值；
步骤3.2.2、定义第t次迭代的第一个学习因子为
Figure FDA0003852252370000061
并初始化为固定值，根据式(12)-式(14)计算第t次迭代的四个学习因子
Figure FDA0003852252370000062
和
Figure FDA0003852252370000063
Figure FDA0003852252370000064
Figure FDA0003852252370000065
Figure FDA0003852252370000066
式(12)和(13)中，cmin为学习因子的最小值，cmax为学习因子的最大值；
步骤3.2.3、利用式(9)计算第t代种群的适应度值，并根据适应度值对第t代种群中的粒子进行升序排序，得到排序后的第t代种群；
步骤3.2.4、根据适应度值将排序后的第t代种群划分为三层，并将第t代种群的粒子数目M划分为
Figure FDA0003852252370000067
和
Figure FDA0003852252370000068
从而根据式(15)-式(17)分别计算排序后的第t代种群的第一层的粒子数目
Figure FDA0003852252370000069
第二层的粒子数目
Figure FDA00038522523700000610
和第三层的粒子数目
Figure FDA00038522523700000611
Figure FDA00038522523700000612
Figure FDA00038522523700000613
Figure FDA00038522523700000614
式(15)-式(17)中，floor()为向下取整，ceil()为向上取整，参数a1、a2、a3和a4取固定的参数，且满足a1+a2+a3＝1；
步骤3.2.5、初始化i＝1；
步骤3.2.6、初始化j＝1；
步骤3.2.7、当i<n1时，利用式(18)更新排序后的第t代种群的第一层粒子i的速度
Figure FDA00038522523700000615
当i<n1+n2时，利用式(19)更新排序后的第t代种群的第二层粒子i的速度
Figure FDA00038522523700000616
否则，利用式(20)更新排序后的第t代种群的第三层粒子i的速度
Figure FDA00038522523700000617
Figure FDA00038522523700000618
Figure FDA00038522523700000619
Figure FDA00038522523700000620
式(18)-式(20)中，
Figure FDA00038522523700000621
表示第t-1代第i个粒子的第j个路径点的位置，α1表示从第一层随机选取的粒子的序号，
Figure FDA00038522523700000622
表示第t代第α1个粒子的第j个路径点的位置，α2表示从第二层随机选取的粒子的序号，
Figure FDA0003852252370000071
表示第t代第α2个粒子的第j个路径点的位置，
Figure FDA0003852252370000072
表示第t-1代第i个粒子的第j个路径点的速度，
Figure FDA0003852252370000073
表示第t代第i个粒子的第j个路径点的速度，
Figure FDA0003852252370000074
表示第t-1代第i个粒子的第j个路径点的局部最优解，
Figure FDA0003852252370000075
表示第t-1代的第j个路径点的全局最优解，wt表示第t代种群的惯性权重，
Figure FDA0003852252370000076
和
Figure FDA0003852252370000077
为第t次迭代(0,1)之间的随机数；
步骤3.2.8、利用式(21)计算第t代第i个粒子的新位置
Figure FDA0003852252370000078
Figure FDA0003852252370000079
式(21)中，
Figure FDA00038522523700000710
表示排序后的第t代种群中第i个粒子的第j个路径点的位置；
步骤3.2.9、将j+1赋值给j，判断j≤N是否成立，若成立，则执行步骤3.2.7；否则，表示完成对排序后的第t代种群中第i个粒子的迭代；
步骤3.2.10、利用式(9)计算第t代种群中第i个粒子的适应度值；
步骤3.2.11、比较第t代种群和第t-1代种群中第i个粒子的适应度值，选择具有较小适应度值的第i个粒子的位置作为第t代种群第i个粒子的局部最优解
Figure FDA00038522523700000711
步骤3.2.12、将i+1赋值给i，判断i≤M是否成立，若成立，则执行步骤3.2.6；否则，表示完成对排序后的第t代种群的迭代，并获得第t代种群的全局最优解为
Figure FDA00038522523700000712
步骤3.2.13、将t+1赋值给t，判断t≤tmax是否成立，若成立，则执行步骤3.2.1；否则，表示获得第tmax代种群的全局最优解
Figure FDA00038522523700000713
并以全局最优解
Figure FDA00038522523700000714
对应的路径为最佳路径。
2.一种电子设备，包括存储器以及处理器，其特征在于，所述存储器用于存储支持处理器执行权利要求1所述的无人机路径规划方法的程序，所述处理器被配置为用于执行所述存储器中存储的程序。
3.一种计算机可读存储介质，计算机可读存储介质上存储有计算机程序，其特征在于，所述计算机程序被处理器运行时执行权利要求1所述的无人机路径规划方法的步骤。
CN202211138201.9A 2022-09-19 2022-09-19 一种基于分层粒子群算法的无人机路径规划方法及其应用 Pending CN115357050A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211138201.9A CN115357050A (zh) 2022-09-19 2022-09-19 一种基于分层粒子群算法的无人机路径规划方法及其应用

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211138201.9A CN115357050A (zh) 2022-09-19 2022-09-19 一种基于分层粒子群算法的无人机路径规划方法及其应用

Publications (1)

Publication Number Publication Date
CN115357050A true CN115357050A (zh) 2022-11-18

Family

ID=84005838

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211138201.9A Pending CN115357050A (zh) 2022-09-19 2022-09-19 一种基于分层粒子群算法的无人机路径规划方法及其应用

Country Status (1)

Country Link
CN (1) CN115357050A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115576333A (zh) * 2022-12-08 2023-01-06 青岛科技大学 最优向避障策略

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115576333A (zh) * 2022-12-08 2023-01-06 青岛科技大学 最优向避障策略
CN115576333B (zh) * 2022-12-08 2023-05-05 青岛科技大学 最优向避障策略

Similar Documents

Publication Publication Date Title
CN110632931B (zh) 动态环境下基于深度强化学习的移动机器人避碰规划方法
CN111240319B (zh) 室外多机器人协同作业系统及其方法
CN111880561B (zh) 城市环境下基于改进鲸鱼算法的无人机三维路径规划方法
CN112130581B (zh) 一种面向空中机动作战的无人机集群协同任务规划方法
CN110134140B (zh) 一种环境信息未知连续状态下基于势函数奖赏dqn的无人机路径规划方法
CN107607120B (zh) 基于改进修复式Anytime稀疏A*算法的无人机动态航迹规划方法
CN112747736B (zh) 一种基于视觉的室内无人机路径规划方法
CN112068588A (zh) 一种基于飞行走廊和贝塞尔曲线的无人飞行器轨迹生成方法
CN109871021B (zh) 一种基于粒子群优化算法的机器人导航方法
CN101118622A (zh) 在城市环境下微型飞行器三维轨迹仿真方法
CN108919818B (zh) 基于混沌种群变异pio的航天器姿态轨道协同规划方法
CN112327939B (zh) 一种城市街区环境下高层消防多无人机协同路径规划方法
CN113848919A (zh) 一种基于蚁群算法的室内agv路径规划方法
CN114840020A (zh) 一种基于改进鲸鱼算法的无人机飞行轨迹规划方法
CN112161627A (zh) 一种消防机器人智能路径规划方法
CN115903879A (zh) 基于地形数据插值技术的无人机航迹规划方法
CN115357050A (zh) 一种基于分层粒子群算法的无人机路径规划方法及其应用
CN115903888A (zh) 一种基于天牛群算法的旋翼无人机自主路径规划方法
CN112733251A (zh) 一种多无人飞行器协同航迹规划方法
CN113220008B (zh) 多火星飞行器的协同动态路径规划方法
Kareem et al. Planning the Optimal 3D Quadcopter Trajectory Using a Delivery System-Based Hybrid Algorithm.
CN115840465A (zh) 一种基于粒子群算法的无人机集群航迹规划方法
CN114777789A (zh) 一种基于球面矢量鸽群算法的三维避障航路规划方法
CN113311864B (zh) 一种栅格尺度自适应的多无人机协同搜索方法
CN111896001A (zh) 一种三维蚁群航迹优化方法

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