CN113997288A - Numerical algorithm for solving inverse kinematics of non-spherical wrist 6R industrial robot - Google Patents

Abstract

The invention provides an inverse kinematics solving method of a non-spherical wrist 6R industrial robot. The method comprises the following steps: s1, firstly, using the improved disconnecting-reconnecting method and DIXON synthesis method, deducing that the continuity is achieved and only contains theta₆And the inverse solution formula of other joint variables; s2, then proving that the distance of inverse solutions of adjacent pose points in the same unique domain is minimum, and providing a method for determining the positive and negative signs in the inverse solution formula according to the judgment of the unique domain; s3, finally, searching and solving the nonlinear equation by using a golden section method; s4, simulation results show that the algorithm can ensure that the displacement of each joint is minimum without obtaining all inverse solutions of the non-spherical wrist 6R robot; s5, the average solving time of the algorithm is about 12us, and the average pose error norm is about 7.07 multiplied by 10^‑12. Compared with the traditional method, the method has higher efficiency and simpler derivation process.

Description

Numerical algorithm for solving inverse kinematics of non-spherical wrist 6R industrial robot

Technical Field

The invention relates to an inverse solution numerical algorithm, in particular to a solving method of inverse kinematics of an unspheral wrist 6R industrial robot.

Background

The inverse kinematics solution of the robot is used as the basis for the research of other subjects such as robot off-line programming, trajectory planning, control algorithm design and the like, is always a classical problem in the robotics, and is also a research hotspot. The essence of inverse kinematics solution is to complete the mapping from the working space of the robot to the joint space, and the inverse kinematics equation set has the characteristics of high dimension and nonlinearity, and the solution is complex and is not easy to solve. When the structure of the robot meets the PIEPER criterion, i.e. the last three joints are designed as spherical wrists with axes intersecting at one point, an analytic solution can be obtained.

Compared with a spherical wrist 6R robot, the wrist offset type 6R robot has higher load capacity, longer horizontal reaching distance and flexibility, and is widely applied to industries such as welding, spraying, material processing and the like. Although the wrist bias type structure improves the performance of the robot in the aspects of kinematics and the like, the robot cannot obtain an inverse kinematics analysis solution, and an inverse kinematics nonlinear equation set of the robot is more complicated and has higher coupling degree. At the moment, the inverse kinematics solution of the wrist offset type 6R robot can be solved by using the pose inverse solution result of a general 6R robot, the methods mainly use the semitangent of the joint to convert the kinematics equation into a 1-element 16-degree polynomial for solving, but the formula derivation process of the methods is very complicated and time-consuming.

Although the existing kinematics solution can solve the inverse kinematics problem of the robot, a general method is still lacked, the inverse solution formula is derived in a geometric visual sense, and real-time and high-precision control can be met in a numerical solution process.

Disclosure of Invention

The method aims at solving the problems that the derivation process of the inverse kinematics solution in the prior art is very complicated and time-consuming, and the off-line solution and real-time solution cannot be guaranteed; an inverse kinematics solving numerical algorithm of an unspherical wrist 6R industrial robot is provided.

In order to achieve the purpose, the invention adopts the following technical scheme:

a commercial non-spherical wrist 6R industrial robot inverse kinematics solving method comprises the following steps:

s1, firstly, using improved method of disconnection-reconnection (IMDR) and DIXON synthesis method, deducing that the continuity is contained only theta₆And the inverse solution formula of other joint variables;

the positive kinematic formula of the tail end of the robot relative to the base coordinate system can be obtained by multiplying the homogeneous transformation matrix of each connecting rod:

^baseT＝T₁(θ₁)T₂(θ₂)T₃(θ₃)T₄(θ₄)T₅(θ₅)T₆(θ₆)

after establishing the positive kinematic equation, the above equation can be transformed into:

T_L＝T₂(θ₂)T₃(θ₃)T₄(θ₄)Rot(-α₄,x₄)

