A method and system for cluster robot cooperative operation based on hierarchical path planning

Abstract

A kind of cluster robot cooperative operation method and system based on hierarchical path planning, wherein the cluster robot cooperative operation method comprises: the spatial partitioning of the environment map of cluster robot operation is carried out, and a plurality of unobstructed cells are obtained;Improved RRT* algorithm is used to preliminarily plan the path of cluster robot, and heuristic function is combined to guide the expansion of tree structure to target cell boundary;The path conflict between multiple measurement robots is coordinated using CBS algorithm, and a collision-free path set is formed;CBS algorithm is used to uniformly plan the path for one or more tracking robots;The path of all measurement robots and the path of each tracking robot are respectively subjected to path smoothing processing, and the final planned collision-free path is obtained.The present application can significantly improve the path planning efficiency and target orientation by dividing the operation environment into bounded cells and combining improved RRT* algorithm to sample at cell boundary.

Description

Technical Field

[0001] This invention relates to the field of robot path planning technology, and in particular to a method and system for collaborative operation of swarm robots based on hierarchical path planning. Background Technology

[0002] In swarm robot operations, path planning and collaborative control of multiple robots are key technologies for achieving efficient operation. Traditional path planning methods, such as Rapid Random Tree Exploration (RRT) and its improved algorithms, suffer from low path planning efficiency and poor path quality when dealing with complex environments. In particular, the generated paths contain numerous redundant nodes and sharp corners, affecting the robot's motion stability and energy consumption. Furthermore, in multi-robot collaborative operation scenarios, effectively avoiding path conflicts between multiple robots to achieve safe and efficient collaborative operation is also a pressing issue. Existing conflict detection and coordination methods struggle to simultaneously meet the specific operational needs of various robot types when handling swarm systems composed of multiple robots with different functions (such as measurement robots and tracking robots). Moreover, during actual operation, tracking robots are prone to losing perception of the target measurement robot due to environmental occlusion, signal interference, and other factors. Current technologies lack effective perception recovery mechanisms and tracking distance maintenance strategies, leading to tracking task failure and reducing the overall operational efficiency and reliability of the swarm robot system. Therefore, a new method is urgently needed to address the numerous problems in multi-robot path planning and collaborative control. Summary of the Invention

[0003] This invention provides a method and system for collaborative operation of cluster robots based on hierarchical path planning, in order to solve the technical problems mentioned in the background art.

[0004] To achieve the above objectives, the technical solution of the present invention is implemented as follows:

[0005] This invention provides a method for collaborative operation of swarm robots based on hierarchical path planning, comprising the following steps:

[0006] S1. The environment map of the swarm robot operation is spatially partitioned using obstacle detection and bounded cell partitioning methods to obtain multiple obstacle-free cells; the swarm robot includes several tracking robots and multiple measuring robots.

[0007] S2. The improved RRT* algorithm is used to perform preliminary path planning for the cluster robots to obtain an initial path set. The improvement of the improved RRT* algorithm is that it adopts a boundary constraint-guided adaptive sampling strategy for node sampling. The boundary constraint-guided adaptive sampling strategy dynamically adjusts the sampling node distribution according to the cell topology, target position, and obstacle distribution. In the node generation process, nodes are generated only on the cell boundary.

[0008] S3. Perform global conflict detection on all paths generated by the measurement robots, and use the CBS algorithm to coordinate path conflicts between multiple measurement robots to form a set of collision-free paths;

[0009] S4. Based on the set of collision-free paths of the measurement robot, the CBS algorithm is used to uniformly plan the tracking path for one or more tracking robots. At the same time, in the path optimization process of each tracking robot, the constructed path cost function ensures that the current tracking robot maintains a minimum set safe tracking distance with the multiple measurement robots responsible for tracking.

[0010] S5. When a tracking robot cannot perceive the position of any of the measurement robots it is responsible for, it enters the perception recovery mode. If the perception recovery fails, the tracking robot automatically terminates the perception recovery state and returns to S4 to replan the tracking robot's tracking path.

[0011] S6. Perform path smoothing on the paths of all measuring robots and the tracking paths of each tracking robot to obtain the final planned collision-free path.

[0012] Furthermore, step S1 specifically includes the following steps:

[0013] S11. Combining the radius of the cluster robot body and the set safety distance, perform shape expansion processing on the area where the obstacle is located in the environment map, and update the obstacle boundary in the environment map; among them, the measurement robot in the cluster robot is responsible for performing specific measurement tasks, and the tracking robot is responsible for acquiring the pose status of one or more measurement robots in real time.

