Offset robot kinematics inverse solution method and device and medium

Abstract

The invention discloses an inverse kinematics solution method and device for a biased robot and a medium, and the method comprises the steps: firstly, carrying out the modeling of a biased robot as an unbiased robot, carrying out the inverse solution to obtain a joint angle approximate analytical solution, and setting the dynamic search range of an initial population and a particle swarm according to the approximate solution; taking the difference between the actual tail end pose and the tail end pose corresponding to the approximate solution as a fitness function, solving the minimum value of the fitness function, improving the adjustment mode of an inertia factor of the particle swarm algorithm, shortening the iterative calculation time, and finally solving an accurate numerical solution of the robot through particle swarm iteration to complete the inverse solution of the joint angle of the robot. According to the method, the inverse kinematics efficiency of the bias robot can be improved, and practicability is provided.

Description

Technical Field

[0001] The present application relates to the field of robotics, and in particular to a method, device and medium for inverse kinematics solution of a biased robot. Background Art

[0002] The inverse solution of industrial robot kinematics is the most important part of robot motion control. The accuracy and real-time performance of the inverse solution are key factors in ensuring that the robot moves according to instructions. Since the configuration of the offset robot does not meet the Pieper criterion and other conditions, it is impossible to separate the individual joint parameters to obtain an analytical solution. Numerical iteration is required to obtain an accurate joint solution. Therefore, how to accurately and quickly inversely solve the joint angles of the offset robot has become a difficult problem that needs to be overcome.

[0003] Chinese patent CN107685330A proposes a method for solving the inverse kinematics of a six-degree-of-freedom wrist-biased serial robot. This method uses the analytical solution of the inverse kinematics of a six-degree-of-freedom wrist-unbiased serial robot as the approximate solution and iterative starting point of the inverse kinematics of the six-degree-of-freedom wrist-biased serial robot, and continuously iterates and approximates through the Newton method to obtain a numerical solution of the inverse kinematics of the six-degree-of-freedom wrist-biased serial robot that meets the accuracy. In the article "Inverse Solution Algorithm of Wrist-Biased Six-Degree-of-Freedom Joint Robot Based on Improved Newton Iteration Method", Han Lei et al. proposed to use the LM algorithm for iterative solution based on the approximate solution. The LM algorithm adds a damping factor λ on the basis of Newton iteration, which reduces the probability of Newton's method falling into the local optimum, but at the same time reduces the iteration efficiency. This type of method for numerical solution based on Newton iteration requires multiple calculations of Jacobian matrix and inverse matrix, etc., which is computationally intensive, time-consuming, and has weak real-time performance.

[0004] In the article "Inverse Kinematics Analysis of 6R Offset Wrist Photography Robot", Ruan Deshan et al. proposed to first analyze the joints θ2, θ3, θ4, θ5, and θ6 into functions about θ1, determine the minimum value of θ1, the iteration step Δθ1, and the iteration accuracy ε. The accuracy ε is composed of the difference between the actual end position and the end position obtained by the current joint forward solution. The end position meets the accuracy requirements by successively superimposing the step size. In the process of finding the exact value, this method can only search globally in the overall motion space of θ1 step by step, and the calculation time is long due to too many iterations.

[0005] Therefore, the existing technology has the disadvantages of large amount of calculation and poor real-time performance when solving the inverse kinematics problem of the offset robot. Summary of the invention

[0006] The present application provides a method, device and medium for inverse kinematic solution of an offset robot, which has the advantage of improving the efficiency of inverse kinematic solution of the offset robot and providing practicality.

[0007] The technical solution of this application is as follows:

[0008] On the one hand, the present application provides a method for inverse kinematics solution of a biased robot, comprising the following steps:

[0009] Step A: Treat the biased robot as a non-biased robot and find an approximate solution;

[0010] Step B: Use the approximate solution to determine the initial value of the particle swarm, use the particle swarm algorithm to iterate and find the exact solution, improve the inertia factor parameter in the particle swarm algorithm, and establish a linear dynamic inertia factor parameter. The inertia factor parameter is larger in the early stage of the algorithm iteration than in the later stage of the iteration.

