Construction method and analysis method of kinematic analysis model of pipe splicing machine

Abstract

This invention relates to a method for constructing and analyzing the kinematic analytical model of a segment assembly machine. The method includes the following steps: establishing multiple coordinate systems with all joint strokes at zero, including an initial coordinate system with the foot of the perpendicular drawn from the center of the rotary table to the vertical plane containing the fixed end of the translation cylinder as its origin; an intermediate coordinate system with the center of the top surface of the mechanical gripper as its origin; a target coordinate system with the end of the mechanical gripper's hook as its origin; a first transition coordinate system connecting the initial and intermediate coordinate systems based on the motion mechanism of each joint; and a second transition coordinate system connecting the intermediate and target coordinate systems. The total transformation matrix between the initial and target coordinate systems is calculated based on the stroke of each joint and the relationships between the coordinate systems, and represented by attitude and position matrices to form the kinematic analytical model. This invention provides a kinematic analytical tool for assembly machines with a six-degree-of-freedom mechanical gripper.

Description

Technical Field

[0001] This invention relates to the field of shield tunneling technology, and in particular to a method for constructing and analyzing the kinematic analytical model of a segment assembly machine. Background Technology

[0002] The segment assembly machine is a key component of a tunnel boring machine (TBM) that is manually controlled via a handle to assemble multiple tunnel segments into a ring. Segment assembly is a high-risk operation in TBM construction, and the skill and experience of the assemblers are decisive factors in determining the final tunnel quality. Due to design and manufacturing errors, inherent structural gaps, load-induced deformation of the assembly machine, and the precision of electrical control, traditional segment assembly machines exhibit significant differences between their theoretical and actual end-effector poses under given target motion values ​​for each joint. With the increasing application of artificial intelligence in traditional TBM construction, breakthroughs in unmanned, automated segment assembly technology are urgently needed. Summary of the Invention

[0003] To address the aforementioned issues, this invention provides a method for constructing and analyzing the kinematic analytical model of a segment assembly machine, which solves the problem that it is impossible to manually calculate the end pose and target motion of each joint of this type of assembly machine online during the automatic segment assembly process.

[0004] This invention is achieved through the following scheme: a method for constructing a kinematic analytical model of a segment assembly machine, wherein the segment assembly machine includes a mechanical gripper with six degrees of freedom, a V-shaped lifting mechanism for driving the mechanical gripper to move up and down, a rotary table for driving the V-shaped lifting mechanism to rotate, and a translation cylinder for driving the rotary table to move longitudinally. The construction method includes the following steps:

[0005] Multiple coordinate systems are established with all joints of the segment assembly machine having zero travel. These coordinate systems include an initial coordinate system with the foot of the perpendicular drawn from the center of the rotary table to the vertical plane containing the fixed end of the translation cylinder as its origin; an intermediate coordinate system with the center of the top surface of the mechanical gripper as its origin; a target coordinate system with the end of the gripper's hook as its origin; at least one first transition coordinate system that connects the initial and intermediate coordinate systems based on the motion mechanism of the translation cylinder, the rotary table, and the V-shaped lifting mechanism; and at least one second transition coordinate system that connects the intermediate and target coordinate systems based on the motion mechanism of the mechanical gripper.

[0006] Based on the motion strokes of the translation cylinder, the V-shaped lifting mechanism, the rotary table, and the mechanical gripper in six degrees of freedom, and combined with the correlation between the coordinate systems, the total transformation matrix between the initial coordinate system and the target coordinate system is calculated;

[0007] The total transformation matrix is ​​converted into a form characterized by the attitude matrix and position matrix of the segment to be grabbed and the position to be assembled, thereby forming a kinematic analytical model for analyzing the motion of the segment assembly machine.

[0008] A further improvement to the method for constructing the kinematic analytical model of the segment assembly machine of the present invention lies in establishing six coordinate systems, the steps of which include:

[0009] The origin O0 is obtained by drawing a perpendicular line from the center of the rotary table to the vertical plane where the fixed end of the translation cylinder is located. The Z0 axis is the positive direction of the tunnel segment assembly machine, and the vertical direction is the positive direction of the Y0 axis. The initial coordinate system O0X0Y0Z0 is established in right-hand right-angle form.

