A master-slave unified admittance control method for dual-arm robots facing coordination tasks

Abstract

The present invention discloses a master-slave unified admittance control method for a dual-arm robot for coordinated tasks, including: establishing a reference coordinate system and a transformation matrix between coordinate systems, and obtaining the expected end motion trajectory and the master-slave arm motion model of the dual-arm robot by combining closed-chain and open-chain constraints; decomposing the resultant force received by an object and introducing a grasping matrix to obtain the object dynamic equation during the coordinated motion, and combining with admittance control to obtain the trajectory parameters output by the admittance control algorithm, and designing a master-slave unified admittance control UAMSC algorithm; based on the expected end motion trajectory and the master-slave arm motion model, applying the UAMSC algorithm, establishing a cooperative control strategy for the dual-arm robot for different tasks and executing it to maintain the stability of the internal force of the system under the conditions of human-robot cooperation and the existence of external force interference. The present invention can meet the requirements of human-robot cooperation and suppression of external force interference when the dual-arm robot executes coordinated tasks, and can be applied to scenarios such as cooperative handling by the dual-arm robot.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot control, and particularly relates to a master-slave unified admittance control method for a dual-arm robot for coordinated tasks. Background Art

[0002] Compared with single-arm robots, dual-arm robots exhibit significant advantages in various coordinated tasks due to their excellent flexibility, outstanding operability, and strong load capacity. However, this high task adaptability also comes with an increase in system modeling and control complexity. Specifically, the coordinated operation tasks of dual-arm robots can be finely divided into non-coordinated, loosely coordinated, and tightly coordinated types. Especially in complex tasks such as object handling and assembly, due to the formation of closed-chain constraints, precise control of the operating force at the end of the robotic arm is required to prevent damage to the object.

[0003] In terms of control methods, although traditional algorithms such as master-slave control, force-position hybrid control, and admittance / impedance control have certain applicability, to meet higher requirements such as human-robot collaborative operations and stable internal forces in the system, these algorithms often need to be combined with technologies such as adaptive control theory and neural networks. However, this combination not only further increases the difficulty of dual-arm robot planning and control but also may face many challenges during actual deployment, having a greater impact on the practicality and stability of the control algorithm. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to provide a master-slave unified admittance control method for a dual-arm robot for coordinated tasks in view of the above-mentioned deficiencies of the prior art. By designing a master / slave unified admittance control (Unified Admittance-Master / Slave Control, UAMSC) algorithm, applying the kinematic models of open-chain and closed-chain systems, the transformation relationship of the end coordinate system, and absolute / relative constraint conditions, and combining the characteristics of various coordinated tasks, a unified collaborative control strategy for dual-arm robots for coordinated tasks is proposed to meet the requirements of human-robot collaboration and external force interference suppression. Through the collaborative control strategy of dual-arm robots based on UAMSC, problems such as high control system complexity and poor stability when dual-arm robots perform various coordinated tasks are solved.

[0005] To achieve the above technical objectives, the technical solution adopted by the present invention is as follows:

[0006] A master-slave unified admittance control method for a dual-arm robot for coordinated tasks, comprising the following steps:

[0007] S10. Establish a reference coordinate system for the motion of the dual-arm robot and the transformation matrices between coordinate systems. Combine the closed-chain and open-chain constraints when the dual-arm robot performs different types of tasks to obtain the desired end motion trajectory of the dual-arm robot and the master-slave arm motion models;

[0008] S20. Decompose the resultant force acting on the object and introduce a grasping matrix to obtain the dynamic equation of the object during the coordinated motion. Combine with admittance control to obtain the trajectory parameters output by the admittance control algorithm, and design the unified admittance master-slave control UAMSC algorithm based on the trajectory parameters;

[0009] S30. Based on the desired end motion trajectory and the master-slave arm motion models, apply the proposed UAMSC algorithm to establish and execute the cooperative control strategy for the dual-arm robot for non-coordinated, loosely coordinated, and tightly coordinated tasks, so as to maintain the stability of the internal force of the system under the conditions of human-robot cooperation and external force interference.

[0010] To optimize the above technical solution, the specific measures taken also include:

[0011] The above S10 specifically includes:

[0012] S1001. Establish a reference coordinate system, including the world coordinate system W, the base coordinate systems B1 and B2 of the dual-arm robot respectively, the end coordinate systems E1 and E2 of the robot, and the coordinate system O for the dual-arm cooperative operation of the object;

[0013] Based on the established coordinate systems, the homogeneous transformation matrices of each robot end coordinate system relative to the corresponding base coordinate system are respectively and The homogeneous transformation matrices of each base coordinate system relative to the world coordinate system are respectively and The homogeneous transformation matrices of the object coordinate system relative to each robot end coordinate system are respectively and

[0014] S1002. Assume that the desired trajectory of the object is known. When the dual-arm robot performs tightly coordinated tasks, the closed-chain constraint is:

[0015]

