一种基于PSO-MPC融合的多智能体路径规划方法

本发明涉及人工智能路径规划，尤其是一种基于pso-mpc融合的多智能体路径规划方法。

背景技术：

0、技术背景

1、路径规划是人工智能系统自主导航的关键环节之一，同时也是人工智能系统领域的研究热点。路径规划目的主要是让智能体在规定范围的区域内找到一条从起点到终点的路径，而且这条路径没有和中途的障碍物发生碰撞。路径规划以及轨迹跟踪的核心就是算法的设计，目前常见的路径规划和轨迹跟踪算法有：a*算法、dijkstra算法、rrt算法、遗传算法、粒子群算法、动态窗口算法、pid控制算法、stanley算法、lqr算法、mpc算法等。

2、粒子群算法是一种基于群体智能的优化算法，模拟鸟群或鱼群等群体的觅食行为。在粒子群算法中，每个“粒子”代表一个潜在的解，并且在搜索空间中移动以寻找最优解。粒子群算法的基本思想是通过模拟鸟群或鱼群的行为，每个粒子根据自身的经验和群体的经验来调整自己的位置和速度。在搜索过程中，粒子会记录自己的最佳位置(个体最优解)和整个群体的最佳位置(全局最优解)，并根据这些信息来更新自己的速度和位置。最终，选择一条整个群体的最佳位置，即距离最短的路径。粒子算法是一种全局的群智能算法，具有简单易实现、具有较好的全局搜索能力，能够快速收敛到全局最优解等优点。

3、mpc算法可以应用于控制系统对目标轨迹的跟踪。mpc算法通过预测未来一段时间内系统的行为，以及基于这些预测结果来生成控制输入，从而实现对目标轨迹的跟踪。mpc算法在轨迹跟踪方面的一般步骤：1、建立模型；2、预测参考轨迹；3、滚动优化；4、误差补偿；从而实现对pso算法形成的全局路径规划进行跟踪。mpc算法具有较强的预测性、优化性能。

4、但是在多智能体路径规划和轨迹跟踪中，传统pso算法和mpc算法存在以下不足：

5、(1)传统pso群算法中由于没有导向功能容易使粒子陷入局部最优。

6、(2)在传统pso算法中，粒子运动速度中的权重因子对其速度的影响是固定的，并没有考虑它对其粒子运动的影响。

7、(3)传统pso群算法中并未考虑在形成路径后智能体与已知的障碍物保持安全运行距离。

8、(4)由于传统pso算法一般用于对单智能体的路径规划中，并未涉及到多智能体的路径规划领域中。

9、(5)在传统的pso使形成全局路径规划之后，缺少未对其形成的路径进行跟踪预测。

10、(6)传统的mpc算法通常只是对单智能体的运动轨迹进行跟踪，而缺少对多智能体局部运动的轨迹跟踪。

11、(7)多智能体在对pso算法形成的全局路径实行实时跟踪的过程中对新出现的阻碍原始运行轨迹的未知障碍物并未实行避障策略。

12、(8)多智能体在运动之中和同一起点，不同的智能体之间存在碰撞问题。

技术实现思路

1、为解决上述算法存在的不足，本发明提出一种基于pso-mpc融合的多智能体路径规划方法。

2、为了达到上述目的，本发明提出一种基于pso-mpc融合的多智能体路径规划方法。针对传统pso群算法中由于没有导向功能容易使粒子陷入局部最优，且无效迭代次数过多等问题，本发明引入多目标点以及多目标点运动的导向函数，此函数把原始对粒子的运动速度公式变成多目标点向理论最优运动。该设置能形成智能体的运动路径，从而避免了局部最优问题

3、针对在传统pso算法中，粒子运动速度中的权重因子对其速度的影响是固定的，并没有考虑它对其粒子运动的影响，本发明引入了自适应权重因子函数，使得权重因子随着pso算法根据目标点的具体运行情况来使权重因子递减。

4、针对传统pso群算法中并未考虑在形成路径后智能体与已知的障碍物保持安全运行距离，本发明改进传统pso算法中启适应度函数，在适应度函数中引入了光滑曲线以及安全路径判断，使得形成的路径即平滑又在后续的智能体运行中与已知的障碍物保持安全距离。

