3RRR spherical parallel robot inverse kinematics solving method, system, medium and equipment

Abstract

The application provides a 3RRR spherical parallel robot inverse kinematics solving method, system, medium and equipment, including: step S1: solving the matrix of rotation transformation according to the attitude angle of the motion platform; step S2: defining the positions of three rotating pairs on each supporting arm from bottom to top as G, F and S respectively; calculating the position S according to the matrix of rotation transformation; step S3: calculating the position F according to the position S and the position G, and calculating the space angle of the connecting shaft of the driving link and the driven link based on the calculated position F. The inverse kinematics solving method based on the geometric method provided by the application can be directly converted into a computer readable program without the aid of other means, and is intuitive and fast.

Description

Technical Field

[0001] This invention relates to the fields of robot kinematics and robot motion control, specifically to a method, system, medium, and equipment for solving the inverse kinematics of a 3RRR spherical parallel robot. Background Technology

[0002] With the development of parallel robot technology, people have gradually begun to pay attention to non-six-degree-of-freedom parallel mechanisms. These parallel mechanisms have better applicability in specific situations, such as teleoperated manipulators and medical rehabilitation. Because the mechanical structure of parallel mechanisms is more special than that of serial mechanisms, their kinematic analysis is more complex.

[0003] Patent document CN113255111A (application number: 202110491224.7) discloses a method and system for solving the inverse kinematics of a two-degree-of-freedom parallel mechanism, including establishing the coordinate system of each link, finding the kinematic parameter expression of the kinematic equation of the parallel mechanism, finding the pose parameter expression of the kinematic equation of the parallel mechanism, solving the motion of the driving device of the parallel mechanism, and solving the motion of other kinematic pairs of the parallel mechanism.

[0004] For 3RRR spherical parallel mechanisms, the current general inverse kinematics methods are analytical methods based on spherical coordinates or algebraic methods based on quaternions. However, these two methods are complex to express, cumbersome to calculate, not intuitive, and not easy to understand. Summary of the Invention

[0005] To address the shortcomings of existing technologies, the purpose of this invention is to provide a method, system, medium, and device for solving the inverse kinematics of a 3RRR spherical parallel robot.

[0006] A method for solving the inverse kinematics of a 3RRR spherical parallel robot, provided by the present invention, includes: Step S1: Solve for the rotation transformation matrix based on the attitude angles of the motion platform; Step S2: Define the positions of the three revolute joints on each arm from bottom to top as follows: , as well as ; Calculate the position based on the rotation transformation matrix ; Step S3: Based on location and location Calculate position Based on the calculated position Calculate the spatial angle of the connecting axis between the driving link and the driven link.

[0007] Preferably, step S3 employs the following methods: Step S3.1: Based on location and location Calculated and ; Step S3.2: Based on the calculation and Calculated ; Step S3.3: Based on the calculation... Calculated position ; Step S3.4: Based on the calculated position The spatial angle of the connecting axis between the driving link and the driven link is calculated. ; Among them, the center of the base is defined as Point; location ,Location and location The intersection point is defined as ; express Click The perpendicular segment; yes arrive The perpendicular segment; express and The angle between them.

[0008] Preferably, step S3 employs the following methods:

[0009]

[0010]

[0011]

[0012]

[0013]

[0014]

[0015] in, and This indicates that the entire mechanism is in its initial, set state. and The location.

[0016] Preferably, the correctness of the spatial angle of the connection axis between the active link and the driven link is verified by using constraint equations; The constraints are as follows:

[0017]

[0018]

[0019] in, Indicates the included angle between the two axes on the drive link; This indicates the included angle between the two axes on the driven rod.

[0020] A 3RRR spherical parallel robot inverse kinematics solution system provided by the present invention includes: Module M1: Solve for the rotation transformation matrix based on the attitude angles of the motion platform; Module M2: Defines the positions of the three revolute joints on each arm from bottom to top as follows: , as well as ; Calculate the position based on the rotation transformation matrix ; Module M3: Based on location and location Calculate position Based on the calculated position Calculate the spatial angle of the connecting axis between the driving link and the driven link.

