Robot end effector trajectory prediction method

Abstract

The invention particularly relates to a track prediction method for an end effector of a robot. The track prediction method comprises the following steps: calculating joint angle deviation related to a reverse error; calculating the sum of joint angular offsets related to the joint stiffness, the gravity and the balancing device; calculating a joint angle deviation related to the proportional error; and calculating actual track point coordinates of the end effector of the robot based on the joint angular deviation related to the reverse error, the sum of the joint angular deviations related to the joint rigidity, the gravity and the balancing device, and the joint angular deviation related to the proportional error. The method comprehensively considers the influence of a reverse error, a proportional error, joint rigidity, gravity and a balancing device on the trajectory of the end effector, describes the relationship among the above-mentioned components through a mathematical expression, and expands a trajectory prediction method which is simplified in documents and does not consider all factors into a trajectory prediction method which comprehensively considers all factors. Therefore, the actual track of the end effector of the robot under various positions, directions and loads can be accurately predicted.

Description

Technical Field

[0001] The present invention relates to the technical field of trajectory prediction, and in particular to a method for predicting the trajectory of the end effector of a robot, and more particularly to a method for predicting the actual trajectory of the end effector of an industrial robot in space by considering various factors. Background Art

[0002] During the robot milling process, since the robot has a serial structure and the joint stiffness is relatively low, under the influence of factors such as backlash, gravity, balance device, and scale error, the trajectory of its end effector will deviate from its theoretical trajectory, and the deviation amount and deviation direction will change with the change of the robot posture and load. This limits the user of the robot to optimize the trajectory control and error compensation. Therefore, it is of great theoretical and engineering application value to propose a trajectory prediction method that can comprehensively consider various factors and characterize their influence on the trajectory of the end effector.

[0003] The literature "Qizhi Chen, Chengrui Zhang, Tianliang Hu, et al. Posture optimization in robotic machining based on comprehensive deformation index considering spindle weight and cutting force[J]. Robotics and Computer-Integrated Manufacturing, 2022, 74: 102290." discloses a method for predicting the trajectory of a robot by comprehensively considering the weight of the end effector and the joint stiffness. Based on the classical stiffness model, this method derives a new comprehensive error index to characterize the influence of the robot joint stiffness and the weight of the end effector on its trajectory under different milling forces. However, the weight of the end effector only accounts for a small part of the total weight of the robot. This method ignores the influence of the robot link whose weight is much greater than that of the end effector and the balance device used to balance the torque of gravity acting on joint 2 on the trajectory. At the same time, the influence of backlash and scale error is not considered either, which limits the accuracy of this method.

[0004] The literature "Peng Xu, Xiling Yao, Shibo Liu, et al. Stiffness modeling of an industrial robot with a gravity compensator considering link weights[J]. Mechanism and machine theory, 2021, 161: 104331." discloses a method for accurately calculating the forces and deformations of various components of a robot, and then predicting the milling trajectory of the final end effector. Its disadvantages are as follows: The established model only considers the factors that affect the offset of the end effector under the action of forces and torques, such as joint stiffness, weight, and balancing devices, without considering robot errors, such as backlash error and scale error.

[0005] The characteristics of the above reference are as follows: During the prediction process, not all sources of the trajectory offset of the end effector are fully considered, but a selection is made to establish a simplified model, which is difficult to accurately predict the actual trajectory of the robot end effector under various positions, orientations, and loads. Naturally, it is also difficult to accurately guide subsequent control optimization and error compensation.

[0006] It should be noted that the information disclosed in the above background art section is only used to enhance the understanding of the background of the present invention, and thus may include information that does not constitute the prior art known to those of ordinary skill in the art. Summary of the Invention

[0007] To overcome the problem that the existing prediction method does not fully consider all error sources and thus cannot accurately predict the actual trajectory of the end effector, the present invention proposes a prediction method that comprehensively considers various influencing factors during the movement of the robot and is used to predict the actual trajectory of the robot end effector under various positions, orientations, and loads.