the above formula shows that the original robot is decomposed into two sub-chains, and the terminal pose of the sub-chain L is T_LIt is formed by the origin of the joint 2The part from the point to the intersection point of the axes of the joint 4 and the joint 5 is a plane structure. The rest part forms a subchain R, and the tail end pose of the subchain R is T_RAnd a virtual link whose origin between the joint 1 and the joint 6 is represented by T.

At the tail ends of the sub chains, the two sub chains can be reconnected as long as the poses of the two sub chains meet the following conditions:

p_L＝p_R

formula P_L＝P_RIn fact, when all the 6 joint variables satisfying the above 6 equations are solved, the two sub-chains will be recombined into the original robot at the disconnected position, and these obtained joint variables are the inverse solution of the robot. The first two equations are used to solve for theta₁、θ₂、θ₃And theta₆The third equation can obtain θ₄The fourth equation can obtain θ₅。

S2, then proving that the distance of the inverse solutions of the adjacent pose points in the same unique domain is minimum, and providing a method for determining the positive and negative signs in the inverse solution formula according to the judgment of the unique domain:

firstly, the formula is represented by₄c₃-l₃s₃＝0(c₃Represents cos θ₃，s₃Denotes sin θ₃) Can directly obtain theta₃Boundary value of (2):

thereby to obtain

k ∈ Z. Thus, θ₃Is identical to the unique field, k₂The value of (d) can be determined by:

if l₄c₃-l₃s₃≤0

k₂＝1

else

k₂＝-1

k₁can be determined by

if l₁+l₄c₂₃-l₃s₂₃-l₂s₂≥0

k₁＝1

else

k₁＝-1

Since the robot end tracks a continuous track, k₁And k₂Can be determined by substituting the initial point into the above equation.

S3, finally, searching and solving the nonlinear equation by using the golden section method to obtain theta₁、θ₂、θ₃、θ₄、θ₅The solution formula of (2);

and S4, verifying the effectiveness of the method through simulation.

Further, the homogeneous transformation formula between the adjacent links in step S1 is:

wherein:

θ_i——x_i-1axis and x_iBetween axes with respect to z_iThe included angle of (a).

d_iThe origin of the coordinate system { i-1} is along z_i-1Axis to x_iDistance of the shaft.

a_iAlong x_iAxis, z_i-1Axis and z_iThe distance between the axes.

α_i——z_i-1Axis and z_iBetween axes with respect to x_iThe included angle of (a).

Further, in step S1, a Dixon synthesis method is used to solve the system of one-dimensional quadratic equations:

assume a one-dimensional quadratic system as follows:

f(x)＝Ax²+Bx+C

g(x)＝Dx²+Ex+F

according to the principle of elimination of elements in the Dixon synthesis method, the following determinant can be constructed:

the above equation can be further written as:

where α ∈ R is a new variable introduced, U ═ α,1],V＝[x,1]^TAnd an

|Σ|＝(AF-CD)²-(AE-BD)(BF-CE)

Where | Σ | represents a determinant, so long as the equation is equal to zero, then there is at least one solution common to x in equations f (x) and g (x).

Further, in step S2: determining k₁Can ensure theta₁Is minimized, k is determined₂Can ensure theta₃Is minimal and the initial point can ensure theta₆Is minimal. When theta is₁、θ₃And theta₆All ensure a minimum displacement, theta₂、θ₄And theta₅Will naturally be guaranteed because their inverse solution is given by θ₁、θ₃And theta₆Is obtained only.

Further, θ in step S3₁、θ₂、θ₃、θ₄、θ₅Is solved forThe formula is as follows:

θ₁＝atan2(k₁B₁,k₁A₁)；θ₂＝atan2(s₂,c₂)；

θ₄＝atan2(0，-s_β)+atan2(B₄,A₄)；θ₅＝atan2(0,s_β)+atan2(B₅,A₅)。

the invention has the beneficial effects that: compared with the common traditional 6R robot inverse kinematics solution method, the solution method has more definite geometric significance, is more convenient to derive, and meets the requirements of real-time performance, high precision and stability.

Drawings

FIG. 1 is a link coordinate system of an industrial robot with an obliquely offset wrist;

FIG. 2 is a flow chart of an inverse solution method;