[0014] S12. Using the bounded cell division method, scan each column of cells in the environment map from left to right. Through obstacle detection, merge adjacent passable cells in each column into one cell to obtain multiple barrier-free cells.

[0015] Furthermore, step S2 specifically includes the following steps:

[0016] S21. Using the starting point and target of all measurement robots as input to the improved RRT* algorithm, and based on the spatial topological relationship of the cells, combined with a heuristic function to guide the sampling direction, nodes are randomly sampled on the boundaries of multiple cells to generate nodes. ;

[0017] S22, Determine the distance node The nearest node Connecting nodes and nodes And find the nodes among the intersections of the line with the boundaries of multiple cells. The nearest node ;

[0018] S23, Determine the node With nodes Are there any obstacles between the lines? If not, proceed to S24; otherwise, proceed to S25.

[0019] S24, Node As a new node in the tree structure, and treat the new node as a node Then, it is determined whether the current iteration number has reached the set number. If so, the initial path planning ends and the initial path set is obtained; otherwise, proceed to S22.

[0020] S25, Abandon Node Then, it determines whether the current iteration count has reached the set number. If so, it ends the initial path planning and obtains the initial path set; otherwise, it returns to S21.

[0021] Furthermore, the heuristic function in S21 is specifically as follows:

[0022] ;

[0023] in, Represents a heuristic function. Indicates the location of the current sampling point. This indicates the endpoint of the path planning.

[0024] Furthermore, step S3 specifically includes the following steps:

[0025] S31. Perform global conflict detection on the generated measurement robot path and construct a spatiotemporal occupancy constraint table based on the initial path set;

[0026] S32. The high-level searcher in the CBS algorithm references the spatiotemporal occupancy constraint table and constructs the initial root node of the constraint tree in the "unconstrained" case.

[0027] S33. The high-level searcher refers to the spatiotemporal occupancy constraint table to perform conflict detection on the initial path set and determine whether there are multiple measurement robots occupying the same position at the same time or being less than the set safe distance threshold.

[0028] S34. Once a conflict is detected, the high-level searcher uses the conflict as a split point to generate two child nodes on the constraint tree. Each child node adds an exclusionary constraint to the conflicting measurement robot. Then, the low-level path planner is called only for the measurement robot with the exclusionary constraint to replan a complete path from the starting point to meet the new constraint conditions. The paths and historical constraints of all unaffected measurement robots are inherited as is by the two child nodes generated.

[0029] S35. The higher-level searcher performs conflict detection again. If path conflicts still exist, it returns to S34 until a globally conflict-free path is obtained, forming a set of collision-free paths.

[0030] Furthermore, the path cost function in S4 is specifically as follows:

[0031] ;

[0032] in, Indicates the cost of the current tracking path; As a regulating factor; This indicates the position of the tracking robot at time t. This represents the position of the i-th measuring robot being tracked by the tracking robot at time t. For the first The tracking weight of a measurement robot, where N represents the total number of measurement robots that the current tracking robot is responsible for tracking; and T represents the total tracking time.

[0033] Furthermore, step S5 specifically includes the following steps:

[0034] S51. If the current tracking robot cannot perceive the position of any of the measurement robots it is responsible for, it enters the perception recovery mode.

[0035] S52. After entering the perception recovery mode, the current tracking robot first uses the last observable position of the most recent measurement robot in its target set as the center to construct a circular orbital trajectory.

[0036] S53. The current tracking robot circles the center and follows a circular trajectory to the designated position, and attempts to restore perception of any of the measurement robots it is responsible for; if the longest perception recovery time is reached... If the current tracking robot's detour radius has reached the maximum detour radius, then the perception recovery is considered to have failed. The automatic perception recovery of the tracking robot will be terminated and the process will revert to S4 to re-execute the current tracking robot's path planning.

[0037] Furthermore, the expression for the circular trajectory in S52 is as follows:

[0038] ;

[0039] in, This indicates the position of the tracking robot at time t; Indicates the coordinates of the center of the circle; Indicates the dynamic radius; Let represent the orbital angular velocity at time t; This represents the instantaneous lateral offset component.

[0040] Furthermore, the implementation method in S53 where the current tracking robot revolves around the center and travels along a circular trajectory to the designated position is as follows:

[0041] Each time the tracking robot rotates around the center of the circle and follows a circular trajectory, it rotates by a set angle. Then, it calculates the minimum distance between the current tracking robot and all other robots in the cluster. ;when Less than the set safety threshold Immediately adjust the dynamic radius. To achieve expansion, that is ,in This represents the difference in radius of the current expansion circle; if Three consecutive sampling periods are greater than Then The radius is reduced in a certain way, where Indicates the initial detour radius, the starting detour radius. Set as the tracking robot's body radius and the desired safe distance The sum of the components; the radius of the tracking robot body refers to the minimum radius obtained by fitting the outer contour of the tracking robot, which is used to measure the space occupied by the tracking robot;

