A collaborative robot dynamic error modeling and compensation method

Abstract

The application relates to a collaborative robot dynamic error modeling and compensation method, which comprises the following steps: step one: a kinematics model of the collaborative robot is established by using an M-DH method; step two: a dynamics model of the collaborative robot is established based on a Newton-Euler method by considering the influence of robot self-weight, inertial force and load on joint torque, so that joint driving torque during robot operation is obtained; step three: a collaborative robot end dynamic error model is established according to the relationship between dynamic force and joint torsional stiffness, and the influence of supplementary stiffness on the end dynamic error is considered; and step four: the end dynamic error is converted to the joint space, and dynamic error compensation is carried out on the collaborative robot. The application establishes a collaborative robot dynamics model considering factors such as gravity, inertial force and end load, and further establishes an end dynamic error model considering supplementary stiffness, so that the end dynamic error compensation problem under the action of dynamic force is solved.

Description

Technical Field

[0001] This invention relates to the field of collaborative robots, specifically a method for dynamic error modeling and compensation of collaborative robots. Background Technology

[0002] Collaborative robots possess advantages such as lightweight structure, strong environmental adaptability, and high operational flexibility, along with low energy consumption, large operating space, and high efficiency, exhibiting rapid and accurate response. Therefore, collaborative robots are commonly used in assembly, dispensing, welding, and material removal. However, due to their high flexibility, collaborative robots are susceptible to dynamic errors caused by gravity, inertial forces, and external loads during operation. This leads to deviations between the actual trajectory of the robot's end effector and the desired trajectory, limiting the further development of collaborative robots in precision manufacturing and automation. Summary of the Invention

[0003] The purpose of this invention is to provide a method for modeling and compensating dynamic errors of collaborative robots. It establishes a dynamic model of the collaborative robot that considers factors such as gravity, inertial force and end-effector load, and further establishes an end-effector dynamic error model that considers supplementary stiffness, thereby solving the problem of end-effector dynamic error compensation under dynamic force.

[0004] The objective of this invention is achieved through the following technical solution:

[0005] A method for dynamic error modeling and compensation of collaborative robots includes the following steps:

[0006] Step 1: Establish the kinematic model of the collaborative robot using the M-DH method;

[0007] Step 2: Calculate the inertial force and moment of inertia of each link based on the Newton-Euler formula:

[0008] i+1 f c(i+1) =m i+1 + i+1 ω i+1 ×(m i+1 i+1 v c(i+1) )

[0009]

[0010] in, i+1 f c(i+1) The inertial force of link i+1 in coordinate system {i+1} represents the force exerted by link i+1 in coordinate system {i+1}; m i+1 This represents the mass of link i+1; i+1 v c(i+1) This represents the acceleration of the center of mass of link i+1 in coordinate system {i+1}; i+1 τc(i+1) This represents the moment of inertia of link i+1 in coordinate system {i+1}; c(i+1) I i+1 Let represent the inertia tensor at the center of mass of link i+1. i+1 ω i+1 This represents the angular velocity of link i+1 in the link coordinate system {i+1}. This represents the angular acceleration of link i+1 in coordinate system {i+1};

[0011] Starting from the robot's end effector and proceeding sequentially to the base, calculate the forces and torques of the interactions between each link, as well as the joint driving torques:

[0012]

[0013]

[0014]

[0015] Among them, F i N represents the force acting on link i. i τ represents the torque acting on link i; i This represents the driving torque of joint i. i p i+1 This represents the position vector of the origin of coordinate system {i+1} within coordinate system {i}. Let {i+1} be the rotation transformation matrix of coordinate system {i} in coordinate system {i};

[0016] Step 3: Describe all joints as torsion springs and consider the additional stiffness K. c Then, the dynamic error model of the robot's end effector is obtained:

[0017] ΔD=J·(K θ -K c ) -1 ·Γ;

[0018] Where ΔD is the deformation of the robot's end effector, and K θ Γ is the joint torsional stiffness matrix, J is the joint driving torque, and K is the robot Jacobian matrix. c It is a supplementary stiffness matrix;

