A multi-agent path planning method based on PSO-MPC fusion

Abstract

The application relates to a multi-agent path planning method based on PSO-MPC fusion and relates to the technical field of artificial intelligence path planning. Multi-agent path planning is a challenging task, which involves shortening path length and keeping safe driving distance. A multi-agent path planning algorithm based on the fusion of PSO and MPC is provided in the paper. In the algorithm, firstly, the PSO algorithm is used to plan a global path, and a multi-target point guiding function and a weight factor adaptive function are introduced to reduce the path length. Secondly, the global path is used as a reference trajectory for real-time tracking of MPC, and a safe prediction point and a control agent function driving speed are introduced to keep the safety of agent driving. Finally, compared with traditional PSO, MOPSO and other algorithms such as the ant colony algorithm in the simulation experiment, the global path formed by the algorithm is shorter in length, faster in convergence speed, the agent is located in the safe area at any time, and the path formed is smoother.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of artificial intelligence path planning, in particular to a multi-agent path planning method based on PSO-MPC fusion. TECHNICAL BACKGROUND

[0002] Path planning is one of the key links of autonomous navigation of artificial intelligence systems, and is also a research hotspot in the field of artificial intelligence systems. The main purpose of path planning is to find a path from the starting point to the end point in a specified range of area, and the path does not collide with the obstacles on the way. The core of path planning and trajectory tracking is the design of algorithm. The common path planning and trajectory tracking algorithms at present include A* algorithm, Dijkstra algorithm, RRT algorithm, genetic algorithm, particle swarm optimization algorithm, dynamic window algorithm, PID control algorithm, Stanley algorithm, LQR algorithm and MPC algorithm.

[0003] Particle swarm optimization algorithm is a kind of optimization algorithm based on swarm intelligence, which simulates the foraging behavior of bird swarm or fish swarm. In particle swarm optimization algorithm, each "particle" represents a potential solution and moves in the search space to find the optimal solution. The basic idea of particle swarm optimization algorithm is to simulate the behavior of bird swarm or fish swarm, and each particle adjusts its position and speed according to its own experience and the experience of the whole group. In the search process, the particle records its best position (individual optimal solution) and the best position of the whole group (global optimal solution), and updates its speed and position according to these information. Finally, the best position of the whole group is selected, that is, the shortest path. Particle swarm optimization algorithm is a global swarm intelligence algorithm, which has the advantages of simple implementation, good global search ability and fast convergence to global optimal solution.

[0004] MPC algorithm can be applied to the tracking of target trajectory in control system. MPC algorithm predicts the behavior of system in future period of time, and generates control input based on these predictions, so as to realize the tracking of target trajectory. The general steps of MPC algorithm in trajectory tracking are: 1, establishing model; 2, predicting reference trajectory; 3, rolling optimization; 4, error compensation; so as to realize the tracking of global path planning formed by PSO algorithm. MPC algorithm has strong predictability and optimization performance.

[0005] However, in multi-agent path planning and trajectory tracking, the traditional PSO algorithm and MPC algorithm have the following shortcomings:

[0006] (1) In the traditional PSO swarm algorithm, there is no guiding function, which can easily make the particle fall into local optimum.

[0007] (2) In the traditional PSO algorithm, the weight factor in the particle motion speed has a fixed effect on its speed, and it does not consider its effect on its particle motion.

[0008] (3) The traditional PSO swarm algorithm does not consider that the intelligent agent keeps a safe running distance from the known obstacles after forming the path.

[0009] (4) Since the traditional PSO algorithm is generally used for single-agent path planning, it does not involve the field of multi-agent path planning.

[0010] (5) After the traditional PSO forms the global path planning, it lacks tracking and prediction of the formed path.

[0011] (6) The traditional MPC algorithm usually only tracks the motion trajectory of a single agent, and lacks trajectory tracking of local motion of multiple agents.

[0012] (7) In the process of real-time tracking of the global path formed by the PSO algorithm, the multi-agent does not implement an obstacle avoidance strategy for the unknown obstacles that appear and block the original running trajectory.

[0013] (8) There is a collision problem between different agents at the same starting point in the motion of the multi-agent. SUMMARY

