Multi-robot path planning system and method based on sampling

Abstract

The invention discloses a multi-robot path planning system and method based on sampling, and belongs to the technical field of robot path planning, and the system comprises a route map initialization module, a scanning volume calculation module, a collision inspection module, a path expansion module and a reconnection module. A space is explored and planned in a random sampling mode, the method is suitable for dynamically changing industrial scenes, in addition, compared with a traditional method, the method pays more attention to improving the planning efficiency instead of an absolute optimal path, the collision detection process is optimized by pre-calculating the sweeping volume, the calculation overhead in the search process is reduced, and the search efficiency is improved. The problem of mutual interference among multiple machines is solved through a deadlock strategy, unnecessary calculation is reduced through a simplified updating strategy, and the efficiency of multi-robot path planning is remarkably improved while the effect of a solution is kept.

Description

Technical Field

[0001] The present invention relates to the technical field of robot path planning, and more specifically, to a sampling-based multi-robot path planning system and method. Background Art

[0002] With the continuous development and widespread application of robotics technology, tasks in industrial scenarios such as welding and stacking are undergoing profound changes. Using robots to perform operations not only improves production efficiency and reduces labor costs, but also greatly improves work quality and safety.

[0003] However, as the application of robots continues to expand, they are also facing some challenges and problems. In complex industrial scenarios, multiple robots are often required to collaborate to complete tasks. In the past, some path planning only performed static path planning at the beginning of the machine movement, without considering the mutual influence between multiple machines, resulting in the need for re-planning when a collision occurs.

[0004] Although some algorithms that use sampling can achieve relatively optimal path planning between multiple robots, they are not widely applicable to the field of industrial manufacturing. Their dynamic collision detection will waste computing time, resulting in the failure to ensure the real-time performance of industrial production. Secondly, previous solutions such as dRRT did not consider the deadlock problem caused by the mutual influence between machines.

[0005] Therefore, there is an urgent need for a sampling-based multi-robot planning method designed specifically for real-time industrial automation scenarios. Through path planning, deadlock and collision problems between machines can be avoided to ensure the real-time efficiency of industrial production. Summary of the invention

[0006] The object of the present invention is to provide a multi-robot path planning system and method based on sampling to solve the above-mentioned problems.

[0007] In order to achieve the above purpose, the technical solution provided by an embodiment of the present invention is as follows: A multi-robot path planning system based on sampling, characterized in that: the path planning system comprises: The roadmap initialization module is used to calculate the optimal cost path between the goals of each robot by sampling, and create a roadmap; The swept volume calculation module is used to pre-calculate the three-dimensional volume of the robot during movement offline, define a reachable bounding box for the robot, and use it to represent the geometry of the robot shape; discretize the machine path into a series of configuration points through discretization, which are sets of robot joint angles or positions of the robot's end effector; calculate the geometry of each configuration point, and for each configuration point on the path, calculate the position and posture of each part of the robot at the corresponding position; construct a swept volume, and for each adjacent configuration point on the path, calculate the geometry of the robot and perform linear interpolation between two points to obtain an approximate volume as the machine swept volume, and use voxelization to represent it; A collision check module is used to determine whether there is a collision in the paths of multiple robots based on the three-dimensional volumes of all robots acquired during movement; The path extension module is used to deal with the interference of robots without obvious task objectives on other robots by exchanging path information through communication; The reconnection module is used to simplify the path when collisions and possible deadlocks occur, allowing the system to replan the path and generate the final path.

[0008] Preferably, the roadmap initialization module further includes: The optimal cost path between the targets of each robot is calculated by sampling. The steps of the roadmap initialization module for path planning for a single robot are as follows: Initialization submodule: used to initialize an empty tree containing a root node; Random sampling submodule: used to randomly sample a point in the motion space to add a new node; Nearest neighbor search submodule: used to find the node closest to the random sampling point in the tree; Path extension submodule: used to expand from the nearest neighbor point to the randomly sampled node, while checking the obstacles in the environment for static restrictions, and judging whether there is a collision in the multi-robot path. If there is no collision, a new node is added and connected to the nearest neighbor node; Target check submodule: used to check whether the newly added node is close to the target area and judge whether path < Pfinal. If so, the target is added to the path tree as a child node of the new node; Where Pfinal is set to the average path length of the machine path nodes, and path is the path; Reconnection submodule: after a new node is added to the path tree, it searches for nodes adjacent to the new node in the path tree and calculates the cost of reaching these adjacent nodes through the new node. If a lower-cost path can be found through the new node, these paths are updated; The cost calculation formula is as follows: ; in the formula Indicates the weight coefficient, which is preset according to the machine model. Indicates length, including horizontal and vertical distances. Indicates the time when the machine moves, represents the path curvature, Indicates the number of potential collision points; Iterative path submodule: Repeat the above steps until the minimum cost path is not updated N times, where N is a preset threshold constant.

