A multi-robot collaborative handling control method and device

Abstract

The present invention discloses a multi-robot collaborative handling control method and device, wherein the method includes: establishing a kinematic model of the gripping points of the multi-robots and the center of the gripped material; establishing a motion relationship model between the speed of each robot terminal in the multi-robots and the load based on the kinematic model; establishing a kinematic constraint model of the collaborative handling process based on the motion relationship model and the motion law of the center of the gripped material; taking the speed optimization of each robot system as an indicator, and establishing a handling mathematical model of the multi-robot collaborative handling based on the kinematic constraint model; calculating the acceleration change rate of the corresponding robot at the current moment based on the handling mathematical model of the multi-robot collaborative handling, and forming a control instruction to control the corresponding robot for collaborative handling. In an embodiment of the present invention, the collaborative handling of a common gripped object by multiple robots is realized, and the overall movement speed of the system is optimized at the same time.

Description

Technical Field

[0001] The present invention relates to the field of collaborative control technology, and in particular to a multi-robot collaborative handling control method and device. Background Art

[0002] Multiple robots have better task execution capabilities than single robots. For example, with more degrees of freedom, they can complete more complex tasks, and with stronger load capacity, they can complete the handling of large and heavy objects. However, in the multi-machine handling process, since the robots all have a grasping effect on the object, in general, there is no relative movement between the robot and the object, so there will be a complex structural closed chain. Therefore, each robot must coordinate with each other during the handling process: Specifically, the nature of the structural closed chain physically requires that the movement of the robot's end effector must be synchronized with each other without relative slippage; second, each robot has its own joint angle and other constraints, and the above factors must be taken into account in synchronous handling. Summary of the invention

[0003] The purpose of the present invention is to overcome the deficiencies of the prior art. The present invention provides a multi-robot collaborative handling control method and device to achieve collaborative handling of a common grasped object by multiple robots while optimizing the overall movement speed of the system.

[0004] In order to solve the above technical problems, an embodiment of the present invention provides a multi-robot collaborative handling control method, the method comprising:

[0005] Establish the kinematic model of the grasping points and the center of gravity of the grasped material of multiple robots;

[0006] Based on the kinematic model, a motion relationship model between the terminal speed and the load of each robot in the multi-robot is established;

[0007] Establishing a kinematic constraint model of the collaborative handling process based on the motion relationship model and the motion law of the center of the grasped material;

[0008] Taking the speed optimization of each robot system as an indicator, a mathematical model of multi-robot collaborative handling is established based on the kinematic constraint model;

[0009] The mathematical model of multi-robot collaborative transport is used to calculate the acceleration change rate of the corresponding robot at the current moment, and a control instruction is generated to control the collaborative transport of the corresponding robot.

[0010] Optionally, the establishing of a kinematic model of the grasping points of the multiple robots and the center of gravity of the grasped material includes:

[0011] According to the position of the gripping point of each robot end gripper on the gripped object, a kinematic model of the end motion of the multi-robot to the centroid motion of the gripped object is established.

[0012] Optionally, the kinematic model is as follows:

[0013] r i =Rx i +b (1);

[0014]

[0015] The cross product operator of the vector ω = [ω1; ω2; ω3] is defined as follows:

[0016]

[0017] The above formula (2) can be rewritten as:

[0018]

[0019] Among them, r i represents the position of the end gripper of the ith robot in the global coordinate system; R and b are the rotation matrix and displacement vector between the coordinate system of the grasped object and the global coordinate system, respectively; r i , R, derivative of b; x i is the relative displacement between the grasping point and the center of the grasped mass. After the grasping point is determined, x i Is a constant value.

[0020] Optionally, the kinematic relationship model between the terminal speed and the load of each robot in the multi-robot is established based on the kinematic model as follows:

[0021]

[0022] Among them, J i represents the Jacobian matrix of the i-th robot; represents the angular velocity of the i-th robot; R and b are the rotation matrix and displacement vector between the coordinate system of the grasped object and the global coordinate system, respectively: are the derivatives of R and b respectively.

[0023] Optionally, the load moves according to a preset given trajectory, and the motion law of the center of the grasped mass satisfies:

[0024]

[0025] in, k>0; x0 is the center of the material being grasped, is the first-order derivative of x0; xd is the expected value of x0.

[0026] Optionally, the kinematic constraint model of the collaborative handling process established based on the motion relationship model and the motion law of the center of the grasped material is as follows:

[0027]

[0028] And formula (7) has been widely described as:

[0029]

[0030] Among them, J i represents the Jacobian matrix of the i-th robot; represents the angular velocity of the i-th robot; ω is a definition vector, ω=[ω1; ω2; ω3]; R and b are the rotation matrix and displacement vector between the coordinate system of the grasped object and the global coordinate system, respectively; are the derivatives of R and b respectively; x i is the relative displacement between the grasping point and the center of the grasped mass. After the grasping point is determined, x i is a constant value; k>0; x0 is the center of the material being grasped, is the first-order derivative of x0; x d is the expected value of x0.

[0031] Optionally, the speed optimization of each robot system is used as an indicator, and a mathematical model of multi-robot collaborative handling is established based on the kinematic constraint model as follows:

[0032]

[0033]

[0034]

[0035]

[0036] Among them, J i represents the Jacobian matrix of the i-th robot; represents the angular velocity of the i-th robot; ω is a definition vector, ω=[ω1; ω2; ω3]; R and b are the rotation matrix and displacement vector between the coordinate system of the grasped object and the global coordinate system, respectively; are the derivatives of R and b respectively; x i is the relative displacement between the grasping point and the center of the grasped mass. After the grasping point is determined, x i is a constant value; k>0; x0 is the center of the material being grasped, is the first-order derivative of x0; x d is the expected value of x0; I represents the identity matrix with matching dimensions; are the lower and upper bounds of the joint angles of each robot respectively; They represent the lower and upper bounds of the angular velocity of each robot respectively.

[0037] Optionally, the mathematical model for handling based on multi-robot collaborative handling calculates the acceleration change rate of the corresponding robot at the current moment as follows:

[0038]

[0039] in, for The projection function of Upper limit is Indicates J i The transpose of J i represents the Jacobian matrix of the ith robot; R represents the rotation matrix between the coordinate system of the grasped object and the global coordinate system; represents the angular velocity of the i-th robot; for Derivative; x i is the relative displacement between the grasping point and the center of the grasped mass. After the grasping point is determined, x i is a constant value; k>0; x0 is the center of the material being grasped, is the first-order derivative of x0; x d is the expected value of x0; are the lower and upper bounds of the joint angles of each robot respectively; Respectively represent the lower and upper bounds of the angular velocity of each robot; ∈ represents a positive constant; λ i is an auxiliary variable, is i The first derivative of .

[0040] In addition, an embodiment of the present invention further provides a multi-robot collaborative handling control device, the device comprising:

[0041] The first building module: used to build the kinematic model of the grasping points and the center of mass of the grasped objects of multiple robots;

[0042] The second establishment module is used to establish a motion relationship model between the terminal speed and the load of each robot in the multi-robot based on the kinematic model;

[0043] The third establishment module is used to establish a kinematic constraint model of the collaborative handling process based on the motion relationship model and the motion law of the center of the grasped material;

[0044] The fourth building module is used to establish a transport mathematical model of multi-robot collaborative transport based on the kinematic constraint model, taking the speed optimization of each robot system as an indicator;

[0045] Calculation control module: used to calculate the acceleration change rate of the corresponding robot at the current moment based on the mathematical model of multi-robot collaborative handling, and form control instructions to control the collaborative handling of the corresponding robot.

[0046] Optionally, the first establishing module is further used to establish a kinematic model of the end motion of the multi-robots to the centroid motion of the grasped object according to the position of the grasping point of each robot end gripper of the multi-robots on the grasped object.

[0047] In the embodiments of the present invention, it is possible to realize the coordinated transportation of a common object by multiple robots while optimizing the overall movement speed of the system; at the same time, it is possible to prevent the joint angles and joint angular velocities of the robots from exceeding the limits; there is no need to consider the real-time inversion problem of high-order matrices (due to the existence of structural closed chains in physics, in the traditional Jacobian inversion method, the Jacobian matrix is ​​a cascaded form of the Jacobian matrices of multiple robots. If it is directly inverted, the required amount of calculation is very large), and there is no need to invert the matrix, which will greatly reduce the complexity of the operation, reduce the hardware requirements of the robot, and reduce costs. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0049] Figure 1 is a simple schematic diagram of multi-robot collaborative handling in an embodiment of the present invention;

[0050] Figure 2 is a flow chart of a multi-robot collaborative handling control method in an embodiment of the present invention;

[0051] Figure 3 Schematic diagram of the structure of a multi-robot collaborative handling control device in an embodiment of the present invention. DETAILED DESCRIPTION

