Three-Dimensional Calibration Method and Calibration Platform for Six-Axis Physiotherapy Robot

Abstract

The present application relates to a three-dimensional calibration method and a calibration platform for a six-axis physiotherapy robot. The calibration method includes: using a 3D camera to collect depth images of three-dimensional feature blocks, and obtaining the theoretical coordinates of the three-dimensional feature blocks based on the depth images; controlling the infrared indicator light to turn on and the laser ranging component to emit laser light; using the images collected by the infrared camera to adjust the robotic arm so that the light spots of the infrared indicator light and the laser light emitted by the laser ranging component overlap; calculating the actual coordinates of the three-dimensional feature blocks when the light spots of the infrared indicator light and the laser light emitted by the laser ranging component overlap; based on multiple groups of theoretical coordinates and actual coordinates, fitting to obtain a deviation compensation matrix; wherein, the deviation compensation matrix is used to compensate a first transformation matrix, and the first transformation matrix is a transformation matrix from the 3D camera coordinate system to the base coordinate system. The present application achieves high-precision calibration of the six-axis physiotherapy robot.

Description

Technical Field

[0001] The present invention relates to the field of manufacturing physiotherapy robots, and particularly to a three-dimensional calibration method and a calibration platform for a six-axis physiotherapy robot. Background Art

[0002] A physiotherapy robot refers to a robot that performs physiotherapy on the human back or face. Compared with other robots, the physiotherapy robot has very high requirements for the accuracy of visual calibration.

[0003] Currently, there are mainly two three-dimensional system layout schemes for six-axis physiotherapy robots: hand-on-eye and hand-off-eye. The three-dimensional calibration schemes for the two layouts are very different, and both use fixed and moving 3D cameras or calibration plates to collect images at different spatial positions. The calibration accuracy is generally between 2 cm and 3 cm.

[0004] Currently, the calibration accuracy of physiotherapy robots is insufficient. Summary of the Invention

[0005] The present invention aims at the problem of insufficient calibration accuracy of physiotherapy robots.

[0006] In a first aspect, the present application provides a three-dimensional calibration method for a six-axis physiotherapy robot, which is applied to a calibration platform. The calibration platform includes: a plurality of infrared indicator lights, a control device, an infrared camera disposed on the robotic arm of the six-axis physiotherapy robot, and a plurality of three-dimensional feature blocks. Among them, the plurality of infrared indicator lights are respectively disposed on the plurality of three-dimensional feature blocks, and a laser ranging component and a 3D camera are disposed on the robotic arm of the six-axis physiotherapy robot. The method includes:

[0007] Using the 3D camera to collect depth images of the three-dimensional feature blocks, and obtaining the theoretical coordinates of the three-dimensional feature blocks based on the depth images;

[0008] Controlling the infrared indicator lights to light up and the laser ranging component to emit laser;

[0009] Using the images collected by the infrared camera, adjusting the robotic arm so that the light spots of the infrared indicator lights and the laser emitted by the laser ranging component overlap;

[0010] Calculating the actual coordinates of the three-dimensional feature blocks when the light spots of the infrared indicator lights and the laser emitted by the laser ranging component overlap;

[0011] Based on multiple groups of the theoretical coordinates and the actual coordinates, fitting to obtain a deviation compensation matrix; wherein, the deviation compensation matrix is used to compensate a first transformation matrix, and the first transformation matrix is a transformation matrix from the 3D camera coordinate system to the base coordinate system.

[0012] In some embodiments, the using the infrared images collected by the infrared camera and adjusting the robotic arm includes:

[0013] When the end height of the robotic arm is controlled to be fixed, the robotic arm is iteratively adjusted based on multiple frames of infrared images so that the distance between the infrared indicator light and the spot of the laser emitted by the laser ranging component is less than the distance threshold.

[0014] In some embodiments, calculating the actual coordinates of the three-dimensional feature block when the spots of the laser emitted by the infrared indicator light and the laser ranging component overlap includes:

[0015] Obtain the coordinates of the robotic arm of the six-axis physiotherapy robot before installing the laser ranging component and the infrared camera, and the coordinates of the robotic arm of the six-axis physiotherapy robot after installing the laser ranging component and the infrared camera, and confirm the translation vector of the robotic arm of the six-axis physiotherapy robot;

[0016] Add the coordinate values when the spots of the laser emitted by the infrared indicator light and the laser ranging component overlap to the translation vector to obtain the actual coordinates.

