A method for detecting and compensating for the rigidity of the AC axis connection in a cradle-type five-axis machine.

Abstract

This invention discloses a method for detecting and compensating for the AC axis connection rigidity of a cradle-type five-axis machine. Based on the coordinate information of the rotation circle around the C axis at different A values ​​in a reference coordinate system, the center of the rotation circle is fitted to obtain the coordinate information of the rotation circle around the A axis. The AC axis connection rigidity is obtained from the coordinate information of the rotation circle around the C axis and the rotation circle around the A axis. A practical coordinate system is established for the machine tool in each A-axis posture, and the transformation vector between the theoretical coordinate system and the practical coordinate system is used to describe the AC axis connection rigidity. Error compensation is performed on the AC axis connection rigidity, and error compensation is also performed on the rotation vector between the theoretical coordinate system and the practical coordinate system under different A-axis postures, realizing the mutual conversion between the TheoryCoordinate and the practical coordinate system under different A-axis postures. This invention can determine the AC axis connection rigidity using coordinate information, is easy to operate, and has good practicality.

Description

Technical Field

[0001] This invention belongs to the technical field of rigidity detection and compensation for shaft connections in five-axis machines, specifically relating to a method for rigidity detection and error compensation of AC shaft connections in a cradle-type five-axis machine. Background Technology

[0002] Compared to traditional three-axis machines, five-axis machines offer higher machining accuracy, higher efficiency, and the ability to process more complex workpieces, giving them a greater competitive advantage in the market. However, the addition of two rotary axes also makes the mechanical structure of five-axis machines more complex. Combined with inherent geometric errors in the machine's mechanical structure and wear errors during operation, these factors present significant challenges in calibrating the geometric parameters of five-axis machines and eliminating various errors.

[0003] Five-axis rotary machines on the market are mainly divided into three types: dual-rotor table structure, single-rotor table-single-rotor table structure, and dual-head structure. Corresponding geometric and mathematical models are established based on different mechanical structures. The radial, axial, and tangential axis deviations of the rotating shafts are measured using instruments such as levels and ballbars, and the eccentricity of the rotating shafts is calculated for shaft inspection. However, there is no good description or detection method for the connection rigidity issues of some five-axis rotary shafts.

[0004] Therefore, this invention provides a method for detecting and compensating for the connection rigidity of the AC axis on a cradle-type five-axis machine. This invention utilizes the geometric information of a point on the machine tool rotating around the C-axis in a reference coordinate system to describe the connection rigidity of the AC axis using a comparison of theoretical and actual normal vectors and a transformation matrix between the theoretical and actual coordinate systems. This AC axis connection rigidity error compensation achieves calibration technology, which can improve the machining accuracy of the five-axis machine and still improve the usability of equipment with AC axis problems. It can be widely applied to various current five-axis machines. The description method and error compensation for connection rigidity can serve as a reference for mechanical rotating shaft connections. Summary of the Invention

[0005] The purpose of this invention is to provide a method for detecting the rigidity of the AC axis connection and compensating for errors in a cradle-type five-axis machine, in order to solve the above-mentioned problems.

[0006] This invention is mainly achieved through the following technical solutions:

[0007] A method for rigidity detection and error compensation of AC axis connection in a cradle-type five-axis machine includes a machine coordinate system and a reference coordinate system that can be converted between each other. The reference coordinate system is established with the laser emission position at the origin of the machine coordinate system as the origin. The method includes the following steps:

[0008] Step S100: Obtain the coordinate information of the circle rotating around the C-axis under different A values ​​in the reference coordinate system, and obtain the coordinate information of the circle rotating around the A-axis CircleA by fitting the center of the circle rotating around the C-axis under different A values;

[0009] Step S200: Obtain the AC axis connection rigidity from the coordinate information of the circle rotating around the C axis and the circle rotating around the A axis; establish an actual coordinate system for the machine tool under each A axis attitude, and use the transformation vector between the theoretical coordinate system and the actual coordinate system to describe the AC axis connection rigidity.

[0010] Step S300: Perform error compensation on the AC axis connection rigidity, perform error compensation on the rotation vectors of the theoretical coordinate system and the actual coordinate system under different A-axis postures, and realize the mutual conversion between the theoretical coordinate system TheoryCoordinate and the actual coordinate system Coordinate under different A-axis postures.

[0011] Step S400: Perform mechanical calibration of the five-axis machine.

[0012] To better realize the present invention, further, in step S100, a laser rangefinder is used to obtain the mechanical coordinates of the sphere's center, and the mechanical coordinates of the same point on the machine platform are obtained at different angles along the A-axis, with the C-axis rotated respectively. Let the mechanical coordinates of this point include the mechanical coordinates of the XYZ axes (coordXYZ). M =[X i ,Y i Z i ] T Mechanical coordinates coordAC of A-axis and C-axis M =[A i C i ] T The machine coordinates are transformed into the reference coordinate system to obtain the corresponding reference coordinates.

[0013] To better implement the present invention, step S200 further includes the following steps:

[0014] Step S210: Suppose that when rotating the A-axis a certain number of times by the same angle, several circles Circle1, Circle2, Circle3, ..., Circle are obtained, each rotating around the C-axis. m For each circle, there are corresponding center points cpt1, cpt2, cpt3, ..., cpt. m And the corresponding unit normal vectors cn1, cn2, cn3, ..., cn m The center of the circle in which the calibrator rotates around the C-axis when A is 0, cpt5, and the unit normal vector cn5 are used as references.

[0015] Step S220: Establish the actual coordinate system under different poses of the A-axis: For each point under each A-axis pose, in Circle1, Circle2, Circle3, ..., Circle... m In each reference frame, there is a calibration sphere with a C-axis coordinate of 0, and the reference frame coordinates are set as cPointInitial1, cPointInitial2, ..., cPointInitial m The center of the circle is cpt1, cpt2, cpt3, ..., cpt m To the corresponding coordinates cPointInitial1, cPointInitial2, ..., cPointInitial m The vectors are vecInitial1, vecInitial2, ..., vecInitial m For Circle1, establish a Cartesian coordinate system with cpt1 as the origin, vecInitial1 as the X-axis, and cn1 as the Z-axis, and set it as Coordinate1. Similarly, for Circle2, Circle3, ..., Circle... m Establish the corresponding Cartesian coordinate system Coordinate2, Coordinate3, ..., Coordinate m This refers to the actual coordinate system under each value of A;

