CN106372443B - Stewart parallel mechanism kinematics forward solution method - Google Patents

Abstract

The invention discloses a kinematics forward solution method for a Stewart parallel mechanism. According to the method, a test rod and an additional sensor for measuring the rod length of the test rod are added at the center of a planar platform type Stewart parallel mechanism, degradation and rise processing are carried out on 11 capacitance equations by measuring the rod length of the test rod and combining algebraic elimination elements and other methods for an attitude matrix of the new Stewart parallel mechanism, and finally a Stewart parallel mechanism analysis positive solution required by closed-loop real-time feedback control is obtained, so that the singular limitation is solved, and the position and the attitude can obtain a unique solution.

Description

Stewart parallel mechanism kinematics forward solution method

Technical Field

The invention belongs to the field of kinematics, dynamics and control research of a robot system, and particularly relates to a forward kinematics analysis algorithm of a Stewart parallel mechanism.

Background

In the past thirty years, Stewart is increasingly applied to the fields of vehicles, tire detection mechanisms, flight driving training simulators, 6-degree-of-freedom coordinate measuring equipment, entertainment and medical equipment and the like because of the advantages of stable structure, high positioning precision, working space saving, high rigidity, high bearing capacity, high speed and the like. However, the Stewart kinematic positive solution is not completely solved.

RAGAVAAN, WAMPLER, HUSTY, etcBy adopting a numerical method, a dual quaternion method and special geometric properties to carry out variable elimination, 40 positive solutions, 40 nonsingular solutions and a unitary 40-order algebraic equation are finally obtained. The application of the sequence of the yellow oxicam, CD Zhang, Huangzhen, WD Wu and the like in the classification dictionary

And (3) solving the 20-order, 21-order and 15-order coefficient determinants by using a basis algorithm or a cell elimination method and the like to finally obtain a unitary 20-order algebraic equation, wherein most researches have the phenomena of a plurality of irrational complex roots, root-added invalid roots and the like in the derivation and solving process. The Chengshili represents the kinematics positive solution of the 6-6 platform as a unary 17-degree algebraic equation through the established 11 compatibility equations, and unreasonable roots still exist. Huang adopts an algebraic elimination method to solve a 15 multiplied by 15 determinant to obtain a unary 14-degree algebraic equation. And P.ji researches the forward solution problem of the parallel mechanism with similar upper and lower platform vertex polygons, expresses the forward solution problem of the mechanism as a group of independent 2-order algebraic equations, and removes all irrational complex roots and root increasing. Up to now, p.ji is the most compact expression form solved by forward solution in the 6-6Stewart platform. The above is a complex process of solving univariate polynomials of variables to obtain the value of one of the variables. And X.Y.Yang adopts quaternion to obtain a fast numerical solution of forward kinematics, but the problem of parallel singularity cannot be solved. To avoid complex calculations and facilitate engineering applications, j. -p.merlet, l.baron, k.han quickly get the pose by adding 4, 3, and 2 additional rod length sensors, respectively, but hardware costs are also increased. Zuubizareta uses 3 sensors to simplify the dynamic modeling of 3-RRR planar parallel robots. And H, adding at least one sensor on a proper passive joint of the spherical parallel mechanism by Saafi, reducing the complexity of forward kinematics solution, improving the kinematics modeling precision, realizing real-time control and eliminating the influence of parallel singularity on the forward kinematics modeling precision.

By combining the above, through an algebraic elimination method, a unitary high-order polynomial equation is obtained, so that the kinematics solving process is complicated, the problem of multi-solution selection is faced, and the real-time full feedback control is not facilitated; the numerical solution method has the problem of singular limitation; and the hardware cost is greatly increased by adding a plurality of sensors to solve.

Therefore, a new technical solution is needed to solve the above problems.

Disclosure of Invention

The invention aims to provide a Stewart parallel mechanism kinematics forward solution method capable of reducing hardware cost aiming at the defects in the prior art.

In order to solve the problems, the Stewart parallel mechanism kinematics forward solution method provided by the invention can adopt the following technical scheme:

a Stewart parallel mechanism kinematics forward solving method is characterized in that a test rod is additionally arranged in the Stewart parallel mechanism, the test rod is also a telescopic rod, and the upper end of the test rod is connected with the circumscribed circle center O of the upper platform₂The lower end is connected with the circle center O of the circumscribed circle of the lower platform₁And the testing rod is provided with a rod length L for measuring the testing rod₀The additional sensor of (3), the method comprising the steps of:

(1) establishing a posture matrix of the Stewart parallel mechanism:

the quaternion ε is expressed as: e ═ e-₁i+ε₂j+ε₃k+ε₀

In the formula: epsilon₁、ε₂、ε₃、ε₀∈R；i²＝j²＝k²＝-1；ij＝-ji＝k；jk＝-kj＝i；ki＝-ik＝j；ε^Tε＝1；

The specific form of the 3 rd order attitude matrix R expressed by unit quaternion is as follows:

in order to solve the positive solution conveniently by an algorithm and finally obtain a unique group of position gestures, some elements in the R are equivalently substituted by new symbols, the rest elements are also represented by the new symbols, and the gesture matrix R is equivalently represented as follows:

wherein,

C＝2ε₀ε₃，D＝2ε₁ε₂，γ₁＝2ε₀ε₂+2ε₁ε₃，

α₁＝-2ε₀ε₂+2ε₁ε₃，α₂＝2ε₀ε₁+2ε₂ε₃，

(2) separating primary and secondary variables:

the vector representation of the Stewart type parallel mechanism is as follows:

L_ke_k＝P+Ra_k-b_k k＝0,1,...,6

wherein: l is_kIs the k-th link rod length, when k is 0, L₀For testing rod O₁O₂The length of the rod; (ii) a e.g. of the type_kIs the kth connecting rod unit vector; p is the position vector of the movable platform position in the static coordinate system, and P is { P ═ P_x,P_y,P_z}^T(ii) a R is an attitude matrix; a is_kSeven coordinates including six vertexes and circle center of the movable platform, a_k＝{a_xk,a_yk,a_zk}^T；b_kSeven coordinates including six vertexes and circle center of the static platform, b_k＝{b_xk,b_yk,b_zk}^T；

From this vector form, a scalar equation is obtained:

wherein: l is_kThe kth connecting rod length; r is₁The radius of a circle is circumscribed to the static platform; r is₂The radius of a circumscribed circle of the movable platform; ax, ay, az are k-th vertex a of the movable platform_kThe respective components of (a); bx, by and bz are k-th vertexes b of the movable platform_kThe respective components of (a); p_x、P_yIs a component of the position vector P of the moving platform; w_x、W_yIs a component of the position vector W of the stationary platform; p_pBeing the square of the position vector P modulo the length,

and P is_pValue L measurable by a sensor₀Indirectly obtain, P_pIs known;

and (3) carrying out primary and secondary variable separation on the scalar equation to obtain a linear equation system with the primary and secondary variable separation as follows:

wherein: l is₀The length of the test rod measured by the sensor is used;

k₁＝-r₂Sin[θ₁+2θ₂]Csc[θ₁-θ₂]；k₂＝r₁Sin[2θ₁+θ₂][-Csc[θ₁-θ₂]]；

therefore, according to the structural parameter r of the Stewart parallel mechanism₁、r₂、θ₁、θ₂And rod length parameter L_kk 0, 1.., 6 may determine variables C and a in the pose matrix R;

(3) and structural variables:

according to the orthogonality and the normalization of R, the following constraint conditions exist in 9 elements in R:

(A+B)(C+D)+(D-C)(A-B)+γ₁γ₂＝0

(A+B)(D-C)+(C+D)(A-B)+α₁α₂＝0

γ₁＝(D+C)α₂-(A-B)α₁

γ₂＝(D-C)α₁-(A+B)α₂

γ₃＝(A+B)(A-B)-(D-C)(D+C)

the position of the movable platform is in a static coordinate system O₁x₁y₁z₁The position vector P and its moving coordinate system O₂x₂y₂z₂The position vectors W in (1) have the following relationship:

P＝R·W

from this, the following relation can be obtained:

P_x＝(A+B)W_x+(D-C)W_y+γ₁W_z，

P_y＝(C+D)W_x+(A-B)W_y+γ₂W_z；

wherein, P_x、P_yIs a component of the position vector P of the moving platform; w_x、W_y、W_zIs a component of the position vector W of the stationary platform;

the two formulas are solved as follows:

γ₁W_z＝P_x-(A+B)W_x-(D-C)W_y

