Mirror center based dual hybrid robot cooperative singularity avoidance planning method

Abstract

The application discloses a kind of based on mirror center's double hybrid robot cooperative singularity-avoiding planning method, it is related to robot motion control field.This method first calculates the singular axis vector of each tool path interpolation point and support head path interpolation point, judges whether double hybrid robot is in singular configuration, then using the method of correcting tool axis vector and support head axis vector makes two sides robot avoid singular region, and using the method of constructing mirror center corrects tool tip position vector and support head end center position vector makes robot still keep motion synchronization, finally carries out iterative detection and correction to all path interpolation points.The application has the beneficial effect that it can be implanted in the kinematics inverse solution algorithm in mirror milling system motion controller, while correcting singularity of double machine, it is guaranteed that the attitude of double machine always keeps collinear in the correction process, ensures the continuous smoothness of motion trajectory.

Description

Technical Field

[0001] This invention relates to robot control methods, and more particularly to a collaborative singularity avoidance planning method for dual hybrid robots based on mirror centering. Background Technology

[0002] The singularity problem of hybrid 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 two hybrid robots are arranged symmetrically, and both exhibit kinematic singularities. Furthermore, due to the fixed connection between the A / C rotating head and the parallel mechanism, the singular axis direction dynamically changes with the robot pose. In addition, the tool direction is determined by both the rotating head posture and the joint motion of the parallel mechanism, leading to the problem of singular regions coupling with each other during correction and the need to strictly maintain the mirror symmetry of the two machines, further increasing the difficulty of identifying singular regions and synchronously correcting them.

[0003] Patent CN116330251A discloses a method for motion singularity correction of a five-DOF hybrid robot, which features online singularity detection and real-time correction, effectively solving the motion instability problem of a single hybrid robot in singular regions. Currently, no dual-machine synchronous singularity correction method has been reported for dual hybrid robot mirror milling systems. Summary of the Invention

[0004] The purpose of this invention is to overcome the shortcomings of existing technologies and provide a collaborative singularity avoidance planning method for dual hybrid robots based on mirror center. This method can ensure that the attitudes of the two robots remain collinear during the correction process while simultaneously correcting singularities, thus achieving smooth motion of the dual hybrid robots when traversing singular regions.

[0005] The present invention provides a collaborative singularity avoidance planning method for dual hybrid robots based on mirror centering, comprising the following steps:

[0006] 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 process the workpiece, while the support head of the support robot performs a support movement on the workpiece. Use interpolation to interpolate the tool path of the milling robot and the support head path of the support robot, respectively, to obtain several tool path interpolation points and support head path interpolation points. Each tool path interpolation point includes the tool tip position vector r. C,1 The axis vector w1 of the tool, and the position vector r of each support head path interpolation point including the center of the support head end. C,2 The vector of the tool tip position and the vector of the center of the support head end are combined and denoted as r, which is the vector of the axis of the support head. C,i (i = 1, 2), the axis vector of the tool and the axis vector of the support head are combined and denoted as w. i(i = 1, 2); then calculate the singular axis vector s between each toolpath 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 ) T When i = 1, r C,i The x represents the position vector of the tool tip after interpolation. C,i y C,i z C,i These are the x-axis, y-axis, and z-axis coordinates of the tool tip in the milling robot's reference coordinate system B1-x1y1z1, respectively. i b is the distance between the tool tip and the intersection of the two rotation axes of the A / C rotating head of the milling robot. i The perpendicular distance from the intersection of the two rotating axes of the A / C rotating head of the milling robot to the tool axis;

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

[0011] Step 2: Determine whether the milling robot and the support robot simultaneously experience singularities. The specific process is as follows:

[0012] Calculate the angle τ1 between the tool axis vector w1 and the singular axis vector s1 at the current path interpolation point, and the angle τ2 between the support head axis vector w2 and the singular axis vector s2. Then determine whether angles τ1 and τ2 are greater than the singularity threshold [τ]. If τ... i If (i=1,2) are not greater than the singularity threshold, proceed to step three; otherwise, do not make any correction and proceed to step five.

[0013]

[0014] [τ]——Singularity threshold, Represents vector w iThe transpose of the matrix;

[0015] Step 3: Correct the axis vector of the cutting tool and the axis vector of the support head so that the milling robot and the support robot avoid the singularity area;

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