[0019] Step 4: Convert the end-effector dynamic error to joint space and perform dynamic error compensation for the collaborative robot;

[0020] Step 5: Simulate and verify the dynamic error of the compensated collaborative robot end effector, and compare the simulated value with the theoretical value to guide the robot's actions.

[0021] Step one is as follows:

[0022] Step 1.1: Find the rotation axis of each joint. The intersection of the axes i and i+1 of two adjacent joints is defined as the origin Oi of coordinate system {i}. Define the axis i of the rotation joint as the Z-axis of coordinate system {i}. i axis, and Z i and Z i+1 The normal to the formed plane is the Xi axis of coordinate system {i}. Then, the direction of the yi axis is determined by the right-hand rule of Cartesian coordinate system. It is stipulated that the base coordinate system {0} and the link coordinate system {1} have the same direction. n coordinate systems are established according to the above rules.

[0023] Step 1.2: A coordinate system is fixed to each joint of the robot. The link coordinate system {i} is relative to the coordinate system {i-1} through the link length a. i-1 Linkage twist angle α i-1 Linkage offset d i With joint angle θ i The four parameters are related; therefore, the transformation matrix of coordinate system {i} relative to coordinate system {i-1} is:

[0024]

[0025] Step 1.3: By multiplying the matrices of each adjacent joint, the pose of the robot end effector in the base coordinate system is obtained.

[0026] In step two, 3D modeling software is first used with the coordinate system of each link as the output coordinate system to extract the mass attributes of each joint of the robot, including mass, inertia tensor, and center of mass position. Then, the velocity and acceleration of each link are calculated recursively from the base toward the end link.

[0027]

[0028]

[0029]

[0030] in, i+1 ω i+1 This represents the angular velocity of link i+1 in the link coordinate system {i+1}; Let {i+1} be the rotation transformation matrix of coordinate system {i} in coordinate system {i}; Indicates joint angular velocity; i+1 z i+1 The unit vector representing the z-axis of coordinate system {i+1}; This represents the angular acceleration of link i+1 in coordinate system {i+1}; Indicates joint angular acceleration; This represents the linear acceleration of link i+1 in coordinate system {i+1}; i p i+1 This represents the position vector of the origin of coordinate system {i+1} within coordinate system {i}.

[0031] Then, the inertial force and moment of inertia of each link are calculated using the Newton-Euler formula.

[0032] Step three specifically involves:

[0033] Step 3.1: Describe all joints as torsion springs. The dynamic error model of the robot's end effector due to joint flexibility is expressed as:

[0034] ΔD=K -1 ·F;

[0035] Where ΔD is the end deformation, K is the Cartesian stiffness matrix, and F is the end generalized force.

[0036] According to Cartesian stiffness and joint torsional stiffness K θ The relationship is obtained as follows:

[0037] K = J -T ·K θ ·J -1 ;

[0038] Step 3.2: The relationship between the generalized force at the robot's end effector and the joint driving torque is as follows:

[0039] Γ=J T ·F;

[0040] Differentiating the above equation, we get:

[0041]

[0042] After simplification, we get:

[0043]

[0044] Let matrix Then supplement stiffness K c The relationship with Cartesian stiffness K is as follows:

[0045] K = J -T ·(K θ -K c )·J -1 ;

[0046] Therefore, we consider adding stiffness K. c Subsequently, the dynamic error model of the robot's end effector is expressed as:

[0047] ΔD=J·(K θ -K c )-1 ·Γ.

[0048] Step four is as follows:

[0049] Step 4.1: Convert the end-effector dynamic error, which takes into account the supplementary stiffness, to the joint space:

[0050] Δθ=J -1 ·ΔD;

[0051] Step 4.2: Compensate for the dynamic error of the robot's end effector by correcting the robot's joint variables. The compensated joint angles are:

[0052] θ c =θ a +Δθ;

[0053] Where, θ c It is the compensated joint angle, θ a It is the actual joint angle.

