Reverse solution rapid calculation method for SCARA robot with PRRR configuration

Abstract

The invention discloses an inverse solution rapid calculation method for an SCARA robot of a PRRR configuration. Belongs to the field of robots. The SCARA robot comprises a shaft 1, a shaft 2, a shaft 3 and a shaft 4 which are connected with one another; the method comprises the following operation steps: step 1, modeling by using a standard DH method (SDH), and determining DH parameters (theta, d, a and alpha) of each joint; 2, a robot tail end pose matrix is known, and an algebraic analysis method is used for solving a set of formulas for solving the angles or positions of all the shafts; 3, due to the fact that + / -number, arc tangent, arc cosine and other formulas are used, multiple groups of solutions exist; q3 is determined according to the set posture of the left arm or the right arm; and related variables in the matrix are selected, and the solved key angle value is verified to quickly determine q2 and q4. The method has the characteristics of high operation speed, accurate calculation result and the like, can meet the requirements of real-time operation and control of the robot, and can be applied to a real-time control system of the robot.

Description

Technical Field

[0001] The invention belongs to the field of robots and relates to a fast inverse solution calculation method for a SCARA robot of PRRR configuration. Background Art

[0002] In the field of industrial robots, SCARA four-axis robots are widely used, mainly for assembly, handling and positioning tasks. SCARA robotic arms are widely used in automated production lines and industrial applications. They can effectively replace manual labor and improve production efficiency and quality. At the same time, due to its simple structure, stability and reliability, it is relatively easy to maintain and operate.

[0003] There are two commonly used inverse solution methods: numerical method and analytical method. The analytical method can be divided into geometric analysis and algebraic analysis. In the currently available information, there are few studies on the inverse solution of the SCARA robot with PRRR configuration. A Chinese patent (patent number: CN201811233506.1) discloses an inverse solution method for a four-axis SCARA robot, which is an inverse solution method for a SCARA robot with RRRP configuration. However, its disadvantage is that the method is not applicable to PRRR. Moreover, it uses a geometric analysis method. Although the analysis process is relatively intuitive, its applicability is limited. Summary of the invention

[0004] In view of the above problems, the purpose of the present invention is to propose a fast calculation method for the inverse solution of a SCARA robot with a PRRR configuration. The calculation speed is fast, the calculation result is highly accurate, and the algorithm operation and deployment efficiency can be improved.

[0005] The technical solution of the present invention is: a fast inverse solution calculation method of a SCARA robot of PRRR configuration described in the present invention comprises the following steps:

[0006] Step (1), use the standard DH method (SDH) to build a model and determine the DH parameters (theta, d, a, alpha) of each joint;

[0007] Step (2), given the robot end position matrix, use algebraic analytical methods to solve the formulas for the angles or positions of each axis;

[0008] Step (3), since the formula in step (2) uses the ± sign and the inverse tangent and inverse cosine formulas, there will be multiple sets of solutions;

[0009] Determine q3 according to the set left arm or right arm posture;

[0010] Select the relevant variables in the matrix and verify the required key angle values ​​to quickly determine q2 and q4.

[0011] Furthermore, in step (1), the DH parameters (theta, d, a, alpha) of the SCARA robot with PRRR configuration are joint 1: (0, d1, a1, 0); joint 2: (q2, 0, a2, 0); joint 3: (q3, 0, a3, π); joint 4: (q4, 0, 0, 0);

[0012] Among them, d1, q2, q3, and q4 are the unknown quantities to be solved; d1 is the height of axis 1, and q2, q3, and q4 are the joint angles of axes 2, 3, and 4, respectively.

[0013] Furthermore, in step (2), the algebraic analysis method is used as follows:

[0014] d1:d1=q z

[0015]

[0016] Where k1 = -3a*, k2 = a3*cos(q3)+a2, k3 = q x -a1

[0017] q4:q4=q2+q3-arccos(u x ).

[0019] Furthermore, in step (3), the method for quickly selecting the correct result is:

[0020] Determine q3 according to the left or right arm posture you set. In many programming languages, the range of arccos return values ​​is from 0 to π. If it is a right arm posture, select a positive value, which is in the range of 0 to π. If it is a left arm posture, negate the value of arccos return value, which is in the range of 0 to -π.

[0021] In the formula for calculating q2, due to the presence of ± signs, first use + to calculate the angle of q2, substitute the q2 value into the formula y2 = a3*sin(q2+q3)+a2*sin(q2), and then add the y2 value to q y Compare the values. If they match, the value is correct. Otherwise, remove the - sign in the formula for q2 and solve it again.

