An inverse kinematics solution method for an oblique six-joint robot

Abstract

A method for solving inverse kinematics of an oblique six-joint robot, according to the robot inverse kinematics equation, calculates the joint angle increment from the angles of each joint axis at the current position and the target posture matrix; according to the obtained joint angle increment, the joint angle increment after the damping factor is introduced is calculated from the Jacobian matrix and the damping factor; the joint angle increment after the damping factor is introduced is used to obtain the current joint angle from the multiplication factor and the previous joint angle. The present invention can solve the problem that the traditional iterative method cannot meet the requirements of accuracy and speed at the same time when solving inverse kinematics.

Description

Technical Field

[0001] The invention belongs to the technical field of robot motion control, relates to the field of inverse kinematics of robots, and particularly relates to an inverse kinematics solving method for an oblique angle six-joint robot. Background Art

[0002] With the development of robot technology, the application scope of the oblique six-joint robot is becoming wider and wider, but its inverse kinematics solution is still a big problem. For robots that do not meet the Pieper criterion, numerical iteration is usually used to solve inverse kinematics. There are two commonly used iterative algorithms. One is intelligent algorithms such as neural networks and simulated annealing, but the computational efficiency of these algorithms has yet to be verified in engineering; the other is the Gaussian iteration method. The traditional Gaussian iteration method has two problems when solving inverse kinematics. One is that the iterative algorithm has a large amount of computation, which makes it difficult for the robot to meet the real-time requirements of the industrial field. The second is that the speed and accuracy of the solution cannot be achieved at the same time. If you want to increase the speed of the solution, you must reduce the accuracy requirements, and if you want to increase the stability, you must reduce the speed requirements. Summary of the invention

[0003] In order to overcome the shortcomings of the above-mentioned prior art, the purpose of the present invention is to provide an inverse kinematics solution method for an oblique six-joint robot, so as to solve the problem that the traditional inverse kinematics solution using iterative method cannot meet the requirements of accuracy and speed at the same time.

[0004] In order to achieve the above object, the technical solution adopted by the present invention is:

[0005] An inverse kinematics solution method for an oblique six-joint robot comprises the following steps:

[0006] Step 1, according to the robot inverse kinematics equation, the joint angle increment is calculated from the angles of each joint axis at the current position and the target posture matrix;

[0007] Step 2, according to the obtained joint angle increment, the joint angle increment after introducing the damping factor is calculated by using the Jacobian matrix and the damping factor;

[0008] Step 3, using the joint angle increment after the damping factor is introduced, the current joint angle is obtained by multiplying the factor and the previous joint angle.

[0009] Compared with the prior art, the present invention reduces the time required for iterative solution by constructing the differential motion increment formula and introducing dynamic multiplication factors based on the Gaussian damping iteration method to solve inverse kinematics; introduces the damping factor in the process of solving the inverse matrix of the Jacobian matrix, avoiding the problem of too large inverse matrix value when solving the inverse matrix of the Jacobian matrix by the SVD decomposition method; selects the optimal solution by the singular region avoidance algorithm, reducing the frequency of singularity in the robot. The present invention is suitable for angled robots such as spraying and welding, and is more practical than traditional methods for solving inverse kinematics, meeting the real-time and high-precision requirements required by the industrial field. BRIEF DESCRIPTION OF THE DRAWINGS

[0010] Figure 1 It is the algorithm flow chart.

[0011] Figure 2 It is a three-dimensional trajectory diagram.

[0012] Figure 3 It is a graph of joint angle changes. DETAILED DESCRIPTION

[0013] The embodiments of the present invention are described in detail below with reference to the accompanying drawings and examples.

[0014] like Figure 1 As shown, a method for solving inverse kinematics of an oblique six-joint robot is characterized by comprising the following steps:

[0015] Step 1: According to the inverse kinematics equation of the robot, the joint angle increment is calculated from the angles of each joint axis at the current position and the target posture matrix.