[0017] Step 5: Determine if all interpolation points have been corrected. If yes, exit the correction program; otherwise, execute steps 2-4 for the next interpolation point until all interpolation points have been judged and corrected.

[0018] The advantages of this invention are: by combining singularity processing with motion planning, while simultaneously correcting singularities in both machines, it ensures that the attitudes of the two machines remain collinear during the correction process, thus ensuring the continuous and smooth motion trajectory. Attached Figure Description

[0019] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0020] Figure 1 This is a three-dimensional structural diagram of an existing five-degree-of-freedom hybrid robot with a rotating support, taken from the first angle.

[0021] Figure 2 This is a three-dimensional structural diagram of an existing five-DOF hybrid robot with a rotating support, taken from the second angle.

[0022] Figure 3 A 3D structural diagram of a symmetrically arranged dual hybrid robot;

[0023] Figure 4 A simplified diagram of the mechanism of a symmetrically arranged dual hybrid robot;

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

[0025] In the picture:

[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 rotary head.

[0028] 11. First fixed shaft seat; 21. Second fixed shaft 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] I. Milling robot, II. Support robot

[0032] B1-x1y1z1, the reference coordinate system for the milling robot; B2-x2y2z2, the reference coordinate system for the support robot.

[0033] a1. The distance between the tool tip and the intersection of the two rotation axes of the A / C rotating head of the milling robot.

[0034] b1. The perpendicular distance from the intersection of the two rotating axes of the A / C rotating head of the milling robot to the tool axis.

[0035] a2. The distance between the center of the support head end and the intersection of the two rotation axes of the A / C rotating head of the support robot.

[0036] b2. The perpendicular distance from the intersection of the two rotating axes of the A / C rotating head of the support robot to the axis of the support head.

[0037] e1. The perpendicular distance from the intersection of the axes of the A and C axes of the milling robot's A / C rotating head to the moving platform.

[0038] e2. The perpendicular distance from the intersection of the axes of the A and C axes of the supporting robot's A / C rotating head to the moving platform.

[0039] θ 4,1 The angle of rotation of the cutting tool relative to the moving platform of the milling robot around the C-axis of the A / C rotating head.

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

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

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

[0043] M, Mirror Center. Detailed Implementation

[0044] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

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

[0046] The five-degree-of-freedom hybrid robot (milling robot I, support robot II) used in this invention can employ existing structures, such as the structure of "a five-degree-of-freedom hybrid robot with a rotating support" disclosed in patent CN104985596A: combined with Figure 1 and Figure 2 The robot includes two rotational A / C swing heads 6, a moving platform 5 connected in series with the A / C swing heads 6 at its 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, a first rotating bracket 12, and a second rotating bracket 22. Each side of the first rotating bracket 12 is connected to two first fixed shaft seats 11 via a rotating joint, and each side of the second rotating bracket 22 is connected to two second fixed shaft seats 21 via a rotating joint.

[0047] 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, 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 revolute joint, 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 rotatably connected to two second fixed shaft seats 21 through a Hooke hinge, 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 revolute joints, and the bottoms are connected to the moving platform 5 through the second hinge 34 and the third hinge 44, respectively.

[0048] The support mechanism of the hybrid robot (support robot) used in the method of the present invention adopts the "magnetically guided pneumatic variable stiffness mirror milling flexible support mechanism" disclosed in patent publication number CN110227954A. This structure can suppress vibration phenomena during the processing of thin-walled parts.

[0049] As shown in the attached figure, the present invention provides a collaborative singularity avoidance planning method for dual hybrid robots based on a mirror center, comprising the following steps:

[0050] Step 1, such as Figure 3 As shown, a milling robot and a support robot are placed on the left and right sides of the workpiece, respectively. The milling robot is started to machine the workpiece, while the support head of the support robot performs a support movement on the workpiece. Interpolation is used to interpolate the tool path of the milling robot and the support head path of the support robot, resulting in several tool path interpolation points and support head path interpolation points. Each tool path interpolation point includes the tool tip position vector r. C,1 The axis vector w1 of the tool, and the position vector r of each support head path interpolation point including the center of the support head end. C,2 The vector of the tool tip position and the vector of the center of the support head end are combined and denoted as r, which is the vector of the axis of the support head. C,i (i = 1, 2), the axis vector of the tool and the axis vector of the support head are combined and denoted as w. i (i = 1, 2); then calculate the singular axis vector s between each toolpath interpolation point and the support head path interpolation point. i :

