CN110125982B - Orthogonality measurement method of motion trajectory of micro-manipulated robot three-degree-of-freedom motion control system - Google Patents

Abstract

The invention relates to a motion trajectory orthogonality measurement method of a three-degree-of-freedom motion control system of a micro-manipulation robot, in particular to a motion trajectory orthogonality evaluation method and a motion trajectory non-orthogonality correction method using the motion control system. The method mainly includes the following steps: constructing a motion trajectory orthogonality measurement system, analyzing the orthogonality of a standard orthogonal reference frame, collecting motion trajectories, linear fitting of motion trajectories, evaluating motion trajectory orthogonality, and correcting motion trajectory non-orthogonality . The invention solves the problem of non-orthogonal motion trajectories of the motion control system, realizes accurate positioning under the orthogonal condition, and effectively ensures the positioning accuracy of the micro-manipulation robot.

Description

Orthogonality measurement of motion trajectories of three-degree-of-freedom motion control systems for micromanipulators method

technical field

The invention relates to a method for measuring the orthogonality of motion trajectories of a three-degree-of-freedom motion control system of a micro-manipulation robot, in particular to a method for evaluating the orthogonality of motion trajectories of a motion control system and a method for correcting the non-orthogonality of motion trajectories, so as to realize the orthogonality of motion trajectories. The accurate positioning of the micro-manipulation robot can effectively ensure the positioning accuracy of the micro-manipulation robot.

Background technique

Micro-manipulation robots are operating systems that grasp, transfer, and assemble tiny objects (such as biological tissues, cells, MEMS microstructures, micro-electromechanical systems, etc.) , used in micro-assembly, micro-injection, bioengineering, minimally invasive surgery and other fields, it usually consists of three parts: vision system, micromanipulator and motion control system. The vision system constructed with the optical stereo microscope as the main body is combined with the micro-manipulation to form a special kind of micro-manipulation robot. The position information of the object is then driven by the motion control system to move the micromanipulator to complete various micromanipulation precision operations.

The motion control system is an important component of the micro-manipulation robot. It mainly realizes the motion control and precise positioning of the micro-manipulator. Its performance affects the positioning accuracy of the micro-manipulation robot. It usually includes multiple degrees of freedom, mainly composed of translational degrees of freedom and rotation. The three-degree-of-freedom motion control system is the most common type. The performance indicators of the motion control system with three translational degrees of freedom mainly include three categories: assembly orthogonality, positioning accuracy and three-axis motion trajectory orthogonality. Assembly orthogonality can be evaluated by angle measurement, and positioning accuracy can be evaluated by laser interferometric ranging. These two types of indicators are relatively easy to measure; it is difficult to evaluate the orthogonality of three-axis motion trajectories. There are micro-manipulation studies that are often ignored, but such indicators are important factors that affect the precise positioning of micro-manipulation robots. Sun Yanbo et al. (2013) proposed an evaluation index and a test method for the assembly orthogonality of the motion control system, mainly measuring the parallelism, perpendicularity and straightness of the motion control system assembly. In actual use, although the motion control system has good assembly orthogonality, it cannot guarantee that the motion trajectory of the three axes has good orthogonality, that is, the motion trajectory of the three axes does not necessarily satisfy the absolute orthogonal relationship. If the motion trajectory of the motion control system is non-orthogonal, no matter how accurate the object space point coordinates obtained by the visual calculation are, the accuracy of the object space positioning cannot be guaranteed, resulting in the actual position change of the micromanipulator and the calculated position change. The quantity is inconsistent, and the deviation of the two types of positions is affected by the orthogonality of the motion trajectory of the motion control system. In view of the above problems, the present invention proposes a method for measuring the orthogonality of the motion trajectory of a three-degree-of-freedom motion control system of a micro-manipulation robot. For the micro-manipulation robot, the present invention has special significance, as shown in: (1) the established laser The displacement positioning system can be used as a standard orthogonal reference system, which can provide necessary basic data for the orthogonality measurement of the motion control system motion trajectory; (2) the invented motion trajectory orthogonality measurement method is the motion trajectory of the micro-manipulation robot motion control system. Orthogonality evaluation provides the basis; (3) The established motion trajectory non-orthogonality correction method can effectively ensure the positioning accuracy of the micro-manipulation robot.

