Motion Planning Method, System and Device for Dual-Arm Robots in Strong Coupling Scenarios

Abstract

The present invention discloses a motion planning method, system and device for a dual-arm robot facing a strong coupling scenario, and the main contents are as follows: constructing a kinematic model of a closed-chain system of a strong coupling dual-arm robot, given a set of joint configurations of a robotic arm, the corresponding set of joint configurations of the other robotic arm can be output through this kinematic model, thereby greatly reducing the dimension of the planning space; master-slave batch adaptive planning strategy: the robotic arm performs active batch adaptive path planning, and secondly, based on the aforementioned kinematic model of the strong coupling dual-arm closed-chain system, the robotic arm performs passive batch adaptive path planning; performing pre-motion safety inspection on the dual-arm robot to ensure the absolute safety of the coordinated motion of the two arms during the closed-chain operation. It effectively solves the problems of real-time performance and safety in the strong coupling scenario, and provides reliable technical support for industrial-level dual-arm collaboration.

Description

Technical Field

[0001] The present invention belongs to the technical field of motion planning of closed-chain dual-arm robot systems, and particularly relates to a motion planning method, system, and device for a dual-arm robot facing a strongly coupled scenario. Background Art

[0002] In recent years, due to their flexibility and collaboration ability, dual-arm robot systems have shown great potential in fields such as industrial assembly and precision operation. However, in strongly coupled collaborative scenarios (such as dual-arm collaborative handling, closed-chain operation, etc.), the existing motion planning methods face the following key problems: 1. Complexity of high dimensions and motion constraints: The dual-arm closed-chain system forms a strongly coupled configuration by two manipulators jointly operating an object, and its motion needs to simultaneously meet the requirements of closed-chain constraints, obstacle avoidance, and self-collision avoidance. Traditional open-chain planning methods cannot be directly applied, and it is necessary to coordinate the motion of the two arms in a high-dimensional joint space (with degrees of freedom up to ), resulting in an exponential increase in the planning complexity. 2. Insufficient real-time performance and computational efficiency: Existing methods (such as algorithms based on sampling RRT*) need to frequently solve inverse kinematics and collision detection in the closed-chain system, significantly increasing the computational time consumption, making it difficult to meet the real-time planning requirements and restricting the feasibility of industrial applications. 3. Lack of a dedicated modeling framework for closed-chain systems: Most studies regard the two arms as independent individuals for planning, ignoring the kinematic coupling characteristics of the closed-chain system, and need to repeatedly call inverse kinematics to solve the trajectory of the secondary arm, resulting in computational redundancy and being easily trapped in local optima.

[0003] To address the above problems, the present invention proposes a motion planning method and system for a dual-arm robot facing a strongly coupled collaborative scenario. Summary of the Invention

[0004] To solve the above technical problems, the present invention provides a motion planning method, system, and device for a dual-arm robot facing a strongly coupled scenario.

[0005] The technical solution adopted by the present invention to solve its technical problems is:

[0006] A motion planning method for a dual-arm robot facing a strongly coupled scenario, the method comprising the following steps:

[0007] S100: Construct a kinematic model of a closed-chain system of a strongly coupled dual-arm robot, input a set of joint configurations of one manipulator , and output a corresponding set of joint configurations of the other manipulator through the kinematic model;

[0008] S200: Initialize the active path search tree of one manipulator and the passive path search tree of the other manipulator , and in the manipulator , in the manipulator Perform batch target-guided sampling in the joint space, obtain K sampling points, perform batch adaptive step size expansion, and form candidate expansion edges;

[0009] S300: Perform equidistant interpolation on each candidate expansion edge, input the interpolation points into the robotic arm batch collision estimation neural network model to obtain the collision estimation results of the interpolation points, and find the last interpolation point estimated to be collision-free for each candidate expansion edge as the active path search tree of the robotic arm new expansion node, and obtain the updated active path search tree of the robotic arm ;

[0010] S400: Input the active path search tree of the robotic arm 's new expansion nodes into the kinematic model of the strongly coupled dual-arm robot closed-chain system, obtain the corresponding set of joint configuration points of the robotic arm , find the best joint configuration point in each set as the passive path search tree of the robotic arm new expansion node, and obtain the updated passive path search tree of the robotic arm ;

[0011] S500: Determine whether both the active path search tree of the updated robotic arm and the passive path search tree of the updated robotic arm are successfully connected to the corresponding target points. If the connection is successful, the motion planning of the strongly coupled dual-arm robot closed-chain system ends, and the robotic arm and respectively obtain a joint space motion path and . If the connection fails, return to S200 - S400 until a feasible path is successfully found;

