A c-axis rotation angle smoothing correction method for a five-degree-of-freedom hybrid robot

Abstract

The application discloses a C-axis rotation angle smoothing correction method for a five-degree-of-freedom hybrid robot, which comprises the following steps: firstly, calculating a singular axis direction vector, and judging whether singularity occurs by the included angle between the tool axis direction vector and the singular axis vector; if the robot is in a singular state, calculating a singular factor, and adopting a correction strategy based on a weighted trigonometric function to adaptively and smoothly correct the C-axis rotation angle of an A / C rotary head, wherein the composite correction factor and the trigonometric function smoothing factor are dynamically adjusted according to the singular factor; after the correction, synchronously updating the tool axis direction vector, and iteratively processing all tool path points. The method takes the C-axis rotation angle correction as the core, combines the dynamic change of the singular axis, quantifies the singular degree through the singular factor, realizes the smooth correction, ensures the smooth movement of the robot in the singular region, and effectively improves the machining precision.

Description

Technical Field

[0001] This invention relates to the fields of robotics and automation control, specifically to a method for smoothing the C-axis rotation angle of a five-degree-of-freedom hybrid robot. Background Technology

[0002] Traditional five-DOF hybrid robots equipped with A / C rotary heads are prone to motion singularities during machining. When the tool axis direction vector coincides with the singular axis direction vector, instantaneous abrupt changes occur in the robot's drive joint variables. This not only causes nonlinear deviations on the machined surface but can also damage the tool and workpiece in severe cases, affecting machining quality and equipment safety. Currently, there are three main solutions to this singularity problem in five-axis machining equipment: correcting joint commands, correcting toolpaths, and adjusting workpiece mounting posture.

[0003] Unlike traditional five-axis CNC machine tools with decoupled translation and rotation, the singular axis direction of a five-DOF hybrid robot changes dynamically with its own configuration. This presents two major challenges for existing technologies in tool trajectory planning: first, accurately capturing the region where singular phenomena occur and clarifying the change law of singular state with tool pose; second, combining the dynamic change characteristics of singular axes and adopting reasonable correction strategies to avoid singularities and achieve smooth transition of joint movements.

[0004] Chinese patent CN116330251A discloses "A method for motion singularity correction of a five-DOF hybrid robot." The method first calculates the singular axis vector and determines whether a singularity has occurred by checking if the angle between the tool axis vector and the singular axis vector is less than a threshold. Then, a correction algorithm is used to correct the C-axis rotation angle of the head, thereby correcting the tool axis vector. Finally, singularity detection and correction are performed on all interpolation points. It features online singularity detection and real-time correction. However, this method is too simplistic in correcting the C-axis rotation angle and cannot adaptively adjust the correction amount according to the degree of singularity. Summary of the Invention

[0005] The purpose of this invention is to overcome the shortcomings of the prior art and provide a method for smoothing the C-axis rotation angle of a five-degree-of-freedom hybrid robot. This method solves the technical problem of abrupt changes in the C-axis rotation angle and large machining errors caused by motion singularities in the prior art. It achieves a smooth transition of the C-axis rotation angle in singular regions, thereby improving the smoothness of robot motion and machining accuracy.

[0006] The present invention provides a method for smoothing the C-axis rotation angle of a five-degree-of-freedom hybrid robot, comprising the following steps: Step S1: Calculate the singular axis direction vector ; Step S2: Determine if the robot has encountered any anomalies. The specific steps are as follows: Calculate the tool axis direction vector and the singular axis direction vector obtained in step S1. The angle between Then, the included angle is compared with a preset singularity threshold. The comparison and determination formula are as follows: ; In the formula: This is the transpose of the tool axis direction vector at the current toolpath point after interpolation; If the above conditions are met, it indicates that the robot is in a singular motion state. In this case, the singularity factor is calculated using the following formula. Then proceed to step S3; otherwise, if the robot does not experience a motion singularity, no corrective action is performed.