[0014] To solve the above problems, a multi-agent path planning method based on PSO-MPC fusion is proposed.

[0015] To achieve the above purpose, a multi-agent path planning method based on PSO-MPC fusion is proposed. In view of the problems in the traditional PSO swarm algorithm, such as the particle being easily trapped in local optimum due to the lack of guidance function, and too many invalid iterations, a multi-target point and a guidance function of multi-target point motion are introduced. This function changes the original particle motion speed formula to the movement of the multi-target point to the theoretical optimum. This setting can form the motion path of the intelligent agent, thereby avoiding the problem of local optimum

[0016] In view of the fact that in the traditional PSO algorithm, the weight factor in the particle motion speed has a fixed effect on its speed, and it does not consider its effect on its particle motion, an adaptive weight factor function is introduced, so that the weight factor decreases with the specific running condition of the target point according to the PSO algorithm.

[0017] The improved fitness function of the traditional PSO algorithm is introduced into the smooth curve and the safety path judgment, so that the formed path is smooth and keeps a safe distance from the known obstacles in the subsequent operation of the agent.

[0018] The agent group function is given for the traditional PSO algorithm which is generally used for single-agent path planning and does not involve the field of multi-agent path planning. The number of populations and the number of particles in each agent are allocated according to the starting point, the end point and the task quantity, so that the single-agent path planning becomes multi-agent path planning.

[0019] The MPC algorithm is introduced to track the global path formed by the PSO algorithm, because the traditional PSO lacks tracking and prediction of the formed path.

[0020] The trajectory tracking of multi-agent is introduced because the traditional MPC algorithm usually only tracks the trajectory of single-agent, and lacks the trajectory tracking of multi-agent local motion.

[0021] Two different obstacle avoidance schemes are introduced for different agents in the process of real-time tracking of the global path formed by the PSO algorithm by the multi-agent, which avoids the unknown obstacles that obstruct the original running trajectory. One is to give a new safe prediction point, and the other is to translate the original trajectory to a safe range.

[0022] Finally, there is a collision problem between different agents at the same starting point in the movement of multi-agent. At the initial moment: the agent with a short path travels at a faster acceleration, while the other agent stops for a period of time before starting to move. During the movement, when the agent predicts that different agents have a collision risk in a certain range: if the time to reach the end point is short, the agent travels at a faster speed; if the time to reach the end point is long, the agent stops or accelerates until the other agent drives through the collision area.

[0023] The technical scheme adopted is:

[0024] First of all, it is declared that the multi-agent path planning algorithm based on the fusion of PSO-MPC is based on the following assumptions:

[0025] 1. The working environment of the agent is simplified as path planning in a space with part of the information known, the blue circle is regarded as an obstacle, and the other non-blue area is regarded as a free space that the agent can move in;

[0026] 2. Assume that the motion of the intelligent agent can be regarded as a certain range of rigid body motion.

[0027] A multi-agent path planning method based on PSO-MPC fusion, characterized in that it mainly comprises the following steps:

[0028] Step 1, environment model, determine the starting point of the task start of the intelligent agent, the final task target point goal, the starting point and the end point of different intelligent agents are not necessarily the same, and the position information of all obstacles;

[0029] Step 2, use the traditional PSO algorithm for global path planning, and provide global path for subsequent multi-agent motion trajectory;

[0030] Step 3, set the initial iteration number and the number of particles, and introduce the number of multi-agents and the population number of each multi-agent according to the number and size of the task;

[0031] Step 4, add the number of multi-target points, and initialize the position of the target point. After initializing the position of the target point, introduce a sorting function, sort from small to large according to the distance to the starting point;

[0032] Step 5, due to the introduction of multi-target points, the particle motion speed formula becomes multi-target point motion, the speed of the target point is initialized, and the guide function guide n (k) is introduced to make the target point close to the straight line from the starting point to the end point;

[0033] Step 6, optimize the weight factor, change the fixed weight factor into a decreasing weight factor function;

[0034] Step 7, update the speed and position, calculate the fitness function, and make the formed path smooth, and add safety obstacle avoidance and collision detection;