[0009] Preferably, the swept volume calculation module further includes: Define a reachable bounding box unit: Define a reachable bounding box for the robot, which is used to represent the geometry of the robot shape; Discretization representation unit: discretize the machine path into a series of configuration points, which are sets of robot joint angles or positions of the robot end effector; Calculate the geometry of each configuration point: For each configuration point on the path, calculate the position and posture of each part of the robot when it is at the corresponding position; Constructing a swept volume unit: For each adjacent configuration point on the path, an approximate volume is obtained as the machine swept volume by calculating the robot's geometry and linearly interpolating between the two points, and represented using voxelization.

[0010] Preferably, the collision checking module further includes: After obtaining the swept volumes of all robots performing this task, the system will perform collision checks in real time, detecting robots in pairs. If the voxel representations of the two robots have overlapping voxels in space, this indicates that the robots may collide during movement. In this process, the swept volume of the robot is represented as and , collision detection can be expressed as: ; in, represents the intersection operation of sets, Indicates that the intersection is not empty.

[0011] Preferably, the path extension module and the reconnection module further include: If the robot tasks that collide or deadlock have different priorities, the path will be expanded according to the priority and the subsequent route will be replanned; When one or more robots do not have a clear moving target, and one or more robots with a clear target are involved, the robots without a target are regarded as "arbitrary target" robots, and their positions are kept unchanged as much as possible during the path expansion process; if they hinder other robots, they are moved to a position that does not affect the path; if all robots are without a target, there is no path planning problem involved, and they can simply fall back to the previous step to randomly select a path; When the robots involved have the same priority or no priority, one of them is randomly selected for path expansion, and the deadlocked robots are allowed to collaborate through the communication mechanism. If the same path is still selected after path expansion and the deadlock is not resolved, any robot is given priority to re-plan the path, and other robots in the deadlock state stop moving.

[0012] On the other hand, a sampling-based multi-robot path planning method is characterized by comprising: S1. Create a roadmap by calculating the optimal cost path between the goals of each robot through sampling; S2. Based on the created roadmap, the three-dimensional volume of the machine during movement is pre-calculated offline; a reachable bounding box is defined for the robot to represent the geometry of the robot shape; the machine path is discretized into a series of configuration points through discretization, which are sets of robot joint angles or positions of the robot end effector; the geometry of each configuration point is calculated, and for each configuration point on the path, the position and posture of each part of the robot at the corresponding position are calculated; a swept volume is constructed, and for each adjacent configuration point on the path, the geometry of the robot is calculated and linear interpolated between the two points to obtain an approximate volume as the machine swept volume, and voxelization is used for representation; S3, based on the obtained three-dimensional volumes of all robots during the movement, determine whether there is a collision between the paths of multiple robots; S4, processing the interference of robots without significant task objectives on other robots through communication and mutual exchange of path information; S5 is used to simplify the path when collision and possible deadlock problems occur, so that the system can re-plan the path and generate the final path.

[0013] Preferably, in step S1, the step of performing path planning for a single robot is also included as follows: S11, initialization: initialize an empty tree containing a root node; S12, random sampling: randomly sampling a point in the motion space to add a new node; S13, nearest neighbor search: find the node closest to the random sampling point in the tree; S14, path extension: expand from the nearest neighbor point to the randomly sampled node, and check the obstacles in the environment for static restrictions, and determine whether there is a collision in the multi-robot path. If there is no collision, add a new node and connect it to the nearest neighbor node; S15, target check: check whether the newly added node is close to the target area, and judge whether path < Pfinal. If so, add the target to the path tree as a child of the new node; Where Pfinal is set to the average path length of the machine path nodes, and path is the path; S16, reconnection: after the new node is added to the path tree, find the nodes adjacent to the new node in the path tree, and calculate the cost of reaching these adjacent nodes through the new node, and if a lower cost path can be found through the new node, update these paths; The cost calculation formula is as follows: ; in the formula Indicates the weight coefficient, which is preset according to the machine model. Indicates length, including horizontal and vertical distances. Indicates the time when the machine moves, represents the path curvature, Indicates the number of potential collision points; S17, iterative path: repeat the above steps until the minimum cost path is not updated N times, where N is a preset threshold constant.