[0054] Step five specifically involves: using 3D modeling software to create a 3D model of the robot, simplifying the model, importing the model into dynamics simulation software, adding material properties for each robot component, setting constraints and applying drives, and then performing dynamics simulation. In the dynamics simulation software, torsion springs are added to the rotary joints, and joint stiffness and damping are set to replace the original rigid kinematic pairs. Finally, the simulation values ​​are compared with the theoretical values.

[0055] The advantages and positive effects of this invention are as follows:

[0056] This invention establishes a collaborative robot dynamic model that considers factors such as gravity, inertial force, and end-effector load, and further establishes an end-effector dynamic error model that considers supplementary stiffness, thereby solving the problem of end-effector dynamic error compensation under dynamic force. Attached Figure Description

[0057] Figure 1 This is a schematic diagram of the process of the present invention;

[0058] Figure 2 This is a schematic diagram of the collaborative robot that this invention addresses;

[0059] Figure 3 for Figure 2 A schematic diagram of the linkage coordinate system of a collaborative robot;

[0060] Figure 4 for Figure 3 A schematic diagram of joint parameters in the middle link coordinate system;

[0061] Figure 5 This is a comparison chart of the simulation results of the joint-driving torque in the method of the present invention;

[0062] Figure 6 This is a comparison chart of the simulation results of the joint driving torque in the method of the present invention;

[0063] Figure 7 This is a comparison chart of simulation results of the three driving torques of the joint in the method of the present invention;

[0064] Figure 8 This is a comparison chart of the simulation results of the four-drive torque of the joint in the method of the present invention;

[0065] Figure 9 This is a comparison chart of the simulation results of the five driving torques of the joint in the method of this invention;

[0066] Figure 10 This is a comparison chart of the simulation results of the six driving torques of the joint in the method of this invention;

[0067] Figure 11 This is a comparison chart of the simulation results of the seven driving torques of the joint in the method of this invention;

[0068] Figure 12 This is a flowchart of a collaborative robot dynamic error verification method used in this invention;

[0069] Figure 13 for Figure 12 Comparison of dynamic errors in the X-axis of collaborative robot dynamic error verification method;

[0070] Figure 14 for Figure 12 Comparison of dynamic errors in the Y-axis of collaborative robot dynamic error verification method;

[0071] Figure 15 for Figure 12 Comparison of Z-axis dynamic error in collaborative robot dynamic error verification method.

[0072] Among them, 1. base; 2. joint one; 3. link one; 4. joint two; 5. link two; 6. joint three; 7. link three; 8. joint four; 9. link four; 10. joint five; 11. link five; 12. joint six; 13. link six; 14. joint seven; 15. link seven. Detailed Implementation

[0073] The invention will now be described in further detail with reference to the accompanying drawings.

[0074] like Figures 1-15 As shown, the present invention includes the following steps:

[0075] Step 1: Establish the kinematic model of the collaborative robot using the M-DH method;

[0076] This step is specifically as follows:

[0077] Step 1.1: As Figure 2 As shown, the collaborative robot includes, from bottom to top, a base 1, joint 1 2, link 1 3, joint 2 4, link 2 5, joint 3 6, link 3 7, joint 4 8, link 4 9, joint 5 10, link 5 11, joint 6 12, link 6 13, joint 7 14, and link 7 15.

[0078] like Figure 3 As shown, find the rotation axis of each joint. The intersection of the axes i and i+1 of two adjacent joints is the origin Oi of coordinate system {i}. Define the axis i of the rotation joint as the zi axis of coordinate system {i}, and the normal of the plane formed by zi and zi+1 is the xi axis of coordinate system {i}. Then, determine the direction of the yi axis by the right-hand rule of Cartesian coordinate system. It is stipulated that the coordinate system {0} of base 1 and the coordinate system {1} of link are in the same direction. Establish n coordinate systems according to the above rules.

[0079] Step 1.2: As Figures 3-4 As shown, a coordinate system is fixed to each joint of the robot. The link coordinate system {i} is relative to the coordinate system {i-1} through the link length a. i-1 Linkage twist angle α i-1 Linkage offset d i With joint angle θ i The four parameters are related; therefore, the transformation matrix of coordinate system {i} relative to coordinate system {i-1} is:

[0080]

[0081] Step 1.3: As Figures 3-4 As shown, by multiplying the matrices of adjacent joints, the pose of the robot's end effector in the coordinate system of base 1 can be obtained:

[0082]

[0083] The robot M-DH parameter table in this embodiment is shown in Table 1 below:

[0084] Table 1. Parameters of Robot M-DH

[0085]

[0086] Step 2: Considering the influence of robot weight, inertia, and load on joint torques, establish a dynamic model of the collaborative robot based on the Newton-Euler method to obtain the joint driving torques during robot operation;

[0087] This step is specifically as follows:

[0088] Step 2.1: Before establishing the robot dynamics model, use 3D modeling software to extract the mass attributes of each joint of the robot, including mass, inertia tensor, and center of mass position, with the link coordinate system as the output coordinate system.

[0089] Step 2.2: When establishing the robot's dynamic model, first calculate the velocity and acceleration of each link sequentially from the base 1 towards the end link:

[0090]

[0091]

[0092]

[0093] in, i+1 ω i+1 This represents the angular velocity of link i+1 in the link coordinate system {i+1}; Let {i+1} be the rotation transformation matrix of coordinate system {i} in coordinate system {i}; Indicates joint angular velocity; i+1 z i+1 The unit vector representing the z-axis of coordinate system {i+1}; This represents the angular acceleration of link i+1 in coordinate system {i+1}; Indicates joint angular acceleration; This represents the linear acceleration of link i+1 in coordinate system {i+1}; i p i+1 This represents the position vector of the origin of coordinate system {i+1} within coordinate system {i}.

[0094] Step 2.3: Calculate the inertial force and moment of inertia of each link using the Newton-Euler formula:

[0095] i+1 f c(i+1) =m i+1 + i+1 ω i+1 ×(m i+1 i+1 v c(i+1) )

[0096]

[0097] in, i+1 f c(i+1) The inertial force of link i+1 in coordinate system {i+1} represents the force exerted by link i+1 in coordinate system {i+1}; m i+1 This represents the mass of link i+1; i+1 v c(i+1) This represents the acceleration of the center of mass of link i+1 in coordinate system {i+1}; i+1 τc(i+1) This represents the moment of inertia of link i+1 in coordinate system {i+1}; c(i+1) I i+1 This represents the inertia tensor at the center of mass of link i+1;

[0098] Step 2.4: Starting from the end link and proceeding sequentially to base 1, calculate the forces and torques of the interaction between each link and the joint driving torque:

[0099]

[0100]

[0101] τ i =N i T · i z i ;

[0102] Among them, F i N represents the force acting on link i. i τ represents the torque acting on link i; i This represents the driving torque of joint i.

[0103] Step 3: Establish a dynamic error model for the end effector of the collaborative robot based on the relationship between dynamic force and joint torsional stiffness, and consider the influence of supplementary stiffness on the dynamic error of the end effector;

[0104] This step is specifically as follows:

[0105] Step 3.1: Describe all joints as torsion springs. The dynamic error model of the robot's end effector due to joint flexibility can be expressed as:

[0106] AD = K -1 ·F;

[0107] Where ΔD is the end deformation, K is the Cartesian stiffness matrix, and F is the end generalized force.

[0108] According to Cartesian stiffness and joint torsional stiffness K θ Relationship:

[0109] K = J -T ·K θ ·J -1 ;

[0110] It can be seen that the robot's Cartesian stiffness matrix is ​​related not only to the joint torsional stiffness matrix but also to the Jacobian matrix J. Since dynamic error modeling targets highly dynamic processes, changes in the Jacobian matrix have a significant impact on stiffness. To improve modeling accuracy, a supplementary stiffness matrix is ​​introduced.

[0111] Step 3.2: The relationship between the generalized force at the robot's end effector and the joint driving torque is as follows:

[0112] Γ=J T ·F;

