Robot kinematics parameter calibration method based on four PSDs

Abstract

The invention discloses a robot kinematics parameter calibration method based on four PSDs, relates to the technical field of joint robot calibration, and aims to solve the problem that the absolute calibration precision of an industrial robot is directly influenced by the pose precision of a tool system relative to the tail end of the robot and the pose precision of a PSD device relative to the robot in an existing calibration method. The method comprises the following steps of S1, establishing a measurement system, S2, measuring the light state, S3, calculating the linear distance of different planes under an O1 system, S4, establishing a robot kinematics parameter error identification model, S5, calculating the linear distance of different planes under an O system, and S6, constructing an error constraint equation to identify and solve the robot kinematics parameter error. According to the method, the robot kinematics parameter error model is constructed based on the distance between the laser lines in different planes, the robot kinematics parameter error is identified through the PSO algorithm, the method does not depend on the pose precision of a tool system relative to the tail end of the robot and the pose precision of a PSD device relative to the robot, and the robot kinematics parameter calibration precision is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of articulated robot calibration, and in particular to a robot kinematic parameter calibration method based on 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 106777656 A", and the patent name is "A method for absolute precision calibration of industrial robots based on PMPSD". This method establishes a robot error kinematic model, and then uses the PSD device to change the position multiple times to perform multiple data sampling to construct a multi-point virtual constraint in space; uses the spatial vector relationship to correct the position of the robot's end laser, and uses the corrected position to correct the robot's joint angle to replace the joint angle obtained from the robot teach pendant or controller; constructs a model constraint objective function, and uses the minimization method to optimize the constraint objective function to obtain the parameter error of the industrial robot. Finally, the parameter error is used to correct the nominal value of the geometric parameter, complete the kinematic calibration of the industrial robot, and achieve the absolute precision calibration of the robot. The shortcomings of this method are: the posture accuracy of the tool system relative to the end of the robot and the posture accuracy of the PSD device relative to the robot directly affect the absolute calibration accuracy of the industrial robot. Summary of the invention

[0004] In order to solve the problem in the existing calibration methods that the posture accuracy of the tool system relative to the robot end and the posture accuracy of the PSD device relative to the robot directly affect the absolute calibration accuracy of the industrial robot, the present invention proposes a robot kinematic parameter calibration method based on four PSDs to improve the calibration accuracy of the robot kinematic parameters.

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

[0006] A robot kinematic parameter calibration method based on four PSDs, the method comprising the following steps:

[0007] Step S1: Establish measurement 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, establish O system on the robot base, establish O system on the robot end E On the laser, establish O L Tie;

[0008] Step S2: Measure the light state: Control the laser to Shoot to PSD-1 to get the coordinates Light Reflect from PSD 1 to PSD 2 to get the coordinates Control the laser to Shoot to PSD3 to get the coordinates Light Reflect from PSD 3 to PSD 4 to get the coordinates Repeat the above steps n times to obtain n sets of coordinates and robot joint angle data, where i represents the number of sets, i = 1, 2, 3...n;

[0009] Step S3: Calculate the distance between the two planes in the O1 system:

[0010] S31: Calculate the coordinates in the O1 system

[0011] S32: Calculation Symmetrical points based on the XOY plane of the O1 system and Symmetrical points based on the XOY plane of the O3 system

[0012] S33: Calculate the reference distance

[0013] Step S4: Establishing a robot kinematic parameter error identification model:

[0014] S41: Build O L Relative to O E The actual pose transformation matrix of the system

[0015] S42: Establish the actual kinematic model of the robot;

[0016] Step S5: Calculate the distance between the different planes in the O system:

[0017] S51: Calculate the pose matrix and

[0018] S52: Calculate incident light and point and

[0019] S53: Calculate distance expression

[0020] Step S6: construct error constraint equations to identify and solve the robot kinematic parameter errors.

[0021] Compared with the prior art, the present invention has the following beneficial effects: the present invention proposes a robot kinematic parameter calibration method based on four PSDs, constructs a robot kinematic parameter error model based on the spacing of eccentric laser lines, and adopts the PSO algorithm to identify the robot kinematic parameter error. This method does not rely on the posture accuracy of the tool system relative to the robot end and the posture accuracy of the PSD device relative to the robot, thereby improving the calibration accuracy of the robot kinematic parameters. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Figure 1 A schematic diagram of a device for calibrating robot kinematic parameters based on four PSDs according to the present invention;

[0023] Figure 2 It is a flow chart of a robot kinematic parameter calibration method based on four PSDs of the present invention;

[0024] In the figure: 1. PSD 1; 2. PSD 2; 3. PSD 3; 4. PSD 4; 5. Robot end; 6. Laser; 7. Robot. DETAILED DESCRIPTION

