Multi-agent path planning method based on PSO-MPC fusion

Through the PSO-MPC fusion algorithm, the multi-objective point-oriented and adaptive weights, group functions of the agent group and obstacle avoidance strategy are used to solve the local optimization, obstacle safety and trajectory tracking problems in the multi-agent path planning, and realize the Efficient and secure path planning and trajectory tracking.

CN119022944AActive Publication Date: 2024-11-26HEILONGJIANG DIFAN TECHNOLOGY CO LTD

Patent Information

Application Number
CN202411054469.3
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-02
Publication Date
2024-11-26
Estimated Expiration
2044-08-02

AI Technical Summary

Technical Problem

Traditional PSO and MPC algorithms have problems in multi-agent path planning, local optimal traps, fixed speed weights, unacceptable obstacle safety, lack of tracking of single-agent trajectory, failure to avoid obstacles in real time, and agent collisions.

Method used

Introduce multi-objective point-oriented functions, adaptive weight factors, agent group functions, MPC trajectory tracking, obstacle avoidance strategies and collaborative optimization. Through the PSO-MPC fusion algorithm, path planning, obstacle avoidance and trajectory tracking are optimized to ensure safe distance and iteration efficiency.

Benefits of technology

Effectively avoid local optimization, reduce the number of iterations, ensure path safety and smoothness, realize multi-agent tracking and obstacle avoidance, reduce energy consumption, and improve path planning efficiency.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure FT_1
    Figure FT_1
  • Figure FT_2
    Figure FT_2
  • Figure FT_3
    Figure FT_3
Patent Text Reader

Abstract

The invention 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, and relates to shortening of path length and keeping of safe driving distance. The invention provides a PSO and MPC fused multi-agent path planning algorithm. In the algorithm, firstly, a global path is planned by using a PSO algorithm, and a multi-target-point guide function and a weight factor adaptive function are introduced to reduce the length of the path; and secondly, the global path is used as a reference trajectory of MPC real-time tracking, and a safe prediction point is introduced to control the driving speed of an intelligent agent function, so that the driving safety of the intelligent agent is kept. Finally, a comparison experiment with traditional PSO, MOPSO, ant colony and other algorithms in a simulation experiment shows that a global path formed by the algorithm provided by the invention is shorter in length and higher in convergence speed; the intelligent agent is located in the safe area all the time, and the formed path is smoother.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the technical field of artificial intelligence path planning, and 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 in the autonomous navigation of artificial intelligence systems, and it is also a research hotspot in the field of artificial intelligence systems. The purpose of path planning is mainly to allow the intelligent agent to find a path from the starting point to the end point within a specified range, and this path does not collide with obstacles in the middle. The core of path planning and trajectory tracking is the design of the algorithm. Currently, common path planning and trajectory tracking algorithms include: A* algorithm, Dijkstra algorithm, RRT algorithm, genetic algorithm, particle swarm algorithm, dynamic window algorithm, PID control algorithm, Stanley algorithm, LQR algorithm, MPC algorithm, etc.

[0003] The particle swarm algorithm is an optimization algorithm based on swarm intelligence, simulating the foraging behavior of groups such as bird flocks or fish schools. In the particle swarm algorithm, each "particle" represents a potential solution and moves in the search space to find the optimal solution. The basic idea of ​​the particle swarm algorithm is to simulate the behavior of bird flocks or fish schools, and each particle adjusts its position and speed based on its own experience and the experience of the group. During the search process, the particle will record its own best position (individual optimal solution) and the best position of the entire group (global optimal solution), and update its speed and position based on this information. Finally, the best position of the entire group is selected, that is, the path with the shortest distance. The particle algorithm is a global swarm intelligence algorithm with the advantages of being simple and easy to implement, having good global search capabilities, and being able to quickly converge to the global optimal solution.

[0004] The MPC algorithm can be applied to the control system to track the target trajectory. The MPC algorithm tracks the target trajectory by predicting the behavior of the system in the future and generating control inputs based on these predictions. The general steps of the MPC algorithm in trajectory tracking are: 1. Establish a model; 2. Predict the reference trajectory; 3. Rolling optimization; 4. Error compensation; thereby tracking the global path planning formed by the PSO algorithm. The MPC algorithm has strong predictive and optimization performance.

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

[0006] (1) In traditional PSO group algorithm, particles are easily trapped in local optimality due to the lack of guidance function.

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