[0017] In some embodiments, using the images collected by the infrared camera to adjust the robotic arm so that the spots of the laser emitted by the infrared indicator light and the laser ranging component overlap includes:

[0018] Light up each infrared indicator light in sequence. When the current infrared indicator light is lit, control the infrared camera to collect an image of a single three-dimensional feature block, and adjust the robotic arm so that the spots of the laser emitted by the infrared indicator light and the laser ranging component overlap;

[0019] After calculating the actual coordinates of the three-dimensional feature block when the spots of the laser emitted by the current infrared indicator light and the laser ranging component overlap, turn off the current infrared indicator light to obtain the actual coordinates of the three-dimensional feature block corresponding to each infrared indicator light.

[0020] In some embodiments, the step of lighting up each infrared indicator light in sequence. When the current infrared indicator light is lit, control the infrared camera to collect an image of a single three-dimensional feature block, and adjust the robotic arm so that the spots of the laser emitted by the infrared indicator light and the laser ranging component overlap includes:

[0021] Use the PID algorithm to adjust the robotic arm. When the distance between the spots of the laser emitted by the infrared indicator light and the laser ranging component is less than the preset threshold, use the PD algorithm to control the robotic arm to move along a preset trajectory so that the spots of the laser emitted by the infrared indicator light and the laser ranging component overlap multiple times;

[0022] The calculation of the actual coordinates of the three-dimensional feature block when the spots of the laser emitted by the infrared indicator light and the laser ranging component overlap includes:

[0023] Calculate the actual coordinates of the three-dimensional feature block during multiple overlaps to obtain multiple sets of the actual coordinates.

[0024] In some embodiments, fitting a deviation compensation matrix based on multiple groups of the theoretical coordinates and the actual coordinates includes:

[0025] The first formula: ;

[0026] Wherein, is the actual coordinate, is the deviation compensation matrix, is the theoretical coordinate;

[0027] The second formula: ;

[0028] Wherein, is the sum of squared errors between the coordinate after error compensation and the actual coordinate, is the coordinate after error compensation, is the actual coordinate, and N is the number of three-dimensional feature blocks or infrared indicator lights;

[0029] Fitting the deviation compensation matrix based on the sum of squared errors and the least squares method.

[0030] In some embodiments, the value of N is greater than or equal to 10 and less than or equal to 15.

[0031] In some embodiments, using a 3D camera to collect a depth image of a three-dimensional feature block and obtaining the theoretical coordinates of the three-dimensional feature block based on the depth image includes:

[0032] Calculating a second transformation matrix between the 3D camera coordinate system and the robotic arm coordinate system based on the relative positions of the 3D camera and the robotic arm tool;

[0033] Using the initial coordinates corresponding to the depth image, multiplying by the second transformation matrix, and the third transformation matrix between the robotic arm coordinate system and the base coordinate system to obtain the theoretical coordinates.

[0034] In some embodiments, the infrared indicator light is located at the image center of the three-dimensional feature block.

[0035] In a second aspect, the present application provides a calibration platform, which includes:

[0036] An infrared camera for being installed on the robotic arm of a six-axis physical therapy robot;

[0037] A plurality of infrared indicator lights and a plurality of three-dimensional feature blocks, wherein the plurality of infrared indicator lights are respectively arranged on the plurality of three-dimensional feature blocks; and

[0038] A control device, connected to the infrared indicator light, the robotic arm of the six-axis physiotherapy robot, the laser ranging component, and the infrared camera. When the control device executes the calibration program, the three-dimensional calibration method of the six-axis physiotherapy robot as described in any one of the above is implemented.

[0039] In the embodiment of the present application, by turning on the infrared indicator light and controlling the laser ranging component to work, then controlling the infrared camera to capture an infrared image, and adjusting the robotic arm based on the infrared image so that the light spots of the laser emitted by the infrared indicator light and the laser ranging component overlap; to calculate the actual coordinates of the three-dimensional feature block when the light spots of the laser emitted by the infrared indicator light and the laser ranging component overlap; thus, a deviation compensation matrix can be calculated according to the actual coordinates and the physiotherapy coordinates, which is used to compensate the first transformation matrix, achieving an improvement in the calibration accuracy of the six-axis physiotherapy robot. Description of the Drawings

[0040] Figure 1 It is a schematic structural diagram of an embodiment of the calibration platform of the present application;

[0041] Figure 2 It is a flowchart of an embodiment of the three-dimensional calibration method of the six-axis physiotherapy robot of the present application;