γ₂W_z＝P_y-(C+D)W_x-(A-B)W_y

meanwhile, according to the orthogonality of the attitude matrix, the following can be obtained:

W＝R^T·P

the following relationships can thus be obtained:

W_x＝(A+B)P_x+(C+D)P_y+α₁P_z

W_y＝(D-C)P_x+(A-B)P_y+α₂P_z

by the above two formulas to obtain alpha₁P_z、α₂P_zExpression (c):

α₁P_z＝W_x-(A+B)P_x-(C+D)P_y

α₂P_z＝W_y-(D-C)P_x-(A-B)P_y

(4) construction compatibility equation

The following 12 compatibility equations containing B, D can be obtained for a Stewart-like parallel mechanism:

eq1:

eq2:

eq3:γ₁γ₂W_z ²-(γ₁W_z)(γ₂W_z)＝0

eq4:

eq5:

eq6:

eq7:

eq8:

eq9:

eq10:

eq11:

eq12:

wherein eq6 and eq12 are fully equivalent;

(5) equation of constructive kinematics

For the Stewart parallel mechanism, substituting the variables in the step (3) into 11 capacitance equations in the step (4) to obtain 11 correlation equations containing B and D, and uniformly writing the 11 capacitance equations into the following form:

f_1,jB⁴+f_2,jB³D+f_3,jB²D²+f_4,jBD³+f_5,jD⁴+f_6,jB³+f_7,jB²D+f_8,jBD²+f_9,jD³+f_10,jB²

+f_11,jBD+f_12,jD²+f_13,jB+f_14,jD+f_15,j＝0j＝1,…11

f_i,jare all determined by the structural parameter r₁、r₂、θ₁、θ₂And rod length parameter L_kA determined constant;

(6) solving equations

And (3) simplifying 11 compatible equations into a quadratic linear equation with 6 constant coefficients and about B and D according to the concept of descending, descending and ascending for the equation in the step (5) and by utilizing the fact that the result of the third ascending is equal to the result of the second descending, wherein the equation set is in the form as follows:

t_1,jB²+t_2,jBD+t_3,jD²+t_4,jB+t_5,jD+t_6,j＝0；j＝1～6

and solving the equation system to obtain unique solutions of B and D.

Has the advantages that: the subsequent equation set simplification process containing B and D is greatly simplified by utilizing the attitude matrix represented by the simplified form; the length of the test rod is measured by adding an additional sensor, so that the complex process of solving a linear equation set in the early stage by an algebraic elimination method is avoided, the problem of multi-solution selection is solved, and compared with the use of a plurality of sensors, the hardware cost is greatly saved. The most important point is that a single group of positions and postures can be obtained by adding a test rod long sensor and performing kinematics forward solution calculation, so that the full-closed-loop real-time feedback control is greatly facilitated.

Further, in the specific case where the upper and lower platforms are parallel, only the following compatibility equation is used in step (4),

eq6:

eq7:

eq8:

eq9:

eq10:

eq11:

firstly, solving the 4 power combination B of B and D from the equations eq 7-eq 11⁴、B³D、B²D²、BD³、D⁴Substituting a set of solutions into equation eq6, the form is: t is t₁·B²+t₁·D²Form 0, t₁Is a constant coefficient related to the structural parameter and the rod length; that is, B ═ D ═ 0 can be solved, B, D is unique.

Further, in the step (2), theStructural parameter r₁、r₂、θ₁、θ₂And rod length parameter L_kSystem of substitution equations

Find P_pPx, Py, Wx, Wy; wherein, P_zTaking a positive number, namely:

obtaining a position vector P; substituting the structural parameters, the rod length parameter, A, B, C, D and Px, Py, Pz, Wx, Wy into the equation alpha in step (3)₁P_z＝W_x-(A+B)P_x-(C+D)P_y；α₂P_z＝W_y-(D-C)P_x-(A-B)P_y；

γ₁＝(D+C)α₂-(A-B)α₁；γ₂＝(D-C)α₁-(A+B)α₂；

γ₃The variable α in R was calculated as (a + B) (a-B) - (D-C) (D + C)₁、α₂、γ₁、γ₂、γ₃And then an attitude matrix R can be obtained.

Drawings

Fig. 1 is a schematic diagram of a Stewart mechanism with a test bar added in the invention.