[0035] Step 8, after the PSO algorithm forms the path, use the MPC algorithm as the local motion of the intelligent agent, and extend the trajectory tracking for single intelligent agent to multi-agent;

[0036] Step 9, when the multi-agent encounters other unknown objects in motion, introduce a recognition function to identify whether the unknown object is a new unknown obstacle or other intelligent agent, so as to facilitate the design of different optimization strategies;

[0037] Step 10, after identifying that the unknown object within the visual range of the intelligent agent is an obstacle, introduce two obstacle avoidance strategies based on obstacle partition function for different intelligent agents to avoid obstacles;

[0038] Step 11, after the obstacle avoidance optimization is completed, the cooperative optimization between intelligent agents is carried out.

[0039] Further, in the PSO-MPC fused multi-agent path planning, in order to make the formed curve have no intersection point after introducing multiple target points, after initializing the population information such as speed, position, in step 3, according to the starting point and the end point of the task, the agent group function is introduced, and the definition is as follows:

[0040]

[0041] Where Multi_pso is the number of agents.

[0042] Further, in order to make the formed curve have no intersection point after introducing multiple target points, after initializing the population information such as speed, position, in step 4, the sorting function dai is introduced, and its expression is:

[0043]

[0044] In the formula, xs ni (k) represents the i-th target point of agent n, distxs ni (k) represents the distance between the position of the current target point and the starting point, that is, all generated target points are arranged in order of distance from the starting point from small to large.

[0045] Further, in order to make the algorithm more effective in global path planning iteration, fast convergence and form a better global path, in step 5, the multi-target point and the multi-target point close to the theoretical optimum are introduced, and the particle speed formula becomes a multi-target point motion formula and a guide function is introduced. The expressions are respectively:

[0046]

[0047] guide n (k) = c3r3guide1 n (k) + c4r4guide2 n (k)

[0048] guide1 n (k) = goal n -x ni (k)

[0049] guide2 n (k) = start n -x ni (k)

[0050] Where start n , goal nThe start point and the end point of the nth agent respectively, c3 and c4 represent weight coefficients, and r3 and r4 represent random numbers between 0 and 1.

[0051] Further, in order to make the algorithm more effective iteration, fast convergence in global path planning, the weight factor function is introduced in step 6, and the expression is:

[0052]

[0053] Where pbest n (k) local optimal path, gfitness n (k) global optimal path, PAth1 i , PAth2 i Respectively represent the distance from the current target point to the start point and the end point respectively.

[0054] Further, in order to make the path smooth and avoid obstacles while maintaining the safety of the agent when performing trajectory tracking, a smooth curve is formed by using a cubic spline curve in step 7, and a safety function is given, which is defined as follows:

[0055] Rpso_safety={1|distpso≤Rcar+Rob+R}

[0056] Where distpso represents the Euclidean distance between the obstacle and the path, Rcar, Rob, and R represent the agent's own distance, obstacle distance, and safety distance between the obstacle and the agent respectively.

[0057] Further, in order to apply the PSO path planning algorithm from single agent to multi-agent path planning, the agent group function is given in step 8, and the definition is as follows:

[0058]

[0059] Where Multi_pso is the number of agents.

[0060] Further, in order to identify whether the unknown object in the detection range is an obstacle or an agent, the agent identification function is introduced in step 9, and the expression is:

[0061] mpc_Qn=1|mpc_Qn≠1,mpc_distJc≤mpcRange

[0062] wherein mpc_distic represents whether an unknown object appears in the detection range of the agent, mpcRange represents the detection range of the agent, mpc_Qn=1 represents that the unknown object appearing in the detection range of the agent is the agent, and otherwise, the unknown object appearing in the detection range of the agent is a newly appearing obstacle.

[0063] Further, in order to avoid the unknown obstacle by different agents, in step 10,

[0064] Step 10.1 In order to determine whether the newly appearing obstacle has an impact on the original running track of the agent, an unknown obstacle record function Hobn and a pre-avoidance function Hobnewn are introduced, and the expressions are as follows:

[0065] Hobn={obnew|mpc_Qn=0}

[0066] Hobnewn={Hobnew|Rpso_safety=1}