[0007] in, The preset singularity threshold is typically set to 0.0001 rad. Step S3: Smoothly correct the C-axis rotation angle of the A / C head using a weighted trigonometric function-based correction method. The specific process is as follows: Step S31: Based on the tool axis direction vector and tool tip vector corresponding to the current interpolation point, solve the robot's joint variables using the robot position inverse algorithm. , , , , ;in, , , These refer to the lengths of the first, second, and third active length adjustment devices of the robot. The rotation angle of turntable 61 in the A / C rotary head around the C-axis. The rotation angle of the oscillating head 62 in the A / C swivel head around the A-axis; Step S32, for The correction is performed, and the correction algorithm is as follows: when hour, ; , , ; when hour, ; in, —Indicates the position of the current toolpath point. Value using the above The rotation angle of the turntable 61 in the A / C head after the correction algorithm is applied around the C-axis. —Indicates the position of the previous toolpath point. Value using the above The algorithm has corrected the rotation angle of the turntable around the C-axis in the A / C head; It is a composite correction factor; It is the smoothing factor for trigonometric functions; The trigonometric function scaling factor; Preset thresholds for singular factors; The preset smoothing coefficient; Step S4: Calculate the tool axis direction vector after C-axis rotation correction; Step S5: Repeat steps S1-S4 until the C-axis rotation angle of the A / C head corresponding to all toolpath points after interpolation is completely corrected.

[0008] The present invention has the following beneficial effects: Using the C-axis rotation angle as the core correction, combined with the dynamic change characteristics of the singular axis, the degree of singularity is quantified by the singularity factor, and a weighted trigonometric function correction strategy is adopted to achieve adaptive smooth correction of the C-axis rotation angle. The tool axis direction vector is updated synchronously to ensure that the robot moves smoothly in the singular region and effectively avoids the machining deviation caused by joint mutation. Attached Figure Description

[0009] Figure 1 This is a structural schematic diagram of a five-degree-of-freedom hybrid robot from the first angle; Figure 2 This is a structural diagram of a five-degree-of-freedom hybrid robot from the second angle; Figure 3 This is a schematic diagram of the structural parameters of the A / C rotating head of a five-degree-of-freedom hybrid robot; Figure 4 This is a schematic diagram of the singularity detection and correction process; In the picture: 1. Driven length adjustment device; 2. First active length adjustment device; 3. Second active length adjustment device; 4. Third active length adjustment device; 5. Moving platform; 6. A / C rotary head. 11. First fixed shaft seat; 21. Second fixed shaft seat. 12. First rotating bracket; 22. Second rotating bracket; 23. First servo motor; 33. Second servo motor; 43. Third servo motor. 24. First ball joint; 34. Second ball joint; 44. Third ball joint. 15. Fourth revolute joint; 25. Hooke's joint; 35. Second revolute joint; 45. Third revolute joint; 55. First revolute joint; 61. Turntable; 62. Swing head; 65. Fifth revolute joint. J, the central axis of the tool mounted at the end of the A / C rotary head; C, the C-axis of the two-degree-of-freedom cascaded mechanism; A, the A-axis of the two-degree-of-freedom cascaded mechanism. C-axis rotation angle A-axis rotation angle; A plane passing through axis A and perpendicular to the central axis J of the tool; The effective distance along the central axis J of the tool in the A / C rotary head; ,flat The intersection with the central axis J of the tool, , the tip of the knife The intersection of axis A and axis C; B-xyz, the robot's reference coordinate system. Detailed Implementation

[0010] The following detailed description of the C-axis rotation smoothing correction method for a five-degree-of-freedom hybrid robot, in conjunction with the accompanying drawings and specific embodiments, is provided below.

[0011] The method of this invention is applicable to the existing C-axis rotation smoothing correction method for five-degree-of-freedom hybrid robots.

