CN114589700A

CN114589700A - Parallel six-axis robot speed forward solving method based on additional encoder

Info

Publication number: CN114589700A
Application number: CN202210395720.7A
Authority: CN
Inventors: 郭鹏
Original assignee: Borunte Robot Co Ltd
Current assignee: Borunte Robot Co Ltd
Priority date: 2022-04-15
Filing date: 2022-04-15
Publication date: 2022-06-07
Anticipated expiration: 2042-04-15
Also published as: CN114589700B

Abstract

The invention relates to a parallel six-axis robot speed positive solution method based on an additional encoder and a six-axis robot. Because more information can be obtained through five encoders, the difficulty of solving a positive solution can be reduced, and in the operation process, only elementary matrix multiplication operation, namely simple multiplication and addition operation, is available, so that the calculation efficiency is effectively improved. The method is convenient to reconstruct by using programming languages such as C/C + + and the like, and the control system can be operated after reconstruction.

Description

Parallel six-axis robot speed forward solving method based on additional encoder

Technical Field

The invention relates to the technical field of industrial robots, in particular to a parallel six-axis robot speed forward solving method based on an additional encoder.

Background

The parallel robot is a robot with a brand new structure, has the advantages of higher rigidity, precision, better bearing capacity and the like compared with the traditional industrial robot, and almost relates to many fields of modern sophisticated technologies. The method has wide application in the fields of positioning platforms, simulation equipment, entertainment equipment and the like, and has great potential in automatic processing application scenes.

In the current academic and commercial field, the vast majority of six-axis robots employ either a 6-6UPS or a 6-6UPU configuration.

6-6UPS shows that 6 hooke joint joints are arranged on the static platform, and 6 ball joint joints are arranged on the movable platform. And the middle of each Hooke joint on the static platform and the middle of each Hooke joint on the movable platform are connected through an electric cylinder, and a piston rod of the electric cylinder can stretch out and draw back. The piston rods of the 6 electric cylinders can stretch and contract to drive the position and the posture of the platform to change, and the total degree of freedom is 6.

6-6UPU shows that 6 Hooke joints are arranged on the static platform, and 6 Hooke joints are arranged on the movable platform. And the middle of each hook hinge joint on the static platform and the middle of each hook hinge joint on the movable platform are connected through an electric cylinder, and a piston rod of the electric cylinder can stretch out and draw back. The position and the posture of the actuating platform can be changed by the extension and contraction of piston rods of 6 electric cylinders, and the total degree of freedom is 6.

In the motion analysis of the parallel robot, the positions of the driving parts are given, and the pose of the moving platform is solved to obtain a pose positive solution, which has profound significance for the working space analysis and the error compensation of the robot mechanism. The position and the posture information of the tail end can be used for limiting the working state of the tail end of the robot, namely the control system continuously judges the speed in the running process, and when the speed exceeds the set limit of a user, the alarm is given out and the robot is stopped. At present, no speed positive solution algorithm is disclosed for a six-axis robot with a 6-6UPU configuration.

Disclosure of Invention

Based on the above, the invention aims to provide a parallel six-axis robot speed positive solution method based on an additional encoder, which is used for monitoring linear speed and angular speed of the tail end of a parallel robot at any moment.

In a first aspect, the invention provides a parallel six-axis robot speed positive solution method based on an additional encoder, wherein the six-axis robot comprises a static platform, a movable platform, a hook joint assembly, a joint assembly and an electric cylinder assembly;

the joint assembly includes:

a bearing seat;

the crossed shaft shell is rotatably arranged on the bearing seat;

the shaft lug is rotatably arranged on the crossed shaft shell, the rotating axis of the shaft lug on the crossed shaft shell is intersected with the rotating axis of the crossed shaft shell on the bearing seat and is mutually vertical, two bearing holes are arranged on the shaft lug and are symmetrically distributed about the rotating axis of the shaft lug on the crossed shaft shell, the axes of the two bearing holes are mutually parallel and are positioned on the same horizontal plane, and the axes of the bearing holes are mutually vertical to the rotating axis of the crossed shaft shell on the bearing seat;

the Hooke joint assembly is arranged on the static platform, the joint assembly is arranged on the movable platform, one end of the electric cylinder assembly is arranged on the Hooke joint assembly, the other end of the electric cylinder assembly is rotatably connected to the shaft lug of the joint assembly through a bearing pin shaft, and the bearing pin shaft is rotatably arranged in the bearing hole of the shaft lug;

the static platform is provided with six hooke joint assemblies which are respectively a first hooke joint assembly, a second hooke joint assembly, a third hooke joint assembly, a fourth hooke joint assembly, a fifth hooke joint assembly and a sixth hooke joint assembly, the movable platform is provided with three joint assemblies which are respectively a first joint assembly, a second joint assembly and a third joint assembly, six electric cylinder assemblies are arranged between the static platform and the movable platform, and the six electric cylinder assemblies are respectively a first electric cylinder assembly, a second electric cylinder assembly, a third electric cylinder assembly, a fourth electric cylinder assembly, a fifth electric cylinder assembly and a sixth electric cylinder assembly;

the second electric cylinder assembly, the third electric cylinder assembly, the second hook joint assembly, the third hook joint assembly and the first joint assembly form a first control assembly, the fourth electric cylinder assembly, the fifth electric cylinder assembly, the fourth hook joint assembly, the fifth hook joint assembly and the second joint assembly form a second control assembly, and the sixth electric cylinder assembly, the first electric cylinder assembly, the sixth hook joint assembly, the first hook joint assembly and the third joint assembly form a third control assembly;

the speed positive solution method comprises the following steps:

s10: installing five angle encoders on a first control assembly of the robot to read joint angle values and joint angle velocity values, comprising the sub-steps of:

s101: the first control assembly is used for measuring the rotation angle and the rotation angular velocity of a fixed shaft part and a swing shaft part of the first Hooke joint assembly relative to the Hooke joint lower base and the Hooke joint upper base respectively, and the rotation angle and the rotation angular velocity are recorded as j theta₁、jθ₂And

s102: an angle encoder is arranged on a Hooke joint upper base of a third Hooke joint component of the first control component, is recorded as a third angle encoder, is used for measuring the rotating angle and the rotating angular speed of the swinging shaft relative to the Hooke joint upper base of the Hooke joint component, and is recorded as j theta₃And

s103: an angle encoder is arranged in the joint component of the first control component, is recorded as a fourth angle encoder, is used for measuring the angle value and the angular velocity value of the shaft part of the shaft lug of the joint component relative to the crossed shaft shell, and is recorded as j theta₄And

s104: an angle encoder is arranged at the position, connected with the first control assembly, of the movable platform, is recorded as a fifth angle encoder, is used for measuring the rotation angle value and the rotation angular velocity value of the movable platform relative to the joint assembly, and is recorded as j theta₅And

s20: constructing kinematic elements of the parallel six-axis robot;

B_iand i is 1-6: origin of Hooke's hinge assembly, connecting B_iThe point i is 1-6 and is arranged in the center of a cross shaft of the hook hinge assembly;

{ O }: establishing a rectangular coordinate system { O } on the static platform based on the coordinate system, and setting the origin O of the coordinate system at B₁～B₆On a certain plane and located at B₁～B₆The center position of the circle is determined, and the y-axis direction is arranged at OB₁,OB₂The angular bisector position of the line segment is determined, at the moment, the six hook hinge assemblies are symmetrical relative to the y axis, the z axis is arranged upwards, and the x axis can be automatically determined according to the right-hand rule;

R_b: is represented by B₁～B₆The radius of the determined circle is called as the radius of the virtual circle of the Hooke's hinge;

^OOB₁～^OOB₆：^OOB_i: the Hooke's joint position vector specifically represents the ith Hooke's joint origin B with the origin of the base coordinate system as the starting point_iI is a vector with 1-6 as an end point, and the reference coordinate system is a base coordinate system;

β_iand i is 1-6: offset angle of hook joint, expression^OOB_iAn angle to the Y-axis of the base coordinate system { O };