[0022] In the formula for calculating q4, the range of arccos return values ​​is 0 to π. Substitute the obtained q4 result into the formula y4 = sin (q2 + q3 - q4), and then compare the y4 value with u y The values ​​are compared. If they are consistent, the value is correct. Otherwise, use the formula q4 = q2 + q3 + arccos (u x ) to get the correct q4 value.

[0023] The beneficial effects of the present invention are as follows: 1. The present invention takes into account the offset between the vertical linear axis of the first axis and the ground. By using the method of the present invention, the center of the robot base can be directly selected as the origin of the coordinate system; 2. A selection method is given when there are multiple solutions, which can quickly give the correct result; 3. The calculation accuracy is high, and the error is less than 10 -13 ; 4. Fast computing speed. According to actual tests, on an Intel i5-8500 processor, the average computing cycle is about 10us, which can meet the real-time computing and control needs of the robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] Figure 1 1 is a device structure diagram of a SCARA robot of PRRR configuration in an embodiment of the present invention;

[0025] In the figure, 1 is axis 1, 2 is axis 2, 3 is axis 3, and 4 is axis 4. DETAILED DESCRIPTION

[0026] The specific technical scheme of the present invention is further described in detail below with reference to specific examples.

[0027] A SCARA robot of PRRR configuration, wherein the first axis is a linear axis (axis 1), driving the robot arm to realize vertical movement, the second axis is a rotation axis (axis 2), also called a shoulder joint, the third axis is a rotation axis (axis 3), which is an elbow joint, and the fourth axis is a rotation axis (axis 4), which is a wrist joint;

[0028] Based on this robot, the standard DH method (SDH) is used to model and determine the DH parameters (theta, d, a, alpha) of each joint.

[0029] joint theta d a alpha 1 0 d1 a1 0 2 q2 0 a2 0 3 q3 0 a3 π 4 q4 0 0 0

[0030] According to the DH parameter table, the transformation matrix of each link of the robot can be established;

[0031]

[0032] The transformation matrix from the robot coordinate origin to the end is:

[0033]

[0034] Among them, the terminal pose matrix is

[0035]

[0036] The inverse solution is the process of solving the length (linear axis) or angle (rotational axis) of each joint when the end pose matrix is ​​known;

[0037] Transforming this transformation, we get the equation:

[0038]

[0039] Expanding this equation yields:

[0040]

[0041] According to the formula:

[0042] q z -d1=0

[0043] Get q z Solution formula:

[0044] q z =d1

[0045] according to:

[0046] q x -a1=a3*cos(q2+q3)+a2*cos(q2)

[0047] Simplifying the sum of the squares on both sides, we can get the solution formula for q3:

[0048]

[0049] Substitute the obtained q3 result into the formula:

[0050] q x -a1=a3*cos(q2+q3)+a2*cos(q2)

[0051] Get the solution formula for q2:

[0052]

[0053] in:

[0054] k1=-a3*sin(q3)

[0055] k2=a3*cos(q3)+a2

[0056] k3=q x -a1

[0057] according to:

[0058] u x =cos(q2+q3-q4)

[0059] Substituting the results of q2 and q3 into the formula, we can get q4:

[0060] q4=q2+q3+arccos(u x )

[0061] Since the ± sign and formulas such as inverse tangent and inverse cosine are used in the above formula, there will be multiple sets of solutions; select the relevant variables in the matrix and verify the required key angle values ​​to quickly select the correct result.

[0062] First, in the q3 solution formula, q3 can be determined according to the preset left arm or right arm posture. The range of arccos value is 0 to π. If it is the right arm posture, select a positive value, ranging from 0 to π; if it is the left arm posture, the value of the arccos return value is negated, ranging from 0 to -π;

[0063] In the formula for solving q2, due to the existence of ± signs, first use + to find the angle of q2, substitute the q2 value into the formula y2 = a3*sin(q2+q3)+a2*sin(q2), and then compare the y2 value with q y Compare the values. If they match, the value is correct. Otherwise, remove the - sign in the formula for q2 and solve it again.

[0064] In the formula for calculating q4, the range of arccos return values ​​is 0 to π. Substitute the obtained q4 result into the formula y4 = sin (q2 + q3 - q4), and then compare the y4 value with u y The values ​​are compared. If they are consistent, the value is correct. Otherwise, use the formula q4 = q2 + q3 + arccos (u x ) to get the correct q4 value.

Claims

1. A fast inverse solution calculation method for a SCARA robot with a PRRR configuration, characterized in that: The SCARA robot comprises an axis 1, an axis 2, an axis 3 and an axis 4 which are connected to each other; The axis 1 is a linear axis, and the axes 2, 3 and 4 are all rotation axes.