[0010] Based on the initial coordinate system O0X0Y0Z0, move r1 in the positive direction of the Z0 axis and rotate λ1 around the Z0 axis to obtain the first first transition coordinate system O1X1Y1Z1, where r1 is the total length of the translation cylinder and λ1 is the rotation angle of the rotary table.

[0011] Based on the first first transition coordinate system O1X1Y1Z1, move q2 in the opposite direction of the Y1 axis to obtain the second first transition coordinate system O2X2Y2Z2, where q2 is the distance from O1 to the center of the line connecting the hinge points of the two vertical lifting cylinders of the V-shaped lifting mechanism.

[0012] Based on the second first transition coordinate system O2X2Y2Z2, move q3 in the opposite direction of the Y2 axis to obtain the intermediate coordinate system O3X3Y3Z3, where the magnitude of q3 satisfies that the origin O3 falls on the center of the upper top surface of the mechanical gripper;

[0013] Based on the intermediate coordinate system O3X3Y3Z3, move α, β, and γ respectively in the directions of X3 axis, Y3 axis, and Z3 axis, and rotate along X3 axis, Y3 axis, and Z3 axis respectively. Angles θ and ψ yield a second transition coordinate system O4X4Y4Z4, where α, β, γ, The magnitudes of θ and ψ satisfy the condition that the origin O4 falls at the center of the lower surface of the mechanical gripper;

[0014] Based on the second transition coordinate system O4X4Y4Z4, move q5 in the opposite direction of the Y4 axis to obtain the target coordinate system O5X5Y5Z5, where the value of q5 satisfies that the origin O5 falls on the end of the grappling hook.

[0015] A further improvement to the method for constructing the kinematic analytical model of the segment assembly machine of the present invention lies in that the method for calculating the total transformation matrix is ​​as follows:

[0016] Calculate the transformation matrix T between each coordinate system based on the correlation between adjacent coordinate systems. i (i = 1 to 5), where the expression for the transformation matrix T1 between the initial coordinate system and the first transition coordinate system is:

[0017]

[0018] The expression for the transformation matrix T between the first first transition coordinate system and the second first transition coordinate system is:

[0019]

[0020] The expression for the transformation matrix T3 between the second transition coordinate system and the intermediate coordinate system is:

[0021]

[0022] The expression for the transformation matrix T4 between the intermediate coordinate system and the second transition coordinate system is:

[0023]

[0024] The expression for the transformation matrix T5 between the second transition coordinate system and the target coordinate system is:

[0025]

[0026] The total transformation matrix T is: T = T1T2T3T4T5.

[0027] A further improvement to the method for constructing the kinematic analytical model of the segment assembly machine of the present invention is that: the mechanical gripper includes a small top plate forming the upper top surface, a large bottom plate forming the lower bottom surface, and six hydraulic cylinders connected in parallel between the edges of the small top plate and the large bottom plate. The origin O3 of the intermediate coordinate system falls on the center of the small top plate, the origin O4 of the second transition coordinate system falls on the center of the large bottom plate, and the gripper hook is connected to the center of the large bottom plate. The six degrees of freedom of the gripper hook can be realized through the extension and retraction of the six hydraulic cylinders.

[0028] The transformation matrix T4 between the intermediate coordinate system and the second transition coordinate system is calculated as follows: based on the geometric relationships between the side vectors of the quadrilateral formed by the origin O3, origin O4, and the two hinged ends of the hydraulic cylinder, Newton's iteration method is used to solve for α, β, γ. θ, ψ, and use α, β, γ, The transformation matrix T4 is calculated using θ and ψ.

[0029] A further improvement to the method for constructing the kinematic analytical model of the segment assembly machine of the present invention lies in that the total transformation matrix T, characterized by the attitude matrix and position matrix of the segment to be grasped and the position to be assembled, is:

[0030]

[0031] in: The attitude matrix, Let r be the position matrix; 11 =cos(λ1+ψ)cosθ; r 21 =sin(λ1+ψ)cosθ; r 31 = -sinθ; s1=αcosλ1-βsinλ1-(q2+q3)sinλ1+q5r 12 ;s2=αsinλ2+βcosλ2+(q2+q3)cosλ1+q5r 22 ;s3=γ+r1+q5r 32 .