[0014] Preferably, in step S2, it also includes: The steps for path planning for a single robot are as follows: S21. Define a reachable bounding box: define a reachable bounding box for the robot, which is used to represent the geometry of the robot shape; S22, Discretization Representation: Discretize the machine path into a series of configuration points, which are sets of robot joint angles or positions of the robot end effector; S23, calculating the geometric shape of each configuration point: for each configuration point on the path, calculating the position and posture of each part of the robot when it is at the corresponding position; S24. Construct the swept volume: For each adjacent configuration point on the path, linearly interpolate between the two points by calculating the robot's geometry to obtain an approximate volume as the machine swept volume, and represent it using voxelization.

[0015] Preferably, in step S3, it also includes: After obtaining the swept volumes of all robots performing this task, the system will perform collision checks in real time, detecting robots in pairs. If the voxel representations of the two robots have overlapping voxels in space, this indicates that the robots may collide during movement. In this process, the swept volume of the robot is represented as and , collision detection can be expressed as: ; in, represents the intersection operation of sets, Indicates that the intersection is not empty.

[0016] Preferably, in step S4 and step S5, it also includes: If the robot tasks that collide or deadlock have different priorities, the path will be expanded according to the priority and the subsequent route will be replanned; When one or more robots do not have a clear moving target, and one or more robots with a clear target are involved, the robots without a target are regarded as "arbitrary target" robots, and their positions are kept unchanged as much as possible during the path expansion process; if they hinder other robots, they are moved to a position that does not affect the path; if all robots are without a target, there is no path planning problem involved, and they can simply fall back to the previous step to randomly select a path; When the robots involved have the same priority or no priority, one of them is randomly selected for path expansion, and the deadlocked robots are allowed to collaborate through the communication mechanism. If the same path is still selected after path expansion and the deadlock is not resolved, any robot is given priority to re-plan the path, and other robots in the deadlock state stop moving.

[0017] Compared with the prior art, the advantages of the present invention are: 1. Compared with the traditional sampling scheme, this scheme emphasizes real-time efficiency, reduces the computational overhead in the search process, avoids deadlock problems, and is suitable for multi-robot path planning in the industrial field.

[0018] 2. Compared with the existing multi-robot path planning scheme, this invention places more emphasis on real-time efficiency for the industrial field. Through the designs of roadmap initialization module, swept volume calculation module, collision check module, path extension module and reconnection module, this system improves the speed and efficiency of path planning while maintaining the path quality, which is more in line with practical applications and has advantages in efficiency, robustness, deadlock avoidance and rapid response.

[0019] 3. This solution does not need to rely on prior knowledge of a specific environment. It explores the planning space through random sampling and is suitable for dynamically changing industrial scenarios. In addition, compared with traditional methods, the present invention focuses more on improving planning efficiency rather than the optimal path in an absolute sense. It optimizes the collision detection process by pre-calculating the swept volume, reduces the computational overhead in the search process, solves the problem of mutual interference between multiple machines through the deadlock strategy, and reduces unnecessary calculations through a simplified update strategy. While maintaining the solution effect, it significantly improves the efficiency of multi-robot path planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] Figure 1 This is the overall architecture diagram of the sampling-based multi-robot path planning system of the present invention; Figure 2 This is a flow chart of the sampling-based multi-robot path planning system of the present invention; Figure 3 This is a flow chart of the sampling-based multi-robot path planning method of the present invention. DETAILED DESCRIPTION

[0021] The technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention; it is obvious that the described embodiments are only part of the embodiments of the present invention, rather than all the embodiments, and all other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making creative work are within the scope of protection of the present invention.

