Three-dimensional calibration optimization method for six-axis physiotherapy robot

Abstract

The invention discloses a three-dimensional calibration optimization method for a six-axis physiotherapy robot, and relates to the field of robot control, and the method comprises the steps: obtaining a first theoretical calibration matrix through calculation; calculating to obtain a second theoretical calibration matrix; obtaining a third theoretical calibration matrix; setting a three-dimensional feature platform; obtaining theoretical three-dimensional feature coordinates of the three-dimensional feature points in the base coordinate system; forming a feature automatic identification alignment mechanism to obtain actual three-dimensional feature coordinates; and generating at least one optimization compensation matrix, performing screening to obtain a target optimization compensation matrix, and using a compensation result of the target optimization compensation matrix on the theoretical three-dimensional feature coordinates as optimized coordinates. By forming a third theoretical calibration matrix, theoretical three-dimensional feature coordinates and a feature automatic identification alignment mechanism, coordinates measured by a calibration tool are converted into actual three-dimensional feature coordinates, and by forming a target optimization compensation matrix, subsequent measurement results are compensated, so that measurement errors are effectively controlled.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of robot control, in particular to a six-axis physiotherapy robot three-dimensional calibration optimization method. BACKGROUND

[0002] The calibration of the six-axis robot three-dimensional vision system mainly refers to obtaining the spatial transformation matrix between the output coordinate system of the mechanical arm and the 3D camera coordinate system. The six-axis robot mainly has two three-dimensional system layout schemes: hand over eye and hand outside eye, and the three-dimensional calibration schemes of the two layouts are quite different. Fixed and mobile 3D cameras or calibration boards are used to collect images at different spatial positions. The calibration accuracy is generally between 2cm and 3cm.

[0003] The six-axis robot is used in the physiotherapy industry, and the calibration accuracy of the vision is very high, which needs to be guaranteed at the millimeter level. The current six-axis robot three-dimensional vision calibration has the problem of insufficient accuracy, which is difficult to meet the product demand. SUMMARY

[0004] To solve the above technical problems, a six-axis physiotherapy robot three-dimensional calibration optimization method is provided, which solves the problems in the above background technology.

[0005] To achieve the above purpose, the technical scheme adopted by the present application is:

[0006] A six-axis physiotherapy robot three-dimensional calibration optimization method, comprising:

[0007] Obtaining a 3D camera arranged at the end of the six-axis arm of the physiotherapy robot, taking the axis arm connected with the 3D camera among the six-axis arms of the physiotherapy robot as a tool mechanical arm, and taking the initial end of the six-axis arm of the physiotherapy robot as a basic position;

[0008] Taking the coordinate system used by the 3D camera as the 3D camera coordinate system, taking the coordinate system used by the tool mechanical arm as the mechanical arm tool coordinate system, and taking the coordinate system used by the basic position as the base coordinate system;

[0009] According to the Euler angle, a first theoretical calibration matrix M_C2T of the 3D camera coordinate system transformed to the mechanical arm tool coordinate system is calculated;

[0010] According to the Euler angle, a second theoretical calibration matrix M_T2B of the mechanical arm tool coordinate system transformed to the base coordinate system is calculated;

[0011] The second theoretical calibration matrix is multiplied by the first theoretical calibration matrix to obtain a third theoretical calibration matrix M_C2B of the 3D camera coordinate system transformed to the base coordinate system;

[0012] Setting a three-dimensional feature platform;

[0013] The center pixel position of the three-dimensional feature point in the three-dimensional feature platform is identified in the coordinates of the 3D camera coordinate system, depth matching is performed on the 3D camera, and the theoretical three-dimensional feature coordinates of the three-dimensional feature point in the base coordinate system are obtained;

[0014] A feature automatic recognition alignment mechanism is formed, and the feature automatic recognition alignment mechanism is used to obtain the actual three-dimensional feature coordinates of the three-dimensional feature point in the base coordinate system;

[0015] According to the actual three-dimensional feature coordinates and the theoretical three-dimensional feature coordinates, at least one optimized compensation matrix M_B2R is generated, the target optimized compensation matrix is screened according to the compensation condition of the optimized compensation matrix on the theoretical three-dimensional feature coordinates, and the compensation result of the target optimized compensation matrix on the theoretical three-dimensional feature coordinates is used as the optimized coordinates.

[0016] Preferably, the calculation of the first theoretical calibration matrix of the transformation of the 3D camera coordinate system to the mechanical arm tool coordinate system comprises the following steps:

[0017] The first translation amount (A, B, C) of the coordinate origin of the 3D camera coordinate system moving to the coordinate origin of the mechanical arm tool coordinate system is obtained;

[0018] After the first translation amount (A, B, C) is translated, the first rotation amount of the 3D camera coordinate system rotating to coincide with the mechanical arm tool coordinate system is obtained, and the first rotation amount is composed of a first roll angle Roll, a first pitch angle Pitch and a first yaw angle Yaw, wherein the first roll angle is the angle of rotation of the 3D camera coordinate system around the z-axis, the first pitch angle is the angle of rotation of the 3D camera coordinate system around the x-axis, and the first yaw angle is the angle of rotation of the 3D camera coordinate system around the y-axis;

[0019] A first roll angle matrix, a first pitch angle matrix and a first yaw angle matrix are generated, and the first roll angle matrix, the first pitch angle matrix and the first yaw angle matrix are multiplied to obtain a first rotation matrix;

[0020] According to the first rotation matrix and the first translation amount, a first theoretical calibration matrix is formed.

