Double-hybrid-robot collaborative singularity avoidance planning method based on mirror image center

Abstract

The invention discloses a dual-hybrid robot collaborative singularity avoidance planning method based on a mirror image center, and relates to the field of robot motion control. The method comprises the following steps: firstly, calculating a singular axis vector of each tool path interpolation point and a support head path interpolation point, judging whether the double-hybrid robot is in a singular configuration, and then enabling the robots on two sides to avoid a singular area by utilizing a method for correcting a tool axis vector and a support head axis vector. And a method for constructing a mirror image center to correct a tool nose point position vector and a supporting head tail end center position vector is utilized to enable the robot to still keep movement synchronization, and finally iterative detection and correction are carried out on all path interpolation points. The method has the beneficial effects that the method can be implanted into a kinematics inverse solution algorithm in a motion controller of a mirror image milling system, so that the attitude of the two machines is always kept collinear in the correction process while the singularity is synchronously corrected by the two machines, and the continuity and smoothness of a motion trail are ensured.

Description

Technical Field

[0001] The present invention relates to a robot control method, and particularly to a cooperative singularity avoidance planning method for a dual hybrid serial-parallel robot based on a mirror center. Background Art

[0002] The singularity problem of hybrid serial-parallel robots is more complex than that of traditional five-axis machine tools, mainly due to the enhanced kinematic coupling effect caused by their parallel-series hybrid structure. In a mirror milling system, the dual hybrid serial-parallel robots are symmetrically arranged, and both have motion singularities. Moreover, due to the fixed connection relationship between the A / C turret and the parallel mechanism, the direction of the singular axis changes dynamically with the robot pose. In addition, the tool direction is jointly determined by the turret attitude and the joint motion of the parallel mechanism, resulting in the problems of mutual coupling of singular regions and the need to strictly maintain the mirror symmetry of the two machines during the correction process, further increasing the difficulty of determining the singular region and synchronous correction.

[0003] Patent CN116330251A discloses a motion singularity correction method for a five-degree-of-freedom hybrid serial-parallel robot, which has the functions of online singularity detection and real-time correction, and effectively solves the problem of motion instability of a single hybrid serial-parallel robot in the singular region. At present, there is no reported method for synchronous singularity correction of two machines in the machining of a dual hybrid serial-parallel robot mirror milling system. Summary of the Invention

[0004] The object of the present invention is to overcome the shortcomings of the existing technology and provide a cooperative singularity avoidance planning method for a dual hybrid serial-parallel robot based on a mirror center, which can synchronously correct the singularities of the two machines while ensuring that the postures of the two machines are always collinear during the correction process, and realize the smooth motion of the dual hybrid serial-parallel robot when passing through the singular region.

[0005] A cooperative singularity avoidance planning method for a dual hybrid serial-parallel robot based on a mirror center of the present invention includes the following steps:

[0006] Step 1: Place the milling robot and the supporting robot on the left and right sides of the workpiece respectively, start the milling robot to machine the workpiece, and the supporting head of the supporting robot performs a supporting motion on the workpiece; use the interpolation method to interpolate the tool motion trajectory of the milling robot and the supporting head motion trajectory of the supporting robot respectively to obtain a number of tool path interpolation points and supporting head path interpolation points. Each tool path interpolation point includes the position vector r C,1 of the tool tip point of the tool and the axis vector w1 of the tool, and each supporting head path interpolation point includes the position vector r C,2 of the center of the end of the supporting head and the axis vector w2 of the supporting head. Combine the position vector of the tool tip point of the tool and the position vector of the center of the end of the supporting head and denote it as r C,i (i = 1, 2), and combine the axis vector of the tool and the axis vector of the supporting head and denote it as w i(i = 1, 2); then calculate the singular axis vector s between each tool path interpolation point and the support head path interpolation point i :

[0007]

[0008] In the formula:

