Double-arm robot motion planning method, system and equipment facing strong coupling scene

Abstract

The invention discloses a two-arm robot motion planning method, system and device for a strong coupling scene, and the main content is as follows: constructing a strong coupling two-arm robot closed chain system kinematic model, giving a group of mechanical arm # imgabs0 # joint configuration, and outputting a joint configuration set corresponding to another mechanical arm # imgabs1 # through the kinematic model, therefore, the planning space dimension is greatly reduced; according to a master-slave batch self-adaptive planning strategy, a mechanical arm # imgabs2 # carries out active batch self-adaptive path planning, and then a mechanical arm # imgabs3 # carries out passive batch self-adaptive path planning based on the strong-coupling double-arm closed-chain system kinematic model; and carrying out pre-motion safety check on the double-arm robot to ensure absolute safety of double-arm cooperative motion in the closed chain operation process. The problems of real-time performance and safety in a strong coupling scene are effectively solved, and reliable technical support is provided for industrial-grade double-arm cooperation.

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 collaborative capabilities, 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 reaching ), resulting in an exponential increase in the planning complexity. 2. Lack of 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, resulting in a significant increase in computational time and making it difficult to meet the requirements of real-time planning, which limits the feasibility of industrial applications. 3. Lack of a dedicated modeling framework for closed-chain systems: Most studies plan the two arms as independent individuals, 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 prone to falling into local optima.

[0003] In view of 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] In view of 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 as follows: A motion planning method for a dual-arm robot facing a strongly coupled scenario, the method comprising the following steps: 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; S200: Initialize an active path search tree of one manipulator and a passive path search tree of the other manipulator in the joint space of the manipulator, perform batch target-guided sampling, obtain K sampling points for batch adaptive step size expansion, and form Candidate extended edges; S300: Perform equidistant interpolation on each candidate extended 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 extended edge as the robotic arm Active path search tree New extended nodes, obtaining the updated robotic arm Active path search tree ; S400: Input the active path search tree of the robotic arm new extended nodes into the kinematic model of the strongly coupled dual-arm robot closed-chain system to obtain the corresponding set of joint configuration points for the robotic arm , find the optimal joint configuration point in each set as the robotic arm passive path search tree new extended nodes, obtaining the updated robotic arm passive path search tree ; 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; 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.

[0006] Preferably, 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 represented as and respectively, and their tool coordinate system matrices are represented as and , when the dual-arm robot operates an object together, the dual-arm robot and the object form a strongly coupled closed-chain system of the dual-arm robot. 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 under it according to its forward kinematics. , denotes the forward kinematics function of the robotic arm; Since the robotic arm and operate an object together, 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 is , where is the pose transformation matrix from the base coordinate system to ; 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 in the joint configuration can be solved through 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 , , is , is the number of corresponding inverse solutions.

[0007] Preferably, S200 includes: S210: Initialize the active path search tree of the robotic arm and the passive path search tree of the robotic arm , including the starting node of the joint configuration and the target node of the joint configuration , including the starting node of the joint configuration and a point ; S220: Perform 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: 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 , forming candidate expansion edges , , …, .

[0008] Preferably, S300 includes: S310: Perform equidistant interpolation for each candidate expansion edge, with the interpolation step size being , so as to obtain a total of interpolation points ; S320: Input the interpolation points into the robotic arm 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 .

[0009] Preferably, the robotic arm batch collision estimation neural network model includes four parts. The first part takes the robotic arm joint configuration node as the initial input and uses the 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. Among them, is a hyperfrequency 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 a fully connected layer, and the number of output features is , aiming to achieve the mapping from the robotic arm joint space to the collision state between the robotic arm and environmental obstacles, where the collision state includes collision or no collision.

[0010] 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 robotic arm corresponding set of joint configuration points ; 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; 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 ; S440: For any set of joint configuration points , its corresponding best joint configuration point , where , ; is the robotic arm The joint configuration points planned in the previous step are aimed at avoiding excessive displacement of the joints of the robot R2; repeat the above process, and finally, a collision-free joint configuration point set in the 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

[0011] Preferably, S500 is specifically: 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 synchronously detect 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, 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.

[0012] Preferably, S600: Use a geometric collision checker component to perform a collision safety check 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 no collision nodes are found after the FCL component checks and , then the two robotic arms of the dual-arm robot and respectively execute the planned paths and .

