Dual-arm robot collaborative motion planning method and system for multi-robot collaboration and coexistence

Abstract

A dual-arm robot collaborative motion planning method and system for multi-robot collaboration and coexistence. The method comprises: constructing a robot-robot-environment coexistence collision information estimation model; discretizing an overall reachable space of a dual-arm robot, and using a depth camera to acquire environmental information; using a gap estimation neural network model to acquire interaction information between the dual-arm robot and the environment, and to also acquire interaction information between left and right robotic arms, so as to realize robot-robot and robot-environment synchronous collision detection; on the basis of the robot-robot-environment coexistence collision information estimation model, performing dual-arm robot collaborative path planning to acquire an estimated feasible path; and on the basis of a geometric collision examination device, performing dual-arm robot motion path safety examination and path repair, so as to ensure the absolute safety of a planned path. Therefore, the present application can solve the problem of the low efficiency of existing dual-arm robot motion planning methods making it impossible to effectively implement a multi-robot collaborative operation.

Description

A Method and System for Cooperative Motion Planning of Dual-Arm Robots for Multi-Machine Collaboration

[0001] This application claims priority to Chinese patent application filed on December 27, 2024, with application number CN202411947228.1 and entitled "A Method and System for Cooperative Motion Planning of a Dual-Arm Robot for Multi-Machine Collaboration and Integration", the entire contents of which are incorporated herein by reference. Technical Field

[0002] This invention belongs to the field of collaborative motion planning technology for dual-arm robots, and in particular relates to a collaborative motion planning method and system for dual-arm robots oriented towards multi-machine cooperation and integration. Background Technology

[0003] Compared to single-arm robots, dual-arm robots are not simply combinations of two robotic arms, but significantly expand the system's operational modes. Dual-arm robots possess greater flexibility and rigidity, capable of switching between closed-chain and open-chain states structurally according to different tasks. They combine the flexibility of serial structures with the rigidity of parallel structures, improving the system's maneuverability. Due to their superior performance, dual-arm robots show broad application prospects in the industrial field. To adapt to complex tasks, the two robotic arms need to coordinate and cooperate in a specific spatial environment to complete the work, i.e., multi-robot collaboration. When the distance between two robotic arms is too close, their workspaces will overlap, forming a shared workspace. In an open-chain structure, the working characteristics of a dual-arm robot include: 1) overlapping workspaces; 2) dispersed task objects. The overlapping workspaces mean that the robotic arms face the risk of collision when moving in the shared area, while the dispersed task objects indicate that each robotic arm is not constrained by the spatial position or posture of the shared task objects during movement. Therefore, when achieving synchronous collision-free path planning, a dual-arm robot in an open-chain structure must meet the following constraints: in addition to avoiding environmental obstacles, it must also avoid collisions with other robotic arms. Therefore, each robotic arm must not only avoid external obstacles during the planning process, but also avoid potential collisions with adjacent robotic arms. Furthermore, the high-dimensional joint configuration space of dual-arm robots greatly increases the planning difficulty.

[0004] Existing methods, such as sampling-based motion planning algorithms and artificial potential field methods, suffer from low planning efficiency, struggle to solve the challenge of synchronous motion planning for dual-arm robots, and are unable to effectively achieve multi-robot collaborative integration. To address these problems and challenges, this invention proposes a collaborative motion planning method and system for dual-arm robots oriented towards multi-robot collaboration and integration. Summary of the Invention

[0005] To address the above technical problems, this invention provides a method and system for collaborative motion planning of dual-arm robots for multi-machine cooperation and integration.

[0006] The technical solution adopted by this invention to solve its technical problem is:

[0007] A collaborative motion planning method for dual-arm robots oriented towards multi-robot cooperation and integration, the method comprising the following steps:

[0008] S100: Set the overall reachable space of the dual-arm robot and discretize the reachable space;

[0009] S200: Utilizes a depth camera to acquire point cloud information of obstacles within the overall reachable space of the dual-arm robot, constructing an environmental obstacle occupancy matrix E. o Construct the occupancy matrix U of the left robotic arm respectively. L And the right robotic arm occupies matrix U R , among which, U L and U R The information used to determine the element value settings is obtained based on the gap inference neural network model;

[0010] S300: Based on the environmental obstacle occupancy matrix E o The left robotic arm occupies the matrix U. L And the right robotic arm occupies matrix U R Construct a machine-machine-environment collision information estimation model;

[0011] S400: Construct the starting configuration node and target configuration node of the dual-arm robot, initialize a path search tree, randomly sample H groups of configuration points in the dual-arm joint space, find the H path nodes closest to each sampling point in the path search tree, and form H candidate path edges;