[0032] This invention provides a kinematic analysis method for a segment assembly machine. Given the travel data of the translation cylinder, the V-shaped lifting mechanism, the rotary table, and the mechanical gripper in six degrees of freedom, the method uses the kinematic analysis model described above to perform forward kinematic analysis and solve for the current actual position of the end of the gripper hook.

[0033] This invention provides another kinematic analysis method for a segment assembly machine. Given the target position of the end of the gripper hook, the method employs the inverse kinematic analysis model described in any of the preceding claims to solve for the stroke data of the translation cylinder, the V-shaped lifting mechanism, the rotary table, and the mechanical gripper in six degrees of freedom. The method includes the following steps:

[0034] First, the mechanical gripper is set to zero in each of the six degrees of freedom. Based on the target position at the end of the gripper hook, the kinematic analytical model is used to perform inverse kinematics solution to obtain the stroke data of the translation cylinder, the V-shaped lifting mechanism and the rotary table.

[0035] The translation cylinder, the V-shaped lifting mechanism, and the rotary table are controlled according to the stroke data to perform the corresponding actions.

[0036] After execution, the actual execution results fed back by the translation cylinder, the V-shaped lifting mechanism and the rotary table, as well as the actual position of the end of the grab hook are obtained;

[0037] The strokes of the translation cylinder, the V-shaped lifting mechanism, and the rotary table are locked, and then the difference between the actual position and the target position of the end of the grab hook is calculated to obtain the difference value.

[0038] Based on the difference, the kinematic analytical model is used to perform inverse kinematics again, and the difference is transformed to the intermediate coordinate system. Then, based on the difference in the intermediate coordinate system, the target stroke of the mechanical gripper in each of the six degrees of freedom is calculated.

[0039] The kinematic analytical model of this invention establishes the correlation between the motion parameters of each joint and the position parameters of the end of the gripper. It can perform forward kinematics on the end pose of the assembly machine based on the motion parameters of each joint, and inverse kinematics on the target motion of each joint based on the position parameters of the end of the gripper. This solves the problem that it is impossible to manually calculate the end pose and target motion of each joint online during the automatic assembly of tunnel segments, and provides a kinematic analytical tool for assembly machines with six-degree-of-freedom mechanical grippers. Attached Figure Description

[0040] Figure 1 A flowchart illustrating the kinematic analytical model construction method of the present invention is shown.

[0041] Figure 2 The diagram shows a structural comparison of the front and side views of the segment assembly machine in this invention.

[0042] Figure 3 A schematic diagram illustrating the construction process of each coordinate system in the kinematic analytical model of the present invention is shown.

[0043] Figure 4 A top view of the mechanical gripper with six degrees of freedom in this invention is shown.

[0044] Figure 5 The equivalent geometry of the mechanical gripper with six degrees of freedom in this invention is shown.

[0045] Figure 6 A schematic diagram of the present invention is shown, which is based on the equivalent geometry of a mechanical gripper for solving the problem.

[0046] Figure 7 The diagram illustrates the inverse kinematic analysis process of this invention. Detailed Implementation

[0047] To address the challenge of manually calculating the end-effector pose and target motion quantities of each joint during automated segment assembly, this invention provides a method for constructing and analyzing a kinematic analytical model of a segment assembly machine. The following detailed description, in conjunction with accompanying drawings, further illustrates this method for constructing and analyzing the kinematic analytical model of the segment assembly machine using specific embodiments.