[0021] Preferably, the calculation of the second theoretical calibration matrix of the transformation of the mechanical arm tool coordinate system to the base coordinate system comprises the following steps:

[0022] The second translation amount (a, b, c) of the coordinate origin of the mechanical arm tool coordinate system moving to the coordinate origin of the base coordinate system is obtained;

[0023] After the second translation (a, b, c) is translated, a second rotation amount of which the robot tool coordinate system is rotated to coincide with the base coordinate system is obtained, the second rotation amount is composed of a second roll angle, a second pitch angle and a second yaw angle, wherein the second roll angle is an angle of rotation of the robot tool coordinate system around the z-axis, the second pitch angle is an angle of rotation of the robot tool coordinate system around the x-axis, and the second yaw angle is an angle of rotation of the robot tool coordinate system around the y-axis;

[0024] A second roll angle matrix, a second pitch angle matrix and a second yaw angle matrix are generated, and the second roll angle matrix, the second pitch angle matrix and the second yaw angle matrix are multiplied to obtain a second rotation matrix;

[0025] According to the second rotation matrix and the second translation, a second theoretical calibration matrix is formed.

[0026] Preferably, the setting of the three-dimensional feature platform comprises the following steps:

[0027] The three-dimensional feature platform is a pre-set fixed shape platform, 13 three-dimensional feature points are arranged on the surface of the three-dimensional feature platform, the part of the three-dimensional feature platform other than the three-dimensional feature points is a plane and is white in color, the height of the three-dimensional feature points is higher than the rest of the three-dimensional feature platform, and the three-dimensional feature points are black.

[0028] Preferably, the identification of the coordinates of the center pixel position of the three-dimensional feature points in the three-dimensional feature platform in the 3D camera coordinate system comprises the following steps:

[0029] The contour of the three-dimensional feature points is obtained by contour recognition on the image obtained by the 3D camera;

[0030] A sampling coordinate system is obtained by modeling a plane coordinate system in the image obtained by the 3D camera, at least one sampling point is evenly taken on the contour of the three-dimensional feature points, and the sampling coordinates of the sampling points in the sampling coordinate system are obtained;

[0031] The sampling coordinates of the at least one sampling point are averaged to obtain the image coordinates (u, v) of the center pixel position, and the coordinates (g, d) of the center pixel position in the xy plane of the 3D camera coordinate system are comprehensively calculated according to the physical focal length of the 3D camera and the coordinates of the center of the image obtained by the 3D camera in the sampling coordinate system;

[0032] An infrared light pulse is emitted from the 3D camera to the three-dimensional feature point, the included angle K between the infrared light pulse and the z-axis of the 3D camera coordinate system is obtained, the direction of the z-axis of the 3D camera coordinate system is perpendicular to the direction of the 3D camera mirror, the interval T between the sending and receiving of the infrared light pulse is obtained, and the depth value e is comprehensively calculated;

[0033] (g, d, e) is taken as the coordinates of the center pixel position in the 3D camera coordinate system.

[0034] Preferably, the method for obtaining the theoretical three-dimensional feature coordinates of the three-dimensional feature points in the base coordinate system comprises the following steps:

[0035] The third theoretical calibration matrix is multiplied by the coordinates of the center pixel position in the 3D camera coordinate system to obtain the theoretical three-dimensional feature coordinates.

[0036] Preferably, the forming of the feature automatic recognition alignment mechanism comprises the following steps:

[0037] An infrared camera at the end of the mechanical arm is used to respectively collect infrared indication and infrared ranging laser contrast images of the three-dimensional feature platform single block;

[0038] Through image segmentation recognition, the first center of the infrared indication light spot and the second center of the laser ranging light spot are respectively obtained;

[0039] The area of the minimum actual range of the infrared image captured by the infrared camera is obtained as the feature area, and the number of pixels of the infrared image captured by the infrared camera is obtained as the feature value;

[0040] The feature area and the feature value are square rooted to obtain the regulation coefficient m;

[0041] The row and column pixel difference (ΔU, ΔV) of the first center and the second center is determined, ΔU is the number of pixels of the interval of the first center and the second center in the horizontal direction, and ΔV is the number of pixels of the interval of the first center and the second center in the vertical direction;

[0042] During regulation, the height of the end of the mechanical arm is unchanged, ΔX in the X-axis direction of the base coordinate system and ΔY in the Y-axis direction of the base coordinate system are moved, ΔX = mΔU, and ΔY = mΔV;

[0043] After regulation, if the length of the row and column pixel difference of the first center and the second center exceeds the allowable error, the position of the first center and the second center is updated, and the above step is repeated until the length of the row and column pixel difference of the first center and the second center does not exceed the allowable error.

[0044] Preferably, the method for obtaining the actual three-dimensional feature coordinates of the three-dimensional feature points in the base coordinate system comprises the following steps:

[0045] During calibration, a fixed posture value is selected, and the first center (X1, Y1, Z1) of the working end under the fixed posture is measured by the calibration tool TCP;

[0046] The second center (X2, Y2, Z2) of the laser range finder after the calibration tool is installed is obtained;

[0047] After feature recognition and alignment using the feature automatic recognition alignment mechanism, the three-dimensional coordinate position (Xi , Y i , Z i );

[0048] The actual three-dimensional feature coordinates are calculated as (X i , Y i , Z i ) + (X1, Y1, Z1) - (X2, Y2, Z2).