[0016] Step S230: Obtain the theoretical coordinate system under different poses of axis A: Taking the actual coordinate system Coordinate5 established when A is 0 as the reference, the rotation angle of cpt5 to cpt1 around axis A is disAngle1, obtained from the circle information of Circle A. Then, rotating Coordinate5 around axis A by disAngle1 yields the theoretical coordinate system TheoryCoordinate1; then, rotating Coordinate5 around axis A by disAngle2, obtained from the circle information of Circle A, yields the theoretical coordinate system TheoryCoordinate2; similarly, rotating Coordinate5 around axis A yields a series of theoretical coordinate systems TheoryCoordinate1, TheoryCoordinate2, ..., TheoryCoordinate for different values ​​of A. m ;

[0017] Step S240: Establish the connection between the theoretical coordinate system and the actual coordinate system, and use a rotation vector to describe the connection rigidity of the AC axis: For Coordinate1 and TheoryCoordinate1, since the origin of both coordinate systems is cpt1, and both coordinate systems are Cartesian coordinate systems, a rotation matrix can be used to establish the connection between the two coordinate systems, calculated as follows:

[0018] Let the XYZ axes of the Coordinate1 coordinate system in the reference coordinate system be vectors e1, e2, and e3, respectively; and let the unit vectors of the TheoryCoordinate1 coordinate system in the reference coordinate system be e1, e2, and e3, respectively. t1 e t2 e t3 Let matR be the rotation matrix from the theoretical coordinate system TheoryCoordinate1 to the actual coordinate system Coordinate1. Then [e1 e2 e3] = matR × [e t1 e t2 e t3 ]; We get matR=[e1 e2 e3][e t1 e t2 e t3 ] -1 ;

[0019] The direction of the rotation vector is the axis around which the C-axis is deflected on the A-axis, and the magnitude of the rotation vector is the degree of deflection of the C-axis on the A-axis. Let rotateVec1 represent the transformation relationship between the theoretical and actual coordinate systems. The rotation matrix rotateMat can be converted into the rotation vector rotateVec1 using the Rodriguez formula. The conversion process is as follows:

[0020] Suppose there is a rotation with axis n and angle θ. Obviously, its corresponding rotation vector is θ. n That is, rotateVec1;

[0021] For the rotation angle θ, we have:

[0022]

[0023] Where R is the rotation matrix rotateMat,

[0024] Regarding the axis of rotation n, since the vectors on the axis of rotation do not change after rotation, this indicates that R... n =n, therefore, the rotation axis n is the eigenvector corresponding to the eigenvalue 1 of matrix R; solving this equation and then normalizing yields the rotation axis n and θ. n That is, the rotation vector;

[0025] The process of converting a rotation vector to a rotation matrix is ​​obtained using the Rodriguez formula, and the conversion formula is as follows:

[0026]

[0027] Where Ⅰ is the identity matrix,

[0028] nn T Let n be the basis vector of the rotation axis. ∧ The length of the mold,

[0029] Similarly, we can obtain rotateVec1, rotateVec2, ..., rotateVec m The connection rigidity of the AC axis can be described by the magnitude |rotateVec1| of the rotation vector.

[0030] To better realize the present invention, further, in step S300, let the actual coordinate system with the A-axis to be determined as a be Coordinate a , a is at two known angles angle 1M , angle 2M Between; given A = angle 1M Rotation vectors between the theoretical and actual coordinate systems: rotateVec1, A=angle 2M RotateVec2, the rotation vector between the theoretical and actual coordinate systems; solve for Coordinate. a The corresponding axis vectors of the XYZ axes are e a1 e a2 e a3 ;

[0031] First, we obtain the theoretical coordinate system with A=a degrees as TheoryCoordinate, and the corresponding axis vectors of the XYZ axes are e. T1 e T2 e T3 The actual rotation angle from coordinate 0 to coordinate a is set as θ; the reference coordinate system is Coordinate5, and the XYZ axis vectors are e1, e2, and e3 respectively. Now, let the direction vector of axis A be set as n. A Coordinate5 rotates about axis A by θ, with the rotation vector being θ. nA The transformation from rotation vector to rotation matrix yields θ. nA The corresponding rotation matrix is ​​set to matRA;

[0032] TheoryCoordinate = matRA × Coordinate5; the formula is [e T1 e T2 e T3]=matRA×[e T1 e T2 e T3 ] ;

[0033] Next, obtain the rotation vector between the theoretical and actual coordinate systems when A=a. Let this rotation vector be denoted as rotateVec. a Then rotateVec a =rotateVec1×(a-angle1 M ) + rotateVec2 × (angle2) M -a);

[0034] Use the Rodriguez formula to rotateVec a Convert to rotation matrix rotateMat a ;

[0035] The actual coordinate system of A=a is Coordinate a =rotateMat a ×TheoryCoordinate a ;

[0036] The formula is [e a1 e a2 e a3 ]=rotateMat a ×[e T1 e T2 e T3 ].

[0037] To better implement the present invention, further, in step S400, in order to ensure that the relative position of a specific point on the tool or workpiece remains unchanged in space, it is necessary to obtain the real-time coordinates of the specific point as it changes with the A-axis and C-axis. Assuming the coordinates (x, y, z, a, c) of one mechanical coordinate system are converted to coordinates (x1, y1, z1, a1, c1) of another mechanical coordinate system, the calibration steps are as follows:

[0038] Step S410: First, convert the machine coordinates (x, y, z) to reference coordinates (x, y, z). 参 ,y 参 ,z 参 );

[0039] Step S420: First, obtain the machine coordinate system 1 with A-axis in attitude a. Calculate the actual rotation angle of C-axis relative to 0, setting it as θ1. Rotate machine coordinate system 1 around the C-axis at this time by θ1 to obtain machine coordinate system 1'. Then obtain (x... 参 ,y 参 ,z 参The coordinates of (x', y', z') in the machine coordinate system 1'.

[0040] Step S430: Obtain the machine coordinate system 2 under attitude a1, calculate the actual rotation angle of the C-axis relative to 0 and set it as θ2, rotate the machine coordinate system 3 around the C-axis at this time by θ2 to obtain the machine coordinate system 2', and transfer the coordinates (x', y', z') under the machine coordinate system 2' to the reference coordinate system to obtain (x 参1 ,y 参1 ,z 参1 );

[0041] Step S440: Change the coordinates (x, y) in the reference coordinate system. 参1 ,y 参1 ,z 参1 Converting the coordinates to the machine coordinate system (x1, y1, z1) yields (x1, y1, z1, a1, c1).

[0042] Reference coordinate system: standard Cartesian rectangular coordinate system.

