A Heterogeneous Multi-Robot Path Planning and Cooperative Obstacle Avoidance Method Based on Fusion Algorithm

Abstract

This invention relates to a heterogeneous multi-robot path planning and cooperative obstacle avoidance method based on a fusion algorithm, belonging to the field of mobile robot path planning and autonomous navigation technology. It addresses the problems of spatiotemporal trajectory conflicts, physical collisions, and the tendency of single algorithms to get trapped in local optima caused by differences in communication capabilities and physical dimensions among heterogeneous multi-robots. A discrete two-dimensional spatiotemporal grid map and a heterogeneous system model including connected and non-connected robots are constructed. Non-cascaded obstacle expansion preprocessing is performed on large-sized robots to transform physical volume constraints into environmental path constraints. An initial path is planned using a cosine adaptive genetic-ant colony fusion algorithm. Conflicts are detected based on a position-time matrix, and low-priority robots are locked according to dynamic priority. A spatiotemporal buffer expansion strategy is used to replan the path until there are no conflicts. This effectively improves global optimization efficiency and cooperative obstacle avoidance robustness, achieving conflict resolution without manual intervention, and balancing global path quality and real-time dynamic obstacle avoidance.

Description

Technical Field

[0001] This invention belongs to the field of mobile robot path planning and autonomous navigation technology, and relates to a heterogeneous multi-robot path planning and cooperative obstacle avoidance method based on a fusion algorithm. Background Technology With the deepening application of artificial intelligence technology in industrial manufacturing, warehousing and logistics, collaborative operation of mobile robot swarms has become a core means to improve production efficiency. Among them, heterogeneous multi-robot systems (HMRS) composed of robots of different types and capabilities have become a research hotspot in recent years due to their ability to adapt to complex scenarios. The heterogeneity of this type of system is mainly reflected in differences in physical size (such as large-sized cargo robots and small-sized inspection robots), asymmetry in communication capabilities (such as networked robots with bidirectional communication capabilities and non-networked robots that only support unidirectional status broadcasting), and differentiation of operational objectives.

[0002] Path planning, as a core technology for autonomous navigation of mobile robots, directly affects the safety and efficiency of swarm operations. Existing path planning algorithms are mainly divided into two categories: local path planning and global path planning. Local path planning methods (such as multi-strategy adaptive network algorithms, improved artificial potential field methods, and adaptive dynamic window methods) can achieve real-time obstacle avoidance in dynamic environments, but they generally suffer from insufficient adaptability in highly dynamic and strongly disturbed scenarios, easily getting trapped in local optima and failing to meet the stability requirements under complex working conditions. Global path planning methods (such as diversity population enhancement genetic algorithms, multi-strategy optimization slime mold algorithms, and improved ant colony algorithms) improve the single-machine optimization capability by optimizing search strategies, but they still suffer from slow convergence speed and premature convergence in high-dimensional solution spaces; moreover, these algorithms are mostly designed for single robots and do not consider the spatiotemporal conflicts in multi-robot collaborative operations, making them difficult to directly apply to heterogeneous swarm scenarios.

[0003] For multi-robot cooperative obstacle avoidance, existing research largely focuses on homogeneous robot systems, such as dynamic obstacle avoidance algorithms for UAVs based on optimal control and formation control methods based on virtual navigation. These methods lack robustness in cooperative control in unstructured environments and fail to consider the differences in physical size and communication capabilities among heterogeneous robots, making them unsuitable for direct application in mixed formation scenarios. Currently, research on path planning for heterogeneous multi-robot systems is relatively limited. Existing improved dragonfly algorithms and improved cat swarm algorithms primarily focus on task allocation and target localization, requiring high computational power from individual robot hardware, making them difficult to adapt to the complex needs of real-world industrial scenarios.

[0004] In real-world industrial manufacturing and warehousing logistics scenarios, heterogeneous robot swarms often face the need for mixed formation operations. On the one hand, the swarm includes robots with significantly different physical sizes (such as large 2×2 non-connected robots and small 1×1 connected robots); on the other hand, the robots' communication capabilities are hierarchically differentiated (such as connected robots with real-time replanning capabilities and low-cost non-connected robots that can only run along preset trajectories). Existing technologies neither fully consider the impact of such heterogeneous characteristics on path planning nor have effective spatiotemporal trajectory conflict resolution mechanisms. This leads to swarms being prone to physical collisions in narrow passages and areas with dense dynamic obstacles, severely restricting the operational safety and efficiency of heterogeneous systems.