[0067] wherein Rpso_safety=1 represents that there is a collision risk between the agent and the unknown obstacle and the related information of the obstacle is recorded, and the agent needs to make corresponding adjustment to the original running scheme;

[0068] Step 10.2 In order to avoid the unknown obstacle by different agents, an obstacle partition function is introduced, and the obstacle closest to the motion position of the agent is taken as the origin of the obstacle partition, and the partition function is defined as follows:

[0069]

[0070] wherein ob_xoy=1 represents that it is in the first quadrant, and the others are similar;

[0071] Step 10.3 In order to avoid the unknown obstacle by different agents, and according to the partition function, a plurality of avoidance strategies are introduced: the first avoidance strategy gives a preliminary prediction point, and according to the actual number of prediction points, the related definitions are as follows:

[0072]

[0073] Pmpcj=Pjmax-Pjmin

[0074] wherein Pjmin and Pjmax are the minimum and maximum position index values recorded in the prediction range, Pmpcj is the number of coordinates to be predicted, newobs is the obstacle index value having an impact on the motion of the agent, mpc_dist n (mpc_p ni ,obmew) is the distance between the motion point of the agent and the obstacle;

[0075] The second obstacle avoidance strategy, the translation method, according to the second partitioning condition, then gives the translation function definition as follows:

[0076]

[0077] Wherein, mpcY, mpcX are the difference of two coordinates corresponding to the index value of Pjmin, Pjmax, and PYixoy represents the new coordinate position after translation.

[0078] Further, in order to coordinate the operation between multiple agents, the intersection function is introduced in step 11:

[0079] mpcdl={F id ,distH≤0.1}

[0080] Wherein, mpcdl is the intersection point between the reference trajectories of different agents; distH is the distance between two points; the initial motion function is introduced and defined as follows:

[0081]

[0082] Wherein, mpcCy represents the relationship of the initial motion of the agent; mpcV ni represents the initial motion speed of the agent; a1, a2, a3 represent the acceleration under different conditions; when other agents are checked in the motion, the information exchange function is introduced and defined as follows:

[0083]

[0084] Wherein, mpcdt n represents the motion time of the agent to the intersection point, or the motion time to the end point if there is no intersection point; mpcdist represents the distance from the motion point of the agent to the intersection point, or the distance to the end point if there is no intersection point; mpcV ni represents the motion speed at the current time; mpcgh ni+1 represents the motion speed of the agent at the next time.

[0085] By using the above technical scheme, in the global path planning of multiple agents PSO, a path can be planned which does not collide with obstacles and maintains a certain safety distance. The number of iterations and the length of the path are reduced, and the convergence of the algorithm is accelerated. The MPC algorithm is used to track the trajectory during the motion of the agent, and multiple obstacle avoidance optimization strategies are used for different agents. This shows that the method in the present application effectively reduces the energy consumption of the mobile robot in path planning. BRIEF DESCRIPTION OF DRAWINGS

[0086] Further details are described below in conjunction with the accompanying drawings and specific embodiments.

[0087] Figure 1 The multi-agent path planning method based on PSO-MPC fusion collaborative obstacle avoidance

[0088] Figure 2 The path graph in two and three-dimensional environments

[0089] Figure 3 Collaborative obstacle avoidance graph

[0090] Figure 4 Distance graph between agents and obstacles, and distance graph between agents

[0091] Figure 5 Different algorithm simulation comparison graph and comparison table after optimization in the algorithm DETAILED DESCRIPTION

[0092] In order to make the improvement purpose, technical scheme and advantages clearer, the technical scheme will be described in conjunction with the drawings. Obviously, the following examples are only a part of the examples, not all the examples. Based on the examples in the prior art, all other examples obtained by those skilled in the art without creative labor fall within the scope of the present application.

[0093] For the convenience of understanding the present embodiment, please refer to Figure 1 A path planning method based on multi-agent PSO-MPC fusion is provided for detailed introduction.

[0094] Embodiment:

[0095] A multi-agent path planning method based on PSO-MPC fusion, characterized in that it mainly comprises the following steps:

[0096] Step 1, environment model, determine the starting point start of the task of the agent, the final task target point goal to be reached, the starting point and the end point of different agents are not necessarily the same, and the position information of all obstacles;