[0043] Positioning accuracy: For a specific translational axis (XYZ), high positioning accuracy means the axis moves the same distance when the same movement distance command is sent from different positions. A large deviation in the movement distance indicates insufficient positioning accuracy. Similarly, for a specific rotational axis (AC), high positioning accuracy means the axis rotates the same angle when the same movement distance command is sent from different positions. A large deviation in the rotation angle indicates insufficient positioning accuracy.

[0044] AC axis connection rigidity: For a cradle-type five-axis machine, the C axis is connected to the A axis. During the rotation of the A axis, the relative position of the C axis and the A axis remains unchanged. That is, a point on the C axis center rotates in the same direction as the A axis center. This means that the connection rigidity is good. Conversely, the connection rigidity is poor.

[0045] Error compensation: Error compensation is to artificially create a new original error to offset the original original error that is currently causing the problem. The two errors should be made as equal in magnitude and opposite in direction as possible, so as to reduce machining errors and improve machining accuracy.

[0046] The beneficial effects of this invention are as follows:

[0047] This invention is the first to propose using a transformation vector between the theoretical and actual coordinate systems to describe the connection rigidity of the AC axis. This invention only requires measuring the coordinates of a single point on the machine tool under different AC axis poses to generate relevant information, making it more convenient. This invention performs error compensation for the AC axis connection rigidity to achieve calibration, which can improve the machining accuracy of five-axis machines and still improve the usability of equipment with AC axis problems. It can be widely applied to various current five-axis machines. The described method of connection rigidity and error compensation can serve as a reference for mechanical rotating shaft connections. Attached Figure Description

[0048] Figure 1 This is an overall flowchart of the present invention;

[0049] Figure 2 This is a flowchart of the calibration process of this invention;

[0050] Figure 3 The AC axis connection rigidity test curve for an old-style five-axis machine;

[0051] Figure 4 This is the test curve for the AC axis connection rigidity of a new type of five-axis machine. Detailed Implementation

[0052] Example 1:

[0053] A method for detecting and compensating for the rigidity of the AC axis connection in a cradle-type five-axis machine includes a convertible mechanical coordinate system and a reference coordinate system. The reference coordinate system is established with the laser emission position at the origin of the mechanical coordinate system as the origin. AC axis connection rigidity: For a cradle-type five-axis machine, the closer a point on the C-axis center is to the C-axis center direction as the A-axis rotates, the better the connection rigidity; conversely, the connection rigidity is poor.

[0054] like Figure 1 As shown, the present invention includes the following steps:

[0055] Step S100: Obtain the coordinate information of the circle rotating around the C-axis under different A values ​​in the reference coordinate system, and obtain the coordinate information of the circle rotating around the A-axis CircleA by fitting the center of the circle rotating around the C-axis under different A values;

[0056] Step S200: Obtain the AC axis connection rigidity from the coordinate information of the circle rotating around the C axis and the circle rotating around the A axis; establish an actual coordinate system for the machine tool under each A axis attitude, and use the transformation vector between the theoretical coordinate system and the actual coordinate system to describe the AC axis connection rigidity.

[0057] Step S300: Perform error compensation on the AC axis connection rigidity, perform error compensation on the rotation vectors of the theoretical coordinate system and the actual coordinate system under different A-axis postures, and realize the mutual conversion between the theoretical coordinate system TheoryCoordinate and the actual coordinate system Coordinate under different A-axis postures.

[0058] Step S400: Perform five-axis machine calibration. In order to keep the relative position of a specific point on the tool or workpiece constant in space, it is necessary to obtain the real-time coordinates of the specific point as it changes with the A-axis and C-axis. Let the coordinates of one machine coordinate system (x,y,z,a,c) be transferred to the coordinates of another machine coordinate system (x1,y1,z1,a1,c1).

[0059] Preferably, in step S100, a laser rangefinder is used to obtain the mechanical coordinates of the sphere's center, and the mechanical coordinates of the same point on the machine platform are obtained at different angles along the A-axis, with the C-axis rotated. Let the mechanical coordinates of this point include the mechanical coordinates of the XYZ axes (coordXYZ). M =[X i ,Y i Z i ] T Mechanical coordinates coordAC of A-axis and C-axis M =[A i C i ] T The machine coordinates are transformed into the reference coordinate system to obtain the corresponding reference coordinates.

[0060] Preferably, step S200 includes the following steps:

[0061] Step S210: Suppose that when rotating the A-axis a certain number of times by the same angle, several circles Circle1, Circle2, Circle3, ..., Circle are obtained, each rotating around the C-axis. m For each circle, there are corresponding center points cpt1, cpt2, cpt3, ..., cpt. m And the corresponding unit normal vectors cn1, cn2, cn3, ..., cn m The center of the circle in which the calibrator rotates around the C-axis when A is 0, cpt5, and the unit normal vector cn5 are used as references.

[0062] Step S220: Establish the actual coordinate system under different poses of the A-axis: For each point under each A-axis pose, in Circle1, Circle2, Circle3, ..., Circle... m In each reference frame, there is a calibration sphere with a C-axis coordinate of 0, and the reference frame coordinates are set as cPointInitial1, cPointInitial2, ..., cPointInitialm The center of the circle is cpt1, cpt2, cpt3, ..., cpt m To the corresponding coordinates cPointInitial1, cPointInitial2, ..., cPointInitial m The vectors are vecInitial1, vecInitial2, ..., vecInitial m For Circle1, establish a Cartesian coordinate system with cpt1 as the origin, vecInitial1 as the X-axis, and cn1 as the Z-axis, and set it as Coordinate1. Similarly, for Circle2, Circle3, ..., Circle... m Establish the corresponding Cartesian coordinate system Coordinate2, Coordinate3, ..., Coordinate m This refers to the actual coordinate system under each value of A;

[0063] Step S230: Obtain the theoretical coordinate system under different poses of axis A: Taking the actual coordinate system Coordinate5 established when A is 0 as the reference, the rotation angle of cpt5 to cpt1 around axis A is disAngle1, obtained from the circle information of Circle A. Then, rotating Coordinate5 around axis A by disAngle1 yields the theoretical coordinate system TheoryCoordinate1; then, rotating Coordinate5 around axis A by disAngle2, obtained from the circle information of Circle A, yields the theoretical coordinate system TheoryCoordinate2; similarly, rotating Coordinate5 around axis A yields a series of theoretical coordinate systems TheoryCoordinate1, TheoryCoordinate2, ..., TheoryCoordinate for different values ​​of A. m ;

