A multi-agent path planning method fusing heuristic sampling and terrain perception

Abstract

The application belongs to the field of multi-agent cooperative control and path planning, and discloses a multi-agent path planning method fusing heuristic sampling and terrain perception, comprising the following steps: acquiring map data of a target environment, performing rasterization processing on the map data, and dividing the map into multiple grid units; calculating prior sampling weights for each grid unit according to the distance of each grid unit to a target point and the obstacle distribution in each grid unit; acquiring the starting positions and target positions of multiple agents respectively, and performing global path planning based on the prior sampling weights of each grid unit to generate a global path for each agent from the starting position to the target position, wherein the generation of a sampling point is guided according to the prior sampling weight in the planning process; and the problems of low path planning efficiency and easy congestion in collaborative scheduling of a multi-agent system in a complex terrain in the prior art are effectively solved.

Description

Technical Field

[0001] This invention belongs to the field of multi-agent cooperative control and path planning, specifically involving a multi-agent path planning method that integrates heuristic sampling and terrain perception. Background Technology

[0002] Multi-agent cooperative path planning has been widely used in fields such as warehouse logistics robot scheduling, drone swarm formation, and autonomous driving fleet collaboration. This problem can usually be decomposed into two sub-problems: global path planning and multi-agent cooperative scheduling.

[0003] In global path planning, the Rapidly-exploring RandomTree Star (RRT) algorithm, based on random sampling, has become one of the mainstream methods in this field due to its asymptotic optimality and ability to effectively handle path planning problems in high-dimensional state spaces. However, in complex terrains such as narrow passages and dense obstacles, the global uniform random sampling strategy makes it difficult for sampling points to fall into bottleneck areas, resulting in difficulties in path discovery. At the same time, the search tree expansion lacks goal orientation, has a slow convergence speed, and is time-consuming, which seriously restricts its application effect in complex scenarios.

[0004] In terms of multi-agent collaborative scheduling, the PIBT algorithm (PriorityInheritance with Backtracking) based on priority inheritance and backtracking mechanism is one of the more effective scheduling methods at present. However, its priority allocation strategy mainly sorts agents based on the distance from the target point, lacks awareness of the degree of environmental topology congestion, and is difficult to effectively cope with the demand for multiple machines to pass through bottleneck areas such as narrow channels, which easily leads to livelock and frequent stalling.

[0005] To address the aforementioned issues, existing technologies typically treat global path planning and multi-agent cooperative scheduling as two independent modules. However, the lack of a coordination mechanism between the two makes it difficult for global path planning to generate high-quality paths adapted to complex terrain, and multi-agent scheduling cannot perceive terrain congestion to achieve efficient conflict resolution. Therefore, existing technologies suffer from low path planning efficiency and easy congestion in cooperative scheduling under complex terrain conditions. Summary of the Invention

[0006] To address the shortcomings of existing technologies, the present invention aims to provide a multi-agent path planning method that integrates heuristic sampling and terrain perception, thereby solving the problems of low path planning efficiency and easy congestion in collaborative scheduling of multi-agent systems under complex terrain in existing technologies.

[0007] The objective of this invention can be achieved through the following technical solutions: A multi-agent path planning method integrating heuristic sampling and terrain perception includes the following steps: Acquire map data of the target environment, perform rasterization on the map data, and divide the map into multiple grid cells; Based on the distance from each grid cell to the target point and the distribution of obstacles within each grid cell, calculate the prior sampling weight for each grid cell. The system obtains the starting and target positions of multiple agents, performs global path planning based on the prior sampling weights of each grid cell, and generates a global path from the starting position to the target position for each agent. During the planning process, the generation of sampling points is guided by the prior sampling weights, and the path expansion direction is determined by coordinating the direction towards the target point with the random exploration direction. Multi-agent collaborative scheduling is carried out based on the current position and global path of each agent. During the scheduling process, the terrain congestion level of each agent's current position is perceived in real time. The priority order is determined according to the remaining distance from each agent to its target point and the terrain congestion level. The movement position is assigned to each agent according to the priority order. When a position occupancy conflict is detected, the conflict is recursively resolved through a priority inheritance mechanism. Each agent moves according to the scheduling result and updates its position status in real time. The scheduling and movement steps are executed repeatedly until all agents reach their respective target points.

[0008] Furthermore, based on the distance from each grid cell to the target point and the distribution of obstacles within each grid cell, a priori sampling weight is calculated for each grid cell, specifically including the following steps: For each grid cell, calculate the distance weight from the grid cell to the target point and the density weight within the grid cell based on the obstacle distribution. Multiply the distance weight and density weight corresponding to the grid cell to obtain the comprehensive weight of the corresponding grid cell; Sum the combined weights of all grid cells to obtain the total combined weight; Divide the comprehensive weight of each grid cell by the total comprehensive weight to obtain the normalized prior sampling weight of each grid cell.