[0049] Preferably, the generating at least one optimized compensation matrix according to the actual three-dimensional feature coordinates and the theoretical three-dimensional feature coordinates comprises the following steps:

[0050] Forming at least one random feature point combination, the random feature point combination being composed of any 4 of the 13 three-dimensional feature points;

[0051] Using the actual three-dimensional feature coordinates and the theoretical three-dimensional feature coordinates of the three-dimensional feature points in the random feature point combination to solve the equations, the optimized compensation matrix is obtained by back-solving;

[0052] The optimized compensation matrix is as follows:

[0053]

[0054] Wherein, r 11 , r 12 , r 13 , r 21 , r 22 , r 23 , r 31 , r 32 , r 33 , t1, t2 and t3 are unknown quantities to be solved.

[0055] Preferably, the screening to obtain the target optimized compensation matrix according to the compensation of the theoretical three-dimensional feature coordinates by the optimized compensation matrix comprises the following steps:

[0056] The optimized compensation matrix is multiplied by the theoretical three-dimensional feature matrix to obtain a compensated three-dimensional feature matrix, and the compensated three-dimensional feature coordinates are obtained from the compensated three-dimensional feature matrix;

[0057] According to the compensated three-dimensional feature coordinates of the optimized compensation matrix and the actual three-dimensional feature coordinates, the difference sum of squares error Er of the optimized compensation matrix is generated, and the optimized compensation matrix with the minimum difference sum of squares error Er is taken as the target optimized compensation matrix;

[0058] The theoretical three-dimensional feature matrix is as follows:

[0059]