[0012] The structure of the aforementioned five-degree-of-freedom hybrid robot can be found in Chinese Patent Publication No. CN104985596A, published on October 21, 2015, entitled "A Five-Degree-of-Freedom Hybrid Robot with Multiple Axes". Its specific structure is as follows: The robot consists of a parallel mechanism with two translational and two rotational degrees of freedom and a series-connected two-degree-of-freedom A / C rotating head 6, wherein the A / C rotating head 6 is connected in series with the moving platform 5.

[0013] The two-degree-of-freedom A / C rotary head is a two-degree-of-freedom series structure including two rotary joints, wherein the two rotary joints are formed by the turntable 61 around the C-axis of the two-degree-of-freedom series mechanism and the swing head 62 around the A-axis of the two-degree-of-freedom series mechanism.

[0014] The three-degree-of-freedom parallel mechanism includes a first rotating bracket 12, the two sides of which are rotatably connected to two first fixed bearings 11 mounted on the frame via first rotating joints 55; the two sides of a second rotating bracket 22 are rotatably connected to two second fixed bearings 21 mounted on the frame via fifth rotating joints 65; the upper parts of the second active length adjusting device 3 and the third active length adjusting device 4 are symmetrically rotatably connected to the two sides of the first rotating bracket 12 via second rotating joints 35 and third rotating joints 45, respectively, and their bottom ends are rotatably connected to the moving platform 5 via second ball joints 34 and third ball joints 44, respectively; the driven length adjusting device 1 is rotatably connected to the first rotating bracket 12 via a fourth rotating joint 15, and the bottom of the driven length adjusting device 1 is fixedly connected to the moving platform 5; the upper part of the first active length adjusting device 2 is rotatably connected to two second fixed bearings 21 mounted on the frame via a Hooke joint 25, and its bottom end is rotatably connected to the moving platform 5 via a first ball joint 24. The first active length adjustment device 2, the second active length adjustment device 3, and the third active length adjustment device 4 are driven by the first servo motor 23, the second servo motor 33, and the third servo motor 43, respectively, and can realize axial telescopic movement, thereby driving the moving platform 5 to rotate around the axis of the first rotary joint 55 and the axis of the fourth rotary joint 15, and can move along the axis of the driven length adjustment device 1, forming three degrees of freedom of one translation and two rotations; the rotation axes of the second rotary joint 35, the third rotary joint 45, and the fourth rotary joint 15 are parallel to each other, and all are perpendicular to the rotation axis of the first rotary joint 55.

[0015] Based on the above robot structure, the present invention provides a method for smoothing the C-axis rotation angle of a five-degree-of-freedom hybrid robot, comprising the following steps: Step S1: Calculate the singular axis direction vector; The tool movement trajectory mounted at the end of the A / C rotary head is interpolated using an interpolation method to obtain several toolpath points. Based on these interpolated toolpath points, the singular axis direction vector is calculated. These toolpath points include both the tool axis direction vector and the tool tip vector. The calculation formula is as follows: ; in, This represents the singular axis direction vector obtained at the current toolpath point after interpolation; This represents the tool tip vector at the current toolpath point after interpolation; , , The tool tip of the tool mounted at the end effector of the robot is located in the reference coordinate system B-xyz. axis, axis, Axis coordinates; the reference coordinate system B-xyz for the supporting robot. The shaft coincides with the axis of the first rotating bracket 12. The axis is perpendicular to the axis of the first rotating bracket 12. The axis satisfies the right-hand rule; This represents the tool axis direction vector at the current toolpath point after interpolation; This represents the effective distance along the central axis of the end-effector in the A / C rotary head. This effective distance is defined as: the distance along the plane passing through the A-axis of the two-degree-of-freedom cascaded mechanism of the A / C rotary head and perpendicular to the central axis J of the tool. In the middle, the central axis J of the tool is perpendicular to the plane. intersection With the tip of the knife The distance between them.