2. The inverse solution fast calculation method of a SCARA robot with a PRRR configuration according to claim 1 is characterized in that: The operation steps are as follows: Step (1): Use standard DH modeling to determine the DH parameters (theta, d, a, alpha) of each joint; Step (2): Given the robot end pose matrix, use algebraic analytical methods to solve the formulas for the angles or positions of each axis; Step (3): Since the formula in step (2) uses the ± sign and formulas such as arc tangent and arc cosine, there will be multiple sets of solutions; determine q3 according to the set left arm or right arm posture; Select the relevant variables in the matrix and verify the required key angle values ​​to select the correct results q2 and q4.

3. The inverse solution fast calculation method of a SCARA robot with a PRRR configuration according to claim 2 is characterized in that: In step (1), the DH parameters (theta, d, a, alpha) of the SCARA robot of the PRRR configuration are respectively: joint 1: (0, d1, a1, 0); joint 2: (q2, 0, a2, 0); joint 3: (q3, 0, a3, π); joint 4: (q4, 0, 0, 0); Among them, d1, q2, q3, and q4 are the unknown quantities to be solved; d1 is the height of axis 1, and q2, q3, and q4 are the joint angles of axes 2, 3, and 4, respectively.

4. The inverse solution fast calculation method of a SCARA robot with a PRRR configuration according to claim 3 is characterized in that: According to the DH parameter table, the transformation matrix of each link of the SCARA robot can be established:

5. The inverse solution fast calculation method of a SCARA robot with a PRRR configuration according to claim 4 is characterized in that: The transformation matrix from the SCARA robot coordinate origin to the end is: Among them, the terminal pose matrix is: The inverse solution is the process of solving the length or angle of each joint when the end pose matrix is ​​known.

6. The inverse solution fast calculation method of a SCARA robot with a PRRR configuration according to claim 5 is characterized in that: Transforming this transformation, we get the equation: Expanding this equation yields: According to the formula: what z -d1=0 Get q z Solution formula: what z =d1 according to: <h2 style=";text-align:left;direction:ltr">q<h2 style=";text-align:left;direction:ltr"> x <h2 style=";text-align:left;direction:ltr"> -a1 = a3*cos(q2+q3)+a2*cos(q2) Simplifying the sum of the squares on both sides, we can get the solution formula for q3: Substitute the obtained q3 result into the formula: <h2 style=";text-align:left;direction:ltr">q<h2 style=";text-align:left;direction:ltr"> x <h2 style=";text-align:left;direction:ltr"> -a1 = a3*cos(q2+q3)+a2*cos(q2) Get the solution formula for q2: in: k1=-a3*sin(q3) k2=a3*cos(q3)+a2 k3=q x -a1 according to: her x =cos(q2+q3-q4) Substituting the results of q2 and q3 into the formula, we can get q4: q4=q2+q3+arccos(u x )。 7. The inverse solution fast calculation method of a SCARA robot with a PRRR configuration according to claim 2 is characterized in that, in step (2), the algebraic analytical method is specifically shown as follows: d1: d1=q z q3： q2： <h2 style=";text-align:left;direction:ltr">k1 = a3*sin(q3)+k2 = a3*cos(q3)+a2+k3+q<h2 style=";text-align:left;direction:ltr"> x <h2 style=";text-align:left;direction:ltr"> -a1 q4:q4=q2+q3-arccos(u x )。 8. The inverse solution fast calculation method of a SCARA robot with a PRRR configuration according to claim 2 is characterized in that: In step (3), the method for selecting the correct result is: In the q3 solution formula, q3 is determined according to the set left arm or right arm posture. In many programming languages, the range of arccos return value is 0 to π. If it is the right arm posture, select a positive value, ranging from 0 to π; if it is the left arm posture, negate the value of arccos return value, ranging from 0 to -π.

9. The inverse solution fast calculation method of a SCARA robot with a PRRR configuration according to claim 3 is characterized in that: In the solution formula for q2, due to the existence of ± signs, first use + to find the angle of q2, substitute the q2 value into the formula y2 = a3*sin(q2+q3)+a2*sin(q2), and then add the y2 value to q y Compare the values. If they are consistent, the value is correct. Otherwise, use the minus sign in the formula for q2 to solve it again.

10. The method for fast inverse calculation of a SCARA robot with a PRRR configuration according to claim 2, characterized in that: In the formula for calculating q4, the range of arccos return values ​​is 0 to π. Substitute the obtained q4 result into the formula y4 = sin (q2 + q3 - q4), and then compare the y4 value with u y The values ​​are compared. If they are consistent, the value is correct. Otherwise, use the formula q4 = q2 + q3 + arccos (u x ) to get the correct q4 value.

Images