A four-psd spatial relationship calibration method

Abstract

The application discloses a four-PSD space relation calibration method, relates to the technical field of space relation calibration, and aims at solving the problem of the existing calibration method that coupling error reduces calibration precision, and comprises the following steps: step S1, establishing a coordinate system; step S2, measuring light state; step S3, establishing four-PSD space pose error models; step S4, establishing a target ball measurement system; step S5, constructing error constraint equations, and solving the space poses of PSD one, PSD two and PSD three; and step S6, solving the space poses of PSD one, PSD two and PSD four. The application establishes isomorphic measurement systems, and the movement state of a to-be-measured object is not constrained in the measurement process, and the six-degree-of-freedom movement state can be measured. The trace of the pose transformation matrix calculated between multiple target coordinate systems is equal, a constraint condition is constructed, the space relation between two measurement systems is accurately described by the constraint condition, error correction is realized, the source of movement coupling error is eliminated, and the four-PSD space relation calibration precision is improved.

Description

Technical Field

[0001] This invention relates to the field of spatial relationship calibration technology, specifically to a spatial relationship calibration method using four position sensitive detectors (PSDs). Background Technology

[0002] With the widespread application of articulated robots, their low absolute positioning accuracy has become a key issue restricting the development of articulated robots both domestically and internationally. Since most articulated robot calibration methods are costly, complex to operate, and not easily portable, robot calibration devices using lasers and photosensitive chips have begun to be researched and developed.

[0003] The Chinese patent publication number is "CN 117863179 A", and the patent title is "A Spatial Relationship Calibration Method, Electronic Device, and Storage Medium". This method collects point cloud data of several workpieces under test and obtains the coordinates of fixed points in the coordinate system of the acquisition device corresponding to each set of data. Two sets of point cloud data are randomly selected to obtain the initial spatial relationship between the acquisition device and the end effector of a robotic arm. Based on 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. Gross errors in the coordinates and the point cloud data corresponding to the gross error coordinates are removed. It is then determined whether the remaining point cloud data is less than a set threshold. If it is not less, the final spatial relationship between the acquisition device and the end effector of the robotic arm is obtained based on the remaining point cloud data. If it is less, point cloud data is collected again, and the final spatial relationship is converted into a Lie algebra for iterative differentiation to obtain an optimized spatial relationship. The shortcomings of this method are: the motion state of the workpiece under test is constrained during data acquisition, making it difficult to accurately decouple the different motion states. Coupling errors are introduced into the calibration results, reducing the accuracy of the spatial relationship calibration between the acquisition device and the end effector of the robotic arm. Summary of the Invention

[0004] To address the problem of coupling errors reducing calibration accuracy in existing calibration methods, this invention proposes a four-PSD spatial relationship calibration method to improve the calibration accuracy of four-PSD spatial relationships.

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

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

[0007] Step S1: Establish coordinate systems: Establish the O1 system on PSD1, the O2 system on PSD2, the O3 system on PSD3, the O4 system on PSD4, and the L system on the laser tracker;

[0008] Step S2: Measure the light state: Control laser one and laser two to simultaneously direct light onto PSD one and PSD three to obtain coordinates. and coordinates The coordinates are obtained by reflecting the light from laser one onto PSD two via PSD one. The light from laser two is reflected by PSD three onto PSD four to obtain coordinates. Simultaneously, the laser tracker is controlled to sequentially direct light beams toward target sphere one, target sphere two, and target sphere three to obtain 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 Keeping laser one and laser two stationary, move the calibration device n times to 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: Establish the actual pose transformation matrix of the O2 system relative to the O1 system. Actual pose transformation matrix of O3 system relative to O1 system The actual pose transformation matrix of the O4 system relative to the O1 system

[0011] S32: Calculate the coordinates in the O1 system

[0012] S33: Select a point in the O1 system. and by Construct the M-system with the origin as the reference point;

[0013] S34: Construct the pose transformation matrix of the M system relative to the O1 system, using the O1 system as the reference coordinate system.

[0014] S35: Calculate M at the i-th position i Pose transformation matrix of the system relative to the first position of the M1 system

[0015] S36: Calculate the pose transformation matrix 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 an S-frame with the origin as the origin and an L-frame as the reference coordinate system, and construct the pose transformation matrix of the S-frame relative to the L-frame. Where S i Let S be the S-system at position i, where i = 1, 2, 3…n; calculate the S-system at position i. i Pose transformation matrix of the S1 system relative to the first position Calculate the pose transformation matrix trace value As a benchmark value;