[0022] See also Figure 1-3 , a sampling-based multi-robot path planning system, the path planning system comprising: See also Figure 1 , a multi-robot path planning system based on sampling, characterized in that: the path planning system comprises: The roadmap initialization module is used to calculate the optimal cost path between the goals of each robot by sampling, and create a roadmap; The swept volume calculation module is used to pre-calculate the three-dimensional volume of the robot during its movement offline. The swept volume calculation module is used to pre-calculate the three-dimensional volume of the robot during its movement offline. A reachable bounding box is defined for the robot to represent the geometry of the robot shape; the machine path is discretized into a series of configuration points through discretization, which are sets of robot joint angles or positions of the robot end effector; the geometry of each configuration point is calculated, and for each configuration point on the path, the position and posture of each part of the robot at the corresponding position are calculated; the swept volume is constructed, and for each adjacent configuration point on the path, the geometry of the robot is calculated and linear interpolation is performed between the two points to obtain an approximate volume as the machine swept volume, and voxelization is used for representation; A collision check module is used to determine whether there is a collision in the paths of multiple robots based on the three-dimensional volumes of all robots acquired during movement; The path extension module is used to deal with the interference of robots without obvious task objectives on other robots by exchanging path information through communication; The reconnection module is used to simplify the path when collisions and possible deadlocks occur, allowing the system to replan the path and generate the final path.

[0023] Preferably, the roadmap initialization module further includes: The optimal cost path between the targets of each robot is calculated by sampling. The steps of the roadmap initialization module for path planning for a single robot are as follows: Initialization submodule: used to initialize an empty tree containing a root node; Random sampling submodule: used to randomly sample a point in the motion space to add a new node; Nearest neighbor search submodule: used to find the node closest to the random sampling point in the tree; Path extension submodule: used to expand from the nearest neighbor point to the randomly sampled node, while checking the obstacles in the environment for static restrictions, and judging whether there is a collision in the multi-robot path. If there is no collision, a new node is added and connected to the nearest neighbor node; Target check submodule: used to check whether the newly added node is close to the target area and judge whether path < Pfinal. If so, the target is added to the path tree as a child node of the new node; Where Pfinal is set to the average path length of the machine path nodes, and path is the path; Reconnection submodule: after a new node is added to the path tree, it searches for nodes adjacent to the new node in the path tree and calculates the cost of reaching these adjacent nodes through the new node. If a lower-cost path can be found through the new node, these paths are updated; The cost calculation formula is as follows: ; in the formula Indicates the weight coefficient, which is preset according to the machine model. Indicates length, including horizontal and vertical distances. Indicates the time when the machine moves, represents the path curvature, Indicates the number of potential collision points; Iterative path submodule: Repeat the above steps until the minimum cost path is not updated N times, where N is a preset threshold constant.

[0024] Preferably, the collision checking module further includes: After obtaining the swept volumes of all robots performing this task, the system will perform collision checks in real time, detecting robots in pairs. If the voxel representations of the two robots have overlapping voxels in space, this indicates that the robots may collide during movement. In this process, the swept volume of the robot is represented as and , collision detection can be expressed as: ; in, represents the intersection operation of sets, Indicates that the intersection is not empty.

[0025] Preferably, the path extension module and the reconnection module further include: By adjusting some robots without targets, the reconnection module is mainly responsible for solving collision and possible deadlock problems, and replanning the path for robots that collide or fall into deadlock. The path extension module and the reconnection module are performed together in the path planning, as follows: Initialization unit: For the robot that detects a collision, select an existing node from its path planning tree as the starting point for expansion. This node is the most recently added to the tree or the node closest to the target area. Random sampling unit: randomly sample a point in the configuration space; Calculate neighbor node units: For each robot, calculate a feasible path from the current expansion node to a random sampling point, ensuring that the path does not collide with obstacles or other robots; Path extension unit: calculates the cost as the next priority path cost, selects a new best path from all calculated neighbor nodes, and adds the node to the planning tree; Repeat until there is no change in the unit, and repeat the previous step until the cost does not change after N updates.

[0026] Specifically, to solve collision and possible deadlock problems, most situations can be avoided in advance based on volume pre-calculation: by calculating the swept volume and pre-calculating collision detection during the path extension process, potential collision points can be quickly identified, thereby avoiding conflicts in the early stages of path planning.

[0027] Re-plan the path based on priority: If the robot tasks that collide / deadlock have different priorities, the path is expanded based on the priority and the subsequent route is re-planned.

[0028] When one or more robots do not have a clear moving target, and one or more robots have a clear target, the robots without targets are considered "arbitrary target" robots, and their positions are kept unchanged as much as possible during the path expansion process. If they hinder other robots, they are moved to a position that does not affect the path. If all robots are without targets, there is no path planning problem involved, and you can simply fall back to the previous step to randomly select a path.