[0009] Furthermore, the distance weight from the grid cell to the target point is calculated using the following formula: In the formula, Indicates distance weight, This represents the Euclidean distance from the center of the grid cell to the target point. Indicates the length of the map's diagonal; The density weights within a grid cell, based on the distribution of obstacles, are calculated using the following formula: In the formula, Indicates density weight, This indicates the obstacle density within a grid cell; Among them, obstacle density The specific expression is as follows: In the formula, The number of obstacles within a grid cell. This represents the total number of grid cells.

[0010] Furthermore, global path planning is performed based on the prior sampling weights of each grid cell to generate a global path from the starting position to the target position for each agent. This specifically includes the following steps: During the global path planning process, the target point is directly used as the sampling point with a first probability, and a grid cell is randomly selected based on the prior sampling weight of each grid cell with a second probability, and a sampling point is randomly generated within that grid cell; wherein, the first probability is less than the second probability, and the sum of the two is 1; The direction from the current latest path point to the sampling point is taken as the random exploration direction, and the direction from the latest path point to the target point is taken as the gravitational direction. The direction vector of the random exploration direction is weighted and summed with the direction vector of the gravity direction and then normalized to obtain the fused direction vector, which is used as the direction of this path expansion. Expand the path points sequentially according to the direction until a preliminary global path from the starting point to the target point is generated; Iterate through each non-adjacent path point in the path point sequence. When the line connecting two points does not pass through an obstacle, delete all intermediate path points between the two points to obtain the smoothed global path.

[0011] Furthermore, the global path planning process also includes a step size dynamic adjustment step, which is used to adjust the step size of this path expansion according to the density of local obstacles around the current expansion starting point. The step size decreases as the density of local obstacles around the current expansion point increases, and the step size is not lower than the preset minimum step size value. The local obstacle density around the starting point of the expansion is calculated using the following formula: in, The coordinates of the current starting point of the expansion. This indicates the local obstacle density around the current location. Indicates the current extension starting point to the th k The distance to each obstacle The preset sensing radius, This indicates the number of obstacles within a circular area centered at the current expansion starting point and with a preset sensing radius. The specific calculation formula for dynamic adjustment of step size is as follows: In the formula, Based on step size, This is the adjusted step size.

[0012] Furthermore, the degree of terrain congestion is characterized by degrees of freedom of movement, which is the number of directions in which the agent's current position is neither subject to terrain collision nor occupied by other agents in the eight connected directions around it. When the degree of freedom of movement is less than or equal to the preset degree of freedom threshold, or when the distance between the current location and the map boundary is less than or equal to the preset boundary threshold, the location is marked as a narrow area.

[0013] Furthermore, the priority order is determined based on the remaining distance of each agent to its target point and the degree of terrain congestion. Movement positions are then assigned to each agent according to this priority order, specifically including the following steps: Each agent is sorted from highest to lowest according to the following three-level priority: First priority: the smaller the remaining distance from the agent to its target point, the higher the priority. The second priority is that when the agent's current position is in a non-narrow region, the priority is higher than that of a narrow region. The third priority is that the greater the degree of freedom of movement of the agent at its current position, the higher the priority. Based on the ranking results, a set of candidate movement positions is generated for each agent. The candidate movement positions include the position to move along the global path direction, the current position to wait, and the position in the alternative direction. Candidate movement positions are sorted and optimized according to a three-level priority system to determine the next movement position for each agent. When a position occupancy conflict is detected, the conflict is recursively resolved through a priority inheritance mechanism.

[0014] Furthermore, when a location occupancy conflict is detected, the conflict is recursively resolved through a priority inheritance mechanism, specifically including the following steps: Conflict detection is performed on each agent in sequence, including node conflict detection and edge conflict detection. Node conflict detection is used to check whether the target movement position of an agent has been occupied by another agent, and edge conflict detection is used to check whether the positions of two agents have been swapped. When a conflict occurs between the target movement positions of any agent, a recursive call mechanism is triggered, which passes the priority of the current agent to the agent occupying the position, and recursively determines the movement position of the occupying agent, so as to force it to make room for the current agent. Priority is passed recursively until a movable agent or the end of the recursion is reached. After the terminal agent in the recursion determines its move position, it returns level by level. Each agent completes the position allocation in turn according to the result of the recursive return, until all conflicts are resolved.

[0015] Furthermore, the process of each agent moving according to the scheduling results also includes local minimum escape, specifically including the following steps: The displacement of each agent is detected in multiple consecutive time steps. If the displacement of any agent in each time step is less than a preset stagnation threshold, the agent is determined to be trapped in a local minimum. In response to the decision, the direction vector of the agent’s current position pointing to the target point is obtained, the vertical direction vector of the direction vector is calculated, and a rotation angle randomly selected within the range of plus or minus ninety degrees is applied to the vertical direction vector to obtain the escape direction. An escape force is applied along the escape direction to guide the agent's movement; the escape force is greater than the driving force during normal movement.