[0017] Step S5: Construct error constraint equations and solve for the spatial poses of PSD1, PSD2, and PSD3;

[0018] Step S6: Select a point in the O1 system and by Repeat steps S33-S5 with the origin to solve for the spatial poses of PSD1, PSD2 and PSD4.

[0019] Compared with existing technologies, this invention has the following advantages: It establishes an isomorphic measurement system, and the measurement process is unconstrained by the motion state of the object under test, enabling the measurement of 6-DOF motion states. By leveraging the property that the traces (i.e., the sum of the diagonal elements of the matrix) of the pose transformation matrices calculated across multiple target coordinate systems are equal, a constraint condition is constructed. This constraint condition accurately describes the spatial relationship between the two measurement systems, thereby correcting errors, eliminating sources of motion coupling errors, and improving the calibration accuracy of the four-PSD spatial relationship. Attached Figure Description

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

[0021] Figure 2 This is a flowchart of a four-PSD spatial relationship calibration method proposed in this invention;

[0022] Figure 3 This is a schematic diagram illustrating the principle of a four-PSD spatial relationship calibration method proposed in this invention;

[0023] In the diagram: 1. PSD1; 2. PSD2; 3. PSD3; 4. PSD4; 5. Laser tracker; 6. Laser1; 7. Laser2; 8. Target ball1; 9. Target ball2; 10. Target ball3. Detailed Implementation

[0024] A four-PSD spatial relationship calibration method, which is based on a four-PSD robot kinematic parameter error calibration device, 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, with PSD-1 and PSD-3 placed at an angle to the horizontal plane. PSD-2 is positioned above PSD-1, and PSD-4 is positioned above PSD-3. Target balls-1, 2, 9, and 10 are fixed to the device. Laser tracker 5 is positioned on one side of the device, and lasers-1 and 2 are positioned above the device.

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

[0026] Step S1: Establish a coordinate system;

[0027] like Figure 1 As shown, the O1 system is established on PSD-1, the O2 system is established on PSD-2, the O3 system is established on PSD-3, the O4 system is established on PSD-4, and the L system is established on the laser tracker 5.

[0028] Step S2: Measure the light conditions;

[0029] By controlling laser 6 and laser 7 to simultaneously direct light beams onto PSD1 and PSD3, coordinates can be obtained. and coordinates The coordinates are obtained by reflecting the light from laser 6 onto PSD 2 via PSD 1. The coordinates are obtained by reflecting the light from laser 27 onto PSD33 and then onto PSD4. Simultaneously, the laser tracker 5 controls the laser beam to sequentially strike target sphere 1 (8), target sphere 2 (9), and target sphere 3 (10) 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 Ci ( C X i , C Y i , C Z i ),like Figure 3 As shown, keep laser 6 and laser 7 stationary, move the calibration device n times to 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 the actual pose transformation matrix of the O2 system relative to the O1 system. Actual pose transformation matrix of O3 system relative to O1 system The actual pose transformation matrix of the O4 system relative to the O1 system in,

[0032]

[0033] In equation (1) x m ′、y m ′、z m ′ represents O m System along O n The actual distance of translation along the X, Y, and Z axes; α m ′、β m ′、γ m ′ represents O m Tied around O n The actual angles of rotation along 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 Equals represent the design value, Δx m Equals represents the error of the parameter to be determined.

[0034] The errors of the parameters to be determined are shown in Table 1:

[0035] Table 1. Error of four PSD parameters to be determined

[0036]

[0037] Step S32: Calculate the coordinates in the O1 system

[0038] The formula is as follows:

[0039]

[0040] Step S33: As Figure 3 As shown, points are selected in the O1 system. and by Construct the M-system with the origin as the reference point, and select... and Construct the X-axis vector u i The formula is as follows:

[0041]

[0042] Selected and Construct vector l i The formula is as follows:

[0043]

[0044] Using vector u i sum vector l i Construct the Z-axis vector w i The formula is as follows:

[0045]

[0046] Using vector u i sum vector w i Construct the Y-axis vector v i The formula is as follows:

[0047]

[0048] 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:

[0049]

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

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

[0052]

[0053] in,

[0054] Step S36: Calculate the pose transformation matrix trace value It is a collection and The analytical expression for the error of the 12 unknown parameters.

[0055] Step S4: Establish the target ball measurement system;