[0113] Where Γ is the joint driving torque, Γ=[τ1,τ2,...,τ7] T J is the robot Jacobian matrix, with a dimension of 6×n.

[0114] Differentiating the above equation, we get:

[0115]

[0116] After simplification, we get:

[0117]

[0118] Let matrix The relationship between supplementary stiffness and Cartesian stiffness is then:

[0119] K = J -T ·(K θ -K c )·J -1 ;

[0120] Among them, K c It is a supplementary stiffness matrix.

[0121] Therefore, after considering the added stiffness, the dynamic error model of the robot's end effector can be expressed as:

[0122] ΔD=J·(K θ -K c ) -1 ·Γ.

[0123] Step 4: Convert the end-effector dynamic error to joint space and perform dynamic error compensation for the collaborative robot.

[0124] This step is specifically as follows:

[0125] Step 4.1: Convert the end-effector dynamic error, which takes into account the supplementary stiffness, to the joint space:

[0126] Δθ=J -1 ·ΔD;

[0127] Step 4.2: Compensate for the dynamic error of the robot's end effector by correcting the robot's joint variables. The compensated joint angles are:

[0128] θc=θ a +Δθ;

[0129] Where, θ c It is the compensated joint angle, θa It is the actual joint angle.

[0130] Step 5: Verify the compensated dynamic error of the collaborative robot's end effector and guide the robot's actions.

[0131] like Figure 12 As shown, this invention also provides a method for verifying the dynamic error of a collaborative robot, which includes the following steps: 1) Establishing a 3D model of the robot using 3D modeling software, simplifying complex irregular parts and repetitive small components in the model; 2) Importing the model into multi-rigid-body dynamics simulation software, adding material properties for each robot component, setting constraints and applying drive, and then performing dynamics simulation. The robot's flexible joints can be represented by torsion springs. In the dynamics simulation software, torsion springs are added at the rotary joints, and appropriate joint stiffness and damping are set to replace the original rigid kinematic pairs. The simulation values ​​are compared with theoretical values. For example, the simulation results of the driving torque of joints one through seven are compared... Figures 5-11 As shown, the dynamic error of the robot's end effector is also observed to verify the theoretical analysis results. The dynamic errors of the robot's end effector in the X, Y, and Z directions are compared. Figures 13-15 As shown, all errors are within the allowable range.

Claims

1. A method for dynamic error modeling and compensation for a collaborative robot, the method comprising: Comprise the following steps: Step one: use M-DH method to establish the kinematics model of collaborative robot; Step two: based on Newton-Euler formula to calculate the inertia force and inertia moment of each link: i+1 f c(i+1) = m i+1 + i+1 ω i+1 × (m i+1 i+1 v c(i+1) ) wherein i+1 f c(i+1) denotes the inertial force of the link i+1 in the coordinate system {i+1}; m i+1 denotes the mass of the link i+1; i+ 1 v c(i+1) denotes the center of mass acceleration of the link i+1 in the coordinate system {i+1}; i+1 τ c(i+1) denotes the inertial torque of the link i+1 in the coordinate system {i+1}; c(i+1) I i+1 denotes the inertia tensor at the center of mass of the link i+1, i+1 ω i+1 denotes the angular velocity of the link i+1 in the link coordinate system {i+1}, denotes the angular acceleration of the link i+1 in the coordinate system {i+1}; From the robot end link in turn to the base, the calculation of each link interaction force and torque and joint driving torque: τ i = N i T · i z i ; where F i represents the force acting on the link i; N i represents the torque acting on the link i; τ i represents the driving torque of the joint i, i p i+1 represents the position vector of the origin of the coordinate system {i+1} in the coordinate system {i}, represents the rotation transformation matrix of the coordinate system {i+1} under the coordinate system {i}; Step three: describe all joints as torsion springs and consider supplemental stiffness K c After that, the robot end dynamic error model is obtained: ΔD = J · (K θ - K c ) -1 · Γ; where ΔD is the robot end effector deformation, K θ is the joint twist stiffness matrix, Γ is the joint driving force, J is the robot Jacobian matrix, K c is the supplemental stiffness matrix; This step is specifically: Step 3.1: all joints are described as torsional springs, the dynamic error model of the robot end due to joint flexibility is expressed as: ΔD = K -1 • F; Where, ΔD is the end deformation, K is the Cartesian stiffness matrix, F is the end generalized force. According to the relationship between the Cartesian stiffness and the joint torsional stiffness K θ is obtained: K = J -T • K θ • J -1 ; Step 3.2: the relationship between the robot end generalized force and joint driving torque is: Γ = J T • F; Differential of the above formula is: After simplification: Let the matrix The supplemental stiffness K c The relationship with the Cartesian stiffness K is: K = J -T • (K θ - K c ) • J -1 ; So consider supplementing stiffness K c After that, the robot end dynamic error model is expressed as: ΔD = J · (K θ - K c ) -1 · Γ; Step four: convert the end dynamic error to joint space, and compensate for the dynamic error of the collaborative robot; Step five: simulation verification is carried out on the compensated end dynamic error of the collaborative robot, and the simulation value is compared with the theoretical value to guide the robot action.

