Parallel robot positioning bed control method, system and storage medium

Abstract

This invention relates to the field of medical device technology, specifically to a control method, system, and storage medium for a parallel robot positioning bed. The method includes: S1, inputting the theoretical pose, measuring the actual pose, and calculating the error matrix; S2, constructing an inverse kinematics model and calculating the first theoretical displacement; S3, constructing a forward kinematics model and obtaining the Jacobian matrix; S4, establishing the residual vector equation and decomposing the geometric error parameters; S5, fitting the total error parameters and completing calibration; S6, obtaining the initial iterative pose and the second actual displacement, inputting them into the inverse kinematics model, calculating the difference between the second theoretical displacement and the second actual displacement, and if it is less than a first threshold, outputting the initial iterative pose; otherwise, proceeding to step S7; S7, inputting the forward kinematics model and calculating the pose error, and if the pose error is less than a second threshold, outputting the initial iterative pose; otherwise, repeating step S6. This invention improves the positioning accuracy of the parallel robot positioning bed.

Description

Technical Field

[0001] This invention relates to the technical field of medical devices, and more specifically, to a control method, system, and storage medium for a parallel robot positioning bed. Background Technology

[0002] Radiotherapy is a core method in clinical cancer treatment. With the widespread application of precision radiotherapy technologies such as stereotactic radiotherapy, clinical practice has placed more stringent demands on patient positioning and precise alignment of the tumor target area. Parallel-mechanism radiotherapy robot positioning tables, with their advantages of high structural rigidity, precise six-degree-of-freedom pose control, and fast dynamic response, have become the core positioning component of high-end precision radiotherapy equipment. Their motion control and error compensation effects directly determine the accuracy of radiotherapy positioning and treatment safety.

[0003] Existing control technologies for parallel radiotherapy robotic positioning beds generally suffer from insufficient positioning accuracy. They lack systematic calibration and error compensation for geometric errors of the mechanism and lack closed-loop real-time pose correction, which easily leads to pose deviations and error accumulation, failing to meet the high-precision positioning requirements of parallel robotic positioning beds for precise radiotherapy. Summary of the Invention

[0004] The purpose of this invention is to overcome the shortcomings of existing technologies in terms of positioning accuracy, which makes it difficult to meet the needs of precise radiotherapy. This invention provides a parallel robot positioning bed control method, system, and storage medium that calibrates and compensates for geometric errors during assembly and performs closed-loop correction for errors during motion, thereby improving positioning accuracy.

[0005] To solve the above-mentioned technical problems, the technical solution adopted by the present invention is as follows: A method for controlling a parallel robot positioning bed is provided, comprising the following steps: S1. Input the theoretical pose of the upper platform. And the actual pose of the platform is measured using testing equipment. Calculate the actual pose Theoretical pose Error matrix between ; S2. Construct an inverse kinematics model, and calculate the first theoretical displacement corresponding to each linear module based on the inverse kinematics model. ; S3. Construct a forward kinematics model, and obtain the Jacobian matrix based on the forward kinematics model. ; S4. Based on the error matrix With the Jacobian matrix Establish the residual vector equation and calculate the residual vector. , the residual vector Decompose into geometric error parameters; S5. Fit the geometric error parameters to obtain the total error parameters, which are used to calibrate the mechanical structure of the parallel robot positioning bed; S6. Obtain the initial iterative pose. The second actual displacement of each linear module The initial iterative pose Input the inverse kinematics model and calculate the second theoretical displacement corresponding to each linear module. and the second theoretical displacement With the second actual displacement difference If the second theoretical displacement With the second actual displacement difference Less than the first threshold Then output the initial iterative pose. If the target pose is determined, proceed to step S7; S7, the second theoretical displacement amount Input the forward kinematics model and obtain the Jacobian matrix. According to the Jacobian matrix Calculate pose error If the pose error Less than the second threshold Then output the initial iterative pose. As the target pose, otherwise for the initial iterative pose Perform the update and repeat step S6.