[0012] S600: Use a geometric collision checker to perform a safety check on the joint space motion path. If no collision nodes are found, execute the joint space motion path.

[0013] Preferably, the dual-arm robot body consists of 2 robotic arms, denoted as and respectively, and have degrees of freedom of and respectively, and their base coordinate systems are respectively represented as and , and their tool coordinate system matrices are respectively represented as and . When the dual-arm robot operates an object jointly, the dual-arm robot and the object form a strongly coupled dual-arm robot closed-chain system. S100 includes:

[0014] S110: Given any set of joint configurations of the robotic arm , obtain the pose matrix of the end effector in the base coordinate system according to its forward kinematics. , represents the forward kinematics function of the robotic arm; Since the robotic arms and operate an object jointly, the pose matrix of the end effector of the robotic arm in the base coordinate system can be obtained by . , where is the pose transformation matrix from to ; Further, the pose matrix of the end effector of the robotic arm in the base coordinate system , where is the pose transformation matrix from the base coordinate system to ;

[0015] S120: Based on the pose matrix of the end effector of the robotic arm in the coordinate system , the corresponding set of joint configuration points of can be solved by the inverse kinematics function of the robotic arm ; Since the inverse kinematics of the robotic arm includes multiple solutions, the set includes multiple sets of joint configuration points, that is , where = , representing a set of inverse solutions of the robotic arm = , and is , is the number of corresponding inverse solutions.

[0016] Preferably, S200 includes:

[0017] S210: Initialize the robotic arm Active path search tree and the robotic arm passive path search tree , including joint configuration start node and joint configuration target node , including joint configuration start node and point ;

[0018] S220: Conduct batch target-guided sampling within the joint space of the robotic arm, that is, randomly generate a random number between [0, 1] , if , then select the target node as the sampling point, otherwise randomly sample within the joint space of the robotic arm to obtain a sampling point, repeat the above process times to batch obtain sampling points ( );

[0019] S230: Perform batch adaptive step size expansion on the sampling points of the robotic arm, find the path nodes in the active path search tree that are closest to each sampling point through Euclidean distance calculation , form candidate expansion edges , ,…, ,…, .

[0020] Preferably, S300 includes:

[0021] S310: Perform equidistant interpolation for each candidate expansion edge, with the interpolation step size being , thus obtaining a total of interpolation points ;

[0022] S320: Input the interpolation points into the robotic arm batch collision estimation neural network model at once, obtain the collision estimation results of the interpolation points, and then find the last interpolation point estimated to be collision-free for each candidate expansion edge as the new expansion node of the active path search tree , obtaining a total of new expansion nodes​ , add the active path search tree to the node set.

[0023] Preferably, the robotic arm batch collision estimation neural network model includes four parts. The first part encodes the robotic arm joint configuration nodes as the initial input and uses the kernel function to encode the joint angles, increasing the frequency of the input values. The output of the first part will be used as the input of the second part, where is a dimensional matrix, and

[0024] is an overclocking parameter;

[0025] The second part includes a fully connected layer, the activation function Sigmoid, and a dropout layer. The output of the second part will be used as the input of the third part;

[0026] The third part is exactly the same as the second part, including a fully connected layer, the activation function Sigmoid, and a dropout layer. The output of the third part will be used as the input of the fourth part; The fourth part consists of a fully connected layer, and the number of output features is

[0027] aiming to achieve the mapping from the robotic arm joint space to the collision states between the robotic arm and

[0028] Preferably, S400 includes: S410: For the active path search tree of the robotic arm of new extended nodes , input them into the kinematic model of the strongly coupled two-arm closed-chain system to obtain the corresponding set of joint configuration points of the robotic arm ;

[0029] S420: Input all the joint configuration points in the set of joint configuration points into the robotic arm batch collision estimation neural network model to obtain the collision estimation results of joint configuration points at one time, where the number of all joint configuration points is , , is the number of joint configuration points in the th set;

[0030] S430: Eliminate ​The joint configuration points estimated to be in collision in the set of joint configuration points are obtained to get a collision-free set of joint configuration points ;

[0031] S440: For any set of joint configuration points , its corresponding optimal joint configuration point , where , ; is the joint configuration point planned in the previous step of the robotic arm , aiming to avoid excessive displacement of the joints of the robot R2; repeat the above process, and finally, the optimal joint configuration points in the set of collision-free joint configuration points can be obtained , and they are used as new extended nodes to be added to the robotic arm passive path search tree set.