[0016] Step S2: Determine if the robot has encountered any anomalies. The specific steps are as follows: Calculate the tool axis direction vector and the singular axis direction vector obtained in step S1. The angle between Then, the included angle is compared with a preset singularity threshold. The comparison and determination formula are as follows: ; In the formula: This is the transpose of the tool axis direction vector at the current toolpath point after interpolation; If the above conditions are met, it indicates that the robot is in a singular motion state. In this case, the singularity factor is calculated using the following formula. Then proceed to step S3; otherwise, if the robot does not experience a motion singularity, no corrective action is performed.

[0017] in, The preset singularity threshold is typically set to 0.0001 rad. Step S3: If the judgment condition is met, it is determined that a motion singularity has occurred in the robot. A correction method based on weighted trigonometric functions is used to smoothly correct the C-axis rotation angle of the A / C head. The specific process is as follows: Step S31: Based on the tool axis direction vector and tool tip vector corresponding to the current interpolation point, solve for the robot's joint variables using the robot position inverse algorithm (see "Parallel Robots" pp. 48-70, Machinery Industry Press, May 24, 2021, ISBN: 9787111675884 for details). , , , , .in, , , These refer to the lengths of the first, second, and third active length adjustment devices of the robot. The rotation angle of the turntable 61 in the A / C turn head around the C-axis (i.e., the C-axis rotation angle to be corrected). The rotation angle of the oscillating head 62 in the A / C swivel head around the A-axis; Step S32, for The correction is performed, and the correction algorithm is as follows: when hour, ; , , ; when hour, ; in, —Indicates the position of the current toolpath point. Value using the above The rotation angle of the turntable 61 in the A / C head after the correction algorithm is applied around the C-axis. —Indicates the position of the previous toolpath point. Value using the above The correction algorithm has corrected the rotation angle of turntable 61 in the A / C head around the C-axis. It is a composite correction factor; It is a trigonometric function smoothing factor used to achieve infinite order continuity of the C-axis rotation angle; It is a trigonometric function scaling factor, and the weights can be dynamically adjusted and corrected. To pre-determine a threshold for singularity factors, the robot needs to ensure that, during operation on a given trajectory, all toolpath points after interpolation on that trajectory have different... The values ​​are simulated and analyzed to determine the optimal value that makes the robot move smoothly on the path. In one embodiment, the value is 0.6. To preset the smoothing coefficient, the robot needs to ensure that all toolpath points after interpolation on a certain trajectory are smoothed during its operation. The values ​​are used for simulation analysis, and the values ​​that make the robot move smoothly on the path are selected. In one embodiment, 0.01 is selected. Step S4: Calculate the tool axis direction vector after C-axis rotation correction. The specific process is as follows: The solution obtained using step S3 And the rotation angle of the oscillating head 62 in the A / C swivel head around the A-axis. Preserve the tool tip vector at the current toolpath point after interpolation. Without changing the current toolpath point, calculate the corrected tool axis direction vector. For specific calculation methods, please refer to the calculation method disclosed in the application date of 2023-04-09, publication number CN116330251A, invention title "A method for motion singularity correction of a five-degree-of-freedom hybrid robot"; Step S5: Repeat steps S1-S4 until the C-axis rotation angle of the A / C head corresponding to all toolpath points after interpolation is completely corrected.

[0018] In this embodiment, the above-mentioned correction method can achieve a smooth transition of the C-axis rotation angle of the five-degree-of-freedom hybrid robot, effectively avoid joint mutations caused by singular states, and directly embed the inverse kinematics algorithm of the robot motion controller to realize online real-time judgment of singular states and correction of tool path points after interpolation, effectively improving the smoothness of robot motion and machining accuracy, and adapting to the actual needs of industrial processing scenarios.

[0019] The present invention and its embodiments have been described above illustratively. This description is not restrictive, and the figures shown are only one embodiment of the present invention; the actual structure is not limited thereto. Therefore, if those skilled in the art are inspired by this description and, without departing from the spirit of the invention, devise structural methods and embodiments similar to this technical solution in terms of component shape and connection method without creative design, all such embodiments should fall within the protection scope of the present invention.