[0008] Other features and advantages of the present invention will become apparent from the following detailed description, or be learned in part through the practice of the present invention.

[0009] According to a first aspect of the present invention, there is provided a method for predicting the trajectory of a robot end effector, the method comprising:

[0010] Calculating the joint angle offset Δθ related to the backlash error j,r ;

[0011] Calculating the sum of joint angle offsets Δθ related to joint stiffness, gravity, and balancing devices j,f ;

[0012] Calculating the joint angle deviation Δθ related to the scale error j,s;

[0013] Based on Δθ j,r , Δθ j,f and Δθ j,s calculate the actual trajectory point coordinates of the robot end effector.

[0014] In some exemplary embodiments, calculating the joint angle offset Δθ j,r related to the reverse error, using the following formula:

[0015] Δθ j,r = -q j ⊙δθ r

[0016] And there is:

[0017] q j = [q 1,j q 2,j q 3,j q 4,j q 5,j q 6,j T

[0018] δθ r = [δθ 1,r δθ 2,r δθ 3,r δθ 4,r δθ 5,r δθ 6,r T

[0019] where, q 1,j to q 6,j represent the rotation directions of each joint, ⊙ is the Hadamard multiplication, indicating the multiplication of the corresponding elements of the left and right column vectors, and δθ 1,r to δθ 6,r represent the magnitudes of the backlash of each joint.

[0020] In some exemplary embodiments, the method further includes:

[0021] Calculate the rotation directions of each joint at each path point on the trajectory, and construct the elements in q j in the way that a positive rotation is recorded as 1 and a reverse rotation is recorded as -1.

[0022] In some exemplary embodiments, calculating the sum of joint angle offsets Δθ j,f related to joint stiffness, gravity, and the balance device, using the following formula:

[0023]

[0024] And there is:​​

[0025] M j,l =-{m2g[H′2(θ j,t )] T Δθ j,f +m 3,4 g[H′ 3,4 (θ j,t )] T Δθ j,f +m 5,6 g[H′ 5,6 (θ j,t )] T} T

[0026] M j,s =[0 0 -m e g 0 0 0] T

[0027] M j,c =[0 k 0 0 0 0] T

[0028]

[0029] K = diag(K1 K2 K3 K4 K5 K6)

[0030] where m2, m 3,4 and m 5,6 represent the masses of link groups 2, 3, and 4 respectively; g represents the acceleration due to gravity; H′2(θ j,t ), H′ 3,4 (θ j,t ) and H′ 5,6 (θ j,t ) represent the derivatives of the z - coordinates of the center of gravity of the second joint, the overall center of gravity of the third and fourth joints, and the overall center of gravity of the fifth and sixth joints of the robot in the world coordinate system with respect to the theoretical joint angle θ j,t of the robot; K represents the joint stiffness of the robot, where K i represents the stiffness of the i - th joint of the robot, diag(*) represents the diagonalization operation, J j represents the Jacobian matrix of the robot, M j,l , M j,s , M j,c and k are intermediate variables; a, b, c, = and γ are all dimensions related to the geometric parameters of the balancing device; θ 2,j,t is the theoretical angle of the second joint of the robot; F j,c is the magnitude of the output tension of the balancing device.

[0031] In some exemplary embodiments, the magnitude of the output tension F of the balancing devicej,c , is calculated using the following formula:

[0032]

[0033] where p0 represents the pressure of the gas inside the balancing device in the initial state, V0 represents the volume of the gas inside the balancing device in the initial state, S represents the piston area inside the balancing device, l0 represents the length of the balancing device in the initial state, and l j represents the current length of the balancing device.

[0034] In some exemplary embodiments, the joint angle deviation Δθ related to the proportional error j,s , is calculated using the following formula:

[0035] Δθ j,s = U⊙(θ j,t - θ 0,t )

[0036] And there is:

[0037] U = [U1 U2 U3 U4 U5 U6] T

[0038] where U i represents the ratio by which the rotation angle of the i-th joint is amplified or reduced. When the joint over-rotates, U i is greater than 0. When the joint under-rotates, U i is less than 0; θ 0,t represents the joint angle corresponding to the origin of the trajectory.

[0039] In some exemplary embodiments, the actual trajectory point coordinates of the robot end effector are calculated based on Δθ j,r , Δθ j,f and Δθ j,s using the following formula:

[0040] θ j = θ j,t + Δθ j,r + Δθ j,f + Δθ j,s

[0041] T EE,j = FK(θ j )

[0042] O j = [T EE,j (1,4) T EE,j (2,4) T EE,j (3,4)] T

[0043] where θj,t represents the theoretical joint angle of the robot, θ j represents the actual angles of the joints of the robot. FK(*) represents the forward kinematic transformation of the robot, O j represents the actual trajectory point of the end effector of the robot. T EE,j represents the coordinate transformation matrix from the coordinate system of the end effector of the robot to the world coordinate system, T EE,j (1,4), T EE,j (2,4) and T EE,j (3,4) respectively represent the elements at the 1st, 2nd, 3rd row and the 4th column of the matrix T EE,j at the elements.

[0044] In some exemplary embodiments, the method further includes:

[0045] compensating the trajectory of the end effector of the robot based on the prediction result to improve the machining accuracy and surface quality.

[0046] According to a second aspect of the present invention, there is provided a storage medium having stored thereon a computer program, which when executed by a processor implements the method for predicting the trajectory of the end effector of the robot according to the first aspect above.

[0047] According to a third aspect of the present invention, there is provided a computer program product having stored thereon a computer program, which when executed by a processor implements the method for predicting the trajectory of the end effector of the robot according to the first aspect above.

[0048] According to a fourth aspect of the present invention, there is provided an electronic device, including:

[0049] a processor; and

[0050] a memory for storing executable instructions of the processor;

[0051] wherein the processor is configured to implement the method for predicting the trajectory of the end effector of the robot according to the first aspect above when executing the executable instructions.

[0052] The method for predicting the trajectory of the end effector of the robot provided by the embodiments of the present invention comprehensively considers the influences of reverse error, scale error, joint stiffness, gravity, and balance device on the trajectory of the end effector, and describes the relationship between them through mathematical expressions, expanding the trajectory prediction method in the literature that has been simplified and does not consider all factors into a trajectory prediction method that comprehensively considers all factors, and thus can accurately predict the actual trajectory of the end effector of the robot at various positions, orientations, and loads. Compared with the given literature, the present invention comprehensively considers multiple factors and effectively improves the accuracy of predicting the trajectory of the end effector of the robot.

[0053] It should be understood that the above general description and the following detailed description are merely exemplary and explanatory, and should not limit the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] The accompanying drawings herein are incorporated into and constitute a part of this specification, showing embodiments consistent with the present invention, and are used together with the specification to explain the principles of the present invention. Obviously, the accompanying drawings in the following description are only some embodiments of the present invention, and those of ordinary skill in the art can obtain other drawings based on these drawings without creative efforts.

[0055] Figure 1 It is a schematic diagram of the process of a method for predicting the trajectory of a robot end effector according to an embodiment of the present invention;

[0056] Figure 2 It shows a comparison between the actual trajectory of the robot end effector under a circular target trajectory and the prediction results of the method of the present invention and the method of the given literature;

[0057] Figure 3 It is a comparison between the uncompensated path and the trajectory after offline compensation based on the prediction results of the given method;

[0058] Figure 4 It is a surface photo of a cylindrical workpiece milled by the uncompensated path and the compensated path. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0059] Example embodiments will now be described more fully with reference to the accompanying drawings. However, the example embodiments can be implemented in various forms and should not be construed as limited to the examples set forth herein; rather, these embodiments are provided so that the present invention will be more complete and comprehensive, and will fully convey the concept of the example embodiments to those skilled in the art. The features, structures, or characteristics described can be combined in any suitable manner in one or more embodiments.