[0025] A robot kinematic parameter calibration method based on four PSDs, the method is based on a robot motion indicator spot position rapid acquisition device of four PSDs, such as Figure 1 As shown, the device includes PSD-1, PSD-2, PSD-3, PSD-4, a robot end 5, a laser 6, and a robot 7. PSD-1 and PSD-3 are placed at an angle to the horizontal plane, PSD-2 is arranged above PSD-1, PSD-44 is arranged above PSD-3, and the robot 7 is arranged on one side of the device. The robot 7 includes the robot end 5, and the laser 6 is fixedly connected to the robot end 5.

[0026] like Figure 2 As shown, a robot kinematic parameter calibration method based on four PSDs is shown, and the specific steps of the method are as follows:

[0027] Step S1: Establishing a measurement system;

[0028] 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, the O system is established on the base of the robot 7, and the O system is established on the end of the robot 5. E On laser 6, establish O L Tie.

[0029] Step S2: measuring light status;

[0030] Control laser 6 to emit light Shoot to PSD-1 to get the coordinates Light The coordinates are obtained by reflecting from PSD-1 to PSD-2 Control laser 6 to emit light Shoot to PSD3 to get the coordinates Light Reflect from PSD 3 to PSD 4 to get the coordinates Repeat the above steps n times to obtain n sets of coordinates and robot 7 joint angle data, where i represents the number of sets, i = 1, 2, 3...n.

[0031] Step S3: Calculate the distance between the two planes in the O1 system:

[0032] Step S31: Calculate the coordinates in the O1 system

[0033] The formula is as follows:

[0034]

[0035] in represents the posture transformation matrix of O2 system relative to O1 system, represents the posture transformation matrix of O3 system relative to O1 system, Represents the posture transformation matrix of O4 system relative to O1 system;

[0036] Step S32: Calculation Symmetrical points based on the XOY plane of the O1 system The formula is as follows:

[0037]

[0038] calculate Symmetrical points based on the XOY plane of the O3 system The formula is as follows:

[0039]

[0040] In formula (3), A, B, C, and D are the coefficients of the plane equation of the XOY plane of the O3 system based on the O1 system;

[0041] Step S33: Calculate the reference distance

[0042]

[0043] In formula (4), d mn represents the distance between points m and n, Representation vector The model, ( m X, m Y, m Z) represents the coordinate value of point m;

[0044]

[0045] In formula (5) Representation vector and vector The normal vector of

[0046]

[0047] In formula (6), cos(θ) represents the vector and vector The cosine of the angle between

[0048] d x =d mn cos(θ) (7)

[0049] In formula (7), d x represents the distance between the non-coplanar lines mp and nq in the x system;

[0050] Will and Substitute into formula (4)-(7) to calculate the O1 system The distance between As the reference distance, i = 1, 2, 3…n.

[0051] Step S4: Establishing a robot kinematic parameter error identification model:

[0052] Step S41: Establish O L Relative to O E The actual pose transformation matrix of the system The formula is as follows:

[0053]

[0054] In formula (8), 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 parameter to be determined;

[0055] Step S42: Establish the actual kinematic model of the robot 7, the model is as follows:

[0056]

[0057] In formula (9), θ i ′ represents the actual rotation angle of joint i; a i-1 ′ represents the actual length of the connecting rod; α i-1 ' represents the actual torsion angle; d i ′ represents the actual bias. i-1 ′=α i-1 +Δα i-1 ′、θ i ′=θ i +Δθ i 、a i-1 ′=a i-1 +Δa i-1 d i ′=d i +Δd i ; α i-1 etc. represent design values, Δα i-1 etc. represent the error of the parameters to be determined.

[0058] The robot kinematic parameter errors are shown in Table 1:

[0059] Table 1 Robot kinematic parameter errors

[0060]

[0061] Step S5: Calculate the distance between the different planes in the O system

[0062] Step S51: Calculate the pose matrix and The formula is as follows:

[0063]

[0064] In formula (10), When the incident point is m, O L The pose matrix of the system relative to the O system;

[0065] Step S52: Calculate incident light and point and The formula is as follows:

[0066]

[0067] In formula (11) express From the first row to the third row, express The fourth column from the first to the third row;

[0068] Step S53: Calculate the distance expression

[0069] Will Substitute into formula (4)-(7) to calculate the incident light in the O system and The distance expression between Contains the error parameters to be determined, i=1,2,3…n.

[0070] Step S6: constructing error constraint equations to identify robot kinematic parameter errors;

[0071] Construct the error constraint equation as shown below:

[0072]

[0073] The PSO (Particle Swarm Optimization) algorithm is used to solve equation (12): represents the i-th distance expression, represents the i-th reference distance.