[0008] (3) The traditional PSO group algorithm does not consider the need for the intelligent agent to maintain a safe operating distance from known obstacles after the path is formed.

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

[0010] (5) After the traditional PSO forms the global path planning, there is a lack of tracking prediction of the path it has not formed.

[0011] (6) Traditional MPC algorithms usually only track the motion trajectory of a single agent, but lack the ability to track the local motion trajectories of multiple agents.

[0012] (7) When the multi-agents are tracking the global path formed by the PSO algorithm in real time, they do not implement obstacle avoidance strategies for newly emerging unknown obstacles that block the original running trajectory.

[0013] (8) When multiple agents are in motion and at the same starting point, there is a collision problem between different agents. Summary of the invention

[0014] In order to solve the shortcomings of the above algorithms, the present invention proposes a multi-agent path planning method based on PSO-MPC fusion.

[0015] In order to achieve the above-mentioned purpose, the present invention proposes a multi-agent path planning method based on PSO-MPC fusion. In view of the problems that the traditional PSO group algorithm easily causes particles to fall into local optimality due to the lack of guidance function and too many invalid iterations, the present invention introduces multiple target points and guidance functions for the movement of multiple target points. This function converts the original formula for the movement speed of particles into the movement of multiple target points to the theoretical optimal state. This setting can form the movement path of the intelligent agent, thereby avoiding the problem of local optimality.

[0016] In view of the fact that in the traditional PSO algorithm, the weight factor in the particle movement speed has a fixed effect on its speed, and its influence on its particle movement is not considered, the present invention introduces an adaptive weight factor function so that the weight factor decreases as the PSO algorithm decreases according to the specific operation conditions of the target point.

[0017] In view of the fact that the traditional PSO group algorithm does not consider the safe running distance between the intelligent agent and the known obstacles after the path is formed, the present invention improves the fitness function in the traditional PSO algorithm and introduces smooth curves and safe path judgment into the fitness function, so that the formed path is smooth and keeps a safe distance from the known obstacles in the subsequent operation of the intelligent agent.

[0018] The traditional PSO algorithm is generally used for path planning of a single agent, but not for path planning of multiple agents. The present invention provides an agent group function. We allocate the number of populations and the number of particles in each agent according to the starting point, end point, and task volume, so that the path planning of a single agent becomes the path planning of multiple agents.

[0019] After the traditional PSO forms a global path planning, there is a lack of tracking and prediction of the path formed by it. This paper introduces the MPC algorithm to track the global path formed by the PSO algorithm.

[0020] Traditional MPC algorithms usually only track the motion trajectory of a single agent, but lack trajectory tracking of local motion of multiple agents. This paper introduces trajectory tracking of multiple agents.

[0021] In the process of real-time tracking of the global path formed by the PSO algorithm, multiple agents do not implement obstacle avoidance strategies for newly emerging unknown obstacles that block the original running trajectory. This paper introduces two different obstacle avoidance schemes for different agents: one is to give a safe new prediction point; the other is to translate the original trajectory into a safe drivable range.

[0022] Finally, in view of the collision problem between different agents in motion and at the same starting point, this paper, at the initial moment: the agent with a short path accelerates at a faster acceleration, and one agent stops for a while before starting to move. During the movement, when the agent predicts that different agents have a risk of collision in a certain range: if the time to reach the end point is short, it will drive at a faster speed; if the time to reach the end point is long, it will stop or accelerate until the other agent passes the collision area.

[0023] The technical solution adopted by the present invention is:

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

[0025] 1. The present invention simplifies the working environment of the intelligent agent into path planning in a space with partial known information, regards the blue circle as an obstacle, and regards other non-blue areas as free spaces where the intelligent agent can move.

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

[0027] A multi-agent path planning method based on PSO-MPC fusion, which mainly includes the following steps:

[0028] Step 1: Environmental model, determine the agent's task starting point start, the task target point goal to be reached, the starting point and end point of different agents may not be the same, and the location information of all obstacles.

[0029] Step 2: Use the traditional PSO algorithm to perform global path planning to provide a global path for the subsequent motion trajectory of the multi-agent.

[0030] Step 3, setting the initial number of iterations and the number of particles, introduces the method of determining the number of multi-agents and the population size of each multi-agent based on the number and size of tasks.