[0064] Step S240: Establish the connection between the theoretical coordinate system and the actual coordinate system, and use a rotation vector to describe the connection rigidity of the AC axis: For Coordinate1 and TheoryCoordinate1, since the origin of both coordinate systems is cpt1, and both coordinate systems are Cartesian coordinate systems, a rotation matrix can be used to establish the connection between the two coordinate systems, calculated as follows:

[0065] Let the XYZ axes of the Coordinate1 coordinate system in the reference coordinate system be vectors e1, e2, and e3, respectively; and let the unit vectors of the TheoryCoordinate1 coordinate system in the reference coordinate system be e1, e2, and e3, respectively.t1 e t2 e t3 Let matR be the rotation matrix from the theoretical coordinate system TheoryCoordinate1 to the actual coordinate system Coordinate1. Then [e1 e2 e3] = matR × [e t1 e t2 e t3 ]; We get matR=[e1 e2 e3][e t1 e t2 e t3 ] -1 ;

[0066] The direction of the rotation vector is the axis around which the C-axis is deflected on the A-axis, and the magnitude of the rotation vector is the degree of deflection of the C-axis on the A-axis. Let rotateVec1 represent the transformation relationship between the theoretical and actual coordinate systems. The rotation matrix rotateMat can be converted into the rotation vector rotateVec1 using the Rodriguez formula. The conversion process is as follows:

[0067] Suppose there is a rotation with axis n and angle θ. Obviously, its corresponding rotation vector is θ. n That is, rotateVec1;

[0068] For the rotation angle θ, we have:

[0069]

[0070] Where R is the rotation matrix rotateMat,

[0071] Regarding the rotation axis n, since the vectors on the rotation axis do not change after rotation, Rn = n. Therefore, the rotation axis n is the eigenvector corresponding to the eigenvalue 1 of matrix R. Solving this equation and then normalizing yields the rotation axis n and θ. n That is, the rotation vector;

[0072] The process of converting a rotation vector to a rotation matrix is ​​obtained using the Rodriguez formula, and the conversion formula is as follows:

[0073]

[0074] Where Ⅰ is the identity matrix,

[0075] nn T Let n be the basis vector of the rotation axis. ∧ The length of the mold,

[0076] Similarly, we can obtain rotateVec1, rotateVec2, ..., rotateVec mThe connection rigidity of the AC axis can be described by the magnitude |rotateVec1| of the rotation vector.

[0077] Preferably, in step S300, the actual coordinate system with the A-axis to be determined as 'a' is denoted as Coordinate. a , a is at two known angles angle 1M , angle 2M Between; given A = angle 1M Rotation vectors between the theoretical and actual coordinate systems: rotateVec1, A=angle 2M RotateVec2, the rotation vector between the theoretical and actual coordinate systems; solve for Coordinate. a The corresponding axis vectors of the XYZ axes are e a1 e a2 e a3 :

[0078] First, we obtain the theoretical coordinate system with A=a degrees as TheoryCoordinate, and the corresponding axis vectors of the XYZ axes are e. T1 e T2 e T3 The actual rotation angle from coordinate 0 to coordinate a is set as θ; the reference coordinate system is Coordinate5, and the XYZ axis vectors are e1, e2, and e3 respectively. Now, let the direction vector of axis A be set as n. A Coordinate5 rotates about axis A by θ, with the rotation vector being θ. nA The transformation from rotation vector to rotation matrix yields θ. nA The corresponding rotation matrix is ​​set to matRA;

[0079] TheoryCoordinate = matRA × Coordinate5; the formula is [e T1 e T2 e T3 ]=matRA×[e T1 e T2 e T3 ] ;

[0080] Next, obtain the rotation vector between the theoretical and actual coordinate systems when A=a. Let this rotation vector be denoted as rotateVec. a Then rotateVec a =rotateVec1×(a-angle1 M ) + rotateVec2 × (angle2) M -a);

[0081] Use the Rodriguez formula to rotateVec a Convert to rotation matrix rotateMat a ;

[0082] The actual coordinate system of A=a is Coordinate a =rotateMat a ×TheoryCoordinate a ;

[0083] The formula is [e a1 e a2 e a3 ]=rotateMat a ×[e T1 e T2 e T3 ].

[0084] Preferably, such as Figure 2 As shown, the calibration steps in step S400 are as follows:

[0085] Step S410: First, convert the machine coordinates (x, y, z) to reference coordinates (x, y, z). 参 ,y 参 ,z 参 );

[0086] Step S420: First, obtain the machine coordinate system 1 with A-axis in attitude a. Calculate the actual rotation angle of C-axis relative to 0, setting it as θ1. Rotate machine coordinate system 1 around the C-axis at this time by θ1 to obtain machine coordinate system 1'. Then obtain (x... 参 ,y 参 ,z 参 The coordinates of (x', y', z') in the machine coordinate system 1'.

[0087] Step S430: Obtain the machine coordinate system 2 under attitude a1, calculate the actual rotation angle of the C-axis relative to 0 and set it as θ2, rotate the machine coordinate system 3 around the C-axis at this time by θ2 to obtain the machine coordinate system 2', and transfer the coordinates (x', y', z') under the machine coordinate system 2' to the reference coordinate system to obtain (x 参1 ,y 参1 ,z 参1 );

[0088] Step S440: Change the coordinates (x, y) in the reference coordinate system. 参1 ,y 参1 ,z 参1 Converting the coordinates to the machine coordinate system (x1, y1, z1) yields (x1, y1, z1, a1, c1).

[0089] This invention is the first to propose using a transformation vector between the theoretical and actual coordinate systems to describe the connection rigidity of the AC axis. This invention only requires measuring the coordinate information of the machine tool under different AC axis poses to determine the AC axis connection rigidity, making it convenient to operate and highly practical. This invention performs error compensation for AC axis connection rigidity to achieve calibration, which can improve the machining accuracy of five-axis machines and still improve the use of equipment with AC axis problems. It can be widely applied to various current five-axis machines. The described method of connection rigidity and error compensation can serve as a reference for mechanical rotating shaft connections.

[0090] Example 2:

[0091] A method for detecting and compensating for the connection rigidity of the AC axis in a cradle-type five-axis machine is disclosed. The premise of this invention is that the mechanical XYZ axes and the reference Cartesian coordinate system can be mutually converted. The principle is to obtain the circle information in the reference coordinate system for each circle rotating around the C axis at the same point under different A values, and then detect the connection rigidity of the AC axis. Finally, error compensation is performed on the AC axis connection rigidity to complete the five-axis machine calibration, laying the foundation for five-axis linkage machining. Currently, a laser rangefinder is used to obtain the mechanical coordinates of the sphere center, but other measurement methods can also be used.