[0051]

[0052] In the formula:

[0053] r C,i =(x C,i y C,i z C,i ) T When i = 1, r C,i The x represents the position vector of the tool tip after interpolation. C,i y C,i z C,i These are the x-axis, y-axis, and z-axis coordinates of the tool tip in the milling robot's reference coordinate system B1-x1y1z1, respectively. i b is the distance between the tool tip and the intersection of the two rotation axes of the A / C rotating head of the milling robot. i This is the perpendicular distance from the intersection of the two rotation axes of the A / C rotating head of the milling robot to the tool axis. For example... Figure 4 As shown, the milling robot reference coordinate system B1-x1y1z1 is established on the first rotating support of the milling robot with B1 as the origin. The x1 axis coincides with the axis of the rotating support, the y1 axis is perpendicular to the axis of the rotating support, and the z1 axis satisfies the right-hand rule.

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

[0055] Step 2: Determine whether the milling robot and the support robot experience singularities simultaneously. The specific process is as follows:

[0056] Calculate the angle τ1 between the tool axis vector w1 and the singular axis vector s1 at the current path interpolation point, and the angle τ2 between the support head axis vector w2 and the singular axis vector s2. Then determine whether angles τ1 and τ2 are greater than the singularity threshold [τ]. If τ... i If (i=1,2) are not greater than the singularity threshold, proceed to step three; otherwise, do not make any correction and proceed to step five.

[0057]

[0058] [τ]—Singularity threshold, typically set to 0.0001 rad. Represents vector w i The transpose of .

[0059] Step 3: Correct the axis vector of the cutting tool and the axis vector of the support head so that the milling robot and the support robot avoid singular areas. The specific process is as follows:

[0060] The first step involves using the tool tip position vector corresponding to the current interpolation point, the position vector of the support head end center, the tool axis vector, and the support head axis vector to calculate the variables of each drive joint of the milling robot and the support robot using an inverse kinematics algorithm (see pages 48-70 of "Parallel Robots", ISBN: 9787111675884 for details). These drive joint variables include q. 1,i q 2,i q 3,i θ 4,i θ 5,i , i = 1, 2, where q 1,i q 2,i q 3,iThese are the rod lengths of the three active length adjustment devices in the milling robot and the support robot, θ. 4,i θ is the rotation angle of the cutting tool relative to the moving platform of the milling robot about the C-axis of the A / C head, or the rotation angle of the support head relative to the moving platform of the supporting robot about the C-axis of the A / C head. 5,i This refers to the rotation angle of the cutting tool relative to the moving platform of the milling robot around the A-axis of the A / C head, or the rotation angle of the support head relative to the moving platform of the supporting robot around the A-axis of the A / C head. Figure 4 As shown.

[0061] The second step is to calculate θ for both the milling robot and the support robot. 4,i The correction algorithm is as follows:

[0062]

[0063] In the formula:

[0064] θ 4,inew θ represents the position of the current interpolation point. 4,i The corrected rotation angle of the tool relative to the moving platform of the milling robot about the C-axis of the A / C head, or the rotation angle of the support head relative to the moving platform of the supporting robot about the C-axis of the A / C head; when τ i When θ = [τ], 4,inew =θ 4,i That is, θ at this time 4,inew The first interpolation point is 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.

[0065] θ 4,ipre This indicates 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, which has been corrected at the previous interpolation point position, 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.

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

[0067] Let e1 represent the perpendicular distance from the intersection of the A-axis and C-axis of the milling robot's A / C rotary head to the moving platform of the milling robot, and let e2 represent the perpendicular distance from the intersection of the A-axis and C-axis of the support robot's A / C rotary head to the moving platform of the support robot. Based on the coordinates x of the tool tip and the center point of the support head's end... C,i y C,i z C,i and θ obtained from the above steps 5,i With θ 4,inew We can obtain:

[0068]

[0069] Where Sθ represents sinθ and Cθ represents cosθ.

[0070] Step 4: Based on θ 4,inew θ 5,i With q 4,i Calculate the first intermediate variable θ 2,i .

[0071]