[0011] Furthermore, step A specifically includes the following steps:

[0012] Step 1: Establish the robot forward kinematics model as shown below:

[0013]

[0014] In formula (1), T i Represents the transformation relationship between the i-th coordinate system and the i-1-th coordinate system; θ i Represents the rotation z i-1 Axis, from x i-1 Go to x i Angle; d i Indicates that along z i-1 From x i-1 Move to x i The displacement, a i Indicates that along x i From z i-1 Move to z i The displacement of i Represents around x i Axis from z i-1 Rotate to z i The angle of i = cosθ i , sθ i = sinθ i , cα i =cosα i , sα i = sinα i ;

[0015] Step 2: Establish the kinematic link model of the offset robot;

[0016] Step 3: Establish a non-biased robot model, set the offset robot wrist offset d5 to zero, make the robot's four, five, and six axes intersect at the same point, satisfy the Pieper criterion, and ensure that there is no offset distance at the wrist;

[0017] Step 4: Find the approximate solution and inversely solve the joint approximate solutions θ1', θ2', θ3', θ4', θ5', θ6'.

[0018] Furthermore, step B specifically includes the following steps:

[0019] Step 5: Establish an improved particle swarm algorithm to iteratively find the exact solution. The mathematical expression is as follows:

[0020]

[0021] In formulas (2), (3), and (4), i = 1, 2, 3, ..., m, where m is the size of the particle swarm; d = 1, 2, 3, ..., D, where D is the particle dimension. In formula (2), g is the global extreme value, p is the individual optimal extreme value, k is the number of iterations, ω is the inertia weight factor, c1 and c2 are learning factors, both of which are constants, r1 and r2 are random numbers between [0, 1], and in formula (3) V represents the flying speed of the kth generation particle, and is also the basis for the position change of the k+1th generation particle. max,d represents the maximum flight speed of the dth particle in D dimension, where is the position of the k+1th generation particle;

[0022] Step 6: Improve the inertia factor parameters in the particle swarm algorithm and establish linear dynamic inertia factor parameters:

[0023]

[0024] In formula (5), ω max is the maximum inertia factor, ω min is the minimum inertia factor, T max is the maximum number of iterations, k is the iteration sequence number, ω k represents the inertia factor value of the kth iteration;

[0025] Step 7: Determine the fitness function fitness:

[0026] The fitness function value is used to determine whether the inverse solution meets the accuracy requirements. The smaller the value, the closer the inverse solution is to the accurate solution and the higher the accuracy. The approximate joint angles θ1', θ2', θ3', θ4', θ5', θ6' are combined with forward kinematics to calculate the approximate end position P'[X', Y', Z'] and Euler angle posture R'[α', β', γ']. The difference between the actual end position P and R is used to calculate the second norm and the minimum value. The formula is as follows:

[0027]

[0028] Furthermore, step B also includes step 8: determining the search range of the particle swarm, obtaining the deviation range between the precise joint angle corresponding to the robot posture in the robot motion space and the approximate solution, determining the range of the precise solution by comparing the approximate solution with the precise solution, and taking the range as the initial particle swarm range.

[0029] On the other hand, the present application provides a biased robot kinematics inverse solution device, including a processor and a memory, wherein the memory stores computer instructions, and the processor is configured to: call and execute the computer instructions to implement the biased robot kinematics inverse solution method as described above.

[0030] On the other hand, the present application provides a computer-readable medium, wherein the computer-readable medium stores computer instructions, and when the computer instructions are called and executed by a computer, the offset robot kinematic inverse solution method as described above is implemented.

[0031] In summary, the beneficial effects of this application are:

[0032] 1. The present invention is based on the approximate solution of unbiased robot joints, and uses the Monte Carlo method to evenly select multiple spatial points to calculate the approximate solution, determine the interval range of the exact solution, narrow the particle search range of the particle swarm algorithm, reduce the number of iterations, improve calculation efficiency, have strong real-time performance, and better meet the needs of actual applications.

