Dual-arm cooperative control method and device of robot, control equipment and medium

Abstract

The application provides a dual-arm cooperative control method and device of a robot, a control equipment and a medium, and relates to the technical field of robot control. The method comprises the following steps: determining an actual relative pose according to a first end actual pose of a master arm and a second end actual pose of a slave arm; determining a compensation pose error vector of the slave arm according to a difference between an expected pose matrix of the slave arm relative to the master arm and the actual relative pose; determining a target Jacobian matrix of the slave arm relative to the master arm according to a first Jacobian matrix of the master arm and a second Jacobian matrix of the slave arm; solving a target function constructed according to a master-slave arm joint speed, the target Jacobian matrix and the compensation pose error vector to determine the master-slave arm joint speed; and calculating a next time joint angle according to a current time joint angle and the master-slave arm joint speed to cooperatively control the master arm and the slave arm. The application can improve the anti-disturbance ability and realize active cooperation of dual-arm movement.

Description

Technical Field

[0001] This application relates to the field of robot control technology, and more specifically, to a method, apparatus, control device, and medium for the coordinated control of two arms of a robot. Background Technology

[0002] Currently, the mainstream solutions for collaborative control of robot dual arms mostly adopt the absolute trajectory tracking control method under the "master-slave" architecture. This method usually refers to one arm (master arm) strictly tracking its predefined absolute trajectory in the world coordinate system, while the other arm (slave arm) follows the master arm's movement through a fixed, pre-calculated relative pose relationship.

[0003] However, when subjected to external disturbances, the position and posture error of the slave arm relative to the master arm is relatively large, making it difficult to meet the high precision requirements of the operation. Furthermore, when the two arms are in dynamic motion, the relative position and posture error of the slave arm fluctuates significantly, which can easily lead to the two arms being out of sync and causing the workpiece being transported to tilt. Summary of the Invention

[0004] The purpose of this application is to address the shortcomings of the prior art by providing a method, device, control equipment, and medium for the coordinated control of the two arms of a robot, so as to improve the anti-disturbance capability and realize the active coordination of the two arm movements.

[0005] To achieve the above objectives, the technical solutions adopted in the embodiments of this application are as follows:

[0006] In a first aspect, embodiments of this application provide a method for the cooperative control of two arms of a robot, the method comprising:

[0007] Construct the desired pose matrix of the slave arm relative to the master arm of the robot, wherein the master arm and the slave arm are the two robotic arms of the robot;

[0008] Based on the actual pose of the first end of the main arm and the actual pose of the second end of the slave arm, the actual relative pose of the slave arm with respect to the main arm is determined.

[0009] The compensation pose error vector of the slave arm is determined based on the difference between the desired pose matrix and the actual relative pose.

[0010] Based on the first Jacobian matrix of the end-effector velocity and the end-effector joint velocity, and the second Jacobian matrix of the end-effector velocity and the end-effector joint velocity, determine the target Jacobian matrix of the velocity vector of the end-effector relative to the end-effector and the joint velocity of the end-effector and end-effector.

[0011] Solve the objective function constructed based on the master-slave joint velocity, the target Jacobian matrix, and the compensated pose error vector to determine the master-slave joint velocity;

[0012] Calculate the joint angles of the master arm and slave arm at the next moment based on the current joint angles of the master arm and slave arm and the joint velocity of the master and slave arms.

[0013] The master arm and the slave arm are controlled in a coordinated manner based on their joint angles at the next moment.

[0014] Optionally, determining the actual relative pose of the slave arm with respect to the master arm based on the actual pose of the first end of the master arm and the actual pose of the second end of the slave arm includes:

[0015] Based on the robot's real-time joint angles, determine the actual pose of the first end of the main arm and the actual pose of the second end of the slave arm;

[0016] The actual relative pose is determined based on the actual pose of the first end and the actual pose of the second end.

[0017] Optionally, determining the compensated pose error vector of the slave arm based on the difference between the desired pose matrix and the actual relative pose includes:

[0018] Calculate the deviation matrix based on the desired pose matrix and the actual relative pose;

[0019] The deviation matrix is ​​linearized to determine the linear error vector;

[0020] The linear error vector is compensated by using the logarithmic derivative of the deviation matrix to determine the compensated pose error vector.

[0021] Optionally, the linearization of the deviation matrix to determine the linear error vector includes:

[0022] The deviation matrix is ​​decomposed to determine the rotational and translational deviations;

[0023] Based on the deviation of the rotating part, determine the rotation angle and the unit vector of the rotation axis;

[0024] Calculate the rotation error vector based on the rotation angle and the unit vector of the rotation axis;

[0025] The translation error vector is calculated based on the rotation error vector, the translation deviation, and the rotation angle.

[0026] The linear error vector is determined based on the rotation error vector and the translation error vector.

[0027] Optionally, before using the logarithmic derivative of the deviation matrix to compensate the linear error vector and determine the compensated pose error vector, the method further includes:

[0028] Calculate the rotation error compensation submatrix based on the rotation angle, the unit vector of the rotation axis, and the rotation error vector;

[0029] Calculate the translation error compensation submatrix based on the rotation angle and the rotation error vector;

[0030] The derivative of the logarithmic mapping is calculated based on the rotation error compensation submatrix and the translation error compensation submatrix.

[0031] Optionally, determining the target Jacobian matrix of the velocity vector of the slave arm relative to the master arm and the joint velocity of the master and slave arms based on the first Jacobian matrix of the master arm end-effector velocity and the master arm joint velocity, and the second Jacobian matrix of the slave arm end-effector velocity and the slave arm joint velocity, includes:

[0032] Based on the actual relative pose, determine the accompanying transformation matrix;

[0033] The relative Jacobian matrix of the slave arm relative to the master arm is determined based on the first Jacobian matrix of the end-effector velocity and the master arm joint velocity, the second Jacobian matrix of the slave arm end-effector velocity and the slave arm joint velocity, and the adjoint transformation matrix.

[0034] The target Jacobian matrix is ​​calculated based on the relative Jacobian matrix and the logarithmic map derivative.

[0035] Optionally, the step of determining the joint velocity of the master-slave arm based on the objective function constructed from the joint velocity of the master and slave arms, the target Jacobian matrix, and the compensated pose error vector includes:

[0036] Based on the master-slave joint velocity and the target Jacobian matrix, determine the error rate of change parameter of the master-slave joint velocity;