[0006] The parallel robot positioning bed control method of the present invention first obtains the error matrix by inputting the theoretical pose and measuring the actual pose. Then, it calculates the theoretical displacement of each linear module based on the inverse kinematics model and constructs the Jacobian matrix using the forward kinematics model. Subsequently, it establishes the residual vector equation to solve for the geometric error parameters and obtains the total error parameters through fitting to calibrate and compensate for the assembly geometric errors of the mechanical structure of the parallel robot positioning bed. Then, in the motion control stage, iterative judgment is made by the deviation between the initial iterative pose and the measured displacement. The displacement error and pose error are calculated using the inverse and forward kinematics models until the error is less than the threshold, thereby realizing closed-loop correction of the dynamic error in the motion process. The present invention combines static calibration and dynamic correction, which significantly improves the positioning accuracy of the radial parallel robot positioning bed.

[0007] Preferably, in step S1, the error matrix The calculation method is to calculate the actual pose. The components and the theoretical pose The difference between the corresponding components yields a six-dimensional error matrix, which includes translation errors along the X, Y, and Z axes, as well as rotation errors around the X, Y, and Z axes. By calculating the difference between each component of the actual pose and the theoretical pose, a six-dimensional error vector containing three translation errors and three rotation errors is obtained. This vector can comprehensively and accurately quantify the positioning deviation of the parallel robot positioning bed in six degrees of freedom in space, providing a complete error input for subsequent error parameter identification and avoiding the problem of compensation omission or miscompensation caused by incomplete error representation.

[0008] Preferably, the method for constructing the inverse kinematics model includes the following steps: S21. Obtain the coordinates of each hinge point on the lower platform in the lower platform coordinate system. The coordinates of each hinge point on the upper platform in the upper platform coordinate system Link length and the unit vector of the linear module's motion direction ; S22. Based on the theoretical pose of the upper platform The coordinates of each hinge point on the upper platform in the upper platform coordinate system are obtained by using rotation and translation transformations. Transform to the lower platform coordinate system to obtain the transformed coordinates. ; S23. For each branch, based on the coordinates of each hinge point of the lower platform in the lower platform coordinate system... , coordinate transformation and the unit vector of the linear module's motion direction The first theoretical displacement corresponding to this branch is calculated through spatial geometric relationships. By acquiring the coordinates of the lower platform hinge point, the fixed coordinates of the upper platform hinge point, the link length, and the unit vector of the linear module's motion direction, and using rotation and translation transformations to convert the upper platform hinge point to the lower platform coordinate system, and then calculating the theoretical displacement of each branch based on spatial geometric relationships, a complete inverse kinematic model is constructed. This model can accurately establish the mapping relationship between the target pose of the upper platform and the displacement of each drive module, providing a precise theoretical benchmark for subsequent error calibration and closed-loop control, effectively improving the positioning and control accuracy of the parallel robot positioning bed.

[0009] Preferably, in step S21, the unit vector of the linear module's motion direction is determined by a predefined unit vector matrix. Each column of the unit vector matrix corresponds to a direction component of the linear module in a Cartesian coordinate system, and the motion directions of all linear modules lie within the horizontal plane. Using a predefined unit vector matrix to determine the motion direction of each linear module, and ensuring that all motion directions lie within the horizontal plane, simplifies the input and storage of direction vectors in the inverse kinematics solution. The motion layout within the horizontal plane makes the geometric relationship between the linear modules and the connecting rods more regular, which is beneficial for improving the stability and computational efficiency of the inverse kinematics model.