[0031] Step 4: Add the number of multiple target points and initialize the positions of the target points. After initializing the positions of the target points, introduce a sorting function to sort them from small to large according to the distance to the starting point. This can prevent the formed paths from having intersections and reduce the path length, preventing the agent from deviating from the course during driving.

[0032] Step 5: Due to the introduction of multiple target points, the velocity formula of particle motion is transformed into the motion of multiple target points, the velocity of the target point is initialized, and the guide function guide is introduced. n (k) Let the target point move closer to the line with the starting point and the end point as a straight line. This makes the path formed by the PSO algorithm shorter and prevents it from falling into the local optimum.

[0033] Step 6: Optimize the weight factor and transform the fixed weight factor into a decreasing weight factor function. This method can accelerate the convergence of the PSO algorithm and increase the number of effective iterations.

[0034] Step 7, update the speed and position, calculate the fitness function, smooth the formed path, and add safety obstacle avoidance and collision detection, so that the path formed in the known environmental information can reduce the obstacle avoidance strategy of the agent in the subsequent local movement of the agent, and reduce energy consumption due to obstacle avoidance.

[0035] Step 8: After the PSO algorithm forms a path, the MPC algorithm is used as the local motion of the agent. This paper extends the trajectory tracking for a single agent to multiple agents.

[0036] Step 9: When multiple agents encounter other unknown objects during movement, a recognition function is introduced to identify whether the unknown object is a new unknown obstacle or another agent, which makes it easier to design different optimization strategies.

[0037] Step 10, after identifying the unknown objects within the agent's visible range as obstacles, this paper introduces two obstacle avoidance strategies for different agents to avoid obstacles.

[0038] Step 11: After completing the obstacle avoidance optimization, the collaborative optimization problem between intelligent agents is carried out.

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

[0040]

[0041] Among them, Multi_pso is the number of agents.

[0042] Furthermore, in order to make the formed curve have no intersecting points after introducing multiple target points, after initializing population information such as speed and position, the present invention introduces a sorting function dai in step 4, 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 current target point and the starting point, that is, all generated target points are arranged from small to large according to the distance to the starting point.

[0045] Furthermore, in order to make the algorithm more effective in iteration and rapid convergence in global path planning so as to make the formed global path better, the present invention introduces a goal-oriented function in step 5, and its expressions are:

[0046]

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

[0048] Among them, start n 、goal n are the starting point and end point of the nth agent, c3 and c4 are weight coefficients, and r3 and r4 are random numbers between [0,1].

[0049] Furthermore, in order to make the algorithm more effectively iterate and converge quickly in the global path planning so as to make the formed global path better, the present invention introduces a weight factor function in step 6, and its expression is:

[0050]

[0051]

[0052]

[0053]

[0054]

[0055] Among them, 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 starting point and the end point.

[0056] Furthermore, in order to make the formed path smooth and avoid obstacles while maintaining its safety when the agent is tracking the trajectory, the present invention uses a cubic spline curve to form a smooth curve in step 7 to give a safety function, which is defined as follows:

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

[0058] Among them, 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.

[0059] Furthermore, in order to identify whether the unknown object within the detection range is an obstacle or an intelligent agent, the present invention introduces an intelligent agent recognition function in step 9, and its expression is:

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

[0061] Where mpc_distJc indicates whether an unknown object appears within the agent's detection range, mpcRange indicates the agent's detection range, and mpc_Qn=1 indicates that the unknown object that appears within the agent's range is the agent, otherwise it is a newly appeared obstacle.

[0062] Furthermore, in order to enable different agents to avoid unknown obstacles, this paper introduces an obstacle partition function in step 10. The obstacle closest to the agent's moving position is used as the origin of the obstacle partition. The partition function is defined as follows:

[0063]

[0064] Among them, ob_xoy=1 means it is in the first quadrant, and the others are similar.

[0065] Furthermore, in order to enable different agents to avoid unknown obstacles, this paper introduces multiple obstacle avoidance strategies based on the partition function: The first obstacle avoidance strategy gives preliminary prediction points. According to the actual number of prediction points, the relevant definitions are as follows:

[0066]

[0067]

[0068] Pmpcj=Pjmax-Pjmin

[0069] Among them, Pjmin and Pjmax are the minimum and maximum position index values ​​within the prediction range, Pmpcj is the number of coordinates to be predicted, newobs is the obstacle index value that affects the movement of the intelligent body, and mpc_dist n (mpc_p ni ,obmew) is the distance between the moving point of the agent and the obstacle.