[0042] The angular velocity ω(t) of orbit varies proportionally with the radius, satisfying the following condition: ,in Indicates the initial orbital angular velocity;

[0043] Instantaneous lateral offset components Multiply the unit vertical vector of the line connecting the nearest swarm robots to the currently tracking robot by the offset. Generate; when A lateral offset is triggered once, and then decays exponentially to zero over three sampling periods to avoid continuous oscillation. The specific formula for exponential decay is as follows:

[0044] , ;

[0045] in, , These represent the instantaneous lateral offset components on the x and y axes, respectively. Indicates the attenuation coefficient. Represents the natural constant;

[0046] To ensure simultaneous observation of multiple targets, if the position of any target in the set of measurement robots currently being tracked by the tracking robot exceeds the field of view, the system will execute the following steps first: Force expansion of the circle, then press Reduce angular velocity until all targets re-enter the field of view.

[0047] In another aspect, the present invention provides a swarm robot collaborative operation system, including a swarm of robots, and further including the following modules:

[0048] The partitioning module is used to spatially partition the environment map, using the Bounded Cell Division (BCD) method to divide the free space into multiple barrier-free cells;

[0049] The sampling module is used to sample based on the cell boundary using an improved RRT* algorithm. The extended child nodes are only on the cell boundary, and a heuristic function is used to guide the path to extend to the target cell boundary.

[0050] The measurement robot path planning and coordination module is used to perform parallel path planning for multiple measurement robots and to perform conflict detection and resolution on their initial paths using the CBS algorithm, generating a set of collision-free paths for all measurement robots.

[0051] The tracking robot path planning module is used to generate collision-free tracking paths for one or more tracking robots using the CBS algorithm, with the generated set of collision-free paths for measurement robots as constraint inputs.

[0052] An optimization module is used to introduce a multi-target distance preservation term into the path cost function of the tracking robot path, so that the tracking robot maintains at least a set safe tracking distance with the multiple measurement robots responsible for tracking during operation;

[0053] The perception recovery module is used to control the tracking robot to enter the perception recovery mode when any tracking robot cannot observe any of the measurement robots it is responsible for.

[0054] The path smoothing module uses the B-spline method to smooth the path, thereby obtaining the final planned collision-free path.

[0055] The beneficial effects of this invention are:

[0056] This invention discloses a collaborative operation method for swarm robots based on hierarchical path planning. It divides the environmental map into multiple obstacle-free cells using the Bounded Module Division (BCD) method and expands obstacles based on robot dimensions, thus narrowing the search range and fundamentally avoiding collision risks, improving planning efficiency and safety. At the path planning level, the improved RRT* algorithm employs a boundary constraint-guided adaptive sampling strategy, achieving better paths and higher path planning efficiency compared to traditional methods. This strategy dynamically adjusts the sampling node distribution based on cell topology, target location, and obstacle distribution, and nodes are generated only on cell boundaries. Furthermore, this invention utilizes a hierarchical CBS algorithm for path conflict resolution, significantly improving the collaborative operation of the swarm robots. For path smoothing, this invention uses the B-spline method to smooth the planned paths, achieving continuous path curvature, reducing motion losses in the swarm robots, and improving operational efficiency while saving energy and reducing emissions. Attached Figure Description

[0057] Figure 1 This is a flowchart of the collaborative operation method for cluster robots in this invention;

[0058] Figure 2 This is an example diagram of the final planned collision-free path in an embodiment of the present invention. Detailed Implementation

[0059] To facilitate understanding of the present invention, a more complete description will be given below with reference to the accompanying drawings. Preferred embodiments of the invention are shown in the drawings. However, the invention can be implemented in many other different forms and is not limited to the embodiments described herein. Rather, these embodiments are provided to provide a thorough and complete understanding of the disclosure of the invention.

[0060] Reference Figure 1 This application provides a method for collaborative operation of cluster robots based on hierarchical path planning, including the following steps:

[0061] S1. The environment map of the swarm robot operation is spatially partitioned using obstacle detection methods and bounded cell decomposition (BCD) to obtain multiple obstacle-free cells; the swarm robot includes several tracking robots and multiple measuring robots.

[0062] S2. The improved RRT* algorithm is used to perform preliminary path planning for the cluster robots to obtain an initial path set. The improvement of the improved RRT* algorithm is that it adopts a boundary constraint-guided adaptive sampling strategy for node sampling. The boundary constraint-guided adaptive sampling strategy dynamically adjusts the sampling node distribution according to the cell topology, target position, and obstacle distribution. In the node generation process, nodes are generated only on the cell boundary.