5、针对传统pso算法一般用于对单智能体的路径规划中，并未涉及到多智能体的路径规划领域中，本发明给出了智能体群函数。我们根据起始点、终点、任务量来分配种群个数以及每个智能体所中粒子的个数，这样使得单智能体的路径规划变成了多智能体的路径规划。

6、针对在传统的pso使形成全局路径规划之后，缺少未对其形成的路径进行跟踪预测，本文引入了mpc算法对pso算法形成的全局路径进行跟踪。

7、针对传统的mpc算法通常只是对单智能体的运动轨迹进行跟踪，而缺少对多智能体局部运动的轨迹跟踪,本文引入了对多智能体的轨迹跟踪。

8、针对多智能体在对pso算法形成的全局路径实行实时跟踪的过程中对新出现的阻碍原始运行轨迹的未知障碍物并未实行避障策略，本文对不同智能体引入了两种不同的避障方案：一个是给出安全的新预测点；另一个是把原有轨迹平移至可行驶的安全范围内。

9、最后，针对多智能体在运动之中和同一起点，不同的智能体之间存在碰撞问题，本文在初始时刻：路径短的智能体以较快的加速度加速行驶，而令一个智能体则先停止一段时间后在开始运动。在运动途中，当智能体预测不同智能体在一定范围区间有碰撞风险时：到达终点时间短，则一较快速度行驶；而达到终点时间长，则停止或加速行驶，直至另一个智能体驶过碰撞区域为止。

10、本发明采用的技术方案是：

11、首先声明，基于pso-mpc融合的多智能体路径规划算法是基于以下假设进行的：

12、1.本发明将智能体的工作环境简化为一个部分信息已知的空间中的路径规划，将蓝色圆形作为障碍物，将其他非蓝色区域视为智能体可移动的自由空间。

13、2.假设智能体的运动可视为有一定范围的刚体运动

14、一种基pso-mpc融合的多智能体路径规划方法，其主要包括以下步骤：

15、步骤1，环境模型，确定智能体的任务起始点start，最终要到达的任务目标点goal，不同智能体的起始点、终点不一定相同，以及所有障碍物的位置信息。

16、步骤2，利用传统pso算法进行全局路径规划，为后续的多智能体的运动轨迹提供全局路径。

17、步骤3，进行初始迭代次数，粒子个数的设置，引入了根据任务的个数以及大小等确定多智能体的个数以及每个多智能体的种群数。

18、步骤4，加入多目标点的个数，并初始化目标点的位置。初始化目标点的位置后，引入排序函数，按到起始点的距离长短，从小到大进行排序，这样做可以防止形成的路径有交点，而且减少了路径长度，防止智能体在行驶过程中偏离航道。

19、步骤5，由于多目标点的引入，于是把粒子运动的速度公式变成了多目标点的运动，初始化目标点的速度，并引入了导向函数guiden(k)，让目标点向以起始点和终点为直线的这条线靠经。这样使得pso算法形成的路径更短，以及防止陷入局部最优。

20、步骤6，优化权重因子，把固定的权重因子变成递减的权重因子函数。该方法可以使pso算法加快收敛，有效迭代次数增加。

21、步骤7，更新速度和位置，计算适应度函数，并使形成的路径平滑，并加上安全避障以及碰撞检测，为了使得在已知的环境信息中形成的路径能够为后续的智能体局部运动中减少智能体的避障策略，以及因避障而减少能量的消耗。

22、步骤8，pso算法形成路径后再用mpc算法作为智能体的局部运动，本文把用于单智能体的轨迹跟踪推广到多智能体中。

23、步骤9，对多智能体在运动中遇到其他未知物体时，引入识别函数来识别未知物体是为新的未知障碍物还是其他智能体，这样方便设计不同的优化策略。

24、步骤10，在识别出智能体可视范围内的未知物体是障碍物后，本文引入了两种避障策略用于不同的智能体来进行避障。

25、步骤11，在做完避障优化后，下面进行智能体之间的协同优化问题。

26、进一步地，pso-mpc融合的多智能体路径规划中，为了在引入多目标点后，让形成的曲线没有相交的点，在初始化速度、位置等种群信息后，本发明在步骤3中根据任务起始点以及终点，引入智能体群函数，定义式如下：

27、

28、其中，multi_pso为智能体个数。

29、进一步地，为了在引入多目标点后，让形成的曲线没有相交的点，在初始化速度、位置等种群信息后，本发明在步骤4引入了排序函数dai，其表达式为：