[0013] A motion planning system for a dual-arm robot facing a strong coupling scenario, including a kinematic model construction module, a candidate extension 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; The kinematic model construction module is used to construct a kinematic model of the closed-chain system of the strong coupling 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; The candidate extension edge determination module is used to initialize the active path search tree of the manipulator and the passive path search tree of the manipulator . Batch target-guided sampling is carried out in the joint space of the manipulator to obtain K sampling points for batch adaptive step extension, and form candidate extension 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 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 extension edge, and use it as the new extension node of the active path search tree of the manipulator to obtain the updated active path search tree of the manipulator ; The passive path search tree update module is used to input the new extension nodes of the active path search tree of the manipulator into the kinematic model of the strong coupling dual-arm robot closed-chain system, obtain the corresponding set of joint configuration points of the manipulator , find the best joint configuration point in each set, and use it as the new extension node of the passive path search tree of the manipulator to obtain the updated passive path search tree of the manipulator ; The feasible path determination module is used to judge whether both the updated active path search tree of the manipulator and the updated passive path search tree of the manipulator are successfully connected to the corresponding target points. If the connection is successful, the motion planning of the strong coupling dual-arm robot closed-chain system ends, and the manipulator and Obtain a joint space motion path respectively and , if the connection fails, return to the candidate extended edge determination module until a feasible path is successfully found; A safety inspection module, used to perform safety inspection on the joint space motion path using a geometric collision checker. If no collision nodes are found, execute the joint space motion path.

[0014] 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.

[0015] 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, construct a closed-chain kinematic model. By establishing a strong coupling relationship between the poses of the main and auxiliary arms, repetitive inverse kinematic calculations are avoided, and the planning dimension is significantly reduced. 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 robot executes the planned path, use a geometric collision checker to perform safety inspection on the planned path to ensure the absolute safety of the coordinated motion of the two arms during 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, providing reliable technical support for industrial-level dual-arm collaboration. Brief Description of the Drawings

[0016] Figure 1 It 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; Figure 2 It is a schematic structural diagram of a robotic arm batch collision estimation neural network model according to an embodiment of the present invention; Figure 3 It is a schematic diagram of a master-slave batch adaptive planning strategy according to an embodiment of the present invention. Detailed Embodiment

[0017] In order 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.

[0018] In one embodiment, as Figure 1 , for the motion planning method of a dual-arm robot in a strongly coupled scenario, the method includes the following steps: S100: Construct a kinematic model of a closed-chain system for a strongly coupled dual-arm robot, and input a set of robotic arms The joint configuration outputs another robotic arm through the kinematic model The corresponding set of joint configurations; S200: Initialize the robotic arm The active path search tree and the robotic arm The 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 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 New expansion node of the active path search tree of the robotic arm , and obtain the updated Active path search tree of the robotic arm ; S400: Input the New expansion nodes of the active path search tree of the robotic arm into the kinematic model of the strongly coupled dual-arm robot closed-chain system, obtain the corresponding Set of joint configuration points, find the best joint configuration point in each set as the New expansion node of the passive path search tree of the robotic arm , and obtain the updated Passive path search tree of the robotic arm ; ; S500: Determine whether 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. 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; S600: Use the 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.

[0019]

[0019] 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 denoted as and , and their tool coordinate system matrices are respectively denoted 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: S110: Given any set of joint configurations of the robotic arm , obtain the pose matrix of the end effector of under 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 under the base coordinate system can be obtained from , where is the pose transformation matrix from to ; further, the pose matrix of the end effector of the robotic arm under the base coordinate system , where is the pose transformation matrix from the base coordinate system to ; S120: Based on the pose matrix of the end effector of the robotic arm under the coordinate system , the corresponding set of joint configuration points of can be solved through 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 , and = represents a set of inverse solutions of the robotic arm . , is the number of corresponding inverse solutions.

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

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

[0022] In one embodiment, as Figure 3 shown, S200 includes: S210: Initialize the active path search tree of the manipulator and the passive path search tree of the manipulator , including the starting joint configuration node of and the target joint configuration node , , including the starting joint configuration node of ; S220: Perform batch target-guided sampling in the joint space of the manipulator , 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 manipulator . Repeat the above process times to batch obtain sampling points ( ); S230: Perform batch adaptive step size expansion on the sampling points of the manipulator . Find the path nodes nearest to each sampling point in the active path search tree to form Candidate extended edges , ,…, 。