[0063] S3. Perform global conflict detection on all paths generated by the measurement robots, and use the CBS algorithm to coordinate path conflicts between multiple measurement robots to form a set of collision-free paths;

[0064] The CBS algorithm includes a high-level searcher and a low-level path planner. The high-level searcher uses a constraint tree (CT) structure to organize and manage multi-robot path conflicts.

[0065] S4. Based on the set of collision-free paths for measurement robots, the CBS (Conflict-Based Search) algorithm is used to uniformly plan tracking paths for one or more tracking robots. Simultaneously, during the path optimization process for each tracking robot, a constructed path cost function ensures that the current tracking robot maintains a minimum set safe tracking distance with the multiple measurement robots responsible for tracking. The CBS algorithm is a conflict-based multi-agent path planning algorithm that can provide the globally optimal result for multi-agent path planning problems.

[0066] S5. When a tracking robot is unable to perceive the position of any of the measurement robots it is responsible for due to obstruction, signal loss or other reasons, it enters the perception recovery mode. If the perception recovery fails, the tracking robot automatically terminates the perception recovery state and returns to S4 to replan the tracking robot's tracking path.

[0067] S6. The cubic B-spline method is used to smooth the paths of all measuring robots and the tracking paths of each tracking robot, thereby removing redundant nodes and sharp corners in the path, improving the path continuity and stability, and obtaining the final planned collision-free path. Figure 2 The example diagram of a final planned collision-free path is given.

[0068] The collaborative operation method for swarm robots disclosed in this invention has advantages such as efficient path planning, robust planning, and stable execution. By dividing the working environment into bounded cells and combining the improved RRT* algorithm for sampling at cell boundaries, the path planning efficiency and target orientation can be significantly improved. The CBS algorithm is used to perform global conflict detection and coordination on the paths of multiple measurement robots, ensuring that the paths of multiple measurement robots are free from spatiotemporal collisions and improving system safety. For the tracking robot, a weighted distance cost term is introduced into its path cost function, enabling it to stably follow multiple targets (i.e., measurement robots). A perception recovery mechanism is also included, which can automatically adjust the path and resume the tracking task in the event of occlusion or signal loss. Finally, path smoothing processing improves the continuity and executability of the path. The method provided by this invention is applicable to complex scenarios such as multi-robot collaborative measurement and monitoring, and has good scalability and engineering practical value.

[0069] In some embodiments, S1 specifically includes the following steps:

[0070] S11. Combining the radius of the cluster robot body and the set safety distance, perform shape expansion processing on the area where the obstacle is located in the environment map, and update the obstacle boundary in the environment map; among them, the measurement robot in the cluster robot is responsible for performing specific measurement tasks, and the tracking robot is responsible for acquiring the pose status (including position and attitude) of one or more measurement robots in real time, and transmitting, synchronizing or recording the spatial position information of the measurement head (or ball cage) on the measurement robot when necessary.

[0071] The environmental map is generated using Simultaneous Localization and Mapping (SLAM) technology, and each cell in the map indicates the drivability of the corresponding area. Before the system runs, environmental and robot information is initialized based on the environmental map (including area drivability) and the current pose and motion parameters of the gathering and dispersing robots. In this embodiment, the size of the environmental map is [size missing]. raster resolution Obstacles account for approximately 20% of the environmental map.

[0072] S12. Using the bounded cell division method, scan each column of cells in the environment map from left to right. Through obstacle detection, merge adjacent passable cells in each column into one cell to obtain multiple barrier-free cells.

[0073] In some embodiments, S2 specifically includes the following steps:

[0074] S21. Using the starting point and target of all measurement robots as input to the improved RRT* (Rapidly-exploring Random Tree Star) algorithm, and based on the spatial topological relationship of the cells, combined with a heuristic function to guide the sampling direction, nodes are randomly sampled and generated on the boundaries of multiple cells. The RRT* algorithm is a path planning algorithm for robots, designed to find the shortest path from the starting point to the target while avoiding obstacles. It is an improved version of the RRT (Rapidly-exploring Random Tree) algorithm, offering higher path quality and optimization capabilities. A key feature of the improved RRT* algorithm is its ability to progressively optimize the path during the search process, ultimately finding a path close to the shortest.

[0075] In this context, the spatial topological relationship of a cell refers to the adjacency and accessibility information between unobstructed cells after the bounded cell division is completed. Specifically, if two cells are spatially adjacent and their shared boundary has a passable area (i.e., without any obstacles), they are considered to have a topological connection.