[0037] Based on the compensated pose error vector and the preset task gain, determine the error convergence change parameter;

[0038] The objective function is constructed based on the position error weight and attitude error weight of the slave arm relative to the master arm, the error rate of change parameter, and the error convergence change parameter;

[0039] The objective function is solved based on the joint velocity constraints to determine the joint velocity of the master and slave arms.

[0040] Secondly, embodiments of this application also provide a dual-arm collaborative control device for a robot, the device comprising:

[0041] The desired pose construction module is used to construct the desired pose matrix of the robot's slave arm relative to the master arm, wherein the master arm and the slave arm are the two robotic arms of the robot.

[0042] The actual pose determination module is used to determine the actual relative pose of the slave arm relative to the main arm based on the actual pose of the first end of the main arm and the actual pose of the second end of the slave arm.

[0043] The compensation error calculation module is used to determine the compensation pose error vector of the slave arm based on the difference between the expected pose matrix and the actual relative pose.

[0044] The Jacobian matrix determination module is used to determine the target Jacobian matrix of the velocity vector of the slave arm relative to the master arm and the joint velocity of the master and slave arms based on the first Jacobian matrix of the end-effector velocity and the end-effector joint velocity of the master arm and the second Jacobian matrix of the end-effector velocity and the end-effector joint velocity of the slave arm.

[0045] The joint velocity calculation module is used to solve the objective function constructed based on the master-slave joint velocity, the target Jacobian matrix and the compensated pose error vector, and to determine the master-slave joint velocity.

[0046] The joint angle calculation module is used to calculate the joint angle of the master arm and the slave arm at the next moment based on the current joint angle of the master arm and the slave arm and the joint velocity of the master and slave arms.

[0047] The dual-arm collaborative control module is used to collaboratively control the main arm and the slave arm based on the joint angle of the main arm and the slave arm at the next moment.

[0048] Optionally, the actual pose determination module is specifically used to determine the first end-effector actual pose of the main arm and the second end-effector actual pose of the slave arm based on the real-time joint angles of the robot; and to determine the actual relative pose based on the first end-effector actual pose and the second end-effector actual pose.

[0049] Optionally, the compensation error calculation module is specifically used to calculate a deviation matrix based on the desired pose matrix and the actual relative pose; linearize the deviation matrix to determine a linear error vector; and use the logarithmic derivative of the deviation matrix to compensate the linear error vector to determine the compensated pose error vector.

[0050] Optionally, the compensation error calculation module is further configured to decompose the deviation matrix to determine the rotational deviation and the translational deviation; determine the rotation angle and the unit vector of the rotation axis based on the rotational deviation; calculate the rotational error vector based on the rotation angle and the unit vector of the rotation axis; calculate the translational error vector based on the rotational error vector, the translational deviation, and the rotation angle; and determine the linear error vector based on the rotational error vector and the translational error vector.

[0051] Optionally, the device further includes:

[0052] The logarithmic map derivative calculation module is used to calculate the rotation error compensation sub-matrix based on the rotation angle, the rotation axis unit vector, and the rotation error vector; calculate the translation error compensation sub-matrix based on the rotation angle and the rotation error vector; and calculate the logarithmic map derivative based on the rotation error compensation sub-matrix and the translation error compensation sub-matrix.

[0053] Optionally, the Jacobian matrix determination module is specifically used to determine the accompanying transformation matrix based on the actual relative pose; determine the relative Jacobian matrix of the slave arm relative to the master arm based on the first Jacobian matrix of the end-effector velocity and the master arm joint velocity, the second Jacobian matrix of the slave arm end-effector velocity and the slave arm joint velocity, and the accompanying transformation matrix; and calculate the target Jacobian matrix based on the relative Jacobian matrix and the logarithmic mapping derivative.

[0054] Optionally, the joint velocity solving module is specifically used to: determine the error rate of change parameter of the master-slave joint joint velocity based on the master-slave joint joint velocity and the target Jacobian matrix; determine the error convergence change parameter based on the compensated pose error vector and the preset task gain; construct the objective function based on the position error weight and attitude error weight of the slave arm relative to the master arm, the error rate of change parameter, and the error convergence change parameter; and solve the objective function based on the joint velocity constraints to determine the master-slave joint joint velocity.

[0055] Thirdly, embodiments of this application also provide a robot control device, including: a processor, a storage medium, and a bus. The storage medium stores program instructions executable by the processor. When the robot control device is running, the processor communicates with the storage medium via the bus, and the processor executes the program instructions to perform the steps of the robot dual-arm cooperative control method as described in any of the first aspects.

[0056] Fourthly, embodiments of this application also provide a computer-readable storage medium storing a computer program, which, when executed by a processor, performs the steps of the dual-arm cooperative control method for a robot as described in any of the first aspects.

[0057] The beneficial effects of this application are:

[0058] The robot dual-arm cooperative control method, device, control equipment, and medium provided in this application are based on a closed-loop feedback mechanism that directly constrains the desired pose matrix of the slave arm relative to the master arm and the actual relative pose. This completely eliminates the dependence on the absolute positioning accuracy of the world coordinate system. The model error, tracking error, and external disturbances of the dual arms are all suppressed in real time by compensating for the pose error vector, which greatly improves the cooperative accuracy and anti-interference capability, keeps the relative error of the dual arms within the performance range, and significantly shortens the anti-disturbance response time. Attached Figure Description

[0059] To more clearly illustrate the technical solutions of the embodiments of this application, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of this application and should not be regarded as a limitation of the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

[0060] Figure 1 Flowchart of the dual-arm cooperative control method provided in the embodiments of this application Figure 1 ;

[0061] Figure 2 Flowchart of the dual-arm cooperative control method provided in the embodiments of this application Figure 2 ;

[0062] Figure 3 Flowchart of the dual-arm cooperative control method provided in the embodiments of this application Figure 3 ;

[0063] Figure 4 Flowchart of the dual-arm cooperative control method provided in the embodiments of this application Figure 4 ;

[0064] Figure 5 Flowchart of the dual-arm cooperative control method provided in the embodiments of this application Figure 5 ;

[0065] Figure 6 Flowchart of the dual-arm cooperative control method provided in the embodiments of this application Figure 6 ;

[0066] Figure 7A flowchart illustrating the dual-arm collaborative control method provided in an embodiment of this application;