[0033] 2. The present invention introduces a dynamic inertia factor on the basis of the basic particle swarm algorithm to overcome the shortcomings of local optimum and slow convergence speed brought by the fixed inertia factor, improve the efficiency of the inverse solution from the algorithm point of view, and further reduce the inverse solution time.

[0034] 3. The inverse solution algorithm of the present invention is simple and only requires single-dimensional numerical calculations. There is no need to perform high-dimensional complex calculations such as Jacobian matrix and inverse calculations. The performance requirements for the controller are not high, which can reduce costs. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 is a diagram of a robot connecting rod model in a specific embodiment of the present application;

[0036] Figure 2 It is an iterative solution diagram of an improved particle swarm algorithm in a specific embodiment of the present application;

[0037] Figure 3 This is a schematic diagram of a Monte Carlo method robot spatial point selection in a specific embodiment of the present application;

[0038] Figure 4 It is a diagram of the convergence process of iterative solution of the improved particle swarm algorithm in a specific embodiment of the present application. DETAILED DESCRIPTION

[0039] The specific implementation of the present application is described in detail below with reference to the accompanying drawings.

[0040] Example: A specific embodiment of the present application provides a method for inverse kinematics of a biased robot, referring to Figure 1-4 , including the following steps:

[0041] Step A: Treat the biased robot as a non-biased robot and find an approximate solution;

[0042] Step B: Use the approximate solution to determine the initial value of the particle swarm, use the particle swarm algorithm to iterate and find the exact solution, improve the inertia factor parameter in the particle swarm algorithm, and establish a linear dynamic inertia factor parameter. The inertia factor parameter is larger in the early stage of the algorithm iteration than in the later stage of the iteration.

[0043] Step A specifically includes the following steps:

[0044] Step 1: Establish the robot forward kinematics model as shown below:

[0045]

[0046] In formula (1), T i Represents the transformation relationship between the i-th coordinate system and the i-1-th coordinate system; θ i Represents the rotation z i-1 Axis, from x i-1 Go to x i Angle; d i Indicates that along z i-1 From x i-1 Move to x i The displacement, a i Indicates that along x i From z i-1 Move to z i The displacement of i Represents around x i Axis from z i-1 Rotate to z i The angle of i = cosθ i , sθ i = sinθ i , cα i =cosα i , sα i = sinα i ; The following expressions are similar.

[0047] Step 2: Establish the kinematic link model of the offset robot.

[0048] Step 3: Establish a non-biased robot model, set the offset robot wrist offset d5 to zero, make the robot's four, five, and six axes intersect at the same point, satisfy the Pieper criterion, and ensure that there is no offset distance at the wrist.

[0049] Step 4: Find the approximate solution and inversely solve the approximate joint solutions θ1', θ2', θ3', θ4', θ5', θ6'. Use the conventional analytical method to inversely solve the approximate joint solutions θ1', θ2', θ3', θ4', θ5', θ6'. The robot inverse solution method that meets the Pieper criterion is relatively mature and will not be elaborated here.

[0050] Step B specifically includes the following steps:

[0051] Step 5: Establish an improved particle swarm algorithm to iteratively find the exact solution. The mathematical expression is as follows:

[0052]

[0053] In formulas (2), (3), and (4), i = 1, 2, 3, ..., m, where m is the size of the particle swarm; d = 1, 2, 3, ..., D, where D is the particle dimension. In formula (2), g is the global extreme value, p is the individual optimal extreme value, k is the number of iterations, ω is the inertia weight factor, c1 and c2 are learning factors, both of which are constants, r1 and r2 are random numbers between [0, 1], and in formula (3) V represents the flying speed of the kth generation particle, and is also the basis for the position change of the k+1th generation particle. max,d represents the maximum flight speed of the dth particle in D dimension, where is the position of the k+1th generation particle.