R_Q: called the virtual circle radius of the crossed axes, and represents the radius value of the circle determined by the origin points of the three crossed axes;

Q_iand i is 1-3: the origin of the axle lug, the part of the axle lug and the U_iAnd i is 1-3 corresponding to the overlapped point and is Q_i，i＝1～3；

{Q_i1-3: representing the axis ear coordinate system. The origin of the coordinate system is fixedly connected with the origin Q of the shaft lug_i，{Q_iThe z axis of the hinge is along the axis direction of the shaft part of the shaft lug, the y axis is along the connecting line direction of two points of the hinge of the pin shaft, and the x axis is determined according to the right-hand rule;

U_i: the origin of the crossed shaft and the axes of the two cylindrical surfaces of the joint component form an intersection point which is set as U_i，i＝1～3；

{U_i1-3:respectively represent a cross-axis coordinate system, and the origin of the coordinate system is fixedly connected with the origin U of the cross-axis part_i，{U_iThe z-axis of the pivot lug is oriented along the axis of the shaft portion of the pivot lug, the y-axis is oriented along the axis of the central shaft, and the x-axis is determined according to the right-hand rule.

{ P }: moving platform coordinate System, { P } with origin P at U_iI is 1-3 determined plane and is located on three U_iAnd determining the center position of the circle. Set the y-axis of { P } to: wherein U is₂In the negative direction of the y-axis, U₁,U₃Symmetrical with respect to the y-axis, the z-axis up, and the x-axis can be determined with the right-hand rule. As shown in fig. 13.

Distribution angle of axle ear origin

^OOP: the position of the movable platform represents a position vector of an origin P of a movable platform coordinate system { P } relative to an origin O of a static platform coordinate system;

^OR_P: the attitude of the movable platform represents a rotation matrix of a coordinate system { P } of the movable platform relative to a coordinate system { O } of the static platform;

A₁～A₆: the hinge center of the pin shaft is arranged in the shaft lug part 1 (the shaft lug part 2 and the shaft lug part 3), and the hinge center points A of the two pin shafts₂(Point A)₄Point A₆) And point A₃(Point A)₅Point A₁) Point relative to { Q₁}({Q₂}，{Q₃) } about the Y axis of the substrate, point A₂(Point A)₄Point A₆) And point A₃(Point A)₅Point A₁) Is placed in { Q₁}({Q₂}，{Q₃}) in the XY plane, a is represented by QAX₂And A₃In { Q₁Absolute offset value in the X-axis direction of (A) }, QAY denotes A₂And A₃In { Q₁Absolute offset value in Y-axis direction;

^ΟΒΑ_i: representing the position vector of the electric cylinder, and taking a base coordinate system { O } as a reference system;

Q₁': representing the instant center of the speed of the shaft lug part;

s30: the length of the electric cylinder is read from a control system of the electric cylinder assembly, is recorded as l1 to l6 and is represented by l1 to l6^ΟΒΑ₁～^ΟΒΑ₆Let l1 be non-zero^ΟΒΑ₁|，…，l6＝|^ΟΒΑ₆Reading out the extension and contraction speed v of the piston rod relative to the cylinder body in the electric cylinder assembly from the control system of the electric cylinder assembly_Li,i＝1～6；

S40: on the cross axle of the second hook joint component, the center B of the cross axle is used₂Establishing a Hooke joint coordinate system (B2) for the origin, and using the center B of the cross shaft on the cross shaft of the third Hooke joint assembly₃Establishing a Hooke hinge coordinate system { B3} for the origin;

s50: establishing an electric cylinder coordinate system { L2} on a cross shaft of the second Hooke joint assembly, wherein the origin of the electric cylinder coordinate system is coincident with the origin of { B2}, and establishing an electric cylinder coordinate system { L3} on a Hooke joint upper base of the third Hooke joint assembly, wherein the origin of the electric cylinder coordinate system is coincident with the origin of { B3 };

s60: calculating the center B of the second Hooke's hinge component₂Center B of the third Hooke's joint assembly₃Distance b of₂₃；

S70: solving the poses of the { B2} and { B3} coordinate systems by taking the { O } as a reference system;

s80: solving the poses of the { L2} and { L3} coordinate systems by taking the { O } as a reference system;

s90: solving for A by using { O } as a reference system₂、A₃The location of the point;

s100: solving for { Q₁Pose with respect to { O }, using^OT_Q1Representing;

s110: solving for { U₁Pose with respect to { O }, using^OT_U1Represents;

s120: solving for { P₁Pose with respect to { O }, using^OT_P1Represents;

s130: solving the pose of the moving platform, i.e. of the moving platformPosition of^OPose of OP and moving platform^OR_P；

S140: taking the { O } as a reference coordinate system, solving the angular velocity vector of the cross shafts of the second Hooke's joint assembly and the third Hooke's joint assembly relative to the static platform, and recording the angular velocity vector as^OW_B2,OAnd^OW_B3,O；

s150: taking { O } as a reference coordinate system, solving the angular velocity vector of the cylinder body of the second electric cylinder assembly and the third electric cylinder assembly relative to the static platform, and recording the angular velocity vector as^OW_L2,OAnd^OW_L3,O；

s160: solving pin hinge center A₂，A₃Linear velocity vector of (2), using { O } as reference coordinate system^OV_A2And^OV_A3represents;

s170: solving for the axle lug origin Q on the axle lug part₁Linear velocity of spot^OV_Q1And angular velocity vector of the shaft lug^OW_Q1,O；

S180: solving for the angular velocity of the intersecting axes of the first joint component, using { O } as the reference system, and recording as^OW_U1,O；

S190: solving the angular velocity of the moving platform with { O } as the reference frame, i.e.^OW_P,O；

S200: solving the linear velocity of the center of the moving platform with { O } as a reference system, i.e.^OV_P。

In a second aspect, the present invention provides a six-axis robot comprising:

at least one memory and at least one processor;

the memory for storing one or more programs;

when executed by the at least one processor, cause the at least one processor to implement the steps of the six-axis robot velocity solution method as previously described.

Drawings

FIG. 1 is a schematic structural diagram of a six-axis robot according to an embodiment;

FIG. 2 is an exploded view of a six-axis robot according to an embodiment;

FIG. 3 is a schematic diagram illustrating the connection between a hook joint assembly and a stationary platform according to an embodiment;

FIG. 4 is a schematic connection diagram of the joint assembly, the electric cylinder assembly and the movable platform according to the embodiment;

FIG. 5 is a schematic view of a hooke joint assembly of an embodiment distributed on a stationary platform;

FIG. 6 is a schematic view of the definition of the origin of a Hooke's hinge;

FIG. 7 is a schematic view of an angle encoder measurement according to an embodiment;

FIG. 8 is a schematic view of an embodiment of an angular encoder installation;

FIG. 9 is a schematic view of a position vector of the origin of the moving platform coordinate system relative to the origin of the stationary platform coordinate system;

FIG. 10 is a schematic diagram of constructing a base coordinate system;

FIG. 11 is a schematic view of a hook joint position vector;

FIG. 12 is a schematic illustration of constructing a cross-axis origin;

FIG. 13 is a schematic view of a U coordinate system;

FIG. 14 is a schematic diagram of the construction of a moving platform coordinate system;

FIG. 15 is a schematic diagram of elements of the movable platform;

FIG. 16 is a schematic diagram of the origin of the cross axis;

FIG. 17 is a schematic view of a Q coordinate system;

FIG. 18 is a schematic view showing distribution angles of pivot ear origin points;

FIG. 19 is a schematic diagram of elements of a Q-coordinate system;

FIG. 20 is a schematic view of the instant center of velocity of the journal stirrup;

FIG. 21 is a schematic diagram illustrating an establishment of a coordinate system of a second hook joint assembly and a third hook joint assembly in the embodiment;

FIGS. 22-23 are schematic diagrams of coordinate system establishment at the first joint assembly in an embodiment;

FIG. 24 is a flow chart of an embodiment of the present invention;

in the figure: 10. a static platform; 20. moving the platform; 31. a first hook hinge assembly; 32. a second hook hinge assembly; 33. a third hook hinge assembly; 34. a fourth hook hinge assembly; 35. a fifth hook hinge assembly; 36. a sixth hook hinge assembly; 41. a first electric cylinder assembly; 42. a second electric cylinder assembly; 43. a third electric cylinder assembly; 44. a fourth electric cylinder assembly; 45. a fifth electric cylinder assembly; 46. a sixth electric cylinder assembly; 50. a joint assembly; 51. a first joint assembly; 52. a second joint assembly; 53. a third joint component; 61. a first angle encoder; 62. a second angle encoder; 63. a third angle encoder; 64. a fourth angular encoder; 65. and a fifth angle encoder.

Detailed Description

The invention provides a parallel six-axis robot speed forward solving method based on an additional encoder, and aims to solve the technical problem that a speed forward solving algorithm is not disclosed in the background art by referring to a patent document with application number 202111408697.2, and as shown in fig. 1 and 2, the parallel six-axis robot disclosed by the invention comprises a static platform 10, a movable platform 20, six hooke joint assemblies, six electric cylinder assemblies and three joint assemblies 50, wherein the hooke joint assemblies are mounted on the static platform 10, the joint assemblies 50 are mounted on the movable platform 20, one ends of the electric cylinder assemblies are mounted on the hooke joint assemblies, and the other ends of the electric cylinder assemblies are rotatably connected to the joint assemblies 50 through bearing pin shafts.

The six electric cylinder assemblies are sequentially defined as a first electric cylinder assembly 41, a second electric cylinder assembly 42, a third electric cylinder assembly 43, a fourth electric cylinder assembly 44, a fifth electric cylinder assembly 45 and a sixth electric cylinder assembly 46 in the anticlockwise direction; the six Hooke joint components are sequentially defined as a first Hooke joint component 31, a second Hooke joint component 32, a third Hooke joint component 33, a fourth Hooke joint component 34, a fifth Hooke joint component 35 and a sixth Hooke joint component 36; the three joint assemblies 50 are defined in turn as a first joint assembly 51, a second joint assembly 52 and a third joint assembly 53.

The second electric cylinder assembly 42, the third electric cylinder assembly 43, the second hooke joint assembly 32, the third hooke joint assembly 33 and the first joint assembly 51 form a first control assembly, one end of the second electric cylinder assembly 42 and one end of the third electric cylinder assembly 43 are respectively installed on the second hooke joint assembly 32 and the third hooke joint assembly 33, and the other end of the second electric cylinder assembly 42 and the other end of the third electric cylinder assembly 43 are connected to the first joint assembly 51. The fourth electric cylinder assembly 44, the fifth electric cylinder assembly 45, the fourth hooke joint assembly 34, the fifth hooke joint assembly 35 and the second joint assembly 52 form a second control assembly, one end of the fourth electric cylinder assembly 44 and one end of the fifth electric cylinder assembly 45 are respectively installed on the fourth hooke joint assembly 34 and the fifth hooke joint assembly 35, and the other end of the fourth electric cylinder assembly 44 and the other end of the fifth electric cylinder assembly 45 are connected to the second joint assembly 52. The sixth electric cylinder assembly 46, the first electric cylinder assembly 41, the sixth hook joint assembly 36, the first hook joint assembly 31 and the third joint assembly 53 form a third control assembly, one end of the sixth electric cylinder assembly 46 and one end of the first electric cylinder assembly 41 are respectively connected with the sixth hook joint assembly 36 and the first hook joint assembly 31, and the other end of the sixth electric cylinder assembly 46 and the other end of the first electric cylinder assembly 41 are connected with the third joint assembly 53.

As shown in fig. 3 and 4, each hooke joint assembly fixedly connected to the stationary platform 10 has two rotational joints, so that the cylinder body of the electric cylinder assembly has two rotational degrees of freedom with respect to the stationary platform 10, which are twelve rotational degrees of freedom, and the rotational axis is denoted as J1-J12; each electric cylinder is provided with a translation joint, so that one translation degree of freedom of a piston rod of the electric cylinder relative to the cylinder body is realized, six translation degrees of freedom are realized, and a moving shaft is marked as J13-J18; the bottom of each shaft lug part is provided with 2 single-shaft rotary joints so as to realize that each shaft lug has 1 rotary freedom degree and 6 rotary freedom degrees respectively relative to the connected 2 piston rods, and the rotary shafts are J19-J24 in figure 4; each joint component has 2 rotary joints, can realize the single-axis rotation of the axle lug relative to the crossed shaft shell, and has 3 degrees of freedom in total, and the rotary shaft is J25-J27. The cross shaft shell can also rotate relative to the moving platform 20 in a single shaft, and the total degree of freedom is 3, and the rotating shaft is J28-J30. J25-J27 are correspondingly perpendicular to J28-J30 one by one. The rotational degrees of freedom can be realized by designing a shafting mechanism, and the translational degrees of freedom are realized by using an electric cylinder.

As shown in fig. 5 and 6, the lower hooke joint base of each hooke joint assembly is connected to the stationary platform 10 by screws, and the installation angle of the hooke joint assembly is specific and can be considered as the setting of the hooke joint rotation centers (B1-B6): (a) the intersection points (B1-B6) of the two rotating shafts of the cross shaft of the Hooke joint assembly, namely the rotating centers of the Hooke joints, B1-B6 are arranged on a circle with the point O as the center. (b) The bisector OY1 of OB1 and OB2 is drawn, and OY2 and OY3 are both 120 ° from OY 1. OB3 and OB4 are axisymmetric with respect to ray OY2 and B5 and B6 are axisymmetric with respect to ray OY 3. (c) The axes of the fixed shaft portions of the cross shafts of the second and third hook-and-

loop assemblies

32 and 33 should be at 30 degrees to the OY1 line, the axes of the fixed shaft portions of the cross shafts of the fourth and fifth hook-and-

loop assemblies

34 and 35 should be at 90 degrees to the OY1 line, and the axes of the fixed shaft portions of the cross shafts of the first and sixth hook-and-

loop assemblies

31 and 36 should be at 30 degrees to the OY1 line.

And the Hooke joint upper seat of each Hooke joint assembly is connected with the electric cylinder through a screw. The 'pin hole axis' of the pin hole on the fork frame at the top end of the piston rod is parallel to the axis of the swing shaft part of the hook hinge assembly.

Each joint assembly is connected to the movable platform 20 by screws. The mounting position and mounting angle of the joint assembly on the mobile platform 20 are specific: (a) the intersection points of the axis of the central shaft and the 'shaft lug cylindrical axis' of the shaft lug are set to be Q1-Q3 and respectively correspond to the first joint component to the third joint component. (b) A point P is arranged on the movable platform 20, and Q1-Q3 are uniformly distributed on a circle taking the point P as the center of a circle. (c) The central shaft axis is perpendicular to OQ 1-OQ 3.

Based on the robot structure, the invention discloses a parallel six-axis robot speed positive solution method based on an additional encoder.

Specifically, as shown in fig. 7 and 8, an angle encoder is respectively installed on the upper hooke joint base and the lower hooke joint base of the left hooke joint assembly of the selected control assembly, and is recorded as a first angle encoder 61 and a second angle encoder 62, which are used for measuring the rotation angles of the fixed shaft part and the swinging shaft part of the hooke joint assembly relative to the lower hooke joint base and the upper hooke joint base, and are respectively recorded as j θ₁And j θ₂And measuring the rotation angular velocities of the fixed shaft part and the swing shaft part relative to the lower base of the Hooke joint and the upper base of the Hooke joint respectively, and recording the rotation angular velocitiesIs composed of

And

an angle encoder is arranged on the Hooke joint upper base of the Hooke joint assembly on the right side of the selected control assembly, is recorded as a third angle encoder 63, is used for measuring the rotating angle of the swinging shaft part of the Hooke joint relative to the Hooke joint upper base of the Hooke joint assembly, and is recorded as j theta₃And measuring the rotation angular velocity of the swing shaft part relative to the base on the Hooke's joint, and recording the rotation angular velocity

An angle encoder, denoted as a fourth angle encoder 64, is mounted within the joint assembly of the selected control assembly for measuring an angle value, denoted as j θ, of the shaft portion of the shaft lug of the joint assembly relative to the cross-shaft housing₄And measuring the angular velocity values of the shaft portions of the lugs relative to the cross-shaft housing part, noted

An angle encoder, designated as a fifth angle encoder 65, is mounted on the movable platform 20 at a location where it is connected to the selected control assembly for measuring the rotational angle of the movable platform 20 relative to the joint assembly, designated as j θ₅And measuring the rotation angle of the movable platform 20 relative to the joint assembly, and recording the rotation angle value

As shown in FIG. 9, after the angular encoder is mounted, known are l1 to l6^ΟΒΑ₁～^ΟΒΑ₆Mold (L1) ═ Y^ΟΒΑ₁|，…，l6＝|^ΟΒΑ₆The geometric meaning represents the length of the electric cylinder at any time, and l 1-l 6 can be read by the control system of the electric cylinder and are known values. J theta can be read by five angle encoders₁～jθ₅And

v_L2: representing the extension and retraction speed of the piston rod in the second cylinder assembly 42 relative to the cylinder, which is a known value that can be read from the control system of the electric cylinder, v_L3: which represents the extension and retraction speed of the piston rod in the third cylinder assembly 43 relative to the cylinder, is a known value that can be read from the control system of the electric cylinder.

Then, the speed positive solution problem for a parallel six-axis robot can be described as: from the above known values, the linear velocity of the movable platform 20 is solved^OV_PAnd angular velocity^OW_P,O. In the embodiment, five angle encoders are mounted on the first control assembly, and the angle encoders are mounted on the second hook joint assembly 32, the third hook joint assembly 33 and the first joint assembly 51 respectively. Therefore, the velocity solution of the moving platform 20 specifically comprises the following steps:

s101: an angle encoder is respectively arranged on the upper Hooke joint base and the lower Hooke joint base of the second Hooke joint assembly 32 of the first control assembly, is recorded as a first angle encoder and a second angle encoder, is used for measuring the rotating angle and the rotating angular speed of the fixed shaft part and the swinging shaft part of the second Hooke joint assembly 32 relative to the lower Hooke joint base and the upper Hooke joint base respectively, and is recorded as j theta₁、jθ₂And

s102: an angle encoder is arranged on a Hooke joint upper base of a third Hooke joint assembly 33 of the first control assembly, is recorded as a third angle encoder, is used for measuring the rotating angle and the rotating angular speed of the swinging shaft relative to the Hooke joint upper base of the Hooke joint assembly, and is recorded as j theta₃And

s103: an angle encoder, denoted as a fourth angle encoder, is mounted inside the first joint assembly 51 of the first control assembly for measuring an angular value and an angular velocity value, denoted as j θ, of the shaft portion of the shaft lug of the joint assembly relative to the cross-shaft housing₄And

s104: an angle encoder, denoted as a fifth angle encoder, is installed at a position on the movable platform 20 connected to the first control component, and is used for measuring a rotation angle value and a rotation angular velocity value, denoted as j θ, of the movable platform 20 relative to the first joint component 51₅And

s20: constructing kinematic elements of the parallel six-axis robot;

B_iand i is 1-6: and a Hooke's hinge origin. B is to be_iAnd the point i is 1-6 and is arranged in the center of the cross shaft of the hook joint. As shown in fig. 9.

{ O }: a base coordinate system. And establishing a rectangular coordinate system (O) on the static platform. Setting the origin O of the coordinate system at B₁～B₆On a certain plane and located at B₁～B₆The center position of the circle is determined, and the y-axis direction is set as shown in the figure, at OB₁,OB₂The angle of the line segment bisects the line position, and the six hook joints are symmetrical relative to the y axis. The z-axis is set up upward, and the x-axis can be automatically determined according to the right-hand rule, and the final coordinate system is shown in fig. 10.

R_b: is represented by B₁～B₆The radius of the circle determined, as shown in fig. 10. Referred to as the "hook joint virtual circle radius".

^OOB₁～^OOB₆：^OOB_i: the Hooke's joint position vector specifically represents the ith Hooke's joint origin B with the origin of the base coordinate system as the starting point_iAnd i is a vector with 1-6 as an end point, and the reference coordinate system is a base coordinate system. As shown in fig. 11.

β_iAnd i is 1-6: hook hinge offset angle, representation^OOB_iThe angle to the Y-axis of the base coordinate system { O }, as shown in FIG. 11.

U_i: the origin of the cross axis. The axes of two cylindrical surfaces of the crossed shaft part form a crossing point which is set as U_iI is 1 to 3, as shown in FIG. 12.

{U_i1-3: the cross axis coordinate system is represented, and the establishment method is shown in FIG. 21, the origin of the coordinate system is fixedly connected with the origin U of the cross axis part_i，{U_iThe xyz axes of the pivot ears are shown in fig. 13, with the z axis oriented along the axis of the shaft portion of the pivot ears, the y axis oriented along the axis of the central shaft, and the x axis determined according to the right hand rule.

R_Q: referred to as the "cross-axis virtual circle radius", as shown in fig. 14, represents the radius value of the circle defined by the three cross-axis origins.

{ P }: and a moving platform coordinate system. As shown in fig. 14, a rectangular coordinate system { P } is established on the moving platform. With origin P at U_iI is 1-3 determined plane and is located on three U_iAnd determining the center position of the circle. The y-axis for setting { P } is shown, where U₂In the negative direction of the y-axis, U₁,U₃Symmetrical with respect to the y-axis, the z-axis up, and the x-axis can be determined with the right-hand rule.

Q_iAnd i is 1-3: the pivot ear origin. On the shaft lug part and U_iAnd i is 1-3 corresponding to the overlapped point and is Q_iAnd i is 1 to 3, as shown in fig. 15 and 16.

{Q_i1-3: representing the axis ear coordinate system. The origin of the coordinate system is fixedly connected with the origin Q of the shaft lug_i，{Q_iThe three xyz axes of the hinge are shown in fig. 17, the z axis is along the axis direction of the shaft part of the shaft lug, the y axis is along the connecting line direction of two points of the hinge of the pin shaft, and the x axis is determined according to the right-hand rule.

Distribution angle of axle ear origin

As shown in fig. 18.

^OOP: the position of the movable platform. A position vector representing the origin P of the moving platform coordinate system { P } relative to the origin O of the stationary platform coordinate system, as shown in FIG. 9;

^OR_P: attitude of the moving platform. A rotation matrix representing the moving platform coordinate system { P } relative to the stationary platform coordinate system { O }, as shown in FIG. 9;

A₁～A₆: the hinge center of the pin shaft. In the axle lug part 1 (axle lug part 2, axle lug part 3), the hinge central point A of two pin shafts₂(Point A)₄Point A₆) And point A₃(Point A)₅Point A₁) Point relative to { Q₁}({Q₂}，{Q₃) } about the Y axis of the substrate, point A₂(Point A)₄Point A of₆) And point A₃(Point A)₅Point A₁) Is placed in { Q₁}({Q₂}，{Q₃}) in the XY plane, a is represented by QAX₂And A₃In { Q₁Absolute offset value in the X-axis direction of (A) }, QAY denotes A₂And A₃In { Q₁Absolute displacement value in the Y-axis direction, as shown in fig. 19.

^ΟΒΑ_i: represents the cylinder position vector with reference to the base coordinate system { O }, as shown in FIG. 9.

Q₁': representing the instant center of the speed of the shaft lug part; as shown in fig. 20.

S30: the length of the electric cylinder is read from a control system of the electric cylinder assembly, is recorded as l 1-l 6 and is represented by l 1-l 6^ΟΒΑ₁～^ΟΒΑ₆Let l1 be non-zero^ΟΒΑ₁|，…，l6＝|^ΟΒΑ₆Reading out the expansion and contraction speed v of the piston rod in the electric cylinder assembly relative to the cylinder body from the control system of the electric cylinder assembly_Li,i＝1～6。

S40: on the cross of the second hook joint assembly 32, with the center of the cross B₂Establishing a Hooke joint coordinate system (B2) for an origin, wherein the origin is fixed at the center of the cross shaft, namely the axis of the fixed shaft part and the swing shaftAnd the intersection point of the axis parts, the y axis, the z axis and the z axis of the { O } are the same along the axis of the fixed axis part and point to the right Hooke hinge assembly, and the x axis can be obtained by right-hand rule. As shown in fig. 21, { B2} is indicated by a solid line. On the cross of the third hook joint assembly 33, with the center B of the cross₃And establishing a Hooke joint coordinate system (B3) for an origin, wherein the origin is fixed at the center of the cross shaft, namely the intersection point of the axis of the fixed shaft part and the axis of the swinging shaft part. The y-axis of { B3} is in the same direction as the y-axis of { B2}, the z-axis is in the same direction as the z-axis of { O }, and the x-axis is obtained by right-hand rule.

S50: on the cross shaft of the second Hooke's hinge assembly 32, an electric cylinder coordinate system { L2}, the origin of which coincides with the origin of { B2}, the z-axis is perpendicular to the plane defined by the fixed shaft axis and the swing shaft axis and faces upwards, the x-axis is along the swing shaft axis and faces the same direction as the x-axis of { B2}, and the y-axis is obtained by a right-hand rule. As shown in fig. 21, { L2} is indicated by a broken line. On the Hooke joint upper base of the third Hooke joint assembly 33, an electric cylinder coordinate system { L3} is established, the origin of the electric cylinder coordinate system is coincident with the origin of { B3}, the z-axis is perpendicular to a plane defined by the fixed shaft axis and the swinging shaft axis and faces upwards, the x-axis is along the swinging shaft axis and faces the same direction as the x-axis of { B3}, and the y-axis can be obtained by right-hand determination. As shown in fig. 21, { L3} is indicated by a broken line.

S60: calculating the center B of the second hook hinge assembly 32₂And the center B of the third hook joint assembly 33₃Distance b of₂₃From fig. 11, it can be calculated:

s70: and (4) solving the poses of the { B2} and { B3} coordinate systems by taking the { O } as a reference system. By using^OP_B3Represents the origin position of B3,^OR_B3representing the pose of B3,^OT_B3the calculation formula for the pose of the { B2} coordinate system, representing the pose matrix, is:

^OP_B3＝R_b·[-cos(30°) -sin(30°) 0]^T

^OR_B3＝^OR_B2

using { O } as a reference system^OP_B3Represents the origin position of B3,^OR_B3representing the pose of B3,^OT_B3a pose matrix is represented. The calculation formula of the pose of the { B3} coordinate system is as follows:

^OP_B3＝R_b·[-cos(30°) -sin(30°) 0]^T

^OR_B3＝^OR_B2

s80: and (4) solving the poses of the { L2} and { L3} coordinate systems by taking the { O } as a reference system. By using^B2R_L2Represents the posture of { L2} relative to { B2},^OT_L2representing a pose matrix, and solving a calculation formula as follows:

wherein^OT_L2Is the attitude of the matrix formed by the 1 st to 3 rd rows and the 1 st to 3 rd columns^OR_L2。

By using^B3R_L3Represents the posture of { L3} relative to { B3},^OT_L3representing a pose matrix, and solving a calculation formula as follows:

wherein^OT_L3Is the attitude of the matrix formed by the 1 st to 3 rd rows and the 1 st to 3 rd columns^OR_L3。

S90: solving for A by using { O } as a reference system₂、A₃The position of the point. By using^L2P_A2Denotes the reference system, { L2}, A₂Position of (A)₂Position of the point^OP_A2The solving formula of (2) is:

^L2P_A2＝[0 0 l2]^T

^OP_A2＝^OR_L2·^L2P_A2。

by using^L2P_A3Denotes the reference system, { L2}, A₂Position of (A) is solved₃Position of the point^OP_A3The solving formula of (2) is as follows:

^OP_A3＝^OR_L2·^L2P_A3。

s100: solving for { Q₁Pose with respect to { O }, using^OT_Q1And (4) showing.

Using { L2} as a reference system^L2N₂₃Vector representing point A2 pointing to point A3:^L2N₂₃＝^L2P_A3-^L2P_A2according to the normal vector solving rule of the two-dimensional vector, the sum can be obtained^L2N₂₃The unit vector of the vertical is^L2N_⊥23. By alpha₂₃To represent^L2N₂₃And { L2} the z-axis, vector z_L2＝[001]^TIncluded angle value, use^L2R_Q1Represents { Q₁Pose with respect to { L2}, using^L2A_o23Is represented by A₂Point sum A₃The position vector of the center point of the point,^L2P_Q1represents { Q₁Position vector of origin in { L2} coordinate system, the above geometric elements are shown in FIG. 22, { Q₁Denoted by a solid line, { U }₁Indicated with a dashed line. { Q₁Pose with respect to { O }^OT_Q1The calculation formula of (2) is as follows:

^L2P_Q1＝^L2A_o23+^L2N_⊥23·QAY

wherein the content of the first and second substances,^OT_Q1row 1 to 3, column 4 is a position vector^OP_Q1。

S110: solving for { U₁Pose with respect to { O }, using^OT_U1And (4) showing. With { Q₁Using as a reference frame^Q1R_U1Represents { U₁Relative to { Q }₁The gesture of the electronic component is changed,^Q1P_U1represents { U₁The origin of the equation is at { Q }₁Position vector in the coordinate system, as shown in FIG. 22, { U₁Pose with respect to { O }^OT_U1The calculation formula of (2) is as follows:

^Q1P_U1＝[0 0 0]

s120: solving for { P₁Pose with respect to { O }, using^OT_P1And (4) showing. By { U₁Using as a reference frame^U1R_P1Represents { P₁Relative to { U }₁The gesture of the electronic component is changed,^U1P_P1represents { P₁The origin of { U } is at₁Position vector in the coordinate system, { P, as shown in FIG. 23₁Denoted by a dotted line, { U }₁Denoted by a solid line, { P }₁Pose with respect to { O }^OT_P1The calculation formula of (2) is as follows:

^U1P_P1＝[0 0 0]

s130: solving the pose of the movable platform to { P₁Using as a reference frame^P1R_PIndicating that { P } is relative to { P }₁The gesture of the electronic component is changed,^P1P_Pthe origin of the expression { P } is at { P₁A position vector in the coordinate system is multiplied,^OT_Pshowing the pose of { P } relative to { O } and the position of the moving platform^OPose of OP and moving platform^OR_PThe calculation formula of (2) is as follows:

^P1P_P＝[-R _Q 0 0]

wherein the content of the first and second substances,^OT_Pthe matrix formed by the 1 st to 3 rd rows and the 1 st to 3 rd columns is^OR_P，^OT_PThe vector formed by the 1 st to 3 rd lines and the 4 th column of (A) is^OOP。

S140: solving the second Hooke's joint component and the third Hooke's joint by taking { O } as a reference coordinate systemThe angular velocity vector of the cross-member of the hinge assembly relative to the stationary platform is denoted as^OW_B2,OAnd^OW_B3,O。

by using^B2W_B2,ORepresenting the angular velocity vector of the cross-shaft of the second hooke's joint assembly relative to the stationary platform with { B2} as a reference coordinate system, due to the nature of the robot configuration, the cross-shaft rotates only about the y-axis of { B2}, so that:

has already been obtained^OR_B2Therefore, the angular velocity vector of the cross shaft of the second hooke joint assembly relative to the stationary platform is:

^OW_B2,O＝^OR_B2·^B2W_B2,O

by using^B3W_B3,ORepresenting the angular velocity vector of the cross-axis of the third hook-and-loop assembly relative to the stationary platform, using { B3} as a reference coordinate system, due to the nature of the robot configuration, the cross-axis rotates only about the y-axis of { B3} and since the y-axis of { B2} and the y-axis of { B3} are collinear, it is possible to obtain:

has already been found out^OR_B3Therefore, the angular velocity vector of the cross shaft of the third hooke's joint assembly relative to the stationary platform is:

^OW_B3＝^OR_B3·^B3W_B3,O

s150: taking { O } as a reference coordinate system, solving the angular velocity vector of the cylinder body of the second electric cylinder assembly and the third electric cylinder assembly relative to the static platform, and recording the angular velocity vector as^OW_L2,OAnd^OW_L3,O。

by using^L2W_L2,B2Showing the cylinder of the second cylinder assembly relative to the cross-shaft with { L2} as a reference frameThe angular velocity vector, since the cylinder body of the second electric cylinder assembly rotates around the x-axis of the { L2} coordinate system, can be obtained:

has already been found out^OR_L2By using^OW_L2,B2The angular velocity vector of the cylinder body of the second electric cylinder assembly relative to the cross shaft is expressed by using { O } as a reference coordinate, so that:^OW_L2,B2＝^OR_L2·^L2W_L2,B2according to the velocity superposition principle of rigid body kinematics, the { O } is taken as a reference coordinate, and the angular velocity vector of the cylinder body in the second electric cylinder assembly relative to the static platform is as follows:

^OW_L2,O＝^OW_B2,O+^OW_L2,B2

by using^L3W_L3,B3The angular velocity vector of the cylinder of the third cylinder unit relative to the cross axis is expressed by using { L3} as a reference coordinate system, and the cylinder of the third cylinder unit rotates around the x-axis of { L3} coordinate system, so that:

has already been found out^OR_L3By using^OW_L3,B3The angular velocity vector of the cylinder body of the third electric cylinder assembly relative to the cross shaft is expressed by using { O } as a reference coordinate, so that the following can be obtained:^OW_L3,B3＝^OR_L3·^L3W_L3,B3according to the velocity superposition principle of rigid body kinematics:

^OW_L3,O＝^OW_B3,O+^OW_L3,B3

s160: solving pin hinge center A₂，A₃Linear velocity vector of (2), using { O } as reference coordinate system^OV_A2And^OV_A3and (4) showing.

Solving for a position vector of a second electric cylinder assembly^ΟΒΑ₂Due to the foregoing has already been solved^OP_A2，^OP_B2Therefore, the following can be obtained:

^OBA₂＝^OP_A2-^OP_B2

the linear velocity vector at the point A2 is calculated by the formula:

^OV_A2＝^OW_L2,O×^OBA₂+^OR_L2·[0 0 v_L2]^T

solving for a position vector of a third electric cylinder assembly^ΟΒΑ₃Due to the foregoing has already been solved^OP_A3，^OP_B3Therefore, the following can be obtained:

^OBA₃＝^OP_A3-^OP_B3

the linear velocity vector at the point A3 is calculated by the formula:

^OV_A3＝^OW_L3,O×^OBA₃+^OR_L3·[0 0 v_L3]^T

s170: solving for the axle lug origin Q on the axle lug part₁Linear velocity of spot^OV_Q1And angular velocity vector of the shaft lug^OW_Q1,OThe method comprises the following steps:

s171: solving the angle theta of the instantaneous center of rotation Q1₃Due to the angle theta of the instant center of rotation Q1₃＝180-θ₁-θ₂Wherein theta₁Is represented by A₂A₃And A₂Q₁Angle between, theta₂Is represented by A₃A₂And A₃Q₁The angle between the two is based on { O } as reference system^ON₂₃Vector representing point A2 pointing to point A3:^ON₂₃＝^OP_A3-^OP_A2，^OV_A2(1) the 1 st value representing the three-dimensional vector,^OV_A2(2) to representThe 2 nd value of the three-dimensional vector, as shown in fig. 20, is solved by the following formula:

θ₃＝180-θ₁-θ₂

s172: solving the distance value | A of the rotation instant center Q1' from A2₂Q₁' |, since QAX represents A₂And A₃In { Q₁Absolute offset value in X-axis direction, | A₂A₃I is 2 · QAX, so:

s173: using { O } as reference coordinate system, solving the vector from A2 point to instantaneous center, using^OA₂Q₁' expression, the calculation formula is:

s174: using { O } as reference coordinate system to solve the position coordinate vector of instantaneous center, and using it^OQ₁'Q₁Show, by^OPQ₁Indicating that the origin P of the movable platform points to the origin Q of the shaft lug₁Vector of (1) by^OPQ₁'indicates that the origin P of the movable platform points to the vector of the instant center of rotation Q1', which has been obtained^OPA₂Therefore, the calculation formula is:

^OPQ₁＝^OP_Q1-^OOP，^OPQ₁'＝^OPA₂+^OA₂Q₁'，^OQ₁'Q₁＝^OPQ₁-^OPQ₁'

s175: in the { L2} coordinate system^L2V_A2Is represented by A₂Linear velocity of the spot, projected on the yz plane as^L2V_A2(y, z) the component of angular velocity (angular velocity about the x-axis) perpendicular to the yz plane was studied separately in the { L2} coordinate system and is denoted as^L2W_Q1,O(y, z) and then investigating the angular velocity component in the x-axis direction by^L2W_Q1,O(x) Represents, solves for A₂The linear velocity of the dots is calculated by the formula:

^L2V_A2＝(^OR_L2)^T·^OV_A2

^L2W_Q1,O＝[|^L2W_Q1(y,z)||^L2W_Q1(x)|0]^T

due to the foregoing already obtained^OR_L2So the angular velocity vector of the shaft lug is:^OW_Q1,O＝^OR_L2·^L2W_Q1,Ousing { O } as a reference coordinate system, Q₁The linear velocities of the dots were:^OV_Q1＝^OW_Q1×^OQ₁'Q₁。

s180: solving for the angular velocity of the intersecting axes of the first joint component, using { O } as the reference system, and recording as^OW_U1,O。

With { U₁As a reference coordinate system, with^U1W_U1,Q1Representing the angular velocity of the crossing axis of the first joint component relative to the axle lug, since the crossing axis rotates only about its own z-axis, it is possible to obtain:

according to the angular velocity superposition, the following can be obtained finally:

^OW_U1,O＝^OW_Q1,O+^OR_U1·^U1W_U1,Q1

s190: solving the angular velocity of the moving platform with { O } as the reference frame, i.e.^OW_P,O。

Using { P1} as a reference system^P1W_P,U1Representing the angular velocity of the mobile platform relative to the axis of intersection of the first joint components, we can obtain:

according to the angular velocity superposition. Angular velocity of the moving platform:

^OW_P,O＝^OW_U1,O+^OR_P1·^P1W_P,U1

According to the characteristics of the robot structure, due to P₁Point-fixed on the movable platform, and P₁,U₁,Q₁Three points coincide, so P₁The linear velocities of the dots were:

^OV_P1＝^OV_Q1

since has already obtained^OT_P1，^OT_P11 to 3, column 4, denotes P₁By OP₁Is shown to be so P₁The point-to-P point vector can be found: p₁P＝^OOP-OP₁The linear velocity of the center of the movable platform is as follows:^OV_P＝^OV_P1+^OW_P,O×P₁P。

the present invention also provides a six-axis robot, comprising:

at least one memory and at least one processor;

the memory for storing one or more programs;

when the one or more programs are executed by the at least one processor, the at least one processor may implement the steps of the method for solving the velocity of the six-axis robot according to the present invention.

In summary, the parallel six-axis robot speed positive solution method provided by the invention has the advantages that five angle encoders are installed at specific joints of the robot, the angle values and the angle speed values of 5 positions can be additionally read, and the linear speed and the angular speed of the movable platform at any moment are solved by combining the structural parameters of the given parallel six-axis robot and the extension amount and the extension speed of the piston rod of the electric cylinder at any moment. Because more information can be obtained through five encoders, the difficulty of solving a positive solution can be reduced, and in the operation process, only elementary matrix multiplication operation, namely simple multiplication and addition operation, is available, so that the calculation efficiency is effectively improved. The method is convenient to reconstruct by using programming languages such as C/C + + and the like, and the control system can be operated after reconstruction.

The above-mentioned embodiments only express several embodiments of the present invention, and the description thereof is specific and detailed, but not to be understood as limiting the scope of the invention. It should be noted that, for a person skilled in the art, several variations and modifications can be made without departing from the inventive concept, which falls within the scope of the present invention.

Claims

1. A parallel six-axis robot speed positive solution method based on an additional encoder is characterized in that,

the six-axis robot comprises a static platform, a movable platform, a hook hinge assembly, a joint assembly and an electric cylinder assembly;

the joint assembly includes:

a bearing seat;

the crossed shaft shell is rotatably arranged on the bearing seat;

the shaft lug is rotationally arranged on the crossed shaft shell, the rotation axis of the shaft lug on the crossed shaft shell is intersected with and perpendicular to the rotation axis of the crossed shaft shell on the bearing seat, two bearing holes are formed in the shaft lug and are symmetrically distributed on the crossed shaft shell relative to the rotation axis of the shaft lug, the axes of the two bearing holes are parallel to each other and are positioned on the same horizontal plane, and the axes of the bearing holes are perpendicular to the rotation axis of the crossed shaft shell on the bearing seat;

the speed positive solution method comprises the following steps:

s101: the first control assembly is used for measuring the rotation angle and the rotation angular velocity of a fixed shaft part and a swinging shaft part of the first hook joint assembly relative to the hook joint lower base and the hook joint upper base respectively, and the rotation angle and the rotation angular velocity are respectively recorded as j theta₁、jθ₂And

s20: constructing kinematic elements of the parallel six-axis robot;

R_Q: called the virtual circle radius of the intersecting axes, and represents the radius value of the circle determined by the origin of the three intersecting axes;

Q_iand i is 1 to 3: the origin of the axle lug, the part of the axle lug and the U_iAnd i is 1-3 corresponding to the overlapped points and is Q_i，i＝1～3；

{U_iJ and i ═ i1-3: respectively represent a cross-axis coordinate system, and the origin of the coordinate system is fixedly connected with the origin U of the cross-axis part_i，{U_iThe z-axis of the pivot lug is oriented along the axis of the shaft portion of the pivot lug, the y-axis is oriented along the axis of the central shaft, and the x-axis is determined according to the right-hand rule.

Distribution angle of pivot ear origin

A₁～A₆: the center of the hinge of the pin shaft is that the center point A of the hinge of the two pin shafts is arranged in the shaft lug part 1 (the shaft lug part 2 and the shaft lug part 3)₂(Point A)₄Point A₆) And point A₃(Point A)₅Point A₁) Point relative to { Q₁}({Q₂}，{Q₃) } about the Y axis of the substrate, point A₂(Point A)₄Point A₆) And point A₃(Point A)₅Point A₁) Is placed in { Q₁}({Q₂}，{Q₃}) in the XY plane, a is represented by QAX₂And A₃In { Q₁Absolute offset value in the X-axis direction of (A) }, QAY denotes A₂And A₃In { Q₁Absolute offset value in Y-axis direction;

Q₁': representing the instant center of the speed of the shaft lug part;

s30: the length of the electric cylinder is read from a control system of the electric cylinder assembly, is recorded as l 1-l 6 and is represented by l 1-l 6^ΟΒΑ₁～^ΟΒΑ₆Let l1 be non-zero^ΟΒΑ₁|，...，l6＝|^ΟΒΑ₆Reading out the expansion and contraction speed v of the piston rod in the electric cylinder assembly relative to the cylinder body from the control system of the electric cylinder assembly_Li,i＝1～6；

s60: calculating center B of second hook joint assembly₂Center B of third hook joint component₃Distance b of₂₃；

s100: solving for { Q₁Pose with respect to { O }, using^OT_Q1Represents;

s110: solving for { U₁Pose with respect to { O }, using^OT_U1Representing;

s120: solving for { P₁Pose with respect to { O }, using^OT_P1Representing;

s130: solving the pose of a moving platform, i.e. moving platformPosition of^OPose of OP and moving platform^OR_P；

S180: solving for the angular velocity of the intersecting axes of the first joint component, using { O } as the reference frame, denoted as^OW_U1,O；

2. The parallel six-axis robot speed positive solution method based on additional encoders as claimed in claim 1, wherein the center B of the second Hooke's joint assembly₂And the center B of the third hook joint component₃Distance b of₂₃The calculation formula of (c) is:

3. an additional encoder according to claim 1The method for solving the speed of the parallel six-axis robot is characterized in that the { O } is used as a reference system and is used^OP_B2、^OP_B3Indicates the origin positions of B2 and B3,^OR_B2、^OR_B3the postures of B2 and B3 are shown,^OT_B2、^OT_B3and representing a pose matrix, wherein the calculation formula for solving the poses of the coordinate systems of B2 and B3 is as follows:

^OP_B2＝R_b·[-sin(30°) cos(30°) 0]^T

4. the parallel six-axis robot speed positive solution method based on the additional encoder as claimed in claim 1, characterized in that^B2R_L2、^B3R_L3The postures of { L2}, { L3} relative to { B2}, and { B3} are represented by using { O } as a reference frame,^OT_L2、^OT_L3and representing a pose matrix, wherein the calculation formula for solving the poses of the { L2} and { L3} coordinate systems is as follows:

wherein, in order^OT_L2Is the attitude of the matrix formed by the 1 st to 3 rd rows and the 1 st to 3 rd columns^OR_L2To do so by^OT_L31 to 3 rows and 1 to 3 columns ofThe matrix being a gesture^OR_L3。

5. The parallel six-axis robot speed positive solution method based on the additional encoder as claimed in claim 1, characterized in that^L2P_A2、^L2P_A3Respectively, A represents a reference system of { L2}₂、A₃Position of point, said taking { O } as reference system, solving for A₂、A₃The calculation formula for the position of the point is:

^L2P_A2＝[0 0 l2]^T

^OP_A2＝^OR_L2·^L2P_A2；

^OP_A3＝^OR_L2·^L2P_A3。

6. the parallel six-axis robot speed positive solution method based on the additional encoder as claimed in claim 1, characterized in that^L2N₂₃The vector representing the point A2 points to the point A3, and taking { L2} as a reference frame:^L2N₂₃＝^L2P_A3-^L2P_A2according to the normal vector solving rule of the two-dimensional vector, the sum can be obtained^L2N₂₃The unit vector of the vertical is^L2N_⊥23；

By alpha₂₃To represent^L2N₂₃And the z-axis of { L2}, i.e. the vector z_L2＝[0 0 1]^TThe value of the included angle is shown in the specification,^L2A_o23is represented by A₂Point sum A₃The position vector of the center point of the point,^L2R_Q1represents { Q₁The pose with respect to L2,^L2P_Q1represents { Q₁Position vector with origin in { L2} coordinate system, the solution { Q₁Pose with respect to { O }^OT_Q1The calculation formula of (2) is as follows:

^L2P_Q1＝^L2A_o23+^L2N_⊥23·QAY

7. The parallel six-axis robot speed forward solving method based on the additional encoder as claimed in claim 1, characterized in that { Q } is used₁Using as a reference frame^Q1R_U1Represents { U₁Relative to { Q }₁The gesture of the electronic component is changed,^Q1P_U1represents { U }₁The origin of the equation is at { Q }₁Position vector in the coordinate system, the solution { U }₁Pose with respect to { O }^OT_U1The calculation formula of (2) is as follows:

^Q1P_U1＝[0 0 0]

8. the parallel six-axis robot speed forward solving method based on the additional encoder as claimed in claim 1, characterized in that { U } is used as the basis₁Using as a reference frame^U1R_P1Represents { P₁Relative to { U }₁The gesture of the electronic component is changed,^U1P_P1represents { P₁The origin of { U } is at₁Position vector in the coordinate system, the solution { P }₁Pose with respect to { O }^OT_P1The calculation formula of (2) is as follows:

^U1P_P1＝[0 0 0]

9. the parallel six-axis robot speed forward solving method based on the additional encoder as claimed in claim 1, characterized in that { P } is used₁Using as a reference frame^P1R_PDenotes { P } relative to { P }₁The gesture of the electronic component is changed,^P1P_Pthe origin of the expression { P } is at { P₁A position vector in the coordinate system is multiplied,^OT_Prepresenting the pose of { P } relative to { O }, the position of the mobile platform^OPose of OP and moving platform^OR_PThe calculation formula of (2) is as follows:

^P1P_P＝[-R_Q 0 0]

10. The parallel six-axis robot speed positive solution method based on the additional encoder as claimed in claim 1, characterized in that^B2W_B2,ORepresenting the angular velocity vector of the cross-axis of the second hook-and-loop assembly relative to the stationary platform using { B2} as a reference coordinate system,^B3W_B3,Othe angular velocity vector of the cross shaft of the third hook joint assembly relative to the static platform is expressed by taking { B3} as a reference coordinate system, and the angular velocity vector of the cross shaft of the second hook joint assembly and the angular velocity vector of the cross shaft of the third hook joint assembly relative to the static platform are solved by taking { O } as the reference coordinate system^OW_B2,OAnd^OW_B3,Othe calculation formula of (2) is as follows:

^OW_B2,O＝^OR_B2·^B2W_B2；

^OW_B3＝^OR_B3·^B3W_B3,O。

11. the parallel six-axis robot speed positive solution method based on the additional encoder as claimed in claim 1, characterized in that^L2W_L2,B2Indicating the angular velocity vector of the cylinder of the second cylinder assembly relative to the cross-shaft, using { L2} as a reference coordinate system,^L3W_L3,B3representing the angular velocity vector of the cylinder body of the third cylinder assembly relative to the cross-shaft with { L3} as a reference coordinate system^OW_L2,B2Representing the angular velocity vector of the cylinder body of the second cylinder assembly relative to the cross-shaft, with { O } as a reference coordinate^OW_L3,B3Representing angular velocity vectors of the cylinder body of the third electric cylinder assembly relative to the cross shaft by taking { O } as a reference coordinate system, and solving the angular velocity vectors of the cylinder bodies of the second electric cylinder assembly and the third electric cylinder assembly relative to the static platform by taking { O } as a reference coordinate system^OW_L2,OAnd^OW_L3,Othe calculation formula of (2) is as follows:

^OW_L2,B2＝^OR_L2·^L2W_L2,B2，^OW_L2,O＝^OW_B2,O+^OW_L2,B2；

^OW_L3,B3＝^OR_L3·^L3W_L3,B3，^OW_L3,O＝^OW_B3,O+^OW_L3,B3。

12. the parallel six-axis robot speed forward solution method based on additional encoders as claimed in claim 1, characterized in that { O } is used as a reference coordinate system to^ΟΒΑ₂A position vector representing the second electric cylinder assembly,^ΟΒΑ₃representing a position vector of a third electric cylinder component, solving for a hinge center A of the pin shaft₂，A₃The linear velocity vector of (a) is calculated by the following formula:

^OBA₂＝^OP_A2-^OP_B2，^OV_A2＝^OW_L2,O×^OBA₂+^OR_L2·[0 0 v_L2]^T；

^OBA₃＝^OP_A3-^OP_B3，^OV_A3＝^OW_L3,O×^OBA₃+^OR_L3·[0 0 v_L3]^T。

13. the parallel six-axis robot speed positive solution method based on the additional encoder as claimed in claim 1, characterized by comprising the following steps:

s171: angle theta due to instant center of rotation Q1₃＝180-θ₁-θ₂Wherein, theta₁Is represented by A₂A₃And A₂Q₁Angle between, θ₂Is represented by A₃A₂And A₃Q₁The angle between the two is based on { O } as reference system^ON₂₃Vector representing point A2 pointing to point A3:^ON₂₃＝^OP_A3-^OP_A2，^OV_A2(1) the 1 st value representing the three-dimensional vector,^OV_A2(2) the 2 nd value of the three-dimensional vector is represented, and the calculation formula of the angle of the instant center of rotation Q1' is as follows:

θ₃＝180-θ₁-θ₂；

s172: solving the distance value | A of the instantaneous center of rotation Q1' from A2₂Q₁' |, since QAX represents A₂And A₃In { Q₁Absolute offset value in X-axis direction, | A₂A₃I is 2 · QAX, so:

s174: using { O } as reference coordinate system to solve the position coordinate vector of instantaneous center, and using it^OQ₁'Q₁Show, by^OPQ₁Indicating that the origin P of the movable platform points to the origin Q of the shaft lug₁Vector of (a)^OPQ₁'indicating that the moving platform origin point P points to the vector of the instant center of rotation Q1',^OQ₁'Q₁the calculation formula is as follows:

^OPQ₁＝^OP_Q1-^OOP，^OPQ₁'＝^OPA₂+^OA₂Q₁'，^OQ₁'Q₁＝^OPQ₁-^OPQ₁'；

s175: in the { L2} coordinate system^L2V_A2Is represented by A₂Linear velocity of the spot, projected on the yz plane as^L2V_A2(y, z) the component of angular velocity perpendicular to the yz plane (angular velocity about the x-axis) is separately studied in the { L2} coordinate system and is denoted as^L2W_Q1,O(y, z) and then investigating the angular velocity component in the x-axis direction by^L2W_Q1,O(x) Represents, solves for A₂The linear velocity of the dots is calculated by the formula:

^L2V_A2＝(^OR_L2)^T·^OV_A2

^L2W_Q1,O＝[|^L2W_Q1(y,z)| |^L2W_Q1(x)| 0]^T；

due to the fact that^OR_L2Has been obtained, the angular velocity vector of the shaft ear is:^OW_Q1,O＝^OR_L2·^L2W_Q1,O；

using { O } as a reference coordinate system, Q₁The linear velocities of the dots were:^OV_Q1＝^OW_Q1×^OQ₁'Q₁。

14. the parallel six-axis robot speed forward solving method based on the additional encoder as claimed in claim 1, characterized in that { U } is used as the basis₁Using as a reference coordinate system^U1W_U1,Q1The angular velocity of the crossed shaft of the first joint component relative to the axle ear is expressed, the calculation formula for solving the angular velocity of the crossed shaft of the first joint component by taking { O } as a reference system is as follows:

^OW_U1,O＝^OW_Q1,O+^OR_U1·^U1W_U1,Q1。

15. the parallel six-axis robot speed forward solving method based on the additional encoder as claimed in claim 1, characterized in that { P1} is used as a reference frame^P1W_P,U1And expressing the angular velocity of the movable platform relative to the crossed axis of the first joint component, wherein the calculation formula for solving the angular velocity of the movable platform by taking the { O } as a reference system is as follows:

^OW_P,O＝^OW_U1,O+^OR_P1·^P1W_P,U1。

16. the parallel six-axis robot speed positive solution method based on the additional encoder as claimed in claim 1, characterized in that, since P₁Point-fixed on the movable platform, and P₁,U₁,Q₁Three points coincide, so P₁The linear velocities of the dots were:^OV_P1＝^OV_Q1since it has already obtained^OT_P1，^OT_P11 to 3, column 4, denotes P₁By OP₁Is shown to be so P₁The vector from point to point P can be found: p₁P＝^OOP-OP₁The linear velocity of the center of the movable platform is as follows:^OV_P＝^OV_P1+^OW_P,O×P₁P。

17. a six-axis robot, comprising:

at least one memory and at least one processor;

the memory for storing one or more programs;

when executed by the at least one processor, cause the at least one processor to carry out the steps of the six-axis robot velocity positive solution method of any one of claims 1 to 16.