[0016] where:

[0017] and are respectively the rotation matrix and the position vector of the object coordinate system relative to the world coordinate system. In this patent, is uniformly expressed as the pose transformation matrix of the X coordinate system relative to the Y coordinate system, Uniformly represented as the rotation matrix of the X coordinate system relative to the T coordinate system, Uniformly represented as the position vector of the X coordinate system relative to the T coordinate system;

[0018] And Are respectively represented as the rotation matrix and position vector of the object coordinate system relative to the end of robot i;

[0019] S1003. Obtain the expected motion trajectories of the ends of each robot under the closed-chain constraint through the transformation matrix:

[0020]

[0021] Among them, (·) T Is uniformly represented as the transpose of the matrix.

[0022] S1004. When the dual-arm robot performs uncoordinated or loosely coordinated tasks, the open-chain constraint at its end is:

[0023]

[0024] S1005. The linear velocity And angular velocity Of the main manipulator (i = 1) are expressed as:

[0025]

[0026]

[0027] Among them, And Are respectively the corresponding expected linear velocity and angular velocity of the object at any given time t, Is the corresponding attitude rotation matrix of the main manipulator at any given time t;

[0028] S1006. Similarly, the expected linear velocity And angular velocity Of the slave arm (k = 2) are expressed as:

[0029]

[0030]

[0031] Among them: Represents the initial position vector of the end coordinate system of the slave arm relative to the main arm; υ pi (t) and ω oi (t) are the control signals of the linear velocity and angular velocity;

[0032] S1007. When the dual-arm robot performs uncoordinated and loosely coordinated tasks, the relative motion relationship of the slave arm is as follows:

[0033]

[0034] Among them, is the corresponding expected linear velocity of the slave arm at any given time t;

[0035] The above S20 specifically includes:

[0036] S2001. Decompose the resultant force acting on the object into internal force and external force. The resultant force w F m =( w f m , w n m ) is expressed as:

[0037]

[0038] Among them, and are respectively the three-dimensional force and moment acting on the object in the world coordinate system, and are comprehensively expressed as w F i = w f i T w ni T T , that is, the contact force exerted by the robot (i = 1, 2) on the object; r i is the input vector;

[0039] S2002. To further simplify formula (9), introduce the grasping matrix G:

[0040] w F m =G w F (10)

[0041] Among them, is a row full-rank matrix, G1 and G2 are respectively the grasping matrices formed between the master arm, the slave arm and the object. In this patent, it is uniformly stipulated that O l is an (l×l) -dimensional zero matrix, I l is an (l×l) -dimensional identity matrix, S(r i ) is the cross-product operation operator of the input vector r i , and l takes 1, 2 or 3;

[0042] S2003. During the coordinated motion process, the object satisfies the following dynamic equation: ​

[0043]

[0044] wherein, is the mass coefficient matrix, and m3 = diag(m o ) is the (3×3) diagonal matrix of the mass coefficient m o ;

[0045] is the acceleration of the object in the world coordinate system;

[0046] is the centrifugal force and Coriolis force term, w ω0 is the angular velocity of the object, is the rotation transformation matrix of the object relative to the world coordinate system;

[0047] w F G is the gravity term of the object;

[0048] w F e is the force / moment term exerted on the object by the environment;

[0049] S2004. When the moving speed of the object is relatively low or the mass is relatively small, the inertial force of the object can be ignored, and thus w F e = - w F G - w F m ; At the same time, the contact force w F i exerted by each robot on the object can be decomposed into internal force w f Ii and external force w f Mi , that is, w F i = w f Mi + w f Ii . When this is the case, formula (10) is converted to:

[0050] w F m = W w f M1 + w f I1 w f M2 + w f I2 T (12) ​

[0051] S2005. Using the method of null space decomposition, decompose the internal and external forces into:

[0052]

[0053]

[0054] In the formula, w F I = w F I1 T w F I2 T T is the internal force after decomposition; w F M = w F M1 T w F M2 T T is the external force after decomposition; is the generalized inverse of the grasping matrix W;

[0055] S2006. The said generalized inverse is:

[0056]

[0057] Wherein, is the coefficient matrix; is the (6×6) anti-diagonal identity matrix;

[0058] S2007. Considering the requirement that the system should have a certain compliant response to external disturbances, further introduce admittance control, and its expression is:

[0059]

[0060] Wherein, i = 1, 2, representing the master manipulator and the slave manipulator;

[0061] M, B, and K respectively represent the mass coefficient, damping coefficient, and stiffness coefficient of admittance control;

[0062] is the internal force tracking error, F Ii is the actually input internal force, F Iid is the desired internal force;

[0063] x id 、 are respectively the desired trajectory, desired velocity, and desired acceleration of the end of the manipulator; ​​

[0064] x ic 、 are respectively the control trajectory parameter, control speed parameter, and control acceleration parameter sent to the robotic arm;

[0065] S2008. Formula (16) is transformed into:

[0066]

[0067] S2009. Applying equation (17) to obtain the trajectory parameter output by the admittance control algorithm:

[0068]

[0069] t0 is the communication period of the control system;

[0070] S2010. Combining the above formulas to obtain the UAMSC algorithm to mitigate external interference and generate a flexible response that complies with the internal force of the object being manipulated, ensuring the stable and accurate execution of coordinated tasks by the dual-arm robot system.

[0071] The expression of the above UAMSC algorithm is:

[0072]

[0073] In the formula, x 1m (t) and x 2m (t) are respectively the control quantities output by UAMSC;

[0074] x 1d (t) and x 2d (t) are respectively the desired trajectories of the master arm and the slave arm, and their values can be updated according to the linear velocity and angular velocity of the master arm and the slave arm robots within each communication period t0;

[0075] x 1c (t) and x 2c (t) are respectively the compliant responses generated by the desired trajectories of the master arm and the slave arm under external interference;

[0076] and are the relative motion of the slave arm with respect to the master arm at the previous communication period t0, and x 2m (t - t0) and x 1m (t - t0) are respectively the control quantities output by UAMSC at the previous communication period t0;

[0077] In the above S30, in the control strategy for non-coordinated tasks, through the transformation relationship between the ends of the master arm and the slave arm, the desired trajectory of the slave arm is obtained from the desired trajectory of the master arm, that is:

[0078]

[0079] Among them, is the desired trajectory speed at the end of the slave arm, is the desired trajectory speed at the end of the slave arm;

[0080] Furthermore, the pose inputs x 1m (t) and x 2m (t) of the cooperative compliant operation of the dual-arm robot are obtained through the UAMSC algorithm:

[0081]

[0082] In the above S30, the control strategy for the loose coordination type of tasks is to allow a transformation relationship between the desired positions or postures of the two arms on the basis of the control strategy for the non-coordination type of tasks. The comprehensive expression is as follows:

[0083]

[0084] In the formula, r T is the relative motion relationship between the left arm and the right arm.

[0085] In the above S30, the control strategy for the tight coordination type of tasks is as follows:

[0086] First, according to the constraint relationship of the closed-loop system, the desired trajectories at the ends of the two arms are obtained from the desired trajectory of the operating object. At the same time, the force / torque signals output by the six-axis force sensor are decoupled into internal force F I1 and F I2 as well as external force w F m , which are respectively used as the inputs of the manipulator-level UAMSC controller and the object-level admittance controller;

[0087] The object-level admittance controller adjusts the ideal position of the object according to the external force acting on the object, and its expression is:

[0088]

[0089] In the formula, x od , are the preset desired trajectory, desired speed, and desired acceleration of the object; x oc , are the actual object trajectory, speed, and acceleration; M o , B o and K o are respectively the mass coefficient, damping coefficient, and stiffness coefficient at the object level; ΔF M = w FM - w F Md is the external force tracking error, w F M is the actual input external force, w F Md is the desired input external force.

[0090] The present invention has the following beneficial effects:

[0091] The present invention provides a master / slave unified admittance control method for a dual-arm robot for coordinated tasks, establishes a kinematic model of the open-chain and closed-chain systems of the dual-arm robot, and obtains the desired end motion trajectories of the two manipulators through the end coordinate system transformation relationship and absolute / relative constraint conditions; decomposes the resultant force acting on the object into internal and external forces, and combines admittance control and master / slave control design to propose a master / slave unified admittance control UAMSC algorithm to achieve a compliant response of the system to the external environment; provides a cooperative control strategy for a dual-arm robot for coordinated tasks, and combines the UAMSC algorithm to achieve cooperative control of uncoordinated, loosely coordinated, and tightly coordinated tasks. The present invention can meet the requirements of human-robot cooperation and suppression of external force interference when the dual-arm robot executes coordinated tasks, makes up for the problems of high complexity and poor stability of traditional control systems, enables it to be actually deployed in the current system environment, and can be applied to scenarios such as cooperative handling of dual-arm robots. Brief Description of the Drawings

[0092] Figure 1 is a schematic flow chart of the master / slave unified admittance control method for a dual-arm robot in an embodiment of the present invention;

[0093] Figure 2 is a schematic diagram of the coordinate system and closed-chain constraint system of a dual-arm robot in an embodiment of the present invention;

[0094] Figure 3 is a force analysis diagram of the object to be operated in an embodiment of the present invention;

[0095] Figure 4 is a block diagram of the master / slave unified admittance control UAMSC in an embodiment of the present invention;

[0096] Figure 5 is a block diagram of the cooperative control strategy algorithm for a dual-arm robot for coordinated tasks in an embodiment of the present invention. Detailed Embodiment

[0097] In order to make the objectives, technical solutions and advantages of the present invention clearer, the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0098] As Figures 1-5As shown in the figure, a master / slave unified admittance control method for a dual-arm robot facing coordination tasks according to the present invention includes the following steps:

[0099] S10. Establish a reference coordinate system for the motion of the dual-arm robot and a transformation matrix between coordinate systems, and obtain the desired end motion trajectory of the dual-arm robot and the master / slave arm motion models by combining the closed-chain and open-chain constraints when the dual-arm robot performs different types of tasks;

[0100] This step establishes the kinematic models of the open-chain and closed-chain systems of the dual-arm robot, and obtains the desired end motion trajectories of the two arms through the end coordinate system transformation relationship and the absolute / relative constraint conditions;

[0101] In the embodiment, a six-dimensional force sensor is selected and installed at the end of the dual-arm robot, and a flexible gripper is connected through a flange. A space coordinate system of the dual-arm robot and the operating object is established, and according to the established coordinate system relationship, the kinematic models of the open-chain and closed-chain systems, the end coordinate system transformation relationship and the absolute / relative constraint conditions are respectively deduced, so as to obtain the expression of the desired motion trajectory of the two arms at the end;

[0102] The specific method for obtaining the system kinematic model and the expression of the desired motion trajectory of the manipulator end is as follows:

[0103] S1001. First, establish a reference coordinate system. Set the world coordinate system as W, the base coordinate systems of the dual-arm robot are B1 and B2 respectively, the end coordinate systems of the robot are E1 and E2 respectively, and the coordinate system of the dual-arm cooperative operating object is O;

[0104] Then determine the transformation matrix relationship between coordinate systems. Through the established coordinate systems, the homogeneous transformation matrices of each robot end coordinate system relative to the corresponding base coordinate system are respectively and The homogeneous transformation matrices of each base coordinate system relative to the world coordinate system are respectively and The homogeneous transformation matrices of the object coordinate system relative to each robot end coordinate system are respectively and

[0105] S1002. Assume that the desired trajectory of the object is known. When the dual-arm robot performs tightly coordinated tasks, the closed-chain constraint can be expressed as formula (1):

[0106]

[0107] Where:

[0108] and are respectively the rotation matrix and the position vector of the object coordinate system relative to the world coordinate system;

[0109] and are respectively represented as the rotation matrix and the position vector of the object coordinate system relative to the end of robot i.

[0110] S1003. The desired poses of the ends of each robot under the closed-chain constraint can be obtained through matrix transformation:

[0111]

[0112] S1004. Similarly, when the dual-arm robot performs uncoordinated or loosely coordinated tasks, the open-chain kinematic constraints of its end can be expressed as:

[0113]

[0114] S1005. Further, when the dual-arm robot performs tightly coordinated tasks, after the desired position and orientation of the object are given, the desired trajectory of the object can be fitted as a function of time t, and the corresponding desired linear velocity and angular velocity of the object at any given time t can be respectively represented by and respectively.

[0115] For the main manipulator (i = 1), its desired trajectory is represented as an absolute motion, and the required and angular velocity are expressed as:

[0116]

[0117]

[0118] S1006. Similarly, the expressions for the desired linear velocity and angular velocity of the slave arm (k = 2) are:

[0119]

[0120]

[0121] where: k = 2 represents the slave arm of the dual-arm robot; represents the initial position vector of the end coordinate system of the slave arm relative to the main arm; υ pi (t) and ω oi (t) are the control signals of the linear velocity and the angular velocity.

[0122] S1007. Considering that when the dual-arm robot performs uncoordinated and loosely coordinated tasks, the relative motion relationship of the slave arm is:

[0123]

[0124] Among them, is the corresponding desired linear velocity of the slave arm at any given time t.

[0125] S20. Decompose the resultant force acting on the object and introduce a grasping matrix to obtain the dynamic equation of the object during the coordinated motion. Combine with admittance control to obtain the trajectory parameters output by the admittance control algorithm, and design the unified master-slave admittance control UAMSC algorithm based on the trajectory parameters;

[0126] This step designs the unified master / slave admittance control (UAMSC) algorithm according to the requirements of human-robot cooperation and suppression of external force interference in the coordinated tasks performed by the dual-arm robot, so as to achieve the compliant response of the system to the external environment;

[0127] In the embodiment, the unified master / slave admittance control (UAMSC) algorithm involves a single-arm admittance control module, a dual-arm master / slave control module, and a driving module.

[0128] Among them: The single-arm admittance control module (single-arm admittance controller) first decomposes the six-dimensional force signal obtained by the sensor into internal and external forces to obtain the actual internal force value acting on the object, and further realizes the compliant operation of the robotic arm through the admittance controller;

[0129] The dual-arm master / slave control module (master / slave controller) coordinates and unifies the pose relationship of the two arms according to the relative motion relationship between the master arm and the slave arm, that is, by defining one of the robotic arms as the master arm to generate active motion, and the other robotic arm as the slave arm, which will follow the master arm to perform passive motion; It should be noted that in this controller, the master arm and the slave arm are not absolute, that is, the master-slave mode of the two arms can be interchanged according to the requirements of external forces or tasks;