[0076] S22, Determine the distance node The nearest node Connecting nodes and nodes And find the nodes among the intersections of the line with the boundaries of multiple cells. The nearest node ;

[0077] S23, Determine the node With nodes Are there any obstacles between the lines? If not, proceed to S24; otherwise, proceed to S25.

[0078] S24, Node As a new node in the tree structure, and treat the new node as a node Then, it is determined whether the current iteration number has reached the set number. If so, the initial path planning ends and the initial path set is obtained; otherwise, proceed to S22.

[0079] S25, Abandon Node Then, it determines whether the current iteration count has reached the set number. If so, it ends the initial path planning and obtains the initial path set; otherwise, it returns to S21.

[0080] In some embodiments, the heuristic function in S21 is specifically as follows:

[0081] ;

[0082] in, Represents a heuristic function. Indicates the location of the current sampling point. This represents the endpoint of the path planning. The improved RRT* algorithm prioritizes boundary regions with smaller heuristic function values, i.e., boundary points closer to the target, to improve sampling efficiency and shorten path search time.

[0083] In some embodiments, S3 specifically includes the following steps:

[0084] S31. Perform global conflict detection on the generated measurement robot path and construct a spatiotemporal occupancy constraint table based on the initial path set;

[0085] S32. The high-level searcher in the CBS algorithm references the spatiotemporal occupancy constraint table and constructs the initial root node of the constraint tree in the "unconstrained" case.

[0086] S33. The high-level searcher refers to the spatiotemporal occupancy constraint table to perform conflict detection on the initial path set and determine whether there are multiple measurement robots occupying the same position at the same time or being less than the set safe distance threshold.

[0087] S34. Once a conflict is detected, the high-level searcher uses the conflict as a split point to generate two child nodes on the constraint tree. Each child node adds an exclusionary constraint to the conflicting measurement robot. Then, only for the measurement robots with exclusionary constraints, the low-level path planner is called to replan a complete path from the starting point to satisfy the new constraint conditions. The paths and historical constraints of all unaffected measurement robots are inherited as is by the two generated child nodes. Each node (including child nodes) contains the current set of paths of the measurement robot system and the set of applied path constraints. In addition, in this invention, the low-level path planner uses the same improved BCD-RRT* algorithm as S2.

[0088] S35. The higher-level searcher performs conflict detection again. If path conflicts still exist, it returns to S34 until a globally conflict-free path is obtained, forming a set of collision-free paths.

[0089] In some embodiments, the path cost function in S4 is specifically as follows:

[0090] ;

[0091] in, Indicates the cost of the current tracking path; The adjustment factor is set to 0.6; This indicates the position of the tracking robot at time t. This represents the position of the i-th measuring robot being tracked by the tracking robot at time t. For the first The tracking weight of a measurement robot, where N represents the total number of measurement robots that the current tracking robot is responsible for tracking; and T represents the total tracking time.

[0092] In some embodiments, S5 specifically includes the following steps:

[0093] S51. If the current tracking robot cannot perceive the position of any of the measurement robots it is responsible for, it enters the perception recovery mode.

[0094] S52. After entering the perception recovery mode, the current tracking robot first uses the last observable position of the most recent measurement robot in its target set as the center to construct a circular orbital trajectory.

[0095] S53. The current tracking robot circles the center and follows a circular trajectory to the designated position, and attempts to restore perception of any of the measurement robots it is responsible for; if the longest perception recovery time is reached... If the current tracking robot's detour radius has reached its maximum detour radius (15s or 120s), then the perception recovery is considered to have failed. The automatic perception recovery for this tracking robot will terminate and the process will revert to S4, re-executing the current tracking robot's path planning. Once the tracking robot regains perception of all its measuring robots, it will exit the perception recovery mode and return to normal collaborative mode.

[0096] In some embodiments, the expression for the circular trajectory in S52 is as follows:

[0097] ;

[0098] in, This indicates the position of the tracking robot at time t; Indicates the coordinates of the center of the circle; Indicates the dynamic radius; Let represent the orbital angular velocity at time t; This represents the instantaneous lateral offset component.

[0099] In some embodiments, the implementation method of the current tracking robot circling around the center and following a circular trajectory to the designated position in S53 is as follows:

[0100] Each time the tracking robot revolves around the center and rotates by a set angle (e.g., 30°) according to the circular trajectory, the minimum distance between the current tracking robot and all other robots in the cluster is calculated. ;when Less than the set safety threshold When the value is 0.5m, immediately adjust the dynamic radius. To achieve expansion, that is ,in (Take 0.1m) to represent the radius difference of the current expansion circle; if Three consecutive sampling periods are greater than Then The radius is reduced in a certain way, where Indicates the initial detour radius, the starting detour radius. Set as the tracking robot's body radius and the desired safe distance The sum of the components; the radius of the tracking robot body refers to the minimum radius obtained by fitting the outer contour of the tracking robot, which is used to measure the space occupied by the tracking robot;