30、

31、式中，xsni(k)表示智能体n的第i个目标点，distxsni(k)表示当前目标点的位置到起点之间的距离，即把生成的所有目标点按到起点的距离的大小从小到大排列。

32、进一步地，为了使算法在全局路径规划中更有效的迭代、快速收敛使形成的全局路径更优，本发明在步骤5中引入了目标导向函数，其表达式分别为：

33、

34、guide1n(k)＝goaln-xni(k)

35、其中，startn、goaln分别为第n个智能体的起点、终点，c3、c4表示权重系数，r3、r4表示[0,1]之间的随机数。

36、进一步地，为了使算法在全局路径规划中更有效的迭代、快速收敛使形成的全局路径更优，本发明在步骤6引入了权重因子函数，其表达式为：

37、

38、

39、

40、

41、

42、其中，pbestn(k)局部最优路径，gfitnessn(k)全局最优路径，path1i、path2i分别表示当前目标点分别到起始点和终点的距离。

43、进一步地，为了使得形成的路径平滑且在避开障碍物的同时，还能在智能体进行轨迹跟踪时能够保持其安全性，本发明在步骤7用三次样条曲线形成光滑的曲线，给出安全函数，其定义式如下：

44、rpso_safety＝{1|distpso≤rcar+rob+r}

45、其中，distpso表示障碍物与路径之间的欧式距离，rcar、rob、r分别表示智能体的自身距离、障碍物距离、障碍物与智能体之间的安全距离。

46、进一步地，为了识别检测范围内的未知物体是障碍物还是智能体，本发明在步骤9中引入了智能体识别函数，其表达式为：

47、mpc_qn＝1|mpc_qn≠1，mpc_distjc≤mpcrange

48、式中，mpc_distjc表示智能体检测范围内是否出现未知物体，mpcrange表示智能体检测范围，mpc_qn＝1表示智能体范围内出现的未知物体是智能体，否则是新出现的障碍物。

49、进一步地，为了不同智能体对未知障碍物进行避障，本文在步骤10中引入了障碍物分区函数，对离智能体运动位置最近的障碍物作为障碍物分区的原点，分区函数定义如下：

50、

51、其中，ob_xoy＝1表示在第一象限内，其他类似。

52、进一步地，为了不同智能体对未知障碍物进行避障，并根据分区函数，本文引入多种避障策略：第一种避障策略给出初步预测点，根据实际预测点个数，相关定义如下：

53、

54、

55、pmpcj＝pjmax-pjmin

56、其中，pjmin、pjmax为记录需要预测范围内的最小、最大的位置索引值，pmpcj为需要预测坐标的个数，newobs为对智能体运动有影响的障碍物索引值，mpc_distn(mpc_pni,obmew)为智能体运动点与障碍物的距离。

57、第二种避障策略、平移法，根据第二种分区的情形然后给出平移函数定义如下：

58、

59、其中，mpcy、mpcx——分别为pjmin、pjmax索引值所对应的两坐标的差值，pyixoy表示平移后的新坐标位置。

60、进一步地，为了多智能体之间的协同运作，本文在步骤11中引入了交点函数：

61、mpcdl＝{fid,disth≤0.1}

62、其中，mpcdl为计算并记录不同智能体的参考轨迹之间相交的点；disth为计算两点之间的距离。本文还引入了初始运动函数定义如下：

63、

64、

65、其中，mpccy表示智能体初始运动的关系；mpcvni表示智能体初始运动速度；a1、a2、a3表示不同条件下的加速度。当在运动中检查到其他智能体时，本文引入了信息交流函数，定义如下：

66、

67、

68、其中，mpcdtn表示智能体到交点之间的运动时间，或者如果没有交点则为到终点的运动时间；mpcdist表示智能体运动点到交点的距离，或者如果没有交点，则表示为到终点的距离；mpcvni表示当前时刻的运动速度；mpcghni+1表示为智能体下一时刻的运动速度。

69、本发明采用以上技术方案，在多智能体的pso全局路径规划中，可以规划出一条与障碍物无碰撞且保持着一定安全距离的路径。并且减少了迭代次数以及路径的长度，加快了算法的收敛，而在智能体运动过程中使用mpc算法来对轨迹进行跟踪，并对不同智能体使用了多种避障优化策略。这说明本发明中的方法有效的减少了移动机器人在路径规划中的耗能。