[0052] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0053] For examples, see Figure 1 and Figure 2 ; Figure 1 is a simple schematic diagram of multi-robot collaborative handling in an embodiment of the present invention; Figure 2 4 is a flow chart of a multi-robot collaborative handling control method in an embodiment of the present invention.

[0054] like Figure 1 and Figure 2 As shown, a multi-robot collaborative handling control method, the method comprising:

[0055] S11: Establish the kinematic model of the grasping points and the center of mass of the grasped objects of multiple robots;

[0056] In the specific implementation process of the present invention, the kinematic model of the grasping points of multiple robots and the center of gravity of the grasped object is established, including: based on the position of the grasping points of the end grippers of each robot in the multi-robots on the grasped object, a kinematic model of the end motion of the multi-robots to the center of gravity of the grasped object is established.

[0057] Furthermore, the kinematic model is as follows:

[0058] r i =Rx i +b (1);

[0059]

[0060] The cross product operator of the vector ω = [ω1; ω2; ω3] is defined as follows:

[0061]

[0062] The above formula (2) can be rewritten as:

[0063]

[0064] Among them, r i represents the position of the end gripper of the ith robot in the global coordinate system; R and b are the rotation matrix and displacement vector between the coordinate system of the grasped object and the global coordinate system, respectively; r i , R, derivative of b; x iis the relative displacement between the grasping point and the center of the grasped mass. After the grasping point is determined, x i Is a constant value.

[0065] Specifically, according to the position of the gripping point of the gripper at the end of each robot in the multi-robot, a kinematic model of the end motion of the multi-robot to the centroid motion of the grasped object is established. The kinematic model is as follows:

[0066] r i =Rx i +b (1);

[0067]

[0068] The cross product operator of the vector ω = [ω1; ω2; ω3] is defined as follows:

[0069]

[0070] Substituting formula (3) into the above formula (2) can be rewritten as:

[0071]

[0072] Among them, r i represents the position of the end gripper of the ith robot in the global coordinate system; R and b are the rotation matrix and displacement vector between the coordinate system of the grasped object and the global coordinate system, respectively; r i , R, derivative of b; x i is the relative displacement between the grasping point and the center of the grasped mass. After the grasping point is determined, x i is a constant; the vector ω gives the vector omega as an arbitrary three-dimensional vector.

[0073] S12: establishing a motion relationship model between the terminal speed and the load of each robot in the multi-robot based on the kinematic model;

[0074] In the specific implementation process of the present invention, the motion relationship model between the speed and load of each robot terminal in the multi-robot is established based on the kinematic model as follows:

[0075]

[0076] Among them, J i represents the Jacobian matrix of the i-th robot; represents the angular velocity of the i-th robot; R and b are the rotation matrix and displacement vector between the coordinate system of the grasped object and the global coordinate system, respectively; are the derivatives of R and b respectively.

[0077] Furthermore, the load moves according to a preset given trajectory, and the motion law of the center of the grasped mass satisfies:

[0078]

[0079] in, k>0; x0 is the center of the material being grasped, is the first-order derivative of x0; x d is the expected value of x0.

[0080] Specifically, the relationship between the terminal velocity of each robot and the motion relationship model of the load in each robot is established according to the kinematic model as follows:

[0081] The motion relationship model between the terminal speed and the load of each robot in the multi-robot is established based on the kinematic model as follows:

[0082]

[0083] Among them, J i represents the Jacobian matrix of the i-th robot; represents the angular velocity of the i-th robot; R and b are the rotation matrix and displacement vector between the coordinate system of the grasped object and the global coordinate system, respectively; are the derivatives of R and b respectively.

[0084] In order to make the load move according to the preset given trajectory, the motion law of the center velocity of the grasped material is designed to meet the following requirements:

[0085]

[0086] in, k>0; x0 is the center of the material being grasped, is the first-order derivative of x0; x d is the expected value of x0.

[0087] S13: establishing a kinematic constraint model of the collaborative transport process based on the motion relationship model and the motion law of the center of the grasped material;

[0088] In the specific implementation of the present invention, the kinematic constraint model of the collaborative handling process is established based on the motion relationship model and the motion law of the center of the grasped material as follows:

[0089]

[0090] Formula (7) has been widely described as:

[0091]