[0016] The beneficial effects of this invention are: 1. This invention enables path search to have environmental awareness by constructing prior sampling weights, concentrating sampling points in areas close to the target and with sparse obstacles, thus improving the path discovery rate in complex terrains such as narrow passages; by weighting and fusing the gravitational direction and random exploration direction, path expansion takes into account both exploration and tendency, reducing the number of iterations required for path convergence; dynamically adjusting the step size according to the local obstacle density, reducing the step size in dense obstacle areas to improve obstacle avoidance accuracy, and increasing the step size in open areas to improve search efficiency; in the collaborative scheduling phase, the degree of terrain congestion is perceived in real time and priority order is determined accordingly, making the scheduling decision terrain-adaptive and reducing the probability of congestion in narrow areas for multiple agents; by recursively resolving position occupancy conflicts through a priority inheritance mechanism, loop waiting is broken and livelock is eliminated, enabling global path guidance and terrain-aware scheduling to work together, improving the path search efficiency, success rate, and operational stability of the multi-agent system in complex environments; 2. This invention expands the path with a denser step size in areas with dense obstacles, making the path point distribution more detailed, thereby improving the safety and feasibility of local paths; and expands the path with a sparser step size in areas with sparse obstacles, thereby reducing the number of path points and planning time; by calculating the degrees of freedom of movement in real time and automatically identifying and predicting terrain congestion risks in narrow areas and triggering differentiated priority ranking, combined with the local minimum escape mechanism, the agent can autonomously get rid of itself when it gets stuck, ensuring the task completion rate and robustness of the multi-agent system in extremely complex scenarios. Attached Figure Description

[0017] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, for those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0018] Figure 1 This is an overall flowchart of the multi-agent cooperative path planning method of the present invention; Figure 2 This is a comparison chart of path planning between the traditional RRT asterisk algorithm and the improved RRT asterisk algorithm; Figure 3 This is a comparison chart of path length and planning time between the traditional RRT asterisk algorithm and the improved RRT asterisk algorithm; Figure 4 This is a simulation experiment diagram of multi-agent cooperative planning in the first map scenario in this embodiment of the invention; Figure 5 This is a simulation experiment diagram of multi-agent cooperative planning in the second map scenario in this embodiment of the invention; Figure 6 This is a simulation experiment diagram of multi-agent cooperative planning in the third map scenario of this invention. Figure 7 This is a schematic diagram comparing the number of collisions between the traditional PIBT algorithm and the improved PIBT algorithm in an embodiment of the present invention; Figure 8 This is a schematic diagram comparing the completion time of the traditional PIBT algorithm and the improved PIBT algorithm in an embodiment of the present invention; Figure 9 This is a schematic diagram comparing the number of stalling steps between the traditional PIBT algorithm and the improved PIBT algorithm in an embodiment of the present invention; Figure 10 This is a schematic diagram comparing the planning completion rates of the traditional PIBT algorithm and the improved PIBT algorithm in an embodiment of the present invention; Figure 11 This is a schematic diagram comparing the ablation experiment results of each group of algorithms in the embodiments of the present invention. Detailed Implementation

[0019] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0020] like Figures 1 to 11 As shown, a multi-agent path planning method integrating heuristic sampling and terrain perception includes the following steps: Acquire map data of the target environment, perform rasterization on the map data, and divide the map into multiple grid cells; Based on the distance from each grid cell to the target point and the distribution of obstacles within each grid cell, calculate the prior sampling weight for each grid cell. The system obtains the starting and target positions of multiple agents, performs global path planning based on the prior sampling weights of each grid cell, and generates a global path from the starting position to the target position for each agent. During the planning process, the generation of sampling points is guided by the prior sampling weights, and the path expansion direction is determined by coordinating the direction towards the target point with the random exploration direction. Multi-agent collaborative scheduling is carried out based on the current position and global path of each agent. During the scheduling process, the terrain congestion level of each agent's current position is perceived in real time. The priority order is determined according to the remaining distance from each agent to its target point and the terrain congestion level. The movement position is assigned to each agent according to the priority order. When a position occupancy conflict is detected, the conflict is recursively resolved through a priority inheritance mechanism. Each agent moves according to the scheduling result and updates its position status in real time. The scheduling and movement steps are executed repeatedly until all agents reach their respective target points. This invention enables path search to possess environmental awareness from the initial stage by constructing prior sampling weights—grid cells closer to the target and with sparser obstacles are assigned higher sampling probabilities, thereby concentrating random sampling points in promising areas. This effectively improves the path discovery rate in complex terrains such as narrow passages, avoiding the problems of numerous iterations and high path generation failure rates caused by blind uniform sampling in traditional RRT algorithms. By weightedly fusing the gravitational direction pointing to the target point with the random exploration direction, the search tree expansion direction takes into account both random exploration and target tendency, significantly reducing the number of iterations required for path convergence. Simultaneously, the expansion step size is dynamically adjusted according to the local obstacle density, allowing the algorithm to automatically reduce the step size in areas with dense obstacles to improve obstacle avoidance accuracy, and increase the step size in open areas to improve search accuracy. The system improves efficiency, achieving a good balance between path quality and planning speed. During the collaborative scheduling phase, it senses the terrain congestion level of each agent's current location in real time and determines the priority order based on the remaining distance to the target point and the terrain congestion level. This makes the scheduling decision adaptable to complex terrain, fundamentally reducing the probability of multiple agents clustering and congesting in narrow areas. When location conflicts occur, the conflict is recursively resolved through a priority inheritance mechanism, effectively breaking the cycle of waiting and eliminating livelock. Finally, the collision-free paths with clear destinations provided by global path planning and the multi-agent passage order dynamically adjusted by terrain perception scheduling based on real-time terrain feedback form a deep synergy, enabling the multi-agent system to achieve path search efficiency, multi-agent passage success rate, and overall system stability in complex environments.