[0029] When the robots involved have the same priority or no priority, one of them is randomly selected for path expansion, and the deadlocked robots are allowed to collaborate through the communication mechanism. If the same path is still selected after path expansion and the deadlock is not resolved, any robot is given priority to re-plan the path, and other robots in the deadlock state suspend movement to ensure that no new collisions and deadlocks occur when reaching the neighboring nodes in the path.

[0030] See also Figure 3 On the other hand, a sampling-based multi-robot path planning method is characterized by comprising: S1. Create a roadmap by calculating the optimal cost path between the goals of each robot through sampling; Specifically, the path map initialization module is mainly responsible for generating the path map of the machine movement by sampling. In the scenario of multiple robots, it is not possible to simply randomly sample in the entire motion space. It is necessary to calculate the optimal cost path between the targets of each robot. The steps for path planning for a single robot are as follows: S11, initialization: initialize an empty tree containing a root node; S12, random sampling: randomly sampling a point in the motion space to add a new node; S13, nearest neighbor search: find the node closest to the random sampling point in the tree; S14, Path extension: Expand from the nearest neighbor to the randomly sampled node, while checking the environment Obstacles are statically restricted to determine whether there is a collision in the multi-robot path. If there is no collision, a new node is added and connected to the nearest neighbor node; S15, target check: check whether the newly added node is close to the target area, and judge whether path < Pfinal. If so, add the target to the path tree as a child of the new node; Where Pfinal is set to the average path length of the machine path nodes, and path is the path; S16, reconnection: after the new node is added to the path tree, find the nodes adjacent to the new node in the path tree, and calculate the cost of reaching these adjacent nodes through the new node, and if a lower cost path can be found through the new node, update these paths; The cost calculation formula is as follows: ; in the formula Indicates the weight coefficient, which is preset according to the machine model. Indicates length, including horizontal and vertical distances. Indicates the time when the machine moves, represents the path curvature, Indicates the number of potential collision points; S17, iterative path: repeat the above steps until the minimum cost path is not updated N times, where N is a preset threshold constant.

[0031] By pre-calculating the optimal cost path for a single robot and obtaining a simple path map, we can avoid wasting computing resources in irrelevant areas and reduce the computational complexity of path planning for multiple robots as a whole, avoid including a large number of unnecessary nodes in the path map, and improve computing efficiency.

[0032] It should be noted that the path map initialization module and the swept volume calculation module are all pre-calculated offline, such as Figure 2 As shown, calculating the cost path for each machine will not affect the overall real-time path planning efficiency of the system.

[0033] S2. Based on the created roadmap, the 3D volume of the machine during movement is pre-calculated offline. Specifically, the swept volume calculation module is responsible for offline pre-calculating the three-dimensional volume of the machine during movement to prepare for subsequent collision detection. Its main steps are as follows: S21, define a reachable bounding box, defining a reachable bounding box for the robot, which is used to represent the geometry of the robot shape; S22, discretization representation, discretize the machine path into a series of configuration points, which are sets of robot joint angles or positions of the robot end effector; S23, calculating the geometric shape of each configuration point: for each configuration point on the path, calculating the position and posture of each part of the robot when it is at the corresponding position; S24. Construct the swept volume: For each adjacent configuration point on the path, linearly interpolate between the two points by calculating the robot's geometry to obtain an approximate volume as the machine swept volume, and represent it using voxelization.

[0034] S3, based on the obtained three-dimensional volumes of all robots during the movement, determine whether there is a collision between the paths of multiple robots; Specifically, after obtaining the swept volumes of all robots performing this task, the system will perform collision checks in real time, detecting robots in pairs. If the voxel representations of the two robots have overlapping voxels in space, this indicates that the robots may collide during movement. In this process, the swept volume of the robot is represented as and , collision detection can be expressed as: ; in, represents the intersection operation of sets, Indicates that the intersection is not empty; S4, processing the interference of robots without significant task objectives on other robots through communication and mutual exchange of path information; S5, used to simplify the path when collision and possible deadlock problems occur, so that the system replans the path and generates the final path; Specifically, the path extension module will adjust some robots without targets to prevent them from affecting the existing path.

