基于人工势场算法的复杂环境路径规划优化系统及方法与流程

本发明属于三维航道控制领域，具体来说，特别涉及基于人工势场算法的复杂环境路径规划优化系统及方法。

背景技术：

1、复杂环境路径规划是指在环境多变、存在不确定性和障碍物的情况下，为移动机器人或无人机等自主导航系统确定一条有效路径的过程；复杂环境可能包括拥挤的城市街道、不规则的室内空间、未知的地形或其他对导航有限制的区域；路径规划的目标是找到一条从起点到目标点的路径，同时最小化成本（如时间、能量消耗或路径长度），并确保路径上的安全性，避免碰撞或陷入不可行区域。

2、中国专利cn115686068a公开了一种多威胁环境下的协同路径规划方法，该方法包括：根据无人机集群中每架无人机的起点位置和终点位置规划所有可行的威胁规避路径；根据期望路径长度，在每架无人机的可行的威胁规避路径中确定基础路径，并对所确定的基础路径进行优化调整，使优化后的基础路径的长度与期望路径长度的优化后长度误差最小，并将优化后长度误差从低到高排列的前n条优化后的基础路径作为优化路径集；对每架无人机的优化路径集进行碰撞检测，并将检测到的无碰撞的优化路径作为协同路径。由此，可以使无人机集群在具有威胁规避功能的条件下，生成几乎等长的协同路径，使无人机集群在巡航突防末端满足一定的时空约束条件。

3、针对复杂环境下的路径规划问题，由于环境的复杂性，对于路径规划的影响因素大大增加，目前在进行路径规划时往往只考虑到现存的一些影响障碍，然而，在复杂环境下的外界影响因素时随着时间不断变化，若不能提前对这些即将发生的影响因素进行预测，会使得行进装置在规划好的路径上受到影响。

技术实现思路

1、针对相关技术中的问题，本发明提出基于人工势场算法的复杂环境路径规划优化系统及方法，以克服现有相关技术所存在的上述技术问题。

2、为解决上述技术问题，本发明是通过以下技术方案实现的：

3、本发明为基于人工势场算法的复杂环境路径规划优化方法，包括以下步骤：

4、s1、设定路径规划总区域、路径起点坐标以及路径终点坐标并对所述路径规划总区域中所有的静止障碍物的坐标进行采集，得到静止障碍物坐标集；设定当前运动位置坐标并根据所述当前运动位置坐标、路径终点坐标构建引力场函数；再根据所述当前运动位置坐标以及静止障碍物坐标集构建斥力场函数；最后根据所述斥力场函数以及引力场函数构建合力函数；

5、s2、对所述路径规划总区域进行子区域划分，得到路径规划子区域集；并设定行进影响因素集以及采集时间点集；根据所述行进影响因素集以及采集时间点集对在各个路径规划子区域出现的行进影响因素集中的行进影响因素数据进行采集，得到行进影响因素历史数据矩阵；再根据所述行进影响因素历史数据矩阵对未来时间点行进影响因素出现的数据进行预测，得到行进影响因素预测数据矩阵；

6、s3、对所述路径起点坐标和路径终点坐标之间的待选路径进行优化，最终得到最优路径；

7、通过设定路径规划总区域、路径起点坐标以及路径终点坐标并对所述路径规划总区域中所有的静止障碍物的坐标进行采集，可以得到路径规划区域中的障碍物的分布情况，可以对区域中的各个静止的障碍物进行人工势场建模，即构建出每个障碍物对行进装置的吸引力函数、排斥力函数以及合力函数，通过采用人工势场算法对路径上的障碍物进行建模，只需对行进装置受到的合力这一单个因素进行考虑，为后续对行进装置的行进路径优化目标进行了确定；使得后续的对最优路径的分析过程更加简单和方便；同时，由于行进装置在路径规划区域中行进时，除了道路上的障碍物的影响外，还会受到多种影响因素的影响，因此本方案中对多种除静止障碍物之外的影响因素进行全面分析，预测出这些动态影响因素在未来时刻行进装置行进时的发生状态，从而提前对这些影响因素的发生情况进行了预测，从而为最优路径的选择提供了又一个的依据，使得行进装置在最优路径上进行行驶时更加顺利，保证了行进的效率以及稳定性。

8、优选地，所述s1包括以下步骤：

9、s11、设定路径规划总区域、路径起点坐标为以及路径终点坐标为；、和分别表示路径起点所在位置的经度值、纬度值以及高度值；、和分别表示路径终点的经度值、纬度值以及高度值；