Figure 2 is a view of the Stewart mechanism vertex placement in the present invention.

Figure 3 is a schematic diagram of the position and attitude of an example Stewart mechanism of the invention.

Detailed Description

The present invention is further illustrated by the following figures and specific examples, which are to be understood as illustrative only and not as limiting the scope of the invention, which is to be given the full breadth of the appended claims and any and all equivalent modifications thereof which may occur to those skilled in the art upon reading the present specification.

1. Please refer to FIG. 1, which shows the variables required for constructing forward kinematics

The new Stewart parallel mechanism and its coordinate system are shown in fig. 1. The existing Stewart parallel mechanism comprises a lower platform and an upper platform, wherein the upper platform is connected with the lower platform through at least 6 parallel telescopic rods, and on the basis of the existing mechanism, the lower platform is connected with an O-shaped connecting rod₁O₂A test rod is added, the test rod is also a telescopic rod, and an additional sensor for measuring the length of the test rod is arranged on the test rod. The mechanism belongs to a plane platform type, namely, the centers of 6 spherical hinges of a movable platform and a static platform are respectively arranged in two planes. Static coordinate system O₁x₁y₁z₁A moving coordinate system O fixedly connected with the static platform₂x₂y₂z₂Is fixedly connected with the movable platform, wherein O₁、O₂The centers of the circumscribed circles of the static platform and the dynamic platform are respectively; z is a radical of₁、z₂The axes are respectively perpendicular to the respective planes. The 6 vertexes of the movable platform and the static platform are circularly and symmetrically arranged on the circumference of a plane respectively, as shown in figure 2.

(1) Representation of attitude matrix

In order to solve the positive solution conveniently by an algorithm and finally obtain a unique group of positive solutions, some elements in the R are subjected to equivalent substitution processing by adopting new symbols, the rest elements are also represented by the new symbols, and the attitude matrix R is equivalently represented as follows:

(2) vector equation construction and variable separation

For the mechanism shown in FIG. 1, O on the moving and static platforms₁O₂And a total of 7 link vectors between 6 pairs of corresponding vertices can be expressed as:

L_ke_k＝P+Ra_k-b_k k＝0,1,...,6 (2)

wherein: l is_k-the kth link rod length; e.g. of the type_k-the kth link unit vector; p is the position vector of the position of the movable platform in the static coordinate system, P ═ P_x,P_y,P_z}^T(ii) a R is an attitude matrix;a_kseven coordinates in total, a, of six vertexes and circle center of the movable platform_k＝{a_kx,a_ky,a_kz}^T；b_kSeven coordinates in total for six vertexes and circle center of the static platform, b_k＝{b_kx,b_ky,b_kz}^T. Because the movable platform and the static platform are both arranged in a plane, a_k、b_kIs 0, i.e.: a is_kz＝b _kz0, that is: a is_k＝{a_kx,a_ky,0}^T，b_k＝{b_kx,b_ky,0}^TIt can be seen that: the vertex coordinates and the circle center coordinates of the movable platform and the static platform need 28 parameters in total for description, and the 28 parameters can be described by 4 variables r₁、r₂、θ₁、θ₂The coordinates of each point are shown in table 1.

	axk	ayk	bxk	byk
					0	0	0	0	0
1	r2cos(-π/6-θ2)	r2sin(-π/6-θ2)	r1cos(-π/6-θ1)	r1sin(-π/6-θ1)
					2	r2cos(-π/6+θ2)	r2sin(-π/6+θ2)	r1cos(-π/6+θ1)	r1sin(-π/6+θ1)
3	r2cos(π/2-θ2)	r2sin(π/2-θ2)	r1cos(π/2-θ1)	r1sin(π/2-θ1)
					4	r2cos(π/2+θ2)	r2sin(π/2+θ2)	r1cos(π/2+θ1)	r1sin(π/2+θ1)
5	r2cos(7π/6-θ2)	r2sin(7π/6-θ2)	r1cos(7π/6-θ1)	r1sin(7π/6-θ1)
					6	r2cos(7π/6+θ2)	r2sin(7π/6+θ2)	r1cos(7π/6+θ1)	r1sin(7π/6+θ1)

TABLE 1 dynamic and static platform vertex coordinates