[0072] According to θ 4,inew θ 5,i With q 4,i and the θ obtained above 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: Using θ 2,i θ 1,i θ 4,inew With θ 5,i Calculate the axis vector of the cutting tool and the axis vector of the support head w inew :

[0077]

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

[0079] The first step is to let d be the distance between the real-time position of the tool tip and the real-time position of the center of the end of the support head during the machining process, where d = ||r C,2 -r C,1 ||, construct the mirror center M of the milling robot and the support robot, such as Figure 4As shown, the mirror center is the reference point for the symmetrical movement of the tool and the support head, that is, the midpoint of the line connecting the tool tip and the center of the support head end during machining. This is based on the original axis vector of the tool and the axis vector of the support head before correction. i And the corrected tool axis vector and the support head axis vector w inew Calculate the distance d from the center of the mirror image to the corrected tool tip. M,1 And the distance d from the center of the mirror image to the center point of the corrected support head end. M,2 ;

[0080]

[0081] The second step is to adjust the axis vector of the tool before correction and the axis vector of the support head w. i And the corrected tool axis vector and the support head axis vector w inew Calculate the tool tip offset vector and the offset vector of the center point at the end of the support head

[0082]

[0083] The third step is to use r C,i and Further solve for the new position vector r of the cutting tool tip of the milling robot. C,1new And the new position vector r at the end of the support head of the robot C,2new The corrected tool axis vector and the support head axis vector are symmetrically offset along the mirror center and gradually converge until they are collinear.

[0084]

[0085] Step 5: Determine if all interpolation points have been corrected. If yes, exit the correction program; otherwise, execute steps 2-4 for the next interpolation point until all interpolation points have been judged and corrected.

[0086] This method uses the correction of the tool axis vector and the support head axis vector to make the two robots avoid the singular region. It also uses the method of constructing a mirror center to correct the tool tip position vector and the support head end center position vector to make the two robots correct the singularity synchronously. This ensures that the robot posture remains collinear during the correction process, thus ensuring the continuous and smooth movement of the dual hybrid robots when traversing the singular region.

[0087] Finally, it should be noted that the above description is merely a preferred embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A collaborative singularity avoidance planning method for dual hybrid robots based on mirror center, characterized in that... Includes 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 process the workpiece, while the support head of the support robot performs a support movement on the workpiece. Use interpolation to interpolate the tool path of the milling robot and the support head path of the support robot, respectively, to obtain several tool path interpolation points and support head path interpolation points. Each tool path interpolation point includes the tool tip position vector r. C,1 The axis vector w1 of the tool, and the position vector r of each support head path interpolation point including the center of the support head end. C,2 The vector of the tool tip position and the vector of the center of the support head end are combined and denoted as r, which is the vector of the axis of the support head. C,i (i = 1, 2), the axis vector of the tool and the axis vector of the support head are combined and denoted as w. i (i = 1, 2); then calculate the singular axis vector s between each toolpath interpolation point and the 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 The x represents the position vector of the tool tip after interpolation. C,i y C,i z C,i These are the x-axis, y-axis, and z-axis coordinates of the tool tip in the milling robot's reference coordinate system B1-x1y1z1, respectively. i b is the distance between the tool tip and the intersection of the two rotation axes of the A / C rotating head of the milling robot. i The perpendicular distance from the intersection of the two rotating axes of the A / C rotating head of the milling robot to the tool axis; When i = 2, r C,i The x vector represents the position vector of the center of the end of the interpolated support head. C,i y C,i z C,i These are the x-axis, y-axis, and z-axis coordinates of the center of the support head end in the support robot's reference coordinate system B2-x2y2z2; a i b is the distance between the center of the support head end and the intersection of the two rotation axes of the A / C rotating head of the support robot. i The perpendicular distance from the intersection of the two rotating axes of the robot's A / C rotating head to the axis of the support head; Step 2: Determine whether the milling robot and the support robot simultaneously experience singularities. The specific process is as follows: Calculate the angle τ1 between the tool axis vector w1 and the singular axis vector s1 at the current path interpolation point, and the angle τ2 between the support head axis vector w2 and the singular axis vector s2. Then determine whether angles τ1 and τ2 are greater than the singularity threshold [τ]. If τ... i If (i=1,2) are not greater than the singularity threshold, proceed to step three; otherwise, do not make any correction and proceed to step five. [τ]——Singularity threshold, Represents vector w i The transpose of the matrix; Step 3: Correct the axis vector of the cutting tool and the axis vector of the support head so that the milling robot and the support robot avoid the singularity area; Step 4: Calculate the corrected tool tip position vector and the support head end center position vector. The corrected tool axis vector and support head axis vector are symmetrically offset along the mirror center and gradually converge until they are collinear. The mirror center is the midpoint of the line connecting the tool tip and the support head end center during the machining process. Step 5: Determine if all interpolation points have been corrected. If yes, exit the correction program; otherwise, execute steps 2-4 for the next interpolation point until all interpolation points have been judged and corrected.