[0023] In one embodiment, S300 includes: S310: Perform equidistant interpolation for each candidate extended edge, with an interpolation step size of , so as to obtain a total of interpolation points ; S320: Input the interpolation points into the robotic arm batch collision estimation neural network model at one time, obtain the collision estimation results of the interpolation points, and then find the last interpolation point estimated to be collision-free for each candidate extended edge as a new extended node of the active path search tree , and obtain a total of new extended nodes , and add them to the node set of the active path search tree .

[0024] In one embodiment, as shown in Figure 2 , the robotic arm batch collision estimation neural network model includes four parts. The first part encodes the joint angles by using the robotic arm joint configuration node as the initial input and using the kernel function 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 random inactivation 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 random inactivation 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 output feature number is , aiming to realize the mapping from the robotic arm joint space to the collision states of the robotic arm with environmental obstacles, where the collision states include collision or collision-free.

[0025] In one embodiment, as shown in Figure 3 , S400 includes: S410: For the active path search tree of the robotic arm , the of new extended nodes , input it into the kinematic model of the strong-coupling double-arm closed-chain system to obtain the manipulator corresponding set of joint configuration points ; S420: Input all the joint configuration points in the set of joint configuration points into the neural network model for batch collision estimation of the manipulator, and obtain the collision estimation results of joint configuration points at one time. Among them, the number of all joint configuration points is , , is the number of joint configuration points in the th set; S430: Eliminate 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 ; 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 manipulator, aiming to avoid excessive displacement of the joints of the robot R2; repeat the above process, and finally the optimal joint configuration point in the set of collision-free joint configuration points can be obtained, and it is used as a new expansion node to be added to the manipulator passive path search tree set. In one embodiment, S500 is specifically:

[0026] Detect whether the new expansion nodes of the active path search tree can achieve collision-free connection to the manipulator joint configuration target node , and synchronously detect whether the new expansion nodes of the passive path search tree can achieve collision-free connection to the manipulator joint configuration target node , if the updated manipulator active path search tree and the updated manipulator passive path search tree active path search tree and the updated manipulator passive path search tree All are successfully connected to the corresponding target points. At this time, the motion planning of the strong-coupling 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.

[0027] In one embodiment, S600: 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 does not find any collision nodes after checking and , then the two robotic arms of the dual-arm robot and respectively execute the planned paths and .

[0028] The beneficial effects of the present invention are as follows: (1) Compared with the existing dual-arm robot motion planning technology that directly plans in the entire joint configuration space, the technology proposed in the present invention constructs a kinematic model of the strong-coupling dual-arm robot closed-chain system, greatly reducing the dimension of the planning space and facilitating the improvement of the planning efficiency.

[0029] (2) Compared with the problem of too long planning time in the dual-arm robot motion planning technology, the present invention proposes a master-slave batch adaptive planning strategy, that is, the robotic arm performs active batch adaptive path planning. 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, and a robotic arm batch collision estimation neural network model is used during the planning process to accelerate the collision detection process, which can greatly accelerate the motion planning process of the strong-coupling dual-arm robot closed-chain system.