Wherein: r is₁-radius of the circle circumscribed by 6 vertices of the stationary platform; r is₂-the radius of the circumscribed circle of the 6 vertices of the mobile platform; theta₁-circle center half angle corresponding to the short side of the static platform; theta₂And a circle center half angle corresponding to the short side of the movable platform.

Let W be the position vector of the movable platform in the movable coordinate system, W ═ W_x,W_y,W_z}^TThen, P ═ RW exists, combining the orthogonality of R, and at the same time: w ═ R^TP。

A is to_k、b_kP, R and W are substituted for equation (2), and vector dot multiplication is performed on both sides with itself to obtain the 7 bar length square scalar equation as follows (the subscript k is omitted here):

L²-r₁ ²-r₂ ²＝-2A(ax bx+ay by)-2B(ax bx-ay by)+2C(ay bx-ax by)-2D(ay bx+ax by)-2bx P_x-2by P_y+2ax W_x+2ay W_y+P_p (3)

there are 9 unknown variables in equation (3): p_p、P_x、P_y、W_x、W_yA, B, C, D, the variables are related by the position and the posture of the movable platform. Wherein, P_pIs the square of the position vector P modulo the length, P_p＝P_x ²+P_y ²+P_z ²And P is_pCan be indirectly obtained by the value measured by the sensor，P_pAre known. The coefficients of the 9 variables are determined by the platform construction parameters and the pole length parameters.

First, η is taken from the formula (3)₁＝{P_p、P_x、P_y、W_x、W_yC, A }7 variables as principal variables, η₂The 2 variables are secondary variables. Then, with equation (3), η can be derived₁、η₂Expressed by a system of linear equations as in formula (4):

wherein: l is₀Measuring the length of the test rod for the sensor; k is a radical of₁、k₂Is formed by₁、r₂、θ₁、θ₂A determined constant; w_x0、W_y0、P_x0、P_y0C5 and A5 are represented by the formula r₁、r₂、θ₁、θ₂And length L of the rod_i(i is 1 to 6), namely: C. a is constant and unique.

2. Constructive kinematic forward solution equation and solution thereof

The kinematics positive solution problem is 7 rods of the known mechanism, and the position P and the attitude matrix R of the tail end motion platform are solved. Specifically, B and D required in equation (4) are solved.

(1) Variable in structure R

By W ═ R^TP can be obtained as follows:

α₁＝[W_x-(A+B)P_x-(D+C)P_y]/P_z (5)

α₂＝[W_y-(D-C)P_x-(A-B)P_y]/P_z (6)

further, the orthogonality of R can be:

γ₁＝(D+C)α₂-(A-B)α₁ (7)

γ₂＝(D-C)α₁-(A+B)α₂ (8)

γ₃＝(A+B)(A-B)-(D-C)(D+C) (9)

(2) structural equation

Again, based on the nature of R, such as its normalization, the following 11 compatibility equations containing B, D can be obtained for a Stewart-like parallel mechanism:

γ₁γ₂ W_z ²-(γ₁W_z)(γ₂W_z)＝0 (10-3)

α₁α₂ P_z ²-(α₁P_z)(α₂P_z)＝0 (10-9)

the 11 equations of the equations (10-1) to (10-11) are collectively written as follows:

f_1,jB⁴+f_2,jB³D+f_3,jB²D²+f_4,jBD³+f_5,jD⁴+f_6,jB³+f_7,jB²D+f_8,jBD²+f_9,jD³+f_10,jB²+f_11,jBD+f_12,jD²+f_13,jB+f_14,jD+f_15,j0 j-1, … 11 (11), where f is the result of earlier stage_i,jFor the function containing A to be different, now f_i,jAre constants determined by structural parameters and rod length parameters.

(3) Solving step 1: the first descending

5 equations are selected to solve B⁴、B³D、B²D²、BD³、D⁴By linear expression of a low-order polynomial, which is a combination polynomial with the highest order of B and D being 3, and substituting the other 6 equations, 6 equations about the 3-th-order combination of B and D can be obtained, which is abbreviated as:

h_1,jB³+h_2,jB²D+h_3,jBD²+h_4,jD³+h_5,jB²+h_6,jBD+h_7,jD²+h_8,jB+h_9,jD+h_10,j＝0 (j＝1～6)(12)

(4) solving step 2: second reduction

