Four-PSD spatial relationship calibration method

Abstract

A four-PSD spatial relationship calibration method relates to the technical field of spatial relationship calibration, and aims to solve the problem that the calibration precision is reduced due to coupling errors in an existing calibration method, and the method comprises the following steps: step S1, establishing a coordinate system, step S2, measuring a light state, step S3, establishing four PSD spatial pose error models, step S4, establishing a target ball measurement system, and step S5, establishing a three-dimensional coordinate system, a three-dimensional coordinate system, a three-dimensional coordinate system, a three-dimensional coordinate system, a three-dimensional coordinate system and a three-dimensional coordinate system. S5, constructing an error constraint equation, and solving the spatial poses of the first PSD, the second PSD and the third PSD; and S6, solving the spatial poses of the first PSD, the second PSD and the fourth PSD. The isomorphic measurement system is established, the measurement process has no constraint on the motion state of the to-be-measured object, and the six-degree-of-freedom motion state can be measured. A constraint condition is constructed according to the property that the trace of a pose transformation matrix calculated between multiple target coordinate systems is equal, and the constraint condition accurately describes the spatial relationship between two measurement systems, so that error correction is realized, a motion coupling error source is eliminated, and the four-PSD spatial relationship calibration precision is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of spatial relationship calibration, and in particular to a spatial relationship calibration method of four position sensitive detectors (PSD for short). Background Art

[0002] With the widespread application of articulated robots, the low absolute positioning accuracy has become a key issue restricting the development of articulated robots at home and abroad. As most articulated robot calibration methods are costly, complex to operate, and not easy to carry, robot calibration devices using lasers and photosensitive chips have begun to be studied and developed.

[0003] The Chinese patent publication number is "CN 117863179 A", and the patent name is "A spatial relationship calibration method, electronic device and storage medium". This method collects a number of point cloud data of the workpiece to be measured, and obtains the fixed point coordinates in the acquisition device coordinate system corresponding to each set of data, and selects two sets of point cloud data to obtain the initial spatial relationship between the acquisition device and the end of the robotic arm. According to the initial spatial relationship, the coordinates of the center of the sphere in the base coordinate system corresponding to each set of point cloud data are obtained, and the gross errors in the coordinates and the point cloud data corresponding to the gross error coordinates are eliminated. It is judged whether the remaining point cloud data is less than the set threshold. If not, the final spatial relationship between the acquisition device and the end of the robotic arm is obtained according to the remaining point cloud data; if less than, the point cloud data is continued to be collected, and the final spatial relationship is converted into Lie algebra for derivative iterative solution to obtain the optimized spatial relationship. The shortcomings of this method are: when acquiring data, the motion state of the object to be measured is constrained, and it is difficult to accurately decouple the motion states. The coupling error will be brought into the calibration result, reducing the calibration accuracy of the spatial relationship between the acquisition device and the end of the robotic arm. Summary of the invention

[0004] In order to solve the problem that coupling error reduces calibration accuracy in existing calibration methods, the present invention proposes a four-PSD spatial relationship calibration method to improve the calibration accuracy of the four-PSD spatial relationship.

[0005] The technical solution of the present invention to solve the technical problem is as follows:

[0006] A four-PSD spatial relationship calibration method, the method comprising the following steps:

[0007] Step S1: Establish a coordinate system: Establish O on PSD1 1 System, build O on PSD2 2 System, build O on PSD3 3 System, build O on PSD 4 4 System, establish L system on the laser tracker;

[0008] Step S2: Measure the light state: Control laser 1 and laser 2 to simultaneously shoot light toward PSD 1 and PSD 3 to obtain the coordinates and coordinates The light from laser 1 is reflected from PSD 1 to PSD 2 to obtain the coordinates The light from laser 2 is reflected from PSD 3 to PSD 4 to obtain the coordinates At the same time, the laser tracker is controlled to shoot light to target balls 1, 2 and 3 in turn to obtain coordinates A. i ( A X i , A Y i , A Z i ), B i ( B X i , B Y i , B Z i ) and C i ( C X i , C Y i , C Z i ), keep laser 1 and laser 2 stationary, move the calibration device n times, and obtain n sets of coordinates, where i represents the number of sets, i = 1, 2, 3...n;

[0009] Step S3: Establish four PSD spatial pose error models, including the following steps:

[0010] S31: Build O 2 Relative to O 1 The actual pose transformation matrix of the system O 3 Relative to O 1 The actual pose transformation matrix of the system and O 4 Relative to O 1 The actual pose transformation matrix of the system