[0035] The reconnection module is mainly responsible for solving collision and possible deadlock problems. It replans the path for robots that have collided or are in deadlock. These two modules are performed together in path planning: Its main steps are similar to the path planning in the path initialization module: Initialization: For a robot that detects a collision, select an existing node from its path planning tree as the starting point for expansion. This node is the most recently added to the tree or the node closest to the target area. Random sampling: randomly sample a point in the configuration space; Calculate neighbor nodes: For each robot, calculate a feasible path from the current expansion node to the random sampling point, ensuring that the path does not collide with obstacles or other robots; Path extension: Calculate the cost as the next priority path cost, select a new best path from all calculated neighbor nodes, and add the node to the planning tree; Repeat until there is no change, and repeat the previous step until the cost does not change after N updates.

[0036] For robots that collide, path extension will be performed according to their priority. If the task itself does not involve priority, one of them will be randomly selected for path extension.

[0037] Specifically, to solve collision and possible deadlock problems, most situations can be avoided in advance based on volume pre-calculation: by calculating the swept volume and pre-calculating collision detection during the path extension process, potential collision points can be quickly identified, thereby avoiding conflicts in the early stages of path planning.

[0038] Re-plan the path based on priority: If the robot tasks that collide / deadlock have different priorities, the path is expanded based on the priority and the subsequent route is re-planned.

[0039] When one or more robots do not have a clear moving target, and one or more robots have a clear target, the robots without targets are considered "arbitrary target" robots, and their positions are kept unchanged as much as possible during the path expansion process. If they hinder other robots, they are moved to a position that does not affect the path. If all robots are without targets, there is no path planning problem involved, and you can simply fall back to the previous step to randomly select a path.

[0040] When the robots involved have the same priority or no priority, one of them is randomly selected for path expansion, and the deadlocked robots are allowed to collaborate through the communication mechanism. If the same path is still selected after path expansion and the deadlock is not resolved, any robot is given priority to re-plan the path, and other robots in the deadlock state suspend movement to ensure that no new collisions and deadlocks occur when reaching the neighboring nodes in the path.

[0041] Here, the present invention simplifies the rewiring step in the traditional dRRT algorithm to avoid the algorithm rewiring all nodes in the tree every time a new node is added. The simplified rewiring strategy may only consider the direct predecessor node of the newly added node, rather than all nodes in the entire tree. In this way, the path directly related to the new node can be quickly updated without large-scale updates to the entire tree. Since this step occurs during the replanning process of collision detection, although this simplification may sacrifice the optimality of some paths, it significantly reduces the calculation time, allowing the system to generate feasible paths faster. Multi-Robot Path Planning (MRPP) is a complex problem that requires consideration of not only the path planning of a single robot, but also the interaction and coordination between multiple robots. Therefore, the methods for this problem are often designed for specific scenarios. The following are some common methods and their shortcomings when applied to the industrial field: Here are some common methods and their shortcomings when applied in industry: Decomposition-based methods decompose the multi-robot problem into multiple single-robot problems. Some solutions calculate the path for the robot in order of priority, and then use this path as a constraint for subsequent path planning. This type of method requires strict priority order and is not suitable for industrial stacking, handling and other scenarios.

[0042] Sampling-based methods explore the configuration space through random sampling and are suitable for unknown or dynamically changing environments. However, these methods require resampling and replanning when the environment changes, and the method has high computational resource overhead. In multi-robot scenarios, it cannot meet the real-time requirements of industrial production.

[0043] Graph search-based solutions model the path planning problem as the problem of finding the shortest path in a graph. This type of method is more suitable for single robot scenarios.

[0044] Compared with traditional sampling schemes, the scheme proposed in the present invention places more emphasis on real-time efficiency, reduces the computational overhead in the search process, avoids deadlock problems, and is suitable for multi-robot path planning for industrial fields; compared with existing multi-robot path planning schemes, the present invention places more emphasis on real-time efficiency for industrial fields. Through the designs of roadmap initialization module, swept volume calculation module, collision check module, path extension module and reconnection module, the system improves the speed and efficiency of path planning while maintaining the path quality, is more in line with practical applications, and has advantages in efficiency, robustness, deadlock avoidance and rapid response.