[0021] Preferably, the module M3 adopts: Module M3.1: Based on location and location Calculated and ; Module M3.2: Based on calculations and Calculated ; Module M3.3: Based on the calculation Calculated position ; Module M3.4: Based on the calculated position The spatial angle of the connecting axis between the driving link and the driven link is calculated. ; Among them, the center of the base is defined as Point; location ,Location and location The intersection point is defined as ; express Click The perpendicular segment; yes arrive The perpendicular segment; express and The angle between them.

[0022] Preferably, the module M3 adopts:

[0023]

[0024]

[0025]

[0026]

[0027]

[0028]

[0029] in, and This indicates that the entire mechanism is in its initial, set state. and The location.

[0030] Preferably, the correctness of the spatial angle of the connection axis between the active link and the driven link is verified by using constraint equations; The constraints are as follows:

[0031]

[0032]

[0033] in, Indicates the included angle between the two axes on the drive link; This indicates the included angle between the two axes on the driven rod.

[0034] According to the present invention, a computer-readable storage medium storing a computer program is provided, wherein when the computer program is executed by a processor, it implements the steps of the above-described method for solving the inverse kinematics of a 3RRR spherical parallel robot.

[0035] An electronic device according to the present invention includes: a memory, a processor, and a computer program stored in the memory and executable on the processor. When the computer program is executed by the processor, it implements the steps of the above-described method for solving the inverse kinematics of a 3RRR spherical parallel robot.

[0036] Compared with the prior art, the present invention has the following beneficial effects: 1. This invention proposes a geometric method for solving the inverse kinematics of a 3RRR spherical parallel mechanism. Traditional inverse kinematics methods obtain results by solving a system of equations, which is not suitable for programming logic. The geometric method proposed in this invention can be directly converted into a computer-readable program without the need for other means, which is intuitive and fast. 2. This invention, combined with the 3RRR spherical mechanism, can be used in various scenarios, such as rehabilitation robots for wrists and ankles, and remotely operated robotic hands.

[0037] 3. Traditional inverse kinematics solutions use physical constraints of the mechanism to construct constraint equations for solving, but cannot verify the correctness of the calculation results. This invention uses a geometric method based on physical constraints, which does not involve constraint equations. The correctness of the calculation results can be verified by constructing constraint equations, thus ensuring the correctness of the inverse kinematics solution. Attached Figure Description

[0038] Other features, objects, and advantages of the present invention will become more apparent from the following detailed description of non-limiting embodiments with reference to the accompanying drawings: Figure 1 Flowchart of the inverse kinematics solution method for a 3RRR spherical parallel robot.

[0039] Figure 2 This is a schematic diagram of the solid 3RRR spherical parallel mechanism used in this inverse kinematics method.

[0040] Figure 3 This is a schematic diagram of the joints of a solid 3RRR spherical parallel mechanism used in this inverse kinematics method.

[0041] Figure 4 This is a schematic diagram illustrating the analysis of a single branch of a solid 3RRR spherical parallel mechanism.

[0042] Figure 5 This is a schematic diagram illustrating the solution principle of the inverse kinematics method. Detailed Implementation

[0043] The present invention will now be described in detail with reference to specific embodiments. These embodiments will help those skilled in the art to further understand the present invention, but do not limit the invention in any way. It should be noted that those skilled in the art can make several changes and improvements without departing from the concept of the present invention. These all fall within the protection scope of the present invention.

[0044] Example 1 The present invention provides a method for solving the inverse kinematics of a 3RRR spherical parallel robot, such as... Figure 1 As shown, it includes: Step S1: Solve for the rotation transformation matrix based on the attitude angles of the motion platform; Step S2: Define the positions of the three revolute joints on each arm from bottom to top as follows: , as well as ; Calculate the position based on the rotation transformation matrix ; Step S3: Based on location and location Calculate position Based on the calculated position Calculate the spatial angle of the connecting axis between the driving link and the driven link.

[0045] Specifically, step S3 employs the following: Step S3.1: Based on location and location Calculated and ; Step S3.2: Based on the calculation and Calculated ; Step S3.3: Based on the calculation... Calculated position ; Step S3.4: Based on the calculated position The spatial angle of the connecting axis between the driving link and the driven link is calculated. ; Among them, the center of the base is defined as Point; location ,Location and location The intersection point is defined as ; express Click The perpendicular segment; yes arrive The perpendicular segment; express and The angle between them.