[0032] Preferably, S500 is specifically:

[0033] Detect whether the new extended nodes in the active path search tree can achieve collision-free connection to the joint configuration target node of the robotic arm , and synchronously detect whether the new extended nodes in the passive path search tree can achieve collision-free connection to the joint configuration target node of the robotic arm . If both the updated active path search tree of the robotic arm and the updated passive path search tree of the robotic arm are successfully connected to the corresponding target points, the motion planning of the strongly coupled dual-arm robot closed-chain system ends at this time, and the robotic arms and respectively obtain a joint space motion path and , otherwise, repeat S200 to S400 until a feasible path is successfully found.

[0034] Preferably, S600:

[0035] Use a geometric collision checker component for the robotic arms and Perform collision safety checks on the joint space motion path of the robot. If there is a collision at a joint configuration node, re-execute S200 to S500 until an absolutely safe motion path for the dual-arm robot is found. If no collision nodes are found after the FCL component and checks, the two manipulators of the dual-arm robot and respectively execute the planned paths and .

[0036] A motion planning system for a dual-arm robot facing a strongly coupled scenario, including a kinematic model construction module, a candidate extended edge determination module, an active path search tree update module, a passive path search tree update module, a feasible path determination module, and a safety check module;

[0037] The kinematic model construction module is used to construct a kinematic model of the closed-chain system of the strongly coupled dual-arm robot, input a set of joint configurations of the manipulator , and output a set of corresponding joint configurations of the other manipulator through the kinematic model;

[0038] The candidate extended edge determination module is used to initialize the active path search tree of the manipulator and the passive path search tree of the manipulator . Perform batch target-guided sampling in the joint space of the manipulator , obtain K sampling points for batch adaptive step size expansion, and form candidate extended edges;

[0039] The active path search tree update module is used to perform equidistant interpolation on each candidate extended edge, input the interpolation points into the manipulator batch collision estimation neural network model to obtain the collision estimation results of the interpolation points, find the last interpolation point estimated to be collision-free for each candidate extended edge, and use it as the new extended node of the active path search tree of the manipulator , and obtain the updated active path search tree of the manipulator ;

[0040] The passive path search tree update module is used to input the active path search tree of the manipulator new extended nodes into the kinematic model of the closed-chain system of the strongly coupled dual-arm robot, and obtain the corresponding of the manipulator a set of joint configuration points, find the optimal joint configuration point in each set as the robotic arm Passive path search tree new extended nodes to obtain an updated robotic arm of the passive path search tree ;

[0041] A feasible path determination module for determining whether the active path search tree of the updated robotic arm and the passive path search tree of the updated robotic arm and the updated robotic arm of the passive path search tree are both successfully connected to the corresponding target points. If the connection is successful, the motion planning of the strongly coupled dual-arm robot closed-chain system ends, and the robotic arm and respectively obtain a joint space motion path and , if the connection fails, return to the candidate extended edge determination module until a feasible path is successfully found;

[0042] A safety inspection module for using a geometric collision checker to perform safety inspections on the joint space motion path. If no collision nodes are found, execute the joint space motion path.

[0043] A computer device includes a memory and a processor. The memory stores a computer program, and when the processor executes the computer program, it implements the steps of the motion planning method for a dual-arm robot in a strongly coupled scenario.

[0044] The above-mentioned motion planning method, system and device for a dual-arm robot in a strongly coupled scenario. The core innovation lies in the closed-chain kinematic modeling and batch adaptive planning strategy: First, build a closed-chain kinematic model, and by establishing a strong coupling relationship between the poses of the main and auxiliary arms, avoid repeated inverse kinematic calculations and significantly reduce the planning dimension. Second, based on the collision estimation learning model of the dual-arm robot, perform master-passive batch self-planning. Among them, the main arm actively explores the global path, and the auxiliary arm passively generates a batch of candidate trajectories based on the closed-chain model. Combining adaptive step size optimization and batch collision estimation, the efficiency and path quality are improved. Finally, before the dual-arm machine executes the planned path, use a geometric collision checker to perform safety inspections on the planned path to ensure the absolute safety of the coordinated motion of the two arms during the closed-chain operation. The present invention effectively solves the real-time and safety problems in strongly coupled scenarios through systematic integration of closed-chain modeling, efficient planning and safety verification, and provides reliable technical support for industrial-level dual-arm collaboration. Description of the Drawings