SUMMARY OF THE INVENTION

The invention proposes a method for measuring the orthogonality of the motion trajectory of a three-degree-of-freedom motion control system of a micro-manipulation robot. It can effectively ensure the positioning accuracy of the micro-manipulation robot.

The method for measuring the orthogonality of the motion trajectory of the three-degree-of-freedom motion control system of the micro-manipulation robot involved in the present invention is based on the principle of laser displacement measurement, constructs a motion trajectory orthogonality measurement system, establishes an auxiliary standard orthogonal reference system, and establishes a simulation method to carry out Orthogonality analysis of the standard orthogonal reference system, collecting motion trajectories in the standard orthogonal reference system, fitting the motion trajectories with straight lines, then establishing the orthogonality evaluation method of motion trajectories, and finally carrying out motion trajectories for non-orthogonal motion trajectories Non-orthogonality correction to achieve accurate positioning in the case of orthogonality. The method for measuring the orthogonality of the motion trajectory of the three-degree-of-freedom motion control system of the micro-manipulation robot comprises the following steps:

1) Construct a motion trajectory orthogonality measurement system

The three motion axes of the motion control system are respectively provided with grating scales for closed-loop precision control of the motion axes. In order to collect the motion trajectory of the motion axis, a standard gauge block is placed on the motion control system, and its three adjacent surfaces are used as displacement monitoring surfaces, and the normals of the three displacement monitoring surfaces are parallel to the three motion axes respectively. The central axes of the three laser displacement sensors intersect at one point and are perpendicular to the corresponding displacement monitoring surface, and are used to measure the displacement of the standard gauge block. The laser displacement positioning system established by three laser displacement sensors with orthogonal relationship is used as an auxiliary standard orthogonal reference frame, and the motion trajectory of the standard gauge block is described in the standard orthogonal reference frame.

2) Orthogonality Analysis of Standard Orthogonal Reference System

Affected by assembly factors, the central axes of the three laser displacement sensors do not necessarily satisfy the absolute orthogonal relationship, and the established non-orthogonal standard orthogonal reference system has an impact on the orthogonality measurement of the motion trajectory. How the rotation angle of the displacement sensor affects the measurement of the orthogonality of the motion trajectory A simulation method is established to analyze the influence of the orthogonality of the standard orthogonal reference frame on the measurement of the orthogonality of the motion trajectory. The simulation results show that when the rotation angle between the standard gauge block and the laser displacement sensor is within 10 degrees, the influence of the orthogonality of the standard orthogonal reference frame on the measurement of the orthogonality of the motion trajectory can be ignored. Therefore, when assembling the standard gauge block and the laser displacement sensor, adjust the rotation angle to within 10 degrees, and then proceed to step 3).

3) Collect motion trajectory

The standard gauge blocks move at equal intervals in the direction parallel to the three motion axes in the public effective space of the laser displacement sensor to generate discrete trajectory points. The three motion trajectories formed represent the real motion trajectories of the three motion axes. The three motion trajectories It is used as the three coordinate axes of the motion track coordinate system (O-XYZ). The three laser displacement sensors can measure the displacement of the standard gauge block, and the three displacements obtained at a certain moment are used as the spatial coordinate vector of the current discrete track point of the standard gauge block in the standard orthogonal reference system.

4) Linear fitting of motion trajectory

Perform straight line fitting on the motion trajectory represented by the spatial coordinate vector of the discrete trajectory points in the standard orthogonal reference system. If the distance between the discrete trajectory points and the fitted straight line is greater than 0.02mm, it is considered as a gross error, and this part of the discrete trajectory points is regarded as a gross error. Remove from the set and perform the line fitting again. Finally, the linear equation and vector parameters of the motion trajectory are obtained, which are the linear equations and vector parameters of the X-axis, Y-axis and Z-axis of the motion trajectory coordinate system.

5) Orthogonality evaluation of motion trajectory