FIG. 3 is a joint curve corresponding to each posture;

FIG. 4 is a pose error norm;

FIG. 5 is an inverse kinematics solution time;

fig. 6 is a curve of the fitness function near the singular position.

Detailed Description

The present invention will be further described with reference to the following embodiments.

An inverse kinematics solving method of an aspherical wrist 6R robot comprises the following steps:

the spraying robot is used as a research object. The robot wrist has oblique offset, is special in the non-spherical wrist industrial robot, solves the inverse kinematics problem of the non-spherical wrist industrial robot, and can solve the inverse kinematics problem of other wrist offset industrial robots. Table 1 lists the D-H parameters of the robot, where β is 60 °.

TABLE 1D-H parameters of an industrial robot

The homogeneous transformation formula between adjacent connecting rods is as follows:

wherein:

θ_i——x_i-1axis and x_iBetween axes with respect to z_iThe included angle of (a).

d_iThe origin of the coordinate system { i-1} is along z_i-1Axis to x_iDistance of the shaft.

a_iAlong x_iAxis, z_i-1Axis and z_iThe distance between the axes.

α_i——z_i-1Axis and z_iBetween axes with respect to x_iThe included angle of (a).

The positive kinematic formula of the tail end of the robot relative to the base coordinate system can be obtained by multiplying the homogeneous transformation matrix of each connecting rod:

^baseT＝T₁(θ₁)T₂(θ₂)T₃(θ₃)T₄(θ₄)T₅(θ₅)T₆(θ₆) (2)

after establishing the positive kinematic equation, equation (2) can be transformed into:

the formula (3) shows that the original robot is decomposed into two sub-chains, and the tail end pose of the sub-chain L is T_LThe joint is composed of a part from the origin of the joint 2 to the intersection point of the axes of the joint 4 and the joint 5, and is of a plane structure. The rest part forms a subchain R, and the tail end pose of the subchain R is T_RAnd a virtual link whose origin between the joint 1 and the joint 6 is represented by T.

At the tail ends of the sub chains, the two sub chains can be reconnected as long as the poses of the two sub chains meet the following conditions:

p_L＝p_R (4)

equation (4) is actually three equations, and when 6 joint variables satisfying the above 6 equations are all solved, the two sub-chains will be recombined into the original robot at the disconnected position, and these obtained joint variables are the inverse solution of the robot. Equations (4) - (5) for finding θ₁、θ₂、θ₃And theta₆The equation (6) can be used to obtain θ₄The equation (7) can determine theta₅。

Let the target pose be:

the symbolic variable of each connecting rod parameter is substituted into the formula (3) and can be obtained from the formula (5),

c₂₃(-c₁A₂₃-s₁B₂₃)+s₂₃C₂₃-c_β＝0 (9)

in the formula

A₂₃＝o_xc₆s_β-a_xc_β+n_xs_βs₆

B₂₃＝o_yc₆s_β-a_yc_β+n_ys_βs₆

C₂₃＝a_zc_β-o_zc₆s_β-n_zs_βs₆

From now on s_i、c_i、s_i+j、c_i+j、s_βAnd c_βRespectively represent sin theta_i、cosθ_i、sin(θ_i+θ_j)、cos(θ_i+θ_j) Sin β and cos β.

Is obtained by the formula (4),

l₄c₂₃-l₃s₂₃-l₂s₂＝s₁B₁+c₁A₁-l₁ (10)

l₃c₂₃+l₄s₂₃+l₂c₂＝C₁ (11)

0＝s₁A₁-c₁B₁ (12)

in the formula

A₁＝p_x-a_xl₆+l₅A₂₃

B₁＝p_y-a_yl₆+l₅B₂₃

C₁＝p_z-a_zl₆-l₅C₂₃

Using the formula (9-12), a composition containing only theta₆Is used as a non-linear equation. First, use (12) and trigonometric identity sin²θ+cos²θ is 1, and then:

s₁＝k₁B₁/K (13)

c₁＝k₁A₁/K (14)

in the formula k₁＝±1，

k₁Can be determined by

if l₁+l₄c₂₃-l₃s₂₃-l₂s₂≥0