[0097] Step 2, use traditional PSO algorithm for global path planning, provide global path for subsequent multi-agent motion trajectory;

[0098] Step 3, set the initial iteration number and the number of particles, introduce the number of multi-agents and the population number of each multi-agent according to the number and size of the task;

[0099] Step 4, add the number of multi-target points, and initialize the position of the target point. After initializing the position of the target point, introduce a sorting function, sort from small to large according to the distance to the starting point;

[0100] Step 5, due to the introduction of multi-target points, the velocity formula of particle motion is changed to the motion of multi-target points, the velocity of the target point is initialized, and the guide function is introduced n (k) to make the target point approach the line that is straight with the starting point and the end point;

[0101] Step 6, the weight factor is optimized, and the fixed weight factor is changed to a decreasing weight factor function;

[0102] Step 7, the velocity and position are updated, the fitness function is calculated, the path formed is smoothed, and safety obstacle avoidance and collision detection are added;

[0103] Step 8, after the PSO algorithm forms the path, the MPC algorithm is used as the local motion of the agent, and the trajectory tracking for a single agent is extended to multiple agents;

[0104] Step 9, when multiple agents encounter other unknown objects during motion, an identification function is introduced to identify whether the unknown object is a new unknown obstacle or another agent, so that different optimization strategies can be designed;

[0105] Step 10, after identifying that the unknown object within the visual range of the agent is an obstacle, two obstacle avoidance strategies based on obstacle partition function are introduced for different agents to avoid obstacles;

[0106] Step 11, after the obstacle avoidance optimization is completed, the collaborative optimization between agents is performed.

[0107] Further, in the PSO-MPC fusion multi-agent path planning, in order to let the curve formed after the introduction of multi-target points have no intersection points, after initializing the population information such as velocity and position, in step 3, according to the starting point and the end point of the task, the agent group function is introduced, and the definition is as follows:

[0108]

[0109] Where Multi_pso is the number of agents.

[0110] Further, in order to let the curve formed after the introduction of multi-target points have no intersection points, after initializing the population information such as velocity and position, in step 4, the sorting function dai is introduced, and its expression is:

[0111]

[0112] In the formula, xs ni (k) represents the i-th target point of agent n, distxs ni(k) represents the distance between the current target point and the starting point, that is, all generated target points are arranged in order of the distance from the starting point from small to large.

[0113] Further, in order to make the algorithm more effective iteration, fast convergence in global path planning, and make the global path formed more optimal, in step 5, multiple target points are introduced and the target points are close to the theoretical optimal, the particle velocity formula becomes a multiple target point motion formula and a guide function is introduced, and the expressions are respectively:

[0114]

[0115] guide n (k) = c3r3guide1 n (k) + c4r4guide2 n (k)

[0116] guide1 n (k) = goal n -x ni (k)

[0117] guide2 n (k) = start n -x ni (k)

[0118] Where, start n , goal n are the starting point and the end point of the nth agent respectively, c3, c4 represent the weight coefficient, r3, r4 represent random numbers between 0 and 1.

[0119] Further, in order to make the algorithm more effective iteration, fast convergence in global path planning, and make the global path formed more optimal, in step 6, a weight factor function is introduced, and the expression is:

[0120]

[0121] Where, pbest n (k) is the local optimal path, gfitness n (k) is the global optimal path, PAth1 i , PAth2 i respectively represent the distance from the current target point to the starting point and the end point respectively.

[0122] Further, in order to make the path formed smooth and avoid obstacles while maintaining the safety of the agent during trajectory tracking, in step 7, a smooth curve is formed using a cubic spline curve, and a safety function is given, and the definition is as follows:

[0123] Rpso_safety = { 1 | distpso < Rcar + Rob + R}

[0124] where distpso represents the Euclidean distance between the obstacle and the path, Rcar, Rob, R represent the self-distance of the agent, the obstacle distance, and the safety distance between the obstacle and the agent, respectively.

[0125] Further, in order to apply the PSO path planning algorithm from single agent to multi-agent path planning, the agent group function is given in step 8, and the definition is as follows:

[0126]