[0021] Figure 2 This diagram illustrates a comparison of the traditional RRT star algorithm and the improved RRT star algorithm of this invention in global path planning. The RRT star algorithm (Rapidly-exploring Random Tree Star) is a rapidly expanding random tree star algorithm, a progressively optimal path planning algorithm based on random sampling. The traditional RRT star algorithm is the original, unmodified RRT star algorithm; the improved RRT star algorithm of this invention determines the path expansion direction by coordinating the direction towards the target point with the random exploration direction. As can be seen from the diagram, the traditional RRT star algorithm exhibits a scattered distribution of sampling points in complex areas such as narrow passages, with a large number of sampling points concentrated in obstacle areas or open, useless areas, resulting in slow search tree growth and a high path generation failure rate. In contrast, the improved RRT star algorithm of this invention, guided by prior sampling weights, distributes sampling points more frequently in narrow passage areas, allowing the search tree to quickly traverse bottleneck areas and effectively improve path discovery efficiency.

[0022] Figure 3 This diagram illustrates a comparison of path length and planning time between the traditional RRT asterisk algorithm and the improved RRT asterisk algorithm of this invention, as provided in an embodiment of the present invention. The diagram compares the average path length and average planning time of the two algorithms in multiple experiments under the same map environment. The results show that the improved RRT asterisk algorithm of this invention has a significantly shorter planning time than the traditional RRT asterisk algorithm, and also generates a better path length.

[0023] In one specific embodiment, the environment is provided by It consists of several rooms, each with a side length of [missing information]. r A square shape, with rooms separated by a thickness of d The walls separate the rooms; the overall map size L and the total number of rooms. The calculation formula is as follows: Based on the above parameters, the system can automatically generate two-dimensional map data including boundary information and the locations of obstacles (walls). Narrow passages (i.e., doors or corridors) are randomly opened between rooms, and each room also randomly opens passages between itself and its adjacent rooms. k A narrow passage, its position set at the midpoint offset of the wall, satisfies: By adjusting the location and number of passes, a complex indoor map topology with connectivity is constructed.

[0024] To quantify the complexity of a map, an effective obstacle density is defined. and bottleneck complexity index The specific expression is as follows: in, The total number of passes, The width of the pass; the bottleneck complexity index. The ratio of the number of narrow passageways to the openness of a room reflects the degree of enclosure of the environment. The smaller the value, the more isolated the environment, and the greater the challenge to multi-agent cooperative algorithms.

[0025] Based on the distance from each grid cell to the target point and the distribution of obstacles within each grid cell, a priori sampling weight is calculated for each grid cell, specifically including the following steps: For each grid cell, calculate the distance weight from the grid cell to the target point and the density weight within the grid cell based on the obstacle distribution. Multiply the distance weight and density weight corresponding to the grid cell to obtain the comprehensive weight of the corresponding grid cell; Sum the combined weights of all grid cells to obtain the total combined weight; Divide the comprehensive weight of each grid cell by the total comprehensive weight to obtain the normalized prior sampling weight of each grid cell.

[0026] The distance weight from the grid cell to the target point is calculated using the following formula: In the formula, Indicates distance weight, This represents the Euclidean distance from the center of the grid cell to the target point. Indicates the length of the map's diagonal; The density weights within a grid cell, based on the distribution of obstacles, are calculated using the following formula: In the formula, Indicates density weight, This indicates the obstacle density within a grid cell; Among them, obstacle density The specific expression is as follows: In the formula, The number of obstacles within a grid cell. This represents the total number of grid cells. The formulas for calculating the overall weight and the prior sampling weight are as follows: In the formula, Indicates the overall weight. This represents the prior sampling weight.