[0016] In some embodiments of the present invention, step 1 can be specifically described as the following process:

[0017] S1, from the differential motion increment and Jacobian matrix from the starting point to the target point, the robot's joint angle increment formula can be obtained as follows:

[0018] [Δθ1,Δθ2,Δθ3,Δθ4,Δθ5,Δθ6] T =J -1 ·[d x d y d z k x k y k z ] T

[0019] Wherein, Δθ is the joint angle increment, specifically: Δθ1 is the joint angle increment of the first joint of the robot, Δθ2 is the joint angle increment of the second joint of the robot, Δθ3 is the joint angle increment of the third joint of the robot, Δθ4 is the joint angle increment of the fourth joint of the robot, Δθ5 is the joint angle increment of the fifth joint of the robot, and Δθ6 is the joint angle increment of the sixth joint of the robot. -1 is the inverse matrix of the Jacobian matrix; [d x d y d z k x k y k z ] T Indicates the differential motion increment from the current position to the target point.

[0020] S2, the differential motion increment in S1 [d x d y d z k x k y k z ] T It includes position increment and attitude increment, which are constructed by differential operator. The differential operator formula is as follows:

[0021]

[0022] In the formula, Δ is the differential operator, T cur is the current pose matrix, T end is the target pose matrix, I is the fourth-order identity matrix, [d x ,d y ,d z ] is the position increment, [k x ,k y ,k z ] is the attitude increment.

[0023] S3, when the current pose matrix T in S2 cur When it is singular or nearly singular, a fixed value can be used to replace the determinant used in solving its inverse matrix to speed up the calculation. The optimization formula is as follows:

[0024]

[0025] In the formula, det(T cur ) is the determinant of the current pose matrix before modification, δ is a small constant whose value range is 0 to 0.0001, and det(T) is the determinant of the current pose matrix after modification.

[0026] Step 2: According to the obtained joint angle increment, the joint angle increment after introducing the damping factor is calculated by the Jacobian matrix and the damping factor.

[0027] In some embodiments of the present invention, the calculation of the joint angle increment formula after the introduction of the damping factor requires the inverse matrix of the Jacobian matrix to be obtained. In this process, the damping factor is introduced to obtain the incremental formula of the joint angle for each iteration of the calculation process, so as to prevent the value of the pseudo-inverse matrix of the Jacobian matrix J in step 1 from being too large, thereby improving the comprehensive performance of the inverse kinematics algorithm. The process is as follows:

[0028] S1, when the Jacobian matrix J is singular, there is no inverse matrix. The pseudo-inverse matrix of the Jacobian matrix is ​​solved by SVD decomposition. The SVD decomposition of the Jacobian matrix is ​​as follows:

[0029] J=USV T =U·diag(s0,s1,s2,s3,s4,s5)·V T

[0030] Where U and V are the left and right singular matrices of J, S is a diagonal matrix, s0, s1, s2, s3, s4, s5 are the singular values ​​corresponding to the six joint axes, and s0 ≥ s1 ≥ s2 ≥ s3 ≥ s4 ≥ s5 ≥ 0;

[0031] S2. The three matrices obtained by SVD decomposition can construct the pseudo-inverse matrix of the original Jacobian matrix J, and the expression is as follows:

[0032] J + =VS -1 U T

[0033] S3. After the damping factor is introduced, the joint angle increment of each iteration of the calculation process is as follows:

[0034] Δθ=(J T J+η 2 I) -1 J T D

[0035] Where D is the differential motion increment, D = [d x d y d z k x k y k z ] T , eta is the damping factor, Δθ = [Δθ1, Δθ2, Δθ3, Δθ4, Δθ5, Δθ6] T .

[0036] In the embodiment of the present invention, the damping factor η is constructed as follows:

[0037]

[0038] Where η max is the maximum value of the damping factor, 0.09, i is the singular value after SVD decomposition, s is the adjustment number of the singular value, and the construction formula can be expressed as follows:

[0039]

[0040] Where, β = 0.1, s f =0.1,s z =2.0,s i s0, s1, s2, s3, s4, and s5 are the singular values ​​corresponding to the six joint axes.

[0041] Step 3, using the joint angle increment after the damping factor is introduced, the updated current joint angle is obtained by multiplying the factor and the existing joint angle.

[0042] In some embodiments of the present invention, step 3 can be specifically described as the following process:

[0043] S1, speeds up the algorithm by introducing a multiplication factor.

[0044] Set the maximum error allowed for the working situation. When the error exceeds the set value, the iteration speed needs to be accelerated to make the algorithm converge, and the multiplication factor should be increased. When the error approaches the set value, the multiplication factor should be gradually reduced.

[0045] At the same time, it is known that the joint rotation increment calculated in the singular region is relatively large, so the value of the multiplication factor should also be related to the singular condition number of the Jacobian matrix. Set the maximum value of the multiplication factor n max , the minimum value is n min , its construction formula is as follows:

[0046]

[0047] In the formula, err is the error, n is the introduced multiplication factor;

[0048] S2, substitute the multiplication factor n in S1 into the joint angle increment formula Δθ=(J T J+η 2 I) -1 J T After D, the joint angle update formula is as follows:

[0049] θ newCur =θ cur +nΔθ

[0050] In the formula, θ cur is the current joint angle before updating, θ newCuris the updated current joint angle, n is the multiplication factor, and Δθ is the change in joint angle. In the embodiment of the present invention, the maximum value of the multiplication factor n is set max =57.5, minimum value n min =1.

[0051] According to the above method, the present invention reduces the time required for iterative solution by constructing the formula of differential motion increment and introducing dynamic multiplication factors on the basis of solving inverse kinematics by Gaussian damping iteration method; introduces damping factors in the process of solving the inverse matrix of the Jacobian matrix, thereby avoiding the problem of excessive inverse matrix values ​​when solving the inverse matrix of the Jacobian matrix by SVD decomposition method; selects the optimal solution by singular region avoidance algorithm, thereby reducing the frequency of singularity of the robot.

[0052] In a specific embodiment of the present invention, continue to refer to Figure 1 , including the following steps:

[0053] 1. Through the DH parameter table of the robot body structure, the inverse kinematics equation of the six-joint robot after optimizing the differential motion is obtained.

[0054] Specifically, the inverse kinematics of the robot refers to solving the angles of each joint axis through the end position, which is the inverse process of forward kinematics. cur 、Target pose matrix T end And the DH parameter table of the robot body structure, the inverse kinematics equation can be derived through forward kinematics, the process is as follows:

[0055] 1. Determine the angles of each joint axis at the current position, that is, the current joint angle θ cur , target pose matrix T end ;

[0056] 2. Obtain the current pose matrix T from the angles of each joint axis at the current position cur , the process is as follows:

[0057] 1) From the DH parameter table of the robot body structure, the homogeneous transformation matrix between adjacent links is obtained The formula is as follows:

[0058]

[0059] In the formula, s i = sin(θ),c i =cos(θ),u i = sin(α),v i =cos(α), θ and α are DH parameters.

[0060] 2) Each joint of the six-joint robot adopts a serial design, so the current posture matrix T can be obtained by multiplying the coordinate transformation matrices of each adjacent link in sequence. cur , the formula is as follows:

[0061]

[0062] In the formula, is the coordinate change matrix of the sixth joint axis relative to the base, is the coordinate change matrix of the first joint axis relative to the base, is the coordinate change matrix of the second joint axis relative to the first joint axis, is the coordinate change matrix of the third joint axis relative to the second joint axis, is the coordinate change matrix of the fourth joint axis relative to the third joint axis, is the coordinate change matrix of the fifth joint axis relative to the fourth joint axis, is the coordinate change matrix of the sixth joint axis relative to the fifth joint axis, n x 、n y 、n z , o x , o y , o z 、a x 、a y 、a z 、p x 、p y 、p z It is the pose information obtained after substituting DH parameters.