[0127] where Multi_pso is the number of agents.

[0128] Further, in order to identify whether the unknown object in the detection range is an obstacle or an agent, the agent identification function is introduced in step 9, and its expression is:

[0129] mpc_Qn = 1 | mpc_Qn ≠ 1, mpc_distJc < mpcRange

[0130] where mpc_distJc represents whether an unknown object appears in the agent detection range, mpcRange represents the agent detection range, and mpc_Qn = 1 represents that the unknown object appearing in the agent range is an agent, otherwise it is a newly appearing obstacle.

[0131] Further, in order to avoid obstacles by different agents, in step 10,

[0132] Step 10.1 In order to determine whether the newly appearing obstacle has an impact on the original running trajectory of the agent, the unknown obstacle record function Hobn and the pre-avoidance function Hobnewn are introduced, and their expressions are as follows:

[0133] Hobn = {obnew | mpc_Qn = 0}

[0134] Hobnewn = {Hobnew | Rpso_safety = 1}

[0135] where Rpso_safety = 1 represents that there is a collision risk between the agent and the unknown obstacle and the related information of the obstacle needs to be recorded, and the agent needs to make corresponding adjustments to the original running scheme;

[0136] Step 10.2 In order for different agents to avoid obstacles, a partition function of obstacles is introduced, and the closest obstacle to the agent's movement position is taken as the origin of the partition function, and the partition function is defined as follows:

[0137]

[0138] ob_xoy = 1 indicates that it is in the first quadrant, and the others are similar;

[0139] Step 10.3 In order for different agents to avoid obstacles, and according to the partition function, a variety of obstacle avoidance strategies are introduced: the first obstacle avoidance strategy gives a preliminary prediction point, and according to the actual number of prediction points, the relevant definitions are as follows:

[0140]

[0141] Pmpcj = Pjmax - Pjmin

[0142] Where Pjmin, Pjmax are the minimum and maximum position index values recorded in the prediction range, Pmpcj is the number of coordinates that need to be predicted, newobs is the obstacle index value that affects the agent's movement, mpc_dist n (mpc_p ni ,obmew) is the distance between the agent's movement point and the obstacle;

[0143] The second obstacle avoidance strategy, the translation method, is defined as follows according to the second partition situation:

[0144]

[0145] Where mpcY, mpcX are the difference values of the two coordinates corresponding to the Pjmin, Pjmax index values, PYixoy represents the new coordinate position after translation.

[0146] Further, in order to coordinate the operation of multiple agents, an intersection function is introduced in step 11:

[0147] mpcdl = {F id ,distH≤0.1}

[0148] Where mpcdl is the point of intersection between the reference trajectories of different agents; distH is the distance between two points; and an initial movement function is introduced and defined as follows:

[0149]

[0150] Where mpcCy represents the relationship of the agent's initial movement; mpcV nirepresents the initial motion speed of the agent; a1, a2, a3 represent the acceleration under different conditions; when other agents are detected in the motion, the information exchange function is introduced, which is defined as follows:

[0151]

[0152] wherein mpcdt n represents the motion time of the agent to the intersection point, or the motion time to the end point if there is no intersection point; mpcdist represents the distance from the motion point of the agent to the intersection point, or the distance to the end point if there is no intersection point; mpcV ni represents the motion speed at the current time; mpcgh ni+1 represents the motion speed of the agent at the next time.

[0153] The above method is simulated as follows:

[0154] As shown in the simulation results of Figure 2 Fig. (a) and Fig. (b) respectively realize the global path planning of multi-agent PSO in two-dimensional and three-dimensional environments, providing a reference path for real-time trajectory tracking of multi-agent; Fig. (c) and Fig. (d) are views from different angles to see whether the path formed in the three-dimensional environment avoids the known obstacles.

[0155] As shown in the simulation results of Figure 3 Fig. (a) and Fig. (b) are obstacle avoidance graphs in two-dimensional and three-dimensional environments respectively, from which it can be seen that when the agent encounters obstacles that affect its motion during the motion, obstacle avoidance is performed; in Fig. (c), the agents will collide when starting to move, so the agent that reaches the end point in a shorter time is allowed to drive first, and the other agent stops in place; in Fig. (d), the agent is predicted to have a collision interval during the motion, so the speed planning is performed according to the length of the time to reach the interval: accelerate when the time to reach is short, decelerate or stop until the other agent drives through the collision interval before continuing to drive.