[0011] S32: Calculate O 1 Coordinates of the system

[0012] S33: In O 1 Select the point and by Construct M system for the origin;

[0013] S34: With O 1The system is the reference coordinate system, and the M system is constructed relative to O 1 The pose transformation matrix of the system

[0014] S35: Calculate the M at the i-th position i The system is relative to the first position M 1 The pose transformation matrix of the system

[0015] S36: Calculate the pose transformation matrix The trace value

[0016] Step S4: Establish the target ball measurement system: Select point A in the L system i , B i and C i , with A i Construct the S system as the origin, take the L system as the reference coordinate system, and construct the posture transformation matrix of the S system relative to the system Where S i is the S system at the ith position, i = 1, 2, 3…n; calculate the S system at the ith position i The system is relative to the first position S 1 The pose transformation matrix of the system Calculate the pose transformation matrix The trace value As a benchmark value;

[0017] Step S5: construct error constraint equations to solve the spatial poses of PSD 1, PSD 2 and PSD 3;

[0018] Step S6: 1 Select the point and by Repeat steps S33-S5 for the origin to solve the spatial poses of PSD 1, PSD 2, and PSD 4.

[0019] Compared with the prior art, the present invention has the following beneficial effects: the present invention establishes an isomorphic measurement system, and the measurement process has no constraints on the motion state of the object to be measured, and the 6-DOF motion state can be measured. For the pose transformation matrix calculated between multiple target coordinate systems, the trace (i.e., the sum of the matrix diagonal elements) is equal to construct a constraint condition, which accurately describes the spatial relationship between the two measurement systems and thus realizes error correction, eliminates the source of motion coupling error, and improves the calibration accuracy of the four PSD spatial relationship. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] Figure 1 A schematic diagram of a device based on a four-PSD spatial relationship calibration method proposed in the present invention;

[0021] Figure 2 A flowchart of a four-PSD spatial relationship calibration method proposed by the present invention;

[0022] Figure 3 A schematic diagram of the principle of a four-PSD spatial relationship calibration method proposed by the present invention;

[0023] In the figure: 1. PSD one; 2. PSD two; 3. PSD three; 4. PSD four; 5. Laser tracker; 6. Laser one; 7. Laser two; 8. Target ball one; 9. Target ball two; 10. Target ball three. DETAILED DESCRIPTION

[0024] A four-PSD spatial relationship calibration method, the method is based on a robot kinematic parameter error calibration device of four PSDs, such as Figure 1 As shown, the device includes PSD-1, PSD-2, PSD-3, PSD-4, target ball-1, 8, target ball-2, 9 and target ball-3, 10. PSD-1 and PSD-3 are placed at an angle to the horizontal plane, PSD-2 is arranged above PSD-1, PSD-4 is arranged above PSD-3, target ball-1, 8, target ball-2, 9 and target ball-3, 10 are fixed to the device, a laser tracker 5 is arranged on one side of the device, and laser-1, 6 and laser-2, 7 are arranged above the device.

[0025] like Figure 2 As shown, a four-PSD spatial relationship calibration method, the specific steps of the method are as follows:

[0026] Step S1: Establish a coordinate system;

[0027] like Figure 1 As shown, build O on PSD-1 1 System, build O on PSD2 2 System, build O on PSD3 3 System, build O on PSD44 4 System, establish L system on laser tracker 5.

[0028] Step S2: measuring light status;

[0029] Control laser 1 6 and laser 2 7 to simultaneously shoot light to PSD 1 and PSD 3 to obtain the coordinates and coordinates The light from laser 6 is reflected from PSD 1 to PSD 2 to obtain the coordinates The light from laser 2 7 is reflected by PSD 3 3 to PSD 4 4 to obtain the coordinates At the same time, the laser tracker 5 is controlled to sequentially shoot light to the target ball 1 8, the target ball 2 9 and the target ball 3 10 to obtain the coordinate A.i ( A X i , A Y i , A Z i ), B i ( B X i , B Y i , B Z i ) and C i ( C X i , C Y i , C Z i ),like Figure 3 As shown, keep laser 1 6 and laser 2 7 stationary, move the calibration device n times, and obtain n sets of coordinates, where i represents the number of sets, i = 1, 2, 3...n.

[0030] Step S3: Establish four PSD spatial pose error models, including the following steps:

[0031] Step S31: Establish O 2 Relative to O 1 The actual pose transformation matrix of the system O 3 Relative to O 1 The actual pose transformation matrix of the system and O 4 Relative to O 1 The actual pose transformation matrix of the system in,