[0092] Among them, Ji represents the Jacobian matrix of the i-th robot; represents the angular velocity of the i-th robot; ω is a definition vector, ω=[ω1; ω2; ω3]; R and b are the rotation matrix and displacement vector between the coordinate system of the grasped object and the global coordinate system, respectively; are the derivatives of R and b respectively; x i is the relative displacement between the grasping point and the center of the grasped mass. After the grasping point is determined, x i is a constant value; k>0; x0 is the center of the material being grasped, is the first-order derivative of x0; x d is the expected value of x0.

[0093] Specifically, in the process of multi-robot handling, the structure is closed and there is no relative motion. Therefore, the kinematic constraint model of the collaborative handling process is established based on the motion relationship model and the motion law of the center of the grasped material as follows:

[0094]

[0095] And formula (7) has been widely described as:

[0096]

[0097] Among them, J i represents the Jacobian matrix of the i-th robot; represents the angular velocity of the i-th robot; ω is a definition vector, ω=[ω1; ω2; ω3]; R and b are the rotation matrix and displacement vector between the coordinate system of the grasped object and the global coordinate system, respectively; are the first-order derivatives of R and b respectively; x i is the relative displacement between the grasping point and the center of the grasped mass. After the grasping point is determined, x i is a constant value; k>0; x0 is the center of the material being grasped, is the first-order derivative of x0; x d is the expected value of x0.

[0098] S14: taking the speed optimization of each robot system as an indicator, and establishing a handling mathematical model of multi-robot collaborative handling based on the kinematic constraint model;

[0099] In the specific implementation process of the present invention, the speed optimization of each robot system is used as an indicator, and the transport mathematical model of multi-robot collaborative transport is established based on the kinematic constraint model as follows:

[0100]

[0101]

[0102]

[0103]

[0104] Among them, J i represents the Jacobian matrix of the i-th robot; represents the angular velocity of the i-th robot; ω is a definition vector, ω=[ω1; ω2; ω3]; R and b are the rotation matrix and displacement vector between the coordinate system of the grasped object and the global coordinate system, respectively; are the derivatives of R and b respectively; x i is the relative displacement between the grasping point and the center of the grasped mass. After the grasping point is determined, x i is a constant value; k>0; x0 is the center of the material being grasped, is the first-order derivative of x0; x d is the expected value of x0; I represents the identity matrix with matching dimensions; are the lower and upper bounds of the joint angles of each robot respectively; They represent the lower and upper bounds of the angular velocity of each robot respectively.

[0105] Specifically, in the process of multi-robot collaborative handling, the speed optimization of each robot system is used as an indicator, and the physical constraints of the mechanical arms corresponding to each robot, that is, the kinematic constraint model, are considered. The mathematical model of multi-robot collaborative handling is established based on the kinematic constraint model as follows:

[0106]

[0107]

[0108]

[0109]

[0110] Among them, J i represents the Jacobian matrix of the i-th robot; represents the angular velocity of the i-th robot; ω is a definition vector, ω=[ω1; ω2; ω3]; R and b are the rotation matrix and displacement vector between the coordinate system of the grasped object and the global coordinate system, respectively; are the derivatives of R and b respectively; x i is the relative displacement between the grasping point and the center of the grasped mass. After the grasping point is determined, x i is a constant value; k>0; x0 is the center of the material being grasped, is the first-order derivative of x0; xd is the expected value of x0; I represents the identity matrix with matching dimensions; are the lower and upper bounds of the joint angles of each robot respectively; They represent the lower and upper bounds of the angular velocity of each robot respectively.

[0111] S15: Calculate the acceleration change rate of the corresponding robot at the current moment based on the multi-robot collaborative transport mathematical model, and generate control instructions to control the collaborative transport of the corresponding robot.

[0112] In the specific implementation process of the present invention, the mathematical model of multi-robot collaborative handling calculates the acceleration change rate of the corresponding robot at the current moment as follows:

[0113]

[0114] in, for The projection function of Upper limit is Indicates J i The transpose of J i represents the Jacobian matrix of the i-th robot; R represents the rotation matrix between the coordinate system of the grasped object and the global coordinate system; represents the angular velocity of the i-th robot; for Derivative; x i is the relative displacement between the grasping point and the center of the grasped mass. After the grasping point is determined, x i is a constant value; k>0; x0 is the center of the material being grasped, is the first-order derivative of x0; are the lower and upper bounds of the joint angles of each robot respectively; Respectively represent the lower and upper bounds of the angular velocity of each robot; ∈ represents a positive constant; λ i is an auxiliary variable, is i The first derivative of .

[0115] Specifically, the acceleration change rate of each robot at the current moment, that is, the angular acceleration command, is calculated; this is specifically achieved through the following formula:

[0116]

[0117] in, for The projection function of Upper limit is Indicates J iThe transpose of J i represents the Jacobian matrix of the i-th robot; R represents the rotation matrix between the coordinate system of the grasped object and the global coordinate system; represents the angular velocity of the i-th robot; for Derivative; x i is the relative displacement between the grasping point and the center of the grasped mass. After the grasping point is determined, x i is a constant value; k>0; x0 is the center of the material being grasped, is the first-order derivative of x0; are the lower and upper bounds of the joint angles of each robot respectively; Respectively represent the lower and upper bounds of the angular velocity of each robot; ∈ represents a positive constant; λ i is an auxiliary variable, is i The first derivative of .

[0118] In the embodiments of the present invention, it is possible to realize the coordinated transportation of a common object by multiple robots while optimizing the overall movement speed of the system; at the same time, it is possible to prevent the joint angles and joint angular velocities of the robots from exceeding the limits; there is no need to consider the real-time inversion problem of high-order matrices (due to the existence of structural closed chains in physics, in the traditional Jacobian inversion method, the Jacobian matrix is ​​a cascaded form of the Jacobian matrices of multiple robots. If it is directly inverted, the required amount of calculation is very large), and there is no need to invert the matrix, which will greatly reduce the complexity of the operation, reduce the hardware requirements of the robot, and reduce costs.

[0119] Example

[0120] See also Figure 3 , Figure 3 Schematic diagram of the structure of a multi-robot collaborative handling control device in an embodiment of the present invention.

[0121] like Figure 3 As shown, a multi-robot collaborative handling control device comprises:

[0122] The first building module 21 is used to build a kinematic model of the grasping points of the multi-robots and the center of gravity of the grasped material;

[0123] In the specific implementation process of the present invention, the kinematic model of the grasping points of multiple robots and the center of gravity of the grasped object is established, including: based on the position of the grasping points of the end grippers of each robot in the multi-robots on the grasped object, a kinematic model of the end motion of the multi-robots to the center of gravity of the grasped object is established.

[0124] Furthermore, the kinematic model is as follows:

[0125] ri =Rx i +b (1);

[0126]

[0127] The cross product operator of the vector ω = [ω1; ω2; ω3] is defined as follows:

[0128]

[0129] The above formula (2) can be rewritten as:

[0130]

[0131] Among them, r i represents the position of the end gripper of the ith robot in the global coordinate system; R and b are the rotation matrix and displacement vector between the coordinate system of the grasped object and the global coordinate system, respectively; r i , R, derivative of b; x i is the relative displacement between the grasping point and the center of the grasped mass. After the grasping point is determined, x i Is a constant value.

[0132] Specifically, according to the position of the gripping point of the gripper at the end of each robot in the multi-robot, a kinematic model of the end motion of the multi-robot to the centroid motion of the grasped object is established. The kinematic model is as follows:

[0133] r i =Rx i +b (1);

[0134]

[0135] The cross product operator of the vector ω = [ω1; ω2; ω3] is defined as follows:

[0136]

[0137] Substituting formula (3) into the above formula (2) can be rewritten as:

[0138]

[0139] Among them, r i represents the position of the end gripper of the ith robot in the global coordinate system; R and b are the rotation matrix and displacement vector between the coordinate system of the grasped object and the global coordinate system, respectively; r i , R, derivative of b; x i is the relative displacement between the grasping point and the center of the grasped mass. After the grasping point is determined, xi is a constant; the vector ω gives the vector omega as an arbitrary three-dimensional vector.

[0140] The second establishing module 22 is used to establish a motion relationship model between the terminal speed and the load of each robot in the multi-robot based on the kinematic model;

[0141] In the specific implementation process of the present invention, the motion relationship model between the speed and load of each robot terminal in the multi-robot is established based on the kinematic model as follows:

[0142]

[0143] Among them, J i represents the Jacobian matrix of the i-th robot; represents the angular velocity of the i-th robot; R and b are the rotation matrix and displacement vector between the coordinate system of the grasped object and the global coordinate system, respectively: are the derivatives of R and b respectively.

[0144] Furthermore, the load moves according to a preset given trajectory, and the motion law of the center of the grasped mass satisfies:

[0145]

[0146] in, k>0; x0 is the center of the material being grasped, is the first-order derivative of x0; x d is the expected value of x0.

[0147] Specifically, the relationship between the terminal velocity of each robot and the motion relationship model of the load in each robot is established according to the kinematic model as follows:

[0148] The motion relationship model between the terminal speed and the load of each robot in the multi-robot is established based on the kinematic model as follows:

[0149]

[0150] Among them, J i represents the Jacobian matrix of the i-th robot; represents the angular velocity of the i-th robot; R and b are the rotation matrix and displacement vector between the coordinate system of the grasped object and the global coordinate system, respectively; are the derivatives of R and b respectively.

[0151] In order to make the load move according to the preset given trajectory, the motion law of the center velocity of the grasped material is designed to meet the following requirements:

[0152]

[0153] in, k>0; x0 is the center of the material being grasped, is the first-order derivative of x0; x d is the expected value of x0.

[0154] The third establishing module 23 is used to establish a kinematic constraint model of the collaborative handling process based on the motion relationship model and the motion law of the center of the grasped material;

[0155] In the specific implementation of the present invention, the kinematic constraint model of the collaborative handling process is established based on the motion relationship model and the motion law of the center of the grasped material as follows:

[0156]

[0157] Formula (7) has been widely described as:

[0158]

[0159] Among them, J i represents the Jacobian matrix of the i-th robot; represents the angular velocity of the i-th robot; ω is a definition vector, ω=[ω1; ω2; ω3]; R and b are the rotation matrix and displacement vector between the coordinate system of the grasped object and the global coordinate system, respectively; are the derivatives of R and b respectively; x i is the relative displacement between the grasping point and the center of the grasped mass. After the grasping point is determined, x i is a constant value; k>0; x0 is the center of the material being grasped, is the first-order derivative of x0; x d is the expected value of x0.

[0160] Specifically, in the process of multi-robot handling, the structure is closed and there is no relative motion. Therefore, the kinematic constraint model of the collaborative handling process is established based on the motion relationship model and the motion law of the center of the grasped material as follows:

[0161]

[0162] And formula (7) has been widely described as:

[0163]

[0164] Among them, J i represents the Jacobian matrix of the i-th robot; represents the angular velocity of the i-th robot; ω is a definition vector, ω=[ω1; ω2; ω3]; R and b are the rotation matrix and displacement vector between the coordinate system of the grasped object and the global coordinate system, respectively; are the first-order derivatives of R and b respectively; x i is the relative displacement between the grasping point and the center of the grasped mass. After the grasping point is determined, x i is a constant value; k>0; x0 is the center of the material being grasped, is the first-order derivative of x0; x d is the expected value of x0.

[0165] The fourth establishing module 24 is used to establish a transport mathematical model of multi-robot collaborative transport based on the kinematic constraint model, taking the speed optimization of each robot system as an indicator;

[0166] In the specific implementation process of the present invention, the speed optimization of each robot system is used as an indicator, and the transport mathematical model of multi-robot collaborative transport is established based on the kinematic constraint model as follows:

[0167]

[0168]

[0169]

[0170]

[0171] Among them, J i represents the Jacobian matrix of the i-th robot; represents the angular velocity of the i-th robot; ω is a definition vector, ω=[ω1; ω2; ω3]; R and b are the rotation matrix and displacement vector between the coordinate system of the grasped object and the global coordinate system, respectively; are the derivatives of R and b respectively; x i is the relative displacement between the grasping point and the center of the grasped mass. After the grasping point is determined, x i is a constant value; k>0; x0 is the center of the material being grasped, is the first-order derivative of x0; x d is the expected value of x0; I represents the identity matrix with matching dimensions; are the lower and upper bounds of the joint angles of each robot respectively; They represent the lower and upper bounds of the angular velocity of each robot respectively.

[0172] Specifically, in the process of multi-robot collaborative handling, the speed optimization of each robot system is used as an indicator, and the physical constraints of the mechanical arms corresponding to each robot, that is, the kinematic constraint model, are considered. The mathematical model of multi-robot collaborative handling is established based on the kinematic constraint model as follows:

[0173]

[0174]

[0175]

[0176]

[0177] Among them, J i represents the Jacobian matrix of the i-th robot; represents the angular velocity of the i-th robot; ω is the definition vector, ω=[ω1;ω ω ω2; ωω3]; R and b are the rotation matrix and displacement vector between the coordinate system of the grasped object and the global coordinate system, respectively; are the derivatives of R and b respectively; x i is the relative displacement between the grasping point and the center of the grasped mass. After the grasping point is determined, x i is a constant value; k>0; x0 is the center of the material being grasped, is the first-order derivative of x0; x d is the expected value of x0; I represents the identity matrix with matching dimensions; are the lower and upper bounds of the joint angles of each robot respectively; They represent the lower and upper bounds of the angular velocity of each robot respectively.