[0063] 3) The current pose matrix T obtained in step 2) cur , find its inverse matrix, the formula is as follows:

[0064]

[0065] In the formula, the expression of the last column element is as follows:

[0066] f(1)=a x (o y p z -o z p y )-o x (a y p z -a z p y )-p x (o y a z -o z a y )

[0067] f(2)=n x (a y p z -a z p y )-a x (n y p z -n z p y )+p x (n y a z -n z a y )

[0068] f(3)=o x (n y p z -n z p y )-n x (o y p z -o z p y )-p x 9n y 0 z -n z o y )

[0069] det(T cur )=n x (o y a z -o z a y )-o x (n y a z -n z a y )-a x (n y o z -n z o y )

[0070] Where n x 、n y 、n z , o x , o y , o z 、a x 、a y 、a z 、p x 、p y 、p z It is the pose information obtained after substituting DH parameters.

[0071] 4) When the current pose matrix T in step 3) cur When it is singular or close to singular, a fixed value is used instead to speed up the operation. Its optimization formula is the same as the previous det(T) formula.

[0072] 5) Through the inverse matrix of the current pose matrix and the known target pose matrix T end , find the differential operator Δ and the differential motion increment D, where the differential operator formula is the same as the previous Δ formula.

[0073] 6) The inverse kinematics equation of the six-joint robot can be obtained through the pseudo-inverse matrix of the Jacobian matrix J and the differential motion increment D in step 5), that is, the joint angle increment formula of the robot in the above step 1.

[0074] 2. When solving the inverse kinematics equation of the six-joint robot, it is necessary to obtain the inverse matrix of the Jacobian matrix. In this process, the damping factor is introduced to obtain the incremental formula of the joint angle for each iteration.

[0075] Specifically, in the process of deriving the inverse kinematics equation of the six-joint robot in step 6), the inverse matrix of the Jacobian matrix is ​​used. When the Jacobian matrix is ​​singular, there is no inverse matrix. At this time, the pseudo-inverse matrix of the Jacobian matrix needs to be solved by SVD decomposition. When the value of the pseudo-inverse matrix of the Jacobian matrix J is too large, the robot terminal trajectory fluctuates back and forth. By introducing a damping factor, the value of the pseudo-inverse matrix of the Jacobian matrix J can be prevented from being too large, making the robot motion smooth. The specific process is as follows:

[0076] 1) When the Jacobian matrix J is singular, there is no inverse matrix. The pseudo-inverse matrix of the Jacobian matrix is ​​solved by the aforementioned SVD decomposition method.

[0077] 2) The three matrices obtained by SVD decomposition can construct the pseudo-inverse matrix J of the original Jacobian matrix J + , the expression is as described above.

[0078] 3) After the damping factor is introduced, the expression of the increment Δθ of the joint angle in each iteration is as described above.

[0079] 4) Construct the damping factor η in the above manner.

[0080] 5) Construct the adjustment number s of the singular values ​​in the above manner.

[0081] 3. Based on the joint angle increment formula in step 2, a multiplication factor is introduced to obtain the joint angle update formula after the multiplication factor is introduced;

[0082] Specifically, the algorithm is accelerated by introducing a multiplication factor. When the error is too large, the iteration speed needs to be accelerated to make the algorithm converge. At this time, the multiplication factor should be increased. When the error tends to the set threshold, the multiplication factor should be gradually reduced. At the same time, it is known that the joint angle increment calculated in the singular region is relatively large, so the value of the multiplication factor should also be related to the singular condition number of the Jacobian matrix. The maximum value of the multiplication factor is set to 57.5, and the minimum value is 1. The construction formula is as follows:

[0083]

[0084] In the formula, err is the error, n is the introduced multiplication factor;