[0045] Figure 1 is a flowchart of a motion planning method for a dual-arm robot in a strongly coupled scenario according to an embodiment of the present invention;

[0046] Figure 2 Schematic diagram of the neural network model structure for batch collision estimation of the robotic arm in an embodiment of the present invention;

[0047] Figure 3 Schematic diagram of the master - slave batch adaptive planning strategy in an embodiment of the present invention. Detailed implementation manners

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

[0049] In one embodiment, as Figure 1 , for the motion planning method of a dual - arm robot facing a strongly - coupled scenario, the method includes the following steps:

[0050] S100: Construct a kinematic model of the closed - chain system of the strongly - coupled dual - arm robot, input a set of joint configurations of one robotic arm , and output a set of corresponding joint configurations of the other robotic arm through the kinematic model;

[0051] S200: Initialize the active path search tree of the robotic arm and the passive path search tree of the robotic arm . Conduct batch target - guided sampling in the joint space of the robotic arm to obtain K sampling points for batch adaptive step - size expansion, and form candidate expansion edges; S300: Perform equidistant interpolation on each candidate expansion edge, input the interpolation points into the neural network model for batch collision estimation of the robotic arm to obtain the collision estimation results of the interpolation points, find the last interpolation point estimated to be collision - free for each candidate expansion edge, and use it as the new expansion node of the active path search tree of the robotic arm

[0052] , and obtain the updated active path search tree of the robotic arm ;

[0053] S400: Input the new expansion nodes of the active path search tree of the robotic arm into the kinematic model of the closed - chain system of the strongly - coupled dual - arm robot to obtain a set of corresponding joint configuration points of the robotic arm . Find the best joint configuration point in each set and use it as the passive path search tree of the robotic arm corresponding ;​A new extended node to obtain an updated robotic arm of the passive path search tree ;

[0054] S500: Determine whether the active path search tree of the updated robotic arm and the passive path search tree of the updated robotic arm are both successfully connected to the corresponding target points. If the connection is successful, the motion planning of the strongly coupled dual-arm robot closed-chain system ends, and the robotic arms and respectively obtain a joint space motion path and . If the connection fails, return to S200 - S400 until a feasible path is successfully found;

[0055] S600: Use a geometric collision checker to perform a safety check on the joint space motion path. If no collision nodes are found, execute the joint space motion path.

[0056] In one embodiment, the dual-arm robot body consists of 2 robotic arms, denoted as and , and have degrees of freedom of and respectively, and their base coordinate systems are respectively represented as and . Their tool coordinate system matrices are respectively represented as and . When the dual-arm robot operates an object together, the dual-arm robot and the object form a strongly coupled dual-arm robot closed-chain system. S100 includes:

[0057] S110: Given any set of joint configurations of the robotic arm , obtain the pose matrix of the end effector in the base coordinate system according to its forward kinematics. , represents the forward kinematics function of the robotic arm; Since the robotic arms and operate an object together, the pose matrix of the end effector of the robotic arm in the base coordinate system can be obtained from , , where is from Pose transformation matrix; further, the base coordinate system of the robotic arm end effector pose matrix , where is the pose transformation matrix from the base coordinate system to ;

[0058] S120: Based on the pose matrix of the end effector of the robotic arm under the coordinate system , the inverse kinematics function of the robotic arm can be used to solve for the corresponding set of joint configuration points in the joint configuration of the robotic arm ; Since the inverse kinematics of the robotic arm includes multiple solutions, the set contains multiple sets of joint configuration points, that is , where , and = , representing a set of inverse solutions of the robotic arm , , is the number of corresponding inverse solutions.

[0059] Specifically, for the closed-loop system of a strongly coupled dual-arm robot, just input any set of joint configurations of the robotic arm , and the corresponding set of joint configuration points of the robotic arm can be obtained through S110 and S120, thus simplifying the subsequent motion planning process of the dual-arm robot.

[0060] Next, the master-slave batch adaptive planning strategy is carried out: The robotic arm performs active batch adaptive path planning. Secondly, based on the aforementioned kinematic model of the strongly coupled dual-arm closed-loop system, the robotic arm performs passive batch adaptive path planning.

[0061] In one embodiment, as shown in Figure 3 : S200 includes:

[0062] S210: Initialize the active path search tree of the robotic arm and the passive path search tree of the robotic arm , including the starting joint configuration node And the joint configuration target node , including the joint configuration starting node and point ;