[0048] See Figure 2 As shown, a segment assembly machine includes: a mechanical gripper 40 with six degrees of freedom, and a V-shaped lifting mechanism 30 for driving the mechanical gripper 40 to move up and down. The V-shaped lifting mechanism 30 includes two vertically arranged lifting cylinders 31 and two inclined lifting beams 32. One end of each of the two inclined lifting beams 32 is hinged to the bottom end of the two vertically arranged lifting cylinders 31, and the other end of each of the two inclined lifting beams 32 is hinged to opposite sides of the mechanical gripper 40. By controlling the synchronous extension and retraction of the two vertically arranged lifting cylinders 31 and the movement of the two inclined lifting beams 32... The lifting beam 32 drives the mechanical gripper 40 to move up and down; a turntable 20, which drives the V-shaped lifting mechanism 30 to rotate, is connected between two vertical lifting cylinders 31. The rotation of the turntable 20 drives the mechanical gripper 40 to rotate via the V-shaped lifting mechanism 30; and a translation cylinder 10, which drives the turntable 20 to move longitudinally, drives the mechanical gripper 40 to move horizontally via the turntable 20 and the V-shaped lifting mechanism 30. Combined with the six degrees of freedom of the mechanical gripper 40 itself, the hook 41 connected to the end of the mechanical gripper 40 can achieve nine degrees of freedom of movement.

[0049] See Figures 1-3 As shown, this invention constructs a kinematic analytical model for the aforementioned special type of segment assembly machine. The method for constructing this model includes the following steps:

[0050] Step 1: Establish multiple coordinate systems with all moving joints of the segment assembly machine (including the translation cylinder 10, the rotary table 20, the two vertical lifting cylinders 31, and the moving joints that realize the six-degree-of-freedom movement of the mechanical gripper 40) having zero stroke. In this embodiment, six coordinate systems are preferably established, as follows:

[0051] The origin O0 is obtained by drawing a perpendicular line from the center of the rotary table 20 to the vertical plane where the fixed end of the translation cylinder 10 is located. The Z0 axis is the positive direction of the segment assembly machine's advancing direction, and the vertical direction is the positive direction of the Y0 axis. The initial coordinate system 00X0Y0Z0 is established in right-hand right-angle form.

[0052] Based on the initial coordinate system O0X0Y0Z0, move r1 in the positive direction of the Z0 axis and rotate λ1 around the Z0 axis to obtain the first transition coordinate system O1X1Y1Z1, where r1 is the total length of the translation cylinder 10, λ1 is the rotation angle of the turntable 20, so that the extension line of the Y1 axis in the opposite direction points to the direction of the mechanical gripper 40, and the origin O1 is the foot of the perpendicular line obtained by drawing a perpendicular line from the center of the turntable 20 to the vertical plane containing the central axis of the vertical lifting cylinder 31.

[0053] Based on the first transition coordinate system O1X1Y1Z1, move q2 in the opposite direction of the Y1 axis to obtain the second transition coordinate system O2X2Y2Z2, where q2 is the distance from O1 to the center of the line connecting the hinge points of the two vertical lifting cylinders 31, and the stroke of the vertical lifting cylinders 31 must be taken into account.

[0054] Based on the second first transition coordinate system O2X2Y2Z2, move q3 in the opposite direction of the Y2 axis to obtain the intermediate coordinate system O3X3Y3Z3. The size of q3 satisfies that the origin O3 falls on the center of the top surface of the mechanical gripper 40. q3 is a fixed structural parameter that can be measured.

[0055] Based on the intermediate coordinate system O3X3Y3Z3, move α, β, and γ respectively in the directions of X3 axis, Y3 axis, and Z3 axis, and rotate along X3 axis, Y3 axis, and Z3 axis respectively. Angles θ and ψ are used to obtain a second transition coordinate system O4X4Y4Z4, where α, β, γ, The magnitudes of θ and ψ satisfy the condition that the origin O4 lies at the center of the lower surface of the mechanical gripper 40.

[0056] Based on the second transition coordinate system O4X4Y4Z4, move q5 in the opposite direction of the Y4 axis to obtain the target coordinate system O5X5Y5Z5, where the size of q5 satisfies that the origin O5 falls at the end of the grappling hook 41.

[0057] Step 2: Based on the motion strokes of the translation cylinder 10, the V-shaped lifting mechanism 30, the rotary table 20, and the mechanical gripper 40 in six degrees of freedom (i.e., α, β, γ, ... θ, ψ) and combine the correlation between the coordinate systems to calculate the total transformation matrix between the initial coordinate system and the target coordinate system.

[0058] Specifically, the total transformation matrix is ​​calculated as follows:

[0059] Calculate the transformation matrix T between each coordinate system based on the correlation between adjacent coordinate systems. i (i = 1 to 5), where the expression for the transformation matrix T1 between the initial coordinate system and the first transition coordinate system is:

[0060]

[0061] The expression for the transformation matrix T between the first first transition coordinate system and the second first transition coordinate system is:

[0062]

[0063] The expression for the transformation matrix T3 between the second transition coordinate system and the intermediate coordinate system is:

[0064]

[0065] The expression for the transformation matrix T4 between the intermediate coordinate system and the second transition coordinate system is:

[0066]

[0067] The expression for the transformation matrix T5 between the second transition coordinate system and the target coordinate system is:

[0068]

[0069] The total transformation matrix T is: T = T1T2T3T4T5.

[0070] Step 3: The total transformation matrix T, characterized by the attitude matrix and position matrix of the segment to be grabbed and the position to be assembled, is:

[0071]

[0072] in: For this attitude matrix, This is the position matrix; r 11 =cos(λ1+ψ)cosθ; r 21 =sin(λ1+ψ)cosθ; r 31 = -sinθ; s1=αcosλ1-βsinλ1-(q2+q3)sinλ1+q5r 12 ;s2=αsinλ1+βcosλ1+(q2+q3)cosλ1+q5r 22 ;s3=γ+r1+q5r 32 .

[0073] Therefore, the position coordinates of the end of the segment assembly machine's grab hook 41 are (s1, s2, s3), and the three attitude angles, rotation angle, yaw angle, and pitch angle, are respectively: Atan2(r 32 ,r33 )=λ1+ψ;Asin(r 31 ) = θ; This leads to the development of a kinematic analytical model for analyzing the motion of the segment assembly machine.

[0074] The calculation of the transformation matrix T4 depends on the structure of the mechanical gripper 40. In this embodiment, refer to... Figure 4 and Figure 5 As shown, the mechanical gripper 40 includes a small top plate forming the upper top surface, a large bottom plate forming the lower bottom surface, and six hydraulic cylinders (represented as Q1B2, P2B1, P1A2, R2A1, R1C2, and Q2C1 respectively) connected in parallel and hinged between the edges of the small top plate and the large bottom plate. The origin O3 of the intermediate coordinate system falls on the center of the small top plate (represented as O in the figure). u The origin O4 of the second transition coordinate system falls at the center of the large base plate (represented as O in the figure). l The grab 41 is connected to the center of the large base plate, and its six degrees of freedom of motion can be achieved through the extension and retraction of the six hydraulic cylinders. Based on the structure of the mechanical gripper 40 described above, the transformation matrix T4 between the intermediate coordinate system and the second transition coordinate system is calculated as follows:

[0075] Given the actual lengths of six hydraulic cylinders and the relative positions of points on the top and bottom surfaces, calculate the position and attitude change of the bottom surface relative to the top surface. Since the calculation methods for all six cylinders are identical, we will use one cylinder, such as Q1B2, as an example. (Refer to...) Figure 6 As shown:

[0076] Because the six rods are not absolutely independent, this embodiment uses Newton's iteration method to solve the equations. For ease of expression, variables requiring iteration are represented by Greek letters, and constants are represented by English letters. Where there is ambiguity in the coordinate system, use... 3 The representation of X expresses the X parameters in the coordinate system O3X3Y3Z3.

[0077] for and There exists a transformation matrix T, therefore the following equation holds:

[0078]

[0079] Furthermore, the transformation matrix T is based on Euler angles It can be represented as:

[0080]

[0081] Among them, ψ, θ, These are unknowns that need to be solved using Newton's iteration method, therefore they are represented by Greek letters, ψ, θ, The iteration results are the three angles from coordinate system O3X3Y3Z3 to coordinate system O4X4Y4Z4, corresponding to the rotation angle, yaw angle, and pitch angle, respectively. The iteration results of α, β, and γ are... The three components in the coordinate system O3X3Y3Z3.

[0082] Let the iteration vector be:

[0083]

[0084] The iteration function is:

[0085]