[0070] The second obstacle avoidance strategy, the translation method, is based on the second partition situation and then gives the translation function definition as follows:

[0071]

[0072] Among them, mpcY, mpcX are the difference between the two coordinates corresponding to the index values ​​of Pjmin and Pjmax respectively, and PYixoy represents the new coordinate position after translation.

[0073] Furthermore, in order to coordinate the operation of multiple agents, this paper introduces the intersection function in step 11:

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

[0075] Among them, mpcdl is used to calculate and record the intersection points between the reference trajectories of different agents; distH is used to calculate the distance between two points. This paper also introduces the initial motion function defined as follows:

[0076]

[0077]

[0078] Among them, mpcCy represents the relationship of the initial movement of the intelligent agent; mpcV ni represents the initial motion speed of the agent; a1, a2, and a3 represent the acceleration under different conditions. When other agents are detected during motion, this paper introduces an information exchange function, which is defined as follows:

[0079]

[0080]

[0081] Among them, mpcdt n represents the movement time between the agent and the intersection, or if there is no intersection, it is the movement time to the end point; mpcdist represents the distance from the agent's movement point to the intersection, or if there is no intersection, it is the distance to the end point; mpcV ni Indicates the current speed of movement; mpcgh ni+1 Represented as the agent's movement speed at the next moment.

[0082] The present invention adopts the above technical solution, and in the PSO global path planning of multiple agents, a path can be planned that does not collide with obstacles and maintains a certain safe distance. It also reduces the number of iterations and the length of the path, speeds up the convergence of the algorithm, and uses the MPC algorithm to track the trajectory during the movement of the agent, and uses a variety of obstacle avoidance optimization strategies for different agents. This shows that the method in the present invention effectively reduces the energy consumption of the mobile robot in path planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0083] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments.

[0084] Figure 1 The overall flow chart of the multi-agent path planning method based on the PSO-MPC fusion collaborative obstacle avoidance of the present invention

[0085] Figure 2 Path diagram for two- and three-dimensional environments

[0086] Figure 3 Collaborative obstacle avoidance graph

[0087] Figure 4 Distance graphs between agents and obstacles, and distance graphs between agents

[0088] Figure 5 Simulation comparison charts and tables of different algorithms after optimization in the algorithm DETAILED DESCRIPTION

[0089] In order to make the improved purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be described in conjunction with the accompanying drawings. Obviously, the following embodiments are only part of the embodiments of the present invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present invention.

[0090] For easier understanding of this embodiment, please refer to Figure 1 , the present invention provides a path planning method based on multi-agent PSO-MPC fusion and introduces it in detail.

[0091] Example:

[0092] A path planning method based on multi-agent PSO-MPC fusion mainly includes the following steps:

[0093] Step 1: Environmental model, determine the starting point of the task where the agent is located, the final target point of the task to be reached, the starting point and end point of different agents may not be the same, and the location information of all obstacles.

[0094] Step 2: Use the traditional PSO algorithm to perform global path planning to provide a global path for the subsequent motion trajectory of the multi-agent.

[0095] Step 3, setting the initial number of iterations and the number of particles, introduces the method of determining the number of multi-agents and the population size of each multi-agent based on the number and size of tasks.

[0096] Step 4: Add the number of multiple target points and initialize the positions of the target points. After initializing the positions of the target points, introduce a sorting function to sort them from small to large according to the distance to the starting point. This can prevent the formed paths from having intersections and reduce the path length, preventing the agent from deviating from the course during driving.

[0097] Step 5: Due to the introduction of multiple target points, the velocity formula of particle motion is transformed into the motion of multiple target points, the velocity of the target point is initialized, and the guide function guide is introduced. n (k) Let the target point move closer to the line with the starting point and the end point as a straight line. This makes the path formed by the PSO algorithm shorter and prevents it from falling into the local optimum.

[0098] Step 6: Optimize the weight factor and transform the fixed weight factor into a decreasing weight factor function. This method can accelerate the convergence of the PSO algorithm and increase the number of effective iterations.

[0099] Step 7, update the speed and position, calculate the fitness function, smooth the formed path, and add safety obstacle avoidance and collision detection, so that the path formed in the known environmental information can reduce the obstacle avoidance strategy of the agent in the subsequent local movement of the agent, and reduce energy consumption due to obstacle avoidance.