[0178] The calculation control module 25 is used to calculate the acceleration change rate of the corresponding robot at the current moment based on the multi-robot collaborative transport mathematical model, and to generate control instructions to control the collaborative transport of the corresponding robot.

[0179] In the specific implementation process of the present invention, the mathematical model of multi-robot collaborative handling calculates the acceleration change rate of the corresponding robot at the current moment as follows:

[0180]

[0181] in, for The projection function of Upper limit is Indicates J i The transpose of J irepresents the Jacobian matrix of the i-th robot; R represents the rotation matrix between the coordinate system of the grasped object and the global coordinate system; represents the angular velocity of the i-th robot; for Derivative; x i is the relative displacement between the grasping point and the center of the grasped mass. After the grasping point is determined, x i is a constant value; k>0; x0 is the center of the material being grasped, is the first-order derivative of x0; are the lower and upper bounds of the joint angles of each robot respectively; Respectively represent the lower and upper bounds of the angular velocity of each robot; ∈ represents a positive constant; λ i is an auxiliary variable, is i The first derivative of .

[0182] Specifically, the acceleration change rate of each robot at the current moment, that is, the angular acceleration command, is calculated; this is specifically achieved through the following formula:

[0183]

[0184] in, for The projection function of Upper limit is Indicates J i The transpose of J i represents the Jacobian matrix of the i-th robot; R represents the rotation matrix between the coordinate system of the grasped object and the global coordinate system; represents the angular velocity of the i-th robot; for Derivative; x i is the relative displacement between the grasping point and the center of the grasped mass. After the grasping point is determined, x i is a constant value; k>0; x0 is the center of the material being grasped, is the first-order derivative of x0; are the lower and upper bounds of the joint angles of each robot respectively; Respectively represent the lower and upper bounds of the angular velocity of each robot; ∈ represents a positive constant; λ i is an auxiliary variable, is i The first derivative of .

[0185] Optionally, the first establishing module is further used to establish a kinematic model of the end motion of the multi-robots to the centroid motion of the grasped object according to the position of the grasping point of each robot end gripper of the multi-robots on the grasped object.

[0186] In the embodiments of the present invention, it is possible to realize the coordinated transportation of a common object by multiple robots while optimizing the overall movement speed of the system; at the same time, it is possible to prevent the joint angles and joint angular velocities of the robots from exceeding the limits; there is no need to consider the real-time inversion problem of high-order matrices (due to the existence of structural closed chains in physics, in the traditional Jacobian inversion method, the Jacobian matrix is ​​a cascaded form of the Jacobian matrices of multiple robots. If it is directly inverted, the required amount of calculation is very large), and there is no need to invert the matrix, which will greatly reduce the complexity of the operation, reduce the hardware requirements of the robot, and reduce costs.

[0187] A person skilled in the art may understand that all or part of the steps in the various methods of the above embodiments may be completed by instructing related hardware through a program, and the program may be stored in a computer-readable storage medium, and the storage medium may include: a read-only memory (ROM), a random access memory (RAM), a disk or an optical disk, etc.

[0188] In addition, the above is a detailed introduction to a multi-robot collaborative handling control method and device provided in an embodiment of the present invention. Specific examples are used herein to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the method of the present invention and its core idea; at the same time, for general technical personnel in this field, according to the idea of ​​the present invention, there will be changes in the specific implementation method and application scope. In summary, the content of this specification should not be understood as a limitation on the present invention.

Claims