[0012] S500: Perform equidistant interpolation on each edge to obtain Q interpolation points. The first N joint angles and the last N joint angles of each interpolation point are used to form the joint angle configuration points of the left and right robotic arms, respectively. The collision estimation results of Q interpolation points are obtained by batch inputting the machine-machine-environment fusion collision information estimation model. The last collision-free interpolation point in the interpolation points of each candidate path edge is selected as a new path expansion node and added to the path search tree.

[0013] S600: When the preset path planning termination condition is met, the path planning process ends. Based on the geometric collision checker, the dual-arm robot path is checked and safety repaired, and finally an effective and absolutely collision-free safe path is obtained.

[0014] Preferably, S100 specifically comprises:

[0015] S110: Assume the reachable spaces of the left and right robotic arms of the dual-arm robot are represented by S respectively. L and S R , for S Land S R By taking the union of the sets, we can obtain the overall reachable space S of the dual-arm robot. DR ;

[0016] S120: For S L and S R Find the intersection to obtain the common workspace S of the dual-arm robot. AR ;

[0017] S130: The overall reachable space of the dual-arm robot S DR It is divided into K voxels, each voxel representing a cube space of size d×d×d, where the value of d is set by the user.

[0018] Preferably, S200 specifically includes:

[0019] S210: Use a depth camera to acquire point cloud information of obstacles within the reachable space of the dual-arm robot, and construct an environmental obstacle occupancy matrix E. o E o It is a K×1 dimensional matrix, where E o The rules for setting the values ​​of the K elements in the equation are as follows: If a point cloud exists within the k-th voxel, it indicates that an obstacle exists within that voxel, then E o The corresponding k-th element is set to 1, otherwise it is set to 0, where k = 1, 2, ..., K;

[0020] S220: Construct the left robotic arm occupancy matrix U respectively. L And the right robotic arm occupies matrix U R All are K×1 dimensional matrices, U L and U R The setting rules are as follows: (Different from the environmental obstacle occupancy matrix E) o U L and U R The information used to determine the element value setting is obtained based on the gap inference neural network model.

[0021] Preferably, U in S220 L and U R The information used to determine the element value setting is obtained based on the gap inference neural network model, specifically:

[0022] S221: For the left and right robotic arms, the joint space of the left robotic arm is represented as follows: The joint space of the right robotic arm is represented as Where N represents the degrees of freedom of the left and right robotic arms, assuming that the degrees of freedom of the left and right robotic arms are the same;

[0023] S222: By inputting the joint configuration nodes B1 and B2 of the left and right robotic arms into the neural network model for gap estimation, the reachable space S of the left and right robotic arms to the overall dual-arm robot can be obtained simultaneously. DR The corresponding gap value matrix F of K voxels L and F R Where B1 and B2 are both N×1 dimensional matrices, F L and F R All are K×1 dimensional matrices;

[0024] S223: For matrix F L Or F R The k-th element in the array, where k = 1, 2, ..., K, if its value is less than the safety threshold ξ, indicates that the left or right robotic arm is in contact with the reachable space S. DR If the k-th voxel has an intersection, then the matrix U L or U R Set the k-th element to 1, otherwise set it to 0, and finally let U... L =F L U R =F R .

[0025] Preferably, S300 includes:

[0026] S310: Define the collision information matrix W between the robotic arm and the environment. er =(E o &U L )|(E o &U R If matrix W er If there is an element with a value of 1, it indicates that the dual-arm robot has collided with an environmental obstacle; otherwise, it indicates that the dual-arm robot has not collided with an environmental obstacle.

[0027] S320: Define the machine-to-machine collision information matrix W rr =U L &U R If W rr The existence of an element with a value of 1 indicates that there is a shared workspace S for the dual-arm robot. AR If a collision occurs between the left and right robotic arms, it indicates that a collision did not occur between the left and right robotic arms.

[0028] Preferably, S400 includes:

[0029] S410: Assume the initial configuration node q of the left robotic arm of the dual-arm robot. SL and target configuration node q GL In joint space, they are respectively represented as and The starting configuration node q of the right robotic arm SR and target configuration node q GR In joint space, they are respectively represented as and The motion planning of the dual-arm robot is performed as a whole, that is, the initial configuration node q of the dual-arm robot is constructed. SD for and target configuration node q GD for Initialize a path search tree T D T D The initial nodes are q SD ;

[0030] S420: Batch random sampling of H groups of configuration points in the bi-arm joint space Where the h-th sampling point This represents the nth joint angle of the robotic arm at the hth sampling point, where n = 1, 2, ..., 2N;

[0031] S430: For the above H group configuration points, find the path tree T. D The H nearest path nodes to each sampling point H candidate path edges are formed.

[0032] Preferably, S500 includes:

[0033] S510: Perform equal-distance interpolation on each edge, with an interpolation step size of St. o Thus, a total of Q interpolation points are obtained.

[0034] S520: For Q interpolation points The first N joint angles and the last N joint angles of each interpolation point are respectively used to form the joint angle configuration points of the left robot arm and the joint angle configuration points of the right robot arm. By batch inputting these into the machine-machine-environment fusion collision information estimation model, the collision estimation results of Q interpolation points can be obtained.

[0035] S530: Select the last collision-free interpolation point among the interpolation points of each candidate path edge as the new path expansion node, and obtain the total H. N Add a new expansion node to the path search tree T. D , where H N ≤H.

[0036] Preferably, the preset path planning termination condition in S600 is as follows:

[0037] The latest extended node q new Satisfy ||q new -qGD If || < ε, it indicates that a pre-calculated feasible path P for the dual-arm robot has been successfully found. cb Where ε>0, is preset by the user; or

[0038] The path planning time has reached its limit.

[0039] Preferably, in S600, a geometric collision checker is used to perform path checking and safety repair for the dual-arm robot, ultimately obtaining an effective, absolutely collision-free safe path, including:

[0040] Use a geometric collision checker to detect P cb Is it possible for each path node to collide if P...? cb Without collision, the dual-arm robot executes motion path P. cb If any collision is detected, initiate the following path safety repair process:

[0041] The geometry collision checker identified the collision nodes. Where z = 1, 2, ..., Z, Z is the number of colliding nodes. For each colliding node... Select the nearest non-collision-free node before and after it as the local starting configuration node. and local target nodes Then, a path is planned between these local starting nodes using traditional path planning methods;

[0042] If the initial repair attempt fails, then the local start configuration node... and local target configuration nodes Adjust them respectively to be closer to the global starting configuration node q SD and global initial configuration node target q GD Repeat the above process for each collision-free path node until a valid, absolutely collision-free safe path is found.

[0043] A collaborative motion planning system for dual-arm robots oriented towards multi-robot cooperation includes a reachability space discretization module, a robot arm occupancy matrix construction module, a robot-machine-environment collision information estimation model construction module, a candidate path edge determination module, a path search tree expansion module, and a collision-free safe path determination module.

[0044] The reachable space discretization module is used to set the overall reachable space of the dual-arm robot and to discretize the reachable space.

[0045] The robotic arm occupancy matrix construction module is used to acquire obstacle point cloud information within the overall reachable space of the dual-arm robot using a depth camera, and to construct the environmental obstacle occupancy matrix E. o Construct the occupancy matrix U of the left robotic arm respectively. LAnd the right robotic arm occupies matrix U R , among which, U L and U R The information used to determine the element value settings is obtained based on the gap inference neural network model;

[0046] The module for constructing a machine-machine-environment fusion collision information estimation model is used to estimate collision information based on the environmental obstacle occupancy matrix E. o The left robotic arm occupies the matrix U. L And the right robotic arm occupies matrix U R Construct a machine-machine-environment collision information estimation model;

[0047] The candidate path edge determination module is used to construct the starting configuration node and target configuration node of the dual-arm robot, initialize a path search tree, randomly sample H groups of configuration points in the dual-arm joint space, find the H path nodes that are closest to each sample point in the path search tree, and form H candidate path edges.

[0048] The path search tree extension module is used to perform equidistant interpolation on each edge to obtain Q interpolation points. The first N joint angles and the last N joint angles of each interpolation point are used to form the left robot arm joint angle configuration points and the right robot arm joint angle configuration points, respectively. The collision estimation results of Q interpolation points are obtained by batch inputting the machine-machine-environment fusion collision information estimation model. The last collision-free interpolation point in the interpolation points of each candidate path edge is selected as a new path extension node and added to the path search tree.

[0049] A collision-free safe path determination module is used to terminate the path planning process when preset path planning termination conditions are met. Based on a geometric collision checker, the module performs path checks and safety repairs on the dual-arm robot, ultimately obtaining an effective, absolutely collision-free safe path. The aforementioned collaborative motion planning method and system for multi-robot cooperative operation of dual-arm robots achieves synchronous collision detection between robots and between the robot and the environment by constructing a machine-machine-environment cooperative collision information estimation model. Furthermore, based on the machine-machine-environment cooperative information model, a collaborative planning strategy and path safety repair steps for dual-arm robots are designed, ultimately aiming to achieve multi-robot cooperative operation. Attached Figure Description

[0050] Figure 1 is a flowchart of a collaborative motion planning method for a dual-arm robot oriented towards multi-machine cooperation and integration in one embodiment of the present invention;

[0051] Figure 2 is a schematic diagram of path repair by a dual-arm robot in one embodiment of the present invention. Detailed Implementation

[0052] To enable those skilled in the art to better understand the technical solution of the present invention, the present invention will be further described in detail below with reference to the accompanying drawings.

[0053] In one embodiment, as shown in Figure 1, a collaborative motion planning method for a dual-arm robot oriented towards multi-machine cooperation includes the following steps:

[0054] S100: Set the overall reachable space of the dual-arm robot and discretize the reachable space;

[0055] S200: Utilizes a depth camera to acquire point cloud information of obstacles within the overall reachable space of the dual-arm robot, constructing an environmental obstacle occupancy matrix E. o Construct the occupancy matrix U of the left robotic arm respectively. L And the right robotic arm occupies matrix U R , among which, U L and U R The information used to determine the element value settings is obtained based on the gap inference neural network model;

[0056] S300: Based on the environmental obstacle occupancy matrix E o The left robotic arm occupies the matrix U. L And the right robotic arm occupies matrix U R Construct a machine-machine-environment collision information estimation model;

[0057] S400: Construct the starting configuration node and target configuration node of the dual-arm robot, initialize a path search tree, randomly sample H groups of configuration points in the dual-arm joint space, find the H path nodes closest to each sampling point in the path search tree, and form H candidate path edges;

[0058] S500: Perform equidistant interpolation on each edge to obtain Q interpolation points. The first N joint angles and the last N joint angles of each interpolation point are used to form the joint angle configuration points of the left and right robotic arms, respectively. The collision estimation results of Q interpolation points are obtained by batch inputting the machine-machine-environment fusion collision information estimation model. The last collision-free interpolation point in the interpolation points of each candidate path edge is selected as a new path expansion node and added to the path search tree.

[0059] S600: When the preset path planning termination condition is met, the path planning process ends. Based on the geometric collision checker, the dual-arm robot path is checked and safety repaired, and finally an effective and absolutely collision-free safe path is obtained.

[0060] In one embodiment, S100 specifically includes:

[0061] S110: Assume the reachable spaces of the left and right robotic arms of the dual-arm robot are represented by S respectively. L and S R , for S L and SR By taking the union of the sets, we can obtain the overall reachable space S of the dual-arm robot. DR ;

[0062] S120: For S L and S R Find the intersection to obtain the common workspace S of the dual-arm robot. AR ;

[0063] S130: The overall reachable space of the dual-arm robot S DR It is divided into K voxels, each voxel representing a cube space of size d×d×d, where the value of d is set by the user.

[0064] In one embodiment, S200 specifically includes:

[0065] S210: Use a depth camera to acquire point cloud information of obstacles within the reachable space of the dual-arm robot, and construct an environmental obstacle occupancy matrix E. o E o It is a K×1 dimensional matrix, where E o The rules for setting the values ​​of the K elements in the equation are as follows: If a point cloud exists within the k-th voxel, it indicates that an obstacle exists within that voxel, then E o The corresponding k-th element is set to 1, otherwise it is set to 0, where k = 1, 2, ..., K;

[0066] S220: Construct the left robotic arm occupancy matrix U respectively. L And the right robotic arm occupies matrix U R All are K×1 dimensional matrices, U L and U R The setting rules are as follows: (Different from the environmental obstacle occupancy matrix E) o U L and U R The information used to determine the element value settings (whether there is an intersection between the robotic arm and the voxel) does not come from the depth camera, but is obtained based on the gap inference neural network model.

[0067] In one embodiment, U in S220 L and U R The information used to determine the element value setting is obtained based on the gap inference neural network model, specifically:

[0068] S221: For the left and right robotic arms, the joint space of the left robotic arm is represented as follows: The joint space of the right robotic arm is represented as Where N represents the degrees of freedom of the left and right robotic arms, assuming that the degrees of freedom of the left and right robotic arms are the same;

[0069] S222: By inputting the joint configuration nodes B1 and B2 of the left and right robotic arms into the neural network model for gap estimation, the reachable space S of the left and right robotic arms to the overall dual-arm robot can be obtained simultaneously. DR The corresponding gap value matrix F of K voxels L and F R Where B1 and B2 are both N×1 dimensional matrices, F L and F R All are K×1 dimensional matrices;

[0070] S223: For matrix F L Or F R The k-th element in the array, where k = 1, 2, ..., K, if its value is less than the safety threshold ξ (ξ ≥ 0, which can be set by the user), indicates that the left or right robotic arm is in contact with the reachable space S. DR If the k-th voxel has an intersection, then the matrix U L or U R Set the k-th element to 1, otherwise set it to 0, and finally let U... L =F L U R =F R .

[0071] In one embodiment, the gap estimation neural network model comprises four sequentially forward-connected parts, as follows:

[0072] The first part uses the robotic arm joint configuration node as the initial input and employs the kernel function P. H =[sin(2 0 πQ),cos92 0 πQ),sin(2 1 πQ),cos(2 1 πQ),…sin(2 V πQ),cos(2 V [πQ)] Encodes the joint angles and increases the frequency of the input values, where Q represents the input robotic arm joint configuration node and V is an overclocking parameter;

[0073] The second part consists of a fully connected layer, a ReLU activation function, and a DropOut layer. The output of the second part will be used as the input of the third part.

[0074] The third part consists of a fully connected layer, a ReLU activation function, and a DropOut layer. The output of the third part will be used as the input of the fourth part.

[0075] The fourth part consists of a fully connected layer, which outputs K features. Its purpose is to map the joint space of the robotic arm to the K-dimensional gap distance.

[0076] In one embodiment, S300 includes:

[0077] S310: Define the collision information matrix W between the robotic arm and the environment. er =(E o &U L )|(E o &U R If matrix W er If there is an element with a value of 1, it indicates that the dual-arm robot has collided with an environmental obstacle; otherwise, it indicates that the dual-arm robot has not collided with an environmental obstacle.

[0078] S320: Define the machine-to-machine collision information matrix W rr =U L &U R If W rr The existence of an element with a value of 1 indicates that there is a shared workspace S for the dual-arm robot. AR If a collision occurs between the left and right robotic arms, it indicates that a collision did not occur between the left and right robotic arms.

[0079] During motion planning, the dual-arm robot needs to ensure W at all times. rr and W er All are zero matrices, which means that collisions between all robotic arms and environmental obstacles are avoided, and collisions between the left and right robotic arms are also avoided.

[0080] In one embodiment, S400 includes:

[0081] S410: Assume the initial configuration node q of the left robotic arm of the dual-arm robot. SL and target configuration node q GL In joint space, they are respectively represented as and The starting configuration node q of the right robotic arm SR and target configuration node q GR In joint space, they are respectively represented as and The motion planning of the dual-arm robot is performed as a whole, that is, the initial configuration node q of the dual-arm robot is constructed. SD for and target configuration node q GD for Initialize a path search tree T D T D The initial nodes are q SD ;

[0082] S420: Batch random sampling of H groups of configuration points in the bi-arm joint space Where the h-th sampling point This represents the nth joint angle of the robotic arm at the hth sampling point, where n = 1, 2, ..., 2N;

[0083] S430: For the above H group configuration points, find the path tree T. D The H nearest path nodes to each sampling point H candidate path edges are formed.

[0084] In one embodiment, S500 includes:

[0085] S510: Perform equal-distance interpolation on each edge, with an interpolation step size of St. o Thus, a total of Q interpolation points are obtained.

[0086] S520: For Q interpolation points The first N joint angles and the last N joint angles of each interpolation point are respectively used to form the joint angle configuration points of the left robot arm and the joint angle configuration points of the right robot arm. By batch inputting these into the machine-machine-environment fusion collision information estimation model, the collision estimation results of Q interpolation points can be obtained.

[0087] S530: Select the last collision-free interpolation point among the interpolation points of each candidate path edge as the new path expansion node, and obtain the total H. N Add a new expansion node to the path search tree T. D , where H N ≤H.

[0088] In one embodiment, the preset path planning termination condition in S600 is specifically as follows:

[0089] The latest extended node q new Satisfy ||q new -q GD If || < ε, it indicates that a pre-calculated feasible path P for the dual-arm robot has been successfully found. cb Where ε>0 is preset by the user; or the path planning time reaches the upper limit.

[0090] The P planned above cb Because a machine-machine-environment collision information estimation model incorporating a gap neural network is used, path safety repair is necessary. This is because neural networks may make prediction errors, such as incorrectly estimating potential collision points of the robotic arms as collision-free. Therefore, after successfully finding a predicted feasible path P for the dual-arm robot... cb Afterwards, a path check and safety repair of the dual-arm robot are required.

[0091] In one embodiment, as shown in Figure 2, S600 performs path checking and safety repair on the dual-arm robot based on a geometric collision checker, ultimately obtaining an effective, absolutely collision-free safe path, including:

[0092] Use a geometric collision checker to detect P cb Is it possible for each path node to collide if P...? cb Without collision, the dual-arm robot executes motion path P. cb If any collision is detected, initiate the following path safety repair process:

[0093] The geometry collision checker identified the collision nodes. Where z = 1, 2, ..., Z, Z is the number of colliding nodes. For each colliding node... Select the nearest non-collision-free node before and after it as the local starting configuration node. and local target nodes Then, a path is planned between these local starting nodes using traditional path planning methods (RRT or PRM methods);

[0094] If the initial repair attempt fails, then the local start configuration node... and local target configuration nodes Adjust them respectively to be closer to the global starting configuration node q SD and global initial configuration node target q GD Repeat the above process for each collision-free path node until a valid, absolutely collision-free safe path is found.

[0095] A collaborative motion planning system for a dual-arm robot oriented towards multi-machine cooperation includes a reachability space discretization module, a robot arm occupancy matrix construction module, a robot-machine-environment collision information estimation model establishment module, a candidate path edge determination module, a path search tree expansion module, and a collision-free safe path determination module.

[0096] The reachability space discretization module is used to set the overall reachability space of the dual-arm robot and to discretize the reachability space.

[0097] The robotic arm occupancy matrix construction module is used to acquire obstacle point cloud information within the overall reachable space of the dual-arm robot using a depth camera, and to construct the environmental obstacle occupancy matrix E. o Construct the occupancy matrix U of the left robotic arm respectively. L And the right robotic arm occupies matrix U R , among which, U L and U R The information used to determine the element value settings is obtained based on the gap inference neural network model;

[0098] The machine-machine-environment fusion collision information estimation model building module is used to establish a model based on the environmental obstacle occupancy matrix E. o The left robotic arm occupies the matrix U. L And the right robotic arm occupies matrix U R Construct a machine-machine-environment collision information estimation model;

[0099] The candidate path edge determination module is used to construct the starting configuration node and target configuration node of the dual-arm robot, initialize a path search tree, randomly sample H groups of configuration points in the dual-arm joint space, find the H path nodes in the path search tree that are closest to each sample point, and form H candidate path edges;

[0100] The path search tree extension module is used to perform equidistant interpolation on each edge to obtain Q interpolation points. The first N joint angles and the last N joint angles of each interpolation point are used to form the joint angle configuration points of the left and right robotic arms, respectively. The collision estimation results of Q interpolation points are obtained by batch inputting the machine-machine-environment fusion collision information estimation model. The last collision-free interpolation point in the interpolation points of each candidate path edge is selected as a new path extension node and added to the path search tree.

[0101] The collision-free safe path determination module is used to end the path planning process when the preset path planning termination conditions are met. Based on the geometric collision checker, the dual-arm robot path is checked and safety repaired, and finally an effective absolutely collision-free safe path is obtained.

[0102] Specific limitations regarding the collaborative motion planning system for a dual-arm robot oriented towards multi-machine cooperation and harmony can be found in the limitations of the collaborative motion planning method for a dual-arm robot oriented towards multi-machine cooperation and harmony described above, and will not be repeated here. Each module in the aforementioned collaborative motion planning system for a dual-arm robot oriented towards multi-machine cooperation and harmony can be implemented entirely or partially through software, hardware, or a combination thereof. These modules can be embedded in the processor of a computer device in hardware form or independent of it, or stored in the memory of a computer device in software form, so that the processor can call and execute the corresponding operations of each module.

[0103] The above-mentioned collaborative motion planning method and system for dual-arm robots oriented towards multi-machine cooperation and integration has the following technical effects:

[0104] (1) This invention innovatively constructs a machine-machine-environment fusion collision information estimation model, which can simultaneously achieve efficient collision detection between robots and environmental obstacles, and between robots;

[0105] (2) By treating the dual-arm robot as a whole for motion planning, the present invention greatly simplifies the motion planning process of the dual-arm robot, thereby achieving efficient collaborative planning of the dual-arm robot.

[0106] (3) This invention utilizes the batch data processing capability of neural networks to achieve efficient collision detection between machines and the environment, and also proposes corresponding motion path safety repair steps. While improving planning efficiency, it also ensures the safety of the dual-arm robot during the movement process.

[0107] The above provides a detailed description of the cooperative motion planning method and system for dual-arm robots oriented towards multi-machine collaboration and integration provided by this invention. Specific examples have been used to illustrate the principles and implementation methods of this invention. The descriptions of the embodiments above are merely for the purpose of helping to understand the core ideas of this invention. It should be noted that those skilled in the art can make various improvements and modifications to this invention without departing from its principles, and these improvements and modifications also fall within the protection scope of the claims of this invention.

Claims

1. A method for cooperative motion planning of a dual-arm robot for multi-machine collaboration and integration, characterized in that, The method includes the following steps: S100: Set the overall reachable space of the dual-arm robot and discretize the reachable space; S200: Utilizes a depth camera to acquire point cloud information of obstacles within the overall reachable space of the dual-arm robot, constructing an environmental obstacle occupancy matrix E. o Construct the occupancy matrix U of the left robotic arm respectively. L And the right robotic arm occupies matrix U R , among which, U L and U R The information used to determine the element value settings is obtained based on the gap inference neural network model; S300: Based on the environmental obstacle occupancy matrix E o The left robotic arm occupies the matrix U. L And the right robotic arm occupies matrix U R Construct a machine-machine-environment collision information estimation model; S400: Construct the starting configuration node and target configuration node of the dual-arm robot, initialize a path search tree, randomly sample H groups of configuration points in the dual-arm joint space, find the H path nodes closest to each sampling point in the path search tree, and form H candidate path edges; S500: Perform equidistant interpolation on each edge to obtain Q interpolation points. The first N joint angles and the last N joint angles of each interpolation point are used to form the joint angle configuration points of the left and right robotic arms, respectively. The collision estimation results of Q interpolation points are obtained by batch inputting the machine-machine-environment fusion collision information estimation model. The last collision-free interpolation point in the interpolation points of each candidate path edge is selected as a new path expansion node and added to the path search tree. S600: When the preset path planning termination condition is met, the path planning process ends. Based on the geometric collision checker, the dual-arm robot path is checked and safety repaired, and finally an effective and absolutely collision-free safe path is obtained.

2. The method according to claim 1, characterized in that, S100 specifically refers to: S110: Assume the reachable spaces of the left and right robotic arms of the dual-arm robot are represented by S respectively. L and S R , for S L and S R By taking the union of the sets, we can obtain the overall reachable space S of the dual-arm robot. DR ; S120: For S L and S R Find the intersection to obtain the common workspace S of the dual-arm robot. AR ; S130: The overall reachable space of the dual-arm robot S DR It is divided into K voxels, each voxel representing a cube space of size d×d×d, where the value of d is set by the user.

3. The method according to claim 2, characterized in that, S200 specifically refers to: S210: Use a depth camera to acquire point cloud information of obstacles within the reachable space of the dual-arm robot, and construct an environmental obstacle occupancy matrix E. o E o It is a K×1 dimensional matrix, where E o The rules for setting the values ​​of the K elements in the equation are as follows: If a point cloud exists within the k-th voxel, it indicates that an obstacle exists within that voxel, then E o The corresponding k-th element is set to 1, otherwise it is set to 0, where k = 1, 2, ..., K; S220: Construct the left robotic arm occupancy matrix U respectively. L And the right robotic arm occupies matrix U R All are K×1 dimensional matrices, U L and U R The setting rules are as follows: (Different from the environmental obstacle occupancy matrix E) o U L and U R The information used to determine the element value setting is obtained based on the gap inference neural network model.

4. The method according to claim 3, characterized in that, S220 in U L and U R The information used to determine the element value setting is obtained based on the gap inference neural network model, specifically: S221: For the left and right robotic arms, the joint space of the left robotic arm is represented as follows: The joint space of the right robotic arm is represented as Where N represents the degrees of freedom of the left and right robotic arms, assuming that the degrees of freedom of the left and right robotic arms are the same; S222: By inputting the joint configuration nodes B1 and B2 of the left and right robotic arms into the neural network model for gap estimation, the reachable space S of the left and right robotic arms to the overall dual-arm robot can be obtained simultaneously. DR The corresponding gap value matrix F of K voxels L and F R Where B1 and B2 are both N×1 dimensional matrices, F L and F R All are K×1 dimensional matrices; S223: For matrix F L Or F R The k-th element in the array, where k = 1, 2, ..., K, if its value is less than the safety threshold ξ, indicates that the left or right robotic arm is in contact with the reachable space S. DR If the k-th voxel has an intersection, then the matrix U L or U R Set the k-th element to 1, otherwise set it to 0, and finally let U... L =F L U R =F R .

5. The method according to claim 4, characterized in that, The S300 includes: S310: Define the collision information matrix W between the robotic arm and the environment. er =(E o &U L )|(E o &U R If matrix W er If there is an element with a value of 1, it indicates that the dual-arm robot has collided with an environmental obstacle; otherwise, it indicates that the dual-arm robot has not collided with an environmental obstacle. S320: Define the machine-to-machine collision information matrix W rr =U L &U R If W rr The existence of an element with a value of 1 indicates that there is a shared workspace S for the dual-arm robot. AR If a collision occurs between the left and right robotic arms, it indicates that a collision did not occur between the left and right robotic arms.

6. The method according to claim 5, characterized in that, The S400 includes: S410: Assume the initial configuration node q of the left robotic arm of the dual-arm robot. SL and target configuration node q GL In joint space, they are respectively represented as and The starting configuration node q of the right robotic arm SR and target configuration node q GR In joint space, they are respectively represented as and The motion planning of the dual-arm robot is performed as a whole, that is, the initial configuration node q of the dual-arm robot is constructed. SD for and target configuration node q GD for Initialize a path search tree T D T D The initial nodes are q SD ; S420: Batch random sampling of H groups of configuration points in the bi-arm joint space Where the h-th sampling point This represents the nth joint angle of the robotic arm at the hth sampling point, where n = 1, 2, ..., 2N; S430: For the above H group configuration points, find the path tree T. D The H nearest path nodes to each sampling point H candidate path edges are formed.

7. The method according to claim 6, characterized in that, The S500 includes: S510: Perform equal-distance interpolation on each edge, with an interpolation step size of St. o Thus, a total of Q interpolation points are obtained. S520: For Q interpolation points The first N joint angles and the last N joint angles of each interpolation point are respectively used to form the joint angle configuration points of the left robot arm and the joint angle configuration points of the right robot arm. By batch inputting these into the machine-machine-environment fusion collision information estimation model, the collision estimation results of Q interpolation points can be obtained. S530: Select the last collision-free interpolation point among the interpolation points of each candidate path edge as the new path expansion node, and obtain the total H. N Add a new expansion node to the path search tree T. D , where H N ≤H.

8. The method according to claim 7, characterized in that, The specific path planning termination conditions preset in S600 are as follows: The latest extended node q new Satisfy ||q new -q GD If || < ε, it indicates that a pre-calculated feasible path P for the dual-arm robot has been successfully found. cb Where ε>0, is preset by the user; or The path planning time has reached its limit.

9. The method according to claim 8, characterized in that, In the S600, a geometric collision checker is used for path inspection and safety repair of the dual-arm robot, ultimately resulting in an effective, absolutely collision-free safe path, including: Detect P using a geometric collision checker cb Is it possible for each path node to collide if P...? cb Without collision, the dual-arm robot executes motion path P. cb If any collision is detected, initiate the following path safety repair process: The geometry collision checker identified the collision nodes. Where z = 1, 2, ..., Z, Z is the number of colliding nodes. For each colliding node... Select the nearest non-collision-free node before and after it as the local starting configuration node. and local target nodes Then, a path is planned between these local starting nodes using traditional path planning methods; If the initial repair attempt fails, then the local start configuration node... and local target configuration nodes Adjust them respectively to be closer to the global starting configuration node q SD and global initial configuration node target q GD Repeat the above process for each collision-free path node until a valid, absolutely collision-free safe path is found.

10. A collaborative motion planning system for dual-arm robots oriented towards multi-machine cooperation and integration, characterized in that, It includes modules for discretization of reachable space, construction of robotic arm occupancy matrix, construction of machine-to-machine-to-environment collision information estimation model, determination of candidate path edges, expansion of path search tree, and determination of collision-free safe path. The reachable space discretization module is used to set the overall reachable space of the dual-arm robot and to discretize the reachable space. The robotic arm occupancy matrix construction module is used to acquire obstacle point cloud information within the overall reachable space of the dual-arm robot using a depth camera, and to construct the environmental obstacle occupancy matrix E. o Construct the occupancy matrix U of the left robotic arm respectively. L And the right robotic arm occupies matrix U R , among which, U L and U R The information used to determine the element value settings is obtained based on the gap inference neural network model; The module for constructing a machine-machine-environment fusion collision information estimation model is used to estimate collision information based on the environmental obstacle occupancy matrix E. o The left robotic arm occupies the matrix U. L And the right robotic arm occupies matrix U R Construct a machine-machine-environment collision information estimation model; The candidate path edge determination module is used to construct the starting configuration node and target configuration node of the dual-arm robot, initialize a path search tree, randomly sample H groups of configuration points in the dual-arm joint space, find the H path nodes in the path search tree that are closest to each sample point, and form H candidate path edges. The path search tree extension module is used to perform equidistant interpolation on each edge to obtain Q interpolation points. The first N joint angles and the last N joint angles of each interpolation point are used to form the left robot arm joint angle configuration points and the right robot arm joint angle configuration points, respectively. The collision estimation results of Q interpolation points are obtained by batch inputting the machine-machine-environment fusion collision information estimation model. The last collision-free interpolation point in the interpolation points of each candidate path edge is selected as a new path extension node and added to the path search tree. The collision-free safe path determination module is used to end the path planning process when the preset path planning termination conditions are met. Based on the geometric collision checker, the dual-arm robot path is checked and safety repaired, and finally an effective absolutely collision-free safe path is obtained.

Images