[0054] Step 6: Improve the inertia factor parameter in the particle swarm algorithm. The inertia weight ω in the particle swarm algorithm has a great influence on the optimization performance of the algorithm. A larger ω is conducive to jumping out of the local optimum, but converges slowly, which is suitable for global optimization in the early stage of iteration; a smaller ω is conducive to accelerating convergence, but it is easy to fall into the local optimum, which is suitable for rapid convergence in the later stage of iteration. The basic particle swarm algorithm ω is a fixed value, so there are disadvantages of local optimum or slow convergence. At the beginning of the search process, a global search is required, so ω needs to be set larger, and after searching a certain number of times, rapid convergence is required, and the ω value needs to be reduced at this time. Establish a linear and dynamically changing inertia factor parameter to overcome the disadvantage of the early search being prone to falling into the local optimum, and also accelerate the convergence in the later stage, as shown in the following formula:

[0055]

[0056] In formula (5), ω max is the maximum inertia factor, ω min is the minimum inertia factor, T maxis the maximum number of iterations, k is the iteration sequence number, ω k Represents the inertia factor value of the kth iteration.

[0057] Step 7: Determine the fitness function fitness:

[0058] The fitness function value is used to determine whether the inverse solution meets the accuracy requirements. The smaller the value, the closer the inverse solution is to the accurate solution and the higher the accuracy. The approximate joint angles θ1', θ2', θ3', θ4', θ5', θ6' are combined with forward kinematics to calculate the approximate end position P'[X', Y', Z'] and Euler angle posture R'[α', β', γ']. The difference between the actual end position P and R is used to calculate the second norm and the minimum value. The formula is as follows:

[0059]

[0060] Step 8: Determine the search range of the particle swarm. The initial value of the particle swarm algorithm has a great influence on the convergence speed during the search iteration. If the search range is unlimited, the search time will be long. In order to determine the deviation range between the precise joint angle and the approximate solution corresponding to the robot posture in the robot motion space, the Monte Carlo method is used to select the spatial point for inverse solution. The range of the exact solution is determined by comparing the approximate solution with the exact solution, and this range is used as the initial particle swarm range.

[0061] Step 9: After finding the approximate solution for the pose points evenly distributed in the robot motion space, it is found that the exact solution of the current model is within the range of ±10° of the approximate solution. The exact position of the joint is obtained by the relevant simulation software, so the dynamic range of the initial point position can be set to X 0 [X min ,X max ], where X max = approximate solution +15°, X min = approximate solution -15°, and also set the search range of the population. To ensure the accuracy of the search, it can be appropriately increased based on the initial point position. In this embodiment, the search range is set to W[X min -2°,X max +2°].

[0062] Based on a six-joint offset robot, the inverse solution process is gradually developed using the above algorithm.

[0063] Step 1: Establish the robot kinematic model using the standard DH modeling method.

[0064] Step 2: Determine the DH parameters of the model, as shown in the following table:

[0065] Table 1DH parameter table

[0066] i <![CDATA[a i ]]> <![CDATA[α i ]]> <![CDATA[d i ]]> 1 0 0 410 2 0 -π / 2 90 3 700 0 200 4 0 -π / 2 630 5 0 π / 2 90 6 0 π / 2 120

[0067] Step 3: Build a bias-free robot model, set d5 in the table above to zero, and make axes 4, 5, and 6 intersect at one point to satisfy the Pieper criterion.

[0068] Step 4: To find an approximate solution, randomly select a point in the robot motion space, position P[377.3, 359.745, -200.236] mm, Euler angle R[-47, 61.9, -71.89]°.

[0069] 1) Calculate the wrist position W[w x 、w y 、w z ]

[0070]

[0071] In formula (7), T e represents the terminal pose matrix, T e =T1*T2*T3*T4*T5*T6, the wrist position coordinates can be obtained from the above formula.

[0072] 2) The wrist position is determined by the front five-axis angles, which can be obtained Multiplying the matrix T1 on the left can strip off joint 1, and selecting the second element on the left and right sides, we get the following formula:

[0073] -sθ1w x +cθ1w y =d2+d3+cθ4d5(8)

[0074] At this time, θ1 and θ4 are coupled, and the exact solution of θ1 cannot be obtained. Therefore, the bias d5 is set to 0 to obtain the approximate solution θ1' of θ1.

[0075] From the above formula, we can get:

[0076]

[0077] According to formula (9), θ1' can be obtained as 19.13°.