From these 6 equations, 4 were selected, and the 3 rd power combination of B and D was solved, i.e.: b is³、B²D、BD²、D³Is represented as follows:

wherein: i + j is 3, i is 0 to 3, and j is 0 to 3.

Subjecting the obtained product to

Substituting (12) the remaining two equations to obtain a system of equations containing B and D in the highest order of 2, in the form:

g_1,jB²+g_2,jBD+g_3,jD²+g_4,jB+g_5,jD+g_6,j＝0 (j＝5,6) (14)

(5) solving step 3: procedure of increasing order

Multiplying (14) by 1 and B, D respectively to obtain 6 new equations, wherein: 2 are combinations of the highest B and D orders of 2, and 4 are combinations of the highest B and D orders of 3. Solved by 4 equations combined by 3 power

The specific form is as follows:

wherein: i + j is 3, i is 0 to 3, and j is 0 to 3.

From (13) and (15) can be obtained:

the combination containing the highest order of B and D, which can be updated again, is the following:

t_1,jB²+t_2,jBD+t_3,jD²+t_4,jB+t_5,jD+t_6,j＝0 (j＝1～4) (16)

the equation set (16) is combined with 2-degree equations after the rising degree of the formula (14), and 6 equations with the highest degree of B and D being 2 degrees are total, therefore, the optional 5 equations adopt the linear equation set theory to solve B²、BD、D²Five monomial unknown variables, B, D, etc., and the solutions for B and D are unique.

3. Solving for position P and attitude R

The formula (4) is substituted by the structural parameter and the rod length parameter to obtain P_pPx, Py, Wx, Wy. Considering the actual working state of Stewart, without loss of generality, the selective platform is above the static platform, so P_zTaking a positive number, namely:

the position P can be found.

The variable α in R can be calculated by substituting the structural parameter, the rod length parameter, A, B, C, D, Px, Py, Pz, Wx, Wy into (5) - (9)₁、α₂、γ₁、γ₂、γ₃And then an attitude matrix R can be obtained. At this point, the kinematics resolution forward solution is complete.

In summary, P and R are unique.

4. Numerical calculation and analysis

A specific numerical example is adopted to verify the correctness and the effectiveness of the method.

The specific idea is as follows: the respective rod length is calculated by a set of inverse solution conditions P and R given in advance, and this rod length is substituted into the proposed algorithm, i.e.: performing kinematic positive solution calculation by the formulas (11) to (16); if the obtained P, R is consistent with the inverse solution condition, the correctness of the algorithm can be verified.

(1) Reverse solution: structural parameter theta₁＝0.046988π、θ₂＝0.286345πr₁＝r₂850, the vertex parameters of the mechanism are determined, the mechanism position vector P { -328.687,61.9785,1214.31}^TAnd an attitude matrix:

the attitude parameters can be solved: a is 0.976938, B is-0.007921, C is-0.168440, and D is 0.003469. Thus, the rod length calculated by inverse solution is:

L₀＝1259.534，L₁＝1637.068,L₂＝1490.996,L₃＝1403.641,

L₄＝1327.753,L₅＝1360.072,L₆＝1231.446。

(2) positive solution: by substituting the platform structure parameter and the rod length parameter obtained by inverse solution according to the proposed algorithm, a-0.976938 and C-0.168440 can be obtained according to equation (4). From (11) to (16), B is obtained²、BD、D²B, D, etc. 6 equations for 5 variables. And (3) arbitrarily taking 5 equations, and solving to obtain: b is²＝0.000063、BD＝-0.000027,D²＝0.000012,

B-0.007921 and D-0.003469. It is clear that both B and D are unique solutions, and it is clear that a unique set of A, B, C and D is obtained by the forward solution method, consistent with the values given by the reverse solution.

It can be calculated from equation (4): p_p＝1.586×10⁶Where Px is-328.687, Py is 61.9785, Wx is-105.514, Wy is-22.6052, and formula (17) is combined to yield Pz is 1214.31, i.e.: p { -328.687,61.9785,1214.31}^T。

And alpha can be obtained by the following formulas (5) to (9)₁＝0.18382、α₂＝-0.0223512、γ₁＝-0.17735、γ₂＝0.0532589、γ₃0.982706. 1 group of attitude matrixes can be obtained by substituting formula (1):