[0156] As shown in the simulation results of Figure 4 In order to have a clearer understanding of the obstacle avoidance and coordination strategies of the agents, from Fig. (a) it can be seen that the agents maintain a safe distance from the obstacles during the motion. In Fig. (b), when other agents are detected in the detection range of the agent, the distance between the agents also maintains a safe distance.

[0157] As shown in the simulation results of Figure 5The simulation results show that the path formed by the algorithm is shorter and the convergence speed is faster than other algorithms. The results of the algorithm compared with other algorithms are as follows: the iteration times are 80% better than the ant colony algorithm and 60% better than the traditional algorithm (several experiments have fallen into local optimal solution from the first time), the path length is 100% shorter than other algorithms, the effective iteration number is 50% more than the ant colony algorithm and 90% more than the original optimization algorithm.

Claims

1. A multi-agent path planning method based on PSO-MPC fusion, characterized in that, Mainly includes the following steps: Step 1, the environment model, determine the starting point of the task of the agent start, the final goal to be reached the task target point goal, the starting point of different agents, the end point is not necessarily the same, and the position information of all obstacles; Step 2, using the traditional PSO algorithm for global path planning, for subsequent multi-agent motion trajectory provides global path; Step 3, the initial iteration number, the number of particles is set, the number of multi-agent is determined according to the number and size of the task, and the population number of each multi-agent is determined; Step 4, the number of multi-target points is added, and the position of the target point is initialized; after initializing the position of the target point, the sorting function is introduced, and the distance from the starting point is sorted from small to large; Step 5, due to the introduction of multi-target points, the velocity formula of particle motion becomes the motion of multi-target points, the velocity of target points is initialized, and the guide function is introduced n (k), let the target points approach the line that is the straight line of the starting point and the end point: multi-target points are introduced and the multi-target points approach the theoretical optimum, the particle velocity formula becomes the multi-target point motion formula and the guide function is introduced, and their expressions are respectively: guide n (k) = c3r3guide1 n (k) + c4r4guide2 n (k) guide1 n (k) = goal n -x ni (k) guide2 n (k) = start n - x ni (k) Wherein, start n , goal n are the starting point and the end point of the nth agent respectively, c3, c4 represent the weight coefficient, and r3, r4 represent random numbers between [0, 1]; Step 6, the weight factor is optimized, and the fixed weight factor is changed into a decreasing weight factor function; Step 7, update the speed and position, calculate the fitness function, and make the path smooth, and add safety obstacle avoidance and collision detection; Step 8, after the PSO algorithm forms the path, the MPC algorithm is used as the local motion of the agent, and the trajectory tracking for single agent is extended to multi-agent; Step 9, when the multi-agent encounters other unknown objects in the motion, the identification function is introduced to identify whether the unknown object is a new unknown obstacle or other agent, so as to facilitate the design of different optimization strategies; Step 10, after identifying that the unknown object in the visible range of the agent is an obstacle, two obstacle avoidance strategies based on obstacle partition function are introduced for different agents to avoid obstacles; Step 11, after the optimization of obstacle avoidance, the collaborative optimization between agents is carried out.

2. The multi-agent path planning method based on PSO-MPC fusion according to claim 1, characterized in that, In step 4, in order to let the curve formed after introducing multi-target points have no intersection points, after initializing the population information such as speed and position, the sorting function dai is introduced, and its expression is: where xs ni (k) denotes the i-th goal point of the agent n, distxs ni (k) denotes the distance between the current goal point and the start point, i.e. all generated goal points are sorted by the distance to the start point from small to large.

3. The multi-agent path planning method based on PSO-MPC fusion according to claim 1, characterized in that, In step 6, in order to make the guide function close to the theoretical optimum, the adaptive weight factor function is introduced, and its expression is: where pbest n (k) local optimal path, gfitness n (k) global optimal path, PAth1 i , PAth2 i respectively represent the distance from the current target point to the start point and end point respectively; the adaptive weight factor function and the multi-target point guiding function work together to make the path better.