[0030] (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.

[0031] A dual-arm robot motion planning system for a strong-coupling scenario 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 inspection module; A kinematic model construction module for constructing a kinematic model of a strongly coupled dual-arm robot closed-chain system, which inputs a set of joint configurations of one robotic arm and outputs a corresponding set of joint configurations of the other robotic arm through the kinematic model; A candidate extended edge determination module for initializing the active path search tree of the robotic arm and the passive path search tree of the robotic arm to perform batch target-guided sampling in the joint space of the robotic arm, obtain K sampling points for batch adaptive step size extension, and form a number of candidate extended edges; An active path search tree update module for performing equidistant interpolation on each candidate extended edge, inputting the interpolation points into the robotic arm batch collision estimation neural network model to obtain the collision estimation results of the interpolation points, and finding the last interpolation point estimated to be collision-free for each candidate extended edge as a new extended node of the active path search tree of the robotic arm to obtain the updated active path search tree of the robotic arm ; A passive path search tree update module for inputting the new extended nodes of the active path search tree of the robotic arm into the kinematic model of the strongly coupled dual-arm robot closed-chain system, obtaining the corresponding set of joint configuration points of the robotic arm, finding the best joint configuration point in each set as a new extended node of the passive path search tree of the robotic arm to obtain the updated passive path search tree of the robotic arm ; A feasible path determination module for determining whether 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. 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, it returns to the candidate extended edge determination module until a feasible path is successfully found; ​​​​​​​​​​​​​​A safety check module is used to perform a safety check on the joint space motion path using a geometric collision checker. If no collision nodes are found, the joint space motion path is executed.

[0032] 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 in the above text, which will not be elaborated here. Each module in the above dual-arm robot motion planning system for strongly coupled scenarios can be implemented in whole or in part by software, hardware, and their combinations. The above modules can be embedded in the processor of the computer device in hardware form or be independent of it, or can be stored in the memory of the computer device in software form, so that the processor can call and execute the operations corresponding to the above modules.

[0033] 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.

[0034] Those of ordinary skill in the art can understand that all or part of the processes in the above 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 method embodiments. Among them, any reference to the 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.

[0035] 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 of this technology, 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 dual-arm robot motion planning method for strongly coupled scenarios, characterized in that: The method comprises the following steps: S100: Construct a kinematic model of a strongly coupled dual-arm robot closed-chain system, input a set of robotic arms The joint configuration of another robot arm is output through the kinematic model The corresponding joint configuration set; S200: Initialize the robot arm Active Path Search Tree And the robotic arm Passive path search tree , in the robotic arm Perform batch target guided sampling in the joint space, obtain K sampling points for batch adaptive step expansion, and form candidate extension edges; S300: Perform equidistant interpolation on each candidate extension edge, and input the interpolation point into the batch collision estimation neural network model of the robot arm to obtain the collision estimation result of the interpolation point, and find the last interpolation point estimated as collision-free for each candidate extension edge as the robot arm Active Path Search Tree New extension node, get updated robot arm Active Path Search Tree ; S400: Put the robot arm Active Path Search Tree of The new extended node is input into the kinematic model of the strongly coupled dual-arm robot closed-chain system to obtain the robot arm Corresponding joint configuration point sets, find the best joint configuration point in each set as the robot arm Passive Path Search Tree New extension node, get updated robot arm Passive path search tree ; S500: Determine the updated robotic arm Active Path Search Tree And the updated robotic arm Passive path search tree Are they all connected to the corresponding target points successfully? If so, the motion planning of the strongly coupled dual-arm robot closed-chain system is completed, and the robot arm and Get a joint space motion path respectively and , if the connection fails, return to S200-S400 until a feasible path is successfully found; S600: Use the geometric collision checker to perform a safety check on the joint space motion path. If no collision nodes are found, the joint space motion path is executed.

2. The method according to claim 1, characterized in that The dual-arm robot body consists of two mechanical arms, which are respectively and , and The degrees of freedom are and , and its base coordinate system is expressed as and , and its tool coordinate system matrices are expressed 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: S110: Given a robot arm Any set of joint configurations , obtain the base coordinate system according to its forward kinematics Down The pose matrix of the end effector , Represents the positive kinematic function of the robot arm; since the robot arm and To manipulate an object together, Get the base coordinate system Lower Robotic Arm The pose matrix of the end effector , ,in for arrive The pose transformation matrix; further, the base coordinate system Lower Robotic Arm The pose matrix of the end effector ,in Base coordinate system arrive The pose transformation matrix of S120: Based on coordinate system Lower Robotic Arm The pose matrix of the end effector , you can use the robot arm The inverse kinematics function , solve In the robotic arm Joint Configuration The corresponding joint configuration point set ; Since the inverse kinematics of the robot contains multiple sets of solutions, the set It includes multiple sets of joint configuration points, that is, ,in = , indicating the robotic arm A set of inverse solutions, , yes The number of corresponding inverse solutions.

3. The method according to claim 2, characterized in that S200 includes: S210: Initialize the robot arm Active Path Search Tree And the robotic arm Passive path search tree , include The joint configuration start node and joint configuration target nodes , include The joint configuration start node and Point ; S220: In the robot arm Batch target guided sampling is performed in the joint space, that is, a random number is randomly generated between [0,1] ,like , then select the target node As sampling point, otherwise in the robot arm Randomly sample in the joint space to obtain a sampling point and repeat the above process Batch acquisition Sampling points ( ); S230: For the robot arm of sampling points to perform batch adaptive step size expansion, and find the active path search tree through Euclidean distance calculation The closest to each sampling point Path Node ,form Candidate extension edges , ,…, .

4. The method according to claim 3, characterized in that S300 includes: S310: Perform equidistant interpolation for each candidate extension edge, with an interpolation step length of , thus obtaining a total of Interpolation points ; S320: interpolation points are input into the robotic arm batch collision estimation neural network model at one time to obtain The collision estimation results of the interpolation points are then used to find the last interpolation point estimated to be collision-free for each candidate extension edge as the active path search tree. New expansion nodes, a total of New extension nodes , join the active path search tree A collection of nodes.

5. The method according to claim 4, characterized in that The neural network model for batch collision estimation of robotic arms consists of four parts. The first part is to configure the nodes of the robotic arm joints. As initial input, and use the kernel function Encode the joint angles and increase the frequency of the input values. The output of the first part will be used as the input of the second part, where yes dimensional matrix, It is an overclocking parameter; The second part includes a fully connected layer, an activation function Sigmoid, and a random 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 the fully connected layer, the activation function Sigmoid and the random inactivation 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 The purpose is to achieve from the robot arm joint space to the robot arm and A mapping of the collision status of environmental obstacles, wherein the collision status includes collision or no collision.

6. The method according to claim 5, characterized in that S400 includes: S410: For robotic arms Active Path Search Tree of New extension nodes , input it into the kinematic model of the strongly coupled double-arm closed-chain system to obtain the robotic arm Corresponding Joint configuration point set ; S420: All joint configuration points in the joint configuration point set are input into the robot arm batch collision estimation neural network model, and the results are obtained at one time. The collision estimation results of joint configuration points, where the number of all joint configuration points is , , For the The number of joint configuration points in a set; S430: Remove The joint configuration points estimated to be colliding in the set of joint configuration points are used to obtain a collision-free Joint configuration point set ; S440: For any set of joint configuration points , the corresponding optimal joint configuration point ,in , ; For robotic arm The joint configuration points planned in the previous step are intended to prevent excessive displacement of the joints of robot R2. Repeat the above process and you will eventually get A set of collision-free joint configuration points The best joint configuration point in , and add it to the robot arm as a new extension node Passive Path Search Tree gather.

7. The method according to claim 6, characterized in that S500 is specifically: Detection of active path search tree of New extension nodes Can we connect the robotic arms without collision? Joint configuration target node , and synchronously detect the passive path search tree of New extension nodes Can we connect the robotic arms without collision? Joint configuration target node , if the updated robot arm Active Path Search Tree And the updated robotic arm Passive Path Search Tree All are successfully connected to the corresponding target points. At this time, the motion planning of the closed-chain system of the strongly coupled dual-arm robot is completed. and Get a joint space motion path respectively and , otherwise, repeat S200 to S400 until a feasible path is successfully found.

8. The method according to claim 7, characterized in that S600 includes: Use the geometric collision checker component to check the robot arm and The joint space motion path is checked for collision safety. If there is a collision with a joint configuration node, S200 to S500 are executed again until an absolutely safe dual-arm robot motion path is found. and After checking, no collision nodes were found. The two arms of the dual-arm robot and Execute the planned paths separately and .

9. A dual-arm robot motion planning system for strongly coupled scenarios, characterized in that: It includes a kinematic model building module, a candidate extension 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; Kinematic model building module, used to build a kinematic model of a strongly coupled dual-arm robot closed-chain system, input a set of robotic arms The joint configuration of another robot arm is output through the kinematic model The corresponding joint configuration set; Candidate extension edge determination module, used to initialize the robotic arm Active Path Search Tree And the robotic arm Passive path search tree , in the robotic arm Perform batch target guided sampling in the joint space, obtain K sampling points for batch adaptive step expansion, and form candidate extension edges; The active path search tree update module is used to perform equidistant interpolation on each candidate extension edge and input the interpolation point into the batch collision estimation neural network model of the robot arm to obtain the collision estimation result of the interpolation point, and find the last interpolation point estimated as collision-free for each candidate extension edge as the robot arm Active Path Search Tree New extension node, get updated robot arm Active Path Search Tree ; Passive path search tree update module, used to move the robot arm Active Path Search Tree of The new extended node is input into the kinematic model of the strongly coupled dual-arm robot closed-chain system to obtain the robot arm Corresponding joint configuration point sets, find the best joint configuration point in each set as the robot arm Passive Path Search Tree New extension node, get updated robot arm Passive path search tree ; The feasible path determination module is used to determine the updated robotic arm Active Path Search Tree And the updated robotic arm Passive path search tree Are they all connected to the corresponding target points successfully? If so, the motion planning of the strongly coupled dual-arm robot closed-chain system is completed, and the robot arm and Get a joint space motion path respectively and ,If the connection fails, it returns to the candidate extension edge determination module until a feasible path is successfully found; The safety check module is used to perform a safety check on the joint space motion path using a geometric collision checker. If no collision nodes are found, the joint space motion path is executed.

10. A computer device comprising a memory and a processor, wherein the memory stores a computer program, characterized in that: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 8 are implemented.

Images