[0010] Preferably, in step S22, the transformed coordinates The calculation method includes the following steps: S221, Based on the theoretical pose of the upper platform The translation amount determines the position of the origin of the upper platform in the coordinate system of the lower platform. ; S222, Based on the theoretical pose of the upper platform The rotation amounts in the figure are used to construct rotation matrices around the X, Y, and Z axes of the lower platform coordinate system in sequence. ; S223. For each hinge point, calculate the position of the upper platform origin in the lower platform coordinate system. The coordinates of each hinge point of the upper platform in the upper platform coordinate system With the rotation matrix The sum of the products yields the transformed coordinates. The position vector of the upper platform origin in the lower platform coordinate system is determined by the translation amount in the theoretical pose. Then, a rotation matrix is ​​constructed sequentially around the X, Y, and Z axes based on the rotation amount. Finally, the position vector and the rotation matrix are multiplied by the sum of the fixed coordinates of the upper platform hinge point to obtain the transformed coordinates. This method strictly follows the RPY rotation order in the fixed coordinate system for coordinate transformation, avoiding calculation deviations caused by incorrect rotation order or coordinate confusion. It ensures the accuracy of the position of the upper hinge point in the lower platform coordinate system, thereby improving the accuracy of the inverse kinematics solution and laying a reliable data foundation for error compensation.

[0011] Preferably, in step S23, the first theoretical displacement... The calculation methods include: S231. Calculate the coordinates of each hinge point of the lower platform in the lower platform coordinate system. With the transformed coordinates Spatial vectors between ; S232, Calculate the space vector The unit vector of the linear module's motion direction The dot product of the two vectors yields the spatial vector. Projected length in the direction of motion; S233, according to the length of the connecting rod The Pythagorean theorem is used to calculate the length component perpendicular to the direction of motion. The projected length is then subtracted from the length component perpendicular to the direction of motion to obtain the first theoretical displacement. By calculating the spatial vector between the lower platform hinge point and the transformed coordinates, the projected length of this vector in the linear module's motion direction is obtained. Then, combining the link length with the Pythagorean theorem, the component length perpendicular to the motion direction is obtained. Finally, the theoretical displacement is obtained by subtracting this component length from the projected length. This method fully considers the spatial geometric constraints of the parallel mechanism and the invariant link length characteristic, enabling accurate calculation of the required driving displacement for each branch. It avoids calculation errors caused by simplified models, thus significantly improving the accuracy of inverse kinematics and facilitating high-precision pose control.

[0012] Preferably, in step S3, the Jacobian matrix is ​​obtained based on the kinematic forward solution model. The method is to obtain the first actual displacement. Calculate the first theoretical displacement. With the first actual displacement difference For the first theoretical displacement With the first actual displacement difference For each component in the equation, calculate the partial derivative with respect to the pose parameters of the upper platform, including translations along the X, Y, and Z axes and rotations about the X, Y, and Z axes. Construct the partial derivatives into a Jacobian matrix. The difference between the theoretical and actual displacements of each branch is used to construct a residual vector. The partial derivatives of each component of the residual vector with respect to the pose parameters of the upper platform are then calculated, and all partial derivatives are arranged into a six-row, six-column Jacobian matrix. This Jacobian matrix directly reflects the linearized mapping relationship between the driving displacement error and the pose error of the upper platform, providing a core computational tool for subsequent iterative solutions to the actual pose using the Newton-Raphson method. This effectively accelerates the convergence speed of the forward kinematics solution and improves the real-time performance and accuracy of pose error compensation.

[0013] Preferably, in step S7, based on the Jacobian matrix... Calculate pose error The method is to calculate the Jacobian matrix. inverse matrix The negative second theoretical displacement With the second actual displacement difference Left-multiply the inverse matrix The pose error is obtained. By using the inverse of the Jacobian matrix to convert the driving displacement difference into the pose correction of the upper platform, the deviation between the current iterative pose and the true pose can be calculated quickly and stably, thereby realizing closed-loop correction of dynamic errors during motion.

[0014] The present invention also provides a parallel robot positioning bed control system, including a lower platform, an upper platform, and a branch assembly connecting the upper platform and the lower platform, the branch assembly including an independently driven linear module, characterized in that it further includes a controller configured to execute the parallel robot positioning bed control method as described above.