[0056] Select point A in the L system. i B i and C i , with A i Construct the S-frame with the origin at the origin according to formulas (3)-(6), and construct the pose transformation matrix of the S-frame relative to the L-frame according to formula (7). Where S i This is the S-system at the i-th position, where i = 1, 2, 3…n; calculate the S-system at the i-th position according to formula (8). i Pose transformation matrix of the S1 system relative to the first position Calculate the pose transformation matrix trace value As a benchmark value.

[0057] Step S5: Construct the error constraint equation, as shown in the following formula:

[0058]

[0059] The PSO (Particle Swarm optimization) algorithm is used to solve the spatial poses of PSD-1, PSD-2 and PSD-3 in equation (9).

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

[0061] Example:

[0062] A four-PSD spatial relationship calibration method, which is based on a four-PSD robot kinematic parameter error calibration device, such as... Figure 1 As shown, the device includes PSD-1, PSD-2, PSD-3, PSD-4, target sphere-8, target sphere-9, and target sphere-10. PSD-1 and PSD-3 are placed at a 25° angle to the horizontal plane, with a center distance of 110mm between them. PSD-2 is positioned above PSD-1, and PSD-4 is positioned above PSD-3. Target spheres-8, 9, and 10 are fixed to the device. A laser tracker 5 is positioned on one side of the device, and lasers-16 and 7 are positioned above it. The lasers used are precision semiconductor pulsed lasers with a beam spot diameter of less than 1mm. The PSD used is a DRX-2DPSD-GJD02 with a resolution of 0.002mm, a linear error of 0.1%, and an effective surface of a 9mm*9mm square, capable of detecting the two-dimensional position of the pulsed laser beam spot on the PSD surface. The lasers and PSDs are modulated to reduce background light interference.

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

[0064] Step S1: Establish a coordinate system:

[0065] like Figure 1 As shown, the O1 system is established on PSD-1, the O2 system is established on PSD-2, the O3 system is established on PSD-3, the O4 system is established on PSD-4, and the L system is established on the laser tracker 5.

[0066] Step S2: Measure the light conditions:

[0067] By controlling laser 6 and laser 7 to simultaneously direct light beams onto PSD1 and PSD3, coordinates can be obtained. and coordinates The coordinates are obtained by reflecting the light from laser 6 onto PSD 2 via PSD 1. The coordinates are obtained by reflecting the light from laser 27 onto PSD33 and then onto PSD4. Simultaneously, the laser tracker 5 controls the laser beam to sequentially strike target sphere 1 (8), target sphere 2 (9), and target sphere 3 (10) 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 ),like Figure 3 As shown, with laser 6 and laser 7 kept stationary, the calibration device was moved 15 times to obtain 15 sets of coordinates. The data are shown in Tables 2 and 3.

[0068] Table 2. PSD Location 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 the actual pose transformation matrix of the O2 system relative to the O1 system. Actual pose transformation matrix of O3 system relative to O1 system The actual pose transformation matrix of the O4 system relative to the O1 system in,

[0075]

[0076] In equation (1) x m ′、y m ′、z m ′ represents O m System along O n The actual distance of translation along the X, Y, and Z axes; α m ′、β m ′、γ m ′ represents O m Tied around O n The actual angles of rotation along 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 +Δym ,z m ′=z m +Δz m x m Equals represent the design value, Δx m The error of the parameter to be determined is represented by the equation. The theoretical spatial relationship of the four PSDs in this embodiment is shown in Table 4.

[0077] Table 4. Theoretical Spatial Relationships of Four PSDs

[0078]

[0079] Step S32: Calculate the coordinates in the O1 system

[0080] The formula is as follows:

[0081]

[0082] Step S33: As Figure 3 As shown, points are selected in the O1 system. and by Construct the M-system with the origin as the reference point, and select... and Construct the X-axis vector u i The formula is as follows:

[0083]

[0084] Selected and Construct vector l i The formula is as follows:

[0085]

[0086] Using vector u i sum vector l i Construct the Z-axis vector w i The formula is as follows:

[0087]

[0088] Using vector u i sum vector w i Construct the Y-axis vector v i The formula is as follows:

[0089]

[0090] 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:

[0091]

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

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

[0094]

[0095] in,

[0096] Step S36: Calculate the pose transformation matrix trace value It is a collection and The analytical expression for the error of the 12 unknown parameters.

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