[0063] S220: Perform batch target-guided sampling in the manipulator joint space, that is, randomly generate a random number between [0, 1] . If , then select the target node as the sampling point, otherwise randomly sample in the manipulator joint space to obtain a sampling point, and repeat the above process times to batch obtain sampling points ( );

[0064] S230: Perform batch adaptive step size expansion on the sampling points of the manipulator . Find the nearest active path search tree path nodes to each sampling point through Euclidean distance calculation, and form candidate expansion edges , ,…, .

[0065] In one embodiment, S300 includes:

[0066] S310: Perform equidistant interpolation for each candidate expansion edge, and the interpolation step size is , so as to obtain a total of interpolation points ;

[0067] S320: Input the interpolation points into the manipulator batch collision estimation neural network model at one time to obtain the collision estimation results of the interpolation points. Then, find the last interpolation point estimated to be collision-free for each candidate expansion edge as the new expansion node of the active path search tree . A total of new expansion nodes are obtained and added to the node set of the active path search tree .

[0068] In one embodiment, as Figure 2 shown, the manipulator batch collision estimation neural network model includes four parts. The first part is by using the manipulator joint configuration node As the initial input and using a kernel function Encode the joint angles to increase the frequency of the input values. The output of the first part will be used as the input of the second part, where is a dimensional matrix, and

[0069] The second part includes a fully connected layer, an activation function Sigmoid, and a dropout layer. The output of the second part will be used as the input of the third part;

[0070] The third part is exactly the same as the second part, including a fully connected layer, an activation function Sigmoid, and a dropout layer. The output of the third part will be used as the input of the fourth part;

[0071] The fourth part consists of a fully connected layer, and the number of output features is The purpose is to realize the mapping from the manipulator joint space to the collision states of the manipulator with environmental obstacles, where the collision states include collision or no collision.

[0072] In one embodiment, as Figure 3 shown, S400 includes:

[0073] S410: For the active path search tree of the manipulator of the new extended nodes, input them into the kinematic model of the strongly coupled two-arm closed-chain system, and obtain the corresponding manipulator of a set of joint configuration points ;

[0074] S420: Input all the joint configuration points in the set of joint configuration points into the manipulator batch collision estimation neural network model to obtain the collision estimation results of the joint configuration points at one time, where the number of all joint configuration points is , , is the number of joint configuration points in the th set;

[0075] S430: Remove the joint configuration points estimated to be in collision in the set of joint configuration points to obtain a collision-free set of joint configuration points ;

[0076] S440: For any set of joint configuration points , the corresponding optimal joint configuration point , where , ; is the joint configuration point planned in the previous step for the robotic arm , aiming to avoid excessive displacement of the joints of the robot R2; repeating the above process, finally, no-collision joint configuration point sets in the optimal joint configuration point is obtained and added as a new extended node to the robotic arm passive path search tree

[0077] In one embodiment, S500 is specifically:

[0078] Detect whether the new extended nodes in the active path search tree can achieve collision-free connection to the robotic arm joint configuration target node , and simultaneously detect whether the new extended nodes in the passive path search tree can achieve collision-free connection to the robotic arm joint configuration target node . If both the updated robotic arm active path search tree and the updated robotic arm passive path search tree are successfully connected to the corresponding target points, at this time, the motion planning of the strongly coupled dual-arm robot closed-chain system ends, and the robotic arms and respectively obtain a joint space motion path and . Otherwise, repeat S200 to S400 until a feasible path is successfully found.

[0079] In one embodiment, S600:

[0080] Use the geometric collision checker FCL (Flexible-collision-library) component to perform collision safety checks on the joint space motion paths of the robotic arms and . If there is a collision at a joint configuration node, re-execute S200 to S500 until an absolutely safe motion path for the dual-arm robot is found. If the FCL component for and After inspection, no collision nodes are found, so the two manipulators of the dual-arm robot and respectively execute the planned path and .

[0081] The beneficial effects of the present invention are as follows:

[0082] (1) Compared with the existing motion planning technology of dual-arm robots that directly plans in the entire joint configuration space, the technology proposed in the present invention greatly reduces the dimension of the planning space by constructing a kinematic model of the closed-chain system of strongly coupled dual-arm robots, which is beneficial to improving the planning efficiency.

[0083] (2) Compared with the problem of too long planning time in the motion planning technology of dual-arm robots, the present invention proposes a master-slave batch adaptive planning strategy, that is, the manipulator performs active batch adaptive path planning. Secondly, based on the aforementioned kinematic model of the strongly coupled dual-arm closed-chain system, the manipulator performs passive batch adaptive path planning, and a manipulator batch collision estimation neural network model is used in the planning process to accelerate the collision detection process, which can greatly accelerate the motion planning process of the strongly coupled dual-arm robot closed-chain system.