[0100] Step 8: Update the local optimum and the global optimum until the conditions are met.

[0101] Step 9: After the PSO algorithm ends, the global optimal path is selected as the reference trajectory for the local motion of the agent. After the PSO algorithm forms the path, the MPC algorithm is used as the local motion of the agent. This paper extends the trajectory tracking used for a single agent to multiple agents.

[0102] Step 10: When multiple agents encounter other unknown objects during movement, a recognition function is introduced to identify whether the unknown object is a new unknown obstacle or another agent, which makes it easier to design different optimization strategies.

[0103] Step 11, after identifying the unknown objects within the agent's visible range as obstacles, this paper introduces two obstacle avoidance strategies for different agents to avoid obstacles.

[0104] Step 12: After completing the obstacle avoidance optimization, the collaborative optimization problem between intelligent agents is carried out.

[0105] Furthermore, in the multi-agent path planning of multi-agent PSO-MPC fusion, in order to make the formed curve have no intersecting points after introducing multiple target points, after initializing population information such as speed and position, the present invention introduces the agent group function in step 3 according to the task starting point and end point, and the definition is as follows:

[0106]

[0107] Among them, Multi_pso is the number of agents.

[0108] Furthermore, in order to make the formed curve have no intersecting points after introducing multiple target points, after initializing population information such as speed and position, the present invention introduces a sorting function dai in step 4, and its expression is:

[0109]

[0110] 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 from small to large according to the distance to the starting point.

[0111] Furthermore, in order to make the algorithm more effective in iteration and rapid convergence in global path planning so as to make the formed global path better, the present invention introduces a goal-oriented function in step 5, and its expressions are:

[0112]

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

[0114] Among them, start n 、goal n are the starting point and end point of the nth agent respectively, c3 and c4 represent weight coefficients, r3 and r4 represent random numbers between [0,1], and Pw is 1.

[0115] Furthermore, in order to make the algorithm more effectively iterate and converge quickly in the global path planning so as to make the formed global path better, the present invention introduces a weight factor function in step 6, and its expression is:

[0116]

[0117]

[0118]

[0119]

[0120]

[0121] Among them, 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 starting point and the end point.

[0122] Furthermore, in order to make the path smooth and avoid obstacles, and at the same time the intelligent agent travels with reference to the reference trajectory and maintains its safety, the present invention uses a cubic spline curve in step 7 to make the curve smooth, and proposes a safety function, which is defined as follows:

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

[0124] Among them, 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.

[0125] Furthermore, in order to identify whether the unknown object in the detection range is an obstacle or an intelligent agent, the present invention introduces an intelligent agent recognition function in step 10, and its expression is:

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

[0127] Where mpc_distJc indicates whether an unknown object appears within the agent's detection range, mpcRange indicates the agent's detection range, and mpc_Qn=1 indicates that the unknown object that appears within the agent's range is the agent, otherwise it is a newly appeared obstacle.

[0128] Furthermore, in order to enable different agents to avoid unknown obstacles, this paper introduces an obstacle partition function in step 11. The obstacle closest to the agent's moving position is used as the origin of the obstacle partition. The partition function is defined as follows:

[0129]

[0130] Among them, ob_xoy=1 means it is in the first quadrant, and the others are similar.

[0131] Furthermore, in order to help different agents avoid unknown obstacles, this paper introduces multiple obstacle avoidance strategies based on the partition function: The first obstacle avoidance strategy gives preliminary prediction points. According to the actual number of prediction points, the relevant definitions are as follows:

[0132]

[0133]

[0134] Pmpcj=Pjmax-Pjmin

[0135] Among them, Pjmin and Pjmax are the minimum and maximum position index values ​​within the prediction range, Pmpcj is the number of coordinates to be predicted, newobs is the obstacle index value that affects the movement of the intelligent body, and mpc_dist n (mpc_p ni ,obmew) is the distance between the moving point of the agent and the obstacle.

[0136] The second obstacle avoidance strategy, the translation method, is based on the second partition situation and then gives the translation function definition as follows:

[0137]

[0138] Among them, mpcY, mpcX are the difference between the two coordinates corresponding to the index values ​​of Pjmin and Pjmax respectively, and PYixoy represents the new coordinate position after translation.

[0139] Furthermore, in order to coordinate the operation of multiple agents, this paper introduces the intersection function in step 12:

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