[0098] Select point A in the L system. i B i and C i , with A i Construct the S-frame with the origin at the origin according to formulas (3)-(6), and construct the pose transformation matrix of the S-frame relative to the L-frame according to formula (7). Where S i This is the S-system at the i-th position, where i = 1, 2, 3…15; calculate the S-system at the i-th position according to formula (8). i Pose transformation matrix of the S1 system relative to the first position Calculate the pose transformation matrix trace value As a benchmark value;

[0099] Step S5: Construct the error constraint equation, as shown in the following formula:

[0100]

[0101] The PSO (Particle Swarm optimization) algorithm is used to solve equation (9) to obtain the spatial poses of PSD-1, PSD-2 and PSD-3.

[0102] Step S6: Select a point in the O1 system and by Repeat steps S33-S5 with the origin to solve for 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. Results of PSD parameter error identification

[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; Create the O1 system on PSD1, the O2 system on PSD2, the O3 system on PSD3, the O4 system on PSD4, and the L system on the laser tracker. Step S2: Measure the light conditions; By controlling laser one and laser two to simultaneously direct light beams onto PSD one and PSD three, coordinates can be obtained. and coordinates The coordinates are obtained by reflecting the light from laser one onto PSD two via PSD one. The light from laser two is reflected by PSD three onto PSD four to obtain coordinates. Simultaneously, the laser tracker is controlled to sequentially direct light beams toward target sphere one, target sphere two, and target sphere three to obtain 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 Keeping laser one and laser two stationary, move the calibration device n times to 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 pose transformation matrix of the O2 system relative to the O1 system. Actual pose transformation matrix of O3 system relative to O1 system The actual pose transformation matrix of the O4 system relative to the O1 system S32: Calculate the coordinates in the O1 system S33: Select a point in the O1 system. and by Construct the M-system with the origin as the reference point; S34: Using the O1 system as the reference coordinate system, construct the pose transformation matrix of the M system relative to the O1 system. S35: Calculate M at the i-th position i Pose transformation matrix of the system relative to the first position of the M1 system S36: Calculate the pose transformation matrix 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 an S-frame with the origin as the origin and an L-frame as the reference coordinate system, and construct the pose transformation matrix of the S-frame relative to the L-frame. Where S i Let S be the S-system at position i, where i = 1, 2, 3…n; calculate the S-system at position i. i Pose transformation matrix of the S1 system relative to the first position Calculate the pose transformation matrix trace value As a benchmark value; Step S5: Construct error constraint equations and solve for the spatial poses of PSD1, PSD2, and PSD3; Step S6: Select a point in the O1 system and by Repeat steps S33-S5 with the origin to solve for the spatial poses of PSD1, PSD2 and PSD4.

2. The four-PSD spatial relationship calibration method according to claim 1, characterized in that, Step S3 specifically includes: Step S31: Establish the actual pose transformation matrix of the O2 system relative to the O1 system. Actual pose transformation matrix of O3 system relative to O1 system The actual pose transformation matrix of the O4 system relative to the O1 system in, In equation (1) x m ′、y m ′、z m ′ represents O m System along O n The actual distance of translation along the X, Y, and Z axes; α m ′、β m ′、γ m ′ represents O m Tied around O n The actual angles of rotation along 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 y m z m α m β m and γ m Indicates the design value, Δx m Δy m Δz m , Δα m Δβ m and Δγ m This indicates 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 the M-system with the origin as the reference point, and select... and Construct the X-axis vector u i The formula is as follows: Selected and Construct vector l i The formula is as follows: Using vector u i sum vector l i Construct the Z-axis vector w i The formula is as follows: Using vector u i sum 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: Where M i It is the M system at the i-th position, where i = 1, 2, 3…n; Step S35: Calculate M at the i-th position i Pose transformation matrix of the system relative to the first position of the M1 system The formula is as follows: in, Step S36: Calculate the pose transformation matrix trace value It is a collection and Analytical expressions for the errors of the 12 parameters to be determined; 3. The four-PSD spatial relationship calibration method according to claim 2, characterized in that, Step S4 specifically involves: Select point A in the L system. i B i and C i , with A i Construct the S-frame with the origin at the origin according to formulas (3)-(6), and construct the pose transformation matrix of the S-frame relative to the L-frame according to formula (7). Where S i This is the S-system at the i-th position, where i = 1, 2, 3…n; calculate the S-system at the i-th position according to formula (8). i Pose transformation matrix of the S1 system relative to the first position Calculate the pose transformation matrix trace value As a benchmark value.

4. The 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 PSD1, PSD2 and PSD3 in equation (9).

Images