基于粒子群算法的城市多障碍物环境下无人机路径规划方法

本发明属于无人机路径规划领域，具体涉及一种基于粒子群算法的城市多障碍物环境下无人机路径规划方法，用于实现在充满障碍物的城市环境中为无人机规划一条满足曲率约束的可执行飞行路径。

背景技术：

1、无人机路径规划技术可以有效地帮助无人机实现自主飞行和自主决策，这不仅是无人机发展的基础，更是自主无人机工程应用的核心挑战。先进的无人机路径规划技术，将为实现高效、可靠的自动驾驶提供强大的支撑。然而，无人机路径规划是一个极具挑战性的任务，必须综合考量多种外部影响因素，如环境影响、能源消耗、航空成本、环境中的不确定性等。通常，为了确保无人机的安全，在设计无人机路线时应尽量避开其飞行环境中的障碍物，并且尽量降低无人机的消耗。

2、无人机全局路径规划旨在根据已知的地理环境、障碍物和威胁等因素，确定一条能够满足无人机需求的最优飞行路线。全局路径规划可以保证无人机的安全性和可靠性，并且能够提高任务的完成效率。局部路径规划在当无人机遭遇突发情况也能作出及时应对，比如恶劣的天气，而不得不改变原有的航线时，就会面对风险及时作出调整。传统无人机路径规划问题已有多种规划算法，例如遗传算法、快速搜索随机树算法、a*算法、概率路图法等。每个算法有各自的特点和适用范围，例如地图的数据存储结构直接影响算法的选取。

3、城市环境的构成要素错综复杂，纷繁多样，并且城市内部，高楼林立，建筑密集，市区道路纵横交错，对无人机的通行会产生很大的影响。根据城市环境的特点，本技术采用粒子群算法并且考虑了多方面的约束，为无人机进行高效地路径规划。

技术实现思路

1、本发明解决了城市多障碍环境中无人机的路径规划问题，先对城市环境中的建筑物进行预处理得到障碍物区域，然后加载初始点和目标点，并在初始点和目标点之间生成一定数量的随机点，紧接着利用粒子群算法对生成的随机点位置进行优化，最终生成的路径需满足无人机曲率约束，最后生成无人机可执行的路径。

2、本发明的技术解决方案是：

3、基于粒子群算法的城市多障碍物环境下无人机路径规划方法，其特征在于该方法的步骤包括：

4、步骤1，对城市环境中的建筑物进行预处理，预处理的具体方法为：从俯视的角度将城市环境中的建筑物处理为多边形，对得到多边形求外接圆，对得到的外接圆进行膨胀处理，得到该城市环境中的障碍物区域；

5、步骤2，基于粒子群算法生成无人机路径规划的多个初始个体，第一个初始个体的生成方法为：

6、在城市环境中加载初始点nodestart和目标点nodeend，并在初始点nodestart和目标点nodeend之间生成设定数量等间距的点pk，其中k＝1,2,…,nmiddle，等间距点的数量为nmiddle，按照顺序依次连接初始点nodestart、等间距的点pk以及目标点nodeend得到第一个初始个体，其中nmiddle的取值范围为5-10；

7、剩余初始个体的具体生成方法为：

8、按照顺序依次连接初始点nodestart、点qk以及目标点nodeend得到初始剩余个体，点qk为在城市环境内随机生成的点；

9、步骤3，对步骤2中生成的多个初始个体的速度和位置进行更新，得到更新后的多个初始个体的速度和位置；

10、步骤4，当更新后的初始个体位置坐标超出了城市环境的边界时，则将超出边界的初始个体的位置倒退至城市环境的边界内；

11、步骤5，对步骤2中个体的位置进行优化，重复步骤3-步骤4，根据适应值函数对优化后的结果进行评价，根据评价结果得到无人机曲率约束的优化路径。

12、所述步骤1中，对得到的多边形求外接圆的方法为：

13、先获取多边形的所有顶点，nodei表示第i个顶点，i＝1,2,…,i；

14、求任意两个顶点之间的距离，dij表示nodei和nodej之间的距离；

15、求最大的两顶点之间的距离，公式如下所示：

16、dmax＝max dij,i,j∈i  (1)

17、nodei和nodej的坐标分别是(xi,yi)和(xj,yj)；

18、外接圆的圆心nodeij坐标为(0.5(xi+xj),0.5(yi+yj))，外接圆的半径rij为0.5dmax。

19、所述步骤1中，对得到的外接圆进行膨胀处理的方法为：

20、将外接圆进行膨胀，膨胀的宽度为无人机的安全飞行距离，用dsafe进行表示，此时外接圆的圆心保持不变，半径更新为rij*＝0.5dmax+dsafe，得到该城市环境障碍物区域。