[0042] Figure 3 It is a flowchart of another embodiment of the three-dimensional calibration method of the six-axis physiotherapy robot of the present application;

[0043] Figure 4 It is a flowchart of yet another embodiment of the three-dimensional calibration method of the six-axis physiotherapy robot of the present application.

[0044] In the drawings, the list of components represented by each reference numeral is as follows:

[0045] 10. Laser ranging component; 20. 3D camera; 31. Infrared indicator light; 32. Three-dimensional feature block; 40. Robotic arm; 50. Control device; 60. Infrared camera. Detailed Embodiments

[0046] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative efforts fall within the protection scope of the present invention.

[0047] In the description of the present invention, the terms "first" and "second" are used for descriptive purposes only and cannot be construed as indicating or implying relative importance or implicitly specifying the quantity of the indicated technical features. Thus, features defined with "first" and "second" may explicitly or implicitly include one or more of the described features. In the description of the present invention, "a plurality of" means two or more unless otherwise specifically defined.

[0048] In the description of the present invention, the term "for example" is used to mean "serving as an example, illustration, or explanation". Any embodiment described as "for example" in the present invention is not necessarily construed as being more preferred or having more advantages than other embodiments. The following description is provided to enable any person skilled in the art to make and use the present invention. In the following description, details are set forth for purposes of explanation. It should be understood that those of ordinary skill in the art can recognize that the present invention can be implemented without the use of these specific details. In other instances, well-known structures and processes are not described in detail to avoid obscuring the description of the present invention with unnecessary details. Therefore, the present invention is not intended to be limited to the embodiments shown, but is to be accorded the widest scope consistent with the principles and features disclosed herein.

[0049] Embodiment 1: The calibration accuracy requirements of a six-axis physiotherapy robot are very high, generally in the millimeter level. The present application provides a three-dimensional calibration method for a six-axis physiotherapy robot, aiming to improve the calibration accuracy of the six-axis physiotherapy robot.

[0050] Referring to Figure 1 , applied to a calibration platform, the calibration platform may include: a plurality of infrared indicator lights 31, a control device 50, an infrared camera 60 disposed on the robotic arm 40 of the six-axis physiotherapy robot, and a plurality of three-dimensional feature blocks 32.

[0051] Exemplarily, the three-dimensional feature blocks 32 are distributed at a certain interval in the horizontal direction;

[0052] Exemplarily, along the height direction, the heights of the three-dimensional feature blocks 32 are uneven.

[0053] Exemplarily, each three-dimensional feature block 32 is provided with an infrared indicator light 31, which can be collected by the infrared camera 60 when lit.

[0054] Exemplarily, the background color of each three-dimensional feature block 32 is black to show the light spot of the laser ranging component 10.

[0055] Referring to Figure 1 , the six-axis physiotherapy robot includes a robotic arm 40, and a laser ranging component 10 and a 3D camera 20 disposed at the working end of the robotic arm 40;

[0056] Referring toFigure 2 , in one embodiment, the three-dimensional calibration method of the six-axis physical therapy robot of the present application includes:

[0057] S100. Use the 3D camera 20 to collect the depth image of the three-dimensional feature block 32, and obtain the theoretical coordinates of the three-dimensional feature block 32 based on the depth image;

[0058] It should be noted that the robotic arm 40 of the six-axis physical therapy robot is six-axis controlled, including three translational controls in the up and down, left and right, and front and back directions, and three rotational controls in the pitch axis, roll axis, and yaw axis. Refer to Figure 1 , in actual application, the end of the robotic arm 40 is used as the origin of the base coordinate system.

[0059] First, based on the relationship between the installation position of the 3D camera 20 and the rotational position of the robotic arm 40, establish the rotation matrix between the 3D camera 20 coordinate system and the robotic arm 40 coordinate system:

[0060] ;

[0061] ;

[0062] ;

[0063] ;

[0064] Among them, , , respectively represent the roll rotation matrix, pitch rotation matrix, and yaw rotation matrix of the rotation matrix, is the rotation matrix between the 3D camera 20 coordinate system and the robotic arm 40 coordinate system. The , , inside cos and sin respectively represent the roll angle, pitch angle, and yaw angle in the relationship between the installation position of the 3D camera 20 and the rotational position of the robotic arm 40.