[0005] In summary, there is an urgent need for a method that can take into account the physical and communication differences of heterogeneous robots and solve the problems of global path optimization and dynamic cooperative obstacle avoidance, so as to improve the operational reliability of heterogeneous multi-robot systems in complex dynamic environments. Summary of the Invention

[0006] In view of this, the purpose of this invention is to provide a heterogeneous multi-robot path planning and cooperative obstacle avoidance method based on a fusion algorithm.

[0007] To achieve the above objectives, the present invention provides the following technical solution: A method for heterogeneous multi-robot path planning and cooperative obstacle avoidance based on a fusion algorithm includes the following steps: S11: Construct a discrete two-dimensional spatiotemporal grid map, establish a heterogeneous multi-robot system model including connected robots (CR) and unconnected robots (UR), and determine the start and end positions and physical attributes of each robot; S12: Perform obstacle map inflation preprocessing on large-size robots to generate a dedicated passable spatiotemporal grid map, transforming the robot's physical volume constraints into environmental path constraints; S13: A genetic-ant colony fusion algorithm using a cosine adaptive parameter adjustment mechanism is used to plan the initial optimal path for all heterogeneous robots in a dedicated traversable spatiotemporal grid map. S14: Based on the position-time matrix, perform collision detection on the initial path, determine the spatiotemporal conflict points, obtain their time and position information, and use a priority setting strategy to lock low-priority robots. S15: A spatiotemporal no-go zone is constructed using a spatiotemporal buffer expansion strategy. The spatiotemporal grid map of low-priority robots is updated, and a fusion algorithm is called to replan the path for low-priority robots. The process is iterated until no conflict points can be detected, thus completing the heterogeneous multi-robot path planning and cooperative obstacle avoidance.

[0008] Furthermore, the obstacle map inflation preprocessing described in S12 for large-sized robots specifically includes: Establish a geometric occupancy model of a large-size robot and select specific corner points or geometric centers as motion reference points; Traverse the original grid map to detect the collision neighborhood state corresponding to the motion reference point; A non-cascading state identification mechanism is introduced: if an original static obstacle is detected in the collision neighborhood, the original obstacle's attributes are not directly modified. Instead, a specific expansion state value is assigned to the current node as a virtual obstacle to avoid cascading effects during sequential traversal of the expansion operation. A feasible configuration space map is generated based on the state identifiers containing the original static obstacles and the virtual obstacles, transforming the robot's physical volume constraints into positional constraints.

[0009] Furthermore, the genetic-ant colony fusion algorithm described in S13, which utilizes a cosine adaptive parameter adjustment mechanism, plans the initial optimal path for all heterogeneous robots, specifically including: When calculating the state transition probability, a dynamic parameter adjustment mechanism based on the cosine function is introduced, and a pheromone heuristic factor is defined. and expected heuristic factors The variation of χ with the number of iterations is as follows:

[0010]

[0011] In the formula, This represents the pheromone heuristic factor for the current iteration. This represents the expected heuristic factor for the current iteration. Indicates the current iteration number. Indicates the maximum number of iterations. This represents the initial value of the pheromone heuristic factor. This represents the initial value of the expected heuristic factor; In the path search generation stage, a diagonal crossing geometric constraint strategy is introduced: when both sides of the diagonal adjacent grid that the robot wants to transfer to are obstacles, the robot is forced to first translate along the coordinate axis to an obstacle-free middle node, and then turn to the target node. After the ant colony completes one round of path construction, the generated path set is transformed into the initial population for the genetic algorithm. A fitness function containing path length weights and trajectory smoothness weights is constructed, and crossover and mutation operations are performed on the population. The formula for the fitness function is as follows:

[0012] In the formula, Indicates path fitness. This represents the path length weighting coefficient. This represents the path smoothness weighting coefficient. n This indicates the total number of nodes contained in the path. Represents the node coordinates. D Represents Euclidean distance. This represents the path corner penalty function; A fusion feedback mechanism is established, and the pheromone matrix of the ant colony is updated in reverse using the globally optimal solution obtained by the genetic algorithm. The update formula is as follows:

[0013]

[0014] In the formula, This indicates the updated pheromone concentration. Indicates the pheromone evaporation coefficient. Q Represents the pheromone enhancement constant. This represents the globally optimal path in the current genetic algorithm population. This represents the total length of the globally optimal path.