[0078] 3) After finding θ1', use the formula The first and third elements on the left and right sides are listed as equations, as shown below:

[0079]

[0080] According to equation group (10), θ2' and θ3' can be solved to be 20.983° and 7.987° respectively.

[0081] 4) Solve θ4', θ5', θ6', and use the formula T e=T1*T2*T3*T4*T5*T6, we can get

[0082] (T1*T2*T3) -1 *T e =T4*T5*T6 (11)

[0083] θ1', θ2', and θ3' have been obtained in equation (11), so the left side of the equation is a known quantity. For the convenience of description, the posture matrix in the equations on the left and right sides is extracted as shown in the following equation:

[0084]

[0085] 5) According to formula (12), the following equation can be listed:

[0086]

[0087] Solving equation (13) yields θ4', θ5', and θ6' as 52.306°, 50.9458°, and 2.5433°, respectively.

[0088] Step 5: Determine the relevant parameters of the particle swarm algorithm. The parameters to be determined are 6 joint angles, the particle swarm dimension D is 6, the values ​​of c1 and c2 are set to 2, and r1 and r2 are random numbers between [0, 1].

[0089] Step 6: Determine the dynamic inertia factor calculation formula. Usually, the fixed inertia factor ω value range is [0.1~1]. The optimal ω is selected based on multiple experimental experiences. max The value is 0.9, the optimal ω min The value is 0.2, the dynamic inertia factor parameter ω k The calculation formula is:

[0090]

[0091] In formula (14), T max is the maximum number of iterations, which is set to 100 based on experimental experience, k is the iteration sequence number, ω k Indicates the inertia factor value of the kth iteration. Inertia factor ω k It gradually decreases with the increase of the number of iterations. From the particle swarm iteration formula, it can be seen that in the early stage of iteration, due to the large inertia factor, the point velocity in the particle displacement change occupies a large weight, and the particle change range is large to achieve the global optimum; in the later stage of particle iteration, the inertia factor is small, and the particle change range will decrease when it approaches the global optimal point, achieving rapid convergence to the global optimum.

[0092] Step 7: Calculate the fitness function value fitness under the current approximate solution. The approximate terminal position P'[380.666,419.191,-388.1286]mm and Euler angle posture R'[-47, 61.9,-71.883]° are calculated by combining θ1', θ2', θ3', θ4', θ5', and θ6' with forward kinematics. The difference between the actual terminal position P[377.3, 359.745, -200.236]mm and Euler angle R[-47, 61.9, -71.89]° is used to calculate the second norm, and the fitness value is 197.1012.

[0093] Step 8: Determine the initial population of the particle swarm. According to the range of the initial particle swarm determined by the Monte Carlo method, the range of the initial particle swarm of each joint in the current case is θ1∈[4.13,34.13]°, θ2∈[5.983,35.983]°, θ3∈[-7.013,22.987]°, θ4∈[37.306,67.306]°, θ5∈[35.9458,65.9458]°, θ6∈[-12.4567,17.5433]°. The dimension of the current iteration population is 6. The number of particle populations m is set to 11. Therefore, the particle swarm range is divided into 10 equal parts. According to the above particle swarm formula, the expression of the initial particles is as follows:

[0094]

[0095] In formula (15) Represents the second particle of joint 1 in the initial population (generation 0).

[0096] Step 9: According to the application requirements, the iteration accuracy ε is set to 0.001. The parameters required for improving the particle swarm and the initial population setting have been completed. According to the iteration rules of the particle swarm algorithm, a global search for the optimal solution is performed. The iteration process is shown in the relevant pictures.

[0097] Another embodiment of the present application provides a biased robot kinematics inverse solution device, including a processor and a memory, the memory storing computer instructions, and the processor being configured to: call and execute the computer instructions to implement the biased robot kinematics inverse solution method as described above.

[0098] Another embodiment of the present application provides a computer-readable medium, wherein the computer-readable medium stores computer instructions. When the computer instructions are called and executed by a computer, the above-mentioned offset robot kinematic inverse solution method is implemented.