[0067] Figure 8 This is a schematic diagram of the dual-arm collaborative control device provided in the embodiments of this application;

[0068] Figure 9 A schematic diagram of the control device provided in an embodiment of this application. Detailed Implementation

[0069] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are some embodiments of this application, but not all embodiments.

[0070] Therefore, the following detailed description of the embodiments of this application provided in the accompanying drawings is not intended to limit the scope of the claimed application, but merely to illustrate selected embodiments of the application. All other embodiments obtained by those skilled in the art based on the embodiments of this application without inventive effort are within the scope of protection of this application.

[0071] Furthermore, the terms "first," "second," etc., used in the specification, claims, and accompanying drawings of this application are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments of this application described herein can be implemented in orders other than those illustrated or described herein. Additionally, the terms "comprising" and "having," and any variations thereof, are intended to cover a non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0072] It should be noted that, where there is no conflict, the features in the embodiments of this application can be combined with each other.

[0073] The robot dual-arm cooperative control method provided in this application can be a controller installed in the robot or a computer device independent of the robot. This embodiment does not limit the application to either.

[0074] Figure 1 Flowchart of the dual-arm cooperative control method provided in the embodiments of this application Figure 1 ,like Figure 1 As shown, the method may include:

[0075] S101. Construct the desired pose matrix of the robot's slave arm relative to the master arm, where the master arm and slave arm are the robot's two robotic arms.

[0076] In this embodiment, for a robot with two robotic arms, one robotic arm is designated as the master robotic arm (or main arm), and the other robotic arm is designated as the slave robotic arm (or slave arm). The master arm reference coordinate system (M) is a coordinate system established with the center of the robot's master arm end effector, and the slave arm target coordinate system (S) is a coordinate system established with the center of the robot's slave arm end effector. The world coordinate system (W) serves as the global reference. The end effector is the hand of the robotic arm, and the center of the end effector is the palm of the hand.

[0077] The desired pose matrix of the slave arm relative to the master arm describes the ideal relative pose of the slave arm relative to the master arm. ,in, SE (3) Represents a special eutectic group in 3D, which is a Lie group that describes the set of all possible rotational and translational motions in 3D space. It can be represented by a 3D special orthogonal group. SO (3) and three-dimensional real space Composition, three-dimensional special orthogonal group SO (3) Describes all possible three-dimensional rotations in three-dimensional real space. It describes all possible three-dimensional translations.

[0078] Desired pose matrix The mathematical form follows SE (3) Pose matrix structure, desired pose matrix It can be represented as:

[0079]

[0080] in, This represents the ideal orientation rotation matrix of the slave arm relative to the master arm, satisfying... and For example, in a collaborative assembly task, the following needs to be met: To indicate that the ends of both arms are in the same position. 3 A 3-dimensional identity matrix.

[0081] This represents the translation vector from the arm to the ideal position relative to the main arm, for example, in a collaborative grasping task. The workpiece size needs to be matched to ensure that both arms work together to contact the workpiece.

[0082] S102. Determine the actual relative pose of the slave arm with respect to the master arm based on the actual pose of the first end of the master arm and the actual pose of the second end of the slave arm.

[0083] In this embodiment, during the movement of the robot's two arms, the actual pose of the end effector of the main arm in the world coordinate system is obtained. and the actual pose of the second end of the arm in the world coordinate system. .

[0084] Among them, the actual pose of the first end The mathematical form follows the SE(3) pose matrix structure, and the actual pose of the first end effector is... It can be represented as:

[0085]

[0086] in, This represents the actual attitude rotation matrix of the end effector in the world coordinate system. This indicates the actual position of the end of the main arm in the world coordinate system.

[0087] Second end actual pose Compared with the actual pose of the first end The form is consistent, and the actual pose of the second end is consistent. It can be represented as:

[0088]

[0089] in, This represents the actual attitude rotation matrix from the end of the arm in the world coordinate system. This indicates the actual position of the end of the arm in the world coordinate system.

[0090] In some embodiments, in order to improve the accuracy of obtaining the actual pose of the first end of the master arm and the actual pose of the second end of the slave arm, a visual sensor can be used to collect the end marker points of the master arm and the slave arm, and the forward kinematics solution results can be calibrated to ensure that the pose error meets the task requirements.

[0091] Determine the actual position of the first end of the main arm. and the actual pose of the second end of the arm Afterwards, according to SE (3) Derive the actual relative pose of the slave arm (S) with respect to the master arm (M) by performing inverse and composite operations of the transformation. .

[0092] In some embodiments, the process of determining the actual relative pose of the slave arm with respect to the master arm based on the actual pose of the first end of the master arm and the actual pose of the second end of the slave arm in S102 may include:

[0093] Based on the robot's real-time joint angles, determine the actual pose of the first end effector of the main arm and the actual pose of the second end effector of the slave arm; based on the actual poses of the first and second end effectors, determine the actual relative pose.

[0094] In this embodiment, through the forward kinematics and SE (3) Transformation operation to establish the pose association between the world coordinate system, the master arm coordinate system, and the slave arm coordinate system, and determine the actual relative pose of the slave arm (S) with respect to the master arm (M). .

[0095] Specifically, based on the Unified Robot Description Format (URDF) model of the robot's two arms, and combined with the real-time joint angles collected by the joint encoder, the actual pose of the end effector of the main arm in the world coordinate system is calculated using forward kinematics. and the actual pose of the second end of the arm in the world coordinate system. .

[0096] Actual pose of the first end of the main arm Perform an inverse transformation to obtain the actual pose of the end effector of the main arm in the world coordinate system relative to the main arm coordinate system. Actual position of the end effector of the main arm It can be represented as:

[0097]

[0098] The actual pose of the end effector of the main arm in the world coordinate system relative to the main arm coordinate system. and the actual pose of the second end effector in the arm coordinate system relative to the world coordinate system. Perform a composite transformation to obtain the actual relative pose of the slave arm with respect to the master arm. Actual relative pose It can be represented as:

[0099]

[0100] S103. Based on the difference between the desired pose matrix and the actual relative pose, determine the compensation pose error vector of the slave arm.

[0101] In this embodiment, due to SE (3) Since it is a nonlinear Lie group, the pose deviation needs to be linearized into... se (3) Velocity vector, and compensate for nonlinear errors to ensure the accuracy of error quantization. se (3) is a Lie algebra.