[0009] r C,i =(x C,i y C,i z C,i ), when i = 1, r T represents the position vector of the tool tip point after interpolation, x C,i , y C,i , z C,i , z C,i are the x-axis, y-axis, and z-axis coordinates of the tool tip point in the reference coordinate system B1 - x1y1z1 of the milling robot respectively, a i is the distance between the tool tip point and the intersection of the two rotation axes of the A / C turret of the milling robot, b i is the perpendicular distance from the intersection of the two rotation axes of the A / C turret of the milling robot to the tool axis;

[0010] When i = 2, r C,i represents the position vector of the center of the end of the support head after interpolation, x C,i , y C,i , z C,i are the x-axis, y-axis, and z-axis coordinates of the center of the end of the support head in the reference coordinate system B2 - x2y2z2 of the support robot respectively; a i is the distance between the center of the end of the support head and the intersection of the two rotation axes of the A / C turret of the support robot, b i is the perpendicular distance from the intersection of the two rotation axes of the A / C turret of the support robot to the support head axis;

[0011] Step 2: Determine whether the milling robot and the support robot are both singular at the same time. The specific process is as follows:

[0012] Calculate the angle τ1 between the axis vector w1 of the tool at the current path interpolation point and the singular axis vector s1, and the angle τ2 between the support head axis vector w2 and the singular axis vector s2 respectively, and determine whether the angle τ1 and the angle τ2 are greater than the singular determination threshold [τ]. If τ i (i = 1, 2) are not greater than the singular determination threshold, execute Step 3; otherwise, without correction, execute Step 5:

[0013]

[0014] [τ] - singular determination threshold, represents the vector w iThe transposed matrix;

[0015] Step 3. Modify the axis vectors of the tool and the support head so that the milling robot and the support robot avoid the singular region;

[0016] Step 4. Calculate the position vectors of the corrected tool tip point and the center position of the support head end. The corrected axis vectors of the tool and the support head are symmetrically offset along the mirror center and gradually approach until they are collinear. The mirror center is the midpoint of the line connecting the tool tip point and the center of the support head end during the machining process;

[0017] Step 5. Determine whether all interpolation points have been corrected. If so, exit the correction program; if not, execute Steps 2 - 4 for the next interpolation point until all interpolation points have been judged and corrected.

[0018] The beneficial effect of the present invention is that by combining singularity handling with motion planning, while synchronously correcting singularities for the two robots, it ensures that the postures of the two robots always remain collinear during the correction process, ensuring the continuity and smoothness of the motion trajectory. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for use in the embodiments. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained based on these drawings.

[0020] Figure 1 is a three - dimensional structure diagram of a five - degree - of - freedom hybrid robot with a rotating bracket at a first angle;

[0021] Figure 2 is a three - dimensional structure diagram of a five - degree - of - freedom hybrid robot with a rotating bracket at a second angle;

[0022] Figure 3 is a three - dimensional structure diagram of a symmetrically arranged dual - hybrid robot;

[0023] Figure 4 is a schematic diagram of the mechanism of a symmetrically arranged dual - hybrid robot;

[0024] Figure 5 is a schematic diagram of the judgment and correction process of the collaborative singularity avoidance planning method for a dual - hybrid robot based on the mirror center of the present invention.

[0025] In the figure:

[0026] 1. Driven length adjustment device, 2. First active length adjustment device, 3. Second active length adjustment device,

[0027] 4. Third active length adjustment device, 5. Moving platform, 6. A / C turret,

[0028] 11. First fixed axis seat, 21. Second fixed axis seat,

[0029] 12. First rotating bracket, 22. Second rotating bracket,

[0030] 23 / 33 / 43. Servo motor, 24. First hinge, 34. Second hinge, 44. Third hinge,

[0031] Ι. Milling robot, ΙΙ. Support robot,

[0032] B1-x1y1z1. Milling robot reference coordinate system, B2-x2y2z2. Support robot reference coordinate system,

[0033] a1. Distance between the tool tip point and the intersection of the two rotation axis lines of the A / C turret of the milling robot,

[0034] b1. Perpendicular distance from the intersection of the two rotation axis lines of the A / C turret of the milling robot to the tool axis,