[0074] Example:

[0075] A robot kinematic parameter calibration method based on four PSDs, the method is based on a robot motion indicator spot position rapid acquisition device of four PSDs, such as Figure 1As shown, the device includes PSD-1, PSD-2, PSD-3, PSD-4, robot end 5, laser 6, laser tracker 7, robot 8, first target ball 9 and second target ball 10, 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, robot 7 is arranged on one side of the device, robot 7 includes robot end 5, laser 6 is fixedly connected to robot end 5; the laser 6 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 6 and PSD are modulated to reduce background light interference.

[0076] A robot kinematic parameter calibration method based on four PSDs, the specific steps of the method are as follows:

[0077] Step S1: Establishing a measurement system;

[0078] 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, the O system is established on the base of the robot 7, and the O system is established on the end of the robot 5. E On laser 6, establish O L Tie.

[0079] Step S2: measuring light status;

[0080] Control laser 6 to emit light Shoot to PSD-1 to get the coordinates Light The coordinates are obtained by reflecting from PSD-1 to PSD-2 Control laser 6 to emit light Shoot to PSD3 to get the coordinates Light Reflect from PSD 3 to PSD 4 to get the coordinates Repeat the above steps 15 times to obtain 15 sets of coordinates and robot 7 joint angle data. The data are shown in Table 2-4:

[0081] Table 2 Four PSD position data

[0082]

[0083] Table 3 The first group of robot joint angle data

[0084]

[0085] Table 4 The second group of robot joint angle data

[0086]

[0087]

[0088] Step S3: Calculate the distance between the different planes in the O1 system;

[0089] Step S31: Calculate the coordinates in the O1 system

[0090] The formula is as follows:

[0091]

[0092] in represents the posture transformation matrix of O2 system relative to O1 system, represents the posture transformation matrix of O3 system relative to O1 system, It represents the posture transformation matrix of the O4 system relative to the O1 system. The theoretical spatial relationship of the four PSDs in this embodiment is shown in Table 5:

[0093] Table 5 Theoretical spatial relationships of the four PSDs

[0094]

[0095] Step S32: Calculation Symmetrical points based on the XOY plane of the O1 system The formula is as follows:

[0096]

[0097] calculate Symmetrical points based on the XOY plane of the O3 system The formula is as follows:

[0098]

[0099] In formula (3), A, B, C, and D are the coefficients of the plane equation of the XOY plane of the O3 system based on the O1 system;

[0100] Step S33: Calculate the reference distance

[0101]

[0102] In formula (4), d mn represents the distance between points m and n, Representation vector The model, ( m X, m Y, m Z) represents the coordinate value of point m;

[0103]

[0104] In formula (5) Representation vector and vector The normal vector of

[0105]

[0106] In formula (6), cos(θ) represents the vector and vector The cosine of the angle between

[0107] d x =d mn cos(θ) (7)

[0108] In formula (7), d x represents the distance between the non-coplanar lines mp and nq in the x system;

[0109] Will and Substitute into formula (4)-, (7) to calculate the O1 system The distance between As the reference distance, i = 1, 2, 3…n.

[0110] Step S4: Establishing a robot kinematic parameter error identification model;

[0111] The robot 7 in this embodiment adopts a Sino-Swiss robot, and the DH parameters are shown in Table 6:

[0112] Table 6 DH parameters of Sino-Swiss robot

[0113]

[0114] Step S41: Establish O L Relative to O E The actual pose transformation matrix of the system The formula is as follows:

[0115]

[0116] In formula (8), x m ′、y m ′、z m ' indicates O m Along O nThe 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.

[0117] Step S42: Establish the actual kinematic model of the robot 7, the formula is as follows:

[0118]

[0119] In formula (9), θ i ′ represents the actual rotation angle of joint i; a i-1 ′ represents the actual length of the connecting rod; α i-1 ' represents the actual torsion angle; d i ′ represents the actual bias. i-1 ′=α i-1 +Δα i-1 ′、θ i ′=θ i +Δθ i 、a i-1 ′=a i-1 +Δa i-1 d i ′=d i +Δd i ; α i-1 etc. represent design values, Δα i-1 etc. represent the error of the parameters to be determined.

[0120] Step S5: Calculate the distance between the different planes in the O system;

[0121] Step S51: Calculate the pose matrix and The formula is as follows:

[0122]

[0123] In formula (10), When the incident point is m, O L The pose matrix of the system relative to the O system.

[0124] Step S52: Calculate incident light and point and The formula is as follows

[0125]

[0126] In formula (11) express From the first row to the third row, express The fourth column from the first to the third row;