2. The dual-hybrid robot cooperative singularity avoidance planning method based on mirror center as described in claim 1, characterized in that: The specific process of step three is as follows: The first step involves using the tool tip position vector corresponding to the current interpolation point, the position vector of the support head end center, the tool axis vector, and the support head axis vector to calculate the variables of each drive joint of the milling robot and the support robot using an inverse kinematics algorithm. These drive joint variables include q. 1,i q 2,i q 3,i θ 4,i θ 5,i , i = 1, 2, where q 1,i q 2,i q 3,i These are the rod lengths of the three active length adjustment devices in the milling robot and the support robot, θ. 4,i θ is the rotation angle of the cutting tool relative to the moving platform of the milling robot about the C-axis of the A / C head, or the rotation angle of the support head relative to the moving platform of the supporting robot about the C-axis of the A / C head. 5,i The rotation angle of the cutting tool relative to the moving platform of the milling robot around the A-axis of the A / C head, or the rotation angle of the support head relative to the moving platform of the supporting robot around the A-axis of the A / C head; The second step is to calculate θ for both the milling robot and the support robot. 4,i The correction algorithm is as follows: In the formula: θ 4,inew θ represents the position of the current interpolation point. 4,i The corrected rotation angle of the tool relative to the moving platform of the milling robot around the C-axis of the A / C head, 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; when τ i When θ = [τ], 4,inew =θ 4,i θ at this time 4,inew 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 at the first interpolation point position, 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; θ 4,ipre This indicates the angle of rotation of the tool relative to the moving platform of the milling robot about the C-axis of the A / C head, which has been corrected at the previous interpolation point position; or the angle of rotation of the support head relative to the moving platform of the supporting robot about the C-axis of the A / C head. Step 3: Calculate the length q of the driven length adjustment device rod for both the milling robot and the support robot. 4,i ; Let e1 represent the perpendicular distance from the intersection of the A-axis and C-axis of the milling robot's A / C rotary head to the moving platform of the milling robot, and let e2 represent the perpendicular distance from the intersection of the A-axis and C-axis of the support robot's A / C rotary head to the moving platform of the support robot. Based on the coordinates x of the tool tip and the center point of the support head's end... C,i y C,i z C,i and θ obtained from the above steps 5,i With θ 4,inew get: Where Sθ represents sinθ and Cθ represents cosθ; Step 4: Based on θ 4,inew θ 5,i With q 4,i Calculate the first intermediate variable θ 2,i ; According to θ 4,inew θ 5,i With q 4,i and the 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 With θ 5,i Calculate the axis vector of the cutting tool and the axis vector of the support head w inew : The specific process of step four is as follows: First, let d be the distance between the real-time position of the tool tip and the real-time position of the center of the end of the support head during the machining process, where d = ||r C,2 -r C,1 ||, based on the original tool axis vector and the support head axis vector w i And the corrected tool axis vector and the support head axis vector w inew Calculate the distance d from the center of the mirror image to the corrected tool tip. M,1 And the distance d from the center of the mirror image to the center point of the corrected support head end. M,2 ; The second step is to adjust the axis vector of the tool before correction and the axis vector of the support head w. i And the corrected tool axis vector and the support head axis vector w inew Calculate the tool tip offset vector and the offset vector of the center point at the end of the support head The third step is to use r C,i and Further solve for the new position vector r of the cutting tool tip of the milling robot. C,1new And the new position vector r at the end of the support head of the robot C,2new The corrected tool axis vector and the support head axis vector are symmetrically offset along the mirror center and gradually converge until they are collinear;

Images