According to the vector orthogonality calculation of the motion trajectory vector parameters, the angle between the motion trajectories is obtained, which is the angle between the X axis, the Y axis and the Z axis of the motion trajectory coordinate system, and the orthogonality of the motion trajectory is evaluated according to the size of the angle. The criterion for evaluating the orthogonality of the motion trajectory is: set the angle threshold to 0.3 degrees, and the three included angles are respectively 90 degrees apart. When the difference between them is less than or equal to the angle threshold, the orthogonal condition is satisfied. The motion trajectories are orthogonal; if any of the differences between them is greater than the angle threshold, the orthogonal condition is not satisfied, and the motion trajectories are non-orthogonal, and proceed to step 6) to correct the non-orthogonality of the motion trajectories.

6) Non-orthogonality correction of motion trajectory

Correct the non-orthogonality of the motion trajectory for the non-orthogonal motion trajectory, set up a virtual orthogonal coordinate system, establish the mapping relationship between the virtual orthogonal coordinate system and the non-orthogonal motion trajectory coordinate system, and know any point in the space The coordinate vector in the virtual orthogonal coordinate system, and the coordinate vector of the point in the non-orthogonal motion trajectory coordinate system is obtained according to the mapping relationship, so that the motion control system can accurately move to the non-orthogonal motion trajectory coordinate system. At this point, accurate positioning in the case of orthogonality is achieved, and the positioning accuracy of the micro-manipulation robot is effectively guaranteed.

Description of drawings

Fig. 1 is the flow chart of the method for measuring the orthogonality of the motion trajectory of the three-degree-of-freedom motion control system of the micro-manipulation robot involved in the present invention

Fig. 2 is the motion trajectory orthogonality measuring system involved in the present invention

FIG. 3 is a schematic diagram of a collection motion trajectory involved in the present invention

FIG. 4 is a schematic diagram of straight line fitting of motion trajectory involved in the present invention

FIG. 5 is a schematic diagram of the orthogonality evaluation of the motion trajectory involved in the present invention

6 is a schematic diagram of the non-orthogonality correction of the motion trajectory involved in the present invention

Description of the symbols in the attached drawings:

S1. Construct a motion trajectory orthogonality measurement system

S2. Orthogonality Analysis of Standard Orthogonal Reference System

S3. Collect motion trajectory

S4, motion trajectory straight line fitting

S51. Orthogonality evaluation of motion trajectory

S52, motion trajectory orthogonal

S6, the motion trajectory is not orthogonal

S71. Non-orthogonality correction of motion trajectory

S72, the coordinate vector in the virtual orthogonal coordinate system

S73, the coordinate vector in the motion track coordinate system

S8. Accurate positioning in the case of orthogonality

1. Motion control system

2. X motion axis

3. Y motion axis

4. Z motion axis

5. X motion axis grating ruler

6. Y motion axis grating ruler

7. Z motion axis grating ruler

8. Movement track coordinate system

9. Standard gauge block

10. X-axis monitoring surface

11. Y-axis monitoring surface

12. Z-axis monitoring surface

13. X-axis laser displacement sensor

14. Y-axis laser displacement sensor

15. Z-axis laser displacement sensor

16. Standard Orthogonal Reference System

17. Computer

α, the angle between the X axis and the Y axis

β, the angle between the X axis and the Z axis

γ, the angle between the Y axis and the Z axis

Detailed ways

The present invention will now be described in further detail with reference to the accompanying drawings. 1 is a flowchart of a method for measuring the orthogonality of motion trajectories of a three-degree-of-freedom motion control system of a micro-manipulation robot involved in the present invention, and the method for measuring the orthogonality of motion trajectories of a three-degree-of-freedom motion control system of a micro-manipulation robot comprises the following steps:

1. Construct a motion trajectory orthogonality measurement system