10、再采用监控设备对所述路径规划总区域中所有的静止障碍物的坐标进行采集，得到静止障碍物坐标集；、和分别表示所述路径规划总区域中第个静止障碍物所在位置的经度值、纬度值以及高度值，表示所述路径规划总区域中静止障碍物的总个数；

11、s12、设定当前运动位置坐标为，、和分别表示当前运动位置的经度值、纬度值以及高度值；

12、根据所述当前运动位置坐标、路径终点坐标构建引力场函数；如下，

13、；

14、式中，表示所述当前运动位置处受到的吸引力大小；表示引力尺度因子；表示当前运动位置与路径终点所在位置之间的距离；

15、根据所述当前运动位置坐标以及静止障碍物坐标集构建斥力场函数；如下，

16、；

17、式中，表示当前运动位置与所述路径规划总区域中第个静止障碍物所在位置之间的距离；表示所述当前运动位置处受到的排斥力大小；表示斥力尺度因子；表示为障碍物与当前运动位置之间的最小安全距离；

18、根据所述斥力场函数以及引力场函数构建合力函数；如下，

19、；

20、式中，表示所述当前运动位置处受到的合力大小；

21、通过对区域中的各个静止的障碍物进行人工势场建模，即构建出每个障碍物对行进装置的吸引力函数、排斥力函数以及合力函数，通过采用人工势场算法对路径上的障碍物进行建模和计算，为后续对行进装置的行进路径优化提供了依据。

22、优选地，所述s2包括以下步骤：

23、s21、对所述路径规划总区域进行子区域划分，得到路径规划子区域集，表示划分出的第个子区域，表示划分处的子区域的总个数；再设定行进影响因素集，表示设定的第个行进影响因素，表示设定的行进影响因素的总个数；

24、设定历史行进影响因素数据采集周期，对所述历史行进影响因素数据采集周期进行时间点划分，得到采集时间点集，表示对所述历史行进影响因素数据采集周期划分出的第个时间点，表示划分的采集时间点的总个数；

25、s22、在所述采集时间点集中各个时间点对在各个路径规划子区域出现的行进影响因素集中的行进影响因素数据进行采集，得到行进影响因素历史数据矩阵；如下，

26、；

27、其中，表示在第个时间点对第个路径规划子区域出现的行进影响因素的数据集；，表示中的第个行进影响因素对应的数据；

28、s23、设定训练数据比例为以及测试数据比例为；根据所述训练数据比例以及测试数据比例对所述行进影响因素历史数据矩阵进行数据划分，得到历史训练数据矩阵、历史测试数据矩阵以及历史待预测数据矩阵；分别如下，

29、；；；

30、其中，表示历史训练数据矩阵中在第个时间点对第个路径规划子区域出现的行进影响因素的数据集；表示历史测试数据矩阵中在第个时间点对第个路径规划子区域出现的行进影响因素的数据集；表示历史待预测数据矩阵中在第个时间点对第个路径规划子区域出现的行进影响因素的数据集；、和分别表示所述历史训练数据矩阵、历史测试数据矩阵以及历史待预测数据矩阵中划分的时间点的总个数；

31、s24、设定未来预测时间点集，表示设定的未来预测的第个时间点，表示设定的未来预测时间点的总个数；根据所述历史训练数据矩阵、历史测试数据矩阵以及历史待预测数据矩阵对未来时间点行进影响因素出现的数据进行预测，得到行进影响因素预测数据矩阵；如下，

32、；

33、其中，表示预测得到的未来第个时间点在第个路径规划子区域出现的行进影响因素的数据集；，表示中的第个行进影响因素对应的数据；

34、通过设定采集时间点以及行进影响因素集，不仅充分考虑了多种对行进装置行进会造成影响的因素，也为后续对行进影响因素数据的采集提供了依据；再通过采集行进影响因素数据，为后续对未来的行进影响因素数据进行预测提供了数据支持。

35、优选地，所述s24包括以下步骤：

36、s241、构建初始bp神经网络模型；将所述历史训练数据矩阵输入至初始bp神经网络模型中对初始bp神经网络模型进行训练操作，得到训练好的bp神经网络模型；

37、s242、采用历史测试数据矩阵对所述训练好的bp神经网络模型进行测试，测试完成后，得到最终bp神经网络模型；