[0027] Global path planning is performed based on the prior sampling weights of each grid cell to generate a global path from the starting position to the target position for each agent. This includes the following steps: During the global path planning process, the target point is directly used as the sampling point with a first probability, and a grid cell is randomly selected based on the prior sampling weight of each grid cell with a second probability, and a sampling point is randomly generated within that grid cell; wherein, the first probability is less than the second probability, and the sum of the two is 1; The direction from the current latest path point to the sampling point is taken as the random exploration direction, and the direction from the latest path point to the target point is taken as the gravitational direction. The direction vector of the random exploration direction is weighted and summed with the direction vector of the gravity direction and then normalized to obtain the fused direction vector, which is used as the direction of this path expansion. Expand the path points sequentially according to the direction until a preliminary global path from the starting point to the target point is generated; Iterate through each non-adjacent path point in the path point sequence. When the line connecting two points does not pass through an obstacle, delete all intermediate path points between the two points to obtain the smoothed global path. In this embodiment, the first probability is 0.18 and the second probability is 0.82, that is, the target point is directly used as the sampling point with an 18% probability, and the grid cell is selected according to the prior sampling weight with an 82% probability and sampling points are randomly generated within it. Specifically, the path smoothing optimization adopts a shortcut pruning algorithm, which performs connectivity checks on each path point in the initially generated global path. When the connection between two path points does not pass through an obstacle, all intermediate path points between them are deleted, thereby eliminating redundant nodes and obtaining the shortest collision-free path. The specific formula for calculating the fusion direction vector is: In the formula, To fuse the direction vectors, Gravity-guided weights, The direction vector for random exploration direction, In this embodiment, the direction vector of gravity is... The value is 0.5.

[0028] The global path planning process also includes a step size dynamic adjustment step, which is used to adjust the step size of the current path expansion based on the density of local obstacles around the current expansion starting point. The step size decreases as the density of local obstacles around the current expansion point increases, and the step size is not lower than the preset minimum step size value. The local obstacle density around the starting point of the expansion is calculated using the following formula: in, The coordinates of the current starting point of the expansion. This indicates the local obstacle density around the current location. Indicates the current extension starting point to the th k The distance to each obstacle The preset sensing radius, This indicates the number of obstacles within a circular area centered at the current expansion starting point and with a preset sensing radius. This is an indicator function. It takes the value 1 when the condition in parentheses is true and 0 otherwise. That is, it counts the number of obstacles in a circular area with the current expansion starting point as the center and the preset sensing radius as the radius, and then divides it by the total number of grids to obtain the local obstacle density around the current expansion starting point. The specific calculation formula for dynamic adjustment of step size is as follows: In the formula, Based on step size, The adjusted step size; In this embodiment, the basic step size The set value is 0.5 grid units, and the preset sensing radius is 2.0 grid units. In the formula, 0.4 is the density adjustment coefficient, and 0.35 is the minimum step size ratio factor. That is, the adjusted step size is not less than 35% of the base step size to prevent the slow growth of the search tree caused by the step size being too small in extremely dense areas. This invention enables global path planning to be spatially adaptive by dynamically adjusting the step size. In areas with dense obstacles, a smaller expansion step size is automatically adopted, resulting in a finer distribution of path nodes. This allows for precise passage through narrow passages and avoidance of small obstacles, thereby improving the safety and feasibility of local paths. In areas with sparse obstacles, a larger expansion step size is automatically adopted, allowing the search tree to quickly traverse open areas. This effectively reduces the number of path nodes and planning time, thereby improving global search efficiency. The two strategies automatically switch based on environmental information without manual intervention, achieving a good balance between obstacle avoidance accuracy and convergence speed. This ensures high-quality path generation in complex terrain while avoiding search failures or wasted computational resources due to improper step size settings.

[0029] The degree of terrain congestion is represented by the degree of freedom of movement, which is the number of directions in which the agent's current position is neither subject to terrain collision nor occupied by other agents in the eight connected directions around it. When the degree of freedom of movement is less than or equal to the preset degree of freedom threshold, or when the distance between the current location and the map boundary is less than or equal to the preset boundary threshold, the location is marked as a narrow area; Preferably, the preset degree of freedom threshold is 3, and the preset boundary threshold is 1.5 grid side lengths. That is, when the number of directions in which the agent can move in the 8 connected directions is less than or equal to 3, or when the distance from the agent's current position to the map boundary is less than or equal to 1.5 grid side lengths, the position is determined to be a narrow area. Through this determination mechanism, the system can accurately identify narrow passages and boundary areas in the map, providing a perception basis for subsequent priority scheduling.

[0030] The priority order is determined based on the remaining distance of each agent to its target point and the degree of terrain congestion. Movement positions are then assigned to each agent according to this priority order, specifically including the following steps: Each agent is sorted from highest to lowest according to the following three-level priority: First priority: the smaller the remaining distance from the agent to its target point, the higher the priority. The second priority is that when the agent's current position is in a non-narrow region, the priority is higher than that of a narrow region. The third priority is that the greater the degree of freedom of movement of the agent at its current position, the higher the priority. Based on the ranking results, a set of candidate movement positions is generated for each agent. The candidate movement positions include the position to move along the global path direction, the current position to wait, and the position in the alternative direction. Candidate movement positions are sorted and optimized according to three levels of priority to determine the next movement position for each agent. When a position occupancy conflict is detected, the conflict is recursively resolved through a priority inheritance mechanism. In this embodiment, the candidate positions include positions in four orthogonal directions (up, down, left, and right), which, together with the position moving along the global path and the current position where the agent is waiting, constitute the candidate movement position set. For each candidate position, it is scored and sorted according to a three-level priority, and the optimal candidate position is selected as the agent's next movement position.