[0141] Among them, mpcdl is used to calculate and record the intersection points between the reference trajectories of different agents; distH is used to calculate the distance between two points. This paper also introduces the initial motion function defined as follows:

[0142]

[0143]

[0144] Among them, mpcCy represents the relationship of the initial movement of the intelligent agent; mpcV ni represents the initial motion speed of the agent; a1, a2, and a3 represent the acceleration under different conditions. When other agents are detected during motion, this paper introduces an information exchange function, which is defined as follows:

[0145]

[0146]

[0147] Among them, mpcdt n represents the movement time between the agent and the intersection, or if there is no intersection, it is the movement time to the end point; mpcdist represents the distance from the agent's movement point to the intersection, or if there is no intersection, it is the distance to the end point; mpcV ni Indicates the current speed of movement; mpcgh ni+1 Represented as the agent's movement speed at the next moment.

[0148] The above method of the present invention is simulated below:

[0149] like Figure 2 As shown in the simulation results, Figure (a) and Figure (b) implement multi-agent PSO global path planning in two-dimensional and three-dimensional environments respectively, providing reference paths for multi-agent real-time trajectory tracking; Figure (c) and Figure (d) observe whether the path formed in the three-dimensional environment avoids known obstacles from different perspectives.

[0150] like Figure 3As shown in the simulation results, Figure (a) and Figure (b) are obstacle avoidance diagrams in two-dimensional and three-dimensional environments, respectively. It can be seen from the figure that when the intelligent agent encounters an obstacle that affects its own movement during movement, it avoids the obstacle; Figure (c) shows that the intelligent agent may collide when starting to move, so the intelligent agent with a shorter arrival time is allowed to move first, and the other intelligent agent stops where it is; Figure (d) shows that the intelligent agent is predicted to have a collision interval during movement, so the acceleration and deceleration plan is made according to the length of time it takes to reach the interval: the intelligent agent with a shorter arrival time accelerates, and the intelligent agent with a longer arrival time decelerates or stops and waits for the other intelligent agent to pass the collision interval before continuing to move.

[0151] like Figure 4 As shown in the simulation results, in order to give a clearer understanding of the agent's obstacle avoidance and coordination strategies, it can be seen from Figure (a) that the agent always maintains a safe distance from obstacles during movement. In Figure (b), when other agents appear in the agent's detection range, the distance between the agents always maintains this safe distance.

[0152] like Figure 5 As shown in the simulation results, from the algorithm iteration comparison diagram in Figure (a), we can see that the path formed by this algorithm is shorter than that of other algorithms and converges faster. As can be seen from the table, the algorithm of this paper has the following results when compared with other algorithms separately: 80% of the iterations are better than the ant colony algorithm, 60% are better than the traditional algorithm (several experiments have fallen into the local optimal solution since the first time), and the final path length is 100% shorter than that of other algorithms; 50% of the effective iterations exceed the ant colony algorithm, and 90% exceed the original optimization algorithm.

Claims

1. A multi-agent path planning method based on PSO-MPC fusion, characterized in that: In order to make the formed curve have no intersecting points after introducing multiple target points, after initializing population information such as speed and position, the present invention introduces a sorting function dai, whose expression is: 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 from small to large according to the distance to the starting point.

2. A multi-agent path planning method based on PSO-MPC fusion, characterized in that: In order to make the algorithm more effective in iteration and fast convergence in global path planning and make the formed global path better, the present invention improves the speed formula, introduces multiple target points and approaches the multiple target points to the theoretical optimum, and the particle speed formula becomes a multiple target point motion formula and introduces a guidance function, and the expressions are: 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) Among them, start n 、goal n are the starting point and end point of the nth agent, c3 and c4 are weight coefficients, and r3 and r4 are random numbers between [0,1].

3. A multi-agent path planning method based on PSO-MPC fusion, characterized in that: In order to make the algorithm more effective in iteration and rapid convergence in global path planning so as to form a better global path, the present invention introduces a weight factor function, the expression of which is: Among them, 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 starting point and the end point.

4. A multi-agent path planning method based on PSO-MPC fusion, characterized in that: In order to make the formed path smooth and avoid obstacles while maintaining its safety when the agent is tracking the trajectory, the present invention uses a cubic spline curve to form a smooth curve and gives a safety function, which is defined as follows: Rpso_safety={1|distpso≤Rcar+Rob+R} Among them, 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.