[0035] a2. Distance between the center of the support head end and the intersection of the two rotation axis lines of the A / C turret of the support robot,

[0036] b2. Perpendicular distance from the intersection of the two rotation axis lines of the A / C turret of the support robot to the support head axis,

[0037] e1. Perpendicular distance from the intersection of the A-axis and C-axis of the A / C turret of the milling robot to the moving platform,

[0038] e2. Perpendicular distance from the intersection of the A-axis and C-axis of the A / C turret of the support robot to the moving platform,

[0039] θ 4,1 . Angle of rotation of the tool relative to the moving platform of the milling robot around the C-axis of the A / C turret,

[0040] θ 4,2 . Angle of rotation of the support head relative to the moving platform of the support robot around the C-axis of the A / C turret,

[0041] θ 5,1 . Angle of rotation of the tool relative to the moving platform of the milling robot around the A-axis of the A / C turret,

[0042] θ 5,2 . Angle of rotation of the support head relative to the moving platform of the support robot around the A-axis of the A / C turret,

[0043] M. Mirror center. Detailed implementation method

[0044] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative work shall fall within the protection scope of the present invention.

[0045] To make the above objects, features, and advantages of the present invention more obvious and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0046] The body structure of the five-degree-of-freedom hybrid robot (milling robot Ι, support robot ΙΙ) adopted by the present invention can adopt the existing structure, such as the structure of "a five-degree-of-freedom hybrid robot with a rotating bracket" disclosed in the patent with the publication number of CN104985596A: combined Figure 1 and Figure 2 , this robot includes two rotational degrees of freedom A / C swivel heads 6, a moving platform 5 connected in series with the A / C swivel heads 6 at the end, a driven length adjustment device 1, a first active length adjustment device 2, a second active length adjustment device 3, a third active length adjustment device 4, as well as a first rotating bracket 12 and a second rotating bracket 22. Both sides of the first rotating bracket 12 are respectively connected to two first fixed shaft seats 11 through a rotating pair, and both sides of the second rotating bracket 22 are respectively connected to two second fixed shaft seats 21 through a rotating pair.

[0047] The first active length adjustment device 2, the second active length adjustment device 3, and the third active length adjustment device 4 are respectively driven by a first servo motor 23, a second servo motor 33, and a third servo motor 43 so that the three active length adjustment devices can extend and shorten. The driven length adjustment device 1 is connected to the first rotating bracket 12 through a rotating pair, and the bottom of the driven length adjustment device 1 is fixedly connected to the moving platform 5; the upper part of the first active length adjustment device 2 is rotationally connected to two second fixed shaft seats 21 through a Hooke joint, and the bottom of the first active length adjustment device 2 is connected to the moving platform 5 through a first hinge 24; the upper parts of the second active length adjustment device 3 and the third active length adjustment device 4 are symmetrically connected to both sides of the first rotating bracket 12 through rotating pairs, and the bottoms are respectively connected to the moving platform 5 through a second hinge 34 and a third hinge 44.

[0048] The support mechanism of the support-side hybrid robot (support robot) adopted by the method of the present invention adopts the "a magnetically guided pneumatic variable stiffness mirror milling flexible support mechanism" disclosed in the patent with the publication number of CN110227954A, and this structure can suppress the vibration phenomenon during the machining of thin-walled parts.

[0049] A method for collaborative singularity avoidance planning of a dual hybrid robot based on the mirror center of the present invention as shown in the accompanying drawings, comprising the following steps:

[0050] Step 1. As Figure 3 shown, place the milling robot and the support robot on the left and right sides of the workpiece respectively, start the milling robot to process the workpiece, and the support head of the support robot performs a support movement on the workpiece; use the interpolation method to interpolate the tool movement trajectory of the milling robot and the support head movement trajectory of the support robot respectively to obtain a number of tool path interpolation points and support head path interpolation points. Each tool path interpolation point includes the position vector r C,1 of the tool tip point of the tool and the axis vector w1 of the tool. Each support head path interpolation point includes the position vector r C,2 of the center of the end of the support head and the axis vector w2 of the support head. Combine the position vector of the tool tip point of the tool and the position vector of the center of the end of the support head and denote it as r C,i (i = 1, 2), and combine the axis vector of the tool and the axis vector of the support head and denote it as w i (i = 1, 2); then calculate the singular axis vector s i of each tool path interpolation point and support head path interpolation point:

[0051]

[0052] In the formula:

[0053] r C,i =(x C,i y C,i z C,i ). When i = 1, r T represents the position vector of the interpolated tool tip point. x C,i , y C,i , z C,i , z C,i are the x-axis, y-axis, and z-axis coordinates of the tool tip point in the reference coordinate system B1-x1y1z1 of the milling robot respectively. a i is the distance between the tool tip point and the intersection of the two rotation axes of the A / C turret of the milling robot, and b i is the perpendicular distance from the intersection of the two rotation axes of the A / C turret of the milling robot to the tool axis. As Figure 4 shown, establish the reference coordinate system B1-x1y1z1 of the milling robot on the first rotating bracket of the milling robot with B1 as the origin. The x1-axis coincides with the axis of the rotating bracket, the y1-axis is perpendicular to the axis of the rotating bracket, and the z-axis satisfies the right-hand rule.

[0054] When i = 2, r C,i represents the position vector of the interpolated center of the end of the support head. x C,i , yC,i , z C,i are the x-axis, y-axis, and z-axis coordinates of the center of the end of the support head in the reference coordinate system B2-x2y2z2 of the support robot respectively; a i is the distance between the center of the end of the support head and the intersection point of the two rotation axes of the A / C rotating head of the support robot, and b i is the perpendicular distance from the intersection point of the two rotation axes of the A / C rotating head of the support robot to the axis of the support head. As Figure 4 shown, with B2 as the origin, the reference coordinate system B2-x2y2z2 of the support robot is established on the first rotating bracket of the support robot. The x2-axis coincides with the axis of the rotating bracket, the y2-axis is perpendicular to the axis of the rotating bracket, and the z2-axis satisfies the right-hand rule.

[0055] Step 2: Determine whether the milling robot and the support robot are both in singularity. The specific process is as follows:

[0056] Calculate the angle τ1 between the axis vector w1 of the tool at the current path interpolation point and the singularity axis vector s1, and the angle τ2 between the axis vector w2 of the support head and the singularity axis vector s2 respectively, and determine whether the angle τ1 and the angle τ2 are greater than the singularity determination threshold [τ]. If τ i (i = 1, 2) are not greater than the singularity determination threshold, execute Step 3; otherwise, without correction, execute Step 5:

[0057]

[0058] [τ] - Singularity determination threshold, usually taken as 0.0001 rad, represents the transpose matrix of the vector w i .

[0059] Step 3: Modify the axis vector of the tool and the axis vector of the support head so that the milling robot and the support robot avoid the singularity area. The specific process is as follows:

[0060] First step, use the position vector of the tool tip point, the position vector of the center of the end of the support head, the axis vector of the tool, and the axis vector of the support head corresponding to the current interpolation point to calculate the driving joint variables of the milling robot and the support robot respectively through the inverse kinematics algorithm (specifically, see pages 48-70 of "Parallel Robots", ISBN: 9787111675884). The driving joint variables include q 1,i , q 2,i , q 3,i , θ 4,i , θ 5,i , i = 1, 2. Among them, q 1,i , q 2,i , q 3,iare the rod lengths of the three active length adjustment devices in the milling robot and the supporting robot, θ 4,i The angle of rotation of the tool relative to the moving platform of the milling robot around the C-axis of the A / C head or the angle of rotation of the support head relative to the moving platform of the supporting robot around the C-axis of the A / C head, θ 5,i It is the rotation angle of the tool relative to the moving platform of the milling robot around the A / C head A axis or the rotation angle of the support head relative to the moving platform of the supporting robot around the A / C head A axis, such as Figure 4 shown.