[0101] The angular velocity ω(t) of orbit varies proportionally with the radius, satisfying the following condition: To ensure that the linear velocity of rotation remains approximately constant as the radius increases, where (Take 0.5 rad / s) to represent the initial orbital angular velocity.

[0102] Instantaneous lateral offset components Multiply the unit vertical vector of the line connecting the nearest swarm robots to the currently tracking robot by the offset. (Take 0.15m) to generate; when A lateral offset is triggered once, and then decays exponentially to zero over three sampling periods to avoid continuous oscillation. The specific formula for exponential decay is as follows:

[0103] , ;

[0104] in, , These represent the instantaneous lateral offset components on the x and y axes, respectively. Indicates the attenuation coefficient. Represents the natural constant;

[0105] To ensure simultaneous observation of multiple targets, if the position of any target in the set of measurement robots currently being tracked by the tracking robot exceeds the field of view, the system will execute the following steps first: Force expansion of the circle, then press Reduce angular velocity until all targets re-enter the field of view.

[0106] In some embodiments, S6 specifically includes the following steps:

[0107] Using the path points generated by the improved BCD-RRT* algorithm as initial input, the path is smoothed using a cubic B-spline method. Specifically, the parameters are first determined, with the number of control points determined based on the complexity of the original path, typically 70% of the original number of path points. A non-uniform node vector is used, with nodes denser in areas of high path curvature and sparser in straight line segments. For example, the spacing between corner nodes is set to 0.1m, and the spacing between nodes in straight lines is set to 0.5m. Then, a B-spline curve is constructed, and the maximum curvature of the smoothed path is determined. It satisfies the dynamic constraints of mobile robots and achieves "local fine smoothing and global trend preservation" of the path.

[0108] Through the specific implementation methods described above, this invention organically combines bounded cell decomposition, the improved RRT* algorithm, the CBS algorithm, and the B-spline method to achieve end-to-end optimization of swarm robots in complex environments, from environment modeling, path planning, conflict resolution to path smoothing. The collaborative processes between each step effectively solve problems such as multi-robot path conflicts and path unevenness. Furthermore, mechanisms such as dynamic monitoring and communication anomaly handling enhance the system's adaptability and reliability. The implementation methods of this invention can be widely applied to multi-robot collaborative operation scenarios such as industrial warehousing and logistics, intelligent inspection, and disaster relief, providing a practical and feasible technical solution for the efficient and stable operation of swarm robots, and possessing significant practical value and promotional significance.

[0109] In another aspect, the present invention provides a swarm robot collaborative operation system, including a swarm of robots, and further including the following modules:

[0110] The partitioning module is used to spatially partition the environment map, using the Bounded Cell Division (BCD) method to divide the free space into multiple barrier-free cells;

[0111] The sampling module is used to sample based on the cell boundary using an improved RRT* algorithm. The extended child nodes are only on the cell boundary, and a heuristic function is used to guide the path to extend to the target cell boundary.

[0112] The measurement robot path planning and coordination module is used to perform parallel path planning for multiple measurement robots and to perform conflict detection and resolution on their initial paths using the CBS algorithm, generating a set of collision-free paths for all measurement robots.

[0113] The tracking robot path planning module uses the existing set of collision-free paths for the measurement robot as constraint input and employs the CBS algorithm to generate collision-free tracking paths for one or more tracking robots. During path expansion, if the distance between any tracking robot path node and the measurement robot or other tracking robots at the same time is less than a set safety threshold, collision detection and constraint tree construction are automatically triggered. New path child nodes are generated in the CBS framework. Only the constrained robot in the child node calls the underlying path planner to replan a complete path from the starting point, while the paths and historical constraints of all unaffected robots are inherited by the child node as is. The same collision detection is performed again on the newly generated child nodes—if there are still conflicts, the "generate-replan-detect" loop continues until a set of collision-free paths is formed.

[0114] An optimization module is used to introduce a multi-target distance preservation term into the path cost function of the tracking robot path, so that the tracking robot maintains at least a set safe tracking distance with the multiple measurement robots responsible for tracking during operation;

[0115] The perception recovery module is used to control the tracking robot to enter the perception recovery mode when any tracking robot cannot observe any of the measurement robots it is responsible for.

[0116] The path smoothing module uses the B-spline method to smooth the path, thereby obtaining the final planned collision-free path.