[0031] When a location conflict is detected, the conflict is recursively resolved through a priority inheritance mechanism, specifically including the following steps: Conflict detection is performed on each agent in sequence, including node conflict detection and edge conflict detection. Node conflict detection is used to check whether the target movement position of an agent has been occupied by another agent, and edge conflict detection is used to check whether the positions of two agents have been swapped. When a conflict occurs between the target movement positions of any agent, a recursive call mechanism is triggered, which passes the priority of the current agent to the agent occupying the position, and recursively determines the movement position of the occupying agent, so as to force it to make room for the current agent. Priority is passed recursively until a movable agent or the end of the recursion is reached. After the terminal agent in the recursion determines its move position, it returns level by level. Each agent completes the position allocation in turn according to the result of the recursive return, until all conflicts are resolved.

[0032] The process of each agent moving according to the scheduling results also includes local minimum escape, which specifically includes the following steps: The displacement of each agent is detected in multiple consecutive time steps. If the displacement of any agent in each time step is less than a preset stagnation threshold, the agent is determined to be trapped in a local minimum. In response to the decision, the direction vector of the agent’s current position pointing to the target point is obtained, the vertical direction vector of the direction vector is calculated, and a rotation angle randomly selected within the range of plus or minus ninety degrees is applied to the vertical direction vector to obtain the escape direction. Specifically, let the current position of the agent be... Target point location Then the direction vector from the current position to the target point is Its vertical direction vector The formula for calculation is: in, This represents the distance from the current agent's position to the target point, used to normalize the vertical vector. Direction vector The vertical direction; Based on the vertical direction, apply a rotation angle randomly selected within a range of ±90 degrees. The escape direction vector is obtained as follows: in, Let be a two-dimensional rotation matrix, and let be a random angle uniformly distributed within the range of -90 degrees to +90 degrees. This is the escape direction vector obtained after random rotation; An escape force is applied along the escape direction to guide the agent's movement; the escape force is greater than the driving force during normal movement. The formula for calculating escape force is: in, This is the escape force intensity coefficient, with a value of 50. This is the escape force vector ultimately applied to the agent; In this embodiment, the strength of the escape force is fifty times that of the driving force during normal movement, causing the agent to produce a significant displacement in a short period of time and get rid of the stagnant state; This invention enables a multi-agent system to possess self-recovery capabilities during execution through a local minimum escape step. When an agent is unable to move for an extended period due to obstacles caused by complex terrain or mutual avoidance with surrounding agents, the system can automatically identify the stagnation state by detecting displacement in real time, without requiring manual intervention or global replanning. In response to this identification result, an escape direction is generated by superimposing a random rotation amount on the direction perpendicular to the target direction, allowing the agent to explore new paths along non-target directions and bypass the current stuck area. The applied escape force is greater than the driving force during normal movement, enabling the agent to generate significant displacement in a short time, quickly escape the stagnation state, and avoid task timeouts or system paralysis caused by prolonged ineffective movement. This ensures that the multi-agent system can still successfully complete all tasks in extreme scenarios, greatly improving the system's robustness.

[0033] When the distance between the agent's current position and the preset target position is less than 0.1, the agent is considered to have completed its task. The system then enters a loop detection state until all participating agents have met the completion conditions, at which point the entire algorithm planning and execution process officially terminates.

[0034] Figure 4 , Figure 5 , Figure 6 These are simulation experiment diagrams illustrating multi-agent cooperative planning under three different maps, representing embodiments of the present invention. All three maps are constructed using... The system consists of several rooms connected by randomly opened narrow passages. However, the number of rooms, passages, and passage widths vary in each diagram, representing different levels of environmental complexity and occupancy. In the three diagrams, the starting point of each agent is marked with a circle, the target point with a star, and the agent's trajectory is represented by curves of different colors. As can be seen from the diagrams, the multi-agent system can start from its respective starting point and proceed along the planned path to its respective target point. In the narrow passage areas, the agents pass through in an orderly manner without severe congestion or stagnation, and ultimately all agents successfully reach their respective target points.

[0035] Figure 7This diagram illustrates a comparison of the number of conflicts between the traditional PIBT algorithm and the improved PIBT algorithm of this invention under three sets of maps with different parameters, as provided in this embodiment of the invention. The PIBT algorithm (Priority Inheritance with Backtracking) is a priority inheritance backtracking algorithm, a multi-agent conflict resolution algorithm based on a priority mechanism. The traditional PIBT algorithm is the original, unmodified PIBT algorithm; the improved PIBT algorithm of this invention is a PIBT algorithm with terrain perception capabilities, which determines the priority order based on the remaining distance of each agent to the target point and the degree of terrain congestion, and recursively resolves conflicts. As can be seen from the diagram, the improved PIBT algorithm has fewer conflicts than the traditional PIBT algorithm under different map parameters.