[0130] The driving module (underlying hardware) obtains the desired angles of each joint through inverse kinematics according to the end position command of a single arm, and drives the robotic arm to complete the task trajectory through joint planning and a controller, etc.

[0131] The specific method for designing the unified master / slave admittance control (UAMSC) algorithm is as follows:

[0132] S2001. First, when there is a closed-chain constraint, the force exerted by the robotic arm on the operating object, that is, the resultant force acting on the object, is decomposed into internal and external forces. The internal force only generates compressive or tensile forces within the object and does not change the kinematics of the object. The role of the external force is to drive the object to change its motion trajectory.

[0133] The single-arm admittance controller decomposes the resultant force acting on the object into internal and external forces, and the resultant force acting on the object w F m =( w f m , w n m) can be expressed as:

[0134]

[0135] Wherein, and are respectively the three-dimensional force and moment acting on the object in the world coordinate system, and can be comprehensively expressed as w F i = w f i T w n i T T , that is, the contact force exerted by the robot (i = 1, 2) on the object.

[0136] S2002. To further simplify formula (9), the grasping matrix G is introduced:

[0137] w F m = G w F (10)

[0138] Wherein, is a row full-rank matrix, G1 and G2 are respectively the grasping matrices formed between the master arm, the slave arm and the object. In this patent, it is uniformly stipulated that O l is an (l×l) dimensional zero matrix, I l is an (l×l) dimensional identity matrix, S(r i ) is the input vector r i cross product operation operator, and l takes 1, 2 or 3;

[0139] S2003. During the coordinated motion process, the object satisfies the following dynamic equation:

[0140]

[0141] Wherein, is the mass coefficient matrix, m3 = diag(m o ) is the (3×3) diagonal matrix of the mass coefficient m o ;

[0142] is the acceleration of the object in the world coordinate system;

[0143] is the centrifugal force and Coriolis force term, w ω0 is the angular velocity of the object, is the rotation transformation matrix of the object relative to the world coordinate system, I​o is the moment of inertia of the object;

[0144] w F G is the gravitational term of the object;

[0145] w F e is the force / moment term exerted by the environment on the object.

[0146] S2004. When the moving speed of the object is relatively low or the mass is relatively small, the inertial force of the object can be ignored, and thus w F e = - w F G - w F m ; meanwhile, the contact forces exerted by the two robots on the object can be decomposed into internal force w f Ii and external force w f Mi i.e., w F i = w f Mi + w f Ii ; thus, formula (10) can be transformed into:

[0147] w F m = W w f M1 + w f I1 w f M2 + w f I2 T (12)

[0148] S2005. Using the method of null space decomposition, the internal and external forces can be decomposed into:

[0149]

[0150]

[0151] wherein, w F I = w F I1 T w F I2 T T is the decomposed internal force; w F M = w ​​F M1 T w F M2 T T is the external force after decomposition; is the generalized inverse of the grasping matrix W;

[0152] S2006. The above generalized inverse can be calculated as follows:

[0153]

[0154] where, is the coefficient matrix; is the (6×6) anti-diagonal identity matrix.

[0155] S2007. Then, considering the requirement that the system should have a certain compliant response to external disturbances, it is necessary to further introduce admittance control, and its expression is:

[0156]

[0157] where i = 1, 2, representing the master manipulator and the slave manipulator;

[0158] M, B, and K respectively represent the mass coefficient, damping coefficient, and stiffness coefficient of admittance control;

[0159] is the internal force tracking error, F Ii is the actually input internal force, F Iid is the desired internal force;

[0160] x id is the desired trajectory of the end of the manipulator;

[0161] x ic is the control trajectory parameter sent to the manipulator;

[0162] S2008. Formula (16) can be transformed into:

[0163]

[0164] S2009. Applying equation (17) can obtain the trajectory parameter output by the admittance control algorithm:

[0165]

[0166] S2010. During the process of dual-arm cooperative operation, these manipulators are very susceptible to external disturbances. To solve this problem and meet the requirements of different coordination tasks, the master / slave unified admittance control (UAMSC) algorithm can be obtained by combining the above formulas.

[0167] ​The master / slave unified admittance control (UAMSC) module aims to mitigate external disturbances and generate a flexible response that conforms to the internal forces of the object being manipulated, ensuring that the dual-arm robot system can stably and accurately perform coordinated tasks. Its expression is as follows:

[0168]

[0169] In the formula, x 1d (t) and x 2d (t) are the desired trajectories of the master arm and the slave arm respectively, and x 1c (t) and x 2c (t) are the compliant responses generated by the desired trajectories of the master arm and the slave arm under external disturbances respectively. and is the relative motion of the slave arm with respect to the master arm at the previous communication cycle t0.