[0065] Based on the relationship between the installation position of the 3D camera 20 and the translational position of the robotic arm 40, add the translation vector to the rotation matrix to obtain the transformation matrix between the 3D camera 20 coordinate system and the robotic arm 40 coordinate system:

[0066] ;

[0067] Among them, represents the transformation matrix between the 3D camera 20 coordinate system and the robotic arm 40 coordinate system, is the rotation matrix between the 3D camera 20 coordinate system and the robotic arm 40 coordinate system. Since It is a 3*3 matrix, so a dashed box is reserved in the formula. X, Y, and Z are the translation matrices between the coordinate systems of the 3D camera 20 and the robotic arm 40.

[0068] Since the coordinate system of the calibrated data is the coordinate system of the robotic arm 40, and the coordinate system used in the actual control process of the robotic arm 40 is the base coordinate system, it is also necessary to calculate the transformation matrix M_T2B between the coordinate system of the robotic arm 40 and the base coordinate system. Through the current point position ( ), by applying Euler angles, it can also be calculated that:

[0069] ;

[0070] Among them, is the transformation matrix between the coordinate system of the robotic arm 40 and the base coordinate system.

[0071] In summary, we can obtain the output data of the base coordinate system of the robotic arm 40 and the measurement data of the coordinate system of the 3D camera 20 The transformation relationship M_C2B between them is:

[0072] ;

[0073] That is, after obtaining , , use the 3D camera 20 to collect the depth image of the three-dimensional feature block 32, obtain the initial coordinates of the target point (such as the center point) of the three-dimensional feature block 32 based on the depth image, and then multiply the initial coordinates by , , and the theoretical coordinates can be obtained.

[0074] It should be noted that after obtaining the theoretical coordinates, we also need to calculate the actual coordinates to fit the deviation compensation matrix according to the difference between the theoretical coordinates and the actual coordinates. In this application, the actual coordinates can be realized through steps S200 to S400.

[0075] S200. Control the infrared indicator light 31 to light up and the laser ranging component 10 to emit laser;

[0076] The control device 50 of the calibration platform can control the infrared indicator light 31 to light up and send a signal to the physical therapy robot to control the laser ranging component 10 to work to form a light spot on the three-dimensional feature block 32.

[0077] S300. Use the image collected by the infrared camera 60 to adjust the robotic arm 40 so that the light spots emitted by the infrared indicator light 31 and the laser ranging component 10 overlap;

[0078] After the infrared camera 60 captures an infrared image, the pixel center positions of the infrared indicator light 31 and the laser ranging spot can be obtained through image segmentation recognition. The difference between the two pixel center positions can be used to control the movement of the robotic arm 40. In this embodiment, on the basis of maintaining a fixed height difference, the robotic arm 40 is continuously and iteratively controlled to move to capture infrared images until the difference between the two pixel center positions in the latest captured infrared image meets the requirements, indicating that the spot of the laser emitted by the infrared indicator light 31 and the laser ranging component 10 overlap.

[0079] S400. Calculate the actual coordinates of the three-dimensional feature block 32 when the spots of the laser emitted by the infrared indicator light 31 and the laser ranging component 10 overlap.

[0080] In this embodiment, the actual coordinates of the three-dimensional feature block 32 can be the coordinates of the infrared indicator light 31.

[0081] After obtaining the theoretical coordinate system, TCP (Tool Center Point) measurement is used. Since the calibration is performed with a fixed pose, only the translation vector of the robotic arm 40 needs to be confirmed to calculate the actual coordinates of the three-dimensional feature block 32.

[0082] Specifically, for the TCP measurement of the calibration tool, before installing the calibration tool, the cone alignment position of the working end of the robotic arm 40 in a fixed pose can be obtained ( ) and then the cone alignment position of the laser rangefinder after installing the calibration tool can be obtained ( ). After the spots of the laser emitted by the infrared indicator light 31 and the laser ranging component 10 overlap, the three-dimensional coordinate position of the current robotic arm 40 can be obtained ( ), and the actual three-dimensional feature information can be obtained after calibration ( ).

[0083] ;

[0084] ;

[0085] Among them, is the translation vector of the robotic arm 40.

[0086] S500. Based on multiple groups of the theoretical coordinates and the actual coordinates, a deviation compensation matrix is obtained by fitting; among them, the deviation compensation matrix is used to compensate the first transformation matrix, and the first transformation matrix is the transformation matrix from the coordinate system of the 3D camera 20 to the base coordinate system.