4. The multi-agent path planning method based on PSO-MPC fusion according to claim 1, characterized in that, In step 7, in order to make the path smooth and avoid obstacles while keeping the safety of the agent during trajectory tracking, a smooth curve is formed by using a cubic spline curve, and a safety function is given, and its definition is as follows: Rpso_safety={1|distpso≤Rcar+Rob+R} Wherein, distpso represents the Euclidean distance between the obstacle and the path, Rcar, Rob, R respectively represent the self distance of the agent, the obstacle distance, the safety distance between the obstacle and the agent.

5. The multi-agent path planning method based on PSO-MPC fusion according to claim 1, characterized in that, In step 8, in order to apply the PSO path planning algorithm from single agent to multi-agent path planning, the agent group function is given, and its definition is as follows: Wherein, Multi_pso is the number of agents.

6. The multi-agent path planning method based on PSO-MPC fusion according to claim 1, characterized in that, In step 9, in order to identify whether the unknown object in the detection range is an obstacle or an agent, the agent identification function is introduced, and its expression is: mpc_Qn=1|mpc_Qn≠1,mpc_distJc≤mpcRange Wherein, mpc_distJc represents whether an unknown object appears in the detection range of the agent, mpcRange represents the detection range of the agent, mpc_Qn=1 represents that the unknown object appearing in the range of the agent is the agent, otherwise it is a newly appeared obstacle.

7. The multi-agent path planning method based on PSO-MPC fusion according to claim 1, characterized in that, In the step 10, Step 10.1 In order to judge whether the newly appeared obstacle has an impact on the original running track of the agent, an unknown obstacle record function Hobn and a pre-avoidance obstacle function Hobnewn are introduced, and the expressions are as follows: Hobn={obnew|mpc_Qn=0} Hobnewn={Hobnew|Rpso_safety=1} Wherein, Rpso_safety=1 represents that there is a collision risk between the agent and the unknown obstacle and the related information of the obstacle is recorded, and the agent needs to make corresponding adjustment to the original running scheme; Step 10.2 In order to avoid obstacles by different agents, a barrier partition function is introduced, and the obstacle closest to the motion position of the agent is taken as the origin of the barrier partition, and the partition function is defined as follows: Wherein, ob_xoy=1 represents in the first quadrant, and the others are similar; Step 10.3 In order to avoid obstacles by different agents, and according to the partition function, a variety of avoidance strategies are introduced: The first avoidance strategy gives a preliminary prediction point, and according to the actual number of prediction points, the related definitions are as follows: Pmpcj=Pjmax-Pjmin Wherein, Pjmin, Pjmax are the minimum and maximum position index values in the record prediction range, Pmpcj is the number of coordinates to be predicted, newobs is the obstacle index value that has an impact on the agent motion, mpc_dist n (mpc_p ni ,obmew) is the distance between the agent motion point and the obstacle. The second avoidance strategy, translation method, according to the situation of the second partition, then gives the translation function definition as follows: Wherein, mpcY, mpcX are the difference values of the two coordinates corresponding to the index values of Pjmin and Pjmax respectively, and PYixoy represents the new coordinate position after translation.

8. The multi-agent path planning method based on PSO-MPC fusion according to claim 1, characterized in that, In the step 11, in order to realize the cooperative operation between multiple agents, an intersection function is introduced: mpcdl = {F id ,distH < 0.1} Wherein, mpcdl is the intersection point of the reference tracks of different agents, and distH is the distance between two points; An initial motion function is also introduced, and the definition is as follows: where mpcCy represents the initial motion of the agent; mpcV ni represents the initial motion velocity of the agent; a1, a2, a3 represent the acceleration under different conditions; When other agents are checked in the motion, an information exchange function is introduced, and the definition is as follows: where mpcdt n denotes the motion time of the agent to the intersection, or to the end point if there is no intersection; mpcdist denotes the distance of the motion point of the agent to the intersection, or to the end point if there is no intersection; mpcV ni representing the motion speed at the current time instant; mpcgh ni+1 representing the motion speed at the next time instant for the agent.

Images