therefore, P, R calculated by the positive solution is identical to the data given by the negative solution. At this time, Stewart's position and posture are schematically shown in fig. 3.

So far, the whole forward solution solving process is completed, and the correctness and the effectiveness of the proposed forward solution method are verified by numerical value examples.

Claims (3)

1. A Stewart parallel mechanism kinematics forward solution method is provided, wherein the Stewart parallel mechanism comprises a lower platform and an upper platform, and the upper platform comprises at least 6 parallel connection unitsThe upper end of the testing rod is connected with the circumscribed circle center O of the upper platform₂The lower end is connected with the circle center O of the circumscribed circle of the lower platform₁And the testing rod is provided with a rod length L for measuring the testing rod₀The additional sensor of (3), the method comprising the steps of:

(1) establishing a posture matrix of the Stewart parallel mechanism:

the quaternion ε is expressed as: e ═ e-₁i+ε₂j+ε₃k+ε₀

In the formula: epsilon₁、ε₂、ε₃、ε₀∈R；i²＝j²＝k²＝-1；ij＝-ji＝k；jk＝-kj＝i；ki＝-ik＝j；ε^Tε＝1；

The specific form of the 3 rd order attitude matrix R expressed by unit quaternion is as follows:

in order to solve the positive solution conveniently by an algorithm and finally obtain a unique group of position gestures, some elements in the R are equivalently substituted by new symbols, the rest elements are also represented by the new symbols, and the gesture matrix R is equivalently represented as follows:

wherein,

C＝2ε₀ε₃，D＝2ε₁ε₂，γ₁＝2ε₀ε₂+2ε₁ε₃，

α₁＝-2ε₀ε₂+2ε₁ε₃，α₂＝2ε₀ε₁+2ε₂ε₃，

(2) separating primary and secondary variables:

the vector representation of the Stewart type parallel mechanism is as follows:

L_ke_k＝P+Ra_k-b_k k＝0,1,...,6

wherein: l is_kIs the k-th link rod length, when k is 0, L₀For testing rod O₁O₂The length of the rod; e.g. of the type_kIs the kth connecting rod unit vector; p is the position vector of the movable platform position in the static coordinate system, and P is { P ═ P_x,P_y,P_z}^T(ii) a R is an attitude matrix; a is_kSeven coordinates including six vertexes and circle center of the movable platform, a_k＝{a_xk,a_yk,a_zk}^T；b_kSeven coordinates including six vertexes and circle center of the static platform, b_k＝{b_xk,b_yk,b_zk}^T；

From this vector form, a scalar equation is obtained:

wherein: l is_kThe kth connecting rod length; r is₁The radius of a circle is circumscribed to the static platform; r is₂The radius of a circumscribed circle of the movable platform; ax, ay, az are k-th vertex a of the movable platform_kThe respective components of (a); bx, by and bz are k-th vertexes b of the movable platform_kThe respective components of (a); p_x、P_yIs a component of the position vector P of the moving platform; w_x、W_yIs a component of the position vector W of the stationary platform; p_pBeing the square of the position vector P modulo the length,

and P is_pCan be used forValue L measured by a sensor₀Indirectly obtain, P_pIs known;

and (3) carrying out primary and secondary variable separation on the scalar equation to obtain a linear equation system with the primary and secondary variable separation as follows:

wherein: l is₀The length of the test rod measured by the sensor is used;

k₁＝-r₂Sin[θ₁+2θ₂]Csc[θ₁-θ₂]；k₂＝r₁Sin[2θ₁+θ₂][-Csc[θ₁-θ₂]]；

therefore, according to the structural parameter r of the Stewart parallel mechanism₁、r₂、θ₁、θ₂And rod length parameter L_kk 0, 1.., 6 may determine variables C and a in the pose matrix R;

(3) and structural variables:

according to the orthogonality and the normalization of R, the following constraint conditions exist in 9 elements in R:

(A+B)(C+D)+(D-C)(A-B)+γ₁γ₂＝0

(A+B)(D-C)+(C+D)(A-B)+α₁α₂＝0

γ₁＝(D+C)α₂-(A-B)α₁

γ₂＝(D-C)α₁-(A+B)α₂

γ₃＝(A+B)(A-B)-(D-C)(D+C)

