Two-arm robot cooperative motion control method based on RRT algorithm and constraint

Abstract

The invention provides a two-arm robot cooperative motion control method based on an RRT algorithm and constraints. The method comprises the following steps: establishing a task constraint model and a tight coordination kinematics model of the double-arm robot; acquiring surrounding environment information of the double-arm robot through a depth camera; when the initial configuration information and the target configuration information obey the task constraint model and the tight coordination kinematics model, a search tree is created based on the initial configuration information, the target configuration information and the task constraint model, and a collision-free motion path of the mechanical arm of the double-arm robot is obtained based on an RRT algorithm, the search tree and the position information of the obstacle; and on the basis of the collision-free motion path, mechanical arms of the double-arm robot are controlled to operate. Thus, in the process of controlling the mechanical arm by the double-arm robot, the double-arm robot can bypass obstacles, collision is avoided, safety control over the mechanical arm can be achieved under the condition that constraint is met, and the double-arm robot can execute tasks safely.

Description

Technical Field

[0001] The present invention relates to the technical field of robot control, and more specifically, to a cooperative motion control method for a dual-arm robot based on the RRT algorithm and constraints. Background Art

[0002] With the rapid development of intelligent manufacturing and service robot technologies, robot technology has become one of the key areas of future technology. Dual-arm service robots have received extensive attention due to their application potential in complex scenarios. Among them, dual-arm robots, with their unique cooperative operation capabilities, have demonstrated great application value in fields such as industrial manufacturing, medical assistance, and home services. The coordination of dual-arm robots enables them to simulate the movements of human arms and perform fine and complex tasks such as assembly, welding, and handling. This ability not only significantly expands the application scope of robots but also provides an efficient and flexible solution for multi-field task execution. In the cooperative operation tasks of dual-arm robots, some tasks are usually subject to strict kinematic constraints, and each constraint forms a low-dimensional manifold embedded in a high-dimensional space, which poses a great challenge to traditional motion planning technologies. For example, when a dual-arm robot performs tasks such as carrying boxes, cases, or trays, the robot needs to keep the directions of the two manipulators fixed. During the process of manipulator control, it is easy to cause the manipulators to collide with obstacles, which is not conducive to the safe execution of tasks by the robot. Summary of the Invention

[0003] In view of this, the purpose of the embodiments of the present application is to provide a cooperative motion control method for a dual-arm robot based on the RRT algorithm and constraints, which can improve the problem that the robot is not conducive to safe task execution due to collisions.

[0004] To achieve the above technical purpose, the technical solution adopted by the present application is as follows:

[0005] The embodiments of the present application provide a cooperative motion control method for a dual-arm robot based on the RRT algorithm and constraints, and the method includes:

[0006] Establish a task constraint model and a tight coordination kinematic model for the dual-arm robot;

[0007] Obtain the surrounding environment information of the dual-arm robot through a depth camera, where the surrounding environment information includes the position information of obstacles;

[0008] Obtain the initial configuration information and the target configuration information of the dual-arm robot, and check whether the initial configuration information and the target configuration information comply with the task constraint model and the tight coordination kinematic model;

[0009] When the initial configuration information and the target configuration information comply with the task constraint model and the tightly coordinated kinematic model, a search tree is created based on the initial configuration information, the target configuration information, and the task constraint model, and a collision-free motion path of the manipulator of the dual-arm robot is obtained based on the RRT algorithm, the search tree, and the position information of the obstacles;

[0010] Based on the collision-free motion path, control the operation of the manipulator of the dual-arm robot.

[0011] In some alternative embodiments, establishing the task constraint model and the tightly coordinated kinematic model of the dual-arm robot includes:

[0012] Based on the general constraint model C, establish the task constraint model C1 of the dual-arm robot;

[0013] The general constraint model x, y, and z are respectively the translational positions of the end of the manipulator of the dual-arm robot in the Cartesian space, and α, β, and γ are respectively the rotation angles of the manipulator around the x, y, and z directions;

[0014] The task constraint model represents restricting the rotation of the end of the manipulator around the x, y, and z directions, and the manipulator includes a main arm and a slave arm;

[0015] Based on a preset tightly coordinated task, create the tightly coordinated kinematic model C2, expressed as where respectively represent the end positions of the main arm and the slave arm of the dual-arm robot, the subscript m represents the main arm, the subscript s represents the slave arm, the superscript b represents the base coordinate system of the dual-arm robot, and T c represents the relative pose matrix between the ends of the main arm and the slave arm.

[0016] In some alternative embodiments, through a depth camera, obtain the surrounding environment information of the dual-arm robot, and the surrounding environment information includes the position information of obstacles, including:

[0017] Collect the surrounding point cloud data of the robot through the depth camera on the dual-arm robot as the surrounding environment information;

[0018] When there are obstacles in the surrounding environment information, filter and cluster the surrounding point cloud data, and generate a geometric model of the obstacles;

[0019] According to the pose relationship between the depth camera and the base of the dual-arm robot, the position of the geometric model of the obstacle is transformed from the camera coordinate system to the base coordinate system of the dual-arm robot, so as to obtain the position information of the obstacle in the base coordinate system.

[0020] In some alternative embodiments, obtaining the initial configuration information and the target configuration information of the dual-arm robot, and checking whether the initial configuration information and the target configuration information comply with the task constraint model and the tight coordination kinematic model includes:

[0021] S31, obtaining the initial configuration information and the target configuration information of the dual-arm robot, where the initial configuration information includes the initial configuration information of the main arm and the initial configuration information of the slave arm The target configuration information includes the target configuration information of the main arm and the target configuration information of the slave arm

[0022] S32, based on the configuration of the preset robotic arm joints The position of the end of the robotic arm of the dual-arm robot relative to the base of the dual-arm robot The position of the task coordinate system w relative to the base coordinate system of the dual-arm robot obtaining the desired relative pose of the end of the main arm in the robotic arm relative to the task coordinate system denoted as and converting to d c ;

[0023]

[0024] In the formula, represents the desired relative position of the end of the main arm relative to the task coordinate system, represents the desired relative rotation angle of the end of the main arm relative to the task coordinate system, represents the expression of the task coordinate system in the base coordinate system of the dual-arm robot;

[0025] S34, determining the error calculation formula of the end of the robotic arm of the dual-arm robot and the task constraint model, denoted as:

[0026]

[0027] In the formula, d c represents the expression of the position of the end of the robotic arm relative to the base coordinate system in six dimensions XYZ-RPY, and the subscript i represents the i-th row in the matrix d c ;

[0028] S35. Based on the error calculation formula, determine whether the initial configuration information and the target configuration information of the main arm comply with the task constraint model, and determine whether the initial configuration information and the target configuration information of the slave arm comply with the tight coordination kinematic model;

[0029] S36. When the initial configuration information and the target configuration information of the main arm do not comply with the task constraint model, and / or the initial configuration information and the target configuration information of the slave arm do not comply with the tight coordination kinematic model, update the initial configuration information and the target configuration information, and based on the updated initial configuration information and target configuration information, repeat step S35 until the first preset cut-off condition is met.

[0030] In some alternative embodiments, based on the initial configuration information, the target configuration information and the task constraint model, create a search tree, and based on the RRT algorithm, the search tree and the position information of the obstacle, obtain a collision-free motion path of the robotic arms of the dual-arm robot, including:

[0031] S41. Use the initial configuration information and the target configuration information as the initial node and the target node to construct a search tree T0 = {+;

[0032] S42. Randomly generate a joint configuration sampling node within the upper and lower limits of the robotic arm joints of the dual-arm robot in the joint space of the main arm of the dual-arm robot and search in the search tree for the node nearest to the node near , and expand the node in the direction from node q near to the node , where n is the degree of freedom of the robotic arm;

[0033] S43. Based on the joint configuration of the main arm in node q near generate a new configuration of the main arm denoted as:

[0034]

[0035] S44. Based on the error calculation formula, determine whether the new configuration of the main arm complies with the task constraint model, and perform a collision detection on the new configuration that complies with the task constraint model, and cache the new configuration that does not cause the main arm to collide with the obstacle and the slave arm;

[0036] ​S45. Based on the tight kinematic model, determine the desired end pose of the slave arm under the sampling point of the master arm, and based on the desired end pose, calculate the new configuration of the slave arm through inverse kinematics. And based on the new configuration of the slave arm Perform collision detection. If the slave arm does not collide with the master arm and obstacles, then use the new configuration of the master arm And the new configuration of the slave arm As a new node and add it to the search tree.

[0037] S46. If the inverse kinematics of the new configuration of the slave arm has no solution, or there is a collision in the new configuration of the slave arm, repeat steps S42 to S45 until the second preset cut-off condition is met.

[0038] S47. If the new node does not reach the target node, and the first distance between the new node and the target node is less than or equal to the second distance, then continue to expand the node in the direction of the new node towards the node Until the target node is reached, where the second distance is the distance between the previous node of the new node and the target node.

[0039] S48. If the new node reaches the target node, then use the path recorded in the search tree from the initial node to the target node as the collision-free motion path of the robotic arm.

[0040] In some alternative embodiments, between the step of obtaining the collision-free motion path of the robotic arm of the dual-arm robot and the step of controlling the operation of the robotic arm of the dual-arm robot based on the collision-free motion path, the method further includes:

[0041] Adopt a path smoothing strategy to smooth the collision-free motion path to obtain a smoothed collision-free motion path.

[0042] In some alternative embodiments, adopting a path smoothing strategy to smooth the collision-free motion path to obtain a smoothed collision-free motion path includes:

[0043] S61. Construct an empty tree and use the collision-free motion path as the initial path.

[0044] S62. Randomly select two nodes in the initial path, denoted as path0,i- and path0,j-, where i < j.

[0045] S63. Adopt a node expansion strategy to generate a candidate path that meets the preset constraint conditions between nodes path0,i- and path0,j-.

[0046] S64, if the length of the candidate path is less than the path length of the original path between nodes path0,i- and path0,j-, replace the original path between nodes path0,i- and path0,j- with the candidate path, and add the candidate path to the empty tree;

[0047] S65, repeat steps S62 to S64 until the number of iterations reaches the preset number and then stop to obtain a collision-free motion path after smoothing processing.

[0048] The invention adopting the above technical solution has the following advantages:

[0049] In the technical solution provided in this application, when the initial configuration information and the target configuration information of the dual-arm robot conform to the task constraint model and the tight coordination kinematic model of the dual-arm robot, a search tree is created based on the initial configuration information, the target configuration information and the task constraint model, and a collision-free motion path of the manipulator of the dual-arm robot is obtained based on the RRT algorithm, the search tree and the position information of the obstacles; the manipulator of the dual-arm robot is controlled based on the collision-free motion path. In this way, when the dual-arm robot controls the manipulator, it can avoid obstacles and collisions, and can achieve safe control of the manipulator while meeting the constraints, which is beneficial for the dual-arm robot to perform tasks safely. Description of the Drawings

[0050] This application can be further illustrated by the non-limiting embodiments given in the drawings. It should be understood that the following drawings only show some embodiments of this application, so it should not be regarded as a limitation of the scope. For those of ordinary skill in the art, other related drawings can also be obtained based on these drawings without creative efforts.

[0051] Figure 1 It is a schematic flowchart of the cooperative motion control method for a dual-arm robot based on the RRT algorithm and constraints provided by an embodiment of this application.

[0052] Figure 2 It is a schematic diagram of a simplified model of a dual-arm robot provided by an embodiment of this application.

[0053] Figure 3 It is a schematic diagram of node expansion provided by an embodiment of this application.

[0054] Figure 4 It is a simulation diagram when a dual-arm robot provided by an embodiment of this application performs a tight coordination task. Detailed Embodiments

[0055] The present application will be described in detail below in conjunction with the accompanying drawings and specific embodiments. It should be noted that in the drawings or the description of the specification, similar or identical parts are denoted by the same reference numerals, and the implementation manners not depicted or described in the drawings are in the forms known to those of ordinary skill in the art. In the description of the present application, the terms "first", "second", etc. are only used for distinguishing descriptions and cannot be construed as indicating or implying relative importance.

[0056] Please refer to Figures 1 to 3 , the embodiment of the present application provides a cooperative motion control method for a dual-arm robot based on the RRT (Rapidly-Exploring Random Tree) algorithm and constraints, hereinafter simply referred to as the control method. This control method can be applied to a dual-arm robot and can be executed or implemented by the dual-arm robot for each step of the method. Among them, the two robotic arms of the dual-arm robot are respectively the main arm and the slave arm, and the joints and degrees of freedom of each robotic arm can be flexibly set according to the actual situation. As an example, each robotic arm can be a humanoid robotic arm with 7 degrees of freedom, and the 7 degrees of freedom can be as follows:

[0057] (1) Shoulder rotation of the robotic arm: The shoulder rotates around the axis to realize the overall rotation of the robotic arm;

[0058] (2) Shoulder pitch: The up and down swing of the upper arm of the robotic arm to adjust the height of the robotic arm;

[0059] (3) Swing of the forearm of the robotic arm: Using the actuator (such as a motor) of the elbow joint of the robotic arm to realize the front and back swing of the forearm to expand the working range;

[0060] (4) Abduction / adduction of the forearm of the robotic arm: Allowing the forearm to move left and right in the horizontal plane to enhance the obstacle avoidance and path planning capabilities;

[0061] (5) Wrist rotation of the robotic arm: The wrist rotates around its own axis to adjust the horizontal attitude of the tool;

[0062] (6) Wrist flip: Lateral swing, the swing of the end effector in the lateral plane, used for fine-tuning the operation angle;

[0063] (7) Wrist pitch: Up and down swing, the pitch movement of the end effector in the vertical plane to achieve fine attitude adjustment.

[0064] Please refer to Figure 1 , in this embodiment, the control method may include the following steps:

[0065] S10. Establish a task constraint model and a tight coordination kinematic model for the dual-arm robot;

[0066] S20. Obtain the surrounding environment information of the dual-arm robot through a depth camera, where the surrounding environment information includes the position information of obstacles.

[0067] S30. Obtain the initial configuration information and target configuration information of the dual-arm robot, and check whether the initial configuration information and target configuration information comply with the task constraint model and the tightly coordinated kinematic model.

[0068] S40. When the initial configuration information and target configuration information comply with the task constraint model and the tightly coordinated kinematic model, create a search tree based on the initial configuration information, target configuration information, and the task constraint model, and obtain a collision-free motion path of the manipulator of the dual-arm robot based on the RRT algorithm, the search tree, and the position information of the obstacles.

[0069] S50. Control the operation of the manipulator of the dual-arm robot based on the collision-free motion path.

[0070] In the above embodiment, when the initial configuration information and target configuration information of the dual-arm robot comply with the task constraint model and the tightly coordinated kinematic model of the dual-arm robot, create a search tree based on the initial configuration information, target configuration information, and the task constraint model, and obtain a collision-free motion path of the manipulator of the dual-arm robot based on the RRT algorithm, the search tree, and the position information of the obstacles; control the operation of the manipulator of the dual-arm robot based on the collision-free motion path. In this way, during the process of controlling the manipulator, the dual-arm robot can avoid obstacles and collisions, and can achieve safe control of the manipulator while meeting the constraints, which is beneficial to the safe execution of tasks by the dual-arm robot.

[0071] The following will elaborate on each step of the control method in detail as follows:

[0072] In step S10, establishing the task constraint model and the tightly coordinated kinematic model of the dual-arm robot includes:

[0073] Based on the general constraint model C, establish the task constraint model C1 of the dual-arm robot.

[0074] The general constraint model x, y, and z are respectively the translational positions of the end of the manipulator of the dual-arm robot in the Cartesian space, and α, β, and γ are respectively the rotation angles of the manipulator around the x, y, and z directions.

[0075] The task constraint model represents restricting the rotation of the end of the manipulator around the x, y, and z directions, and the manipulator includes a main arm and a slave arm.

[0076] Based on a preset tight - coordination task, create the tight - coordination kinematic model C2, expressed as In the formula respectively represent the end positions of the main arm and the slave arm of the dual - arm robot. The subscript m represents the main arm, the subscript s represents the slave arm, and the superscript b represents the base coordinate system of the dual - arm robot. T c represents the relative pose matrix between the ends of the main arm and the slave arm, which can be referred to Figure 2 .

[0077] Understandably, in order to intuitively express the attitude constraints of the task coordinate system relative to the world coordinate system, a 6×2 - dimensional vector is used for parametric expression. The first three rows of the general constraint model C limit the translation ranges of the task coordinate system in the X, Y, and Z directions, and the last three rows limit the rotation ranges of the task coordinate system in the X, Y, and Z directions. In this way, task constraints can be intuitively specified, and at the same time, a distance function can be defined for the task constraints to measure the difference between any pose of the dual - arm robot and the constrained pose.

[0078] As an example, in the task of tightly - coordinated handling of a packaging box by a dual - arm robot, it is necessary to ensure that the objects in the packaging box do not tip over, and the placement surface of the packaging box needs to be horizontal and always facing up relative to the ground. Therefore, the base coordinate system of the base of the dual - arm robot can be selected as the task coordinate system (or called the task reference coordinate system). The task reference coordinate system can be understood as: the position coordinate system of the object carried / grasped by the robot's arm relative to the fixed object in the environment, and this coordinate system can be flexibly determined according to the actual situation. For example, if the packaging box is placed on a workbench, and the workbench is an object with a relatively fixed position, the task coordinate system can be established based on the workbench, and the position of the packaging box can be represented in this task coordinate system.

[0079] In this embodiment, the task constraint model C1 indicates that in this task, the end of the manipulator of the dual - arm robot is allowed to freely translate in the x, y, and z directions in the Cartesian space, but the rotation around the x, y, and z directions is restricted.

[0080] The "tight - coordination" in the "preset tight - coordination task" and the "tight - coordination kinematic model" means that during the movement of the dual - arm robot, two manipulators need to cooperate to complete the task together, and the relative pose between the ends of the two arms remains unchanged during the movement to form a closed kinematic chain.

[0081] The preset tight - coordination task can be flexibly set according to the actual situation. For example, the aforementioned task of handling a packaging box is a tight - coordination task.

[0082] For tightly coordinated tasks, by introducing the main-arm - sub-arm relationship constraint, a kinematic model C2 of the two arms of the dual-arm robot can be established to achieve the description of the coordinated movement between the two arms under complex tasks. The tightly coordinated kinematic model C2 is also a constraint model.

[0083] In step S20, the surrounding environment information of the dual-arm robot is obtained through a depth camera. The surrounding environment information includes the position information of obstacles, including:

[0084] The surrounding point cloud data of the robot is collected through the depth camera on the dual-arm robot as the surrounding environment information;

[0085] When there are obstacles in the surrounding environment information, the surrounding point cloud data is filtered and clustered, and a geometric model of the obstacles is generated;

[0086] According to the pose relationship between the depth camera and the base of the dual-arm robot, the position of the geometric model of the obstacles is transformed from the camera coordinate system to the base coordinate system of the dual-arm robot, and the position information of the obstacles in the base coordinate system is obtained.

[0087] The depth camera can be a camera set on the dual-arm robot. By using the depth camera, the construction of static obstacle information in the surrounding environment of the dual-arm robot can be realized.

[0088] As an example, the point cloud data of static obstacles is collected through the depth camera, and the data is denoised, filtered and clustered. The processing method can be a conventional method to improve the effectiveness of the data; then, the processed point cloud data is used to generate a geometric model of the obstacles; according to the pose relationship between the depth camera and the robot base, the obstacle information is transformed from the camera coordinate system to the robot base coordinate system by using a homogeneous transformation matrix; the transformed obstacle model is stored based on the robot base coordinate system and is expressed in the form of a bounding box or a polygon mesh. In this way, the spatial distribution of static obstacles can be described, providing a unified reference coordinate system and reliable environmental information support for the path planning and collision detection of the manipulators of the dual-arm robot.

[0089] In step S30, the initial configuration information and the target configuration information of the dual-arm robot are obtained, and it is checked whether the initial configuration information and the target configuration information comply with the task constraint model and the tightly coordinated kinematic model, including:

[0090] S31, obtain the initial configuration information q start and the target configuration information q goal , the initial configuration information includes the initial configuration information of the main arm and the initial configuration information of the sub-arm Denoted as The target configuration information includes the main arm target configuration information and the slave arm target configuration information Denoted as

[0091] S32. Based on the configuration of the preset robotic arm joints The position of the end of the robotic arm of the dual-arm robot relative to the base of the dual-arm robot The position of the task coordinate system w relative to the base coordinate system of the dual-arm robot Obtain the desired relative pose of the end of the main arm in the robotic arm relative to the task coordinate system Denoted as And convert to d c ; w represents the task coordinate system; Represents the position of the end of the main arm relative to the base of the dual-arm robot;

[0092]

[0093] In the formula, Represents the desired relative position of the end of the main arm relative to the task coordinate system, Represents the desired relative rotation angle of the end of the main arm relative to the task coordinate system, Represents the expression of the task coordinate system in the base coordinate system of the dual-arm robot; the subscript number of m represents the element in the corresponding row and column in the matrix ; for example, m 12 Represents the element in the 1st row and 2nd column in the matrix ;

[0094] S34. Determine the error calculation formula between the end of the robotic arm of the dual-arm robot and the task constraint model, denoted as:

[0095]

[0096] In the formula, d c Represents the expression of the position of the end of the robotic arm relative to the base coordinate system in six dimensions XYZ - RPY, and the subscript i represents the i-th row in the matrix d c ; The six dimensions XYZ - RPY can be understood as including not only the spatial coordinates (x, y, z), but also α, β, γ; α, β, γ are the rotation angles of the robotic arm around the x, y, z directions respectively.

[0097] S35. Based on the error calculation formula, determine whether the main arm initial configuration information and the main arm target configuration information conform to the task constraint model, and determine whether the slave arm initial configuration information and the slave arm target configuration information conform to the tight coordination kinematic model;

[0098] S36. When the initial configuration information and the target configuration information of the main arm do not conform to the task constraint model, and / or the initial configuration information and the target configuration information of the slave arm do not conform to the tight coordination kinematic model, update the initial configuration information and the target configuration information, and based on the updated initial configuration information and target configuration information, repeat step S35 until a first preset cut-off condition is met.

[0099] In this embodiment, the task constraint model can be understood as the desired position or the desired position range of the end of the robotic arm; the error calculation formula can be used to calculate the difference between the current position and the desired position of the end of the robotic arm.

[0100] In step S35, the determination method of whether to conform to the task constraint model C1 can be:

[0101] Using the main arm joint information, calculate the error through the error calculation formula. If the error of each item is within the preset error tolerance range, it means conforming to the task constraint model C1; if any item is not within the error tolerance range, it means not conforming to the task constraint model C1.

[0102] The determination method of whether to conform to the tight coordination kinematic model C2 can be:

[0103] The main arm and the slave arm calculate the end pose matrix of the robotic arm based on the joint angles The calculation method can refer to step S32. If the end pose matrix of the main arm The end pose matrix of the slave arm Satisfies the equation of the tight coordination kinematic model C2 Then it means conforming to the tight coordination kinematic model C2, where the matrix T c Is a fixed parameter; if the C2 equation is not satisfied, it means not conforming to the tight coordination kinematic model C2.

[0104] The first preset cut-off condition can be flexibly set according to the actual situation. For example, when the updated initial configuration information and target configuration information conform to the task constraint model and conform to the tight coordination kinematic model, stop updating the initial configuration information and target configuration information.

[0105] In step S30, perform constraint detection, and then execute step S40 after passing the constraint detection. In this way, it can be ensured that the dual-arm robot can realize the corresponding task without collision.

[0106] In step S40, based on the initial configuration information, the target configuration information and the task constraint model, create a search tree, and based on the RRT algorithm, the search tree and the position information of the obstacle, obtain a collision-free motion path of the robotic arm of the dual-arm robot, including:

[0107] S41, use the initial configuration information and the target configuration information as the initial node and the target node to construct a search tree T0 = {+;

[0108] S42, randomly generate joint configuration sampling nodes within the upper and lower limits of the robotic arm joints in the joint space of the main arm of the dual-arm robot and search for the node q nearest to the node near in the search tree, and expand the node in the direction from node q near to the node , where n is the degree of freedom of the robotic arm; as an example, set the step size of node expansion to 0.05 and the task constraint error threshold ε = 0.001;

[0109] S43, based on the joint configuration near of the main arm in node q generate a new configuration of the main arm, expressed as:

[0110]

[0111] S44, based on the error calculation formula, determine whether the new configuration of the main arm satisfies the task constraint model, perform collision detection on the new configuration that satisfies the task constraint model, and cache the new configuration that does not cause the main arm to collide with obstacles and the slave arm;

[0112] S45, based on the tight coordination kinematic model, determine the desired end pose of the slave arm at the sampling point of the main arm, and based on the desired end pose, calculate the new configuration of the slave arm through inverse kinematics, and perform collision detection based on the new configuration of the slave arm. If the slave arm does not collide with the main arm and obstacles, then use the new configuration of the main arm and the new configuration new of the slave arm as the new node q and add it to the search tree;

[0113] S46, if the inverse kinematics of the new configuration of the slave arm has no solution, or there is a collision in the new configuration of the slave arm, repeat steps S42 to S45 until the second preset cut-off condition is met;

[0114] S47, if the new node has not reached the target node, and the first distance between the new node and the target node is less than or equal to the second distance, then based on the new node, towards the node Continue to expand the node in the direction of the second distance until the target node is reached, where the second distance is the distance between the previous node of the new node and the target node;

[0115] S48, if the new node reaches the target node, then use the path recorded in the search tree from the initial node to the target node as the collision-free motion path of the robotic arm.

[0116] As an example, from node q near to node (which is the target point q target ) The schematic diagram of the expansion according to a fixed step size can be as shown in Figure 3 As shown. Expand the path step by step from the starting node q near to the target point q target Generate discrete path nodes that meet the constraint conditions and insert them into the search tree. During the expansion process, approach the target point step by step at a step size of min(l,|q target -q s |) in the direction of q target -q near , that is, the main arm expands according to the step size to generate new configuration information of the main arm Then use the method of constraint projection to generate the configuration information of the main arm that meets the task constraints (that is, meets the task constraint model C1) Next, perform an environmental collision detection on the configuration information that meets the constraints. If the main arm does not collide with obstacles and does not collide with the slave arm, it means that the collision detection is passed. Then, based on the forward kinematics of the main arm Calculate the end pose of the main arm Then calculate the error homogeneous transformation matrix of the end of the main arm in the task coordinate system Convert to the expression d in XYZ-RPY c and perform error calculation. Next, use the position of the end of the main arm to calculate the expected end pose of the slave arm at the sampling point of the main arm based on the tight coordination kinematic model C2 of the dual-arm robot Then, based on the inverse kinematics of the slave arm Solve the configuration information of the slave arm Then, based on the configuration information of the slave arm Perform collision detection. It should be noted that if the inverse kinematics of the slave arm has no solution, adjust the sampling point of the main arm and recalculate the above process until the inverse kinematics of the slave arm has a solution. In addition, if the slave arm does not collide with the main arm, based on the sampling points composed of the joint information of the main arm and the slave arm, perform collision detection with environmental obstacles. After confirming no collision, the sampling configuration information of the main arm and the slave arm constitutes a new node qnew Add it to the search tree.

[0117] Forward kinematics means that given the joint variables of a dual-arm robot, the position and pose of the end of the robot's manipulator are calculated; inverse kinematics means that given the position and pose of the end of the dual-arm robot's manipulator, all joint variables at the corresponding position of the robot are calculated. Forward kinematics and inverse kinematics are conventional algorithms in the control process of the robot arm and will not be elaborated here.

[0118] In this embodiment, for the sampling points that do not meet the task constraints Using the idea of iterative optimization, project the given joint configuration q0 onto the manifold C1 that satisfies the constraints to ensure that the newly generated expanded node meets the task constraints. For the given initial joint configuration q0, calculate the deviation Δx and gradient Δq of the current distance constraint pose using the constraint error calculation method. If the current pose deviation Δx is less than the threshold ε, return the current joint configuration q0; otherwise, calculate the Jacobian matrix J of the current joint configuration and update the joint configuration according to the gradient Δq. Recalculate the new error and gradient for the updated joint configuration q0', and repeat the above process until the position deviation meets the threshold condition, and return the joint configuration configuration information that meets the task constraints.

[0119] Specifically, for the sampling points that do not meet the task constraints First, calculate the pose deviation between the current pose and the desired pose (i.e., the task constraint model C1):

[0120]

[0121] Next, calculate the Jacobian matrix J of the lower main arm, and use the Jacobian pseudoinverse to calculate the gradient Δq = J T (JJ T ) -1 Δx; update the joint configuration according to the gradient, q new = q current -Δq, and repeat the iteration until the pose deviation under the updated joint angle satisfies ||Δx|| ≤ ε. If the number of iterations exceeds the set upper limit, resample the joint angles for constraint iteration. For the sampling points of the main arm that meet the constraints perform environmental collision detection. If the lower main arm collides with the slave arm or environmental obstacles at this joint angle, discard this point and resample the node If there is no collision, record the sampling configuration information of the main arm and continue the calculation. Then perform the sampling calculation of the slave arm, and according to the sampling configuration information of the main arm perform the forward kinematics of the main arm calculate the end pose of the main arm Based on this pose, calculate the end pose of the slave arm in combination with the dual-arm tight constraint kinematics model C2 Use inverse kinematics of the slave arm Solve for the slave arm joint angles If there is no solution to the inverse kinematics of the slave arm, adjust the master arm sampling points and recalculate. Perform collision detection on the calculated slave arm joint angles to verify whether a collision occurs between the slave arm and the master arm and environmental obstacles. If a collision occurs, readjust the master arm sampling configuration information; if there is no collision, continue to the next step. Perform collision detection on the combined joint configuration information of the master arm and the slave arm as a whole. If the collision detection is passed, use the master arm and slave arm sampling configuration information as the new node q new Add it to the constructed rapidly-exploring random tree and add an edge to connect to the nearest point q near 。If the newly generated node q new Reaches the target node q goal Or the newly generated node q new Is at a distance greater than a second distance from the target node q goal , then end this node expansion; otherwise, use q new As the new q near Continue to expand in the direction of Until the newly generated node is an invalid node, then end this node expansion process and return the last generated node as q new 。Among them, the second distance refers to the distance between the previous node of the newly generated node q new And the target node q goal . If the node q new Has not been expanded to the sampling point q corresponding to the end configuration information goal Nearby, then repeat the above process until reaching the node q goal , thereby completing the path search. When the path search is completed, return the collision-free path path0 of the dual-arm robot from the initial node to the target node or return a failure message due to timeout of the calculation time.

[0122] As an optional implementation manner, between obtaining the collision-free motion path of the manipulator of the dual-arm robot in step S40 and controlling the operation of the manipulator of the dual-arm robot based on the collision-free motion path in step S50, the method further includes:

[0123] S60, adopt a path smoothing strategy to smooth the collision-free motion path to obtain a smoothed collision-free motion path.

[0124] In step S60, adopting a path smoothing strategy to smooth the collision-free motion path to obtain a smoothed collision-free motion path includes:

[0125] S61, construct an empty tree and use the collision-free motion path as the initial path;

[0126] S62, randomly select two nodes in the initial path, denoted as path0,i- and path0,j-, where i < j;

[0127] S63, adopt a node expansion strategy to generate a candidate path that meets the preset constraint conditions between nodes path0,i- and path0,j-;

[0128] S64, if the length of the candidate path is less than the path length of the original path between nodes path0,i- and path0,j-, then replace the original path between nodes path0,i- and path0,j- with the candidate path, and add the candidate path to the empty tree;

[0129] S65, repeat steps S62 to S64 until the number of iterations reaches the preset number of times and then stop to obtain a collision-free motion path after smoothing processing.

[0130] The node expansion strategy can be to directly and smoothly connect nodes path0,i- and path0,j- to form a new path as the candidate path. There may be other nodes on the original path of nodes path0,i- and path0,j-, so the original path may be longer; while the candidate path is a direct connection between two nodes, so the path may be shorter. If the candidate path does not collide with obstacles, it means that the candidate path meets the preset constraint conditions and the length of the path is less than the path length of the original path between nodes path0,i- and path0,j-, then the candidate path can be added to the empty tree and replace the original path. The empty tree can be used to cache and record the optimized path segments. By repeating steps S62 to S64, the initial path can be iteratively optimized to obtain a smooth and shorter collision-free motion path, thereby removing the redundant parts in the initial path and reducing the path length. Among them, the number of repetitions / iterations can be flexibly set according to the actual situation and will not be specifically limited here.

[0131] The inventor obtained through simulation the Figure 4 schematic diagrams of four different poses of the dual-arm robot during the process of moving a sheet.

[0132] In this embodiment, based on the traditional RRT motion planning algorithm, the control method introduces the task constraint model and the tight coordination kinematic model of the dual-arm robot. By projecting the randomly sampled points onto the task constraint manifold space of the task constraint model, it ensures that the sampled points meet the constraint conditions, and combines the master and slave arms for collaborative planning to ensure that the planned path nodes meet the task constraints, realizing the autonomous collision-free motion of the dual-arm robot under task constraint conditions. For complex task requirements, this method establishes a task constraint model for task constraints and a tight coordination kinematic model between the two arms. Through the collaborative calculation of master arm sampling and slave arm constraints, it ensures that the sampled points meet the task pose constraints of the task and the coordination constraints between the master and slave arms. During the path planning process, by combining the constraint space projection strategy and the collision detection mechanism, the safety and feasibility of the dual-arm constraint planning are improved, and the collision risks between the master arm and the slave arm, and between the robot and environmental obstacles are avoided. In addition, the smoothness and continuity of the path are further improved through trajectory smoothing optimization. This method can not only meet the task requirements of the dual-arm robot in complex environments, but also provide a safe and reliable planning guarantee for the robot to execute complex tasks and collaborative operations.

[0133] Through the description of the above embodiments, those skilled in the art can clearly understand that this application can be implemented by hardware or by means of software plus a necessary general hardware platform. Based on this understanding, the technical solution of this application can be embodied in the form of a software product, which can be stored in a non-volatile storage medium (which can be a CD-ROM, a USB flash drive, a mobile hard disk, etc.), including several instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in various implementation scenarios of this application.

[0134] In the embodiments provided in this application, it should be understood that the disclosed dual-arm robot and method can also be implemented in other ways. The method embodiments described above are merely illustrative. For example, the flowcharts and block diagrams in the drawings show the possible architectures, functions, and operations of the methods and computer program products according to multiple embodiments of this application. In this regard, each block in the flowchart or block diagram may represent a module, a program segment, or a part of code, and the module, program segment, or part of code contains one or more executable instructions for implementing the specified logical function. It should also be noted that each block in the block diagram and / or flowchart, and the combination of blocks in the block diagram and / or flowchart, can be implemented by a dedicated hardware-based system for performing the specified functions or actions, or can be implemented by a combination of dedicated hardware and computer instructions. In addition, the functional modules in various embodiments of this application can be integrated together to form an independent part, or each module can exist separately, or two or more modules can be integrated to form an independent part.

[0135] The above are only the embodiments of the present application and are not intended to limit the protection scope of the present application. For those skilled in the art, various modifications and changes can be made to the present application. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included within the protection scope of the present application.

Claims

1. A dual-arm robot cooperative motion control method based on RRT algorithm and constraints, characterized in that: The method comprises: Establish the task constraint model and tight coordination kinematics model of the dual-arm robot; Acquiring the surrounding environment information of the dual-arm robot through a depth camera, wherein the surrounding environment information includes the location information of obstacles; Acquiring initial configuration information and target configuration information of the dual-arm robot, and checking whether the initial configuration information and the target configuration information comply with the task constraint model and the tight coordination kinematics model; When the initial configuration information and the target configuration information obey the task constraint model and the tightly coordinated kinematic model, a search tree is created based on the initial configuration information, the target configuration information and the task constraint model, and a collision-free motion path of the manipulator arm of the dual-arm robot is obtained based on an RRT algorithm, the search tree and the position information of the obstacle; Based on the collision-free motion path, the operation of the mechanical arm of the dual-arm robot is controlled.

2. The method according to claim 1, characterized in that Establish the task constraint model and tight coordination kinematics model of the dual-arm robot, including: Based on the general constraint model C, a task constraint model C1 of the dual-arm robot is established; The general constraint model x, y, z are the translation positions of the ends of the mechanical arms of the dual-arm robot in Cartesian space, respectively, and α, β, γ are the rotation angles of the mechanical arms around the x, y, z directions, respectively; The task constraint model Indicates limiting the rotation of the end of the robotic arm around the x, y, and z directions, and the robotic arm includes a master arm and a slave arm; Based on the preset tight coordination task, the tight coordination kinematic model C2 is created, which is expressed as In the formula denote the end positions of the master arm and the slave arm of the dual-arm robot, respectively; the subscript m denotes the master arm, the subscript s denotes the slave arm, the superscript b denotes the base coordinate system of the dual-arm robot, and T c Represents the relative pose matrix between the master arm and the end of the slave arm.

3. The method according to claim 1, characterized in that: The surrounding environment information of the dual-arm robot is obtained through a depth camera, and the surrounding environment information includes the location information of obstacles, including: By using the depth camera on the dual-arm robot, collecting the surrounding point cloud data of the robot as the surrounding environment information; When there are obstacles in the surrounding environment information, filtering and clustering the surrounding point cloud data, and generating a geometric model of the obstacle; According to the posture relationship between the depth camera and the base of the dual-arm robot, the position of the geometric model of the obstacle is converted from the camera coordinate system to the base coordinate system of the dual-arm robot to obtain the position information of the obstacle in the base coordinate system.

4. The method according to claim 1, characterized in that Acquiring initial configuration information and target configuration information of the dual-arm robot, and checking whether the initial configuration information and the target configuration information obey the task constraint model and the tight coordination kinematics model, including: S31, obtaining initial configuration information and target configuration information of the dual-arm robot, wherein the initial configuration information includes the initial configuration information of the main arm And the initial configuration information of the slave arm The target configuration information includes main arm target configuration information And the target configuration information of the slave arm S32, based on the configuration of the preset robot joints The position of the end of the dual-arm robot's mechanical arm relative to the base of the dual-arm robot The position of the task coordinate system w relative to the base coordinate system of the dual-arm robot Obtain the desired relative position of the end of the main arm in the manipulator relative to the task coordinate system Expressed as and will Convert to d c ; In the formula, represents the desired relative position of the main arm end relative to the task coordinate system, represents the expected relative rotation angle of the main arm end relative to the task coordinate system, represents the expression of the task coordinate system in the base coordinate system of the dual-arm robot; S34, determining the error calculation formula between the end of the mechanical arm of the dual-arm robot and the task constraint model, expressed as: Where, d c It represents the position of the end of the manipulator relative to the base coordinate system in the six-dimensional XYZ-RPY. The subscript i represents the position of the end of the manipulator relative to the base coordinate system in the matrix d c The i-th row in ; S35, based on the error calculation formula, judging whether the master arm initial configuration information and the master arm target configuration information obey the task constraint model, and judging whether the slave arm initial configuration information and the slave arm target configuration information obey the tight coordination kinematics model; S36, when the main arm initial configuration information and the main arm target configuration information do not obey the task constraint model, and / or the slave arm initial configuration information and the slave arm target configuration information do not obey the tightly coordinated kinematic model, update the initial configuration information and the target configuration information, and based on the updated initial configuration information and the target configuration information, repeat step S35 until the first preset cutoff condition is met.

5. The method according to claim 1, characterized in that Based on the initial configuration information, the target configuration information and the task constraint model, a search tree is created, and based on the RRT algorithm, the search tree and the position information of the obstacle, a collision-free motion path of the manipulator arm of the dual-arm robot is obtained, including: S41, using the initial configuration information and the target configuration information as initial nodes, respectively and the target node Construct search tree T0 = {}; S42, randomly generating joint configuration sampling nodes within the upper and lower limit ranges of the mechanical arm joints of the dual-arm robot in the joint space of the main arm of the dual-arm robot And in the search tree, search for the node The nearest node q near , and from node q near To Node Node expansion is performed in the direction of , where n is the degree of freedom of the robot arm; S43, based on node q near Joint configuration of the middle main arm Generate a new configuration of the main arm It is expressed as: S44, based on the error calculation formula, determine the new configuration of the main arm whether the task constraint model is satisfied, and performing collision detection on a new configuration satisfying the task constraint model, and caching a new configuration that makes the main arm not collide with obstacles and the slave arm; S45, based on the tightly coordinated kinematic model, determining the desired end position of the slave arm at the sampling point of the master arm, and based on the desired end position, calculating the new configuration of the slave arm by inverse kinematics. And based on the new configuration of the slave arm Perform collision detection. If the slave arm does not collide with the master arm or the obstacle, the master arm is reconfigured. And the new configuration of the slave arm As a new node and added to the search tree; S46, if there is no solution for the inverse kinematics of the new configuration of the slave arm, or there is a collision in the new configuration of the slave arm, repeat steps S42 to S45 until a second preset cutoff condition is met; S47, if the new node has not reached the target node, and the first distance between the new node and the target node is less than or equal to the second distance, then based on the new node towards the node Continue to expand nodes in the direction until reaching the target node, wherein the second distance is the distance between the previous node of the new node and the target node; S48, if the new node reaches the target node, the path from the initial node to the target node recorded in the search tree is used as the collision-free motion path of the robot arm.

6. The method according to claim 1, characterized in that Between the step of obtaining the collision-free motion path of the mechanical arm of the dual-arm robot and the step of controlling the operation of the mechanical arm of the dual-arm robot based on the collision-free motion path, the method further includes: A path smoothing strategy is adopted to smooth the collision-free motion path to obtain a smoothed collision-free motion path.

7. The method according to claim 6, characterized in that The collision-free motion path is smoothed by adopting a path smoothing strategy to obtain a smoothed collision-free motion path, including: S61, constructing an empty tree, and using the collision-free motion path as an initial path; S62, randomly selecting two nodes in the initial path, denoted as path0[i] and path0[j], where i<j; S63, adopting the node expansion strategy to generate a candidate path between nodes path0[i] and path0[j] that meets the preset constraint conditions; S64, if the length of the candidate path is less than the path length of the original path between nodes path0[i] and path0[j], the original path between nodes path0[i] and path0[j] is replaced with the candidate path, and the candidate path is added to the empty tree; S65, repeating steps S62 to S64 until the number of iterations reaches a preset number, thereby obtaining a smoothed collision-free motion path.

Images