[0084] (3) The technology proposed in the present invention not only greatly improves the planning efficiency, but also ensures the absolute safety of the coordinated motion of the two arms during the closed-chain operation through the pre-motion safety inspection step of the dual-arm robot.

[0085] A motion planning system for a dual-arm robot facing a strongly coupled scenario includes a kinematic model construction module, a candidate expansion edge determination module, an active path search tree update module, a passive path search tree update module, a feasible path determination module, and a safety inspection module;

[0086] The kinematic model construction module is used to construct a kinematic model of the closed-chain system of strongly coupled dual-arm robots. Input a set of joint configurations of a manipulator , and output a set of corresponding joint configurations of another manipulator through the kinematic model;

[0087] The candidate expansion edge determination module is used to initialize the active path search tree of the manipulator and the passive path search tree of the manipulator . Perform batch target-guided sampling in the joint space of the manipulator , obtain K sampling points for batch adaptive step expansion, and form candidate expansion edges;

[0088] The active path search tree update module is used to perform equidistant interpolation on each candidate extension edge, input the interpolation points into the robotic arm batch collision estimation neural network model to obtain the collision estimation results of the interpolation points, and find the last interpolation point estimated to be collision-free for each candidate extension edge as the active path search tree of the robotic arm of the robotic arm new extended nodes, and obtain the updated active path search tree of the robotic arm of the robotic arm ;

[0089] The passive path search tree update module is used to input the active path search tree of the robotic arm of the robotic arm of the new extended nodes into the kinematic model of the strongly coupled dual-arm robot closed-chain system, obtain the corresponding set of joint configuration points of the robotic arm, find the best joint configuration point in each set as the new extended node of the passive path search tree of the robotic arm of the robotic arm passive path search tree and obtain the updated passive path search tree of the robotic arm of the robotic arm ;

[0090] The feasible path determination module is used to determine whether both the active path search tree of the updated robotic arm and the passive path search tree of the updated robotic arm are successfully connected to the corresponding target points. If the connection is successful, the motion planning of the strongly coupled dual-arm robot closed-chain system ends, and the robotic arm and respectively obtain a joint space motion path and to and . If the connection fails, it returns to the candidate extension edge determination module until a feasible path is successfully found;

[0091] The safety inspection module is used to perform safety inspections on the joint space motion path using a geometric collision checker. If no collision nodes are found, the joint space motion path is executed.

[0092] For the specific limitations of the dual-arm robot motion planning system for strongly coupled scenarios, reference can be made to the limitations of the dual-arm robot motion planning method for strongly coupled scenarios described above, which will not be elaborated here. Each module in the above-mentioned dual-arm robot motion planning system for strongly coupled scenarios can be implemented in whole or in part by software, hardware, and their combination. The above-mentioned modules can be embedded in the processor of the computer device in hardware form or independent of it, or stored in the memory of the computer device in software form, so that the processor can call and execute the operations corresponding to each of the above modules.

[0093] A computer device includes a memory and a processor. The memory stores a computer program, and when the processor executes the computer program, the steps of the dual-arm robot motion planning method for strongly coupled scenarios are implemented.

[0094] Those of ordinary skill in the art can understand that all or part of the processes in the above-mentioned method embodiments can be completed by instructing relevant hardware through a computer program. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the above-mentioned method embodiments. Among them, any reference to memory, storage, database, or other media used in the embodiments provided in this application can include at least one of non-volatile and volatile memories. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, or optical memory, etc. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM), etc.

[0095] The above has introduced in detail a dual-arm robot motion planning method, system, and device provided by the present invention. Specific examples are used in this article to elaborate on the principle and implementation manner of the present invention. The description of the above embodiments is only used to help understand the core idea of the present invention. It should be noted that for those of ordinary skill in the art in the technical field, without departing from the principle of the present invention, several improvements and modifications can be made to the present invention, and these improvements and modifications also fall within the protection scope of the claims of the present invention.

Claims