Figure 2 shows the orthogonality measurement system of the motion trajectory. The three motion degrees of freedom of the motion control system 1 correspond to the X motion axis 2, the Y motion axis 3 and the Z motion axis 4 respectively, and the X motion axis is installed on the three motion axes respectively. The grating ruler 5, the Y motion axis grating ruler 6 and the Z motion axis grating ruler 7, with a resolution of 0.1 microns, are used for closed-loop precision control of the motion axes. In order to collect the motion trajectory of the motion axis, a standard gauge block 9 is placed on the motion control system 1, and its adjacent X-axis monitoring surface 10, Y-axis monitoring surface 11 and Z-axis monitoring surface 12 are used as displacement monitoring surfaces. With good flatness and orthogonality, the normals of the three displacement monitoring surfaces are parallel to the three motion axes. Use the X-axis laser displacement sensor 13, the Y-axis laser displacement sensor 14 and the Z-axis laser displacement sensor 15 to monitor the displacements of the three displacement monitoring surfaces respectively, and their central axes intersect at one point and are perpendicular to the corresponding displacement monitoring surfaces. A laser displacement positioning system established by a laser displacement sensor with an orthogonal relationship is used as an auxiliary standard orthogonal reference frame 16 , and the motion trajectory of the standard gauge block 9 is described in the standard orthogonal reference frame 16 .

2. Orthogonality Analysis of Standard Orthogonal Reference System

Affected by assembly factors, the central axes of the three laser displacement sensors do not necessarily satisfy the absolute orthogonal relationship, and the established non-orthogonal standard orthogonal reference system has an impact on the orthogonality measurement of the motion trajectory. How the rotation angle of the displacement sensor affects the measurement of the orthogonality of the motion trajectory A simulation method is established to analyze the influence of the orthogonality of the standard orthogonal reference frame on the measurement of the orthogonality of the motion trajectory.

The specific simulation method is as follows: unit vectors and discrete trajectory points of the three motion trajectories l _u1 , _lv1 , and l _w1 of the standard gauge block are preset, and the angle between the motion trajectories forms an angle vector Λ _t . Preset the rotation angles of the standard gauge block around the U, V, and W axes and the initial coordinate vectors of the 8 vertices, and calculate the coordinate vectors of the 8 vertices after the standard gauge block is rotated. The central axes l _A , l _B and l _C of the laser displacement sensor are rotated at a small angle with a certain point MA , _MB , and _MC on the axis as reference points, and the unit vectors corresponding to the three axes and points _MA and _M are preset. Coordinate vectors of _B and M _C .

The standard gauge block moves along the three motion trajectories l _u1 , l _v1 , and l _w1 with the attitude of the preset rotation angle, and then arrives at the position of the preset discrete trajectory point in turn. l _A , l _B , l _C and the corresponding displacement monitoring The intersection points generated by the surface are N _A , N _B , and N _C respectively. Calculate the spatial coordinate vector of the intersection point at the current position, which is the measurement value of the discrete trajectory point, and obtain the unit of the corresponding straight line l _u2 , l _v2 , l _w2 through straight line fitting vector, and then perform vector orthogonality calculation to obtain the angle vector Λ formed by the included angle between l _u2 , l _v2 , and l _w2 . Λ _t is the preset value, Λ is the measured value, the difference between them represents the change of the measurement result of the orthogonality of the motion trajectory after adding the rotation angle of the standard gauge block and the laser displacement sensor, which is used to analyze the orthogonality of the standard orthogonal reference system. Influence on the measure of orthogonality of motion trajectories. The simulation results show that when the rotation angle between the standard gauge block and the laser displacement sensor is within 10 degrees, the maximum difference between the preset value and the measured value is 0.0967 degrees, and the orthogonality of the standard orthogonal reference system to the orthogonality of the motion trajectory can be ignored. measure the impact. Therefore, when assembling the standard gauge block and the laser displacement sensor, adjust the rotation angle to within 10 degrees, and then proceed to step 3.

3. Collect movement track

Figure 3 is a schematic diagram of collecting motion trajectories. The computer 17 controls the motion control system 1, so that the standard gauge blocks 9 move at equal intervals in the directions parallel to the three motion axes to generate discrete trajectory points, and the three motion trajectories formed represent the three motions. The real motion trajectory of the axis, the three motion trajectories are used as the X axis, the Y axis and the Z axis of the motion trajectory coordinate system 8 . The three laser displacement sensors can measure the displacement of the standard gauge block 9 , and the three displacements obtained at a certain moment are used as the spatial coordinate vector of the current discrete track point of the standard gauge block 9 in the standard orthogonal reference frame 16 . The discrete track points of the standard gauge block 9 on the X-axis correspond to the spatial coordinate vector {S _AN } in the standard orthogonal reference frame 16, and the three components of {S _AN } correspond to the three laser displacement sensors. The standard gauge block 9 is in The amount of displacement at each discrete trajectory point. Similarly, the discrete track points of the standard gauge block 9 on the Y-axis and the Z-axis correspond to the spatial coordinate vectors {S _BM } and {S _CK } in the standard orthogonal reference system 16 . The samples constructed by these three spatial coordinate vectors are stored in the computer 17 as the basic data for evaluating the orthogonality of the motion trajectory.