[0036] Figure 8 This diagram illustrates a comparison of the completion times of the traditional PIBT algorithm and the improved PIBT algorithm of this invention under three sets of maps with different parameters, as provided in this embodiment of the invention. As can be seen from the diagram, the completion time of the improved PIBT algorithm is shorter than that of the traditional PIBT algorithm under all map conditions.

[0037] Figure 9 This diagram illustrates a comparison of the number of stall steps between the conventional PIBT algorithm and the improved PIBT algorithm of this invention under three sets of maps with different parameters, as provided in this embodiment of the invention. As can be seen from the diagram, the improved PIBT algorithm has significantly fewer stall steps than the conventional PIBT algorithm.

[0038] Figure 10 This diagram illustrates a comparison of the planning completion rates of the traditional PIBT algorithm and the improved PIBT algorithm provided in this embodiment of the invention under three sets of maps with different parameters. As can be seen from the diagram, the improved PIBT algorithm outperforms the traditional PIBT algorithm across all map types. While the traditional PIBT algorithm fails on some highly complex maps, the improved PIBT algorithm consistently maintains a high completion rate.

[0039] Figure 11This diagram illustrates a comparison of ablation experiment results for multiple algorithms provided in this embodiment of the invention. The diagram compares the performance of four algorithm combinations—the traditional RRT star algorithm plus the traditional PIBT algorithm, the improved RRT star algorithm of this invention plus the traditional PIBT algorithm, the traditional RRT star algorithm plus the improved PIBT algorithm of this invention, and the improved RRT star algorithm plus the improved PIBT algorithm of this invention—in multi-agent cooperative planning. As can be seen from the diagram, all four algorithms achieved high planning success rates. However, the improved RRT star algorithm plus the improved PIBT algorithm proposed in this invention outperforms the other three control schemes in key indicators such as completion time, number of conflicts, and number of stall steps. This verifies the necessity and superiority of the global path guidance provided by the improved RRT star algorithm and the terrain-aware scheduling collaboration of the improved PIBT algorithm.

[0040] In the description of this specification, references to terms such as "an embodiment," "example," "specific example," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the invention. In this specification, illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples.

[0041] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The embodiments and descriptions in the specification are merely illustrative of the principles of the invention. Various changes and modifications can be made to the present invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the claimed invention.

Claims

1. A multi-agent path planning method integrating heuristic sampling and terrain perception, characterized in that, Includes the following steps: Acquire map data of the target environment, perform rasterization on the map data, and divide the map into multiple grid cells; Based on the distance from each grid cell to the target point and the distribution of obstacles within each grid cell, calculate the prior sampling weight for each grid cell. The system obtains the starting and target positions of multiple agents, performs global path planning based on the prior sampling weights of each grid cell, and generates a global path from the starting position to the target position for each agent. During the planning process, the generation of sampling points is guided by the prior sampling weights, and the path expansion direction is determined by coordinating the direction towards the target point with the random exploration direction. Multi-agent collaborative scheduling is carried out based on the current position and global path of each agent. During the scheduling process, the terrain congestion level of each agent's current position is perceived in real time. The priority order is determined according to the remaining distance from each agent to its target point and the terrain congestion level. The movement position is assigned to each agent according to the priority order. When a position occupancy conflict is detected, the conflict is recursively resolved through a priority inheritance mechanism. Each agent moves according to the scheduling result and updates its position status in real time. The scheduling and movement steps are executed repeatedly until all agents reach their respective target points.

2. The multi-agent path planning method integrating heuristic sampling and terrain perception according to claim 1, characterized in that, Based on the distance from each grid cell to the target point and the distribution of obstacles within each grid cell, a priori sampling weight is calculated for each grid cell, specifically including the following steps: For each grid cell, calculate the distance weight from the grid cell to the target point and the density weight within the grid cell based on the obstacle distribution. Multiply the distance weight and density weight corresponding to the grid cell to obtain the comprehensive weight of the corresponding grid cell; Sum the combined weights of all grid cells to obtain the total combined weight; Divide the comprehensive weight of each grid cell by the total comprehensive weight to obtain the normalized prior sampling weight of each grid cell.

3. The multi-agent path planning method integrating heuristic sampling and terrain perception according to claim 2, characterized in that, The distance weight from the grid cell to the target point is calculated using the following formula: In the formula, Indicates distance weight, This represents the Euclidean distance from the center of the grid cell to the target point. Indicates the length of the map's diagonal; The density weights within a grid cell, based on the distribution of obstacles, are calculated using the following formula: In the formula, Indicates density weight, This indicates the obstacle density within a grid cell; Among them, obstacle density The specific expression is as follows: In the formula, The number of obstacles within a grid cell. This represents the total number of grid cells.