38、s243、采用所述最终bp神经网络模型根据历史待预测数据矩阵对未来时间的行进影响因素数据进行预测，得到所述行进影响因素预测数据矩阵；

39、采用bp神经网络模型对未来时刻的行进影响因素数据进行预测，便于后续对路径进行优化。

40、优选地，所述s241包括以下步骤：

41、s2411、构建初始bp神经网络模型，设定所述初始bp神经网络模型的输入节点的个数为、隐节点的个数为以及输出节点的个数为；

42、同时设定所述初始bp神经网络模型的初始权重为以及初始阈值为；

43、s2412、设定训练误差阈值为；将所述历史训练数据矩阵输入至初始bp神经网络模型中对初始bp神经网络模型进行训练操作，对所述初始权重以及初始阈值进行调优；当训练过程中的训练误差小于或者等于时，停止训练，得到所述训练好的bp神经网络模型；

44、所述s242包括以下步骤：

45、s2421、将所述历史测试数据矩阵等分为两份，得到第一历史测试数据矩阵和第二历史测试数据矩阵；

46、s2422、将所述第一历史测试数据矩阵输入至训练好的bp神经网络模型中进行预测，得到历史测试预测数据矩阵；

47、设定准确度阈值；将所述历史测试预测数据矩阵与第二历史测试数据矩阵进行对比计算准确度；当所述准确度大于或者等于准确度阈值时，将所述训练好的bp神经网络模型作为最终bp神经网络模型；

48、否则，重复s2421和s2422，直到计算得到的准确度大于或者等于准确度阈值为止；

49、通过对初始bp神经网络模型进行训练和测试，使得bp神经网络模型对行进影响因素数据具有良好的预测效果以及准确性，进而提高了后续对行进路径进行优化的准确性。

50、优选地，所述s3包括以下步骤：

51、s31、构建海鸥种群，设定所述海鸥种群的规模为，所述海鸥种群表示为，表示所述海鸥种群中的第只海鸥；设定所述海鸥种群的最大迭代次数为、当前迭代次数为以及初始步长为；

52、s32、根据所述路径起点坐标、静止障碍物坐标集、路径终点坐标为以及初始步长设定第一初始路径位置点矩阵；如下，

53、；

54、其中，、和分别表示第条初始路径上的第个位置点的经度值、纬度值以及高度值；表示第条初始路径上除位置点以及路径终点外的位置点的总个数；所述第一初始路径位置点矩阵中每行相邻的两个位置点之间的欧式距离小于初始步长；

55、将所述第一初始路径位置点矩阵的每行位置点的坐标数据作为海鸥种群中每只海鸥的初始位置；

56、s33、根据所述行进影响因素预测数据矩阵对第一初始路径位置点矩阵中的点进行替换；得到第二初始路径位置点矩阵；如下，

57、；

58、其中，、和分别表示进行替换后的第条初始路径上的第个位置点的经度值、纬度值以及高度值；

59、s34、根据所述第二初始路径位置点矩阵、合力函数构建海鸥种群的适应度函数；

60、s35、开始迭代操作，每轮迭代过程中根据所述适应度函数对海鸥种群中每只海鸥的位置进行更新，同时在每轮迭代过程中计算全局最优海鸥个体并将其作为下一轮迭代对每只海鸥的位置进行更新的最优位置；

61、当时，输出最优路径；

62、通过采用海鸥算法对行进路径上的位置点进行优化以及调整，同时配合根据人工势场算法对路径上的障碍物进行建立的吸引力、排斥力以及合力函数模型，同时在进行优化之前根据行进影响因素数据的预测结果对初始随机选取的点进行筛选和替换以及考虑了路径的总长度，通过从这三个方面对路径进行优化，使得路径的优化更加准确。

63、优选地，所述s33包括以下步骤：

64、s331、根据所述行进影响因素集设定对应的行进影响因素影响权重值，得到行进影响因素权重数据矩阵，表示第个行进影响因素对应的权重数据；

65、根据所述行进影响因素权重数据矩阵和行进影响因素预测数据矩阵计算得到行进影响因素预测影响数据矩阵；如下，

66、；

67、其中，表示预测得到的未来第个时间点在第个路径规划子区域出现的行进影响因素的数据集对应的影响数据；；

68、s332、设定行进影响数据阈值；对所述第一初始路径位置点矩阵中每行的位置点进行遍历，当在未来第个时间点行进至第个路径规划子区域的范围内且，则将第一初始路径位置点矩阵中该行中位于第个路径规划子区域内的位置点进行删除，再重新随机选取其他位置点进行替换；遍历完成后得到所述第二初始路径位置点矩阵；