[0086] in, Let Q1B2 be the unknown vector corresponding to one of the six hydraulic cylinders in the coordinate system O3X3Y3Z3. The iterative results should satisfy |l i | a This is the measured length of hydraulic cylinder Q1B2.

[0087] Therefore, the zonal results in coordinate system O3X3Y3Z3 satisfy the following geometric relations:

[0088]

[0089] because The measurement was performed in coordinate system O4X4Y4Z4. Therefore, if we transform to coordinate system O3X3Y3Z3, the following conditions must be met:

[0090]

[0091] For convenience, direct writing Expanding equation (1) gives:

[0092]

[0093] To facilitate finding partial derivatives in Newton's iteration, equation (3) needs to be further expanded. Let... The three components are a i,1 a i,2 a i,3 , The three components are b i,1 b i,2 b i,3 ,but:

[0094]

[0095] Find the partial derivative:

[0096]

[0097]

[0098]

[0099]

[0100]

[0101]

[0102] The iterative formula is then:

[0103]

[0104] α, β, γ, ψ, θ can be solved using Newton's iteration. As a result, the final transformation matrix T4 can be calculated as follows:

[0105]

[0106] This invention provides a kinematic analysis method for a segment assembly machine. Given the known stroke data of the translation cylinder 10, the V-shaped lifting mechanism 30, the rotary table 20, and the mechanical gripper 40 in six degrees of freedom, a forward kinematic analysis method, as described above, is used to determine the current actual position of the end of the gripper 41. It should be noted that for the aforementioned mechanical gripper structure with six cylinders achieving six degrees of freedom motion, the stroke data of the six degrees of freedom (i.e., α, β, γ, ψ, θ, ...) can be obtained based on the stroke of the six cylinders using the aforementioned calculation method of the transformation matrix T4. ).

[0107] This invention also provides a kinematic analysis method for a segment assembly machine. Given the target position at the end of the gripper 41, the method employs the inverse kinematic analysis model described in any of the preceding claims to solve for the stroke data of the translation cylinder 10, the V-shaped lifting mechanism 30, the rotary table 20, and the mechanical gripper 40 in six degrees of freedom. (See reference...) Figure 7 As shown, the steps include:

[0108] Step S1: First, set the stroke of the mechanical gripper 40 to zero in each of the six degrees of freedom (that is, set the stroke of the six cylinders of the mechanical gripper 40 to zero). Based on the target position at the end of the hook 41, use the kinematic analytical model to perform the inverse kinematic solution to solve the stroke data of the translation cylinder 10, the V-shaped lifting mechanism 30 and the rotary table 20.

[0109] Specifically, after setting the mechanical gripper 40 to zero in each of the six degrees of freedom, the transformation matrix T4 can be expressed as:

[0110]

[0111] Where α, β, and γ are equal to 0, q4, and 0 respectively. q4 is the relative distance between the origin of coordinate system O3X3Y3Z3 and coordinate system O4X4Y4Z4 when the stroke of the six cylinders of the mechanical gripper 40 is set to zero. It is a fixed parameter that can be directly measured.

[0112] The correct solution can be further expressed with respect to the position coordinates (s1, s2, s3) as follows:

[0113] s1=αcosλ1-βsinλ1-(q2+q3)sinλ1+q5r 12 =-(q2+q3+q4+q5)sinλ1

[0114] s2=αsinλ1+βcosλ1+(q2+q3)cosλ1+q5r 22 = (q2+q3+q4+q5)cosλ1

[0115] s3=γ+r1+q5r 32 =r1

[0116] Therefore, the target position coordinates at the end of the mechanical gripper hook 41 are known ( 0 s 1,0 , 0 s 2,0 , 0 s 3,0 ) and q4, q5, can be used to solve for the unknowns: the stroke data r1 of the translation cylinder 10, the stroke data q2 of the V-shaped lifting mechanism 30, and the stroke data λ1 of the rotary table 20.

[0117]

[0118] Step S2: Control the translation cylinder 10, the V-shaped lifting mechanism 30, and the rotary table 20 according to the stroke data r1, λ1, q2.