[0046] Specifically, step S3 employs the following:

[0047]

[0048]

[0049]

[0050]

[0051]

[0052]

[0053] in, and This indicates that the entire mechanism is in its initial, set state. and The location.

[0054] Specifically, the correctness of the spatial angle of the connection axis between the active link and the driven link is verified by using constraint equations; The constraints are as follows:

[0055]

[0056]

[0057] in, Indicates the included angle between the two axes on the drive link; This indicates the included angle between the two axes on the driven rod.

[0058] A 3RRR spherical parallel robot inverse kinematics solution system provided by the present invention includes: Module M1: Solve for the rotation transformation matrix based on the attitude angles of the motion platform; Module M2: Defines the positions of the three revolute joints on each arm from bottom to top as follows: , as well as ; Calculate the position based on the rotation transformation matrix ; Module M3: Based on location and location Calculate position Based on the calculated position Calculate the spatial angle of the connecting axis between the driving link and the driven link.

[0059] Specifically, module M3 adopts: Module M3.1: Based on location and location Calculated and ; Module M3.2: Based on calculations and Calculated ; Module M3.3: Based on the calculation Calculated position ; Module M3.4: Based on the calculated position The spatial angle of the connecting axis between the driving link and the driven link is calculated. ; Among them, the center of the base is defined as Point; location ,Location and location The intersection point is defined as ; express Click The perpendicular segment; yes arrive The perpendicular segment; express and The angle between them.

[0060] Specifically, module M3 adopts:

[0061]

[0062]

[0063]

[0064]

[0065]

[0066]

[0067] in, and This indicates that the entire mechanism is in its initial, set state. and The location.

[0068] Specifically, the correctness of the spatial angle of the connection axis between the active link and the driven link is verified by using constraint equations; The constraints are as follows:

[0069]

[0070]

[0071] in, Indicates the included angle between the two axes on the drive link; This indicates the included angle between the two axes on the driven rod.

[0072] According to the present invention, a computer-readable storage medium storing a computer program is provided, wherein when the computer program is executed by a processor, it implements the steps of the above-described method for solving the inverse kinematics of a 3RRR spherical parallel robot.

[0073] An electronic device according to the present invention includes: a memory, a processor, and a computer program stored in the memory and executable on the processor. When the computer program is executed by the processor, it implements the steps of the above-described method for solving the inverse kinematics of a 3RRR spherical parallel robot.

[0074] Example 2 Example 2 is a preferred example of Example 1. To address the shortcomings of existing technologies, the purpose of this invention is to provide a method for obtaining the inverse kinematics of a 3RRR spherical parallel mechanism based on geometric methods. This invention first mathematically expresses the 3RRR spherical parallel mechanism, then calculates the joint angles based on geometric constraints, and finally verifies the correctness of the inverse solution based on the constraint equations.

[0075] like Figure 2 As shown, the 3RRR spherical parallel mechanism of this invention comprises a base 1, a motor mount 2, servo motors 3, a coupling 4, a drive shaft 5, a bearing seat 6, a pin 7, a driving link 8, a lower bearing 9, a lower drive shaft 10, a driven link 11, an upper bearing 12, an upper drive shaft 13, a connector 14, a motion platform 15, a grip 16, and a grip sleeve 17. The motor mount 2 secures the three servo motors 3 to the base 1 with bolts, evenly distributed around the center. The output torque of the servo motors 3 is transmitted to the drive shaft 5 via the coupling 4. The drive shaft 5 passes through the holes of the bearing seat 6 and the driving link, which are bolted to the platform. The driving link 8 and the drive shaft 5 are fixed together using the pin 7. The lower bearing 9 is installed at the upper end of the driving link 8, and the lower drive shaft 10 passes through the lower bearing 9 and the lower end of the driven link 11, connecting the driving link 8 and the driven link 11 axially. An upper bearing 12 is mounted on the upper end of the driven link 11, and an upper drive shaft 13 passes through the upper bearing 12 and the connecting member 14. The connecting member 14 is bolted to the motion platform 15. The grip 16 is bolted to the motion platform 15, and a grip sleeve 17 is sleeved on the grip 16.