4. Linear fitting of motion trajectory

Figure 4 is a schematic diagram of linear fitting of the motion trajectory. Linear fitting is performed on the motion trajectory represented by the spatial coordinate vectors {S _AN }, {S _BM } and {S _CK } of the discrete trajectory points in the standard orthogonal reference system. If the distance between the trajectory point and the fitted straight line is greater than 0.02mm, it is considered as a gross error, and this part of the discrete trajectory points is removed from the set, and the straight line fitting is performed again. Finally, the linear equation and vector parameters of the motion trajectory are obtained, which are the linear equations and vector parameters of the X-axis, Y-axis and Z-axis of the motion trajectory coordinate system. In the standard orthogonal reference system, the general formula for the straight line fitting of the motion trajectory corresponding to the space coordinate vectors {S _AN }, {S _BM } and {S _CK } is:

_{SLFM represents a motion trajectory} straight line _fitting method.

5. Orthogonality evaluation of motion trajectory

Figure 5 is a schematic diagram of the orthogonality evaluation of the motion trajectory. The angles between the X axis and the Y axis, the X axis and the Z axis, and the Y axis and the Z axis are represented by α, β, and γ, respectively. According to the motion trajectory vector parameters, calculate the motion trajectory unit vectors n _a , n _b and n _c , and then perform the vector orthogonality calculation to obtain the values of α, β and γ. The vector orthogonality calculation formula is:

According to the size of α, β, γ, the orthogonality of the motion trajectory is evaluated, and the criterion for evaluating the orthogonality of the motion trajectory is:

In the formula, T _abc is the angle threshold, which is 0.3 degrees. When the three conditions in the formula are all established, the orthogonal condition is satisfied, and the motion trajectory is orthogonal S52; if any condition in the formula is not established, the orthogonal condition is not satisfied, the motion trajectory is not orthogonal S6, and the motion is entered into step 6. Trajectory non-orthogonality correction.

6. Non-orthogonality correction of motion trajectory

The non-orthogonality correction of the motion trajectory is performed on the non-orthogonal motion trajectory S71 . Figure 6 is a schematic diagram of the non-orthogonality correction of the motion trajectory. A virtual orthogonal coordinate system (O-XGH) is set. The motion trajectory coordinate system and the virtual orthogonal coordinate system have a common coordinate origin O and coordinate axis X. The coordinate planes XOY and XOG coincide, ∠XOY=α, ∠XOZ=β, ∠YOZ=γ. Establish the mapping relationship between the two coordinate systems, given the coordinate vector r _p,gh =(x _p,gh ,y _p,gh ,z _p,gh ) ^T of any point P in the virtual orthogonal coordinate system, find The coordinate vector of the obtained point P in the motion trajectory coordinate system is r _p,yz =(x _p,yz ,y _p,yz ,z _p,yz ) ^T .

A line parallel to the H axis through the point P, intersecting with the XOG plane at the projection point P ₁ , and a line parallel to the Z axis through the point P, intersecting the plane XOY at the projection point P ₁₁ . A vertical line is drawn from the point _P11 to the G axis, the vertical foot is the point _P13 , the line segment P11 _P13 and the Y axis intersect at the point _P12 , and the points _P13 and _P12 are the points _{P11 on} the G axis and the Y axis, respectively. projection point. A line parallel to the line segment OP ₁₁ is drawn through the point P, and intersects the Z-axis at the projection point P ₇ .