[0045] The present invention does not need to rely on prior knowledge of a specific environment, but explores the planning space through random sampling, which is suitable for dynamically changing industrial scenarios. In addition, compared with traditional methods, the present invention focuses more on improving planning efficiency rather than the optimal path in an absolute sense. It optimizes the collision detection process by pre-calculating the swept volume, reduces the computational overhead in the search process, solves the problem of mutual interference between multiple machines through the deadlock strategy, and reduces unnecessary calculations through a simplified update strategy. While maintaining the solution effect, it significantly improves the efficiency of multi-robot path planning.

[0046] It will be apparent to those skilled in the art that the invention is not limited to the details of the exemplary embodiments described above and that the invention can be implemented in other specific forms without departing from the spirit or essential features of the invention. Therefore, the embodiments should be considered exemplary and non-limiting in all respects, and the scope of the invention is defined by the appended claims rather than the foregoing description, and it is intended that all variations falling within the meaning and scope of the equivalent elements of the claims be included in the invention. Any reference numeral in a claim should not be considered as limiting the claim to which it relates.

[0047] In addition, it should be understood that although the present specification is described according to embodiments, not every embodiment contains only one independent technical solution. This narrative method of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment may also be appropriately combined to form other implementation methods that those skilled in the art can understand.

Claims

1. A sampling-based multi-robot path planning system, characterized in that: The path planning system comprises: The roadmap initialization module is used to calculate the optimal cost path between the goals of each robot by sampling, and create a roadmap; The swept volume calculation module is used to pre-calculate the three-dimensional volume of the robot during movement offline, define a reachable bounding box for the robot, and use it to represent the geometry of the robot shape; discretize the machine path into a series of configuration points through discretization, which are sets of robot joint angles or positions of the robot's end effector; calculate the geometry of each configuration point, and for each configuration point on the path, calculate the position and posture of each part of the robot at the corresponding position; construct a swept volume, and for each adjacent configuration point on the path, calculate the geometry of the robot and perform linear interpolation between two points to obtain an approximate volume as the machine swept volume, and use voxelization to represent it; A collision check module is used to determine whether there is a collision in the paths of multiple robots based on the three-dimensional volumes of all robots acquired during movement; The path extension module is used to deal with the interference of robots without obvious task objectives on other robots by exchanging path information through communication; The reconnection module is used to simplify the path when collisions and possible deadlocks occur, allowing the system to replan the path and generate the final path.

2. A sampling-based multi-robot path planning system according to claim 1, characterized in that: In the roadmap initialization module, it also includes: The optimal cost path between the targets of each robot is calculated by sampling. The steps of the roadmap initialization module for path planning for a single robot are as follows: Initialization submodule: used to initialize an empty tree containing a root node; Random sampling submodule: used to randomly sample a point in the motion space to add a new node; Nearest neighbor search submodule: used to find the node closest to the random sampling point in the tree; Path extension submodule: used to expand from the nearest neighbor point to the randomly sampled node, while checking the obstacles in the environment for static restrictions, and judging whether there is a collision in the multi-robot path. If there is no collision, a new node is added and connected to the nearest neighbor node; Target check submodule: used to check whether the newly added node is close to the target area and judge whether path < Pfinal. If so, the target is added to the path tree as a child node of the new node; Where Pfinal is set to the average path length of the machine path nodes, and path is the path; Reconnection submodule: after a new node is added to the path tree, it searches for nodes adjacent to the new node in the path tree and calculates the cost of reaching these adjacent nodes through the new node. If a lower-cost path can be found through the new node, these paths are updated; The cost calculation formula is as follows: ； in the formula Indicates the weight coefficient, which is preset according to the machine model. Indicates length, including horizontal and vertical distances. Indicates the time when the machine moves, represents the path curvature, Indicates the number of potential collision points; Iterative path submodule: Repeat the above steps until the minimum cost path is not updated N times, where N is a preset threshold constant.

3. The sampling-based multi-robot path planning system according to claim 1, characterized in that: The collision checking module further includes: After obtaining the swept volumes of all robots performing this task, the system will perform collision checks in real time, detecting robots in pairs. If the voxel representations of the two robots have overlapping voxels in space, this indicates that the robots may collide during movement. In this process, the swept volume of the robot is represented as and , collision detection can be expressed as: ； in, represents the intersection operation of sets, Indicates that the intersection is not empty.