1. A motion planning method for a dual-arm robot facing a strong coupling scenario, characterized in that The method includes the following steps: S100: Construct the kinematic model of the closed-chain system of a strongly coupled dual-arm robot, input a set of joint configurations of one robotic arm and output the corresponding set of joint configurations of the other robotic arm through the kinematic model; S200: Initialize the robotic arm Active path search tree and the robotic arm 's passive path search tree , perform batch target-guided sampling in the joint space of the robotic arm , obtain K sampling points for batch adaptive step size expansion, and form candidate expansion edges; S300: Perform equidistant interpolation on each candidate extension edge, input the interpolation points into the robotic arm batch collision estimation neural network model to obtain the collision estimation results of the interpolation points, and find the last interpolation point estimated to be collision-free for each candidate extension edge as the active path search tree of the robotic arm new extended node, obtaining the updated active path search tree of the robotic arm ; S400: Input the active path search tree of the robotic arm into the kinematic model of the strongly coupled dual-arm robot closed-chain system, and obtain the set of corresponding joint configuration points of the robotic arm . Find the best joint configuration point in each set as the new extended node of the passive path search tree of the robotic arm , and obtain the updated passive path search tree of the robotic arm ; Find the best joint configuration point in each set as the new extended node of the passive path search tree of the robotic arm ; Find the best joint configuration point in each set as the new extended node of the passive path search tree of the robotic arm ; ​ S500: Determine the updated robotic arm 's active path search tree and the updated robotic arm 's passive path search tree are both successfully connected to the corresponding target points. If the connection is successful, the motion planning of the strongly coupled dual-arm robot closed-chain system ends, and the robotic arm and respectively obtain a joint space motion path to . If the connection fails, return to S200 - S400 until a feasible path is successfully found; S600: Use a geometric collision checker to perform a safety check on the joint space motion path. If no collision nodes are found, execute the joint space motion path.

2. The method according to claim 1, wherein The double-arm robot body consists of 2 robotic arms, denoted as and , and The degrees of freedom of and are and respectively. Their base coordinate systems are respectively represented as and respectively. Their tool coordinate system matrices are respectively represented as When the double-arm robot operates an object jointly, the double-arm robot and the object form a strongly coupled double-arm robot closed-chain system. S100 includes: S110: Given any set of joint configurations of the robotic arm , obtain the pose matrix of the end effector in the base coordinate system according to its forward kinematics . Denote the forward kinematics function of the robotic arm . Since the robotic arm and jointly operate an object, the pose matrix of the end effector of the robotic arm in the base coordinate system can be obtained by . Herein , where is the pose transformation matrix from to . Further, the pose matrix of the end effector of the robotic arm in the base coordinate system is , where is the pose transformation matrix from the base coordinate system to . S120: Based on the coordinate system the lower robotic arm pose matrix of the end effector , the robotic arm inverse kinematic function can be used to solve the corresponding set of joint configuration points at the joint configuration of the robotic arm ; Since the inverse kinematics of the robotic arm has multiple sets of solutions, the set includes multiple sets of joint configuration points, that is , where = represents a set of inverse solutions of the robotic arm , , is the number of corresponding inverse solutions.

3. The method according to claim 2, wherein S200 includes: S210: Initialize the robotic arm Active path search tree and the robotic arm 's passive path search tree , including the starting joint configuration node and the target joint configuration node , including the starting joint configuration node and the target joint configuration node ; S220: Conduct batch target-guided sampling in the joint space of the robotic arm , that is, randomly generate a random number between [0, 1] . If , then select the target node as the sampling point; otherwise, randomly sample a sampling point in the joint space of the robotic arm . Repeat the above process times to batch obtain sampling points ( ); S230: For the robotic arm of sampling points to perform batch adaptive step size expansion, and find the active path search tree through Euclidean distance calculation the path nodes closest to each sampling point , forming candidate expansion edges , ,…, .

4. The method according to claim 3, characterized in that, S300 includes: S310: Perform equidistant interpolation for each candidate extended edge, with an interpolation step size of , thereby obtaining a total of interpolation points ; S320: Input interpolation points into the robotic arm batch collision estimation neural network model at once to obtain the collision estimation results of interpolation points. Then, find the last interpolation point estimated to be collision-free for each candidate extended edge as the new extended node of the active path search tree . A total of new extended nodes are obtained and added to the node set of the active path search tree .​​​​​​​​​​​​ 5. The method according to claim 4, wherein The robotic arm batch collision estimation neural network model consists of four parts. The first part encodes the joint angles of the robotic arm by using the kernel function with the robotic arm joint configuration as the initial input to increase the frequency of the input values. The output of the first part will be used as the input of the second part. Among them, is a × dimensional matrix, and is an overclocking parameter; The second part includes a fully connected layer, an activation function Sigmoid, and a dropout layer. The output of the second part will be used as the input of the third part; The third part is exactly the same as the second part, including a fully connected layer, an activation function Sigmoid, and a dropout layer. The output of the third part will be used as the input of the fourth part; The fourth part consists of fully connected layers, and the number of output features is , aiming to achieve the mapping from the robotic arm joint space to the collision states between the robotic arm and environmental obstacles. Among them, the collision states include collision or no collision.