[0099] The above is only a preferred implementation of the present application. It should be pointed out that a person skilled in the art can make several modifications and improvements without departing from the inventive concept of the present application, and these all fall within the scope of protection of the present application.

Claims

1. A method for inverse kinematics of an offset robot, characterized in that: The following steps are involved: Step A: Treat the biased robot as a non-biased robot and find an approximate solution; Step B: Use the approximate solution to determine the initial value of the particle swarm, use the particle swarm algorithm to iterate and find the exact solution, improve the inertia factor parameter in the particle swarm algorithm, and establish a linear dynamic inertia factor parameter. The inertia factor parameter is larger in the early stage of the algorithm iteration than in the later stage of the iteration.

2. The method for inverse kinematics solution of an offset robot according to claim 1, characterized in that: Step A specifically includes the following steps: Step 1: Establish the robot forward kinematics model as shown below: In formula (1), T i Represents the transformation relationship between the i-th coordinate system and the i-1-th coordinate system; θ i Represents the rotation z i-1 Axis, from x i-1 Go to x i Angle; d i Indicates that along z i-1 From x i-1 Move to x i The displacement, a i Indicates that along x i From z i-1 Move to z i The displacement of i Represents around x i Axis from z i-1 Rotate to z i The angle of i = cosθ i , sθ i = sinθ i , cα i =cosα i , sα i = sinα i ; Step 2: Establish the kinematic link model of the offset robot; Step 3: Establish a non-biased robot model, set the offset robot wrist offset d5 to zero, make the robot's four, five, and six axes intersect at the same point, satisfy the Pieper criterion, and ensure that there is no offset distance at the wrist; Step 4: Find the approximate solution and inversely solve the joint approximate solutions θ1', θ2', θ3', θ4', θ5', θ6'.

3. The method for inverse kinematics solution of an offset robot according to claim 1, characterized in that: Step B specifically includes the following steps: Step 5: Establish an improved particle swarm algorithm to iteratively find the exact solution. The mathematical expression is as follows: In formulas (2), (3), and (4), i = 1, 2, 3, ..., m, where m is the size of the particle swarm; d = 1, 2, 3, ..., D, where D is the particle dimension. In formula (2), g is the global extreme value, p is the individual optimal extreme value, k is the number of iterations, ω is the inertia weight factor, c1 and c2 are learning factors, both of which are constants, r1 and r2 are random numbers between [0, 1], and in formula (3) V represents the flying speed of the kth generation particle, and is also the basis for the position change of the k+1th generation particle. max,d represents the maximum flight speed of the dth particle in D dimension, where is the position of the k+1th generation particle; Step 6: Improve the inertia factor parameters in the particle swarm algorithm and establish linear dynamic inertia factor parameters: In formula (5), ω max is the maximum inertia factor, ω min is the minimum inertia factor, T max is the maximum number of iterations, k is the iteration sequence number, ω k represents the inertia factor value of the kth iteration; Step 7: Determine the fitness function fitness: The approximate joint angles θ1', θ2', θ3', θ4', θ5', and θ6' are combined with forward kinematics to calculate the approximate end position P'[X', Y', Z'] and the Euler angle posture R'[α', β', γ']. The difference between the actual end position P and R is used to calculate the second norm and the minimum value. The formula is as follows:

4. The method for inverse kinematics solution of an offset robot according to claim 1, characterized in that: Step B also includes step 8: determining the search range of the particle swarm, obtaining the deviation range between the precise joint angle corresponding to the robot posture in the robot motion space and the approximate solution, determining the range of the precise solution by comparing the approximate solution with the precise solution, and taking the range as the initial particle swarm range.

5. A device for inverse kinematics of an offset robot, characterized in that: It comprises a processor and a memory, wherein the memory stores computer instructions, and the processor is configured to: call and execute the computer instructions to implement the offset robot kinematics inverse solution method as described in any one of claims 1-4.

6. A computer-readable medium, characterized in that The computer-readable medium stores computer instructions, and when the computer instructions are called and executed by a computer, the offset robot kinematic inverse solution method as described in any one of claims 1 to 4 is implemented.

Images