[0032]

[0033] In formula (1), x m ′、y m ′、z m ' indicates O m Along O n The actual distance of translation along the X, Y, and Z axes; α m ′、β m ′、γ m ' indicates O m Around O n The actual angle of rotation of the X, Y, and Z axes; α m ′=α m +Δα m ,β m ′=β m +Δβ m ,γ m ′=γm +Δγ m ,x m ′=x m +Δx m ,y m ′=y m +Δy m ,z m ′=z m +Δz m , x m etc. represent the design value, Δx m etc. represent the error of the parameters to be determined.

[0034] The error of the parameters to be determined is shown in Table 1:

[0035] Table 1 Errors of the parameters to be determined for the four PSDs

[0036]

[0037] Step S32: Calculate O 1 Coordinates of the system

[0038] The formula is as follows:

[0039]

[0040] Step S33: Figure 3 As shown, in O 1 Select the point and by Construct M system for the origin and select and Construct the X-axis vector u i , the formula is as follows:

[0041]

[0042] Select and Construct vector l i , the formula is as follows:

[0043]

[0044] With vector u i and vector l i Construct the Z-axis vector w i , the formula is as follows:

[0045]

[0046] With vector u i and vector w i Construct the Y-axis vector vi , the formula is as follows

[0047]

[0048] Step S34: 1 The system is the reference coordinate system, and the M system is constructed relative to O 1 The pose transformation matrix of the system The formula is as follows:

[0049]

[0050] Among them, M i is the M system at the i-th position, i = 1, 2, 3…n;

[0051] Step S35: Calculate the M at the i-th position i The system is relative to the first position M 1 The pose transformation matrix of the system The formula is as follows:

[0052]

[0053] in,

[0054] Step S36: Calculate the pose transformation matrix The trace value is a and Analytical expressions for the 12 unknown parameter errors are given in .

[0055] Step S4: Establishing a target ball measurement system;

[0056] Select point A in the L system i , B i and C i , with A i The S system is constructed according to formulas (3)-(6) with L system as the origin, and the position transformation matrix of S system relative to L system is constructed according to formula (7) with L system as the reference coordinate system. Where S i is the S system at the ith position, i = 1, 2, 3…n; according to formula (8), calculate the S system at the ith position i The system is relative to the first position S 1 The pose transformation matrix of the system Calculate the pose transformation matrix The trace value as a baseline value.

[0057] Step S5: Construct the error constraint equation, the formula is as follows:

[0058]

[0059] The PSO (Particle Swarm optimization) algorithm is used to solve the spatial poses of PSD1, PSD2 and PSD3 in equation (9).

[0060] Step S6: 1 Select the point and by Repeat steps S33-S5 for the origin to solve the spatial poses of PSD-1, PSD-2 and PSD-4.

[0061] Example:

[0062] A four-PSD spatial relationship calibration method, the method is based on a robot kinematic parameter error calibration device of four PSDs, such as Figure 1 As shown, the device includes PSD-1, PSD-2, PSD-3, PSD-4, target ball-1, target ball-2, and target ball-3. PSD-1 and PSD-3 are placed at an angle of 25° to the horizontal plane, the center distance between PSD-1 and PSD-3 is 110 mm, PSD-2 is arranged above PSD-1, PSD-4 is arranged above PSD-3, target ball-1, target ball-2, target ball-3, and target ball-3 are fixed to the device, a laser tracker 5 is arranged on one side of the device, and laser-1 and laser-2 are arranged above the device; the laser used is a precision semiconductor pulse laser, and the spot diameter of the laser beam is less than 1 mm; the PSD used is DRX-2DPSD-GJD02, with a resolution of 0.002 mm, a linear error of 0.1%, an effective surface of a 9 mm*9 mm square, and the two-dimensional position of the pulse laser beam spot on the PSD surface can be detected; the laser and PSD are modulated to reduce background light interference.

[0063] A four-PSD spatial relationship calibration method, the specific steps of the method are as follows:

[0064] Step S1: Establish the coordinate system:

[0065] like Figure 1 As shown, build O on PSD-1 1 System, build O on PSD2 2 System, build O on PSD3 3 System, build O on PSD44 4 System, establish L system on laser tracker 5;

[0066] Step S2: Measure light status:

[0067] Control laser 1 6 and laser 2 7 to simultaneously shoot light to PSD 1 and PSD 3 to obtain the coordinates and coordinates The light from laser 6 is reflected from PSD 1 to PSD 2 to obtain the coordinates The light from laser 2 7 is reflected by PSD 3 3 to PSD 4 4 to obtain the coordinates At the same time, the laser tracker 5 is controlled to sequentially shoot light to the target ball 1 8, the target ball 2 9 and the target ball 3 10 to obtain the coordinate A. i ( A X i , A Y i , A Z i ), B i ( B X i , B Y i , B Z i ) and C i ( C X i , C Y i , C Z i ),like Figure 3 As shown, keep the laser 1 6 and the laser 2 7 stationary, move the calibration device 15 times, and obtain 15 sets of coordinates. The data are shown in Table 2 and Table 3:

[0068] Table 2 Four PSD position data

[0069]

[0070] Table 3 Target ball position data

[0071]

[0072]

[0073] Step S3: Establish four PSD spatial pose error models, including the following steps:

[0074] Step S31: Establish O 2 Relative to O 1 The actual pose transformation matrix of the system O 3 Relative to O 1 The actual pose transformation matrix of the system and O 4 Relative to O 1 The actual pose transformation matrix of the system in,

[0075]

[0076] In formula (1), x m ′、y m ′、z m ' indicates O m Along O n The actual distance of translation along the X, Y, and Z axes; α m ′、β m ′、γ m ' indicates O m Around O n The actual angle of rotation of the X, Y, and Z axes; α m ′=α m +Δα m ,β m ′=β m +Δβ m ,γ m ′=γ m +Δγ m ,x m ′=x m +Δx m ,y m ′=y m +Δy m ,z m ′=z m +Δz m , x m etc. represent the design value, Δx m etc. represent the parameter errors to be determined. The theoretical spatial relationships of the four PSDs in this embodiment are shown in Table 4:

[0077] Table 4 Theoretical spatial relationships of the four PSDs

[0078]

[0079] Step S32: Calculate O 1 Coordinates of the system

[0080] The formula is as follows:

[0081]

[0082] Step S33: Figure 3 As shown, in O 1 Select the point and by Construct M system for the origin and select and Construct the X-axis vector u i, the formula is as follows:

[0083]

[0084] Select and Construct vector l i , the formula is as follows:

[0085]

[0086] With vector u i and vector l i Construct the Z-axis vector w i , the formula is as follows:

[0087]

[0088] With vector u i and vector w i Construct the Y-axis vector v i , the formula is as follows

[0089]

[0090] Step S34: 1 The system is the reference coordinate system, and the M system is constructed relative to O 1 The pose transformation matrix of the system The formula is as follows:

[0091]

[0092] Among them, M i is the M system at the i-th position, i = 1, 2, 3…15;

[0093] Step S35: Calculate the M at the i-th position i The system is relative to the first position M 1 The pose transformation matrix of the system The formula is as follows:

[0094]

[0095] in,

[0096] Step S36: Calculate the pose transformation matrix The trace value is a and Analytical expressions for the 12 unknown parameter errors are given in .

[0097] Step S4: Establishing the target ball measurement system:

[0098] Select point A in the L system i , B i and C i , with A i The S system is constructed according to formulas (3)-(6) with L system as the origin, and the position transformation matrix of S system relative to L system is constructed according to formula (7) with L system as the reference coordinate system. Where S i is the S system at the ith position, i = 1, 2, 3…15; according to formula (8), calculate the S system at the ith position i The system is relative to the first position S 1 The pose transformation matrix of the system Calculate the pose transformation matrix The trace value As a benchmark value;

[0099] Step S5: Construct the error constraint equation, the formula is as follows:

[0100]

[0101] The PSO (Particle Swarm optimization) algorithm is used to solve equation (9) to obtain the spatial poses of PSD1, PSD2 and PSD3.

[0102] Step S6: 1 Select the point and by Repeat steps S33-S5 for the origin to solve the spatial poses of PSD-1, PSD-2 and PSD-4.

[0103] The parameter error identification results are shown in Table 5:

[0104] Table 5 Four PSD parameter error identification results

[0105]

Claims