4. The multi-agent path planning method integrating heuristic sampling and terrain perception according to claim 1, characterized in that, Global path planning is performed based on the prior sampling weights of each grid cell to generate a global path from the starting position to the target position for each agent. This includes the following steps: During the global path planning process, the target point is directly used as the sampling point with a first probability, and a grid cell is randomly selected based on the prior sampling weight of each grid cell with a second probability, and a sampling point is randomly generated within that grid cell; wherein, the first probability is less than the second probability, and the sum of the two is 1; The direction from the current latest path point to the sampling point is taken as the random exploration direction, and the direction from the latest path point to the target point is taken as the gravitational direction. The direction vector of the random exploration direction is weighted and summed with the direction vector of the gravity direction and then normalized to obtain the fused direction vector, which is used as the direction of this path expansion. Expand the path points sequentially according to the direction until a preliminary global path from the starting point to the target point is generated; Iterate through each non-adjacent path point in the path point sequence. When the line connecting two points does not pass through an obstacle, delete all intermediate path points between the two points to obtain the smoothed global path.

5. The multi-agent path planning method integrating heuristic sampling and terrain perception according to claim 4, characterized in that, The global path planning process also includes a step size dynamic adjustment step, which is used to adjust the step size of the current path expansion based on the density of local obstacles around the current expansion starting point. The step size decreases as the density of local obstacles around the current expansion point increases, and the step size is not lower than the preset minimum step size value. The local obstacle density around the starting point of the expansion is calculated using the following formula: in, The coordinates of the current starting point of the expansion. This indicates the local obstacle density around the current location. Indicates the current extension starting point to the th k The distance to each obstacle The preset sensing radius, This indicates the number of obstacles within a circular area centered at the current expansion starting point and with a preset sensing radius. The specific calculation formula for dynamic adjustment of step size is as follows: In the formula, Based on step size, This is the adjusted step size.

6. The multi-agent path planning method integrating heuristic sampling and terrain perception according to claim 1, characterized in that, The degree of terrain congestion is represented by the degree of freedom of movement, which is the number of directions in which the agent's current position is neither subject to terrain collision nor occupied by other agents in the eight connected directions around it. When the degree of freedom of movement is less than or equal to the preset degree of freedom threshold, or when the distance between the current location and the map boundary is less than or equal to the preset boundary threshold, the location is marked as a narrow area.

7. The multi-agent path planning method integrating heuristic sampling and terrain perception according to claim 6, characterized in that, The priority order is determined based on the remaining distance of each agent to its target point and the degree of terrain congestion. Movement positions are then assigned to each agent according to this priority order, specifically including the following steps: Each agent is sorted from highest to lowest according to the following three-level priority: First priority: the smaller the remaining distance from the agent to its target point, the higher the priority. The second priority is that when the agent's current position is in a non-narrow region, the priority is higher than that of a narrow region. The third priority is that the greater the degree of freedom of movement of the agent at its current position, the higher the priority. Based on the ranking results, a set of candidate movement positions is generated for each agent. The candidate movement positions include the position to move along the global path direction, the current position to wait, and the position in the alternative direction. Candidate movement positions are sorted and optimized according to a three-level priority system to determine the next movement position for each agent. When a position occupancy conflict is detected, the conflict is recursively resolved through a priority inheritance mechanism.

8. The multi-agent path planning method integrating heuristic sampling and terrain perception according to claim 7, characterized in that, When a location conflict is detected, the conflict is recursively resolved through a priority inheritance mechanism, specifically including the following steps: Conflict detection is performed on each agent in sequence, including node conflict detection and edge conflict detection. Node conflict detection is used to check whether the target movement position of an agent has been occupied by another agent, and edge conflict detection is used to check whether the positions of two agents have been swapped. When a conflict occurs between the target movement positions of any agent, a recursive call mechanism is triggered, which passes the priority of the current agent to the agent occupying the position, and recursively determines the movement position of the occupying agent, so as to force it to make room for the current agent. Priority is passed recursively until a movable agent or the end of the recursion is reached. After the terminal agent in the recursion determines its move position, it returns level by level. Each agent completes the position allocation in turn according to the result of the recursive return, until all conflicts are resolved.

9. The multi-agent path planning method integrating heuristic sampling and terrain perception according to claim 1, characterized in that, The process of each agent moving according to the scheduling results also includes local minimum escape, which specifically includes the following steps: The displacement of each agent is detected in multiple consecutive time steps. If the displacement of any agent in each time step is less than a preset stagnation threshold, the agent is determined to be trapped in a local minimum. In response to the decision, the direction vector of the agent’s current position pointing to the target point is obtained, the vertical direction vector of the direction vector is calculated, and a rotation angle randomly selected within the range of plus or minus ninety degrees is applied to the vertical direction vector to obtain the escape direction. An escape force is applied along the escape direction to guide the agent's movement; the escape force is greater than the driving force during normal movement.

Images