[0170] S30. Based on the desired end-effector motion trajectory and the master-slave arm motion models, apply the proposed UAMSC algorithm, and establish and execute a cooperative control strategy for the dual-arm robot for non-coordinated, loosely coordinated, and tightly coordinated tasks, so as to maintain the stability of the internal forces of the system under the conditions of human-robot collaboration and the presence of external forces.

[0171] This step applies the proposed UAMSC algorithm, and based on the characteristics of non-coordinated, loosely coordinated, and tightly coordinated tasks, establishes a unified cooperative control strategy for the dual-arm robot for coordinated tasks, and maintains the stability of the internal forces of the system under the conditions of human-robot collaboration and the presence of external forces. That is, the cooperative control strategy for the dual-arm robot for coordinated tasks provides corresponding operation strategies for each type of task according to the requirements of different tasks. This method aims to improve the efficiency, stability, and flexibility of the cooperative operation of the dual-arm robot to adapt to various complex operating environments.

[0172] The cooperative control strategy for the dual-arm robot for coordinated tasks provides corresponding operation strategies for each type of task according to the requirements of different tasks. This method aims to improve the efficiency, stability, and flexibility of the cooperative operation of the dual-arm robot to adapt to various complex operating environments.

[0173] Specifically, the cooperative control strategy of dual-arm robots for coordination tasks divides the coordination tasks of dual-arm robots into three categories: uncoordinated, loosely coordinated, and tightly coordinated tasks. In uncoordinated tasks, the two arms operate independently and only need to be synchronized. For loosely coordinated tasks, there are certain coordination requirements between the manipulators, but a certain degree of position or attitude transformation relationship is allowed. However, when human-robot cooperation or external force interference occurs, the original cooperation of the two arms will still be maintained. For tightly coordinated tasks, not only precise coordination between the manipulators is required to achieve high-precision operations, but also the stability of the internal force of the object is maintained to ensure accuracy and safety. Considering the uniqueness of each task category, the cooperative control strategy of dual-arm robots for coordination tasks can achieve adaptive and efficient coordination control of dual-arm robots to meet the requirements of different operation scenarios.

[0174] In an embodiment, the cooperative control strategy of dual-arm robots for coordination tasks includes three parts: the control strategy for uncoordinated tasks, the control strategy for loosely coordinated tasks, and the control strategy for tightly coordinated tasks.

[0175] Among them, the control strategy for uncoordinated tasks first obtains the desired trajectory of the slave arm from the desired trajectory of the master arm through the transformation relationship between the ends of the master arm and the slave arm, and further realizes the cooperative compliant operation of the dual-arm robot through the master / slave unified admittance controller (UAMSC) at the manipulator level;

[0176] The control strategy for loosely coordinated tasks allows a certain transformation relationship between the desired positions or attitudes of the two arms on the basis of the control strategy for uncoordinated tasks, but when human-robot cooperation or external force interference occurs, the original cooperation of the two arms will still be maintained;

[0177] The control strategy for tightly coordinated tasks first obtains the desired trajectories of the ends of the two arms from the desired trajectory of the operating object according to the constraint relationship of the closed-chain system. At the same time, the force / torque signals output by the six-dimensional force sensor are decoupled by internal and external forces to obtain the internal force F I1 and F I2 and the external force w F m , which are respectively used as the inputs of the UAMSC controller at the manipulator level and the admittance controller at the object level.

[0178] The specific method for designing the cooperative control strategy of dual-arm robots for coordination tasks is as follows:

[0179] S3001. In the control strategy for uncoordinated tasks, the desired trajectory of the slave arm is obtained from the desired trajectory of the master arm through the transformation relationship between the ends of the master arm and the slave arm, that is:

[0180]

[0181] Furthermore, the pose inputs x 1m (t) and x 2m (t) of the cooperative compliant operation of the dual-arm robot are obtained through the master / slave unified admittance controller (UAMSC) at the robotic arm level:

[0182]

[0183] S3002. The loose coordination type task control strategy allows a certain transformation relationship between the desired positions or postures of the two arms on the basis of the non-coordination type task control strategy. The comprehensive expression is as follows:

[0184]

[0185] In the formula, r T is the relative motion relationship between the left arm and the right arm, which can be set according to the task requirements.

[0186] S3003. The tight coordination type task control strategy first obtains the desired trajectories of the two-arm ends from the desired trajectory of the operating object according to the constraint relationship of the closed-chain system. At the same time, the force / torque signals output by the six-axis force sensor are decoupled into internal force F I1 and F I2 as well as external force w F m , which are respectively used as the inputs of the UAMSC controller at the manipulator level and the admittance controller at the object level. The admittance controller at the object level adjusts the ideal position of the object according to the external force acting on the object. Its expression is:

[0187]

[0188] In the formula, x od , are the preset desired trajectory, desired velocity, and desired acceleration of the object; x oc , are the actual object trajectory, velocity, and acceleration; M o , B o and K o are respectively the mass coefficient, damping coefficient, and stiffness coefficient at the object level; Δ FM = w F M - w F Md is the external force tracking error, w F M is the actual input external force, w F Md is the desired input external force.