1. A four-PSD spatial relationship calibration method, characterized in that: The specific steps of this method are as follows: Step S1: Establish a coordinate system; Establish O1 system on PSD 1, establish O2 system on PSD 2, establish O3 system on PSD 3, establish O4 system on PSD 4, and establish L system on the laser tracker; Step S2: measuring light status; Control laser 1 and laser 2 to simultaneously shoot light toward PSD 1 and PSD 3 to obtain coordinates and coordinates The light from laser 1 is reflected from PSD 1 to PSD 2 to obtain the coordinates The light from laser 2 is reflected from PSD 3 to PSD 4 to obtain the coordinates At the same time, the laser tracker is controlled to shoot light to target balls 1, 2 and 3 in turn to obtain coordinates A. i ( A X i , A Y i , A Z i ), B i ( B X i , B Y i , B Z i ) and C i ( C X i , C Y i , C Z i ), keep laser 1 and laser 2 stationary, move the calibration device n times, and obtain n sets of coordinates, where i represents the number of sets, i = 1, 2, 3...n; Step S3: Establish four PSD spatial pose error models, including the following steps: S31: Establish the actual posture transformation matrix of O2 system relative to O1 system The actual posture transformation matrix of O3 system relative to O1 system And the actual posture transformation matrix of O4 system relative to O1 system S32: Calculate the coordinates in the O1 system S33: Select a point in the O1 system and by Construct M system for the origin; S34: Take O1 as the reference coordinate system and construct the pose transformation matrix of M relative to O1 S35: Calculate the M at the i-th position i The pose transformation matrix of the system relative to the M1 system in the first position S36: Calculate the pose transformation matrix The trace value Step S4: Establish the target ball measurement system: Select point A in the L system i , B i and C i , with A i Construct the S system as the origin, take the L system as the reference coordinate system, and construct the posture transformation matrix of the S system relative to the L system Where S i is the S system at the ith position, i = 1, 2, 3…n; calculate the S at the ith position i The pose transformation matrix of the system relative to the S1 system in the first position i=2,3…n; calculate the pose transformation matrix The trace value As a benchmark value; Step S5: construct error constraint equations to solve the spatial poses of PSD 1, PSD 2 and PSD 3; Step S6: Select a point in the O1 system and by Repeat steps S33-S5 for the origin to solve the spatial poses of PSD 1, PSD 2, and PSD 4.

2. A four-PSD spatial relationship calibration method according to claim 1, characterized in that: The step S3 specifically includes: Step S31: Establish the actual posture transformation matrix of the O2 system relative to the O1 system The actual posture transformation matrix of O3 system relative to O1 system And the actual posture transformation matrix of O4 system relative to O1 system in, In formula (1), x m ′、y m ′、z m ' indicates O m Along O n The actual distance of translation along the X, Y, and Z axes; α m ′、β m ′、γ m ' represents O m Around O n The actual angle of rotation of the X, Y, and Z axes; α m ′=α m +Δα m ,β m ′=β m +Δβ m ,γ m ′=γ m +Δγ m ,x m ′=x m +Δx m ,y m ′=y m +Δy m ,z m ′=z m +Δz m , x m etc. represent the design value, Δx m etc. represent the error of the parameter to be determined; Step S32: Calculate the coordinates in the O1 system The formula is as follows: Step S33: Select a point in the O1 system and by Construct M system for the origin and select and Construct the X-axis vector u i , the formula is as follows: Select and Construct vector l i , the formula is as follows: With vector u i and vector l i Construct the Z-axis vector w i , the formula is as follows: With vector u i and vector w i Construct the Y-axis vector v i , the formula is as follows Step S34: Using the O1 system as the reference coordinate system, construct the pose transformation matrix of the M system relative to the O1 system The formula is as follows: Among them, M i is the M system at the i-th position, i = 1, 2, 3…n; Step S35: Calculate the M at the i-th position i The pose transformation matrix of the system relative to the M1 system in the first position The formula is as follows: in, Step S36: Calculate the pose transformation matrix The trace value is a and Analytical expressions for the 12 unknown parameter errors are given in .

3. A four-PSD spatial relationship calibration method according to claim 2, characterized in that: The step S4 is specifically as follows: Select point A in the L system i , B i and C i , with A i The S system is constructed according to formulas (3)-(6) with L system as the origin, and the position transformation matrix of S system relative to L system is constructed according to formula (7) with L system as the reference coordinate system. Where S i is the S system at the ith position, i = 1, 2, 3…n; according to formula (8), calculate the S system at the ith position i The pose transformation matrix of the system relative to the S1 system in the first position i=2,3…n; calculate the pose transformation matrix The trace value as a baseline value.

4. A four-PSD spatial relationship calibration method according to claim 3, characterized in that: The error constraint equation constructed in step S5 is as follows: The PSO algorithm is used to solve the spatial poses of PSD 1, PSD 2 and PSD 3 in equation (9).

Images