[0060] In addition, the accompanying drawings are only schematic illustrations of the present invention and are not necessarily drawn to scale. The same reference numerals in the drawings denote the same or similar parts, and thus repeated descriptions thereof will be omitted. Some of the block diagrams shown in the drawings are functional entities and do not necessarily correspond to physically or logically independent entities. These functional entities can be implemented in software form, or in one or more hardware modules or integrated circuits, or in different networks and / or processor devices and / or microcontroller devices.

[0061] The present invention proposes a prediction method that comprehensively considers various influencing factors during the movement of a robot and is used to predict the actual trajectory of the end effector of the robot at various positions, orientations, and loads. This method first analyzes the action mechanism of the error sources; then, based on this, kinematic and mechanical models are established, and the influence of them on the angular offsets of each joint of the robot is deduced; finally, through forward kinematic transformation, the accurate actual trajectory of the end effector of the robot is obtained.

[0062] Expected technical effect: The trajectory prediction method for the end effector of the robot provided by the present invention can comprehensively consider the influence of joint stiffness, gravity, balancing device, proportional error, and backlash error on the trajectory of the end effector under different positions, orientations, and loads, and accurately predict it.

[0063] The technical solution adopted by the present invention to solve the above problems is: A trajectory prediction method for the end effector of a robot, as Figure 1 shown, includes the following steps:

[0064] Step 1: Calculate the joint angular offset Δθ related to the backlash error using the following formula j,r

[0065] Δθ j,r = -q j ⊙δθ r

[0066] And there is:

[0067] q j = [q 1,j q 2,j q 3,j q 4,j q 5,j q 6,j T

[0068] δθ r = [δθ 1,r δθ 2,r δθ 3,r δθ 4,r δθ 5,r δθ 6,r T

[0069] Wherein, q 1,j to q 6,j represent the rotation directions of each joint, "1" represents forward rotation, and "-1" represents reverse rotation. ⊙ is the Hadamard product, indicating that the corresponding elements of the left and right column vectors are multiplied, and δθ 1,r to δθ 6,r represent the magnitudes of the backlash of each joint.

[0070] ​​Step 2: Calculate the sum of joint angle offsets Δθ related to joint stiffness, gravity, and the balancing device using the following formula j,f .

[0071]

[0072] And we have:

[0073] M j,l = -{m2g[H′2(θ j,t )] T Δθ j,f + m 3,4 g[H′ 3,4 (θ j,t )] T Δθ j,f + m 5,6 g[H′ 5,6 (θ j,t )] T} T

[0074] M j,s = [0 0 -m e g 0 0 0] T

[0075] M j,c = [0 k 0 0 0 0] T

[0076]

[0077] K = diag(K1 K2 K3 K4 K5 K6)

[0078] where, m2, m 3,4 and m 5,6 represent the masses of link groups 2, 3, and 4 respectively. g represents the acceleration due to gravity. H2(θ j,t ), H3′ ,4 (θ j,t ) and H5′ ,6 (θ j,t ) represent the derivatives of the z - coordinates of the center of gravity of the second joint, the overall center of gravity of the third and fourth joints, and the overall center of gravity of the fifth and sixth joints of the robot in the world coordinate system with respect to the theoretical joint angle θ j,t of the robot. K represents the joint stiffness of the robot, where K i represents the stiffness of the i - th joint of the robot, diag(*) represents the diagonalization operation, J j represents the Jacobian matrix of the robot, M j,l , M j,s , M j,ck is an intermediate variable. a, b, c, β, and γ are all dimensions related to the geometric parameters of the balancing device. θ 2,j,t is the theoretical angle of the second joint of the robot. F j,c is the output tensile force of the balancing device, calculated by the following formula:

[0079]

[0080] and H2(θ j,t ), H 3,4 (θ j,t ), and H 5,6 (θ j,t )'s primitive functions are respectively expressed as:

[0081] H2(θ j,t ) = L2 - L g,2 sinθ 2,t

[0082] H 3,4 (θ j,t ) = L2 + L4cos(θ 2,t + θ 3,t ) - L3sinθ2 - L g,3,4 sin(θ 2,t + θ 3,t )

[0083]

[0084] Among them, p0 represents the pressure of the gas inside the balancing device in the initial state. V0 represents the volume of the gas inside the balancing device in the initial state. S represents the piston area inside the balancing device. l0 represents the length of the balancing device in the initial state. l j represents the current length of the balancing device. θ i,t (i = 1, 2, 3, 4, 5, 6) represents the i-th element in the θ j,t matrix. L2, L4, and L5 respectively represent the lengths of the 2nd, 4th, and 5th links of the robot. L g,2 , L g,3,4 , and L g,5,6 respectively represent the distances from the center of gravity of the 2nd joint, the overall center of gravity of the 3rd and 4th joints, and the overall center of gravity of the 5th and 6th joints to the joints 2, 3, and 5.

[0085] Step 3: Calculate the joint angle deviation Δθ related to the proportional error using the following formula j,s

[0086] Δθ j,s = U⊙(θ j,t - θ 0,t )

[0087] And there is:

[0088] U = [U1 U2 U3 U4 U5 U6] T

[0089] Among them, U i represents the ratio by which the rotational angle of the i-th joint is magnified or reduced. When the joint undergoes over-rotation, U i is greater than 0. When the joint undergoes under-rotation, U i is less than 0. θ 0,t represents the joint angle corresponding to the origin of the trajectory.

[0090] Step Four: Calculate the coordinates of the actual trajectory points of the robot end effector using the following formula.

[0091] θ j = θ j,t + Δθ j,r + Δθ j,f + Δθ j,s

[0092] T EE,j = FK(θ j )

[0093] O j = [T EE,j (1,4) T EE,j (2,4) T EE,j (3,4)] T

[0094] Among them, θ j represents the actual angles of each joint of the robot. FK(*) represents the forward kinematic transformation of the robot. O j represents the actual trajectory points of the robot end effector. T EE,j represents the coordinate transformation matrix from the coordinate system of the robot end effector to the world coordinate system. T EE,j (1,4), T EE,j (2,4) and T EE,j (3,4) respectively represent the elements at the 1st, 2nd, and 3rd rows, and the 4th column of the matrix T EE,j .

[0095] Next, each step of the trajectory prediction method for the robot end effector in this exemplary embodiment will be described in more detail with reference to the accompanying drawings and embodiments.

[0096] Embodiment 1

[0097] Step One: Query the manufacturer's manual of the used robot to obtain the D-H parameters, link mass parameters, and balance device geometric parameters.

[0098] The three types of parameters of the robot used in this example are shown in Tables 1, 2, and 3 respectively.

[0099] Table 1 D-H parameters of the robot used in the examples

[0100]

[0101]

[0102] Table 2 Link mass parameters

[0103]

[0104] Table 3 Geometric parameters of the balancing device

[0105]

[0106] Step 2: Apply external forces with varying magnitudes and directions to the end effector of the robot, and use a ballbar to measure the results before and after the application of the forces to calibrate the hysteresis δθ, stiffness K, and proportional error parameter U of each joint of the robot. i,r , stiffness K i and proportional error parameter U i .

[0107] The calibration results are shown in Table 4.

[0108] Table 4 Hysteresis, stiffness, and proportional error parameters of each joint of the robot

[0109]

[0110]

[0111] Step 3: First, calculate the rotational directions of each joint at each path point on the trajectory, and construct the elements in q according to the method where forward rotation is denoted as 1 and reverse rotation is denoted as -1. Then, calculate the joint angle offset Δθ related to the reverse error using the following formula. j in, and then calculate the joint angle offset Δθ related to the reverse error using the following formula. j,r

[0112] Δθ j,r =-q j ⊙δθ r

[0113] And there is:

[0114] q j =[q 1,j q 2,j q 3,j q 4,j q 5,j q 6,j T

[0115] δθ r =[δθ 1,r δθ 2,r δθ​3,r δθ 4,r δθ 5,r δθ 6,r T

[0116] where q 1,j to q 6,j represent the rotation directions of each joint, "1" represents forward rotation, and "-1" represents reverse rotation. ⊙ is the Hadamard multiplication, indicating the multiplication of corresponding elements of the left and right column vectors, and δθ 1,r to δθ 6,r represent the magnitudes of the backlash of each joint.

[0117] Step 4. Calculate the sum of joint angle offsets Δθ related to joint stiffness, gravity, and the balancing device using the following formula j,f .

[0118]

[0119] And there is:

[0120] M j,l = -{m2g[H′2(θ j,t )] T Δθ j,f + m 3,4 g[H′ 3,4 (θ j,t )] T Δθ j,f + m 5,6 g[H′ 5,6 (θ j,t )] T} T

[0121] M j,s = [0 0 -m e g 0 0 0] T

[0122] M j,c = [0 k 0 0 0 0] T

[0123]

[0124] K = diag(K1 K2 K3 K4 K5 K6)

[0125] where the calculation methods of F j,c , H2(θ j,t ), H 3,4 (θ j,t ), and H 5,6 (θ j,t ) are as follows:​

[0126]

[0127] H2(θ j,t ) = L2 - L g,2 sinθ 2,t

[0128] H 3,4 (θ j,t ) = L2 + L4cos(θ 2,t + θ 3,t ) - L3sinθ2 - L g,3,4 sin(θ 2,t + θ 3,t )

[0129]

[0130] Among them, m2, m 3,4 and m 5,6 respectively represent the masses of the connecting rod groups 2, 3, and 4. g represents the acceleration due to gravity. H′2(θ j,t ), H′ 3,4 (θ j,t ), and H′ 5,6 (θ j,t ) respectively represent the derivatives of the z - coordinates of the center of gravity of the second joint of the robot, the overall center of gravity of the third and fourth joints, and the overall center of gravity of the fifth and sixth joints in the world coordinate system with respect to the theoretical joint angle θ j,t of the robot. K represents the stiffness of each joint of the robot, where K i represents the stiffness of the i - th joint of the robot, diag(*) represents the diagonalization operation, J j represents the Jacobian matrix of the robot, M j,l , M j,s , M j,c and k are intermediate variables. a, b, c, β, and γ are all dimensions related to the geometric parameters of the balancing device. θ 2,j,t is the theoretical angle of the second joint of the robot. p0 represents the pressure of the gas inside the balancing device in the initial state. V0 represents the volume of the gas inside the balancing device in the initial state. S represents the area of the piston inside the balancing device. l0 represents the length of the balancing device in the initial state. l j represents the current length of the balancing device. θ i,t (i = 1, 2, 3, 4, 5, 6) represents the i - th element in the θ j,t matrix. L2, L4, and L5 respectively represent the lengths of the second, fourth, and fifth linkages of the robot. L g,2 , L g,3,4 , and L g,5,6respectively represent the distances from the center of gravity of the second joint, the overall centers of gravity of the third and fourth joints, and the overall centers of gravity of the fifth and sixth joints to joints 2, 3, and 5.

[0131] Step Five: Calculate the angles θ of each joint of the robot at the origin of the trajectory 0,t , and then calculate the joint angle deviation Δθ related to the proportional error using the following formula j,s .

[0132] Δθ j,s = U⊙(θ j,t - θ 0,t )

[0133] And there is:

[0134] U = [U1 U2 U3 U4 U5 U6] T

[0135] Among them, U i represents the ratio by which the rotation angle of the i-th joint is amplified or reduced. When the joint is over-rotated, U i is greater than 0, and when the joint is under-rotated, U i is less than 0. θ 0,t represents the joint angle corresponding to the origin of the trajectory.

[0136] Step Six: Integrate the Δθ obtained in Steps Three, Four, and Five j,r , Δθ j,f and Δθ j,s , and obtain the actual coordinates of the trajectory point of the robot's end effector in space through the following formula.

[0137] θ j = θ j,t + Δθ j,r + Δθ j,f + Δθ j,s

[0138] T EE,j = FK(θ j )

[0139] O j = [T EE,j (1,4) T EE,j (2,4) T EE,j (3,4)] T .

[0140] Among them, θ j represents the actual angles of each joint of the robot. FK(*) represents the forward kinematic transformation of the robot. O j represents the actual trajectory point of the robot's end effector. T EE,j represents the coordinate transformation matrix from the end effector coordinate system of the robot to the world coordinate system. TEE,j (1, 4) and T EE,j (2, 4) and T EE,j (3, 4) respectively represent the elements at the 1st, 2nd, and 3rd rows and the 4th column of matrix T EE,j of the matrix T

[0141] It can be seen that by replacing the method in the literature with the trajectory prediction method proposed in the present invention, the trajectory prediction accuracy is greatly improved. It can be seen that by Figure 2 It can be seen that by replacing the method in the literature with the trajectory prediction method proposed in the present invention, the trajectory prediction accuracy is greatly improved. It can be seen that by Figure 3 It can be seen that with the trajectory predicted by this method, the user of the robot can accurately compensate the trajectory of the end effector, thereby greatly improving its accuracy. It can be seen that by Figure 4 It can be seen that due to the adoption of the compensation method based on the method of the present invention, the accuracy and surface quality of the machined workpiece are significantly improved.

[0142] This shows that by comprehensively considering the effects of joint stiffness, gravity, balancing device, proportional error, and backlash error on the trajectory of the end effector at different positions, orientations, and loads, the trajectory prediction accuracy is effectively improved, and on this basis, the machining accuracy and surface quality of the workpiece are improved.

[0143] In addition, the above-mentioned drawings are only schematic illustrations of the processes included in the method according to the exemplary embodiments of the present invention, rather than for limiting purposes. It is easy to understand that the processes shown in the above-mentioned drawings do not indicate or limit the chronological order of these processes. Additionally, it is also easy to understand that these processes can be executed synchronously or asynchronously, for example, in multiple modules.

[0144] Those skilled in the art will readily think of other embodiments of the present invention after considering the specification and practicing the invention herein. This application is intended to cover any variations, uses, or adaptations of the present invention, which follow the general principles of the present invention and include well-known common general knowledge or conventional technical means in the technical field not disclosed in the present invention. The specification and embodiments are only regarded as exemplary, and the true scope and spirit of the present invention are pointed out by the claims.

[0145] It should be understood that the present invention is not limited to the exact structures described above and shown in the drawings, and various modifications and changes can be made without departing from its scope. The scope of the present invention is only defined by the appended claims.

Claims

1. A method for predicting the trajectory of a robot end effector, characterized in that, The method includes: Calculate the joint angle offset Δθ related to the reverse error j,r ; Calculate the sum of joint angle offsets Δθ related to joint stiffness, gravity, and the balancing device j,f ; Calculate the joint angle deviation Δθ related to the proportional error j,s ; Based on Δθ j,r 、Δθ j,f and Δθ j,s Calculate the actual trajectory point coordinates of the robot end effector.

2. The method according to claim 1, characterized in that, Calculating the joint angle offset Δθ related to the reverse error j,r , using the following formula: Δθ j,r =-q j ⊙δθ r And there is: q j = [q 1,j q 2,j q 3,j q 4,j q 5,j q 6,j T ​ δθ r = [δθ 1,r δθ 2,r δθ 3,r δθ 4,r δθ 5,r δθ 6,r T ​ where q 1,j to q 6,j represent the rotation directions of the respective joints, ⊙ is the Hadamard multiplication, indicating the multiplication of the corresponding elements of the left and right column vectors, and δθ 1,r to δθ 6,r represent the magnitudes of the backlash of the respective joints.

3. The method according to claim 2, wherein The method further includes: Calculate the rotation directions of each joint at each path point on the trajectory, and construct the elements of q according to the method of recording 1 for forward rotation and -1 for reverse rotation. j in.

4. The method according to claim 1, wherein Calculating the sum of joint angle offsets Δθ related to joint stiffness, gravity, and the balance device j,f , using the following formula: And there is: M j,l = - {m2g[H′2(θ j,t )] T Δθ j,f + m 3,4 g[H′ 3,4 (θ j,t )] T Δθ j,f + m 5,6 g[H′ 5,6 (θ j,t )] T} T M j,s = [0 0 -m e g 0 0 0] T M j,c = [0 k 0 0 0 0] T K = diag(K1 K2 K3 K4 K5 K6) Among them, m2, m 3,4 and m 5,6 respectively represent the masses of the connecting rod groups 2, 3, and 4; g represents the acceleration due to gravity; H′2(θ j,t ), H′ 3,4 (θ j,t ) and H′ 5,6 (θ j,t ) respectively represent the derivatives of the z - coordinates of the center of gravity of the second joint of the robot, the overall center of gravity of the third and fourth joints, and the overall center of gravity of the fifth and sixth joints in the world coordinate system with respect to the theoretical joint angle θ j,t of the robot; K represents the stiffness of each joint of the robot, where K i represents the stiffness of the i - th joint of the robot, diag(*) represents the diagonalization operation, J j represents the Jacobian matrix of the robot, M j,l , M j,s , M j,c and k are intermediate variables; a, b, c, β, and γ are all dimensions related to the geometric parameters of the balancing device; θ 2,j,t is the theoretical angle of the second joint of the robot; F j,c is the magnitude of the output tension of the balancing device.

5. The method according to claim 4, characterized in that, The output tensile force magnitude F of the balance device j,c , is calculated using the following formula: Among them, p0 represents the pressure of the gas inside the balancing device in the initial state, V0 represents the volume of the gas inside the balancing device in the initial state, S represents the piston area inside the balancing device, l0 represents the length of the balancing device in the initial state, and l j represents the current length of the balancing device.

6. The method according to claim 1, wherein Calculate the joint angle deviation Δθ related to the proportional error j,s , using the following formula: Δθ j,s = U⊙(θ j,t - θ 0,t ) And there is: U = [U1 U2 U3 U4 U5 U6] T Among them, U i represents the ratio by which the rotation angle of the i-th joint is magnified or reduced. When the joint is over-rotated, U i is greater than 0. When the joint is under-rotated, U i is less than 0; θ 0,t represents the joint angle corresponding to the origin of the trajectory.

7. The method according to claim 1, wherein The above-mentioned based on Δθ j,r , Δθ j,f and Δθ j,s To calculate the actual trajectory point coordinates of the end effector of the robot, use the following formula: θ j = θ j,t + Δθ j,r + Δθ j,f + Δθ j,s T EE,j = FK(θ j ) O j = [T EE,j (1,4) T EE,j (2,4) T EE,j (3,4)] T Among them, θ j,t represents the theoretical joint angle of the robot, and θ j represents the actual angle of each joint of the robot. FK(*) represents the forward kinematic transformation of the robot, and O j represents the actual trajectory point of the robot end effector. T EE,j represents the coordinate transformation matrix from the coordinate system of the robot end effector to the world coordinate system, and T EE,j (1, 4), T EE,j (2, 4), and T EE,j (3, 4) respectively represent the elements at the 1st, 2nd, and 3rd rows and the 4th column of the matrix T EE,j .

8. The method according to claim 1, wherein The method further includes: Compensating the trajectory of the robot end effector based on the prediction result to improve the machining accuracy and surface quality.

9. A storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the robot end effector trajectory prediction method according to any one of claims 1 to 8.

10. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by a processor, it implements the robot end effector trajectory prediction method according to any one of claims 1 to 8.

Images