[0085] Substituting the multiplication factor n into the joint angle increment formula in step 2, the joint angle update formula is obtained.

[0086] This embodiment selects a common six-joint spraying robot as the object, first calculate the current joint angle θ cur , if the current joint angle θ cur If it is not in the singular area, compare whether the current joint angle error is less than 0.0001mm. If it is less than 0.0001mm, it means that the calculation result meets the requirements, otherwise continue to calculate through the damping method; if it is in the singular area, the SVD decomposition method of the Jacobian matrix is ​​used to obtain its left singular matrix U, right singular matrix V and the singular values ​​corresponding to each joint axis. The pseudo-inverse matrix of the Jacobian matrix is ​​constructed through the left and right singular matrices, and the damping factor is obtained through the singular values ​​corresponding to each joint axis. The joint angle increment formula after the damping factor is introduced is obtained, and the formula is as follows:

[0087]

[0088] It is determined whether the error of the current joint angle calculated by the above formula is less than 0.0001mm. If it is greater than 0.0001mm, the joint angle is calculated again by multiplying the factor until the error meets the requirement. At this point, the joint angle calculation of one interpolation cycle is completed.

[0089] The parameter table of the six-joint spraying robot DH used in this implementation case is shown in the following table:

[0090] Connecting rod <![CDATA[a i / mm]]> <![CDATA[α i / °]]> <![CDATA[d i / mm]]> <![CDATA[θ i / °]]> <![CDATA[θ i Scope]]> 1 135 90 0 <![CDATA[θ1]]> -150～150 2 700 0 0 <![CDATA[θ2]]> -60～110 3 100 90 0 <![CDATA[θ3]]> -80～90 4 0 60 433 <![CDATA[θ4]]> -720～720 5 0 -60 115.473 <![CDATA[θ5]]> -720～720 6 0 0 174.205 <![CDATA[θ6]]> -410～410

[0091] The input starting point joint angle is [40, -17, 52, 170, -30, 115], the target point joint angle is [31, 81, -48, -99, 93, -28], and the Cartesian space poses corresponding to the starting point and the target point are also known. Through the above steps, we get Figure 2The three-dimensional trajectory diagram shown in the figure and the angle change diagram of each joint axis are as follows Figure 3 As shown in the results, it can be found that the joint angle changes smoothly, the trajectory tracking error is small, and it has good stability and accuracy in the continuous solution of the trajectory.

[0092] In an existing or new oblique six-joint robot, its control unit is configured to run the inverse kinematics solution method of the present invention to obtain the current joint angle, and its execution unit is configured to perform trajectory motion according to the current joint angle, thereby realizing the operation control of the oblique six-joint robot.

[0093] The above are only specific embodiments of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can easily think of various equivalent modifications or replacements within the technical scope disclosed by the present invention, and these modifications or replacements should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention shall be based on the protection scope of the claims.

Claims

1. A method for solving inverse kinematics of an oblique six-joint robot, characterized in that: The steps include: Step 1, according to the inverse kinematics equation of the robot, the joint angle increment is calculated from the angles of each joint axis at the current position and the target posture matrix; Step 2, according to the obtained joint angle increment, the joint angle increment after introducing the damping factor is calculated by using the Jacobian matrix and the damping factor; Step 3, using the joint angle increment after the damping factor is introduced, the updated current joint angle is obtained by multiplying the factor and the existing joint angle; In step 1, the robot's joint angle increment formula is obtained from the differential motion increment from the starting point to the target point and the Jacobian matrix as follows: [Δθ1,Δθ2,Δθ3,Δθ4,Δθ5,Δθ6] T =J -1 ·[d x d y d z k x k y k z ] T Where Δθ1 is the joint angle increment of the first joint of the robot, Δθ2 is the joint angle increment of the second joint of the robot, Δθ3 is the joint angle increment of the third joint of the robot, Δθ4 is the joint angle increment of the fourth joint of the robot, Δθ5 is the joint angle increment of the fifth joint of the robot, Δθ6 is the joint angle increment of the sixth joint of the robot, J -1 is the inverse matrix of the Jacobian matrix; [d x d y d z k x k y k z ] T It represents the differential motion increment from the current position to the target point, including the position increment and attitude increment, which is constructed by the following differential operator: In the formula, Δ is the differential operator, T cur is the current pose matrix, T end is the target pose matrix, I is the fourth-order identity matrix, [d x ,d y ,d z ] is the position increment, [k x ,k y ,k z ] is the posture increment; When the current pose matrix T cur When it is singular or nearly singular, a fixed value is used to replace the determinant used in solving its inverse matrix to speed up the calculation. The optimization formula is as follows: In the formula, det(T cur ) is the determinant of the current pose matrix before modification, δ is a constant whose value range is 0 to 0.0001, and det(T) is the determinant of the current pose matrix after modification.