2. The collaborative robot dynamic error modeling and compensation method of claim 1, wherein: Step one is specifically: Step 1.1: find the rotation axis of each joint, the intersection of the adjacent two joint axes i and i+1 is defined as the origin Oi of the coordinate system{i}, the axis i of the rotating joint is defined as the Zi axis of the coordinate system{i}, and the normal of the plane formed by Zi and Zi+1 is defined as the Xi axis of the coordinate system{i}, then the direction of yi axis is determined through the right-hand rule of Cartesian coordinate system, and the base coordinate system{0} is defined as the same direction as the link coordinate system{1}, and the above rules are used to establish n coordinate systems respectively; Step 1.2: A coordinate frame is attached to each joint of the robot, the link coordinate frame {i} is related to the coordinate frame {i-1} by the link length a i-1 , the link twist angle a i-1 , the link offset d i and the joint angle θ i . Thus, the transformation matrix of the coordinate frame {i} with respect to the coordinate frame {i-1} is Step 1.3: through the multiplication of each adjacent joint matrix, the pose of the robot end in the base coordinate system is obtained.

3. The collaborative robot dynamic error modeling and compensation method of claim 1, wherein: In step two, first use three-dimensional modeling software to extract the mass properties of each joint of the robot, including mass, inertia tensor and center of mass position, and then recursively calculate the velocity and acceleration of each link from the base to the end link: wherein i+1 ω i+1 denotes the angular velocity of the link i+1 in the link coordinate system {i+1}; denotes the rotation transformation matrix of the coordinate system {i+1} in the coordinate system {i}; denotes the joint angular velocity; i+1 z i+1 denotes the unit vector of the z-axis of the coordinate system {i+1}; denotes the angular acceleration of the link i+1 in the coordinate system {i+1}; denotes the joint angular acceleration; denotes the linear acceleration of the link i+1 in the coordinate system {i+1}; i p i+1 denotes the position vector of the origin of the coordinate system {i+1} in the coordinate system {i}; Then calculate the inertia force and inertia moment of each link by Newton-Euler formula.

4. The collaborative robot dynamic error modeling and compensation method of claim 1, wherein: Step four is specifically: Step 4.1: convert the end dynamic error considering the additional stiffness to the joint space: Δθ = J -1 • ΔD; Step 4.2: by modifying the robot joint variables to achieve the purpose of compensating for the dynamic error of the robot end, the compensated joint angle is: θ c = θ a + Δθ: where θ c is the compensated joint angle, and θ α is the actual joint angle.

5. The collaborative robot dynamic error modeling and compensation method of claim 1, wherein: Step five is specifically: use three-dimensional modeling software to establish a three-dimensional model of the robot, simplify the model, import the model into the dynamics simulation software, add material properties of each part of the robot and set constraints to apply driving, and then perform dynamics simulation, in which a torsional spring is added at the rotating joint in the dynamics simulation software and the joint stiffness and damping are set to replace the original rigid joint, finally the simulation value is compared with the theoretical value.

Images