[0060] Wherein, i is an index, (B_X iB_Y i B_Z i is the theoretical three-dimensional feature coordinate of the i th three-dimensional feature point;

[0061] The compensation three-dimensional feature matrix is as follows:

[0062]

[0063] Wherein, (L_X i , L_Y i , L_Z i ) is the compensation three-dimensional feature coordinate of the i th three-dimensional feature point;

[0064] The difference and square error are as follows:

[0065]

[0066] Wherein, (R_X i , R_Y i , R_Z i ) is the actual three-dimensional feature coordinate of the i th three-dimensional feature point.

[0067] Compared with the prior art, the beneficial effects of the present application are that:

[0068] By forming a third theoretical calibration matrix, forming a theoretical three-dimensional feature coordinate, forming a feature automatic recognition alignment mechanism and obtaining a target optimization compensation matrix, the coordinates can be recognized by the calibration tool, and the coordinates measured by the calibration tool are converted into actual three-dimensional feature coordinates by the translation of the position of the calibration tool relative to the actual measurement position, and the coordinates collected by the 3D camera are converted into coordinates in the base coordinate system through the third theoretical calibration matrix, so that the actual three-dimensional feature coordinates and the theoretical three-dimensional feature coordinates can be processed in a unified coordinate system, and then the target optimization compensation matrix is formed according to the difference, and the subsequent measurement results are compensated, thereby effectively controlling the measurement error. BRIEF DESCRIPTION OF DRAWINGS

[0069] Figure 1 is a flowchart of the six-axis physiotherapy robot three-dimensional calibration optimization method of the present application;

[0070] Figure 2 is a flowchart of the first theoretical calibration matrix of the present application for calculating the transformation of the 3D camera coordinate system to the robot tool coordinate system;

[0071] Figure 3 is a flowchart of the second theoretical calibration matrix of the present application for calculating the transformation of the robot tool coordinate system to the base coordinate system;

[0072] Figure 4A flowchart of a process for obtaining coordinates of a center pixel position of a three-dimensional feature point in a three-dimensional feature platform in a 3D camera coordinate system according to the present application;

[0073] Figure 5 A flowchart of a process for forming a feature automatic recognition alignment mechanism according to the present application;

[0074] Figure 6 A flowchart of a process for obtaining actual three-dimensional feature coordinates of a three-dimensional feature point in a base coordinate system according to the present application;

[0075] Figure 7 A flowchart of a process for generating at least one optimized compensation matrix according to actual three-dimensional feature coordinates and theoretical three-dimensional feature coordinates according to the present application;

[0076] Figure 8 A flowchart of a process for screening a target optimized compensation matrix according to compensation of a theoretical three-dimensional feature coordinate by an optimized compensation matrix according to the present application;

[0077] Figure 9 A whole structure diagram of a six-axis robot according to the present application. DETAILED DESCRIPTION

[0078] The following description is provided to enable any person skilled in the art to practice the application. The preferred embodiments described in the following description are only examples of the application and other obvious variants can be conceived by those skilled in the art.

[0079] REFERENCE Figure 1 As shown in the drawings, a six-axis physiotherapy robot three-dimensional calibration optimization method comprises:

[0080] A 3D camera is arranged at the end of each of the six axes of the physiotherapy robot, and the axis of the physiotherapy robot connected to the 3D camera is taken as a tool mechanical arm, and the initial end of the physiotherapy robot controlling the six axes is taken as a base position;

[0081] The coordinate system adopted by the 3D camera is taken as a 3D camera coordinate system, the coordinate system adopted by the tool mechanical arm is taken as a mechanical arm tool coordinate system, and the coordinate system adopted by the base position is taken as a base coordinate system;

[0082] A first theoretical calibration matrix M_C2T of the 3D camera coordinate system transformed to the mechanical arm tool coordinate system is calculated according to Euler angles;

[0083] A second theoretical calibration matrix M_T2B of the mechanical arm tool coordinate system transformed to the base coordinate system is calculated according to Euler angles;

[0084] The second theoretical calibration matrix is multiplied by the first theoretical calibration matrix to obtain a third theoretical calibration matrix M_C2B of the 3D camera coordinate system transformed to the base coordinate system.

[0085] Setting a three-dimensional feature platform;

[0086] Identifying the coordinates of the center pixel position of the three-dimensional feature point in the three-dimensional feature platform in the 3D camera coordinate system, performing depth matching on the 3D camera, and obtaining the theoretical three-dimensional feature coordinates of the three-dimensional feature point in the base coordinate system;

[0087] Forming a feature automatic recognition alignment mechanism, using the feature automatic recognition alignment mechanism, and obtaining the actual three-dimensional feature coordinates of the three-dimensional feature point in the base coordinate system;

[0088] According to the actual three-dimensional feature coordinates and the theoretical three-dimensional feature coordinates, at least one optimization compensation matrix M_B2R is generated, the target optimization compensation matrix is screened according to the compensation condition of the optimization compensation matrix to the theoretical three-dimensional feature coordinates, and the compensation result of the target optimization compensation matrix to the theoretical three-dimensional feature coordinates is used as the optimized coordinates.

[0089] In the present application, since the coordinates recognized by the 3D camera are different from the coordinate system used by the base position, and since the 3D camera is mobile, the coordinate system it uses is composed of the tangent plane of its mirror and the vertical direction of the tangent plane. With the movement of the 3D camera, the coordinates of the same actual position in the 3D camera coordinate system will change, so it cannot be used as the base coordinate system. The coordinate system used by the base position is used as the base coordinate system because the base position is located at the bottom of the six-axis arm and is used to control the movement of the six-axis arm, so it will not move. Therefore, the coordinates of the same actual position in the coordinate system used by the base position are constant, so it can be used as a reference. Therefore, a third theoretical calibration matrix of the 3D camera coordinate system and the base coordinate system is needed to transform the coordinates of the two. Since the recognition of the object position depends on the 3D camera, the coordinates observed initially by the 3D camera are the coordinates in the 3D camera coordinate system.

[0090] Referring to Figure 2 , the first theoretical calibration matrix of the 3D camera coordinate system transformed to the robot tool coordinate system includes the following steps:

[0091] Obtaining the first translation amount (A, B, C) of the coordinate origin of the 3D camera coordinate system moving to the coordinate origin of the robot tool coordinate system;

[0092] After the first translation (A, B, C) is translated, a first rotation amount of the 3D camera coordinate system rotation to coincide with the mechanical arm tool coordinate system is obtained, and the first rotation amount is composed of a first roll angle Roll, a first pitch angle Pitch and a first yaw angle Yaw, wherein the first roll angle is an angle of the 3D camera coordinate system rotation around the z axis, the first pitch angle is an angle of the 3D camera coordinate system rotation around the x axis, and the first yaw angle is an angle of the 3D camera coordinate system rotation around the y axis;

[0093] A first roll angle matrix, a first pitch angle matrix and a first yaw angle matrix are generated, and the first roll angle matrix, the first pitch angle matrix and the first yaw angle matrix are multiplied to obtain a first rotation matrix;

[0094] According to the first rotation matrix and the first translation, a first theoretical calibration matrix is formed.

[0095] There is a shaft arm between the base position and the 3D camera, so multiple transformations need to be performed, that is, first, a transformation matrix between the 3D camera and the shaft arm is determined, and second, a transformation matrix between the shaft arm and the base position is determined, and the two are combined to obtain a third theoretical calibration matrix;

[0096] When the 3D camera moves, it can be divided into translation and rotation, and first, a matrix for rotation is obtained according to Euler angles:

[0097]

[0098] R_C2T=R Z (Roll)·R Y (Pitch)·R X (Yaw),

[0099] Then, the first theoretical calibration matrix M_C2T is obtained by adding the translation vector:

[0100]

[0101] Wherein, R_C2T is a 3*3 matrix, which fills the 3*3 blank part in the upper left corner of M_C2T;

[0102] Referring to FIG. 1, the first roll angle Roll, the first pitch angle Pitch and the first yaw angle Yaw are labeled and shown in the figure. Figure 9

[0103] Referring to FIG. 2, the second theoretical calibration matrix of the mechanical arm tool coordinate system transformation to the base coordinate system is calculated and includes the following steps: Figure 3

[0104] A second translation (a, b, c) of the coordinate origin of the mechanical arm tool coordinate system moving to the coordinate origin of the base coordinate system is obtained; ​​

[0105] After the second translation (a, b, c) is translated, a second rotation amount of the robot tool coordinate system rotation to coincide with the base coordinate system is obtained, the second rotation amount is composed of a second roll angle, a second pitch angle and a second yaw angle, wherein the second roll angle is the angle of rotation of the robot tool coordinate system around the z axis, the second pitch angle is the angle of rotation of the robot tool coordinate system around the x axis, and the second yaw angle is the angle of rotation of the robot tool coordinate system around the y axis;

[0106] A second roll angle matrix, a second pitch angle matrix and a second yaw angle matrix are generated, and the second roll angle matrix, the second pitch angle matrix and the second yaw angle matrix are multiplied to obtain a second rotation matrix;

[0107] According to the second rotation matrix and the second translation, a second theoretical calibration matrix is formed.

[0108] When the shaft arm moves, it can be divided into translation and rotation. First, according to the Euler angle, the matrix for rotation is obtained:

[0109]

[0110] R_T2B=R Z (roll)·R Y (pitch)·R X (yaw),

[0111] Then add the translation vector to obtain the first theoretical calibration matrix M_T2B:

[0112]

[0113] Wherein, R_T2B is a 3*3 matrix, which fills the 3*3 vacancy part in the upper left corner of M_T2B.

[0114] The three-dimensional feature platform includes the following steps:

[0115] The three-dimensional feature platform is a pre-set fixed shape platform, the three-dimensional feature platform is provided with 13 three-dimensional feature points on the surface, the part of the three-dimensional feature platform except the three-dimensional feature points is a plane and the color is white, the height of the three-dimensional feature points is higher than the rest of the three-dimensional feature platform, and the three-dimensional feature points are black.

[0116] Referring to Figure 4 The center pixel position of the three-dimensional feature point in the three-dimensional feature platform in the 3D camera coordinate system includes the following steps:

[0117] The contour of the three-dimensional feature point is obtained by contour recognition on the image obtained by the 3D camera;

[0118] Modeling a planar coordinate system in the image acquired by the 3D camera to obtain a sampling coordinate system, evenly sampling at least one sampling point on the contour of the three-dimensional feature point, and acquiring a sampling coordinate of the sampling point in the sampling coordinate system;

[0119] Taking an average of the sampling coordinates of the at least one sampling point to obtain an image coordinate (u, v) of the center pixel position, and comprehensively calculating a coordinate (g, d) in an xy plane of the center pixel position in the 3D camera coordinate system according to a physical focal length of the 3D camera and a coordinate of a center of the image acquired by the 3D camera in the sampling coordinate system;

[0120] Emitting an infrared light pulse from the 3D camera to the three-dimensional feature point, acquiring an included angle K between the infrared light pulse and a z axis of the 3D camera coordinate system, the direction of the z axis of the 3D camera coordinate system being a direction perpendicular to a mirror surface of the 3D camera, acquiring a transmission-reception interval T of the infrared light pulse, and comprehensively calculating a depth value e;

[0121] Taking (g, d, e) as a coordinate of the center pixel position in the 3D camera coordinate system.

[0122] The image coordinate of the center of the image acquired by the 3D camera is (c x , c y ), the physical focal length is f, f x =f*m x , m x is a number of pixels contained in the image acquired by the 3D camera in the horizontal direction, and f y =f*m y , m y is a number of pixels contained in the image acquired by the 3D camera in the vertical direction;

[0123] Then Here, the pixel points in the image are numbered in the horizontal direction, and the pixel points in the image are numbered in the vertical direction, so that the image coordinate is sequentially composed of the number of the vertical line and the number of the horizontal line of the pixel point at the image coordinate;

[0124] e is equal to q is the speed of light, and the distance from the 3D camera to the three-dimensional feature point is But it needs to be projected onto the z axis of the 3D camera coordinate system, so that to obtain the depth value e.

[0125] Obtaining a theoretical three-dimensional feature coordinate of the three-dimensional feature point in the base coordinate system includes the following steps:

[0126] Multiplying the third theoretical calibration matrix and the coordinate of the center pixel position in the 3D camera coordinate system to obtain the theoretical three-dimensional feature coordinate.

[0127] Referring to Figure 5The feature automatic recognition alignment mechanism shown includes the following steps:

[0128] An infrared camera on the end of the mechanical arm is used to capture infrared indication and infrared ranging laser contrast images of the three-dimensional feature platform single block respectively;

[0129] Through image segmentation recognition, the first center of the infrared indication light spot and the second center of the laser ranging light spot are obtained respectively;

[0130] The area of the minimum actual range of the infrared image captured by the infrared camera is obtained as the feature area, and the number of pixels of the infrared image captured by the infrared camera is obtained as the feature value;

[0131] The feature area and the feature value are square rooted to obtain the control coefficient m;

[0132] The row and column pixel difference (ΔU, ΔV) of the first center and the second center is determined, ΔU is the number of pixels of the interval of the first center and the second center in the horizontal direction, and ΔV is the number of pixels of the interval of the first center and the second center in the vertical direction;

[0133] During the control, the height of the end of the mechanical arm is unchanged, ΔX in the X-axis direction of the base coordinate system is moved, and ΔY in the Y-axis direction of the base coordinate system is moved, ΔX = mΔU, and ΔY = mΔV;

[0134] After the control, if the modulus length of the row and column pixel difference of the first center and the second center is greater than the allowable error, the position of the first center and the second center is updated, and the above step is repeated until the modulus length of the row and column pixel difference of the first center and the second center does not exceed the allowable error.

[0135] Because there is a certain correlation between the image and the actual scene, and the distance between adjacent pixel points and the actual scene has a certain correlation, when the first center and the second center have a row and column pixel difference, the first center and the second center can be moved repeatedly to finally make the row and column pixel difference of the first center and the second center small enough. The movement corresponds to the actual distance, so the first center and the second center need to be closer after each change, so that through continuous iteration, the two can be aligned. The actual range corresponding to the infrared image has large and small, when the actual range is large, the actual distance corresponding to the adjacent pixel points in the infrared image is large, and vice versa. During the control, the control is performed according to the proportion determined according to the minimum actual range. The control coefficient m is easy to know as the control proportion corresponding to the minimum actual range of the infrared image. Therefore, the distance moved by the control coefficient m for the row and column pixel difference must be smaller than the actual distance of the first center and the second center. Therefore, the first center and the second center must be closer after the control, and then through multiple controls, the distance can be controlled within the required range;

[0136] The infrared camera is installed at the 3D camera, and the calibration tool is installed at the laser range finder. Since the calibration tool can obtain coordinates, it is not at the 3D camera because it will block the image acquisition. Therefore, there is a certain deviation between the calibration tool and the 3D camera, and it is necessary to determine the size of the deviation between the calibration tool and the 3D camera. The deviation is determined by aligning the first center and the second center. The coordinates of the first center and the second center can be obtained by TCP measurement of the calibration tool, and the difference between the coordinates of the first center and the second center is actually the deviation between the coordinates of the calibration tool and the 3D camera. This is obtained according to the installation position. Therefore, once the three-dimensional coordinate position of the three-dimensional feature point is determined by the calibration tool, the actual three-dimensional feature coordinate can be determined.

[0137] Referring to Figure 6 , obtaining the actual three-dimensional feature coordinate of the three-dimensional feature point in the base coordinate system includes the following steps:

[0138] During calibration, a fixed posture value is selected, and the first center (X1, Y1, Z1) of the working end under the fixed posture is obtained by TCP measurement of the calibration tool;

[0139] The second center (X2, Y2, Z2) of the laser range finder after installation of the calibration tool is obtained;

[0140] After feature recognition and alignment using the feature automatic recognition alignment mechanism, the three-dimensional coordinate position (X i , Y i , Z i ) of the current mechanical arm is obtained;

[0141] The actual three-dimensional feature coordinate is calculated as (X i , Y i , Z i )+(X1, Y1, Z1)-(X2, Y2, Z2).

[0142] Referring to Figure 7 , generating at least one optimized compensation matrix according to the actual three-dimensional feature coordinate and the theoretical three-dimensional feature coordinate includes the following steps:

[0143] Form at least one random feature point combination, which is composed of any 4 of the 13 three-dimensional feature points;

[0144] The actual three-dimensional feature coordinate and the theoretical three-dimensional feature coordinate of the three-dimensional feature point in the random feature point combination are used to solve the equations to obtain the optimized compensation matrix;

[0145] The optimized compensation matrix has the following form:

[0146]

[0147] wherein, r 11 , r 12 , r 13 , r 21 , r 22 , r 23 , r 31 , r 32 , r 33 , t1, t2 and t3 are all unknowns to be solved.

[0148] Here, there are 12 unknowns to be solved, and the random feature point combination can form 12 equations, so it is enough to solve the unknowns to be solved, and then the optimized compensation matrix can be determined. Then, according to the difference and square error, the optimized compensation matrix can be screened to obtain the target optimized compensation matrix.

[0149] Referring to FIG. 8, the screening of the target optimized compensation matrix according to the compensation of the theoretical three-dimensional feature coordinates by the optimized compensation matrix includes the following steps: Figure 8 The optimized compensation matrix is multiplied by the theoretical three-dimensional feature matrix to obtain a compensated three-dimensional feature matrix, and the compensated three-dimensional feature coordinates are obtained from the compensated three-dimensional feature matrix.

[0150] According to the compensated three-dimensional feature coordinates of the optimized compensation matrix and the actual three-dimensional feature coordinates, the difference and square error Er of the optimized compensation matrix is generated, and the optimized compensation matrix with the smallest difference and square error Er is taken as the target optimized compensation matrix.

[0151] The theoretical three-dimensional feature matrix is as follows:

[0152]

[0153] wherein, i is an index, (B_X i , B_Y i , B_Z i ) is the theoretical three-dimensional feature coordinates of the i-th three-dimensional feature point.

[0154] The compensated three-dimensional feature matrix is as follows:

[0155]

[0156] wherein, (L_X i , L_Y i , L_Z i ) is the compensated three-dimensional feature coordinates of the i-th three-dimensional feature point.

[0157] The difference and square error is as follows:

[0158]

[0159]

[0160] ​Wherein, (R_X i ,R_Y i ,R_Z i ) is the actual three-dimensional feature coordinates of the i th three-dimensional feature point.

[0161] Further, the scheme also proposes a storage medium, which stores computer readable program, and the computer readable program is called to execute the above-mentioned six-axis physiotherapy robot three-dimensional calibration optimization method.

[0162] It can be understood that the storage medium can be a magnetic medium, for example, a floppy disk, a hard disk, a magnetic tape, an optical medium such as a DVD, or a semiconductor medium such as a solid state disk (SSD).

[0163] To sum up, the advantages of the present application are as follows: by forming a third theoretical calibration matrix, forming a theoretical three-dimensional feature coordinate, forming a feature automatic recognition alignment mechanism, and obtaining a target optimization compensation matrix, the coordinates can be recognized by the calibration tool, and the coordinates measured by the calibration tool can be converted into actual three-dimensional feature coordinates by the translation of the calibration tool position relative to the actual measurement position, and the coordinates collected by the 3D camera can be converted into coordinates in the base coordinate system through the third theoretical calibration matrix, so that the actual three-dimensional feature coordinates and the theoretical three-dimensional feature coordinates can be processed in a unified coordinate system, and then the target optimization compensation matrix is formed according to the difference, and the subsequent measurement results are compensated, and the measurement error is effectively controlled.

[0164] The basic principles, main features and advantages of the present application are shown and described above. It should be understood by those skilled in the art that the present application is not limited by the above-mentioned embodiments, and the above-mentioned embodiments and descriptions in the specification are only the principles of the present application. Without departing from the spirit and scope of the present application, various changes and improvements can be made to the present application, and these changes and improvements all fall within the scope of the claimed present application. The scope of protection claimed by the present application is defined by the appended claims and their equivalents.

Claims

1. A six-axis physiotherapy robot three-dimensional calibration optimization method, characterized in that, The method comprises the following steps: A 3D camera is arranged at the end of a six-axis arm of a physiotherapy robot, an axis arm connected with the 3D camera among the six-axis arms of the physiotherapy robot is taken as a tool mechanical arm, and an initial end of the physiotherapy robot controlling the six-axis arms is taken as a base position; A coordinate system adopted by the 3D camera is taken as a 3D camera coordinate system, a coordinate system adopted by the tool mechanical arm is taken as a mechanical arm tool coordinate system, and a coordinate system adopted by the base position is taken as a base coordinate system; A first theoretical calibration matrix M_C2T of the 3D camera coordinate system transformed to the mechanical arm tool coordinate system is calculated according to Euler angles; A second theoretical calibration matrix M_T2B of the mechanical arm tool coordinate system transformed to the base coordinate system is calculated according to Euler angles; The second theoretical calibration matrix is multiplied by the first theoretical calibration matrix to obtain a third theoretical calibration matrix M_C2B of the 3D camera coordinate system transformed to the base coordinate system; A three-dimensional feature platform is arranged; A center pixel position of a three-dimensional feature point in the three-dimensional feature platform in the 3D camera coordinate system is identified, and a depth matching of the 3D camera is performed to obtain a theoretical three-dimensional feature coordinate of the three-dimensional feature point in the base coordinate system; A feature automatic recognition alignment mechanism is formed, and the feature automatic recognition alignment mechanism is used to obtain an actual three-dimensional feature coordinate of the three-dimensional feature point in the base coordinate system; At least one optimization compensation matrix M_B2R is generated according to the actual three-dimensional feature coordinate and the theoretical three-dimensional feature coordinate, a target optimization compensation matrix is screened according to a compensation condition of the optimization compensation matrix on the theoretical three-dimensional feature coordinate, and a compensation result of the target optimization compensation matrix on the theoretical three-dimensional feature coordinate is taken as an optimized coordinate. 2.The six-axis physiotherapy robot three-dimensional calibration optimization method of claim 1, wherein, The first theoretical calibration matrix of the 3D camera coordinate system transformed to the mechanical arm tool coordinate system comprises the following steps: A first translation amount (A, B, C) of a coordinate origin of the 3D camera coordinate system moved to a coordinate origin of the mechanical arm tool coordinate system is obtained; After the first translation amount (A, B, C) is translated, a first rotation amount of the 3D camera coordinate system rotated to coincide with the mechanical arm tool coordinate system is obtained, and the first rotation amount is composed of a first roll angle Roll, a first pitch angle Pitch and a first yaw angle Yaw, wherein the first roll angle is an angle of the 3D camera coordinate system rotated around a z-axis, the first pitch angle is an angle of the 3D camera coordinate system rotated around an x-axis, and the first yaw angle is an angle of the 3D camera coordinate system rotated around a y-axis; A first roll angle matrix, a first pitch angle matrix and a first yaw angle matrix are generated, and the first roll angle matrix, the first pitch angle matrix and the first yaw angle matrix are multiplied to obtain a first rotation matrix; The first theoretical calibration matrix is formed according to the first rotation matrix and the first translation amount.

3. The three-dimensional calibration optimization method of a six-axis physiotherapy robot according to claim 2, characterized in that, The second theoretical calibration matrix of the mechanical arm tool coordinate system transformed to the base coordinate system comprises the following steps: A second translation amount (a, b, c) of a coordinate origin of the mechanical arm tool coordinate system moved to a coordinate origin of the base coordinate system is obtained; After the second translation (a, b, c) is translated, a second rotation amount of the robot tool coordinate system rotation to coincide with the base coordinate system is obtained, the second rotation amount is composed of a second roll angle, a second pitch angle and a second yaw angle, wherein the second roll angle is an angle of rotation of the robot tool coordinate system around the z-axis, the second pitch angle is an angle of rotation of the robot tool coordinate system around the x-axis, and the second yaw angle is an angle of rotation of the robot tool coordinate system around the y-axis; A second roll angle matrix, a second pitch angle matrix and a second yaw angle matrix are generated, and the second roll angle matrix, the second pitch angle matrix and the second yaw angle matrix are multiplied to obtain a second rotation matrix; According to the second rotation matrix and the second translation, a second theoretical calibration matrix is formed.

4. The three-dimensional calibration optimization method of a six-axis physiotherapy robot according to claim 3, characterized in that, The setting of the three-dimensional feature platform comprises the following steps: The three-dimensional feature platform is a fixed shape platform, 13 three-dimensional feature points are arranged on the surface of the three-dimensional feature platform, the part of the three-dimensional feature platform other than the three-dimensional feature points is a plane and is white in color, the height of the three-dimensional feature points is higher than the rest of the three-dimensional feature platform, and the three-dimensional feature points are black.

5. The three-dimensional calibration optimization method of a six-axis physiotherapy robot according to claim 4, characterized in that, The method for identifying the coordinates of the center pixel position of the three-dimensional feature points in the three-dimensional feature platform in the 3D camera coordinate system comprises the following steps: Contour recognition is performed on the image obtained by the 3D camera to obtain the contour of the three-dimensional feature points; A plane coordinate system is modeled in the image obtained by the 3D camera to obtain a sampling coordinate system, at least one sampling point is evenly taken on the contour of the three-dimensional feature points, and the sampling coordinates of the sampling points in the sampling coordinate system are obtained; The sampling coordinates of the at least one sampling point are averaged to obtain the image coordinates (u, v) of the center pixel position, and the coordinates (g, d) of the center pixel position in the xy plane of the 3D camera coordinate system are comprehensively calculated according to the physical focal length of the 3D camera and the coordinates of the center of the image obtained by the 3D camera in the sampling coordinate system; An infrared light pulse is emitted from the 3D camera to the three-dimensional feature points, the included angle K between the infrared light pulse and the z-axis of the 3D camera coordinate system is obtained, the direction of the z-axis of the 3D camera coordinate system is perpendicular to the direction of the 3D camera mirror, the interval T between the sending and receiving of the infrared light pulse is obtained, and the depth value e is comprehensively calculated; (g, d, e) is taken as the coordinates of the center pixel position in the 3D camera coordinate system.

6. The three-dimensional calibration optimization method of a six-axis physiotherapy robot according to claim 5, characterized in that, The method for obtaining the theoretical three-dimensional feature coordinates of the three-dimensional feature points in the base coordinate system comprises the following steps: The third theoretical calibration matrix is multiplied by the coordinates of the center pixel position in the 3D camera coordinate system to obtain the theoretical three-dimensional feature coordinates.

7. The three-dimensional calibration optimization method of a six-axis physiotherapy robot according to claim 6, characterized in that, The forming of the feature automatic recognition alignment mechanism comprises the following steps: An infrared camera on the end of the robot is used to capture infrared indication and infrared ranging laser contrast images of the three-dimensional feature platform respectively; A first center of the infrared indication light spot and a second center of the laser ranging light spot are obtained through image segmentation recognition respectively; The area of the minimum actual range of the infrared image captured by the infrared camera is taken as the feature area, and the number of pixels of the infrared image captured by the infrared camera is taken as the feature value; The feature area and the feature value are square rooted to obtain a control coefficient m; Determine the row and column pixel difference (ΔU, ΔV) of the first center and the second center, ΔU is the number of pixels of the interval of the first center and the second center in the horizontal direction, and ΔV is the number of pixels of the interval of the first center and the second center in the vertical direction; During the regulation, the height of the end of the mechanical arm is unchanged, ΔX is moved in the X-axis direction of the base coordinate system, and ΔY is moved in the Y-axis direction of the base coordinate system, ΔX=mΔU, and ΔY=mΔV; After the regulation, if the modulus of the row and column pixel difference of the first center and the second center is greater than the allowable error, then after updating the positions of the first center and the second center, repeat the previous step until the modulus of the row and column pixel difference of the first center and the second center does not exceed the allowable error. 8.The six-axis physiotherapy robot three-dimensional calibration optimization method of claim 7, wherein, The actual three-dimensional feature coordinates of the obtained three-dimensional feature points in the base coordinate system include the following steps: During calibration, a fixed posture value is selected, and the first center (X1, Y1, Z1) of the working end in the fixed posture is measured by the calibration tool TCP; The second center (X2, Y2, Z2) of the laser range finder after installing the calibration tool is obtained; After feature recognition alignment is performed using the feature automatic recognition alignment mechanism, the three-dimensional coordinate position (X i , Y i , Z i ) of the current mechanical arm is acquired; The actual three-dimensional feature coordinates are calculated as (X i , Y i , Z i )+(X1, Y1, Z1)-(X2, Y2, Z2).

9. The three-dimensional calibration optimization method of a six-axis physiotherapy robot according to claim 8, characterized in that, The generation of at least one optimization compensation matrix according to the actual three-dimensional feature coordinates and the theoretical three-dimensional feature coordinates includes the following steps: Form at least one random feature point combination, which is composed of any 4 of the 13 three-dimensional feature points; The actual three-dimensional feature coordinates and the theoretical three-dimensional feature coordinates of the three-dimensional feature points in the random feature point combination are used to solve the optimization compensation matrix by simultaneous equations; The optimization compensation matrix is as follows: wherein r 11 , r 12 , r 13 , r 21 , r 22 , r 23 , r 31 , r 32 , r 33 , t1, t2 and t3 are unknowns to be solved.

10. The three-dimensional calibration optimization method of a six-axis physiotherapy robot according to claim 9, characterized in that, The screening of the target optimization compensation matrix according to the compensation of the optimization compensation matrix to the theoretical three-dimensional feature coordinates includes the following steps: The optimization compensation matrix is multiplied by the theoretical three-dimensional feature matrix to obtain a compensation three-dimensional feature matrix, and the compensation three-dimensional feature coordinates are obtained from the compensation three-dimensional feature matrix; According to the compensation three-dimensional feature coordinates of the optimization compensation matrix and the actual three-dimensional feature coordinates, the difference sum of squares error Er of the optimization compensation matrix is generated, and the optimization compensation matrix with the minimum difference sum of squares error Er is taken as the target optimization compensation matrix; The theoretical three-dimensional feature matrix is as follows: where i is an index, (B_X i ,B_Y i ,B_Z i ) is the theoretical three-dimensional feature coordinates of the i-th three-dimensional feature point; The compensation three-dimensional feature matrix is as follows: wherein (L_X i ,L_Y i ,L_Z i ) is the compensated three-dimensional feature coordinate of the i-th three-dimensional feature point; The difference sum of squares error is as follows: wherein (R_X i ,R_Y i ,R_Z i ) are the actual three-dimensional feature coordinates of the i-th three-dimensional feature point.

Images