[0092] This invention utilizes the geometric information of a point on the machine tool rotating around the C-axis in a reference coordinate system to detect the connection rigidity of the AC axis. It proposes a method of describing this by comparing theoretical and actual normal vectors and using the transformation matrix between the theoretical and actual coordinate systems. This method can be applied to the detection of the AC axis in various scenarios for five-axis machines, including factory inspection, in-use problem detection, and repair inspection. Error compensation for the AC axis connection rigidity is implemented to achieve calibration technology, which can serve as a reference for mechanical rotating shaft connections.

[0093] I. Purpose of the designation.

[0094] A reference coordinate system is established with the laser emission position at the origin of the machine coordinate system as the origin. This establishes the connection between the machine coordinate system and the reference coordinate system, as well as between the reference coordinate system and the workpiece coordinate system, forming the basis for five-axis linkage machining. Based on the information obtained above, a tracking transformation (a→a1, c→c1) can be achieved from one machine coordinate (x, y, z, a, c) to another coordinate (x1, y1, z1, a1, c1). x1, y1, and z1 are calculated, ensuring that the relative position of a specific point on the tool or workpiece remains unchanged in space.

[0095] II. Mechanical structure of the five-axis dispensing machine.

[0096] The cradle-type five-axis machine tool mainly consists of a machine tool base, three translational axes (X-axis, Y-axis, and Z-axis), and two rotary axes (A-axis and C-axis). Through the connections between the machine tool base and each axis, and the connections between the axes themselves, a machining motion chain is formed, achieving five-axis linkage. During the motion, based on the principles of robot kinematics, the machine tool base, motion axes, needle valve, workpiece, and camera of the five-axis dispensing machine are all considered rigid bodies, thus dividing the aforementioned rigid mechanical structure into two machining motion chains.

[0097] 1. Machine tool base → X-axis → Z-axis → needle.

[0098] 2. Machine tool base → Y-axis → A-axis → C-axis → workpiece coordinate system.

[0099] III. The detailed steps and related principles are as follows.

[0100] 1. Measurement Data. The current measurement uses a laser rangefinder to obtain the mechanical coordinates of the sphere's center. This yields a series of mechanical coordinates for the same point on the machine platform at different angles along axis A, with axis C rotated. Let the mechanical coordinates of this point include the mechanical coordinates of the XYZ axes (coordXYZ). M =[X i ,Y i Z i ] T The machine coordinates coordAC of the AC axis M =[A i C i ] T Note: After establishing the connection between the mechanical XYZ axis coordinate system and the reference coordinate system, all measured points are converted to reference coordinates. All data discussed below will be in the reference coordinate system.

[0101] 2. Obtain the geometric information of the AC axis through fitting. The rotation circles around C under different orientations of the A axis can be obtained in the reference coordinate system. The AC axis information can be obtained from the circle information. Current testing on existing five-axis machines has revealed problems with poor detection of AC axis connection rigidity and inadequate AC axis connection rigidity.

[0102] To facilitate the discussion in the following sections, the following specific example is provided:

[0103] Let the circles rotated around the C-axis be Circle1, Circle2, Circle3, Circle4, Circle5, Circle6, Circle7, Circle8, and Circle9, respectively, when the A-axis is -40, -30, -20, -10, 0, 10, 20, 30, and 40 degrees. Each circle has a corresponding center cpt1, cpt2, cpt3...cpt9, and a corresponding unit normal vector cn1, cn2, cn3,...cn9.

[0104] 3. Check the rigidity of the AC axis connection. Assuming the AC axis connection is rigid, the rotation of the previously mentioned points cpt1, cpt2, cpt3...cpt9 and vectors cn1, cn2, cn3,...cn9 around the A axis should be consistent, because these points or vectors rotate around the A axis by the same angle.

[0105] For example, from the information of Circle A, we can obtain the rotation angle of cpt5 to cpt1 around axis A as disAngle1. Then, the theoretical normal vector cnTheory1 is obtained by rotating cn5 around axis A by disAngle1. cnTheory1 should be the same as cn1. However, due to the poor connection rigidity of some five-axis devices, there is a significant difference between cn1 and cnTheory1.

[0106] Using cpt5 and cn5 (the center of the circle around the C-axis and the unit normal vector when A is 0) as a reference, a series of theoretical unit normal vectors cnTheory1, cnTheory2, ..., cnTheory9 with different values ​​of A can be obtained in the above manner. By comparing them with the corresponding actual normal vectors cn1, cn2, cn3, ..., cn9, the connection rigidity of the AC axis can be detected.

[0107] The actual and theoretical normal vectors described above can describe the connection stiffness of the AC axis to some extent. However, they cannot accurately describe the specific connection deflection of the AC axis as it changes with the A axis, or even torsion (due to the machine rotating around the C axis, not caused by sending C-axis motion commands). To better describe the connection stiffness, similar to the theoretical and actual normal vectors above, an actual coordinate system is established for the machine at each A-axis orientation. The transformation vector (transformation matrix) between the theoretical and actual coordinate systems is used to describe the connection stiffness of the AC axis.

[0108] The specific process is as follows:

[0109] 1. Establish the actual coordinate system under different poses of the A-axis.

[0110] For each point in the aforementioned Circle1, Circle2, Circle3, ..., Circle9, there is a calibration sphere reference system with a C-axis coordinate of 0, denoted as cPointInitial1, cPointInitial2, ..., cPointInitial9. The vector from the center cpt1 to the corresponding coordinate cPointInitial1 is vecInitial1, the vector from the center cpt2 to the corresponding coordinate cPointInitial2 is vecInitial2, and the vector from the center cpt9 to the corresponding coordinate cPointInitial9 is vecInitial9. For Circle1, a Cartesian coordinate system is established with cpt1 as the origin, vecInitial1 as the X-axis, and cn1 as the Z-axis, and designated as Coordinate1. Similarly, corresponding Cartesian coordinate systems are established for Circle2, Circle3, ..., Circle9, designated as Coordinate2, Coordinate3, ..., Coordinate9. The established Coordinate1, Coordinate2, ..., Coordinate9 are the actual coordinate systems for each A-value.

[0111] 2. Obtain the theoretical coordinate system for each A value.