21、所述步骤2中，等间距的点pk的生成方法为：

22、生成等间距步长，具体方法为：设初始点nodestart坐标为(xstart,ystart)，目标点nodeend坐标为(xend,yend)，等间距的步长在横纵坐标上分别用和表示，求解公式如下所示：

23、

24、

25、等间距的点pk表示为：

26、

27、

28、

29、所述步骤3中，对初始个体的速度和位置进行更新的方法为：

30、在粒子群算法中，xh＝(xh1,xh2,…,xhd,…,xhn)表示第h个粒子的位置，vh＝(vh1,vh2,…,vhd,…,vhn)表示第h个粒子的速度，ph＝(ph1,ph2,…,phd,…,phn)表示第h个粒子搜寻到的最好位置，g＝(g1,g2,…,gd,…,gn)表示整个群组搜寻到的最好位置。其中，h＝1,2,3,…,m表示粒子个数，xhd表示第h个粒子第d个中间点的位置，vhd表示第h个粒子第v个中间点的速度，phd表示第h个粒子第d个中间点搜寻到的最好位置，gd表示整个种群第d个中间点的全局最优位置；

31、粒子的速度和位置的更新公式如下所示：

32、vhd(t+1)＝vhd(t)+c1r1(phd(t)-xhd(t))+c2r2(gd(t)-xhd(t)) (4)

33、xhd(t+1)＝xhd(t)+vhd(t+1) (5)

34、其中，h＝1,2,…,m，d＝1,2,…,n，c1和c2为学习因子，t表示迭代次数，r1和r2为在[0,1]上服从均匀分布的随机数。并且n＝nmiddle。

35、所述步骤4中，将超出边界的初始个体的位置倒退至城市环境的边界内的方法为：

36、对粒子的速度进行限定，公式如下所示：

37、vhd(t+1)>vmax→vhd(t+1)＝vmax (6)

38、vhd(t+1)<-vmax→vhd(t+1)＝-vmax (7)

39、其中，vmax表示粒子的最大速度。

40、所述步骤5中，对点pk和qk的位置进行优化的方法为：

41、(1)设置最大迭代次数、种群大小npop、个体学习率c1、群体学习率c2、粒子的最大速度vmax参数；

42、(2)剩余个体随机生成，随机点noderand＝(xrand,yrand)即qk，生成公式如下所示：

43、xrand＝xmin+(xmax-xmin)×rand (8)

44、yrand＝ymin+(ymax-ymin)×rand (9)

45、其中，rand∈[0,1]

46、(3)根据公式(4)和公式(5)对粒子的速度和位置进行更新；

47、(4)根据公式(6)和公式(7)对粒子越界情形进行处理；

48、(5)计算并更新每个粒子的最好位置；

49、(6)计算并更新整个种群或者邻域内最好的位置；

50、(7)如果没有达到终止条件，终止条件即迭代次数达到设定的最大迭代次数或者最优解不再发生变化，转到步骤(3)到(6)；否则停止算法，输出当前最好结果。

51、所述步骤5中，对优化后的结果进行评价的方法为：

52、计算每个粒子的适应度函数值，适应度函数如下公式所示：

53、f＝d×(1+β×v)(10)

54、其中，d表示路径总长度，v表示总约束违反量，β表示权重系数；

55、路径总长度求解公式如下所示：

56、

57、其中，d(start,1)表示初始点和第一个中间的之间的距离，d(1,2)表示第一个中间点和第二个中间的之间的距离，表示最后一个中间的和目标点的距离；

58、总体约束违反量求解公式如下所示：

59、

60、其中，nobs为障碍物的数量，k＝1,2,…,nobs，vk表示第k个障碍物上的约束违反量，障碍物即为步骤1中对城市环境中的建筑物进行预处理后得到的外接圆膨胀后得到的圆形区域；

61、针对每个障碍物的约束违反量求解公式如下所示：

62、

63、其中，d(k,1)表示第k个障碍物的圆心和第一个中间点的距离，第k个障碍物的圆心和最后一个中间点的距离。

64、本发明的有益效果是：

65、第一、本发明提供了一种基于粒子群算法的城市多障碍物环境下无人机路径规划方法，解决了无人机在多障碍物环境中满足曲率约束的路径规划问题；

66、第二、本发明对城市环境进行了预处理，先将城市环境中的障碍物进行外接圆处理得到圆形障碍物，再对圆形障碍物进行膨胀处理得到最终的障碍物，膨胀的宽度为无人机的安全飞行距离；

67、第三、本发明加载城市环境和初始点、目标点，并在初始点和目标点之间生成一定数量等间距的点和一定数量的随机点，紧接着利用粒子群算法对生成的随机点位置进行优化，最终生成的路径需满足无人机曲率约束，最后生成无人机可执行的路径。