[0061] In the second step, the θ of the milling robot and the support robot are respectively 4,i Make corrections, the correction algorithm is:

[0062]

[0063] Where:

[0064] θ 4,inew Indicates the θ at the current interpolation point 4,i The corrected tool rotates around the C-axis of the A / C head relative to the moving platform of the milling robot, or the support head rotates around the C-axis of the A / C head relative to the moving platform of the supporting robot; when τ i =[τ], θ 4,inew =θ 4,i , that is, θ at this time 4,inew It is the rotation angle of the tool relative to the moving platform of the milling robot around the C-axis of the A / C head at the first interpolation point, or the rotation angle of the support head relative to the moving platform of the supporting robot around the C-axis of the A / C head.

[0065] θ 4,ipre Indicates the rotation angle of the tool corrected at the previous interpolation point relative to the moving platform of the milling robot around the A / C head C-axis, or the rotation angle of the support head relative to the moving platform of the support robot around the A / C head C-axis.

[0066] Step 3: Calculate the rod length q of the driven length adjustment device of the milling robot and the supporting robot respectively 4,i .

[0067] Let e1 be the vertical distance from the intersection of the A-axis and C-axis of the milling robot's A / C head to the milling robot's moving platform, and e2 be the vertical distance from the intersection of the A-axis and C-axis of the support robot's A / C head to the support robot's moving platform. C,i 、y C,i 、z C,i , and θ obtained in the above steps 5,i and θ 4,inew We can get:

[0068]

[0069] Among them, Sθ represents sinθ, and Cθ represents cosθ.

[0070] Step 4: According to θ 4,inew and θ 5,i and q 4,i , calculate the first intermediate variable θ 2,i .

[0071]

[0072] According to θ 4,inew and θ 5,i and q 4,i and the obtained θ 2,i , calculate the second intermediate variable θ 1,i .

[0073]

[0074] In the formula:

[0075] D i = ((a i Sθ 5,i - b i Cθ 5,i )Sθ 4,inew )Sθ 2,i - (q 4,i + b i Sθ 5,i + a i Cθ 5,i + e i )Cθ 2,i

[0076] Step 5: Use θ 2,i and θ 1,i and θ 4,inew and θ 5,i to calculate the axis vector of the tool and the axis vector w inew of the support head:

[0077]

[0078] Step 4: Calculate the position vector of the corrected tool tip point and the center position vector of the end of the support head. The specific process is as follows:

[0079] Step 1: Let the distance between the real-time position of the tool tip point before correction and the real-time position of the center of the end of the support head before correction during the machining process be d, d = ||r C,2 - r C,1 ||, and construct the mirror center M of the milling robot and the support robot, as shown in Figure 4As shown, the mirror center is the reference point for the symmetric movement of the tool and the support head, that is, the midpoint of the line connecting the tip point of the tool and the center of the end of the support head during the machining process. According to the axis vector of the tool before correction and the axis vector w of the support head i and the axis vector of the tool after correction and the axis vector w of the support head inew , calculate the distance d from the mirror center to the tip point after correction M,1 and the distance d from the mirror center to the center point of the end of the support head after correction M,2 ;

[0080]

[0081] Second step, according to the axis vector of the tool before correction and the axis vector w of the support head i and the axis vector of the tool after correction and the axis vector w of the support head inew , calculate the offset vector of the tip point and the offset vector of the center point of the end of the support head

[0082]

[0083] Third step, use r C,i and to further solve the new position vector r of the tip point of the milling robot C,1new and the new position vector r of the center of the end of the support head of the support robot C,2new , and the axis vectors of the corrected tool and the support head are symmetrically offset along the mirror center and gradually approach until they are collinear.

[0084]

[0085] Step Five: Judge whether all interpolation points have been corrected. If so, exit the correction program; if not, execute Steps Two - Four for the next interpolation point until all interpolation points are judged and corrected.