[0112] Using Coordinate5 (the actual coordinate system established when A is 0) as the reference, the rotation angle of cpt5 to cpt1 around the A-axis is disAngle1, obtained from the information of Circle A. Rotating Coordinate5 around the A-axis by disAngle1 yields the theoretical coordinate system TheoryCoordinate1. Similarly, the rotation angle of cpt5 to cpt2 around the A-axis is disAngle2, obtained from the information of Circle A. Rotating Coordinate5 around the A-axis by disAngle2 yields the theoretical coordinate system TheoryCoordinate2. Likewise, rotating Coordinate5 around the A-axis yields a series of theoretical coordinate systems TheoryCoordinate1, TheoryCoordinate2, ..., TheoryCoordinate9 for different values ​​of A.

[0113] 3. Establish the connection between the theoretical coordinate system and the actual coordinate system, and use the rotation vector to describe the connection rigidity problem of the AC axis.

[0114] For example, for Coordinate1 and TheoryCoordinate1, since the origin of both coordinate systems is cpt1 and both coordinate systems are Cartesian coordinate systems, a rotation matrix can be used to establish the relationship between the two coordinate systems, calculated as follows:

[0115] Let the XYZ axes of the Coordinate1 coordinate system in the reference coordinate system be vectors e1, e2, and e3, respectively, and let the unit vectors of the TheoryCoordinate1 coordinate system in the reference coordinate system be e1, e2, and e3, respectively. t1 e t2 e t3 Let matR be the rotation matrix from the theoretical coordinate system TheoryCoordinate1 to the actual coordinate system Coordinate1. Then [e1 e2 e3] = matR × [e t1 e t2 e t3 ], thus obtaining matR=[e1 e2 e3][e t1 e t2 e t3 ] -1 .

[0116] To intuitively express the positional relationship between coordinate systems TheoryCoordinate1 and Coordinate1, using a rotation vector is more appropriate. This is because when the connection rigidity is poor, the deflection of the C-axis on the A-axis should gradually change with the angle of the A-axis and have directionality. The direction of the rotation vector is the axis around which the C-axis deflects on the A-axis, and the magnitude of the rotation vector is the degree of deflection of the C-axis on the A-axis. Let a rotation vector rotateVec1 represent the transformation relationship between the two coordinate systems. The Rodriguez formula can be used to convert the rotation matrix rotateMat to rotateVec1. The conversion process between the rotation matrix and the rotation vector is as follows:

[0117] Suppose there is a rotation with axis n and angle θ. Obviously, its corresponding rotation vector is θ. n (That is, rotateVec1, where n is a unit vector).

[0118] The transformation from a rotation matrix to a rotation vector is as follows:

[0119] For the rotation angle θ, we have: R stands for rotateMat;

[0120] Regarding the axis of rotation n, since the vectors on the axis of rotation do not change after rotation, this indicates that R...n =n, therefore, the rotation axis n is the eigenvector corresponding to the eigenvalue 1 of matrix R. Solving this equation and then normalizing it yields the rotation axis n. θ n That is, the rotation vector.

[0121] The process of converting a rotation vector to a rotation matrix is ​​obtained by the Rodriguez formula, specifically:

[0122] The conversion formula is:

[0123] .

[0124] The symbol ∧ is the conversion symbol for vectors to antisymmetric vectors.

[0125] Similarly, rotateVec1, rotateVec2, ..., rotateVec9 can be obtained. The rigidity of the AC axis connection can be described by the magnitude of the rotation vector |rotateVec1|.

[0126] 4. Compensate for the rigidity of the AC axis connection to obtain the actual coordinate system of the machine tool under different A-axis postures.

[0127] Because the AC axis connection is not rigid, the actual coordinate system Coordinate1, Coordinate2, ..., Coordinate9 established earlier is different from the theoretical coordinate system TheoryCoordinate1, TheoryCoordinate2, ..., TheoryCoordinate9 obtained by rotating the theoretical reference coordinate system (the coordinate system when A=0) around the A axis by a corresponding angle. To achieve mutual conversion between the theoretical coordinate system TheoryCoordinate and the actual coordinate system Coordinate under different A-axis orientations, error compensation can be performed on the rotation vectors of the theoretical coordinate system and the actual coordinate system.

[0128] The reason is that when there is a problem with the connection rigidity of the AC axis, the rotation direction (tilt direction) of the C axis relative to the A axis changes gradually with the change of the A axis angle, and this change is regular. For example, as the A axis angle changes, the deflection of the C axis relative to the A axis connection rigidity always increases slowly in one direction. This deflection, as mentioned earlier, is described by the rotation vector rotateVec. The compensation method is as follows:

[0129] During compensation, it is assumed that the measured adjacent angle rotation vectors change uniformly.

[0130] For example: Let the coordinate system to be determined be the actual coordinate system with axis A as a. a 'a' is at two angles that have already been calculated. 1M , angle 2Mbetween.

[0131] Now Coordinate is required a The corresponding axis vectors of the XYZ axes are e a1 e a2 e a3 Given A = angle 1M Rotation vectors between the theoretical and actual coordinate systems: rotateVec1, A=angle 2M The rotation vector between the theoretical and actual coordinate systems is rotateVec2. The specific process is as follows:

[0132] First, we obtain the theoretical coordinate system with A=a degrees as TheoryCoordinate, and the corresponding axis vectors of the XYZ axes are e. T1 e T2 e T3 Let θ be the actual rotation angle from coordinate 0 to coordinate a. The reference coordinate system is Coordinate5 (the actual coordinate system when A=0), and the XYZ axis vectors are e1, e2, and e3, respectively. Now, let n be the direction vector of axis A. A Coordinate5 rotates about axis A by θ, with the rotation vector being θ. nA From the transformation from rotation vector to rotation matrix, we can obtain θ. nA The corresponding rotation matrix is ​​set as matRA; TheoryCoordinate = matRA × Coordinate5; the formula is [e T1 e T2 e T3 ]=matRA×[e T1 e T2 e T3 ];

[0133] Then obtain the rotation vector between the theoretical and actual coordinate systems when A=a. Let this rotation vector be denoted as rotateVec. a Then rotateVec a =rotateVec1×(a-angle 1M ) + rotateVec2 × (angle2) M -a).

[0134] Use the Rodriguez formula to rotateVec a Convert to rotation matrix rotateMat a .

[0135] The actual coordinate system of A=a is Coordinatea =rotateMat a ×TheoryCoordinate a .

[0136] The formula is [e a1 e a2 e a3 ]=rotateMat a ×[e T1 e T2 e T3 ].

[0137] 5. Achieve mechanical calibration.