k₁＝1

else

k₁＝-1

Then, formula (10) and formula (11) include (θ)₂+θ₃) Moves to the equation right square summation and substitutes equations (13) and (14) therein to yield:

cause s₁And c₁Are all expressed by theta₆Thus, both equations (9) and (15) now contain only the variable (θ)₂+θ₃) And theta₆. In order to make the non-linear equation continuous, equations (9) and (15) may be considered as relating to s₂₃And c₂₃The system of one-dimensional quadratic equations is obtained by a solution vector method:

wherein both A, B, C, D, E and F are only related to theta₆In connection with these, they are respectively:

A＝-2l₄C₁-2l₃(l₁-k₁K)

B＝-2l₃C₁+2l₄(l₁-k₁K)

C＝c₂₃K

D＝-k₁(A₁A₂₃+B₁B₂₃)

F＝c_βK

then combining the trigonometric identities to obtain:

f＝(DE-BF)²+(AF-CE)²-(AD-BC)² (17)

θ₆once solved, other joint variables can be derived using the following methods.

From formulae (13) and (14), we have:

θ₁＝atan2(k₁B₁,k₁A₁) (18)

the squares of equations (10) and (11) are summed to obtain:

in the formula, k₂＝±1，p₁The right part of the equal sign in expression (10).

A₃＝2l₂l₃

B₃＝2l₂l₄

Firstly, the formula is represented by₄c₃-l₃s₃Theta can be directly obtained when the value is 0₃Boundary value of (2):

thereby to obtain

k ∈ Z. Thus, θ₃Is identical to the unique field, k₂The value of (d) can be determined by:

if l₄c₃-l₃s₃≤0

k₂＝1

else

k₂＝-1

to obtain theta₃Then, the expressions (10) and (11) contain only θ₂A variable, which is expanded by s₂And c₂The same items are respectively merged and regarded as a system of linear equations of two elements, and the solution is obtained:

θ₂＝atan2(s₂,c₂) (21)

in the formula

a＝-(l₂+l₃c₃+l₄s₃)

b＝l₄c₃-l₃s₃

Determining theta₁、θ₂、θ₃And theta₆Rear, theta₄And theta₅Can be obtained from the formulas (6) and (7), respectively.

From formula (6), we obtain:

θ₄＝atan2(0，-s_β)+atan2(B₄,A₄) (22)

in the formula

A₄＝B₂₃c₁-A₂₃s₁

B₄＝-C₂₃c₂₃-s₂₃(A₂₃c₁+B₂₃s₁)

From formula (7), it follows:

θ₅＝atan2(0,s_β)+atan2(B₅,A₅) (23)

in the formula

A₅＝c₂₃(c₁(n_xc₆-o_xs₆)+s₁(n_yc₆-o_ys₆))+s₂₃(n_zc₆-o_zs₆)

The formula (17) is represented by₆The trigonometric function of (a) is a univariate non-linear equation which can be factorized in the complex domain, and is difficult to obtain due to the difficulty in obtaining its root equation. Assuming it can be factorized, the fitness function can be designed as the following equation:

in the formula, K _fit11 or 1, K _fit21 or 2, they are by default 1.

To test the performance of the algorithm presented herein, the experiment was developed in the VS2017 simulation environment. The tested hardware environment is Intel (R) core (TM) i7-9750H CPU @2.60GHz, the running memory is 16GB, and the operating system is Windows 10 family Chinese edition.

In actual movement, the robot starts to move to a target point from a zero position, so that Q is adjusted₀＝[0,0,0,0,0,0]^TThe pose obtained by substituting the formula (2) is set as an initial pose T₀. The target trajectory is obtained using the following formula:

in the formula, P₀Is the initial pose T₀Where N is the total number of sample points, j is the ordinal number of the sample point, j belongs to Z and j belongs to [1, N ]]. The postures of the target tracks are all equal to T₀And (4) a middle posture.