[0076] When the parallel mechanism is working, the servo motor 3 generates rotation and torque on the output shaft, which is transmitted to the drive shaft 5 through the coupling 4, thereby driving the active connecting rod 8, which is fixed to the drive shaft 5, to rotate around its lower axis. If the three active connecting rods 8 rotate simultaneously, the driven connecting rod 11 will move accordingly due to the constraint of the upper ends of the three upper drive shafts 13 by the connecting piece 14 fixed to the motion platform 15. Ultimately, a centering three-degree-of-freedom rotational motion occurs on the motion platform 15. To control the attitude of the motion platform 15, it is necessary to use this invention to calculate the motion angles of the three servo motors 3, i.e., the rotation angles of the drive shafts, and use... To express.

[0077] like Figure 3 As shown, the three support chains are named chains 1, 2, and 3 in counter-clockwise order. Each support has three revolute joints, named G, F, and S from bottom to top. The three axes of rotation intersect at a single point. Furthermore, the three supporting chains are also concentric. Nine-axis concentric At this time, the moving platform rotates... It has three degrees of freedom of centering motion, so the workspace depends on the angular parameters of the 9 axes.

[0078] like Figure 4 Expand one of the branches. Indicates the offset angle of the motor mount. Indicates the included angle between the two axes on the drive link. This indicates the angle between the two axes on the driven rod. This represents the angle between the plane on the moving platform and the axis. For ease of design, the above parameters for each branch are taken as the same value.

[0079] For this 3RRR spherical parallel mechanism, the included angle between the upper and lower ends of the axes of the driving link 8 and the driven link 11 remains stable, and all three connecting parts 14 are fixed on the plane of the motion platform 15. Three sets of constraint equations used for kinematic calculations can be obtained: (1) (2) (3) Wherein, (1) indicates that the included angle ∠SO'F of the driven rod constraint is fixed, (2) indicates that the included angle ∠FO'G of the driving rod constraint is fixed, and (3) indicates the three vectors of the moving platform constraint. , , Coplanar. This represents the vector formed by S1 and the origin O'. Let S2 be the vector formed by the origin O'. Let S3 be the vector formed by S3 and the origin O'.

[0080] For calculating the target angle of the servo motor, we propose a geometry-based solution algorithm. First, since the shape of the moving platform itself is fixed, the rotation transformation matrix can be obtained from the attitude angle of the moving platform, thus yielding the desired result. That is, the attitude of the 14 connectors; next, a more complex problem is the known... Given two axes, namely the position of the moving platform and the position of the lower end of the driving rod, find the vectors that form fixed angles with each axis. Based on the coordinate positions of F and G, the spatial angle of the connection axis between the driving and driven links can be calculated. In fact, since the mathematical model of a spherical parallel robot is determined by angles, vectors... The length is not important; it can be specified. All are unit lengths. For example... Figure 5 For a square pyramid Given the lengths of the three lateral edges and the vertex angles of the three lateral faces, the lengths of the three sides of the base can be calculated. Therefore The shape is completely determined, and the edges Fixed in space, The feasible region around the edge Inside the cone where the two bases meet after rotation. To determine... Spatial location, setting Edge The angle of rotation is It can be represented in the local coordinate system. ;

[0081]

[0082]

[0083] Among them, due to and The shapes of the triangles are known, and their areas are fixed. According to Heron's formula, the area of ​​a triangle can be calculated given its three sides. Therefore, we can obtain... and :

[0084]

[0085] and :

[0086]

[0087]

[0088] Reuse constraints

[0089] Right now

[0090] Solving

[0091] like If the position is unattainable for the corresponding motion platform 15, then the motion platform can achieve this position. Then, it is represented in a relative coordinate system. Then, a coordinate transformation can be performed to obtain the vector. Values ​​in the absolute coordinate system.

[0092] In order to obtain The rotation angle requires the construction of a new vector. zero position Calculated ,and All are perpendicular to On the plane, thus

[0093] Obtain the rotation angle of the drive shaft The size, combined with Direction determination The positive and negative.