The unit vector of the Y axis in the virtual orthogonal coordinate system is n _y,gh =(cosα,sinα,0) ^T , and the unit vector of the Z axis in the virtual orthogonal coordinate system is n _z,gh =(cosβ,cosβ _{z ,gh} ,cosγ _z,gh ) ^T , cosβ z _,gh and cosγ _z,gh can be obtained according to the vector dot product formula and the unit vector normalization condition. In the right triangle P ₁₁ PP ₁ there is z _p,yz =z _p,gh /cosγ _z,gh and z _p7,yz =z _p,yz is known, then according to r _p7,gh =z _p7,yz ·n _z,gh obtains r _p7,gh , where r _p7,gh is the coordinate vector of the point P ₇ in the virtual orthogonal coordinate system. The coordinate vector of the point P ₁₁ in the virtual orthogonal coordinate system is r _p11,gh , and the coordinate vector in the motion trajectory coordinate system is r _p11,yz . In the right triangle P ₁₂ OP ₁₃ , the following formula can be obtained according to the geometric relationship:

Point P ₁₁ is the projection point of point P on the XOY plane in the motion trajectory coordinate system. The mapping relationship between r _p,gh and r _p,yz can be established by the above formula:

According to the mapping relationship, the coordinate vector r _{p , yz} S73 corresponding to the point P in the virtual orthogonal coordinate system can be calculated from the coordinate vector r _{p, gh} S72 of the point P in the virtual orthogonal coordinate system. It can move to the point P accurately, realize the accurate positioning S8 under the orthogonal condition, and effectively ensure the positioning accuracy of the micro-manipulation robot.

It will be apparent to those skilled in the art that various modifications and variations of the present invention can be made, and the present invention covers such modifications and variations of the present invention as long as they fall within the scope of the appended claims and their equivalents.

Claims (1)

1. The method for measuring the orthogonality of the motion trail of the three-degree-of-freedom motion control system of the micro-operation robot is characterized by comprising the following steps of: based on the laser displacement measurement principle, a motion track orthogonality measurement system is constructed, an auxiliary standard orthogonal reference system is established, a simulation method is established for carrying out standard orthogonal reference system orthogonality analysis, motion tracks are collected in the standard orthogonal reference system, straight line fitting is carried out on the motion tracks, then a motion track orthogonality evaluation method is established, finally motion track non-orthogonality correction is carried out on the non-orthogonal motion tracks, and accurate positioning under the orthogonal condition is achieved, and the method specifically comprises the following steps:

1) system for constructing motion trajectory orthogonality measurement system

Grating rulers for precisely controlling the motion axes in a closed loop are respectively arranged on the three motion axes of the motion control system; in order to collect the motion trail of the motion axis, a standard gauge block is arranged on the motion control system, three adjacent surfaces of the standard gauge block are used as displacement monitoring surfaces, and the normal lines of the three displacement monitoring surfaces are respectively parallel to the three motion axes; the central axes of the three laser displacement sensors are intersected at one point and are vertical to the corresponding displacement monitoring surface, and the three laser displacement sensors are used for measuring the displacement of the standard gauge block; a laser displacement positioning system established by three laser displacement sensors with an orthogonal relation is used as an auxiliary standard orthogonal reference system, and the motion trail of a standard gauge block is described in the standard orthogonal reference system;

2) orthonormal reference system orthogonality analysis

Under the influence of assembly factors, the central axes of the three laser displacement sensors do not necessarily satisfy an absolute orthogonal relationship, the established non-orthogonal standard orthogonal reference system has influence on the measurement of the orthogonality of the motion track, and a simulation method is established for analyzing the influence of the orthogonality of the standard orthogonal reference system on the measurement of the orthogonality of the motion track on how the rotation angle of the standard gauge block and the laser displacement sensors influences the measurement of the orthogonality of the motion track;

three motion tracks and discrete track points of a preset standard gauge block, and included angles among the motion tracks form an angle vector Λ_tThe method comprises the steps of presetting a rotation angle between a standard gauge block and a laser displacement sensor, moving the standard gauge block along a preset motion track at the posture of the preset rotation angle, calculating a space coordinate vector of an intersection point of a central axis of the laser displacement sensor and a displacement monitoring surface, namely a measurement value of a discrete track point, and obtaining an angle vector Λ formed by an included angle between three straight lines through straight line fitting and vector orthogonality calculation, Λ_tFor the preset values, Λ are measured values, the difference representing the addition of the standard quantityThe change of the motion track orthogonality measuring result after the rotation angle of the block and the laser displacement sensor is used for analyzing the influence of the orthogonality of the standard orthogonal reference system on the motion track orthogonality measurement; simulation results show that: when the rotation angle of the standard gauge block and the laser displacement sensor is within 10 degrees, the maximum difference value between the preset value and the measured value is 0.0967 degrees, and the influence of the orthogonality of the standard orthogonal reference system on the orthogonality measurement of the motion trail is ignored; therefore, when the standard gauge block and the laser displacement sensor are assembled, the rotation angle is adjusted to be within 10 degrees, and then the step 3) is carried out;