69、通过计算影响数据，为路径上的位置点进行排除和替换提供了判断依据。

70、优选地，所述s34包括以下步骤：

71、s341、采用所述合力函数计算第二初始路径位置点矩阵中各个位置点的合力，得到合力数据矩阵；如下，

72、；

73、其中，表示进行替换后的第条初始路径上的第个位置点所受到的合力数据；

74、再根据所述第二初始路径位置点矩阵计算每条初始路径的总长度值，得到初始总长度数据集，表示第条初始路径的总长度；

75、s342、根据所述初始总长度数据集以及合力数据矩阵构建适应度函数；如下，

76、；

77、通过确定适应度函数，确定优化目标，使得优化更加具有方向性。

78、一种基于人工势场算法的复杂环境路径规划优化系统，包括静止障碍物坐标采集模块、合力函数构建模块、行进影响因素数据采集模块、行进影响因素数据预测模块和路径优化模块；

79、所述静止障碍物坐标采集模块用于对所述路径规划总区域中所有的静止障碍物的坐标进行采集，得到静止障碍物坐标集；

80、所述合力函数构建模块用于根据所述当前运动位置坐标、路径终点坐标构建引力场函数；再根据所述当前运动位置坐标以及静止障碍物坐标集构建斥力场函数；最后根据所述斥力场函数以及引力场函数构建合力函数；

81、所述行进影响因素数据采集模块用于根据所述行进影响因素集以及采集时间点集对在各个路径规划子区域出现的行进影响因素集中的行进影响因素数据进行采集，得到行进影响因素历史数据矩阵；

82、所述行进影响因素数据预测模块用于再根据所述行进影响因素历史数据矩阵对未来时间点行进影响因素出现的数据进行预测，得到行进影响因素预测数据矩阵；

83、所述路径优化模块用于采用海鸥种群并配合合力函数对所述路径起点坐标和路径终点坐标之间的待选路径进行优化，最终得到最优路径。

84、本发明具有以下有益效果：

85、1.本发明中通过设置静止障碍物坐标采集模块、合力函数构建模块、行进影响因素数据采集模块、行进影响因素数据预测模块和路径优化模块；通过设定路径规划总区域、路径起点坐标以及路径终点坐标并对所述路径规划总区域中所有的静止障碍物的坐标进行采集，可以得到路径规划区域中的障碍物的分布情况，可以对区域中的各个静止的障碍物进行人工势场建模，即构建出每个障碍物对行进装置的吸引力函数、排斥力函数以及合力函数，通过采用人工势场算法对路径上的障碍物进行建模，只需对行进装置受到的合力这一单个因素进行考虑，为后续对行进装置的行进路径优化目标进行了确定；使得后续的对最优路径的分析过程更加简单和方便；同时，由于行进装置在路径规划区域中行进时，除了道路上的障碍物的影响外，还会受到多种影响因素的影响，因此本方案中对多种除静止障碍物之外的影响因素进行全面分析，预测出这些动态影响因素在未来时刻行进装置行进时的发生状态，从而提前对这些影响因素的发生情况进行了预测，从而为最优路径的选择提供了又一个的依据，使得行进装置在最优路径上进行行驶时更加顺利，保证了行进的效率以及稳定性。

86、2.本发明中通过设定采集时间点以及行进影响因素集，不仅充分考虑了多种对行进装置行进会造成影响的因素，也为后续对行进影响因素数据的采集提供了依据；再通过采集行进影响因素数据，为后续对未来的行进影响因素数据进行预测提供了数据支持。

87、3.本发明中采用bp神经网络模型对未来时刻的行进影响因素数据进行预测，便于后续对路径进行优化，其中，通过对初始bp神经网络模型进行训练和测试，使得bp神经网络模型对行进影响因素数据具有良好的预测效果以及准确性，进而提高了后续对行进路径进行优化的准确性；其次通过采用海鸥算法对行进路径上的位置点进行优化以及调整，同时配合根据人工势场算法对路径上的障碍物进行建立的吸引力、排斥力以及合力函数模型，同时在进行优化之前根据行进影响因素数据的预测结果对初始随机选取的点进行筛选和替换以及考虑了路径的总长度，通过从这三个方面对路径进行优化，使得路径的优化更加准确。

88、当然，实施本发明的任一产品并不一定需要同时达到以上所述的所有优点。