[0094] Those skilled in the art will understand that, besides implementing the system and its various devices, modules, and units provided by this invention in the form of purely computer-readable program code, the same functions can be achieved entirely through logical programming of the method steps, enabling the system and its various devices, modules, and units to function in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers. Therefore, the system and its various devices, modules, and units provided by this invention can be considered a hardware component, and the devices, modules, and units included therein for implementing various functions can also be considered structures within the hardware component; alternatively, the devices, modules, and units for implementing various functions can be considered both software modules implementing the method and structures within the hardware component.

[0095] Specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the specific embodiments described above, and those skilled in the art can make various changes or modifications within the scope of the claims, which do not affect the essence of the present invention. Unless otherwise specified, the embodiments and features described in this application can be arbitrarily combined with each other.

Claims

1. A method for solving the inverse kinematics of a 3RRR spherical parallel robot, characterized in that, include: Step S1: Solve for the rotation transformation matrix based on the attitude angles of the motion platform; Step S2: Define the positions of the three revolute joints on each arm from bottom to top as follows: , as well as ; Calculate the position based on the rotation transformation matrix ; Step S3: Based on location and location Calculate position Based on the calculated position Calculate the spatial angle between the connecting axes of the driving link and the driven link; Step S3 employs the following: Step S3.1: Based on location and location Calculated and ; Step S3.2: Based on the calculation and Calculated ; Step S3.3: Based on the calculation... Calculated position ; Step S3.4: Based on the calculated position The spatial angle of the connecting axis between the driving link and the driven link is calculated. ; Among them, the center of the base is defined as Point; location ,Location and location The intersection point is defined as ; express Click The perpendicular segment; yes arrive The perpendicular segment; express and The angle between them; Step S3 employs the following: Regulation All are unit lengths, expressed in the local coordinate system. ; Reuse constraints Right now Solving like If the position is unattainable for the corresponding motion platform, then the position is unattainable; otherwise, it means that the motion platform can achieve this position. Reconstruct vectors zero position Calculated ,and All are perpendicular to On the plane, thus Obtain the rotation angle of the drive shaft The size, combined with Direction determination Positive and negative; The correctness of the spatial angle of the connection axis between the active link and the driven link obtained by the calculation is verified by using constraint equations. The constraints are as follows: in, Indicates the included angle between the two axes on the drive link; This indicates the included angle between the two axes on the driven rod.

2. A system for solving the inverse kinematics of a 3RRR spherical parallel robot, characterized in that, include: Module M1: Solve for the rotation transformation matrix based on the attitude angles of the motion platform; Module M2: Defines the positions of the three revolute joints on each arm from bottom to top as follows: , as well as ; Calculate the position based on the rotation transformation matrix ; Module M3: Based on location and location Calculate position Based on the calculated position Calculate the spatial angle between the connecting axes of the driving link and the driven link; The module M3 adopts: Module M3.1: Based on location and location Calculated and ; Module M3.2: Based on calculations and Calculated ; Module M3.3: Based on the calculation Calculated position ; Module M3.4: Based on the calculated position The spatial angle of the connecting axis between the driving link and the driven link is calculated. ; Among them, the center of the base is defined as Point; location ,Location and location The intersection point is defined as ; express Click The perpendicular segment; yes arrive The perpendicular segment; express and The angle between them; The module M3 adopts: Regulation All are unit lengths, expressed in the local coordinate system. ; Reuse constraints Right now Solving like If the position is unattainable for the corresponding motion platform, then the position is unattainable; otherwise, it means that the motion platform can achieve this position. Reconstruct vectors zero position Calculated ,and All are perpendicular to On the plane, thus Obtain the rotation angle of the drive shaft The size, combined with Direction determination Positive and negative; The correctness of the spatial angle of the connection axis between the active link and the driven link obtained by the calculation is verified by using constraint equations. The constraints are as follows: in, Indicates the included angle between the two axes on the drive link; This indicates the included angle between the two axes on the driven rod.

3. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it implements the steps of the inverse kinematics solution method for the 3RRR spherical parallel robot as described in claim 1.

4. An electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the computer program is executed by the processor, it implements the steps of the inverse kinematics solution method for the 3RRR spherical parallel robot as described in claim 1.

Images