[0189] In practical applications, the stiffness coefficient K at the object level oUsually set to 0 to prevent the object from returning to its initial position after the external force is removed, M o and B o Set according to the task requirements.

[0190] Although the steps in the present invention are arranged with reference numerals, they are not used to limit the order of the steps. Unless the order of the steps is clearly stated or the execution of a certain step requires other steps as a basis, the relative order of the steps can be adjusted. It can be understood that the term "and / or" used herein refers to and encompasses any and all possible combinations of one or more of the associated listed items.

[0191] For those skilled in the art, it is obvious that the present invention is not limited to the details of the above-described exemplary embodiments, and without departing from the spirit or basic characteristics of the present invention, the present invention can be implemented in other specific forms. Therefore, in any aspect, the embodiments should be regarded as exemplary and non-limiting. The scope of the present invention is defined by the appended claims rather than the above description. Therefore, all changes falling within the meaning and scope of the equivalent elements of the claims are intended to be included in the present invention. Any reference numerals in the claims should not be regarded as limiting the claims involved.

[0192] In addition, it should be understood that although this specification is described according to embodiments, not every embodiment only contains an independent technical solution. This narrative way of the specification is only for clarity. Those skilled in the art should regard the specification as a whole, and the technical solutions in each embodiment can also be appropriately combined to form other embodiments that can be understood by those skilled in the art.

Claims

1. A master-slave unified admittance control method for a dual-arm robot facing coordination tasks, characterized in that It includes the following steps: S10. Establish a reference coordinate system for the motion of the dual-arm robot and the transformation matrix between coordinate systems, and obtain the expected end motion trajectory of the dual-arm robot and the master-slave arm motion model by combining the closed-chain and open-chain constraints when the dual-arm robot performs different types of tasks; S20. Decompose the resultant force acting on the object and introduce a grasping matrix to obtain the dynamic equation of the object during the coordinated motion process. Combine with admittance control to obtain the trajectory parameters output by the admittance control algorithm, and design the unified admittance master-slave control UAMSC algorithm based on the trajectory parameters; the expression of the UAMSC algorithm is: ； wherein, and are respectively the control quantities output by the UAMSC; and are the desired trajectories of the main arm and the slave arm respectively, and their values can be updated according to the end linear velocity and the angular velocity 、 of the main arm and the slave arm robots within each communication cycle 、 ; and are the compliance responses generated by the desired trajectories of the master arm and the slave arm under external disturbances, respectively; and is the relative movement of the slave arm with respect to the master arm during the previous communication cycle ; and and are respectively the control quantities output by the UAMSC during the previous communication cycle ; S30. Based on the expected end motion trajectory and the master-slave arm motion model, apply the proposed UAMSC algorithm to establish and execute the cooperative control strategy for the dual-arm robot for non-coordinated, loosely coordinated, and tightly coordinated tasks, so as to maintain the stability of the internal force of the system under the conditions of human-robot collaboration and external force interference.

2. The master-slave unified admittance control method for a dual-arm robot for coordination tasks according to claim 1, characterized in that The specific content of S10 includes: S1001. Establish a reference coordinate system, including the world coordinate system , the base coordinate systems of the dual-arm robot respectively , , the end coordinate systems of the robot , , and the coordinate system O for the dual-arm to cooperate in operating an object; Based on the established coordinate system, the homogeneous transformation matrices of the end coordinate systems of each robot relative to the corresponding base coordinate systems are respectively and ; the homogeneous transformation matrices of each base coordinate system relative to the world coordinate system are respectively and ; the homogeneous transformation matrices of the object coordinate system relative to the end coordinate systems of each robot are respectively and ; and define a unified representation of the pose transformation matrix of a coordinate system relative to a coordinate system; a unified representation of the rotation matrix of a coordinate system relative to a coordinate system; a unified representation of the position vector of a coordinate system relative to a coordinate system; where is , , , , , , can be , , , , ; S1002. Assume that the expected trajectory of the object is known. When the dual-arm robot performs tightly coordinated tasks, the closed-chain constraint is: ； Where: , and are respectively the rotation matrix and position vector of the object coordinate system relative to the world coordinate system, , and are respectively expressed as the rotation matrix and position vector of the object coordinate system relative to the end of the robot; S1003. Obtain the expected motion trajectory of each robot end under the closed-chain constraint through the transformation matrix: ； Among them, represents the transpose of a matrix; S1004. When the dual-arm robot performs non-coordinated or loosely coordinated tasks, the open-chain constraint at its end is: ； S1005, the linear velocity and angular velocity of the main robotic arm are expressed as: ； ； wherein, and are respectively the corresponding desired linear velocity and angular velocity of the object at any given time t, is the corresponding attitude rotation matrix of the main robotic arm at the given time t; S1006. Similarly, the expressions for the desired linear velocity and angular velocity of the slave arm are as follows: ； ； Wherein: represents the initial position vector of the end coordinate system of the slave arm relative to the master arm; and are the control signals of the linear velocity and the angular velocity; S1007. When the dual-arm robot performs non-coordinated and loosely coordinated tasks, the relative motion relationship of the slave arm is: ； wherein, is the corresponding desired linear velocity of the slave arm at any given time t.