5. A multi-agent path planning method based on PSO-MPC fusion, characterized in that: In order to apply the PSO path planning algorithm from a single agent to multi-agent path planning, the present invention provides an agent group function, which is defined as follows: Among them, Multi_pso is the number of agents.

6. A multi-agent path planning method based on PSO-MPC fusion, characterized in that: In order to identify whether an unknown object within the detection range is an obstacle or an intelligent agent, the present invention introduces an intelligent agent recognition function, which is expressed as follows: mpc_Qn=1|mpc_Qn≠1, mpc_distJc≤mpcRange Where mpc_distJc indicates whether an unknown object appears within the agent's detection range, mpcRange indicates the agent's detection range, and mpc_Qn=1 indicates that the unknown object that appears within the agent's range is the agent, otherwise it is a newly appeared obstacle.

7. A multi-agent path planning method based on PSO-MPC fusion, characterized in that: In order to determine whether the newly appeared obstacle affects the original running trajectory of the intelligent agent, the present invention introduces the unknown obstacle recording function Hobn and the pre-obstacle avoidance function Hobnewn, whose expressions are as follows: Hobn = {obnew | mpc_Qn = 0} Hobnown={Hobnew|Rpso_safety=1} Among them, Rpso_safety = 1 means that there is a collision risk between the intelligent agent and an unknown obstacle and the relevant information of the obstacle is recorded, and the intelligent agent needs to make corresponding adjustments to the original operation plan.

8. A multi-agent path planning method based on PSO-MPC fusion, characterized in that: In order to enable different agents to avoid unknown obstacles, this paper introduces an obstacle partition function. The obstacle closest to the agent's moving position is used as the origin of the obstacle partition. The partition function is defined as follows: Among them, ob_xoy=1 means it is in the first quadrant, and the others are similar.

9. A multi-agent path planning method based on PSO-MPC fusion, characterized in that: In order to enable different agents to avoid unknown obstacles, this paper introduces multiple obstacle avoidance strategies based on the partition function: The first obstacle avoidance strategy gives a preliminary prediction point. According to the actual number of prediction points, the relevant definitions are as follows: Pmpcj=Pjmax-Pjmin Among them, Pjmin and Pjmax are the minimum and maximum position index values ​​within the prediction range, Pmpcj is the number of coordinates to be predicted, newobs is the obstacle index value that affects the movement of the intelligent body, and mpc_dist n (mpc_p ni ,obmew) is the distance between the moving point of the agent and the obstacle. The second obstacle avoidance strategy, the translation method, is based on the second partition situation and then gives the translation function definition as follows: Among them, mpcY, mpcX are the difference between the two coordinates corresponding to the index values ​​of Pjmin and Pjmax respectively, and PYixoy represents the new coordinate position after translation.

10. A multi-agent path planning method based on PSO-MPC fusion, characterized in that: In order to coordinate the operation of multiple agents, this paper introduces the intersection function: mpcdl={F id ,distH≤0.1} Among them, mpcdl is used to calculate and record the intersection points between the reference trajectories of different agents; distH is used to calculate the distance between two points. This paper also introduces the initial motion function defined as follows: Among them, mpcCy represents the relationship of the initial movement of the intelligent agent; mpcV ni Indicates the initial movement speed of the agent; a1, a2, and a3 represent accelerations under different conditions. When detecting other agents during motion, this paper introduces an information exchange function, which is defined as follows: Among them, mpcdt n represents the movement time between the agent and the intersection, or if there is no intersection, it is the movement time to the end point; mpcdist represents the distance from the agent's movement point to the intersection, or if there is no intersection, it is the distance to the end point; mpcV ni Indicates the current speed of movement; mpcgh ni+1 Represented as the agent's movement speed at the next moment.

Citation Information

Patent Citations

  • Motion planning and controlling method for tractor-trailer mobile robot in complex environment

    CN103941737A

  • Progressive model prediction unmanned driving planning and tracking cooperative control method

    CN111413966A

  • Space-time path planning method based on multiple intelligent agents

    CN112665592A

  • Intelligent agent path tracking control method

    CN113204236A

  • Multi-dimensional scale node positioning method based on improved particle swarm

    CN113347561A

Cited By

  • Unmanned aerial vehicle formation online route planning method and device based on rolling optimization strategy

    CN119828750A

  • Distributed model predictive control unmanned cluster navigation method based on homotopy perception

    CN120353256A