[0087] In this embodiment, it is assumed that the number of three-dimensional feature blocks 32 is 13 for illustration. In other embodiments, the number of three-dimensional feature blocks 32 can be other numbers, which are not limited herein. Thus, in this embodiment, 13 groups of theoretical three-dimensional feature information of the three-dimensional feature platform are obtained and the actual three-dimensional feature information values , and linear least squares iterative fitting is performed. The optimized compensation matrix M_B2R contains 12 unknowns and requires four sets of data for fitting. The values of the 13 feature points after error compensation and the actual three-dimensional space values are used to calculate the sum of squared differences . By randomly selecting a sufficient amount of sample data, linear least squares iterative fitting is performed until the optimized compensation matrix M_B2R meets the accuracy requirements.

[0088] ;

[0089] ;

[0090] In the embodiment of the present application, the infrared indicator light 31 is lit and the laser ranging component 10 is controlled to work, then the infrared camera 60 is controlled to capture an infrared image, and the robotic arm 40 is controlled to adjust based on the infrared image so that the light spots of the laser emitted by the infrared indicator light 31 and the laser ranging component 10 overlap; to calculate the actual coordinates of the three-dimensional feature block 32 when the light spots of the laser emitted by the infrared indicator light 31 and the laser ranging component 10 overlap; thereby, a deviation compensation matrix can be calculated based on the actual coordinates and the physical therapy coordinates to compensate the first transformation matrix, achieving an improvement in the calibration accuracy of the six-axis physical therapy robot.

[0091] In the embodiment of the present application, the infrared indicator light 31 is lit and the laser ranging component 10 is controlled to work, then the infrared camera 60 is controlled to capture an infrared image, and the robotic arm 40 is controlled to adjust based on the infrared image so that the light spots of the laser emitted by the infrared indicator light 31 and the laser ranging component 10 overlap; to calculate the actual coordinates of the three-dimensional feature block 32 when the light spots of the laser emitted by the infrared indicator light 31 and the laser ranging component 10 overlap; thereby, a deviation compensation matrix can be calculated based on the actual coordinates and the physical therapy coordinates to compensate the first transformation matrix, achieving an improvement in the calibration accuracy of the six-axis physical therapy robot.

[0092] First formula: ;

[0093] Wherein, is the actual coordinate, is the deviation compensation matrix, is the theoretical coordinate;

[0094] Second formula: ;

[0095] Among them, is the sum of squared errors between the coordinates after error compensation and the actual coordinates, are the coordinates after error compensation, are the actual coordinates, and N is the number of three-dimensional feature blocks 32 or infrared indicator lights 31;

[0096] Based on the sum of squared errors and the least squares method, the deviation compensation matrix is obtained by fitting.

[0097] In some embodiments, the value of N is greater than or equal to 10 and less than or equal to 15.

[0098] In some embodiments, using the 3D camera 20 to collect the depth image of the three-dimensional feature block 32 and obtaining the theoretical coordinates of the three-dimensional feature block 32 based on the depth image includes:

[0099] Based on the relative positions of the 3D camera 20 and the robotic arm 40, calculating the second transformation matrix between the coordinate system of the 3D camera 20 and the coordinate system of the robotic arm 40;

[0100] Using the initial coordinates corresponding to the depth image, multiplying by the second transformation matrix, and the third transformation matrix between the coordinate system of the robotic arm 40 and the base coordinate system, to obtain the theoretical coordinates.

[0101] In some embodiments, the infrared indicator light 31 is located at the image center of the three-dimensional feature block 32.

[0102] The infrared indicator light 31 being located at the image center of the three-dimensional feature block 32 allows the infrared camera 60 to better collect the infrared indicator light 31 and the light spot.

[0103] Embodiment 2: The present application provides a calibration platform, and this calibration platform includes:

[0104] An infrared camera 60, for being installed on the robotic arm 40 of the six-axis physical therapy robot;

[0105] A plurality of infrared indicator lights 31 and a plurality of three-dimensional feature blocks 32, among which, the plurality of infrared indicator lights 31 are respectively arranged on the plurality of three-dimensional feature blocks 32; and

[0106] A control device 50, connected to the infrared indicator light 31, the robotic arm 40 of the six-axis physical therapy robot, the laser ranging component 10, and the infrared camera 60. When the control device 50 executes the calibration program, it implements the three-dimensional calibration method of the six-axis physical therapy robot as described below:

[0107] S100. Using the 3D camera 20 to collect the depth image of the three-dimensional feature block 32 and obtaining the theoretical coordinates of the three-dimensional feature block 32 based on the depth image;