[0138] The above assumes that the machine coordinate system and the reference coordinate system can be converted to each other, realizing the conversion of the AC axis machine coordinate rotation angle to the actual rotation angle in the reference coordinate system space, and simultaneously obtaining the real-time coordinate system of the machine tool coordinate system after the AC axis changes. In order to keep the relative position of a specific point on the tool or workpiece constant in space, it is necessary to obtain the real-time coordinates of that specific point as the AC axis changes.

[0139] Suppose a machine coordinate changes from one machine coordinate (x, y, z, a, c) to another coordinate (x1, y1, z1, a1, c1), a→a1, c→ac1, and then x1, y1, z1 are calculated, such as... Figure 2 As shown, the specific process is as follows:

[0140] (1) First, convert the machine coordinates (x, y, z) to reference coordinates (x, y, z). 参 ,y 参 ,z 参 );

[0141] (2) First, obtain the machine coordinate system 1 with A-axis in attitude a. Calculate the actual rotation angle of C-axis relative to 0, setting it as θ1. Rotate coordinate system 1 around the C-axis (Z-axis of coordinate system 1) at this time by θ1 to obtain coordinate system 1'. Then, obtain (x... 参 ,y 参 ,z 参 The coordinates of (x', y', z') in coordinate system 1'.

[0142] (3) Obtain the machine coordinate system 2 under attitude a1, calculate the actual rotation angle of the C-axis relative to 0 and set it as θ2, rotate coordinate system 3 around the C-axis (Z-axis of coordinate system 3) by θ2 to obtain coordinate system 2', and transfer the coordinates (x', y', z') under machine coordinate system 2' to the reference coordinate system to obtain (x 参1 ,y 参1 ,z 参1 );

[0143] (4) The coordinates (x) in the reference coordinate system 参1 ,y 参1 ,z 参1 Converting y1 to machine coordinates (x1, y1, z1) yields (x1, y1, z1, a1, c1).

[0144] IV. Result Verification.

[0145] The AC axis connection rigidity test curve displays the rigidity of the AC axis connection. It is represented by the deviation between the theoretical and actual unit normal vectors of the rotation circle around C at different A-axis angles. The horizontal axis represents the mechanical coordinate values ​​of the A-axis, and the vertical axis represents the deviation between the theoretical and actual unit normal vectors of the rotation circle around C. Different broken lines represent the differences in different XYZ components. It is known that the AC axis connection rigidity accuracy of older five-axis machines is worse than that of newer five-axis machines. (Comparison...) Figure 3 , Figure 4 It was found that the AC axis rigid connection of the old-style five-axis machine was poor, which verified that the detection results of the present invention have better accuracy. Figure 3 As shown, the deviation of the normal vector under different values ​​of A indicates that as the rotation angle of the A-axis increases, the C-axis tends to tilt to one side. Figure 4 The AC axis connection of the corresponding five-axis machine has good rigidity.

[0146] Actual machine verification shows that after mechanical calibration is performed in the above manner, the relative position of a specific point on the tool or workpiece in space remains unchanged from one mechanical coordinate (x,y,z,a,c) to another coordinate (x1,y1,z1,a1,c1). For machines with good AC axes, this can achieve very high accuracy, while for machines with poor AC axes, it can also greatly improve calibration accuracy.

[0147] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any way. Any simple modifications or equivalent changes made to the above embodiments based on the technical essence of the present invention shall fall within the protection scope of the present invention.

Claims