[0102] Specifically, calculate the actual relative pose. and desired pose matrix The difference is addressed by invoking the logarithmic mapping from Lie groups to Lie algebras. The nonlinear difference matrix is ​​transformed into a six-dimensional linear velocity vector. .

[0103] When the nonlinear difference matrix deviates significantly from the identity matrix, resulting in a large error, the linearized velocity vector will exhibit bias, necessitating correction. This correction involves calculating the logarithmic derivative of the nonlinear difference matrix and using this derivative to compensate for the velocity vector, thus obtaining the compensated pose error vector for the slave arm. .

[0104] S104. Based on the first Jacobian matrix of the end-effector velocity and the end-effector joint velocity, and the second Jacobian matrix of the end-effector velocity and the end-effector joint velocity, determine the target Jacobian matrix of the velocity vector of the end-effector relative to the end-effector and the joint velocity of the end-effector and end-effector.

[0105] In this embodiment, the first Jacobian matrix Used to indicate the velocity vector of the end of the main arm relative to the world coordinate system. (Expressed in the main arm coordinate system, belonging to...) Space) and main arm joint speed The relationship can be specifically represented as:

[0106]

[0107] in, , The linear velocity at the end of the main arm. The angular velocity at the end of the main arm is expressed in the main arm coordinate system M.

[0108] Similarly, the second Jacobian matrix Used to indicate the velocity vector relative to the world coordinate system from the end of the arm. (Expressed in the arm coordinate system, belonging to...) Space) and velocity from the arm joint The relationship can be specifically represented as:

[0109]

[0110] in, , For the linear velocity from the end of the arm, The angular velocity at the end of the arm is expressed in the arm coordinate system S.

[0111] First Jacobian Matrix Second Jacobian matrix The contribution of a single joint motion to the end effector motion can be directly reflected by calculating the derivative of the forward kinematics of the robot.