In the inverse solution algorithm, the first pose is used for searchingHas an initial joint angle of Q₀And then, the initial joint angle of each pose in solving is the inverse solution of the previous pose point. The total number of sampling points N is 10000, the step length h is pi/180, the number of searches k is initially 1, and the threshold E is 1 × 10^-12，ε＝10^-8. This inverse kinematics solution yields the following data:

(1) joint curves corresponding to each pose, as shown in fig. 3;

(2) inversely solving two norms between the corresponding pose and the target pose, as shown in fig. 4;

(3) the inverse kinematics for each pose solve for time, as in fig. 5.

The maximum pose error norm in fig. 4 is about 1.2509 × 10^-12mm, average value of about 7.07X 10^-12mm, standard deviation of about 1.6776 × 10^-13mm。

FIG. 5 shows the inverse solution time of each sample point on the target trajectory, wherein the longest time is about 90.4 μ s, and the average time is about 12 μ s.

Further, let Q be [0,0, atan (l)₄/l₃)+0.13,0,0,0]^TSubstituting formula (2) to obtain target pose T, and then combining the target pose T with k₁＝1， K _fit11 and K_fit2Substituting k for 1₂And determining a formula to obtain a fitness function curve at the pose, as shown in FIG. 6.

The Jacobian matrix determinant of the non-spherical wrist robot corresponding to the joint angle Q is 6.5 multiplied by 10^-8And thus it can be judged that the position is located near the singular position. In FIG. 6, θ is shown in a partially enlarged view₆At [ -2.032, -2.026)]There are two roots that are very close together and the distance between these two roots is less than the set step h. Using the algorithm presented herein, the inverse solution is obtained here as:

[0.89755491,0.14028032,1.09887203,3.06847901,-2.99804717,-2.02615162]

the corresponding pose error is 4.5475 multiplied by 10^-13The mean solution time is about 20.7 mus.

It can be seen from FIG. 3 that the displacements of the various joints are smoothly continuous, indicating the coefficient k in the inverse solution formulation₁And k₂Determined by the unique field, non-guaranteedThe displacement of each joint is minimum when the trajectory of the tail end of the spherical wrist industrial robot is tracked, and the division of a unique domain contained in an inverse solution formula derived from the table is verified. The pose error norm of fig. 4 shows that the algorithm can obtain a high-precision inverse solution, and the solution stability is high. The average solution time of the algorithm in fig. 5 reaches 12 mus, which can be comparable to the time of a wrist industrial robot. In addition, the algorithm only starts linear interpolation, and shows that all the position points on the calculated track are far away from a singular position.

The target pose in fig. 6 is close to the singular position, where the two roots are very close as analyzed before, which may cause the linear interpolation method to fail to start due to the step size, and the golden section method to start. After the golden section method is started, the fitness function is squared, so that the searching interval is actually a double peak extremum searching problem. Since the golden section method itself has the function of determining the search interval, in this bimodal extreme value, the algorithm necessarily converges to one extreme value. In summary, the present algorithm is effective and has a high accuracy solution even if the pose is at or near the singular position.

The method has more definite geometric meaning in formula derivation. All inverse solutions can be solved by the algorithm within a fixed operation time by adopting a fixed initial iteration point; meanwhile, the method has stronger theoretical popularization, and provides concrete solving methods of other joint angles; the algorithms used in the algorithm section herein are readily programmable.

The above examples are merely preferred embodiments to fully illustrate the present invention, and the scope of the present invention is not limited thereto. The equivalent substitution or change made by the technical personnel in the technical field on the basis of the invention is all within the protection scope of the invention.

Claims (5)

1. A numerical algorithm for solving inverse kinematics of an unspherical wrist 6R industrial robot is characterized by comprising the following steps of:

s1, firstly, using the improved disconnecting-reconnecting method and DIXON synthesis method, deducing that the continuity is achieved and only contains theta₆Is not a lineEquation of sex, and equations for inverse solutions of other joint variables;

the positive kinematic formula of the tail end of the robot relative to the base coordinate system can be obtained by multiplying the homogeneous transformation matrix of each connecting rod:

^baseT＝T₁(θ₁)T₂(θ₂)T₃(θ₃)T₄(θ₄)T₅(θ₅)T₆(θ₆) (1)