[0119] Step S3: After execution, the actual execution results r1′, λ1′, and q2′ fed back by the translation cylinder 10, the V-shaped lifting mechanism 30, and the rotary table 20, as well as the actual position of the end of the grab hook 41, are obtained. 0 s1, 0 s2, 0 s3).

[0120] Step S4: Lock the stroke of the translation cylinder 10, the V-shaped lifting mechanism 30, and the rotary table 20, and then determine the actual position of the end of the grab hook 41. 0 s1, 0 s2, 0 s3) and target location ( 0 s 1,0 , 0 s 2,0 , 0 s 3,0 By performing the subtraction, we can obtain the difference between the actual position and the target position in the coordinate system O0X0Y0Z0.

[0121]

[0122] Step 5: Based on this difference The inverse kinematic solution is then performed again using the kinematic analytical model to obtain the difference. Transform to coordinate system O3X3Y3Z3:

[0123]

[0124] Therefore, in coordinate system O3X3Y3Z3, the rotation angle of the end of the grappling hook 41 is... 3 ψ, deflection angle 3 θ and pitch angle The target values ​​are as follows:

[0125] 3 ψ=λ0-λ1′

[0126] 3 θ=θ0

[0127]

[0128] Further based on the action mechanism of the six hydraulic cylinders in the mechanical gripper 40 and the relationship between the cylinder length and three angles ( 3 ψ、 3 θ and By understanding the relationship between the six cylinders, the target stroke of the six cylinders can be calculated.

[0129] See Figure 6 As shown, let the coordinate system be O3X3Y3Z3. Then we have:

[0130]

[0131] in,

[0132]

[0133] From the above equation, we can solve for x, y, z, and

[0134] Taking hydraulic cylinder Q1B2 as an example, it satisfies the following formula:

[0135]

[0136] In the above formula It is a vector in coordinate system O3X3Y3Z3, and its corresponding vector can be measured in coordinate system O4X4Y4Z4. Then multiply by the transformation matrix T on the left, that is:

[0137]

[0138] Then, the target length of hydraulic cylinder Q1B2 can be calculated:

[0139]

[0140] Similarly, the target lengths of other cylinders can be calculated, and further, the target stroke of each cylinder can be obtained.

[0141] The present invention has been described in detail above with reference to the accompanying drawings and embodiments. Those skilled in the art can make various modifications to the present invention based on the above description. Therefore, certain details in the embodiments should not be construed as limiting the present invention, and the scope of protection of the present invention shall be defined by the appended claims.

Claims

1. A method for constructing a kinematic analytical model of a segment assembly machine, the segment assembly machine comprising a mechanical gripper with six degrees of freedom, a V-shaped lifting mechanism for driving the mechanical gripper to move up and down, a rotary table for driving the V-shaped lifting mechanism to rotate, and a translation cylinder for driving the rotary table to move longitudinally, characterized in that, The construction method includes the following steps: Establish six coordinate systems with all moving joints of the segment assembly machine at zero travel distance. The steps include: The origin is the foot of the perpendicular line drawn from the center of the rotary table to the vertical plane containing the fixed end of the translation cylinder. Taking the opposite direction of the tunnel segment assembly machine's advancing direction as... Positive axis direction, with the vertical direction as Establish an initial coordinate system in right-hand rectangular form along the positive axis. ; In the initial coordinate system On the basis of Move in the positive direction of the axis and around Axis rotation Obtain the first transition coordinate system ,in, The total length of the translation cylinder is... The rotation angle of the rotary table; In the first transition coordinate system On the basis of axis reverse direction movement Obtain the second first transition coordinate system ,in, for The distance to the center of the line connecting the hinge points of the two vertical lifting cylinders of the V-shaped lifting mechanism; In the second first transition coordinate system On the basis of axis reverse direction movement Obtain the intermediate coordinate system ,in, The size satisfies the origin It rests at the center of the upper top surface of the mechanical gripper; In the intermediate coordinate system Based on the above, respectively to axis, axis, Axial movement and along respectively axis, axis, Shaft rotation An angle is used to obtain a second transition coordinate system. ,in, The size satisfies the origin It rests at the center of the lower surface of the mechanical gripper; Second transition coordinate system On the basis of axis reverse direction movement Obtain the target coordinate system ,in, The size satisfies the origin It landed at the end of the hook; Based on the motion strokes of the translation cylinder, the V-shaped lifting mechanism, the rotary table, and the mechanical gripper in six degrees of freedom, and combined with the correlation between the coordinate systems, the total transformation matrix between the initial coordinate system and the target coordinate system is calculated; The total transformation matrix is ​​converted into a form characterized by the attitude matrix and position matrix of the segment to be grabbed and the position to be assembled, thereby forming a kinematic analytical model for analyzing the motion of the segment assembly machine.

2. The method for constructing the kinematic analytical model of the segment assembly machine as described in claim 1, characterized in that, The method for calculating the total transformation matrix is ​​as follows: Calculate the transformation matrix between each coordinate system based on the correlation between adjacent coordinate systems. The transformation matrix between the initial coordinate system and the first transition coordinate system. The expression is: ； Transformation matrix between the first transition coordinate system and the second transition coordinate system The expression is: ； The second transformation matrix between the first transition coordinate system and the intermediate coordinate system The expression is: ； Transformation matrix between intermediate coordinate system and second transition coordinate system The expression is: ； Transformation matrix between the second transition coordinate system and the target coordinate system The expression is: ； The total transformation matrix for: .

3. The method for constructing the kinematic analytical model of the segment assembly machine as described in claim 2, characterized in that: The mechanical gripper includes a small top plate forming the upper top surface, a large bottom plate forming the lower bottom surface, and six hydraulic cylinders connected in parallel and hinged between the edges of the small top plate and the large bottom plate. The origin of the intermediate coordinate system is... It falls at the center of the small top plate, the origin of the second transition coordinate system. The hook is located at the center of the large base plate and is connected to the center of the large base plate. The six degrees of freedom of the hook can be realized through the extension and retraction of the six hydraulic cylinders. Transformation matrix between intermediate coordinate system and second transition coordinate system The calculation method is based on the origin. ,origin The geometric relationships between the side vectors of the quadrilateral formed by the two hinged ends of the hydraulic cylinder are solved using Newton's iteration method. and use Calculate the transformation matrix .

4. The method for constructing the kinematic analytical model of the segment assembly machine as described in claim 2, characterized in that, The total transformation matrix is ​​characterized by the attitude matrix and position matrix of the segment to be captured and the position to be assembled. for: ； in: The attitude matrix, The position matrix; ; ; ; ; ; ; ; ; ; ; ; .

5. A kinematic analysis method for a segment assembly machine, characterized in that, Given the travel data of the translation cylinder, the V-shaped lifting mechanism, the rotary table, and the mechanical gripper in six degrees of freedom, the kinematic analytical model as described in any one of claims 1 to 4 is used to perform forward kinematic analysis to determine the current actual position of the end of the gripper hook.

6. A kinematic analysis method for a segment assembly machine, characterized in that, Given the target position at the end of the gripper hook, the inverse kinematic analytical model as described in any one of claims 1 to 4 is used to solve for the stroke data of the translation cylinder, the V-shaped lifting mechanism, the rotary table, and the mechanical gripper in six degrees of freedom, including the following steps: First, the mechanical gripper is set to zero in each of the six degrees of freedom. Based on the target position at the end of the gripper hook, the kinematic analytical model is used to perform inverse kinematics solution to obtain the stroke data of the translation cylinder, the V-shaped lifting mechanism and the rotary table. The translation cylinder, the V-shaped lifting mechanism, and the rotary table are controlled according to the stroke data to perform the corresponding actions. After execution, the actual execution results fed back by the translation cylinder, the V-shaped lifting mechanism and the rotary table, as well as the actual position of the end of the grab hook are obtained; The strokes of the translation cylinder, the V-shaped lifting mechanism, and the rotary table are locked, and then the difference between the actual position and the target position of the end of the grab hook is calculated to obtain the difference value. Based on the difference, the kinematic analytical model is used to perform inverse kinematics again, and the difference is transformed to the intermediate coordinate system. Then, based on the difference in the intermediate coordinate system, the target stroke of the mechanical gripper in each of the six degrees of freedom is calculated.

Images