the position of the movable platform is in a static coordinate system O₁x₁y₁z₁The position vector P and its moving coordinate system O₂x₂y₂z₂The position vectors W in (1) have the following relationship:

P＝R·W

from this, the following relation can be obtained:

P_x＝(A+B)W_x+(D-C)W_y+γ₁W_z，

P_y＝(C+D)W_x+(A-B)W_y+γ₂W_z；

wherein, P_x、P_yIs a component of the position vector P of the moving platform; w_x、W_y、W_zIs a component of the position vector W of the stationary platform;

the two formulas are solved as follows:

γ₁W_z＝P_x-(A+B)W_x-(D-C)W_y

γ₂W_z＝P_y-(C+D)W_x-(A-B)W_y

meanwhile, according to the orthogonality of the attitude matrix, the following can be obtained:

W＝R^T·P

the following relationships can thus be obtained:

W_x＝(A+B)P_x+(C+D)P_y+α₁P_z

W_y＝(D-C)P_x+(A-B)P_y+α₂P_z

by the above two formulas to obtain alpha₁P_z、α₂P_zExpression (c):

α₁P_z＝W_x-(A+B)P_x-(C+D)P_y

α₂P_z＝W_y-(D-C)P_x-(A-B)P_y

(4) construction compatibility equation

The following 12 compatibility equations containing B, D can be obtained for a Stewart-like parallel mechanism:

eq1:

eq2:

eq3:

eq4:

eq5:

eq6:

eq7:

eq8:

eq9:

eq10:

eq11:

eq12:

wherein eq6 and eq12 are fully equivalent;

(5) equation of constructive kinematics

For the Stewart parallel mechanism, substituting the variables in the step (3) into 11 capacitance equations in the step (4) to obtain 11 correlation equations containing B and D, and uniformly writing the 11 capacitance equations into the following form:

f_1,jB⁴+f_2,jB³D+f_3,jB²D²+f_4,jBD³+f_5,jD⁴+f_6,jB³+f_7,jB²D+f_8,jBD²+f_9,jD³+f_10,jB²+f_11,jBD+f_{12, j}D²+f_13,jB+f_14,jD+f_15,j＝0 j＝1,…11

f_i,jare all determined by the structural parameter r₁、r₂、θ₁、θ₂And rod length parameter L_kA determined constant;

(6) solving equations

And (3) simplifying 11 compatible equations into a quadratic linear equation with 6 constant coefficients and about B and D according to the concept of descending, descending and ascending for the equation in the step (5) and by utilizing the fact that the result of the third ascending is equal to the result of the second descending, wherein the equation set is in the form as follows:

t_1,jB²+t_2,jBD+t_3,jD²+t_4,jB+t_5,jD+t_6,j＝0；j＝1～6

and solving the equation system to obtain unique solutions of B and D.

2. The Stewart parallel mechanism kinematics forward solution method according to claim 1, wherein when the upper and lower platforms are parallel, only the following compatibility equation is used in step (4),

eq6:

eq7:

eq8:

eq9:

eq10:

eq11:

firstly, solving the 4 power combination B of B and D from the equations eq 7-eq 11⁴、B³D、B²D²、BD³、D⁴Substituting a set of solutions into equation eq6, the form is: t is t₁·B²+t₁·D²Form 0, t₁Is a constant coefficient related to the structural parameter and the rod length; that is, B ═ D ═ 0 can be solved, B, D is unique.

3. Stewart parallel mechanism kinematics forward solution method according to claim 1 or 2, characterized in that in step (2), the structure parameter r is determined₁、r₂、θ₁、θ₂And rod length parameter L_kSystem of substitution equations

Find P_pPx, Py, Wx, Wy; wherein, P_zTaking a positive number, namely:

obtaining a position vector P; substituting the structural parameters, rod length parameters, A, B, C, D and Px, Py, Pz, Wx, Wy into stepsEquation α in step (3)₁P_z＝W_x-(A+B)P_x-(C+D)P_y；α₂P_z＝W_y-(D-C)P_x-(A-B)P_y；γ₁＝(D+C)α₂-(A-B)α₁；γ₂＝(D-C)α₁-(A+B)α₂；γ₃The variable α in R was calculated as (a + B) (a-B) - (D-C) (D + C)₁、α₂、γ₁、γ₂、γ₃And then an attitude matrix R can be obtained.

Images