6. The method according to claim 5, wherein S400 includes: S410: For the robotic arm 's active path search tree of new extended nodes , input them into the kinematic model of the strongly coupled double-arm closed-chain system, and obtain the robotic arm corresponding set of joint configuration points ; S420: Input all joint configuration points in the sets of joint configuration points into the robotic arm batch collision estimation neural network model to obtain the collision estimation results of joint configuration points at one time, where the number of all joint configuration points is , , is the number of joint configuration points in the th set; S430: Exclude the joint configuration points estimated to be in collision from the set of joint configuration points, and obtain a collision-free set of joint configuration points ; S440: For any set of joint configuration points , the corresponding optimal joint configuration point , where , ; is the joint configuration point planned in the previous step of the robotic arm , aiming to avoid excessive displacement of the joints of the robot R2; repeating the above process, finally sets of collision-free joint configuration points can be obtained, and the optimal joint configuration point among them is used as a new extended node to be added to the robotic arm passive path search tree set.

7. The method according to claim 6, wherein S500 is specifically: Detect the active path search tree of new extended nodes Can the manipulator be collision - free connected to the joint configuration target node , and synchronously detect the passive path search tree of new extended nodes Can the manipulator be collision - free connected to the joint configuration target node . If the updated manipulator active path search tree and the updated manipulator passive path search tree are both successfully connected to the corresponding target points, at this time the motion planning of the strongly coupled dual - arm robot closed - chain system ends, and the manipulator and respectively obtain a joint - space motion path to . Otherwise, repeat from S200 to S400 until a feasible path is successfully found.

8. The method according to claim 7, characterized in that, S600 includes: Use a geometric collision checker component for the robotic arm and to perform a collision safety check on the joint space motion paths. If there is a collision at a joint configuration node, re-execute S200 to S500 until an absolutely safe motion path for the dual-arm robot is found. If no collision nodes are found after the FCL component and are checked, the two robotic arms of the dual-arm robot and respectively execute the planned paths and .

9. A motion planning system for a dual-arm robot facing a strong coupling scenario, characterized in that, It includes a kinematic model construction module, a candidate extended edge determination module, an active path search tree update module, a passive path search tree update module, a feasible path determination module, and a safety check module; A kinematic model construction module, which is used to construct a kinematic model of the closed-chain system of a strongly coupled dual-arm robot. Input a set of joint configurations of one manipulator and output, through the kinematic model, the corresponding set of joint configurations of the other manipulator ; Candidate extended edge determination module for initializing the robotic arm Active path search tree and the robotic arm of the passive path search tree to perform batch target-guided sampling in the joint space of the robotic arm and obtain K sampling points for batch adaptive step extension to form candidate extended edges; The active path search tree update module is used to perform equidistant interpolation on each candidate extension edge, input the interpolation points into the robotic arm batch collision estimation neural network model to obtain the collision estimation results of the interpolation points, and find the last interpolation point estimated to be collision-free for each candidate extension edge as the active path search tree of the robotic arm ; A new extension node to obtain an updated active path search tree of the robotic arm ; ; The passive path search tree update module is used to input the active path search tree of the robotic arm into the new extended nodes of the kinematic model of the strongly coupled dual-arm robot closed-chain system to obtain the corresponding set of joint configuration points of the robotic arm, find the best joint configuration point in each set, and use it as the new extended node of the passive path search tree of the robotic arm, and obtain the updated passive path search tree of the robotic arm ; ;​​ A feasible path determination module, which is used to judge the updated robotic arm 's active path search tree and the updated robotic arm 's passive path search tree whether both are successfully connected to the corresponding target points. If the connection is successful, the motion planning of the closed-chain system of the strongly coupled dual-arm robot ends, and the robotic arm and respectively obtain a joint space motion path and . If the connection fails, return to the candidate extended edge determination module until a feasible path is successfully found; The safety check module is used to use a geometric collision checker to perform a safety check on the joint space motion path. If no collision nodes are found, execute the joint space motion path.

10. A computer device, comprising a memory and a processor, the memory storing a computer program, characterized in that, When the processor executes the computer program, it implements the steps of the method according to any one of claims 1 to 8.

Images