1. A multi-robot collaborative handling control method, characterized in that: The method comprises: Establish the kinematic model of the grasping points and the center of gravity of the grasped material of multiple robots; Based on the kinematic model, a motion relationship model between the terminal speed and the load of each robot in the multi-robot is established; Establishing a kinematic constraint model of the collaborative handling process based on the kinematic relationship model and the kinematic model of the center of the grasped material; Taking the speed optimization of each robot system as an indicator, a mathematical model of multi-robot collaborative handling is established based on the kinematic constraint model; Based on the mathematical model of multi-robot collaborative handling, the acceleration change rate of the corresponding robot at the current moment is calculated, and a control instruction is formed to control the collaborative handling of the corresponding robot; When the load moves according to a preset given trajectory, the kinematic model of the center of mass being grasped satisfies: The speed optimization of each robot system is used as an indicator, and the mathematical model of multi-robot collaborative handling is established based on the kinematic constraint model as follows: in, represents the angular velocity of the i-th robot; ω is a definition vector, ω=[ω1; ω2; ω3]; R and b are the rotation matrix and displacement vector between the coordinate system of the grasped object and the global coordinate system, respectively; are the derivatives of R and b respectively; I represents the identity matrix with matching dimensions; The mathematical model for multi-robot collaborative handling calculates the acceleration change rate of the corresponding robot at the current moment as follows: in, for The projection function of Upper limit is Indicates J i The transpose of J i represents the Jacobian matrix of the i-th robot; is θ i The second derivative of x i is the relative displacement between the grasping point and the center of the grasped mass. After the grasping point is determined, x i is a constant value; k>0; x0 is the center of the material being grasped, is the first-order derivative of x0; x d is the expected value of x0; are the lower and upper bounds of each robot’s joint angle respectively; Respectively represent the lower and upper bounds of the angular velocity of each robot; ∈ represents a positive constant; λ i is an auxiliary variable, is i The first derivative of .

2. The multi-robot collaborative handling control method according to claim 1, characterized in that: The kinematic model of the grasping points and the center of gravity of the grasped material of the multi-robots is established, including: According to the position of the gripping point of each robot end gripper on the gripped object, a kinematic model of the end motion of the multi-robot to the centroid motion of the gripped object is established.

3. The multi-robot collaborative handling control method according to any one of claims 1 or 2, characterized in that: The kinematic model is as follows: r i =Rx i +b (7); The cross product operator of the vector ω = [ω1; ω2; ω3] is defined as follows: The above formula (8) can be rewritten as: Among them, r i represents the position of the end gripper of the i-th robot in the global coordinate system; For r i The derivative of .

4. The multi-robot collaborative handling control method according to claim 1, characterized in that: The motion relationship model between the terminal speed and the load of each robot in the multi-robot is established based on the kinematic model as follows:

5. The multi-robot collaborative handling control method according to claim 1, characterized in that: The kinematic constraint model of the collaborative handling process established based on the kinematic relationship model and the kinematic model of the center of the grasped material is as follows: The expanded description of formula (12) is:

6. A multi-robot collaborative handling control device, characterized in that: The device comprises: The first building module: used to build the kinematic model of the grasping points and the center of mass of the grasped objects of multiple robots; The second establishment module is used to establish a motion relationship model between the terminal speed and the load of each robot in the multi-robot based on the kinematic model; The third establishment module is used to establish a kinematic constraint model of the collaborative handling process based on the kinematic relationship model and the kinematic model of the center of the grasped material; The fourth building module is used to establish a transport mathematical model of multi-robot collaborative transport based on the kinematic constraint model, taking the speed optimization of each robot system as an indicator; Calculation control module: used to calculate the acceleration change rate of the corresponding robot at the current moment based on the multi-robot collaborative handling mathematical model, and form control instructions to control the collaborative handling of the corresponding robot; When the load moves according to a preset given trajectory, the kinematic model of the center of mass being grasped satisfies: The speed optimization of each robot system is used as an indicator, and the mathematical model of multi-robot collaborative handling is established based on the kinematic constraint model as follows: in, represents the angular velocity of the i-th robot; ω is a definition vector, ω=[ω1; ω2; ω3]; R and b are the rotation matrix and displacement vector between the coordinate system of the grasped object and the global coordinate system, respectively; are the derivatives of R and b respectively; I represents the identity matrix with matching dimensions; The mathematical model for multi-robot collaborative handling calculates the acceleration change rate of the corresponding robot at the current moment as follows: in, for The projection function of Upper limit is Indicates J i The transpose of J i represents the Jacobian matrix of the i-th robot; is θ i The second derivative of x i is the relative displacement between the grasping point and the center of the grasped mass. After the grasping point is determined, x i is a constant value; k>0; x0 is the center of the material being grasped, is the first-order derivative of x0; x d is the expected value of x0; are the lower and upper bounds of each robot’s joint angle respectively; Respectively represent the lower and upper bounds of the angular velocity of each robot; ∈ represents a positive constant; λ i is an auxiliary variable, is i The first derivative of .

7. The multi-robot collaborative handling control device according to claim 6, characterized in that: The first establishing module is also used to establish a kinematic model of the end motion of the multi-robots to the centroid motion of the grasped object according to the position of the grasping point of each robot end gripper of the multi-robots on the grasped object.

Images