[0108] S200, control the infrared indicator light 31 to light up and the laser ranging component 10 to emit laser light;

[0109] S300, use the image collected by the infrared camera 60 to adjust the robotic arm 40 so that the light spots emitted by the infrared indicator light 31 and the laser ranging component 10 overlap;

[0110] S400, calculate the actual coordinates of the three-dimensional feature block 32 when the light spots emitted by the infrared indicator light 31 and the laser ranging component 10 overlap;

[0111] S500, based on multiple groups of the theoretical coordinates and the actual coordinates, fit to obtain a deviation compensation matrix; wherein, the deviation compensation matrix is used to compensate the first transformation matrix, and the first transformation matrix is the transformation matrix from the coordinate system of the 3D camera 20 to the base coordinate system.

[0112] In the embodiment of the present application, by lighting up the infrared indicator light 31 and controlling the operation of the laser ranging component 10, then controlling the infrared camera 60 to capture an infrared image, and controlling and adjusting the robotic arm 40 based on the infrared image so that the light spots emitted by the infrared indicator light 31 and the laser ranging component 10 overlap; to calculate the actual coordinates of the three-dimensional feature block 32 when the light spots emitted by the infrared indicator light 31 and the laser ranging component 10 overlap; thereby, a deviation compensation matrix can be calculated according to the actual coordinates and the physical therapy coordinates, which is used to compensate the first transformation matrix, realizing the improvement of the calibration accuracy of the six-axis physical therapy robot.

[0113] In some embodiments, for the using the infrared image collected by the infrared camera 60 to adjust the robotic arm 40, it includes: when controlling the end height of the robotic arm 40 to be fixed, iteratively adjusting the robotic arm 40 based on multiple frames of infrared images so that the distance between the light spots emitted by the infrared indicator light 31 and the laser ranging component 10 is less than a distance threshold.

[0114] In some embodiments, for the calculating the actual coordinates of the three-dimensional feature block 32 when the light spots emitted by the infrared indicator light 31 and the laser ranging component 10 overlap, it includes: obtaining the coordinates of the robotic arm 40 of the six-axis physical therapy robot before installing the laser ranging component 10 and the infrared camera 60 and the coordinates of the robotic arm 40 of the six-axis physical therapy robot after installing the laser ranging component 10 and the infrared camera 60, and confirming the translation vector of the robotic arm 40 of the six-axis physical therapy robot; adding the coordinate values when the light spots emitted by the infrared indicator light 31 and the laser ranging component 10 overlap to the translation vector to obtain the actual coordinates.

[0115] In some embodiments, for the image collected by the infrared camera 60 in S300, adjusting the robotic arm 40 to make the light spot of the infrared indicator light 31 and the laser emitted by the laser ranging component 10 overlap includes: S301. Sequentially turning on each infrared indicator light 31. When the current infrared indicator light 31 is turned on, controlling the infrared camera 60 to collect an image of a single three-dimensional feature block 32, and adjusting the robotic arm 40 to make the light spot of the infrared indicator light 31 and the laser emitted by the laser ranging component 10 overlap; S302. After calculating the actual coordinates of the three-dimensional feature block 32 when the light spots of the current infrared indicator light 31 and the laser emitted by the laser ranging component 10 overlap, turning off the current infrared indicator light 31 to obtain the actual coordinates of the three-dimensional feature block 32 corresponding to each infrared indicator light 31.

[0116] In some embodiments, in S301, sequentially turning on each infrared indicator light 31. When the current infrared indicator light 31 is turned on, controlling the infrared camera 60 to collect an image of a single three-dimensional feature block 32, and adjusting the robotic arm 40 to make the light spot of the infrared indicator light 31 and the laser emitted by the laser ranging component 10 overlap includes: S3011. Using the PID algorithm to adjust the robotic arm 40. When the distance between the light spots of the infrared indicator light 31 and the laser emitted by the laser ranging component 10 is less than a preset threshold, using the PD algorithm to control the robotic arm 40 to move along a preset trajectory so that the light spots of the infrared indicator light 31 and the laser emitted by the laser ranging component 10 overlap multiple times; S3012. Calculating the actual coordinates of the three-dimensional feature block 32 when the light spots of the infrared indicator light 31 and the laser emitted by the laser ranging component 10 overlap includes: S3013. Calculating the actual coordinates of the three-dimensional feature block 32 during multiple overlaps to obtain multiple sets of the actual coordinates.