[0112] The velocity vector of the main arm expressed in the main arm coordinate system. Convert the velocity vector of the master arm to the slave arm coordinate system. Based on the velocity vector of the arm relative to the world coordinate system and the velocity vector of the main arm relative to the world coordinate system (Using the same coordinate system as the slave arm), calculate the velocity vector of the slave arm relative to the master arm. Substitute into the first Jacobian matrix Second Jacobian matrix and the speed of the main arm joint and from arm joint speed Integration into joint velocity (belong (in space), determine the velocity vector representing the arm relative to the main arm. With joint velocity Target Jacobian Matrix .

[0113] S105. Solve the objective function constructed based on the joint velocities of the master and slave arms, the target Jacobian matrix, and the compensation pose error vector to determine the joint velocities of the master and slave arms.

[0114] In this embodiment, based on the joint velocity of the master and slave arms Target Jacobian matrix and compensated pose error vector Construct the objective function for the joint velocity of the master and slave arms. And the target Jacobian matrix The rate of change of error caused by joint velocity, and the compensation pose error vector. The target rate of change used to indicate error convergence is used to solve the objective function with the minimum error rate of change as the optimization objective. This yields the master-slave joint velocity, where the error rate of change is approximately equal to the target rate of change. .

[0115] S106. Calculate the joint angles of the master arm and slave arm at the next moment based on the current joint angles of the master arm and slave arm and the joint velocity of the master arm and slave arm.

[0116] In this embodiment, the update of the robot's joint state follows the discrete-time kinematics integral rule, that is, the joint angle at the next moment = the joint angle at the current moment + the joint velocity. The control cycle is updated according to this rule, specifically for both the master arm and slave arm.

[0117] The joint angles of the main arm will be updated in the next instant as follows:

[0118]

[0119] The joint angle of the arm is updated from the next moment:

[0120]

[0121] in, They are respectively k The joint angle vectors of the master arm and slave arm at any given time. They are respectively k The joint angle vectors of the master arm and slave arm at time +1. To control the cycle, which is the time interval between updates to the states of the two arm joints.

[0122] S107. Based on the joint angles of the master arm and slave arm at the next moment, perform coordinated control of the master arm and slave arm.

[0123] In this embodiment, after determining the joint angles of the master arm and slave arm at the next moment, the joint angles of the master arm and slave arm are updated respectively to achieve coordinated control of the robot's master arm and slave arm.

[0124] Due to the joint velocity of the master arm and the joint velocity of the slave arm The solutions from the same optimization problem are naturally synchronized in the time dimension. At the same time, the integral updates use the same control cycle to ensure that the joint angle updates of the master arm and slave arm are completely consistent. This avoids the exaggeration of relative pose deviations caused by "master arm faster, slave arm slower" or "slave arm faster, master arm slower", and ultimately achieves the stability of the coordinated movement of the two arms.

[0125] After updating the joint angles, return to S102 to reacquire the actual pose and enter the next round of closed-loop control for error calculation, optimization solution, and state update until the task error converges to the preset threshold. Then, it is determined that the dual-arm collaborative task is completed. The preset threshold can be that the position error is less than or equal to the preset position error threshold, and the attitude error is less than or equal to the preset attitude error threshold. The preset position error threshold can be 0.005m, and the preset attitude error threshold can be 0.01rad.

[0126] The robot dual-arm cooperative control method provided in the above embodiments, based on a closed-loop feedback mechanism of direct relative constraint between the desired pose matrix of the slave arm relative to the master arm and the actual relative pose, completely eliminates the dependence on the absolute positioning accuracy of the world coordinate system. The model error, tracking error and external disturbance of the dual arm bodies are suppressed in real time by compensating the pose error vector, which greatly improves the cooperative accuracy and anti-interference ability, so that the relative error of the dual arms is controlled within the performance range and the anti-disturbance response time is greatly shortened.

[0127] In one possible implementation, Figure 2 Flowchart of the dual-arm cooperative control method provided in the embodiments of this application Figure 2 ,like Figure 2 As shown, the process of determining the compensated pose error vector of the slave arm based on the difference between the desired pose matrix and the actual relative pose in S103 can include:

[0128] S201. Calculate the deviation matrix based on the desired pose matrix and the actual relative pose.

[0129] In this embodiment, the deviation matrix of the arm relative to the desired pose matrix is ​​defined. This is used to quantify the difference between the actual relative pose and the desired pose matrix. The calculation logic is as follows: SE (3) Inverse transformation and composition operation:

[0130]

[0131] in, This is the inverse transformation of the desired pose matrix, i.e.:

[0132]

[0133] Deviation matrix It contains all the deviation information from the arm in the translation and rotation dimensions, but it needs to be linearized before it can be used for control calculations.

[0134] S202. Linearize the deviation matrix to determine the linear error vector.

[0135] In this embodiment, the logarithmic mapping from Lie groups to Lie algebras is invoked. The nonlinear deviation matrix Convert to a six-dimensional linear velocity vector , as a linear error vector.

[0136] Specifically, for nonlinear deviation matrices Perform a logarithmic mapping to determine the linear rotational error. Translation error Integrating linear rotational errors Translation error Determine the linear error vector .

[0137] Example, linear error vector (belong Space can be represented as:

[0138]

[0139] The first three dimensions represent translation errors. The last three dimensions represent rotational errors. .

[0140] In some embodiments, Figure 3 Flowchart of the dual-arm cooperative control method provided in the embodiments of this application Figure 3 ,like Figure 3 As shown, the process of linearizing the deviation matrix and determining the linear error vector in S202 above may include:

[0141] S301. Decompose the deviation matrix to determine the rotational and translational deviations.

[0142] S302. Determine the rotation angle and the unit vector of the rotation axis based on the deviation of the rotating part.

[0143] S303. Calculate the rotation error vector based on the rotation angle and the unit vector of the rotation axis.

[0144] S304. Calculate the translation error vector based on the rotation error vector, the translation part deviation, and the rotation angle.

[0145] S305. Determine the linear error vector based on the rotation error vector and the translation error vector.

[0146] In this embodiment, from the nonlinear deviation matrix Decomposition yields the rotational deviation Translational deviation .

[0147] Based on the deviation of the rotating part Calculate the rotation angle θ of the slave arm from its current actual pose to the desired pose matrix of the slave arm relative to the master arm, as well as the spatial direction of the rotation required for pose alignment, i.e., the unit vector of the rotation axis. u .

[0148] Specifically, the formula for calculating the rotation angle θ is as follows:

[0149]

[0150] in, For the deviation of the rotating part The trace (the sum of the diagonal elements);

[0151] Rotation axis unit vector u (belong Formula for calculating space:

[0152]

[0153] For the deviation of the rotating part 2nd row, 3rd column For the deviation of the rotating part 3rd row, 2nd column For the deviation of the rotating part 3rd row, 1st column For the deviation of the rotating part 1st row, 3rd column For the deviation of the rotating part 1st row, 2nd column For the deviation of the rotating part The second row and first column.

[0154] If the rotation error vector rotation angle Rotation error vector ,like (Without attitude deviation), then the rotation error vector .

[0155] When calculating the translation error vector, the coupling effect of rotation on translation needs to be considered. (belong The formula for calculating space can be expressed as:

[0156]

[0157] in, Rotation error vector The 3rd order cross product matrix (antisymmetric matrix) has the following form:

[0158]

[0159] S203. The linear error vector is compensated by using the logarithmic derivative of the deviation matrix to determine the compensated pose error vector.

[0160] In this embodiment, a deviation matrix is ​​used. logarithmic map derivative (belong The linear error vector is compensated in space to determine the compensated pose error vector. Compensation for pose error vector It directly reflects the translational and rotational corrections that the arm needs to adjust.

[0161] In some embodiments, Figure 4 Flowchart of the dual-arm cooperative control method provided in the embodiments of this application Figure 4 ,like Figure 4 As shown, before S203 uses the logarithmic derivative of the deviation matrix to compensate for the linear error vector and determine the compensated pose error vector, the method may further include:

[0162] S401. Calculate the rotation error compensation submatrix based on the rotation angle, the unit vector of the rotation axis, and the rotation error vector.

[0163] S402. Calculate the translation error compensation submatrix based on the rotation angle and the rotation error vector.

[0164] S403. Calculate the logarithmic mapping derivative based on the rotation error compensation submatrix and the translation error compensation submatrix.

[0165] In this embodiment, the logarithmic mapping derivative of the deviation matrix (belong The matrix form of the space is a block structure:

[0166]

[0167] Among them, the rotation error compensation submatrix Translation error compensation submatrix The calculation logic is as follows:

[0168]

[0169]

[0170] Since rotation error affects the calculation of translation error, the rotation error vector is used to compensate for the translation error vector.

[0171] Translation-rotation coupling compensation submatrix It is used to correct the indirect effects of rotational changes on translational errors.

[0172]

[0173] Finally, the compensated pose error vector after logarithmic mapping derivative compensation is... .

[0174] The robot dual-arm collaborative control method provided in the above embodiments introduces a nonlinear compensation mechanism and uses Lie algebra logarithmic mapping and its differential to accurately process the geometric features of the SE(3) space. Whether it is accurate linearization under small errors or effective guidance under large errors, it ensures the global stability of the control system and faster convergence speed, thereby improving the accuracy of the robot dual-arm collaborative control.

[0175] In one possible implementation, Figure 5 Flowchart of the dual-arm cooperative control method provided in the embodiments of this application Figure 5 ,like Figure 5As shown, the process of determining the target Jacobian matrix of the velocity vector of the slave arm relative to the master arm and the joint velocity of the master and slave arms based on the first Jacobian matrix of the end-effector velocity and the master arm joint velocity, and the second Jacobian matrix of the slave arm end-effector velocity and the slave arm joint velocity, in S104 above, may include:

[0176] S501. Determine the accompanying transformation matrix based on the actual relative pose.

[0177] In this embodiment, in order to unify the first Jacobian matrix of the main arm and the second Jacobian matrix from the arm The coordinate system is expressed by introducing the adjoint transformation matrix. .

[0178] Specifically, based on the actual relative pose of the slave arm (S) relative to the master arm (M) Determine the actual relative pose of the master arm (M) with respect to the slave arm. Then the accompanying transformation matrix It can be represented as:

[0179]

[0180] in, They are respectively The rotating and translating parts; for The 3rd order cross product matrix.

[0181] S502. Determine the relative Jacobian matrix of the slave arm relative to the master arm based on the first Jacobian matrix of the end-effector velocity and the master arm joint velocity, the second Jacobian matrix of the slave arm end-effector velocity and the slave arm joint velocity, and the associated transformation matrix.

[0182] In this embodiment, the adjoint transformation matrix The first Jacobian matrix used to connect the main arm The velocity vector of the end effector relative to the world coordinate system is expressed by transforming from the master arm coordinate system M to the slave arm coordinate system S. (Expressed in the arm coordinate system) can be represented as:

[0183]

[0184] Substitute the first Jacobian matrix of the main arm We obtain the expression of the first Jacobian matrix of the primary arm in the coordinate system S of the secondary arm, namely:

[0185]

[0186] The relative velocity vector from the endarm tip to the main arm tip (Expressed in the arm coordinate system, belonging to...) (Space) equals the velocity vector from the end of the arm relative to the world coordinate system. The velocity vector of the end of the main arm relative to the world coordinate system :

[0187]

[0188] Substituting into their respective Jacobian matrices, we get:

[0189]

[0190] Integrate the primary arm joint velocity and the secondary arm joint velocity into a combined joint velocity. (belong In space, the relative velocity vector can be rewritten as:

[0191]

[0192] in, Recorded as (belong The space (referred to as the relative Jacobian matrix of the slave arm relative to the master arm) is used to quantify the influence of the joint velocity on the relative velocity vector.

[0193] S503. Calculate the target Jacobian matrix based on the relative Jacobian matrix and the logarithmic mapping derivative.

[0194] In this embodiment, the derivative of the logarithmic mapping is combined. For relative Jacobian matrices Nonlinear compensation is performed to obtain the target Jacobian matrix. (belong space).

[0195] The robot dual-arm cooperative control method provided in the above embodiments unifies the dual-arm coordinate system by introducing an adjoint transformation matrix, and clearly quantifies the coupling effect of the master arm motion on the slave arm in the relative Jacobian matrix, thereby realizing active coordination of the dual-arm motion, effectively reducing relative error fluctuations, and improving cooperative stability.

[0196] In one possible implementation, Figure 6 Flowchart of the dual-arm cooperative control method provided in the embodiments of this application Figure 6 ,like Figure 6 As shown, the process of solving the objective function constructed based on the master-slave joint velocity, the target Jacobian matrix, and the compensated pose error vector in S105 to determine the master-slave joint velocity can include:

[0197] S601. Determine the error rate of change parameter of the master-slave joint velocity based on the joint velocity of the master and slave arms and the target Jacobian matrix.

[0198] S602. Determine the error convergence change parameter based on the compensation pose error vector and the preset task gain.

[0199] S603. Construct the objective function based on the position error weight and attitude error weight of the slave arm relative to the master arm, the error rate of change parameter, and the error convergence change parameter.

[0200] S604. Solve the objective function based on the joint velocity constraints to determine the joint velocity of the master and slave arms.

[0201] In this embodiment, the objective function can be expressed as:

[0202]

[0203] in, The objective function is the square of the 2-norm (Euclidean norm squared). Its physical meaning is as follows:

[0204] This represents the rate of change of error caused by joint velocity.

[0205] The target rate of change for error convergence is represented by the negative sign, which implies the direction of optimization, i.e., it is necessary to... This gradually reduces the error; task gain Task gain This represents the low-pass filter coefficient, used to suppress error fluctuations. No filtering (fast tracking) The smoothing error converges.

[0206] The weight matrix W includes position error weights and attitude error weights, assigning different optimization priorities to the position error dimension and the attitude error dimension. The larger the weight, the more preferentially the error in the corresponding dimension is minimized. The matrix form is as follows:

[0207]

[0208] Among them, the first three diagonal elements This represents the position error weight of the slave arm relative to the master arm (unit: const / m). The larger the weight, the higher the priority of the position control accuracy of the corresponding axis; the last three diagonal elements This represents the attitude error weight of the slave arm relative to the master arm (unit: const / rad). The larger the weight, the higher the priority of the control precision in the corresponding attitude dimension.

[0209] To prevent the joint movements of the robot's two arms from exceeding the robot's hardware limits, such as motor rated speed and joint torque limitations, it is necessary to add inequality constraints on the joint velocities:

[0210]

[0211] in, (belong Space) is the lower and upper limits of the main arm joint velocity. (belong The space represents the lower and upper limits of the arm joint velocity, and the constraint parameters are determined by the physical properties of the robot joint motors.

[0212] The above constrained optimization problem is solved using numerical quadratic programming (QP) solvers (such as OSQP, quadprog, etc.). The core idea is to find the joint velocities that minimize the objective function while satisfying the velocity constraints. (belong space).

[0213] In some embodiments, the solution process needs to be real-time, for example, the solution time should be ≤ 50% of the control cycle, in order to adapt to the dynamic requirements of robot dual-arm collaboration.

[0214] Based on the optimal joint velocity obtained through optimization, the joint angles of the master arm and slave arm are updated synchronously through kinematic integrals to ensure the temporal synchronization of the two arm movements and avoid task failures caused by asynchronous movements, such as workpiece falling off or collisions.

[0215] The robot dual-arm collaborative control method provided in the above embodiments dynamically changes the task priority online through position error weights and posture error weights, without the need to replan the entire absolute trajectory. This allows it to easily adapt to various disparate collaborative tasks such as assembly, handling, and welding, making it more versatile and flexible.

[0216] Example, Figure 7 A flowchart of the dual-arm cooperative control method provided in the embodiments of this application is shown below. Figure 7 As shown, the dual-arm cooperative control method provided in this application mainly consists of six steps, specifically:

[0217] Step 1: Task definition, including defining the desired pose matrix. Priority weight matrix W, control period Task Gains Joint velocity constraints v_min and v_max .

[0218] Step 2: Pose acquisition, including acquiring the actual pose of the first end effector of the main arm. and the actual pose of the second end of the arm And calculate the actual relative pose. .

[0219] Step 3: Error calculation, including calculating the deviation matrix. Linear error vector Nonlinear compensation pose error vector .

[0220] Step 4: Jacobian derivation, including obtaining the first Jacobian matrix of the main arm. and the second Jacobian matrix from the arm Determine the adjoint transformation matrix Constructing the relative Jacobian matrix Calculate the target Jacobian matrix .

[0221] Step 5: Optimize the solution, including constructing the objective function, setting constraints, and solving for the joint velocity of the master and slave arms.

[0222] Step 6: Status update, including updating the joint angle based on the speed of the master and slave arm joints, and returning to step 2 to enter the next control cycle.

[0223] Based on the above method embodiments, this application also provides a dual-arm collaborative control device for a robot. Figure 8 This is a schematic diagram of the structure of the dual-arm cooperative control device provided in the embodiments of this application, as shown below. Figure 8 As shown, the device may include:

[0224] The desired pose construction module 701 is used to construct the desired pose matrix of the robot's slave arm relative to the master arm, where the master arm and slave arm are the robot's two robotic arms.

[0225] The actual pose determination module 702 is used to determine the actual relative pose of the slave arm relative to the main arm based on the actual pose of the first end of the main arm and the actual pose of the second end of the slave arm.

[0226] The compensation error calculation module 703 is used to determine the compensation pose error vector of the slave arm based on the difference between the desired pose matrix and the actual relative pose.

[0227] Jacobian matrix determination module 704 is used to determine the target Jacobian matrix of the velocity vector of the slave arm relative to the master arm and the joint velocity of the master and slave arms based on the first Jacobian matrix of the end velocity of the master arm and the joint velocity of the master arm and the second Jacobian matrix of the end velocity of the slave arm and the joint velocity of the slave arm.

[0228] The joint velocity solving module 705 is used to solve the objective function constructed based on the joint velocities of the master and slave arms, the target Jacobian matrix, and the compensation pose error vector to determine the joint velocities of the master and slave arms.

[0229] The joint angle calculation module 706 is used to calculate the joint angle of the master arm and slave arm at the next moment based on the current joint angle of the master arm and slave arm and the joint velocity of the master and slave arms.

[0230] The dual-arm collaborative control module 707 is used to coordinate the control of the master arm and slave arm based on the joint angle of the master arm and slave arm at the next moment.

[0231] Optionally, the actual pose determination module 702 is specifically used to determine the actual pose of the first end of the main arm and the actual pose of the second end of the slave arm based on the real-time joint angles of the robot; and to determine the actual relative pose based on the actual pose of the first end and the actual pose of the second end.

[0232] Optionally, the compensation error calculation module 703 is specifically used to calculate the deviation matrix based on the desired pose matrix and the actual relative pose; to linearize the deviation matrix to determine the linear error vector; and to compensate the linear error vector by using the logarithmic derivative of the deviation matrix to determine the compensated pose error vector.

[0233] Optionally, the compensation error calculation module 703 is also used to decompose the deviation matrix to determine the rotational deviation and the translational deviation; determine the rotation angle and the unit vector of the rotation axis based on the rotational deviation; calculate the rotational error vector based on the rotation angle and the unit vector of the rotation axis; calculate the translational error vector based on the rotational error vector, the translational deviation, and the rotation angle; and determine the linear error vector based on the rotational error vector and the translational error vector.

[0234] Optionally, the device further includes:

[0235] The logarithmic map derivative calculation module is used to calculate the rotation error compensation submatrix based on the rotation angle, the unit vector of the rotation axis, and the rotation error vector; to calculate the translation error compensation submatrix based on the rotation angle and the rotation error vector; and to calculate the logarithmic map derivative based on the rotation error compensation submatrix and the translation error compensation submatrix.

[0236] Optionally, the Jacobian matrix determination module 704 is specifically used to determine the accompanying transformation matrix based on the actual relative pose; determine the relative Jacobian matrix of the slave arm relative to the master arm based on the first Jacobian matrix of the end-effector velocity and the master arm joint velocity, the second Jacobian matrix of the slave arm end-effector velocity and the slave arm joint velocity, and the accompanying transformation matrix; and calculate the target Jacobian matrix based on the relative Jacobian matrix and the logarithmic mapping derivative.

[0237] Optionally, the joint velocity solving module 705 is specifically used to determine the error rate of change parameter of the master-slave joint joint velocity based on the master-slave joint joint velocity and the target Jacobian matrix; determine the error convergence change parameter based on the compensated pose error vector and the preset task gain; construct an objective function based on the position error weight and attitude error weight of the slave arm relative to the master arm, the error rate of change parameter, and the error convergence change parameter; and solve the objective function based on the joint velocity constraints to determine the master-slave joint joint velocity.

[0238] The above-described device is used to execute the method provided in the foregoing embodiments, and its implementation principle and technical effect are similar, so they will not be described again here.

[0239] These modules can be one or more integrated circuits configured to implement the above methods, such as one or more Application Specific Integrated Circuits (ASICs), one or more microprocessors, or one or more Field Programmable Gate Arrays (FPGAs). Alternatively, when a module is implemented using processing element scheduler code, the processing element can be a general-purpose processor, such as a Central Processing Unit (CPU) or other processor capable of calling program code. Furthermore, these modules can be integrated together as a system-on-a-chip (SOC).

[0240] Figure 9 A schematic diagram of the control device provided in the embodiments of this application, such as... Figure 9 As shown, the control device 800 may include a processor 801, a storage medium 802, and a bus. The storage medium 802 stores program instructions executable by the processor 801. When the robot's control device 800 is running, the processor 801 communicates with the storage medium 802 via the bus, and the processor 801 executes the program instructions to perform the above-described method embodiment. The specific implementation and technical effects are similar and will not be described in detail here.

[0241] Optionally, this application also provides a computer-readable storage medium storing a computer program, which is executed by a processor to perform the above-described method embodiments.

[0242] In the several embodiments provided in this application, it should be understood that the disclosed apparatus and methods can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative; for instance, the division of units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between apparatuses or units may be electrical, mechanical, or other forms.

[0243] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0244] Furthermore, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or in a combination of hardware and software functional units.

[0245] The integrated units implemented as software functional units described above can be stored in a computer-readable storage medium. These software functional units, stored in a storage medium, include several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) or processor to execute some steps of the methods described in the various embodiments of this application. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0246] The above are merely specific embodiments of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A method for coordinated control of two arms of a robot, characterized in that, The method includes: Construct the desired pose matrix of the slave arm relative to the master arm of the robot, wherein the master arm and the slave arm are the two robotic arms of the robot; Based on the actual pose of the first end of the main arm and the actual pose of the second end of the slave arm, the actual relative pose of the slave arm with respect to the main arm is determined. The compensation pose error vector of the slave arm is determined based on the difference between the desired pose matrix and the actual relative pose. Based on the first Jacobian matrix of the end-effector velocity and the end-effector joint velocity, and the second Jacobian matrix of the end-effector velocity and the end-effector joint velocity, determine the target Jacobian matrix of the velocity vector of the end-effector relative to the end-effector and the joint velocity of the end-effector and end-effector. Solve the objective function constructed based on the master-slave joint velocity, the target Jacobian matrix, and the compensated pose error vector to determine the master-slave joint velocity; Calculate the joint angles of the master arm and slave arm at the next moment based on the current joint angles of the master arm and slave arm and the joint velocity of the master and slave arms. Based on the joint angles of the master arm and the slave arm at the next moment, the master arm and the slave arm are controlled in a coordinated manner. The step of determining the compensated pose error vector of the slave arm based on the difference between the desired pose matrix and the actual relative pose includes: Calculate the deviation matrix based on the desired pose matrix and the actual relative pose; The deviation matrix is ​​linearized to determine the linear error vector; The linear error vector is compensated using the logarithmic derivative of the deviation matrix to determine the compensated pose error vector; The step of determining the target Jacobian matrix of the velocity vector of the slave arm relative to the master arm and the joint velocity of the master and slave arms based on the first Jacobian matrix of the master arm end-effector velocity and the master arm joint velocity, and the second Jacobian matrix of the slave arm end-effector velocity and the slave arm joint velocity, includes: Based on the actual relative pose, determine the accompanying transformation matrix; The relative Jacobian matrix of the slave arm relative to the master arm is determined based on the first Jacobian matrix of the end-effector velocity and the master arm joint velocity, the second Jacobian matrix of the slave arm end-effector velocity and the slave arm joint velocity, and the adjoint transformation matrix. The target Jacobian matrix is ​​calculated based on the relative Jacobian matrix and the logarithmic map derivative.

2. The method as described in claim 1, characterized in that, Determining the actual relative pose of the slave arm with respect to the master arm based on the actual pose of the first end of the master arm and the actual pose of the second end of the slave arm includes: Based on the robot's real-time joint angles, determine the actual pose of the first end of the main arm and the actual pose of the second end of the slave arm; The actual relative pose is determined based on the actual pose of the first end and the actual pose of the second end.

3. The method as described in claim 1, characterized in that, The linearization process of the deviation matrix to determine the linear error vector includes: The deviation matrix is ​​decomposed to determine the rotational and translational deviations; Based on the deviation of the rotating part, determine the rotation angle and the unit vector of the rotation axis; Calculate the rotation error vector based on the rotation angle and the unit vector of the rotation axis; The translation error vector is calculated based on the rotation error vector, the translation deviation, and the rotation angle. The linear error vector is determined based on the rotation error vector and the translation error vector.

4. The method as described in claim 3, characterized in that, Before determining the compensated pose error vector by using the logarithmic derivative of the deviation matrix to compensate the linear error vector, the method further includes: Calculate the rotation error compensation submatrix based on the rotation angle, the unit vector of the rotation axis, and the rotation error vector; Calculate the translation error compensation submatrix based on the rotation angle and the rotation error vector; The derivative of the logarithmic mapping is calculated based on the rotation error compensation submatrix and the translation error compensation submatrix.

5. The method as described in claim 1, characterized in that, The solution process, which determines the joint velocity of the master and slave arms based on an objective function constructed from the joint velocities of the master and slave arms, the target Jacobian matrix, and the compensated pose error vector, includes: Based on the master-slave joint velocity and the target Jacobian matrix, determine the error rate of change parameter of the master-slave joint velocity; Based on the compensated pose error vector and the preset task gain, determine the error convergence change parameter; The objective function is constructed based on the position error weight and attitude error weight of the slave arm relative to the master arm, the error rate of change parameter, and the error convergence change parameter; The objective function is solved based on the joint velocity constraints to determine the joint velocity of the master and slave arms.

6. A dual-arm collaborative control device for a robot, characterized in that, The device includes: The desired pose construction module is used to construct the desired pose matrix of the robot's slave arm relative to the master arm, wherein the master arm and the slave arm are the two robotic arms of the robot. The actual pose determination module is used to determine the actual relative pose of the slave arm relative to the main arm based on the actual pose of the first end of the main arm and the actual pose of the second end of the slave arm. The compensation error calculation module is used to determine the compensation pose error vector of the slave arm based on the difference between the expected pose matrix and the actual relative pose. The Jacobian matrix determination module is used to determine the target Jacobian matrix of the velocity vector of the slave arm relative to the master arm and the joint velocity of the master and slave arms based on the first Jacobian matrix of the end-effector velocity and the end-effector joint velocity of the master arm and the second Jacobian matrix of the end-effector velocity and the end-effector joint velocity of the slave arm. The joint velocity calculation module is used to solve the objective function constructed based on the master-slave joint velocity, the target Jacobian matrix and the compensated pose error vector, and to determine the master-slave joint velocity. The joint angle calculation module is used to calculate the joint angle of the master arm and the slave arm at the next moment based on the current joint angle of the master arm and the slave arm and the joint velocity of the master and slave arms. The dual-arm collaborative control module is used to collaboratively control the main arm and the slave arm based on the joint angle of the main arm and the slave arm at the next moment; The compensation error calculation module is specifically used to calculate a deviation matrix based on the desired pose matrix and the actual relative pose; linearize the deviation matrix to determine a linear error vector; and use the logarithmic derivative of the deviation matrix to compensate the linear error vector to determine the compensated pose error vector. The Jacobian matrix determination module is specifically used to determine the accompanying transformation matrix based on the actual relative pose; determine the relative Jacobian matrix of the slave arm relative to the master arm based on the first Jacobian matrix of the end-effector velocity and the master arm joint velocity, the second Jacobian matrix of the slave arm end-effector velocity and the slave arm joint velocity, and the accompanying transformation matrix; and calculate the target Jacobian matrix based on the relative Jacobian matrix and the logarithmic mapping derivative.

7. A robot control device, characterized in that, include: The system includes a processor, a storage medium, and a bus. The storage medium stores program instructions executable by the processor. When the robot's control device is running, the processor communicates with the storage medium via the bus. The processor executes the program instructions to perform the steps of the dual-arm cooperative control method for the robot as described in any one of claims 1 to 5.

8. A computer-readable storage medium, characterized in that, The storage medium stores a computer program, which, when executed by a processor, performs the steps of the dual-arm cooperative control method for a robot as described in any one of claims 1 to 5.

Images