[0015] The present invention also provides a storage medium storing a computer program thereon, characterized in that the computer program, when executed by a processor, implements the parallel robot positioning bed control method as described above.

[0016] Compared with the prior art, the beneficial effects of the present invention are: 1. By calculating the total error parameters, the theoretical pose of the target is systematically compensated, which can effectively eliminate the static geometric errors introduced by the manufacturing and assembly of the mechanism, thereby achieving higher precision positioning in subsequent motion control and meeting the stringent requirements of radiotherapy for positioning accuracy. 2. An inverse kinematics model was constructed, which can accurately calculate the displacement of the linear module based on the compensated target pose, ensuring the smoothness and consistency of the upper platform in six-degree-of-freedom motion in space. 3. The actual displacement of the linear module is collected, and the forward kinematics solution is obtained by Newton-Raphson iteration method to form a closed-loop correction. This can compensate for dynamic errors such as environmental disturbances, friction, and elastic deformation in real time, which significantly improves the positioning reliability and trajectory tracking accuracy of the system under long-term operation or load changes. Attached Figure Description

[0017] Figure 1 A flowchart of a control method for a parallel robot positioning bed; Figure 2 A flowchart for constructing the inverse kinematics model; Figure 3 For coordinate transformation A flowchart of the calculation method; Figure 4 The first theoretical displacement The flowchart of the calculation method. Detailed Implementation

[0018] The present invention will be further described below with reference to specific embodiments.