3) collecting motion trail

The standard gauge blocks move at equal intervals in the public effective space of the laser displacement sensor in the direction parallel to the three motion axes respectively to generate discrete track points, the three formed motion tracks represent the real motion tracks of the three motion axes, and the three motion tracks are used as three coordinate axes of a motion track coordinate system O-XYZ; measuring the displacement of the standard gauge block by the three laser displacement sensors, and taking the three displacement obtained at a certain moment as the space coordinate vector of the current discrete track point of the standard gauge block in the standard orthogonal reference system;

4) fitting of motion trajectory straight line

Performing linear fitting on the motion trail represented by the space coordinate vector of the discrete track point in the standard orthogonal reference system, if the distance from the discrete track point to the fitting straight line is more than 0.02mm, considering the discrete track point as a coarse error, removing the discrete track point from the set, and performing linear fitting again; finally, a motion trail linear equation and vector parameters are obtained, namely linear equations and vector parameters of an X axis, a Y axis and a Z axis of a motion trail coordinate system;

5) motion trajectory orthogonality evaluation

Vector orthogonality calculation is carried out according to the motion track vector parameters to obtain included angles among the motion tracks, namely included angles among X axes, Y axes and Z axes of a motion track coordinate system, the motion track orthogonality is evaluated according to the included angles, and the judgment standard for evaluating the motion track orthogonality is as follows: setting an angle threshold value to be 0.3 degrees, and making difference between the three included angles and 90 degrees respectively, wherein when the difference values between the three included angles are less than or equal to the angle threshold value, an orthogonal condition is met, and the motion tracks are orthogonal; if any difference value between the two is larger than the angle threshold value, the orthogonality condition is not met, the motion trail is not orthogonal, and the step 6) is carried out to carry out the non-orthogonal correction of the motion trail;

6) correction of motion trajectory non-orthogonality

Correcting the non-orthogonality of the motion trail of the non-orthogonal motion trail; setting a virtual orthogonal coordinate system O-XGH, wherein the motion trail coordinate system and the virtual orthogonal coordinate system have a common coordinate origin O and a common coordinate axis X, the coordinate planes XOY and XOG of the two coordinate systems are overlapped, the angle XOY is alpha, the angle XOZ is beta, and the angle YOZ is gamma, and establishing a mapping relation between the virtual orthogonal coordinate system and the motion trail coordinate system:

in the formula, r_p,yz＝(x_p,yz,y_p,yz,z_p,yz)^T，x_p,yzIs the coordinate of point P on the X-axis of the O-XYZ coordinate system, y_p,yzIs the coordinate of point P on the Y-axis of O-XYZ coordinate system, z_p,yzThe coordinate of the point P on the Z axis of the O-XYZ coordinate system; r is_p,gh＝(x_p,gh,y_p,gh,z_p,gh)^T，x_p,ghIs the coordinate of point P on the X-axis of the O-XGH coordinate system, y_p,ghIs the coordinate of point P on the G axis of the O-XGH coordinate system, z_p,ghThe coordinate of the point P on the H axis of the O-XGH coordinate system; n is_z,gh＝(cosβ,cosβ_z,gh,cosγ_z,gh)^TIs a unit vector of Z axis in O-XGH coordinate system, cos β_z,ghCosine value of ∠ ZOG, cos gamma_z,ghA cosine value of ∠ ZOH;

according to the mapping relation, the coordinate vector r of the point P in the virtual orthogonal coordinate system_p,ghCalculating the coordinate vector r corresponding to the coordinate system of the motion trail_p,yzThe motion control system is based on r_p,yzMoving exactly to point P.

Images