[0015] Furthermore, the process described in S14—determining the spatiotemporal conflict point, obtaining its time and location information, and using a priority setting strategy to lock the low-priority robot—specifically includes: Establish a dynamic priority setting strategy to identify and actively avoid low-priority robots: For the non-networked robot ensemble RUR, the highest priority constant ψmax set by the system is assigned, making its path immutable and treating it as a dynamic obstacle. The priority formula is as follows:

[0016] For a networked robot ensemble RCR, its priority is directly proportional to the length of its planned initial path. That is, the longer the initial path, the higher the time delay cost and the higher the priority. The priority formula is as follows:

[0017] In the formula, Let Ri represent the i-th individual robot, and RU represent the set of non-networked robots. It represents a collection of connected robots. This represents a dynamic priority function. This represents the highest priority constant. This indicates the length of the planned initial path; Conduct offline obstacle avoidance simulations and use a position-time matrix model for spatiotemporal collision detection to determine spatiotemporal conflict points: Search for low-priority robots The set of position vectors at each time t It is then compared with the position vector sets of all high-priority robots, and the formula for its collision determination condition is:

[0018] In the formula, This indicates that any one with a higher priority High-priority robots Indicates the high-priority robot At time step t The set of spatiotemporal grids occupied; If the collision determination condition is met, i.e., the intersection of the spatiotemporal grid sets of the low-priority robot and the high-priority robot is not empty, then the earliest time step in which the collision occurred is recorded. and the center of the collision The time and location information of the collision point are obtained and marked as the first collision point.

[0019] Furthermore, the spatiotemporal buffer expansion strategy described in S15 for guiding low-priority robots to perform local path replanning specifically includes: For the first detected collision point, extract the high-priority robot occupancy set of the collision time and the time steps before and after it, using the following formula:

[0020] In the formula, This represents the first type of conflict spacetime set. This indicates that the high-priority robot is at time step t The occupied area Indicates the moment when the first collision was detected; Define the dilation operator For the aforementioned conflict spatiotemporal set any grid point in p ( x , y Perform an eight-neighbor expansion, and take the union of the expansion results for all points to form the obstacle buffer zone. The formula is as follows:

[0021]

[0022] In the formula, Represents the set of eight neighboring coordinates. This represents the generated obstacle buffer zone; The obstacle buffer is mapped to the cognitive map of the low-priority robot for environment matrix update, and the formula is as follows:

[0023] In the formula, This represents the updated local environment matrix. Represents the original environment matrix; The low-priority robot, based on the updated local environment matrix, again invokes the genetic ant colony fusion algorithm for path replanning. It then performs collision detection again on the replanned path. If a new collision point is found, a new expansion buffer is added, and the detection, expansion, and replanning process is repeated until the entire path is conflict-free. The iteration termination determination formula is:

[0024] In the formula, This represents the final conflict-free position vector set of the low-priority robot. This represents the union of the areas occupied by all high-priority robots.

[0025] The beneficial effects of this invention are as follows: (1) This invention constructs a heterogeneous multi-robot system model, which integrates the differences in physical size and communication capabilities into the path planning logic, thus solving the limitation that traditional methods are only applicable to homogeneous robots. By introducing an obstacle expansion preprocessing mechanism with non-cascaded state identifiers, the physical volume constraints of large-size robots are accurately transformed into environmental position constraints, avoiding the problem of incorrect obstacle boundary expansion caused by cascade effects in traditional expansion algorithms, and fundamentally eliminating the risk of corner collisions in narrow passages.

[0026] (2) The cosine adaptive genetic-ant colony fusion algorithm proposed in this invention balances the early global exploration capability and the later local development efficiency of the algorithm by dynamically adjusting the nonlinear variation law of the pheromone heuristic factor and the expected heuristic factor, effectively overcoming the technical defects of the single ant colony algorithm that is prone to getting trapped in local optima and has a slow convergence speed. Combined with the diagonal crossing geometric constraint strategy and genetic operator optimization, the smoothness and feasibility of the path are further improved, ensuring that the planning results conform to the actual motion characteristics of the robot.

[0027] (3) Through the dynamic priority setting strategy, the present invention can automatically allocate right-of-way based on the robot's communication capability and path length, and can realize the division of avoidance responsibility in conflict scenarios without manual intervention. With the spatiotemporal buffer expansion strategy, it can guide low-priority robots to actively perform safe obstacle avoidance without interfering with the normal operation of high-priority robots, and achieve spatiotemporal trajectory decoupling of the whole system with minimal local replanning cost, which significantly improves the safety and efficiency of heterogeneous cluster collaborative operation.

[0028] (4) This invention reduces the dimensionality of the three-dimensional physical collision avoidance problem to the problem of updating no-entry zones in a two-dimensional spatiotemporal grid map, thereby reducing the algorithm complexity and computing power requirements, making it suitable for the deployment requirements of low-cost controllers in industrial sites. The overall solution takes into account both global path quality and dynamic obstacle avoidance real-time performance, and exhibits strong robustness and scalability in complex dynamic environments, providing reliable technical support for the practical application of heterogeneous robot systems in warehousing and logistics, intelligent manufacturing and other fields.

[0029] Other advantages, objectives, and features of the invention will be set forth in part in the description which follows, and in part will be apparent to those skilled in the art from the following examination, or may be learned from practice of the invention. The objectives and other advantages of the invention can be realized and obtained through the following description. Attached Figure Description

[0030] To make the objectives, technical solutions, and advantages of the present invention clearer, the preferred embodiments of the present invention will be described in detail below with reference to the accompanying drawings, wherein: Figure 1 Here is a flowchart of the path planning method; Figure 2 The eight-neighbor expansion method is used. Figure 3 This refers to a diagonal path that crosses a geometrically restricted path. Figure 4 The path is a diagonal line crossing geometric constraints. Figure 5 Map of the first round of spatial expansion for CR1; Figure 6 To ultimately decouple the multidimensional path planning graph; Figure 7 To finally decouple the 3D path planning diagram; Figure 8 This is the final two-dimensional spatiotemporal path map for a 30×30 map. Figure 9 This is the final 3D spatiotemporal path map for a 30x30 map. Figure 10 The distance between the connected robot and the non-connected robot under the initial path planning; Figure 11 The distance between connected and non-connected robots in the final path planning. Detailed Implementation

[0031] The following specific examples illustrate the implementation of the present invention. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and various details in this specification can be modified or changed based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments are only schematic representations of the basic concept of the present invention. Unless otherwise specified, the following embodiments and features can be combined with each other.

[0032] The accompanying drawings are for illustrative purposes only and are schematic diagrams, not actual pictures. They should not be construed as limiting the invention. To better illustrate the embodiments of the invention, some parts in the drawings may be omitted, enlarged, or reduced, and do not represent the actual product dimensions. It is understandable to those skilled in the art that some well-known structures and their descriptions may be omitted in the drawings.

[0033] In the accompanying drawings of the embodiments of the present invention, the same or similar reference numerals correspond to the same or similar components. In the description of the present invention, it should be understood that if terms such as "upper," "lower," "left," "right," "front," and "rear" indicate the orientation or positional relationship based on the orientation or positional relationship shown in the drawings, they are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, the terms used to describe positional relationships in the drawings are only for illustrative purposes and should not be construed as limiting the present invention. For those skilled in the art, the specific meaning of the above terms can be understood according to the specific circumstances.

[0034] One embodiment of the present invention provides a heterogeneous multi-robot path planning and cooperative obstacle avoidance method based on a fusion algorithm, such as... Figure 1 As shown in the flowchart of the complete algorithm for path planning and cooperative obstacle avoidance, this method focuses on resolving the spatiotemporal trajectory conflicts of heterogeneous robots in highly dynamic environments. The specific implementation steps are detailed below: 1. High-level abstraction of the system environment and mathematical modeling of heterogeneous multi-robots Before performing substantive path planning and optimization, it is necessary to first rigorously define the physical workspace and the robot's physical and communication task attributes mathematically. This embodiment maps the physical workspace as a two-dimensional discretized grid map, defining a discrete grid. The status is as follows:

[0035] The system in this embodiment includes A heterogeneous robot, establishing a heterogeneous multi-robot system model (e.g.) Figure 1(Modeling of a heterogeneous multi-robot system is shown). This heterogeneous system mainly consists of two types of robots: one type has bidirectional network communication capabilities and is [size missing]. One type is the connected robot (CR), which occupies 1 unit of space in a grid map; the other type only has unidirectional status transmission capabilities and is small in size. The non-connected robot (UR) occupies 4 units of space in a grid map. This dual difference in physical size and communication sensing capabilities constitutes the core heterogeneity problem that this invention needs to solve.

[0036] For any robot in the system To construct standardized task tuples for it In this tuple, Indicates the robot's starting coordinates. Indicates the target coordinates of the robot. This indicates the constant speed maintained by the robot during its operation, while This is the core field used to identify the robot type (connected or non-connected), which directly determines the robot's priority and spatial occupancy characteristics in subsequent collision avoidance algorithms.

[0037] 2. Non-cascaded obstacle expansion preprocessing (OIP) for large-size robots Because traditional path planning algorithms generally treat mobile devices as dimensionless point masses, direct application would result in devices with actual physical dimensions (such as...). Non-connected robots may collide with obstacles in confined spaces. To address this, embodiments of the present invention perform a large-size robot obstacle expansion preprocessing operation. Specifically, for devices with a size of... For the heterogeneous robot, its top-left corner is selected as the standard motion reference point. The system sequentially traverses each free grid node in the original grid map. It detects the physical collision neighborhood state corresponding to the reference point in real time. Taking robots as an example, the system needs to synchronously detect reference points. Right side Nodes, below Nodes and the bottom right Node State. To completely resolve the "cascading effect" (i.e., virtual obstacles generated in the current step are misjudged as real obstacles by subsequent algorithm steps, leading to erroneous and infinite expansion of obstacle boundaries) caused by the traditional expansion algorithm during matrix sequential traversal, this embodiment innovatively introduces a non-cascading state identification mechanism: if an original static obstacle with a value of 1 is detected in the aforementioned detection neighborhood, the system does not directly update the current baseline node. Instead of marking it as 1, it is assigned a specific expanded intermediate state value (e.g., setting Value=2). This intermediate state value physically marks the current node as "impassable," but in the underlying traversal logic of the algorithm, this value is never considered a "original obstacle." Through this mechanism, the system generates an accurate feasible configuration space map. This perfectly transforms the mechanical volume constraints in the original physical environment into single location constraints in the new map, such as... Figure 2 As shown.

[0038] 3. Initial Optimization of Genetic Ant Colony Fusion Algorithm Based on Cosine Adaptive Mechanism (GA-ACO) An initial globally optimal path is independently planned for each heterogeneous robot. To address the serious drawbacks of the classic ant colony algorithm, which is prone to getting trapped in local minima and has slow convergence speed in later stages, this embodiment constructs a fusion algorithm to overcome these limitations: A1. Transition probability calculation based on cosine adaptive mechanism: In order to achieve an optimal balance between the extensive exploration capability in the early stage of the algorithm and the fast convergence speed in the later stage, a pheromone heuristic factor is defined. and expected heuristic factors With the number of iterations It exhibits nonlinear variation:

[0039]

[0040] In the formula, The maximum number of iterations set for the system. and These are the initial settings. During the iteration process, the pheromone heuristic factor... The algorithm exhibits a decreasing cosine curve, gradually reducing its dependence on historical error pheromones to avoid premature convergence; the expected heuristic factor... This shows an increasing trend, continuously strengthening the weight of goal-oriented factors to accelerate optimization. Based on this, the node state transition probability is calculated:

[0041] A2. Forced intervention of diagonal crossing geometric constraints: In optimization calculations, if the robot expects to move from the current coordinates... Move to the adjacent grid in the diagonal direction And detected adjacent grid cells on both sides ( and Both points are obstacles, representing typical angled terrain. Although the starting and ending points are free spaces, the robot's physical width limits direct diagonal crossing, leading to a corner collision. This triggers a geometric constraint mechanism: forcing the path segment to fail, requiring the robot to sacrifice local path smoothness and first translate along the X or Y axis to an obstacle-free intermediate transition node before turning to the final target node. This fundamentally eliminates illegal corner-crossing movements. Figure 3 and Figure 4 As shown.

[0042] A3. Population Conversion and Genetic Operator Deep Optimization: After the ant colony completes a round of coarse path construction, it generates a huge set of paths. Direct global mapping is transformed into the initial population for the genetic algorithm. To select high-quality paths, a comprehensive fitness function that balances total path length and trajectory smoothness is constructed:

[0043] In the formula, For Euclidean distance, This is the corner penalty function calculated for three consecutive nodes. The corner penalty function... The calculation mechanism is as follows: when three consecutive nodes are in a straight line, the penalty value is 0, and when they turn at a right angle or acute angle, a maximum penalty value is given, thereby guiding the algorithm to eliminate broken line paths. and To balance the weights, the system then finds the common node set of the two paths and performs a crossover operation, adjusting the mutation probability at any cutoff point in the path. Perform barrier-free straight-line reconnection:

[0044] Local optimization sub-path The generation logic is: first judge If there are obstacles obstructing the path, a straight physical connection is used to generate the path if there are no obstacles. If there are obstacles, a local optimization algorithm is used to generate a short-range replacement path.

[0045] A4. Closed-loop reverse pheromone update mechanism: Obtaining the globally optimal solution optimized and selected by the genetic algorithm. This information is then fed back into the ant colony system to perform targeted enhancement and updates to the next generation of the pheromone matrix.

[0046]

[0047] In the formula, The pheromone evaporation coefficient, This is the pheromone enhancement constant. This mechanism ensures that each generation of ant colonies always stands on the shoulders of the "evolved" optimal path to embark on a new round of exploration, achieving a perfect closed-loop control between the underlying structure and the upper-level optimization.

[0048] 4. Spatiotemporal conflict detection and priority assignment for multiple heterogeneous robots Since each robot's initial path is independently planned, parallel operation by the group inevitably leads to collisions. Given that all robots maintain a uniform speed, and the total path length is strictly positively correlated with the total system time, a dynamic priority setting strategy is implemented to minimize obstacle avoidance costs. This strategy is applied to assemblies of non-connected robots lacking environmental awareness and online replanning capabilities. Directly assign the highest priority constant to the system :

[0049] The physical significance of this setting lies in forcibly recognizing non-networked individuals as the absolute right-of-way owners within the system, treating their trajectories as dynamic obstacles. This applies to assemblies of networked robots with real-time communication capabilities. Establish a mapping relationship between priority and path length:

[0050] Long-path connected robots are given higher priority than individuals with shorter paths because of the extremely high sunk costs of obstacle avoidance delays.

[0051] Furthermore, the system introduces a peer-priority mechanism: when the initial planned path lengths of two connected robots are equal or the difference is less than a set threshold, their unique robot IDs are compared, and the one with the smaller ID obtains priority. This mechanism absolutely guarantees that the priority of any two robots in the system has a unique solution, completely eliminating logical deadlocks caused by peer avoidance.

[0052] After establishing the priority gradient, an offline obstacle avoidance simulation is performed: the system retrieves low-priority robots. The spatiotemporal matrix, and its time step position vector set The position vectors of the high-priority clusters are compared. The absolute criterion for collision determination is:

[0053] Once the intersection of the sets is not empty, the system immediately circuit breaks and accurately records the time step of the first physical overlap. The specific coordinates are recorded as the first collision point.

[0054] 5. Generation of eight-neighbor dynamic buffer zones and decoupling and replanning of spatiotemporal trajectory For the first intercepted collision point, a buffer expansion penalty mechanism based on the spatiotemporal domain is implemented. The system first extracts the absolute occupancy set of high-priority robots at the moment of the collision and within the adjacent time steps before and after. :

[0055] Using the defined space dilation operator All high-risk grid points within this set Forced eight-neighbor radioactive spread:

[0056] Merge and expand the results to generate an insurmountable three-dimensional obstacle buffer zone. :

[0057] The system directly maps and overlays this punitive buffer onto the local cognitive map of the low-priority obstacle avoidance robot:

[0058] The low-priority robot receives the updated environment matrix Then, the genetic ant colony fusion algorithm is reactivated to perform local replanning. Simultaneously, the boundary states of the target node are defined: if the robot... At time step Reaching target coordinates Then in In the position-time matrix, the position of the target node is converted into a static obstacle for global updating to prevent subsequent low-priority robots from colliding with it. Since the conflict source has been physically isolated, the newly generated detour path will naturally avoid this spacetime region. The system continuously iterates the above "detection-expansion-replanning" logic on the new path until no second or third collision points are detected, i.e., satisfying:

[0059] At this point, the spatiotemporal trajectories of multiple heterogeneous robots are completely decoupled, and the system outputs the final set of safe and cooperative paths.

[0060] 6. Simulation Verification and Effect Analysis To fully verify the technological advancement of this invention, an experimental environment was built using simulation software. The core parameters were configured as follows: ant colony size 100, maximum number of iterations 100, initial value of pheromone evaporation factor 2.0, initial value of expected heuristic factor 6.0, genetic mutation probability 0.05, and crossover probability 0.20.

[0061] The complete implementation process of the present invention is as follows: Figure 1As shown, the system strictly follows a closed-loop logic from information acquisition, geometric collision avoidance, adaptive optimization to eight-neighbor expansion replanning. In a large-scale three-machine mixed-system scenario (including one low-priority connected CR1 and two high-priority non-connected UR1 / UR2), the spatiotemporal decoupling mechanism was verified. When CR1 detects an impending fatal conflict with UR1, as follows... Figure 5 As shown in the first round of spatial expansion map of CR1, the system decisively performed an eight-neighbor expansion operation on the expected trajectory of UR1. The forbidden buffer zone, constructed of dark gray squares, is clearly visible on the map. After iterative game theory, as... Figure 6 and Figure 7 As shown, Figure 6 The CR1 autonomously completed a safe detour. Figure 7 The three-dimensional spatiotemporal evolution diagram further intuitively confirms the complete separation of the three mechanical operating trajectories in three-dimensional space, and the risk of physical collision is completely eliminated.

[0062] To push the limits of algorithm performance, further research is needed. In a large warehouse setting, six high-density heterogeneous robots were deployed to engage in a game-like scenario. For example... Figure 7 and Figure 8 The final multi-dimensional spatiotemporal path map, showing the final battle situation, demonstrates that the connected robot swarm, at the cost of only a slight increase of 8.92% in the average total path length, managed to create straight, safe passages for high-priority non-connected vehicles within extremely narrow and confined remaining free space, thus constructing a highly efficient, non-interference road network. The final safe distance measurement data is as follows: Figure 10 and Figure 11 The evolution diagram of the Euclidean distance between CR and UR in the initial and final states shows that, compared with the dangerous state of falling below the safety threshold at various frequencies when the obstacle avoidance mechanism is not activated ( Figure 10 After applying the spatiotemporal dynamic expansion technology of this invention, the real-time Euclidean distance between all bodies is forcibly and stably controlled above the safety red line. Figure 11 The system's collision avoidance robustness was directly verified by data.

[0063] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. A heterogeneous multi-robot path planning and cooperative obstacle avoidance method based on a fusion algorithm, characterized in that: Includes the following steps: S11: Construct a discrete two-dimensional spatiotemporal grid map, establish a heterogeneous multi-robot system model including connected robots (CR) and non-connected robots (UR), and determine the start and end positions and physical attributes of each robot. S12: Perform obstacle map inflation preprocessing on large-size robots to generate a dedicated passable spatiotemporal grid map, transforming the robot's physical volume constraints into environmental path constraints; S13: A genetic-ant colony fusion algorithm using a cosine adaptive parameter adjustment mechanism is used to plan the initial optimal path for all heterogeneous robots in a dedicated traversable spatiotemporal grid map. S14: Based on the position-time matrix, perform collision detection on the initial path, determine the spatiotemporal conflict points, obtain their time and position information, and use a priority setting strategy to lock low-priority robots. S15: A spatiotemporal no-go zone is constructed using a spatiotemporal buffer expansion strategy. The spatiotemporal grid map of low-priority robots is updated, and a fusion algorithm is called to replan the path for low-priority robots. The process is iterated until no conflict points can be detected, thus completing the heterogeneous multi-robot path planning and cooperative obstacle avoidance.

2. The heterogeneous multi-robot path planning and cooperative obstacle avoidance method based on the fusion algorithm according to claim 1, characterized in that: The obstacle map inflation preprocessing described in S12 for large-sized robots specifically includes: Establish a geometric occupancy model of a large-size robot and select specific corner points or geometric centers as motion reference points; Traverse the original grid map to detect the collision neighborhood state corresponding to the motion reference point; A non-cascading state identification mechanism is introduced: if an original static obstacle is detected in the collision neighborhood, the original obstacle's attributes are not directly modified. Instead, a specific expansion state value is assigned to the current node as a virtual obstacle to avoid cascading effects during sequential traversal of the expansion operation. A feasible configuration space map is generated based on the state identifiers containing the original static obstacles and the virtual obstacles, transforming the robot's physical volume constraints into positional constraints.

3. The heterogeneous multi-robot path planning and cooperative obstacle avoidance method based on the fusion algorithm according to claim 1, characterized in that: The genetic-ant colony fusion algorithm described in S13, which utilizes a cosine adaptive parameter adjustment mechanism, plans the initial optimal path for all heterogeneous robots, specifically including: When calculating the state transition probability, a dynamic parameter adjustment mechanism based on the cosine function is introduced, and a pheromone heuristic factor is defined. and expected heuristic factors The variation of χ with the number of iterations is as follows: In the formula, This represents the pheromone heuristic factor for the current iteration. This represents the expected heuristic factor for the current iteration. Indicates the current iteration number. Indicates the maximum number of iterations. This represents the initial value of the pheromone heuristic factor. This represents the initial value of the expected heuristic factor; In the path search generation stage, a diagonal crossing geometric constraint strategy is introduced: when both sides of the diagonal adjacent grid that the robot wants to transfer to are obstacles, the robot is forced to first translate along the coordinate axis to an obstacle-free middle node, and then turn to the target node. After the ant colony completes one round of path construction, the generated path set is transformed into the initial population for the genetic algorithm. A fitness function containing path length weights and trajectory smoothness weights is constructed, and crossover and mutation operations are performed on the population. The formula for the fitness function is as follows: In the formula, Indicates path fitness. This represents the path length weighting coefficient. This represents the path smoothness weighting coefficient. n This indicates the total number of nodes contained in the path. Represents the node coordinates. D Represents Euclidean distance. This represents the path corner penalty function; A fusion feedback mechanism is established, and the pheromone matrix of the ant colony is updated in reverse using the globally optimal solution obtained by the genetic algorithm. The update formula is as follows: In the formula, This indicates the updated pheromone concentration. Indicates the pheromone evaporation coefficient. Q Represents the pheromone enhancement constant. This represents the globally optimal path in the current genetic algorithm population. This represents the total length of the globally optimal path.

4. The heterogeneous multi-robot path planning and cooperative obstacle avoidance method based on the fusion algorithm according to claim 1, characterized in that: The process described in S14, which involves identifying spatiotemporal conflict points, obtaining their time and location information, and using a priority setting strategy to lock low-priority robots, specifically includes: Establish a dynamic priority setting strategy to identify and actively avoid low-priority robots: For the non-networked robot ensemble RUR, the highest priority constant ψmax set by the system is assigned, making its path immutable and treating it as a dynamic obstacle. The priority formula is as follows: For a networked robot ensemble RCR, its priority is directly proportional to the length of its planned initial path. That is, the longer the initial path, the higher the time delay cost and the higher the priority. The priority formula is as follows: In the formula, Let Ri represent the i-th individual robot, and RU represent the set of non-networked robots. It represents a collection of connected robots. This represents a dynamic priority function. This represents the highest priority constant. This indicates the length of the planned initial path; Conduct offline obstacle avoidance simulations and use a position-time matrix model for spatiotemporal collision detection to determine spatiotemporal conflict points: Search for low-priority robots The set of position vectors at each time t It is then compared with the position vector sets of all high-priority robots, and the formula for its collision determination condition is: In the formula, This indicates that any one with a higher priority High-priority robots Indicates the high-priority robot At time step t The set of spatiotemporal grids occupied; If the collision determination condition is met, i.e., the intersection of the spatiotemporal grid sets of the low-priority robot and the high-priority robot is not empty, then the earliest time step in which the collision occurred is recorded. and the center of the collision The time and location information of the collision point are obtained and marked as the first collision point.

5. The heterogeneous multi-robot path planning and cooperative obstacle avoidance method based on the fusion algorithm according to claim 1, characterized in that: The spatiotemporal buffer expansion strategy described in S15 for guiding low-priority robots to perform local path replanning specifically includes: For the first detected collision point, extract the high-priority robot occupancy set of the collision time and the time steps before and after it, using the following formula: In the formula, This represents the first type of conflict spacetime set. This indicates that the high-priority robot is at time step t The occupied area Indicates the moment when the first collision was detected; Define the dilation operator For the aforementioned conflict spatiotemporal set any grid point in p ( x , y Perform an eight-neighbor expansion, and take the union of the expansion results for all points to form the obstacle buffer zone. The formula is as follows: In the formula, Represents the set of eight neighboring coordinates. This represents the generated obstacle buffer zone; The obstacle buffer is mapped to the cognitive map of the low-priority robot for environment matrix update, and the formula is as follows: In the formula, This represents the updated local environment matrix. Represents the original environment matrix; The low-priority robot, based on the updated local environment matrix, again invokes the genetic ant colony fusion algorithm for path replanning. It then performs collision detection again on the replanned path. If a new collision point is found, a new expansion buffer is added, and the detection, expansion, and replanning process is repeated until the entire path is conflict-free. The iteration termination determination formula is: In the formula, This represents the final conflict-free position vector set of the low-priority robot. This represents the union of the areas occupied by all high-priority robots.

Images