[0117] In some embodiments, the value of N is greater than or equal to 10 and less than or equal to 15.

[0118] In some embodiments, for using the 3D camera 20 to collect the depth image of the three-dimensional feature block 32 and obtaining the theoretical coordinates of the three-dimensional feature block 32 based on the depth image includes: calculating the second transformation matrix between the 3D camera 20 coordinate system and the robotic arm 40 coordinate system based on the relative position between the 3D camera 20 and the tool of the robotic arm 40; multiplying the initial coordinates corresponding to the depth image by the second transformation matrix and the third transformation matrix between the robotic arm 40 coordinate system and the base coordinate system to obtain the theoretical coordinates.

[0119] In some embodiments, the infrared indicator light 31 is located at the center of the image of the three-dimensional feature block 32.

[0120] It should be noted that in the above embodiments, the descriptions of the various embodiments have their own focuses. For the parts not described in detail in a certain embodiment, reference may be made to the relevant descriptions of other embodiments.

[0121] Those skilled in the art will understand that the embodiments of the present invention can be provided as a method, a system, or a computer program product. Therefore, the present invention can take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present invention can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that contain computer-usable program code.

[0122] The present invention is described with reference to the flowcharts and / or block diagrams of methods, apparatuses (systems), and computer program products according to the embodiments of the present invention. It should be understood that each flow and / or block in the flowchart and / or block diagram, and the combination of flows and / or blocks in the flowchart and / or block diagram, can be realized by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded computer, or other programmable data processing devices to generate a machine, such that the instructions executed by the processor of the computer or other programmable data processing devices generate means for realizing the functions specified in Figure 1 one flow or multiple flows and / or blocks Figure 1 one block or multiple blocks.

[0123] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, such that the instructions stored in the computer-readable memory generate a manufactured article including an instruction device that realizes the functions specified in Figure 1 one flow or multiple flows and / or blocks Figure 1 one block or multiple blocks.

[0124] These computer program instructions can also be loaded onto a computer or other programmable data processing device, such that a series of operation steps are executed on the computer or other programmable device to generate a computer-implemented process, so that the instructions executed on the computer or other programmable device provide steps for realizing the functions specified in Figure 1 one flow or multiple flows and / or blocks Figure 1 one block or multiple blocks.

[0125] Although the preferred embodiments of the present invention have been described, those skilled in the art can make additional changes and modifications once they know the basic creative concept. Therefore, the appended claims are intended to be construed as including the preferred embodiments and all changes and modifications falling within the scope of the present invention.

[0126] Obviously, those skilled in the art can make various modifications and variations to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalent technologies, the present invention is also intended to include these modifications and variations.

Claims

1. A three-dimensional calibration method for a six-axis physiotherapy robot, characterized in that: The method is applied to a calibration platform, the calibration platform comprising: a plurality of infrared indicator lights, a control device, an infrared camera arranged on a mechanical arm of a six-axis therapy robot, and a plurality of three-dimensional feature blocks, wherein the plurality of infrared indicator lights are arranged on the plurality of three-dimensional feature blocks respectively, and the mechanical arm of the six-axis therapy robot is provided with a laser ranging component and a 3D camera; the method comprises: A 3D camera is used to collect a depth image of the three-dimensional feature block, and a theoretical coordinate of the three-dimensional feature block is obtained based on the depth image; Control the infrared indicator light to light up and the laser ranging component to emit laser; Using the image captured by the infrared camera, adjusting the mechanical arm so that the infrared indicator light and the laser light spots emitted by the laser ranging component overlap, including: lighting each infrared indicator light in turn, so that when the current infrared indicator light is lit, controlling the infrared camera to collect the image of a single three-dimensional feature block, using the PID algorithm to adjust the mechanical arm, and when the distance between the infrared indicator light and the laser light spot emitted by the laser ranging component is less than a preset threshold, using the PD algorithm to control the mechanical arm to move along a preset trajectory, so that the infrared indicator light and the laser light spots emitted by the laser ranging component overlap multiple times; Calculating the actual coordinates of the three-dimensional feature block when the light spots emitted by the infrared indicator light and the laser ranging component overlap; Based on the multiple sets of theoretical coordinates and actual coordinates, a deviation compensation matrix is ​​fitted; wherein the deviation compensation matrix is ​​used to compensate for the first transformation matrix, and the first transformation matrix is ​​a transformation matrix from the 3D camera coordinate system to the base coordinate system.