4. The sampling-based multi-robot path planning system according to claim 1, characterized in that: In the path extension module and the reconnection module, it also includes: If the robot tasks that collide or deadlock have different priorities, the path will be expanded according to the priority and the subsequent route will be replanned; When one or more robots do not have a clear moving target, and one or more robots with a clear target are involved, the robots without a target are regarded as "arbitrary target" robots, and their positions are kept unchanged as much as possible during the path expansion process; if they hinder other robots, they are moved to a position that does not affect the path; if all robots are without a target, there is no path planning problem involved, and they can simply fall back to the previous step to randomly select a path; When the robots involved have the same priority or no priority, one of them is randomly selected for path expansion, and the deadlocked robots are allowed to collaborate through the communication mechanism. If the same path is still selected after path expansion and the deadlock is not resolved, any robot is given priority to re-plan the path, and other robots in the deadlock state stop moving.

5. A multi-robot path planning method based on sampling, characterized in that: include: S1. Create a roadmap by calculating the optimal cost path between the goals of each robot through sampling; S2. Based on the created roadmap, the three-dimensional volume of the machine during movement is pre-calculated offline, and a reachable bounding box is defined for the robot to represent the geometry of the robot shape; the machine path is discretized into a series of configuration points through discretization, which are the set of robot joint angles or the position of the robot end effector; the geometry of each configuration point is calculated, and for each configuration point on the path, the position and posture of each part of the robot when it is at the corresponding position are calculated; Construct the swept volume. For each adjacent configuration point on the path, linearly interpolate between two points by calculating the robot's geometry to obtain an approximate volume as the machine swept volume, and represent it using voxelization. S3, based on the obtained three-dimensional volumes of all robots during the movement, determine whether there is a collision between the paths of multiple robots; S4, processing the interference of robots without significant task objectives on other robots through communication and mutual exchange of path information; S5 is used to simplify the path when collision and possible deadlock problems occur, so that the system can re-plan the path and generate the final path.

6. The sampling-based multi-robot path planning method according to claim 5, characterized in that: In step S1, the steps of performing path planning for a single robot are also included as follows: S11, initialization: initialize an empty tree containing a root node; S12, random sampling: randomly sampling a point in the motion space to add a new node; S13, nearest neighbor search: find the node closest to the random sampling point in the tree; S14, Path extension: Expand from the nearest neighbor to the randomly sampled node, while checking the environment Obstacles are statically restricted to determine whether there is a collision in the multi-robot path. If there is no collision, a new node is added and connected to the nearest neighbor node; S15, target check: check whether the newly added node is close to the target area, and judge whether path < Pfinal. If so, add the target to the path tree as a child of the new node; Where Pfinal is set to the average path length of the machine path nodes, and path is the path; S16, reconnection: after the new node is added to the path tree, find the nodes adjacent to the new node in the path tree, and calculate the cost of reaching these adjacent nodes through the new node, and if a lower cost path can be found through the new node, update these paths; The cost calculation formula is as follows: ； in the formula Indicates the weight coefficient, which is preset according to the machine model. Indicates length, including horizontal and vertical distances. Indicates the time when the machine moves, represents the path curvature, Indicates the number of potential collision points; S17, iterative path: repeat the above steps until the minimum cost path is not updated N times, where N is a preset threshold constant.

7. The sampling-based multi-robot path planning method according to claim 6, characterized in that: In step S3, it also includes: After obtaining the swept volumes of all robots performing this task, the system will perform collision checks in real time, detecting robots in pairs. If the voxel representations of the two robots have overlapping voxels in space, this indicates that the robots may collide during movement. In this process, the swept volume of the robot is represented as and , collision detection can be expressed as: ； in, represents the intersection operation of sets, Indicates that the intersection is not empty.

8. The sampling-based multi-robot path planning method according to claim 6, characterized in that: Also includes, If the robot tasks that collide or deadlock have different priorities, the path will be expanded according to the priority and the subsequent route will be replanned; When one or more robots do not have a clear moving target, and one or more robots with a clear target are involved, the robots without a target are regarded as "arbitrary target" robots, and their positions are kept unchanged as much as possible during the path expansion process; if they hinder other robots, they are moved to a position that does not affect the path; if all robots are without a target, there is no path planning problem involved, and they can simply fall back to the previous step to randomly select a path; When the robots involved have the same priority or no priority, one of them is randomly selected for path expansion, and the deadlocked robots are allowed to collaborate through the communication mechanism. If the same path is still selected after path expansion and the deadlock is not resolved, any robot is given priority to re-plan the path, and other robots in the deadlock state stop moving.

Images