1. A method for rigidity detection and error compensation of the AC axis connection of a cradle-type five-axis machine, comprising a convertible machine coordinate system and a reference coordinate system, wherein the reference coordinate system is established with the laser emission position at the origin of the machine coordinate system as the origin, characterized in that, Includes the following steps: Step S100: Obtain the coordinate information of the circle rotating around the C-axis under different A values ​​in the reference coordinate system, and obtain the coordinate information of the circle rotating around the A-axis CircleA by fitting the center of the circle rotating around the C-axis under different A values; Step S200: Obtain the AC axis connection rigidity from the coordinate information of the circle rotating around the C axis and the circle rotating around the A axis; establish an actual coordinate system for the machine tool under each A axis attitude, and use the transformation vector between the theoretical coordinate system and the actual coordinate system to describe the AC axis connection rigidity. Step S200 includes the following steps: Step S210: Suppose that when rotating the A-axis a certain number of times by the same angle, several circles Circle1, Circle2, Circle3, ..., Circle are obtained, each rotating around the C-axis. m For each circle, there are corresponding center points cpt1, cpt2, cpt3, ..., cpt. m And the corresponding unit normal vectors cn1, cn2, cn3, ..., cn m The center of the circle in which the calibrator rotates around the C-axis when A is 0, cpt5, and the unit normal vector cn5 are used as references. Step S220: Establish the actual coordinate system under different poses of the A-axis: For each point under each A-axis pose, in Circle1, Circle2, Circle3, ..., Circle m In each reference frame, there is a calibration sphere with a C-axis coordinate of 0, and the reference frame coordinates are set as cPointInitial1, cPointInitial2, ..., cPointInitial m The center of the circle is cpt1, cpt2, cpt3, ..., cpt m To the corresponding coordinates cPointInitial1, cPointInitial2, ..., cPointInitial m The vectors are vecInitial1, vecInitial2, ..., vecInitial m For Circle1, establish a Cartesian coordinate system with cpt1 as the origin, vecInitial1 as the X-axis, and cn1 as the Z-axis, and set it as Coordinate1. Similarly, for Circle2, Circle3, ..., Circle... m Establish the corresponding Cartesian coordinate system Coordinate2, Coordinate3, ..., Coordinate m This refers to the actual coordinate system under each value of A; Step S230: Obtain the theoretical coordinate system under different poses of axis A: Taking the actual coordinate system Coordinate5 established when A is 0 as the reference, the rotation angle of cpt5 to cpt1 around axis A can be obtained from the circle A information as disAngle1. Then, rotating Coordinate5 around axis A by disAngle1 yields the theoretical coordinate system TheoryCoordinate1; then, the rotation angle of cpt5 to cpt2 around axis A can be obtained from the circle A information as disAngle2. Then, rotating Coordinate5 around axis A by disAngle2 yields the theoretical coordinate system TheoryCoordinate2; similarly, rotating Coordinate5 around axis A yields a series of theoretical coordinate systems TheoryCoordinate1, TheoryCoordinate2, ..., TheoryCoordinate for different values ​​of A. m ; Step S240: Establish the connection between the theoretical coordinate system and the actual coordinate system, and use a rotation vector to describe the connection rigidity of the AC axis: For Coordinate1 and TheoryCoordinate1, since the origin of both coordinate systems is cpt1, and both coordinate systems are Cartesian coordinate systems, a rotation matrix can be used to establish the connection between the two coordinate systems, calculated as follows: Let the XYZ axes of the Coordinate1 coordinate system in the reference coordinate system be vectors e1, e2, and e3, respectively; and let the unit vectors of the TheoryCoordinate1 coordinate system in the reference coordinate system be e1, e2, and e3, respectively. t1 e t2 e t3 Let matR be the rotation matrix from the theoretical coordinate system TheoryCoordinate1 to the actual coordinate system Coordinate1. Then [e1 e2 e3] = matR × [e t1 e t2 e t3 ]; We get matR=[e1 e2 e3][e t1 e t2 e t3 ] -1 ; The direction of the rotation vector is the axis around which the C-axis is deflected on the A-axis, and the magnitude of the rotation vector is the degree of deflection of the C-axis on the A-axis. Let rotateVec1 represent the transformation relationship between the theoretical and actual coordinate systems. The rotation matrix rotateMat can be converted into the rotation vector rotateVec1 using the Rodriguez formula. The conversion process is as follows: Suppose there is a rotation with axis n and angle θ. Obviously, its corresponding rotation vector is θ. n That is, rotateVec1; For the rotation angle θ, we have: Where R is the rotation matrix rotateMat, Regarding the axis of rotation n, since the vectors on the axis of rotation do not change after rotation, this indicates that R... n =n, therefore, the rotation axis n is the eigenvector corresponding to the eigenvalue 1 of matrix R; solving this equation and then normalizing yields the rotation axis n and θ. n That is, the rotation vector; The process of converting a rotation vector to a rotation matrix is ​​obtained using the Rodriguez formula, and the conversion formula is as follows: Where Ⅰ is the identity matrix, nn T Let n be the basis vector of the rotation axis. ∧ The length of the mold, Similarly, we can obtain rotateVec1, rotateVec2, ..., rotateVec m The connection rigidity of the AC axis can be described by the magnitude |rotateVec1| of the rotation vector. Step S300: Perform error compensation on the AC axis connection rigidity, perform error compensation on the rotation vectors of the theoretical coordinate system and the actual coordinate system under different A-axis postures, and realize the mutual conversion between the theoretical coordinate system TheoryCoordinate and the actual coordinate system Coordinate under different A-axis postures. In step S300, let the actual coordinate system with the A-axis to be determined as 'a' be Coordinate. a , a is at two known angles angle 1M , angle 2M Between; given A = angle 1M Rotation vectors between the theoretical and actual coordinate systems: rotateVec1, A=angle 2M RotateVec2, the rotation vector between the theoretical and actual coordinate systems; solve for Coordinate. a The corresponding axis vectors of the XYZ axes are e a1 e a2 e a3 : First, we obtain the theoretical coordinate system with A=a degrees as TheoryCoordinate, and the corresponding axis vectors of the XYZ axes are e. T1 e T2 e T3 The actual rotation angle from coordinate 0 to coordinate a is set as θ; the reference coordinate system is Coordinate5, and the XYZ axis vectors are e1, e2, and e3 respectively. Now, let the direction vector of axis A be set as n. A Coordinate5 rotates about axis A by θ, with the rotation vector being θ. nA The transformation from rotation vector to rotation matrix yields θ. nA The corresponding rotation matrix is ​​set to matRA; TheoryCoordinate=matRA×Coordinate5 Official [e T1 e T2 e T3 ]=matRA×[e T1 e T2 e T3 ] Next, obtain the rotation vector between the theoretical and actual coordinate systems when A=a. Let this rotation vector be denoted as rotateVec. a ,but rotateVec a =rotateVec1×(a-angle1 M )+rotateVec2×(angle2 M -a) Use the Rodriguez formula to rotateVec a Convert to rotation matrix rotateMat a The actual coordinate system of A=a is Coordinate a =rotateMat a ×TheoryCoordinate a Official [e a1 e a2 e a3 ]=rotateMat a ×[e T1 e T2 e T3 ]; Step S400: Perform mechanical calibration of the five-axis machine.

2. The method for detecting and compensating for the rigidity of the AC axis connection of a cradle-type five-axis machine according to claim 1, characterized in that, In step S100, a laser rangefinder is used to obtain the mechanical coordinates of the sphere's center, and the mechanical coordinates of the same point on the machine tool are obtained at different angles along the A-axis, with the C-axis rotated. Let the mechanical coordinates of this point include the mechanical coordinates of the XYZ axes (coordXYZ). M =[X i ,Y i Z i ] T The machine coordinates coordAC of the A-axis and C-axis M =[A i C i ] T The machine coordinates are transformed into the reference coordinate system to obtain the corresponding reference coordinates.

3. A method for detecting and compensating for the rigidity of the AC axis connection of a cradle-type five-axis machine according to claim 1 or 2, characterized in that, In step S400, to ensure that the relative position of a specific point on the tool or workpiece remains constant in space, it is necessary to obtain the real-time coordinates of that specific point as it changes with the A-axis and C-axis. Assuming the coordinates (x, y, z, a, c) in one machine coordinate system are converted to (x1, y1, z1, a1, c1) in another machine coordinate system, the calibration steps are as follows: Step S410: First, convert the machine coordinates (x, y, z) to reference coordinates (x, y, z). 参 ,y 参 ,z 参 ); Step S420: First, obtain the machine coordinate system 1 with A-axis in attitude a. Calculate the actual rotation angle of C-axis relative to 0, setting it as θ1. Rotate machine coordinate system 1 around the C-axis at this time by θ1 to obtain machine coordinate system 1'. Then obtain (x... 参 ,y 参 ,z 参 The coordinates (x', y', z') of the machine coordinate system 1'. Step S430: Obtain the machine coordinate system 2 under attitude a1, calculate the actual rotation angle of the C-axis relative to 0 and set it as θ2, rotate the machine coordinate system 3 around the C-axis at this time by θ2 to obtain the machine coordinate system 2', and transfer the coordinates (x', y', z') under the machine coordinate system 2' to the reference coordinate system to obtain (x 参1 ,y 参1 ,z 参1 ); step S440: The coordinates (x) in the reference coordinate system... 参1 ,y 参1 ,z 参1 Converting the coordinates to (x1, y1, z1) in the machine coordinate system, we get (x1, y1, z1, a1, c1).

Images