[0117] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Furthermore, the technical solutions of the various embodiments of the present invention can be combined with each other, but this must be based on the ability of those skilled in the art to implement them. When the combination of technical solutions is contradictory or cannot be implemented, it should be considered that such a combination of technical solutions does not exist and is not within the scope of protection claimed by the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A method for collaborative operation of swarm robots based on hierarchical path planning, characterized in that, Includes the following steps: S1. The environment map of the swarm robot operation is spatially partitioned using obstacle detection and bounded cell partitioning methods to obtain multiple obstacle-free cells; the swarm robot includes several tracking robots and multiple measuring robots. S2. The improved RRT* algorithm is used to perform preliminary path planning for the cluster robots to obtain an initial path set. The improvement of the improved RRT* algorithm is that it adopts a boundary constraint-guided adaptive sampling strategy for node sampling. The boundary constraint-guided adaptive sampling strategy dynamically adjusts the sampling node distribution according to the cell topology, target position, and obstacle distribution. In the node generation process, nodes are generated only on the cell boundary. S3. Perform global conflict detection on all paths generated by the measurement robots, and use the CBS algorithm to coordinate path conflicts between multiple measurement robots to form a set of collision-free paths; S4. Based on the set of collision-free paths of the measurement robots, the CBS algorithm is used to uniformly plan the tracking path for one or more tracking robots. At the same time, in the path optimization process of each tracking robot, the constructed path cost function ensures that the current tracking robot maintains a minimum set safe tracking distance with the multiple measurement robots responsible for tracking. S5. When a tracking robot cannot perceive the position of any of the measurement robots it is responsible for, it enters the perception recovery mode. If the perception recovery fails, the tracking robot automatically terminates the perception recovery state and returns to S4 to replan the tracking robot's tracking path. S6. Perform path smoothing on the paths of all measuring robots and the tracking paths of each tracking robot to obtain the final planned collision-free path. S1 specifically includes the following steps: S11. Combining the radius of the cluster robot body and the set safety distance, perform shape expansion processing on the area where the obstacle is located in the environment map, and update the obstacle boundary in the environment map; among them, the measurement robot in the cluster robot is responsible for performing specific measurement tasks, and the tracking robot is responsible for acquiring the pose status of one or more measurement robots in real time. S12. Using the bounded cell division method, scan each column of cells in the environment map from left to right. Through obstacle detection, merge adjacent passable cells in each column into one cell to obtain multiple barrier-free cells. S2 specifically includes the following steps: S21. Using the starting point and target of all measurement robots as input to the improved RRT* algorithm, and based on the spatial topological relationship of the cells, combined with a heuristic function to guide the sampling direction, nodes are randomly sampled on the boundaries of multiple cells to generate nodes. ; S22, Determine the distance node The nearest node Connecting nodes and nodes And find the nodes among the intersections of the line with the boundaries of multiple cells. The nearest node ; S23, Determine the node With nodes Are there any obstacles between the lines? If not, proceed to S24; otherwise, proceed to S25. S24, Node As a new node in the tree structure, and treat the new node as a node Then, it is determined whether the current iteration number has reached the set number. If so, the initial path planning ends and the initial path set is obtained; otherwise, proceed to S22. S25, Abandon Node Then, it is determined whether the current iteration number has reached the set number. If so, the initial path planning ends and the initial path set is obtained; otherwise, it returns to S21. S3 specifically includes the following steps: S31. Perform global conflict detection on the generated measurement robot path and construct a spatiotemporal occupancy constraint table based on the initial path set; S32, The high-level searcher in the CBS algorithm references the spatiotemporal occupancy constraint table and constructs the initial root node of the constraint tree in the "unconstrained" case; S33. The high-level searcher refers to the spatiotemporal occupancy constraint table to perform conflict detection on the initial path set and determine whether there are multiple measurement robots occupying the same position at the same time or being less than the set safe distance threshold. S34. Once a conflict is detected, the high-level searcher uses the conflict as a split point to generate two child nodes on the constraint tree. Each child node adds an exclusionary constraint to the conflicting measurement robot. Then, the low-level path planner is called only for the measurement robot with the exclusionary constraint to replan a complete path from the starting point to meet the new constraint conditions. The paths and historical constraints of all unaffected measurement robots are inherited as is by the two child nodes generated. S35. The higher-level searcher performs conflict detection again. If path conflicts still exist, it returns to S34 until a globally conflict-free path is obtained, forming a set of collision-free paths.

2. The method for collaborative operation of cluster robots based on hierarchical path planning according to claim 1, characterized in that, The heuristic function in S21 is as follows: ； in, Represents a heuristic function. Indicates the location of the current sampling point. This indicates the endpoint of the path planning.