after establishing the positive kinematic equation, the above equation can be transformed into:

the formula (2) shows that the original robot is decomposed into two sub-chains, and the tail end pose of the sub-chain L is T_LThe joint consists of a part from the origin of the joint 2 to the intersection point of the axes of the joint 4 and the joint 5, and is a plane structure; the rest part forms a subchain R, and the tail end pose of the subchain R is T_RA virtual link connection whose origin between the joint 1 and the joint 6 is denoted by T; at the tail ends of the sub chains, the two sub chains can be reconnected as long as the poses of the two sub chains meet the following conditions:

p_L＝p_R (3)

equation (3) is actually three equations, and when 6 joint variables satisfying the above 6 equations are all solved, the two sub-chains will be disconnectedThe positions are combined into the original robot again, and the obtained joint variables are the inverse solution of the robot; equations (3) to (4) for finding θ₁、θ₂、θ₃And theta₆The equation (5) can determine theta₄The equation (6) can be used to obtain θ₅；

S2, then proving that the distance of the inverse solutions of the adjacent pose points in the same unique domain is minimum, and providing a method for determining the positive and negative signs in the inverse solution formula according to the judgment of the unique domain:

firstly, the formula is represented by₄c₃-l₃s₃＝0(c₃Represents cos θ_3，s₃Denotes sin θ₃) Can directly obtain theta₃Boundary value of (2):

thereby to obtain

Thus, θ₃Is identical to the unique field, k₂The value of (d) can be determined by:

k₁can be determined by

Since the robot end tracks a continuous track, k₁And k₂Can be determined by substituting the initial point into the above equations (7) and (8);

s3, finally, searching and solving the nonlinear equation by using the golden section method to obtain theta₁、θ₂、θ₃、θ₄、θ₅Is sought afterSolving the formula;

and S4, verifying the effectiveness of the method through simulation.

2. The numerical algorithm for solving the inverse kinematics of an aspherical wrist 6R industrial robot as claimed in claim 2, wherein the homogeneous transformation formula between adjacent links in step S1 is:

wherein

θ_i——x_i-1Axis and x_iBetween axes with respect to z_iThe included angle of (A);

d_ithe origin of the coordinate system { i-1} is along z_i-1Axis to x_iDistance of the shaft;

a_ialong x_iAxis, z_i-1Axis and z_iThe distance between the axes;

α_i——z_i-1axis and z_iBetween axes with respect to x_iThe included angle of (a).

3. A numerical algorithm for solving inverse kinematics of an unspheral wrist 6R industrial robot according to claim 1, wherein a Dixon synthesis is used to solve a system of unary quadratic equations in step S1:

assume a one-dimensional quadratic system as follows:

f(x)＝Ax²+Bx+C (10)

g(x)＝Dx²+Ex+F (11)

according to the principle of elimination of elements in the Dixon synthesis method, the following determinant can be constructed:

equation (12) can be further written as:

where α ∈ R is a new variable introduced, U ═ α,1],V＝[x,1]^TAnd an

|Σ|＝(AF-CD)²-(AE-BD)(BF-CE) (14)

Wherein, | Σ | represents a determinant; equation (14) is an equation of coefficients of equations (10) and (11), and x in equations (10) and (11) has at least one common solution as long as the equation is equal to zero.

4. A numerical algorithm for solving inverse kinematics in an unsphered wrist 6R industrial robot according to claim 1, wherein in step S2: determining k₁Can ensure theta₁Is minimized, k is determined₂Can ensure theta₃Is minimal and the initial point can ensure theta₆Is minimal.

5. The numerical algorithm for solving the inverse kinematics of an aspherical wrist 6R industrial robot as claimed in claim 1, wherein θ is θ in step S3₁、θ₂、θ₃、θ₄、θ₅The solving formula of (2) is as follows:

θ₁＝atan2(k₁B₁,k₁A₁)；θ₂＝atan2(s₂,c₂)；

θ₄＝atan2(0，-s_β)+atan2(B₄,A₄)；θ₅＝atan2(0,s_β)+atan2(B₅,A₅)。

Images