[0086] This method uses the method of correcting the axis vectors of the tool and the support head to make the two robots avoid the singular region, and uses the method of constructing the mirror center to correct the position vectors of the tip point and the center of the end of the support head to synchronously correct the singularity of the two robots, so that the robot postures always remain collinear during the correction process, ensuring the continuous and smooth movement of the dual hybrid robot when crossing the singular region.

[0087] Finally, it should be noted that the above are only the preferred embodiments of the present invention and are not used to limit the protection scope of the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A cooperative singularity avoidance planning method for a dual series-parallel robot based on the mirror center, characterized in that Including the following steps: Step 1: Place the milling robot and the support robot on the left and right sides of the workpiece respectively. Start the milling robot to machine the workpiece, and the support head of the support robot performs a support movement on the workpiece. Use the interpolation method to interpolate the tool movement trajectory of the milling robot and the support head movement trajectory of the support robot respectively, obtaining a number of tool path interpolation points and support head path interpolation points. Each tool path interpolation point includes the position vector r of the tool tip point of the tool C,1 and the axis vector w1 of the tool. Each support head path interpolation point includes the position vector r of the center of the end of the support head C,2 and the axis vector w2 of the support head. Combine the position vector of the tool tip point of the tool and the position vector of the center of the end of the support head and denote it as r C,i (i = 1, 2), and combine the axis vector of the tool and the axis vector of the support head and denote it as w i (i = 1, 2); then calculate the singular axis vector s of each tool path interpolation point and support head path interpolation point i : In the formula: r C,i = (x C,i y C,i z C,i ) T , when i = 1, r C,i represents the position vector of the tool tip point after interpolation, x C,i , y C,i , z C,i are the x-axis, y-axis, and z-axis coordinates of the tool tip point in the reference coordinate system B1-x1y1z1 of the milling robot respectively, a i is the distance between the tool tip point and the intersection point of the two rotation axes of the A / C turret of the milling robot, b i is the perpendicular distance from the intersection point of the two rotation axes of the A / C turret of the milling robot to the tool axis; When i = 2, r C,i represents the position vector of the center of the end of the support head after interpolation, x C,i , y C,i , z C,i are respectively the x-axis, y-axis, and z-axis coordinates of the center of the end of the support head in the reference coordinate system B2-x2y2z2 of the support robot; a i is the distance between the center of the end of the support head and the intersection point of the two rotation axes of the A / C rotating head of the support robot, b i is the perpendicular distance from the intersection point of the two rotation axes of the A / C rotating head of the support robot to the axis of the support head; Step 2: Determine whether the milling robot and the support robot are singular at the same time. The specific process is as follows: Calculate the angle τ1 between the axis vector w1 of the tool at the current path interpolation point and the singular axis vector s1, and the angle τ2 between the axis vector w2 of the support head and the singular axis vector s2, respectively, and determine whether the angle τ1 and the angle τ2 are greater than the singular determination threshold [τ]. If neither τ i (i = 1, 2) is greater than the singular determination threshold, execute Step 3; otherwise, without correction, execute Step 5: [τ] - Singularity determination threshold, Denote the vector w i The transpose matrix of; Step 3: Correct the axis vectors of the tool and the support head so that the milling robot and the support robot avoid the singular region; Step 4: Calculate the position vectors of the corrected tool tip point and the center position of the support head end. The corrected axis vectors of the tool and the support head are symmetrically offset along the mirror center and gradually approach until they are collinear. The mirror center is the midpoint of the line connecting the tool tip point and the center position of the support head end during the machining process; Step 5: Determine whether all interpolation points have been corrected. If so, exit the correction program; if not, execute Steps 2 - 4 for the next interpolation point until all interpolation points are judged and corrected; 2. The method for collaborative singularity avoidance planning of a dual hybrid robot based on the mirror center according to claim 1, wherein: The specific process of Step 3 is as follows: First step: Using the position vector of the tool tip point corresponding to the current interpolation point, the position vector of the center of the end of the support head, the axis vector of the tool, and the axis vector of the support head, calculate the driving joint variables of the milling robot and the support robot respectively through the inverse kinematics algorithm. The driving joint variables include q 1,i 、q 2,i 、q 3,i 、θ 4,i 、θ 5,i , i = 1, 2. Among them, q 1,i 、q 2,i 、q 3,i are the rod lengths of the three active length adjustment devices in the milling robot and the support robot respectively. θ 4,i is the rotation angle of the tool relative to the moving platform of the milling robot around the C axis of the A / C turret or the rotation angle of the support head relative to the moving platform of the support robot around the C axis of the A / C turret. θ 5,i is the rotation angle of the tool relative to the moving platform of the milling robot around the A axis of the A / C turret or the rotation angle of the support head relative to the moving platform of the support robot around the A axis of the A / C turret; Step 2: Respectively correct the θ of the milling robot and the support robot 4,i The correction algorithm is as follows: In the formula: θ 4,inew represents θ at the position of the current interpolation point 4,i the rotation angle of the tool relative to the moving platform of the milling robot around the C-axis of the A / C turret or the rotation angle of the support head relative to the moving platform of the support robot around the C-axis of the A / C turret after correction; when τ i =[τ], θ 4,inew =θ 4,i , and at this time, θ 4,inew is the rotation angle of the tool relative to the moving platform of the milling robot around the C-axis of the A / C turret or the rotation angle of the support head relative to the moving platform of the support robot around the C-axis of the A / C turret at the position of the first interpolation point; θ 4,ipre It represents the corrected rotation angle of the tool relative to the moving platform of the milling robot around the C-axis of the A / C turret or the rotation angle of the support head relative to the moving platform of the support robot around the C-axis of the A / C turret at the position of the previous interpolation point; Step 3: Calculate the rod length q of the driven length adjustment device of the milling robot and the support robot respectively 4,i ; Let e1 denote the perpendicular distance from the intersection point of the axes of the A-axis and C-axis of the milling robot's A / C head to the moving platform of the milling robot, and e2 denote the perpendicular distance from the intersection point of the axes of the A-axis and C-axis of the supporting robot's A / C head to the moving platform of the supporting robot. Based on the coordinates x C,i 、y C,i 、z C,i of the tool tip and the center point at the end of the support head, and the θ 5,i and θ 4,inew obtained from the above steps, we get: Wherein, Sθ represents sinθ, and Cθ represents cosθ; Step 4. According to θ 4,inew , θ 5,i and q 4,i , calculate the first intermediate variable θ 2,i ; According to θ 4,inew , θ 5,i and q 4,i and the above-obtained θ 2,i , calculate the second intermediate variable θ 1,i ; In the formula: D i = ((a i Sθ 5,i - b i Cθ 5,i )Sθ 4,inew )Sθ 2,i - (q 4,i + b i Sθ 5,i + a i Cθ 5,i + e i )Cθ 2,i Step 5: Using θ 2,i , θ 1,i , θ 4,inew and θ 5,i calculate the axis vector of the cutting tool and the axis vector w of the support head inew : The specific process of Step 4 is as follows: First step, let the distance between the real-time position of the tool tip point before correction and the real-time position of the center of the end of the support head during the machining process be d, d = ||r C,2 - r C,1 ||. According to the axis vector of the tool before correction and the axis vector w of the support head i as well as the axis vector of the corrected tool and the axis vector w of the support head inew , calculate the distance d from the mirror center to the tool tip point after correction M,1 and the distance d from the mirror center to the center point of the end of the support head after correction M,2 respectively; Step 2: Calculate the offset vector of the tool tip point and the offset vector of the center point at the end of the support head according to the axis vector of the tool before correction and the axis vector w of the support head i and the axis vector of the tool after correction and the axis vector w of the support head inew and the offset vector of the center point at the end of the support head ​ Step 3: Use r C,i and to further solve for the new position vector r C,1new of the tip point of the milling robot and the new position vector r C,2new of the center of the end of the support head of the support robot, and make the corrected tool axis vector and the support head axis vector symmetrically offset along the mirror center and gradually approach until they are collinear;

Images