2. The three-dimensional calibration method of the six-axis physiotherapy robot according to claim 1, characterized in that: The step of adjusting the mechanical arm using the infrared image collected by the infrared camera comprises: When the height of the end of the robotic arm is controlled to be fixed, the robotic arm is iteratively adjusted based on multiple frames of infrared images so that the distance between the infrared indicator light and the spot of the laser emitted by the laser ranging component is less than a distance threshold.

3. The three-dimensional calibration method of the six-axis physiotherapy robot according to claim 2, characterized in that: The actual coordinates of the three-dimensional feature block when the light spots emitted by the infrared indicator light and the laser ranging component overlap include: Obtain the coordinates of the mechanical arm of the six-axis therapy robot before the laser ranging component and the infrared camera are installed, and the coordinates of the mechanical arm of the six-axis therapy robot after the laser ranging component and the infrared camera are installed, and confirm the translation vector of the mechanical arm of the six-axis therapy robot; The coordinate value when the infrared indicator light and the laser ranging component emit laser spots overlap is added to the translation vector to obtain the actual coordinate.

4. The three-dimensional calibration method of the six-axis physiotherapy robot according to claim 3, characterized in that: The image captured by the infrared camera is used to adjust the mechanical arm so that the infrared indicator light and the laser spot emitted by the laser ranging component overlap, including: Lighting up each infrared indicator light in turn, so that when the current infrared indicator light is lit, the infrared camera is controlled to collect an image of a single three-dimensional feature block, and the mechanical arm is adjusted so that the infrared indicator light and the laser ranging component emit laser spots overlap; After calculating the actual coordinates of the three-dimensional feature block when the current infrared indicator light and the spot of the laser emitted by the laser ranging component overlap, the current infrared indicator light is turned off to obtain the actual coordinates of the three-dimensional feature block corresponding to each infrared indicator light.

5. The three-dimensional calibration method of the six-axis physiotherapy robot according to claim 4, characterized in that: The actual coordinates of the three-dimensional feature block when the light spots emitted by the infrared indicator light and the laser ranging component overlap include: When multiple overlaps are performed, the actual coordinates of the three-dimensional feature blocks are calculated to obtain multiple groups of the actual coordinates.

6. The three-dimensional calibration method of the six-axis physiotherapy robot according to claim 2, characterized in that: The deviation compensation matrix is ​​obtained by fitting based on multiple sets of theoretical coordinates and actual coordinates, including: First formula: ;in, is the actual coordinate, is the deviation compensation matrix, is the theoretical coordinate; Second formula: ; in, is the sum square error between the coordinate after error compensation and the actual coordinate, is the coordinate after error compensation, is the actual coordinate, The number of three-dimensional feature blocks or infrared indicator lights; The deviation compensation matrix is ​​obtained by fitting based on the sum-square error and the least square method.

7. The three-dimensional calibration method of the six-axis physiotherapy robot according to claim 6, characterized in that: The value of N is greater than or equal to 10 and less than or equal to 15.

8. The three-dimensional calibration method of a six-axis physiotherapy robot according to any one of claims 1 to 7, characterized in that: The method of collecting a depth image of the three-dimensional feature block using a 3D camera and obtaining theoretical coordinates of the three-dimensional feature block based on the depth image includes: Based on the relative positions of the 3D camera and the robotic arm tool, a second transformation matrix of the 3D camera coordinate system and the robotic arm coordinate system is calculated; The theoretical coordinates are obtained by using the initial coordinates corresponding to the depth image, multiplying the second transformation matrix, and the third transformation matrix between the robot coordinate system and the base coordinate system.

9. The three-dimensional calibration method of a six-axis physiotherapy robot according to any one of claims 1 to 7, characterized in that: The infrared indicator light is located at the image center of the three-dimensional feature block.

10. A calibration platform, characterized in that: include: Infrared camera, used to be installed on the robotic arm of the six-axis physiotherapy robot; A plurality of infrared indicator lights and a plurality of three-dimensional feature blocks, wherein the plurality of infrared indicator lights are respectively arranged on the plurality of three-dimensional feature blocks; as well as A control device is connected to the infrared indicator light, the mechanical arm of the six-axis therapy robot, the laser ranging component and the infrared camera. When the control device executes the calibration program, the three-dimensional calibration method of the six-axis therapy robot as described in any one of claims 1 to 9 is implemented.

Images