3. A master-slave unified admittance control method for a dual-arm robot facing coordination tasks according to claim 1, characterized in that The specific content of S20 includes: S2001. Decompose the resultant force acting on an object into internal force and external force. The resultant force acting on the object is expressed as: ； Among them, and are respectively the three-dimensional force and torque acting on the object in the world coordinate system, and are comprehensively expressed as , that is, the contact force exerted by the robot ( ) on the object; is the input vector; S2002. To further simplify formula (9), a grasping matrix is introduced : ； Among them, , is a row full-rank matrix; , and are the grasping matrices formed between the main arm, the slave arm and the object respectively; is a zero matrix of dimension is an identity matrix of dimension is the input vector the cross product operation operator takes 1, 2 or 3; S2003. During the coordinated motion process, the object satisfies the following dynamic equation: ； Among them, is the mass coefficient matrix, is the mass coefficient of the (3×3) diagonal matrix; is the acceleration of an object in the world coordinate system; , which are the centrifugal force and the Coriolis force terms, is the angular velocity of the object, , is the rotation transformation matrix of the object relative to the world coordinate system, is the moment of inertia of the object; is the gravity term of the object; is the force / moment term exerted by the environment on the object; S2004. When the moving speed of the object is relatively low or the mass is relatively small, the inertial force of the object can be ignored, thus obtaining ; At the same time, the contact force exerted by each robot on the object can be decomposed into internal force and external force , that is, When, formula (10) is converted to: ； S2005. Use the method of null space decomposition to decompose the internal and external forces into: ； ； Wherein, is the internal force after decomposition; is the external force after decomposition; is the grasping matrix of the generalized inverse; S2006. The generalized inverse is as follows: ； Among them, is the coefficient matrix; is a (6×6) anti-diagonal identity matrix; S2007. Considering the requirement that the system has a certain compliant response to external interference, further introduce admittance control, and its expression is: ； Among them, represents the master manipulator and the slave manipulator; , and represent the mass coefficient, damping coefficient, and stiffness coefficient for admittance control, respectively; is the internal force tracking error, is the actually input internal force, is the desired internal force; They are respectively the desired trajectory, desired velocity, and desired acceleration at the end of the robotic arm; They are respectively the control trajectory parameters, control speed parameters, and control acceleration parameters sent to the robotic arm; S2008. Equation (16) is transformed into: ； S2009. Apply equation (17) to obtain the trajectory parameters output by the admittance control algorithm: ； is the communication cycle of the control system; S2010. Combine the above formulas to obtain the UAMSC algorithm to reduce external interference and generate a flexible response that complies with the internal force of the object to be manipulated, ensuring the stable and accurate execution of coordinated tasks by the dual-arm robot system.

4. A master-slave unified admittance control method for a dual-arm robot for coordination tasks according to claim 1, characterized in that In S30, in the control strategy for non-coordinated tasks, through the transformation relationship between the ends of the master arm and the slave arm, the expected trajectory of the slave arm is obtained from the expected trajectory of the master arm, that is: ； wherein, is the desired trajectory speed at the end of the slave arm, is the desired trajectory speed at the end of the slave arm; Furthermore, the pose input for the cooperative compliant operation of the dual-arm robot is obtained through the UAMSC algorithm and : 。 5. A master-slave unified admittance control method for a dual-arm robot for coordinated tasks according to claim 1, characterized in that In S30, the control strategy for loosely coordinated tasks is to allow a transformation relationship between the expected positions or postures of the two arms on the basis of the control strategy for non-coordinated tasks. The comprehensive expression is as follows: ； In the formula, is the relative motion relationship between the left arm and the right arm.

6. The master-slave unified admittance control method for a dual-arm robot facing coordination tasks according to claim 1, characterized in that In S30, the control strategy for tightly coordinated tasks is: First, according to the constraint relationship of the closed-chain system, the desired trajectories of the two-arm ends are obtained from the desired trajectory of the operating object. At the same time, the force / torque signals output by the six-axis force sensor are decoupled into internal forces and external forces , which are used as the inputs of the manipulator-level UAMSC controller and the object-level admittance controller, respectively. The object-level admittance controller adjusts the ideal position of the object according to the external force acting on the object, and its expression is: ； wherein, is the expected trajectory, expected velocity, and expected acceleration of the object preset in advance; , , are the actual trajectory, velocity, and acceleration of the object; , and are the mass coefficient, damping coefficient, and stiffness coefficient at the object level respectively; is the external force tracking error, is the actual input external force, is the expected input external force.

Images