2. The inverse kinematics solution method of the oblique angle six-joint robot according to claim 1 is characterized in that: In step 2, the calculation process of the joint angle increment after introducing the damping factor is as follows: S1. When the Jacobian matrix J is singular, there is no inverse matrix. The pseudo-inverse matrix of the Jacobian matrix is ​​solved by SVD decomposition. The formula is as follows: J=USV T =U diag(s0,s1,s2,s3,s4,s5) V T Where U and V are the left and right singular matrices of J, S is a diagonal matrix, s0, s1, s2, s3, s4, s5 are the singular values ​​corresponding to the six joint axes, and s0 ≥ s1 ≥ s2 ≥ s3 ≥ s4 ≥ s5 ≥ 0; S2. Construct the pseudo-inverse matrix of the Jacobian matrix J. The formula is as follows: J + =VS -1 U T S3. After the damping factor is introduced, the joint angle increment formula for each iteration of the calculation process is as follows: Δθ=(J T J+h 2 I) -1 J T D In the formula, Δθ = [Δθ1, Δθ2, Δθ3, Δθ4, Δθ5, Δθ6] T ,D=[d x d y d z k x k y k z ] T , the resistance factor.

3. The inverse kinematics solution method of the oblique angle six-joint robot according to claim 2, characterized in that: The damping factor η is constructed as follows: Where η max The maximum value of the damping factor is 0.09, s i are the singular values ​​after SVD decomposition, i.e., s0, s1, s2, s3, s4, s5, where s is the adjustment number of the singular value.

4. The inverse kinematics solution method of the oblique angle six-joint robot according to claim 3 is characterized in that: The construction formula of the adjustment number s of the singular value is as follows: Where, β = 0.1, s f =0.1,s z =2.

0.

5. The inverse kinematics solution method of the oblique six-joint robot according to claim 2, characterized in that: Set the maximum value of the multiplication factor n max and the minimum value n min , the construction formula is as follows: In the formula, err is the error, n is the introduced multiplication factor; After substituting the multiplication factor n into the joint angle increment formula in step 2, the joint angle update formula is as follows: i newCur =θ cur +nΔθ In the formula, θ cur is the current joint angle before updating, θ newCur is the updated current joint angle, n is the multiplication factor, and Δθ is the change in joint angle.

6. The inverse kinematics solution method of the oblique angle six-joint robot according to claim 5, characterized in that: Set the maximum value of the multiplication factor n max =57.5, minimum value n min =1.

7. The inverse kinematics solution method of the oblique six-joint robot according to claim 1, characterized in that: The step 3 sets the maximum error allowed by the working conditions. When the error exceeds the set value, the multiplication factor is increased. When the error approaches the set value, the multiplication factor is gradually reduced.

8. An oblique six-joint robot, comprising a control unit and an execution unit, characterized in that: The control unit executes the inverse kinematics solution method of the oblique six-joint robot as described in any one of claims 1 to 7 to obtain the current joint angle; the execution unit performs trajectory movement according to the current joint angle.

Images