[0127] Will Substitute into formula (4)-(7) to calculate the incident light in the O system and The distance expression between Contains the error parameters to be determined, i=1,2,3...n.

[0128] Step S6: constructing error constraint equations to identify robot kinematic parameter errors;

[0129] Construct the error constraint equation as shown below:

[0130]

[0131] The PSO algorithm is used to solve equation (12): represents the i-th distance expression, represents the i-th reference distance, and the robot kinematic parameter error identification results are shown in Table 7.

[0132] Table 7 Robot kinematic parameter error identification results

[0133]

Claims

1. A robot kinematic parameter calibration method based on four PSDs, characterized in that: The method comprises the following steps: Step S1: Establish measurement 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, establish O system on the robot base, establish O system on the robot end E On the laser, establish O L Tie; Step S2: Measure the light state: Control the laser to Shoot to PSD and get the coordinates Light Reflect from PSD 1 to PSD 2 to get the coordinates Control the laser to Shoot to PSD3 to get the coordinates Light Reflect from PSD 3 to PSD 4 to get the coordinates Repeat the above steps n times to obtain n sets of coordinates and robot joint angle data, where i represents the number of sets, i = 1, 2, 3...n; Step S3: Calculate the distance between the two planes in the O1 system: S31: Calculate the coordinates in the O1 system S32: Calculation Symmetrical points based on the XOY plane of the O1 system and Symmetrical points based on the XOY plane of the O3 system S33: Calculate the reference distance Step S4: Establishing a robot kinematic parameter error identification model: S41: Build O L Relative to O E The actual pose transformation matrix of the system S42: Establish the actual kinematic model of the robot; Step S5: Calculate the distance between the different planes in the O system: S51: Calculate the pose matrix and S52: Calculate incident light and point and S53: Calculate distance expression Step S6: construct error constraint equations to identify robot kinematic parameter errors.

2. The robot kinematic parameter calibration method based on four PSDs according to claim 1, characterized in that: The specific steps of step S3 are as follows: Step S31: Calculate the coordinates in the O1 system The formula is as follows: in represents the posture transformation matrix of O2 system relative to O1 system, represents the posture transformation matrix of O3 system relative to O1 system, Represents the posture transformation matrix of O4 system relative to O1 system; Step S32: Calculation Symmetrical points based on the XOY plane of the O1 system The formula is as follows: calculate Symmetrical points based on the XOY plane of the O3 system The formula is as follows: In formula (3), A, B, C, and D are the coefficients of the plane equation of the XOY plane of the O3 system based on the O1 system; Step S33: Calculate the reference distance In formula (4), d mn represents the distance between points m and n, Representation vector The model, ( m X, m Y, m Z) represents the coordinate value of point m; In formula (5) Representation vector and vector The normal vector of In formula (6), cos(θ) represents the vector and vector The cosine of the angle between d x =d mn cos(θ) (7) In formula (7), d x represents the distance between the non-coplanar lines mp and nq in the x system; Will and Substitute into formula (4)-(7) to calculate the O1 system The distance between As the reference distance, i = 1, 2, 3…n.

3. The robot kinematic parameter calibration method based on four PSDs according to claim 2, characterized in that: The specific steps of step S4 are as follows: Step S41: Establish O L Relative to O E The actual pose transformation matrix of the system The formula is as follows: In formula (8), 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 parameter to be determined; Step S42: Establish the actual kinematic model of the robot, the model is as follows: In formula (9), θ i ′ represents the actual rotation angle of joint i; a i-1 ′ represents the actual length of the connecting rod; α i-1 ′ represents the actual torsion angle; d i ′ represents the actual bias; α i-1 ′=α i-1 +Δα i-1 ′、θ i ′=θ i +Δθ i 、a i-1 ′=a i-1 +Δa i-1 ,d i ′=d i +Δd i ; α i-1 etc. represent the design value, Δα i-1 etc. represent the error of the parameters to be determined.

4. The robot kinematic parameter calibration method based on four PSDs according to claim 3 is characterized in that: The specific steps of step S5 are as follows: Step S51: Calculate the pose matrix and The formula is as follows: In formula (10), When the incident point is m, O L The pose matrix of the system relative to the O system; Step S52: Calculate incident light and point and The formula is as follows: In formula (11) express From the first row to the third row, express The fourth column from the first to the third row; Step S53: Calculate the distance expression Will Substitute into formula (4)-(7) to calculate the incident light in the O system and The distance expression between Contains the error parameters to be determined, i=1,2,3…n.

5. The robot kinematic parameter calibration method based on four PSDs according to claim 4 is characterized in that: The specific steps of step S6 are as follows: Construct the error constraint equation as shown below: The PSO algorithm is used to solve equation (12): represents the i-th distance expression, represents the i-th reference distance.

Images