[0019] Example 1 This embodiment is the first embodiment of the parallel robot positioning bed control method, such as... Figure 1 As shown, it includes the following steps: S1, Input the theoretical pose of the target , ( In this embodiment, n is set to 20, and the actual pose of the platform under the theoretical pose of the target is measured using third-party testing equipment such as a laser tracker. Calculate the pose error vector between the actual pose and the target theoretical pose. :

[0020] in, These represent the translation errors of the upper platform along the X, Y, and Z axes of the coordinate system of the lower platform, respectively. These represent the rotational errors of the upper platform about the X, Y, and Z axes of the coordinate system of the lower platform, respectively. S2. Construct an inverse kinematics model, and calculate the first theoretical displacement of each linear module based on the inverse kinematics model. ; S3. Construct a forward kinematics model and obtain the Jacobian matrix based on the forward kinematics model. ; S4. Based on the pose error vector E and the Jacobian matrix Construct the matrix equation:

[0021]

[0022]

[0023] Therefore, the error parameters can be calculated:

[0024] in, For the residual vector, The coordinate error of the i-th hinge point on the lower platform along the X-axis direction. The coordinate error of the i-th hinge point on the lower platform along the Y-axis direction. The coordinate error of the i-th hinge point on the lower platform along the Z-axis direction. The coordinate error of the i-th hinge point on the upper platform along the X-axis in the upper platform coordinate system. The coordinate error of the i-th hinge point on the upper platform along the Y-axis in the upper platform coordinate system. The coordinate error of the i-th hinge point on the upper platform along the Z-axis in the coordinate system of the upper platform. The component error of the unit vector of the motion direction of the i-th linear module in the X-axis direction. The component error of the unit vector of motion direction of the i-th linear module in the Y direction. The component error of the unit vector of motion direction of the i-th linear module in the Z direction. The length error of the i-th link; S5. The geometric error parameters are fitted using the least squares method to obtain the total error parameters, which are used to calibrate the mechanical structure of the parallel robot positioning bed. S6. Obtain the initial iterative pose. The second actual displacement of each linear module The initial iterative pose Input the inverse kinematics model and calculate the second theoretical displacement corresponding to each linear module. and the second theoretical displacement With the second actual displacement difference If the second theoretical displacement With the second actual displacement difference Less than the first threshold Then output the initial iterative pose. If the target pose is determined, proceed to step S7; S7, the second theoretical displacement Input the forward kinematics model and obtain the Jacobian matrix. According to the Jacobian matrix Calculate pose error If the pose error Less than the second threshold Then output the initial iterative pose. As the target pose, otherwise for the initial iterative pose Perform the update and repeat step S6.

[0025] The parallel robot positioning bed control method of the present invention first obtains the error matrix by inputting the theoretical pose and measuring the actual pose. Then, it calculates the theoretical displacement of each linear module based on the inverse kinematics model and constructs the Jacobian matrix using the forward kinematics model. Subsequently, it establishes the residual vector equation to solve for the geometric error parameters and obtains the total error parameters through fitting to calibrate and compensate for the assembly geometric errors of the mechanical structure of the parallel robot positioning bed. Then, in the motion control stage, iterative judgment is made by the deviation between the initial iterative pose and the measured displacement. The displacement error and pose error are calculated using the inverse and forward kinematics models until the error is less than the threshold, thereby realizing closed-loop correction of the dynamic error in the motion process. The present invention combines static calibration and dynamic correction, which significantly improves the positioning accuracy of the radial parallel robot positioning bed.

[0026] In step S1 of this embodiment, the error matrix The calculation method is to calculate the actual pose. The components and theoretical pose The difference between the corresponding components yields a six-dimensional error matrix, which includes translation errors along the X, Y, and Z axes, as well as rotation errors around the X, Y, and Z axes. By calculating the difference between each component of the actual pose and the theoretical pose, a six-dimensional error vector containing three translation errors and three rotation errors is obtained. This vector can comprehensively and accurately quantify the positioning deviation of the parallel robot positioning bed in six degrees of freedom in space, providing a complete error input for subsequent error parameter identification and avoiding the problem of compensation omission or miscompensation caused by incomplete error representation.

[0027] like Figure 2 As shown, the method for constructing the inverse kinematics model in this embodiment includes the following steps: S21. Obtain the coordinates of each hinge point on the lower platform in the lower platform coordinate system. The coordinates of each hinge point on the upper platform in the upper platform coordinate system Link length and the unit vector of the linear module's motion direction ; S22. Based on the theoretical pose of the upper platform By using rotation and translation transformations, the coordinates of each hinge point on the upper platform in the upper platform coordinate system are... Transform to the lower platform coordinate system to obtain the transformed coordinates. ; S23. For each branch, based on the coordinates of each hinge point on the lower platform in the lower platform coordinate system... , coordinate transformation and the unit vector of the linear module's motion direction The first theoretical displacement corresponding to this branch is calculated through spatial geometric relationships. By acquiring the coordinates of the lower platform hinge point, the fixed coordinates of the upper platform hinge point, the link length, and the unit vector of the linear module's motion direction, and using rotation and translation transformations to convert the upper platform hinge point to the lower platform coordinate system, and then calculating the theoretical displacement of each branch based on spatial geometric relationships, a complete inverse kinematic model is constructed. This model can accurately establish the mapping relationship between the target pose of the upper platform and the displacement of each drive module, providing a precise theoretical benchmark for subsequent error calibration and closed-loop control, effectively improving the positioning and control accuracy of the parallel robot positioning bed.

[0028] Example 2 This embodiment is the second embodiment of the parallel robot positioning bed control method. Similar to the first embodiment, the difference lies in that, in step S21, the unit vector of the linear module's motion direction is determined by a predefined unit vector matrix. Each column of the unit vector matrix corresponds to the direction component of a linear module in a Cartesian coordinate system, and the motion directions of all linear modules are located in the horizontal plane. Using a predefined unit vector matrix to determine the motion direction of each linear module, and ensuring that all motion directions are within the horizontal plane, simplifies the input and storage of direction vectors in the inverse kinematics solution. The motion layout within the horizontal plane makes the geometric relationship between the linear modules and the links more regular, which is beneficial for improving the stability and computational efficiency of the inverse kinematics model.

[0029] In this embodiment, six linear modules are set up. Knowing the motion directions of the six linear modules, their unit vector matrices can be obtained. :

[0030] in, This represents the unit vector indicating the direction of motion of the i-th linear module.

[0031] like Figure 3 As shown, in step S22 of this embodiment, the coordinates are transformed. The calculation method includes the following steps: S221. Based on the theoretical pose of the upper platform The translation amount determines the position of the origin of the upper platform in the coordinate system of the lower platform. ; S222, Based on the theoretical pose of the upper platform The rotation amounts in the figure are used to construct rotation matrices around the X, Y, and Z axes of the lower platform coordinate system in sequence. ; S223. For each hinge point, calculate the position of the origin of the upper platform in the coordinate system of the lower platform. Coordinates of each hinge point on the upper platform in the upper platform coordinate system With rotation matrix The sum of the products yields the transformed coordinates. The position vector of the upper platform origin in the lower platform coordinate system is determined by the translation amount in the theoretical pose. Then, a rotation matrix is ​​constructed sequentially around the X, Y, and Z axes based on the rotation amount. Finally, the position vector and the rotation matrix are multiplied by the sum of the fixed coordinates of the upper platform hinge point to obtain the transformed coordinates. This method strictly follows the RPY rotation order in the fixed coordinate system for coordinate transformation, avoiding calculation deviations caused by incorrect rotation order or coordinate confusion. It ensures the accuracy of the position of the upper hinge point in the lower platform coordinate system, thereby improving the accuracy of the inverse kinematics solution and laying a reliable data foundation for error compensation.

[0032] After performing a rotation transformation using a rotation matrix constructed in RPY order, it is then added to the translation vector of the origin of the upper platform coordinate system in the lower platform coordinate system.

[0033]

[0034] in, This indicates the position of the origin of the upper platform coordinate system P-XYZ in the lower platform coordinate system B-XYZ. This indicates the starting position of the linear module's motion on the upper platform. Let represent the rotation transformation matrix. This formula can be used to transform the coordinates of the upper hinge point in the upper platform coordinate system {P} to the lower platform coordinate system {B}.

[0035] like Figure 4 As shown, in step S23 of this embodiment, the first theoretical displacement... The calculation methods include: S231. Calculate the coordinates of the hinge point of the lower platform in the lower platform coordinate system. With transformed coordinates Spatial vectors between ; S232, Calculate spatial vectors Unit vector of linear module motion direction The dot product of the two vectors yields the spatial vector. Projected length in the direction of motion; S233, Based on the length of the connecting rod The Pythagorean theorem is used to calculate the length component perpendicular to the direction of motion. Subtracting this length component from the projected length yields the first theoretical displacement. ,Right now:

[0036]

[0037]

[0038] in, This indicates the angle between the directions of motion of the connecting rod and the linear module.

[0039] By calculating the spatial vector between the lower platform hinge point and the transformed upper platform hinge point, the projected length of this vector in the linear module's motion direction is obtained. Then, combining the link length with the Pythagorean theorem, the component length perpendicular to the motion direction is obtained. Finally, the theoretical displacement is obtained by subtracting this component length from the projected length. This method fully considers the spatial geometric constraints of the parallel mechanism and the invariant link length characteristic, enabling accurate calculation of the required driving displacement for each branch. It avoids calculation errors caused by simplified models, thus significantly improving the accuracy of inverse kinematics and facilitating high-precision pose control.

[0040] Example 3 This embodiment is the third embodiment of the parallel robot positioning bed control method. This embodiment is similar to the first embodiment, except that in step S3, the Jacobian matrix is ​​obtained based on the forward kinematics model. The method is to obtain the first actual displacement. Calculate the first theoretical displacement. With the first actual displacement difference :

[0041] For the first theoretical displacement With the first actual displacement difference For each component in the equation, calculate the partial derivative with respect to the pose parameters of the upper platform, including translations along the X, Y, and Z axes and rotations about the X, Y, and Z axes. Construct the partial derivatives into a Jacobian matrix. :

[0042] The difference between the theoretical and actual displacements of each branch is used to construct a residual vector. The partial derivatives of each component of the residual vector with respect to the pose parameters of the upper platform are then calculated, and all partial derivatives are arranged into a 6x6 Jacobian matrix. This Jacobian matrix directly reflects the linearized mapping relationship between the driving displacement error and the pose error of the upper platform, providing a core computational tool for subsequent iterative solutions to the actual pose using the Newton-Raphson method. This effectively accelerates the convergence speed of the forward kinematics solution and improves the real-time performance and accuracy of pose error compensation.

[0043] In step S7, based on the Jacobian matrix... Calculate pose error The method is to calculate the Jacobian matrix. inverse matrix The negative second theoretical displacement With the second actual displacement difference Left multiplication by the inverse matrix The pose error is obtained. ,Right now:

[0044] By using the inverse of the Jacobian matrix to convert the driving displacement difference into the pose correction amount of the upper platform, the deviation between the current iterative pose and the true pose can be calculated quickly and stably, thereby realizing closed-loop correction of dynamic errors during motion.

[0045] In step S7, the initial iterative pose is updated by superimposing the current iterative pose with the pose increment to obtain the updated iterative pose. Superimposing the current iterative pose with the pose increment yields the updated iterative pose, achieving successive approximation from the initial estimate to the true pose. This update strategy is simple and efficient, requiring no additional parameter adjustments, and ensuring the stability and monotonically convergent nature of the iterative process.

[0046] Example 4 This embodiment is an example of a parallel robot positioning bed control system, including a lower platform, an upper platform, and a branch assembly connecting the upper platform and the lower platform. The branch assembly includes an independently driven linear module and a controller configured to execute a parallel robot positioning bed control method.

[0047] Example 5 This embodiment is an example of a storage medium storing a computer program. When the computer program is executed by a processor, it implements a parallel robot positioning bed control method.

[0048] In the specific implementation of the above embodiments, the technical features can be combined in any non-contradictory way. For the sake of brevity, not all possible combinations of the above technical features are described. However, as long as the combination of these technical features is not contradictory, it should be considered to be within the scope of this specification.

[0049] Obviously, the above embodiments of the present invention are merely examples for clearly illustrating the present invention, and are not intended to limit the implementation of the present invention. Those skilled in the art can make other variations or modifications based on the above description. It is neither necessary nor possible to exhaustively describe all embodiments here. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the scope of protection of the claims of the present invention.

Claims

1. A control method for a parallel robot positioning bed, characterized in that, Includes the following steps: S1. Input the theoretical pose of the upper platform. And the actual pose of the platform is measured using testing equipment. Calculate the actual pose Theoretical pose Error matrix between ; S2. Construct an inverse kinematics model, and calculate the first theoretical displacement corresponding to each linear module based on the inverse kinematics model. ; S3. Construct a forward kinematics model, and obtain the Jacobian matrix based on the forward kinematics model. ; S4. Based on the error matrix With the Jacobian matrix Establish the residual vector equation and calculate the residual vector. , the residual vector Decompose into geometric error parameters; S5. Fit the geometric error parameters to obtain the total error parameters, which are used to calibrate the mechanical structure of the parallel robot positioning bed; S6. Obtain the initial iterative pose. The second actual displacement of each linear module The initial iterative pose Input the inverse kinematics model and calculate the second theoretical displacement corresponding to each linear module. and the second theoretical displacement With the second actual displacement difference If the second theoretical displacement With the second actual displacement difference Less than the first threshold Then output the initial iterative pose. If the target pose is determined, proceed to step S7; S7, the second theoretical displacement amount Input the forward kinematics model and obtain the Jacobian matrix. According to the Jacobian matrix Calculate pose error If the pose error Less than the second threshold Then output the initial iterative pose. As the target pose, otherwise for the initial iterative pose Perform the update and repeat step S6.

2. The parallel robot positioning bed control method according to claim 1, characterized in that, In step S1, the error matrix The calculation method is to calculate the actual pose. The components and the theoretical pose The difference between the corresponding components yields a six-dimensional error matrix, which includes translation errors along the X, Y, and Z axes, as well as rotation errors around the X, Y, and Z axes. .

3. The parallel robot positioning bed control method according to claim 1, characterized in that, In step S2, the method for constructing the inverse kinematics model includes the following steps: S21. Obtain the coordinates of each hinge point on the lower platform in the lower platform coordinate system. The coordinates of each hinge point on the upper platform in the upper platform coordinate system Link length and the unit vector of the linear module's motion direction ; S22. Based on the theoretical pose of the upper platform The coordinates of each hinge point on the upper platform in the upper platform coordinate system are obtained by using rotation and translation transformations. Transform to the lower platform coordinate system to obtain the transformed coordinates. ; S23. For each branch, based on the coordinates of each hinge point of the lower platform in the lower platform coordinate system... , coordinate transformation and the unit vector of the linear module's motion direction The first theoretical displacement corresponding to this branch is calculated through spatial geometric relationships. .

4. The parallel robot positioning bed control method according to claim 3, characterized in that, In step S21, the unit vector of the linear module's motion direction The direction of motion of the linear module is determined by a predefined unit vector matrix, each column of which corresponds to a direction component of the linear module in a spatial rectangular coordinate system, and the direction of motion of the linear module is located in the horizontal plane.

5. The parallel robot positioning bed control method according to claim 3, characterized in that, In step S22, the transformed coordinates The calculation method includes the following steps: S221, Based on the theoretical pose of the upper platform The translation amount determines the position of the origin of the upper platform in the coordinate system of the lower platform. ; S222, Based on the theoretical pose of the upper platform The rotation amounts in the matrix are constructed sequentially around the X, Y, and Z axes of the lower platform coordinate system to create a rotation matrix. ; S223. For each hinge point, calculate the position of the upper platform origin in the lower platform coordinate system. The coordinates of each hinge point of the upper platform in the upper platform coordinate system With the rotation matrix The sum of the products yields the transformed coordinates. .

6. The parallel robot positioning bed control method according to claim 3, characterized in that, In step S23, the first theoretical displacement The calculation methods include: S231. Calculate the coordinates of each hinge point of the lower platform in the lower platform coordinate system. With the transformed coordinates Space vectors between ; S232, Calculate the space vector The unit vector of the linear module's motion direction The dot product of the two vectors yields the spatial vector. Projected length in the direction of motion; S233, according to the length of the connecting rod The Pythagorean theorem is used to calculate the length component perpendicular to the direction of motion. The projected length is then subtracted from the length component perpendicular to the direction of motion to obtain the first theoretical displacement. .

7. The parallel robot positioning bed control method according to claim 1, characterized in that, In step S3, the Jacobian matrix is ​​obtained based on the kinematic forward solution model. The method is to obtain the first actual displacement. Calculate the first theoretical displacement. With the first actual displacement difference For the first theoretical displacement With the first actual displacement difference For each component in the equation, calculate the partial derivative with respect to the pose parameters of the upper platform, including translations along the X, Y, and Z axes and rotations about the X, Y, and Z axes. Construct the partial derivatives into a Jacobian matrix. .

8. The parallel robot positioning bed control method according to claim 1, characterized in that, In step S7, based on the Jacobian matrix... Calculate pose error The method is to calculate the Jacobian matrix. inverse matrix The negative second theoretical displacement With the second actual displacement difference Left-multiply the inverse matrix The pose error is obtained. .

9. A parallel robot positioning bed control system, comprising a lower platform, an upper platform, and a branch assembly connecting the upper platform and the lower platform, the branch assembly comprising independently driven linear modules, characterized in that, It also includes a controller configured to perform the parallel robot positioning bed control method as described in any one of claims 1 to 8.

10. A storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the parallel robot positioning bed control method as described in any one of claims 1 to 8.

Images