3. The method for collaborative operation of cluster robots based on hierarchical path planning according to claim 2, characterized in that, The path cost function in S4 is as follows: ； in, Indicates the cost of the current tracking path; As a regulating factor; This indicates the position of the tracking robot at time t. This represents the position of the i-th measuring robot being tracked by the tracking robot at time t. For the first The tracking weight of a measurement robot, where N represents the total number of measurement robots that the current tracking robot is responsible for tracking; and T represents the total tracking time.

4. The method for collaborative operation of cluster robots based on hierarchical path planning according to claim 3, characterized in that, S5 specifically includes the following steps: S51. If the current tracking robot cannot perceive the position of any of the measurement robots it is responsible for, it enters the perception recovery mode. S52. After entering the perception recovery mode, the current tracking robot first uses the last observable position of the most recent measurement robot in its target set as the center to construct a circular orbital trajectory. S53. The current tracking robot circles the center and follows a circular trajectory to the designated position, and attempts to restore perception of any of the measurement robots it is responsible for; if the longest perception recovery time is reached... If the current tracking robot's detour radius has reached the maximum detour radius, then the perception recovery is considered to have failed. The automatic perception recovery of the tracking robot will be terminated and the process will revert to S4 to re-execute the current tracking robot's path planning.

5. The method for collaborative operation of cluster robots based on hierarchical path planning according to claim 4, characterized in that, The specific expression for the circular trajectory in S52 is as follows: ； in, This indicates the position of the tracking robot at time t; Indicates the coordinates of the center of the circle; Indicates the dynamic radius; Let represent the orbital angular velocity at time t; This represents the instantaneous lateral offset component.

6. The method for collaborative operation of cluster robots based on hierarchical path planning according to claim 5, characterized in that, The implementation method in S53 where the current tracking robot revolves around the center and travels along a circular trajectory to the designated position is as follows: Each time the tracking robot revolves around the center and rotates by a set angle according to the circular trajectory, it calculates the minimum distance between the current tracking robot and all other robots in the cluster. ;when Less than the set safety threshold Immediately adjust the dynamic radius. To achieve expansion, that is ,in This represents the difference in radius of the current expansion circle; if Three consecutive sampling periods are greater than Then The radius is reduced in a certain way, where Indicates the initial detour radius, the starting detour radius. Set as the tracking robot's body radius and the desired safe distance The sum of the components; the radius of the tracking robot body refers to the minimum radius obtained by fitting the outer contour of the tracking robot, which is used to measure the space occupied by the tracking robot; angular velocity of orbit It varies proportionally with the radius, satisfying ,in Indicates the initial orbital angular velocity; Instantaneous lateral offset components Multiply the unit vertical vector of the line connecting the nearest swarm robots to the currently tracking robot by the offset. Generate; when A lateral offset is triggered once, and then decays exponentially to zero over three sampling periods to avoid continuous oscillation. The specific formula for exponential decay is as follows: , ； in, , These represent the instantaneous lateral offset components on the x and y axes, respectively. Indicates the attenuation coefficient; Represents the natural constant; To ensure simultaneous observation of multiple targets, if the position of any target in the set of measurement robots currently being tracked by the tracking robot exceeds the field of view, the system will execute the following steps first: Force expansion of the circle, then press Reduce angular velocity until all targets re-enter the field of view.

7. A swarm robot collaborative operation system employing the swarm robot collaborative operation method according to any one of claims 1 to 6, comprising a swarm of robots, characterized in that, It also includes the following modules: The partitioning module is used to spatially partition the environment map, using the Bounded Cell Division (BCD) method to divide the free space into multiple barrier-free cells; The sampling module is used to sample based on the cell boundary using an improved RRT* algorithm. The extended child nodes are only on the cell boundary, and a heuristic function is used to guide the path to extend to the target cell boundary. The measurement robot path planning and coordination module is used to perform parallel path planning for multiple measurement robots and to perform conflict detection and resolution on their initial paths using the CBS algorithm, generating a set of collision-free paths for all measurement robots. The tracking robot path planning module is used to generate collision-free tracking paths for one or more tracking robots using the CBS algorithm, with the generated set of collision-free paths for measurement robots as constraint inputs. An optimization module is used to introduce a multi-target distance preservation term into the path cost function of the tracking robot path, so that the tracking robot maintains at least a set safe tracking distance with the multiple measurement robots responsible for tracking during operation; The perception recovery module is used to control the tracking robot to enter the perception recovery mode when any tracking robot cannot observe any of the measurement robots it is responsible for. The path smoothing module uses the B-spline method to smooth the path, thereby obtaining the final planned collision-free path.

Images