Claims

1. A method for smoothing the C-axis rotation angle of a five-DOF hybrid robot, characterized in that... Includes the following steps: Step S1: Calculate the singular axis direction vector ; Step S2: Determine if the robot has encountered any anomalies. The specific steps are as follows: Calculate the tool axis direction vector and the singular axis direction vector obtained in step S1. The angle between Then, the included angle is compared with a preset singularity threshold. The comparison and determination formula are as follows: ； In the formula: This is the transpose of the tool axis direction vector at the current toolpath point after interpolation; If the above conditions are met, it indicates that the robot is in a singular motion state. In this case, the singularity factor is calculated using the following formula. Then proceed to step S3; otherwise, if the robot does not experience a motion singularity, no corrective action is performed. in, The preset singularity threshold is typically set to 0.0001 rad. Step S3: Smoothly correct the C-axis rotation angle of the A / C head using a weighted trigonometric function-based correction method. The specific process is as follows: Step S31: Based on the tool axis direction vector and tool tip vector corresponding to the current interpolation point, solve the robot's joint variables using the robot position inverse algorithm. , , , , ;in, , , These refer to the lengths of the first, second, and third active length adjustment devices of the robot. The rotation angle of turntable 61 in the A / C rotary head around the C-axis. The rotation angle of the oscillating head 62 in the A / C swivel head around the A-axis; Step S32, for The correction is performed, and the correction algorithm is as follows: when hour, ； ， ， ； when hour, ; in, —Indicates the position of the current toolpath point. Value using the above The rotation angle of the turntable 61 in the A / C head after the correction algorithm is applied around the C-axis. —Indicates the position of the previous toolpath point. Value using the above The algorithm has corrected the rotation angle of the turntable around the C-axis in the A / C head; It is a composite correction factor; It is the smoothing factor for trigonometric functions; The trigonometric function scaling factor; Preset thresholds for singular factors; The preset smoothing coefficient; Step S4: Calculate the tool axis direction vector after C-axis rotation correction; Step S5: Repeat steps S1-S4 until the C-axis rotation angle of the A / C head corresponding to all toolpath points after interpolation is completely corrected.

2. The method for smoothing the C-axis rotation angle of a five-degree-of-freedom hybrid robot according to claim 1, characterized in that: The specific process of step S1 is as follows: The tool movement trajectory mounted at the end of the A / C rotary head is interpolated using an interpolation method to obtain several toolpath points. Based on these interpolated toolpath points, the singular axis direction vector is calculated. These toolpath points include both the tool axis direction vector and the tool tip vector. The calculation formula is as follows: ； in, This represents the singular axis direction vector obtained at the current toolpath point after interpolation; This represents the tool tip vector at the current toolpath point after interpolation; , , The tool tip of the tool mounted at the end effector of the robot is located in the reference coordinate system B-xyz. axis, axis, Axis coordinates; This represents the tool axis direction vector at the current toolpath point after interpolation; This represents the effective distance along the central axis of the end-effector in the A / C rotary head. This effective distance is defined as: the distance along the plane passing through the A-axis of the two-degree-of-freedom cascaded mechanism of the A / C rotary head and perpendicular to the central axis J of the tool. In the middle, the central axis J of the tool is perpendicular to the plane. intersection With the tip of the knife The distance between them.

3. A method for smoothing the C-axis rotation angle of a five-degree-of-freedom hybrid robot according to claim 1 or 2, characterized in that: Take 0.6, Take 0.

01.

4. A method for smoothing the C-axis rotation angle of a five-degree-of-freedom hybrid robot according to claim 1 or 2, characterized in that: The specific process of step S4 is as follows: The solution obtained using step S3 And the rotation angle of the oscillating head around the A-axis in the A / C swivel. Preserve the tool tip vector at the current toolpath point after interpolation. Without changing the current toolpath point, calculate the corrected tool axis direction vector. .

Images