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

Abstract

This application relates to a three-dimensional calibration method for a six-axis physiotherapy robot and a six-axis physiotherapy robot. The calibration method includes: executing a calibration program to obtain a deviation compensation matrix for the six-axis physiotherapy robot, where the deviation compensation matrix is used to control the movement of the robotic arm; sequentially turning on the infrared indicator lights at each evaluation point; based on the depth image collected by the 3D camera of the six-axis physiotherapy robot and the deviation compensation matrix, controlling the robotic arm to move to each evaluation point to obtain an infrared indicator light image of each evaluation point; using the target indicator light image to evaluate whether the deviation compensation matrix meets the preset calibration requirements; and storing the deviation compensation matrix when the deviation compensation matrix meets the preset calibration requirements. This application improves the calibration efficiency of the 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 for a six-axis physiotherapy robot and a 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, the calibration method of the physiotherapy robot mainly fixes or moves a 3D camera, and collects images of a calibration board to obtain images at different spatial positions for calibration.

[0004] If the above calibration method is used, when checking whether the calibration result of the physiotherapy robot meets the requirements, it is necessary to manually test the calibration result, resulting in low efficiency of the entire calibration process. Summary of the Invention

[0005] The present invention aims at the problem of how to improve the calibration efficiency of the physiotherapy robot.

[0006] In a first aspect, the present application provides a three-dimensional calibration method for a six-axis physiotherapy robot, and the calibration method includes:

[0007] Execute a calibration program to obtain a deviation compensation matrix of the six-axis physiotherapy robot, and the deviation compensation matrix is used to control the movement of the robotic arm;

[0008] Sequentially light up the infrared indicator lights at each evaluation point;

[0009] Based on the depth image collected by the 3D camera of the six-axis physiotherapy robot and the deviation compensation matrix, control the robotic arm to move to each evaluation point to obtain an infrared indicator light image at each evaluation point;

[0010] Use the target indicator light image to evaluate whether the deviation compensation matrix meets the preset calibration requirements;

[0011] When the deviation compensation matrix meets the preset calibration requirements, store the deviation compensation matrix.

[0012] In some embodiments, the method further includes: controlling a laser ranging component to emit a laser to a three-dimensional calibration block at the evaluation point to form a light spot on the three-dimensional calibration block;

[0013] The three-dimensional calibration block corresponding to the evaluation point is provided with an infrared indicator light, the working end of the robotic arm is provided with a laser ranging component and the 3D camera, and the evaluation point is used to control the light spot to overlap with the infrared indicator light.

[0014] In some embodiments, using the target indicator light image to evaluate whether the deviation compensation matrix meets the preset calibration requirements includes:

[0015] Calculating the distance between the light spot and the infrared indicator light in the target indicator light image;

[0016] When the distance is less than the distance threshold, it is confirmed that the deviation compensation matrix meets the preset calibration requirements.

[0017] In some embodiments, the calibration method further includes:

[0018] When the difference between the laser measurement height and the 3D camera measurement height is less than the height difference threshold, it is confirmed that the deviation compensation matrix meets the preset calibration requirements;

[0019] Wherein, the laser measurement height is the height measured by the laser ranging component, and the 3D camera measurement height is the height calculated from the depth map collected by the 3D camera.

[0020] In some embodiments, executing the calibration program includes:

[0021] Using the depth image collected by the 3D camera to obtain the theoretical coordinates of the three-dimensional feature blocks at each calibration point position;

[0022] Controlling the infrared indicator light to turn on and the laser ranging component to emit laser light, and the laser light forms a light spot on the three-dimensional feature block;

[0023] Using the infrared image collected by the infrared camera to adjust the robotic arm so that the light spot of the infrared indicator light and the laser light emitted by the laser ranging component overlap;

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

[0025] Using the least squares method, based on multiple groups of the theoretical coordinates and actual coordinates, fitting to obtain the deviation compensation matrix.

[0026] In some embodiments, using the infrared image collected by the infrared camera to adjust the robotic arm includes:

[0027] Using the laser ranging component to measure the first height of the working end of the robotic arm from the three-dimensional feature block at the calibration point position;

[0028] When controlling the working end of the robotic arm to maintain the first height, controlling the infrared camera to continuously collect images;

[0029] Based on multiple frames of infrared images, iteratively adjusting the robotic arm until the light spot of the laser ranging component on the three-dimensional calibration block overlaps with the infrared indicator light, and then controlling the infrared camera to collect the target indicator light image.

[0030] In some embodiments, the calibration method further includes:

[0031] When adjusting the robotic arm by using an infrared image collected by an infrared camera such that the infrared indicator light corresponding to the current calibration point and the spot of the laser emitted by the laser ranging component overlap, turn off the infrared indicator lights corresponding to other three-dimensional feature blocks.

[0032] In some embodiments, using the least squares method to fit a deviation compensation matrix based on multiple groups of the theoretical coordinates and actual coordinates includes:

[0033] ;

[0034] wherein, is the actual coordinate, is the deviation compensation matrix, is the theoretical coordinate;

[0035] ;

[0036] 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;

[0037] Based on the sum of squared errors and the least squares method, fit to obtain the deviation compensation matrix.

[0038] In some embodiments, the method further includes:

[0039] In the case where the deviation compensation matrix does not meet the preset calibration requirements, re-execute the calibration program to update the deviation compensation matrix of the six-axis physiotherapy robot.

[0040] In a second aspect, the present application provides a physiotherapy robot, and the deviation compensation matrix of the physiotherapy robot is calibrated by using the above-mentioned three-dimensional calibration method for a six-axis physiotherapy robot.

[0041] The present application controls the robotic arm to move to a target position by using the image collected by the 3D camera and the deviation compensation matrix, turns on the indicator light, uses the camera to collect the image of the indicator light, and then determines whether the robotic arm accurately moves to the target position according to the position of the indicator light in the image or the relative position between the indicator light and other objects in the image, thereby determining whether the calibrated deviation compensation matrix is correct. When it is correct, save the deviation compensation matrix, and when it is incorrect, re-execute the calibration program to update the deviation compensation matrix. The present application realizes the automatic calibration of the physiotherapy robot. BRIEF DESCRIPTION OF THE DRAWINGS

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

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

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

[0045] Figure 4 It is a flowchart of yet another embodiment of the three-dimensional calibration method of the six-axis physical therapy robot of the present application;

[0046] Figure 5 It is a flowchart of yet another embodiment of the three-dimensional calibration method of the six-axis physical therapy robot of the present application.

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

[0048] 10. Laser ranging component; 20. 3D camera; 31. Infrared indicator light; 32. Three-dimensional feature block; 40. Manipulator; 50. Control device; 60. Infrared camera. Detailed implementation manners

[0049] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying 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 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.

[0050] In the description of the present invention, the terms "first" and "second" are only used for descriptive purposes, and cannot be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Thus, the 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.

[0051] 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 advantageous than other embodiments. The following description is provided to enable any person skilled in the art to implement 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 these specific details. In other instances, well-known structures and processes are not elaborated 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 rather to be in line with the broadest scope consistent with the principles and features disclosed in the present invention.

[0052] Embodiment 1: The present application provides a three-dimensional calibration method for a six-axis physical therapy robot. This calibration method can achieve automatic calibration and automatically verify whether the deviation compensation matrix obtained by calibration meets the preset calibration requirements.

[0053] Refer to Figure 1 , in one implementation, the calibration program is executed by a calibration platform, which 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 physical therapy robot, and a plurality of three-dimensional feature blocks 32.

[0054] Exemplarily, the three-dimensional feature blocks 32 are distributed at a certain interval in the horizontal direction; exemplarily, along the height direction, the heights of the three-dimensional feature blocks 32 are uneven. 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. 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] Refer to Figure 2 , in one embodiment, the calibration method includes:

[0056] S10. Execute the calibration program to obtain the deviation compensation matrix of the six-axis physical therapy robot, and the deviation compensation matrix is used to control the movement of the robotic arm 40;

[0057] Refer to Figure 3 , in practical applications, executing the calibration program can achieve the calibration steps of steps S100 to S500 mentioned below. However, this is not limited here, and those skilled in the art can also implement it using other calibration methods as long as they can adapt to steps S20 to S50 of the present application.

[0058] S20. Sequentially light up the infrared indicator lights 31 at each evaluation point;

[0059] Refer to Figure 1There are 13 three-dimensional feature blocks 32 on the calibration platform, and any number of the three-dimensional feature blocks 32 can be selected as the three-dimensional feature blocks 32 corresponding to the evaluation points. For example, four of the feature blocks can be selected as evaluation objects.

[0060] Turning on the infrared indicator light 31 of each evaluation point in turn may mean turning on the indicator light of the evaluation point when the evaluation point is needed, and turning off the indicator light of the evaluation point when the evaluation point is not needed, so as to avoid interfering with the evaluation results of other points.

[0061] S30, based on the depth image captured by the 3D camera 20 of the six-axis physiotherapy robot and the deviation compensation matrix, control the robotic arm 40 to move to each evaluation point to obtain the image of the infrared indicator light 31 of each evaluation point;

[0062] The 3D camera 20 can capture a depth image. In the depth image, we can obtain the height and plane position of the target point, that is, the XYZ three-axis coordinates, and then control the movement of the robotic arm 40 according to the depth image.

[0063] S40, using the target indicator light image to evaluate whether the deviation compensation matrix meets the preset calibration requirements;

[0064] The present application is to evaluate whether the evaluation coordinates calculated by the 3D camera 20 and the deviation compensation matrix can control the robot arm 40 to actually move to the physical position of the evaluation coordinates.

[0065] The target indicator light image can be obtained by taking a picture with a camera. In practical applications, in order to combine the laser distance measuring component 10 , the camera can be an infrared camera 60 , and the indicator light can be an infrared indicator light 31 .

[0066] The preset calibration requirement evaluation may refer to an indicator light image indicating whether the robot arm 40 has moved to a physical position corresponding to the evaluation coordinates.

[0067] In actual applications, the robotic arm 40 can be controlled to move to each evaluation point to obtain the infrared indicator light 31 image of each evaluation point. If the indicator light image represents that the robotic arm 40 is located at the physical position corresponding to the evaluation coordinates, it means that the deviation compensation matrix meets the preset calibration requirements. If the indicator light image represents that the robotic arm 40 deviates from the physical position corresponding to the evaluation coordinates, and the deviation distance is greater than the threshold, it is considered that the preset calibration requirements are not met.

[0068] Exemplarily, the preset calibration requirements may include: a Euclidean distance threshold on the plane (XY coordinates) and a height difference threshold on the height (Z coordinates), and judging whether the robot arm 40 moves to the physical position corresponding to the evaluation coordinates from these two dimensions.

[0069] In other embodiments, the preset calibration requirements can be set according to the type of the physiotherapy robot. For example, for a facial physiotherapy robot, the preset calibration requirements are set more strictly (for example, the Euclidean distance threshold is 5 millimeters and the height difference is 3 millimeters); if it is a back physiotherapy robot, the preset calibration requirements can be set more loosely (for example, the Euclidean distance threshold is 1.5 centimeters and the height difference is 5 millimeters), and specifically, it can be set according to the positioning of the product.

[0070] S50. When the deviation compensation matrix meets the preset calibration requirements, store the deviation compensation matrix.

[0071] In some embodiments, the method further includes: when the deviation compensation matrix does not meet the preset calibration requirements, re - execute the calibration program to update the deviation compensation matrix of the six - axis physiotherapy robot.

[0072] This application controls the robotic arm 40 to move to the target position by using the images collected by the 3D camera 20 and the deviation compensation matrix, lights up the indicator light, uses the camera to collect the image of the indicator light, and then determines whether the robotic arm 40 accurately moves to the target position according to the position of the indicator light in the image or the relative position between the indicator light and other objects in the image, thereby judging whether the deviation compensation matrix obtained by calibration is correct. When it is correct, save the deviation compensation matrix; when it is incorrect, re - execute the calibration program to update the deviation compensation matrix. This application realizes the automatic calibration of the physiotherapy robot.

[0073] In some embodiments, the method further includes: controlling the laser ranging component 10 to emit laser to the three - dimensional calibration block at the evaluation point to form a light spot on the three - dimensional calibration block;

[0074] The three - dimensional calibration block corresponding to the evaluation point is provided with an infrared indicator light 31. The working end of the robotic arm 40 is provided with the laser ranging component 10 and the 3D camera 20. The evaluation point is used to control the light spot to overlap with the infrared indicator light 31.

[0075] The camera in the embodiment is an infrared camera 60, and the indicator light is an infrared indicator light 31. The theoretical position of the robotic arm 40 when the light spot overlaps with the infrared indicator light 31 can be obtained through the 3D camera 20. Then, based on the theoretical position and the current position of the robotic arm 40, a control quantity is generated to control the robotic arm 40 to move to the theoretical position.

[0076] At the theoretical position, the light spot overlaps with the infrared indicator light 31. Therefore, we can determine whether the deviation compensation matrix is correct according to whether the light spot overlaps with the infrared indicator light 31. Compared with directly using the position of the indicator light in the image, calculating whether they overlap is more reliable;

[0077] In some embodiments, whether there is an overlap can be directly confirmed through image analysis without calculating the Euclidean distance.

[0078] In some embodiments, using the target indicator light image to evaluate whether the deviation compensation matrix meets the preset calibration requirements includes:

[0079] Calculating the distance between the light spot in the target indicator light image and the infrared indicator light 31;

[0080] When the distance is less than the distance threshold, it is confirmed that the deviation compensation matrix meets the preset calibration requirements.

[0081] In practical applications, the overlap degree between the light spot and the indicator light can be calculated first. If the overlap degree is greater than the threshold (for example, 1%, that is, whether there is an overlap), it is directly confirmed that the deviation compensation matrix meets the preset calibration requirements;

[0082] When there is no overlap, the Euclidean distance between the spot and the infrared indicator light 31 can be calculated and converted into the distance in the world coordinate system. If it is less than the threshold (for example, 5 mm), it is confirmed that the deviation compensation matrix meets the preset calibration requirements.

[0083] Furthermore, this calibration method further includes:

[0084] When the difference between the laser measurement height and the 3D camera 20 measurement height is less than the height difference threshold, it is confirmed that the deviation compensation matrix meets the preset calibration requirements;

[0085] Wherein, the laser measurement height is the height measured by the laser ranging component 10, and the 3D camera 20 measurement height is the height calculated from the depth map collected by the 3D camera 20.

[0086] In this embodiment, the laser ranging component 10 is also used to detect whether the depth image collected by the 3D camera 20 can correctly calculate the height information. If the height difference is less than the threshold (for example, 3 mm), it is confirmed that the deviation compensation matrix meets the preset calibration requirements.

[0087] Refer to Figure 3 , in some embodiments, executing the calibration program includes:

[0088] 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;

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

[0090] 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 coordinate system of the 3D camera 20 and the coordinate system of the robotic arm 40:

[0091] ;

[0092] ;

[0093] ;

[0094] ;

[0095] 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 coordinate system of the 3D camera 20 and the coordinate system of the robotic arm 40. The , , in 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.

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

[0097] ;

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

[0099] 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 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 ( ), applying Euler angles can also be calculated as follows:

[0100] ;

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

[0102] 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 between them :

[0103] ;

[0104] 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.

[0105] 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.

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

[0107] 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.

[0108] 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;

[0109] After the infrared camera 60 collects the infrared image, the pixel center positions of the infrared indicator light 31 and the laser ranging light spot can be obtained by using image segmentation recognition. The difference between the two pixel center positions can 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 iteratively controlled to move to collect infrared images until the difference between the two pixel center positions in the latest collected infrared image meets the requirements, and it can be considered 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] In this embodiment, the actual coordinates of the three-dimensional feature block 32 can reach the coordinates of the infrared indicator light 31.

[0112] After obtaining the theoretical coordinate system, TCP (Tool Center Point) measurement is used. Since calibration is performed using 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.

[0113] Specifically, for TCP measurement of the calibration tool, the cone alignment position of the working end of the robotic arm 40 in a fixed pose can be obtained before installing the calibration tool , and then the cone alignment position of the laser rangefinder after installing the calibration tool can be obtained . After the light 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 is obtained , and the actual three-dimensional feature information can be obtained after calibration .

[0114] ;

[0115] ;

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

[0117] S500. Based on multiple groups of the theoretical coordinates and 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.

[0118] 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 here. 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 groups 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.

[0119] ;

[0120] ;

[0121] 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 be adjusted 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 physiotherapy coordinates, which is used to compensate the first transformation matrix, achieving an improvement in the calibration accuracy of the six-axis physiotherapy robot.

[0122] In some embodiments, adjusting the robotic arm 40 by using the infrared image collected by the infrared camera 60 includes:

[0123] When controlling the end height of the robotic arm 40 to be fixed, the robotic arm 40 is iteratively adjusted based on multiple frames of infrared images, so that the distance between the light spots of the laser emitted by the infrared indicator light 31 and the laser ranging component 10 is less than a distance threshold.

[0124] In this embodiment, the end height of the robotic arm 40 is controlled to be fixed, and then the infrared images are iteratively collected and the robotic arm 40 is adjusted. Finally, when the distance between the light spots of the laser emitted by the infrared indicator light 31 and the laser ranging component 10 is less than the distance threshold, it can be considered that the two overlap, and the distance threshold can be set according to the error acceptance degree in actual applications.

[0125] In some embodiments, calculating 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 includes:

[0126] Obtain the coordinates of the robotic arm 40 of the six-axis physiotherapy 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 physiotherapy robot after installing the laser ranging component 10 and the infrared camera 60, and confirm the translation vector of the robotic arm 40 of the six-axis physiotherapy robot;

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

[0128] In this embodiment, a fixed posture is used for calibration during calibration. Therefore, only by confirming the translation vector of the robotic arm 40, the actual coordinates of the three-dimensional feature block 32 can be calculated.

[0129] Refer to Figure 4, in some embodiments, for the image collected by the infrared camera 60 in S300, adjusting the robotic arm 40 to make the light spots of the infrared indicator light 31 and the laser emitted by the laser ranging component 10 overlap includes:

[0130] S301. Light each infrared indicator light 31 in sequence. When the current infrared indicator light 31 is lit, control the infrared camera 60 to collect an image of a single three-dimensional feature block 32, and adjust the robotic arm 40 to make the light spots of the infrared indicator light 31 and the laser emitted by the laser ranging component 10 overlap;

[0131] 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, turn 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.

[0132] In this embodiment, among the 13 groups of three-dimensional feature blocks 32, the distance between adjacent three-dimensional feature blocks 32 is less than a preset value, for example, less than 5 cm, which can reduce the moving distance of the robotic arm 40 and reduce the calibration error. However, when the distance between adjacent three-dimensional feature blocks 32 is less than the preset value, the infrared lights of adjacent three-dimensional feature blocks 32 will affect the infrared image of the current three-dimensional feature block 32, and further affect the control accuracy of the robotic arm 40 during the process of making the light spots of the infrared indicator light 31 and the laser emitted by the laser ranging component 10 overlap.

[0133] To solve the above problems, 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 in this application, the current infrared indicator light 31 is turned off, avoiding the interference of the infrared indicator light 31 and improving the accuracy.

[0134] Refer to Figure 5 , in some embodiments, S301. Light each infrared indicator light 31 in sequence. When the current infrared indicator light 31 is lit, control the infrared camera 60 to collect an image of a single three-dimensional feature block 32, and adjust the robotic arm 40 to make the light spots of the infrared indicator light 31 and the laser emitted by the laser ranging component 10 overlap includes:

[0135] S3011. Use 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, use 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;

[0136] S302. Calculate the actual coordinates of the three-dimensional feature block 32 when the light spots of the infrared indicator light 31 and the laser ranging component 10 emit laser light overlap, including: S3021. Calculate the actual coordinates of the three-dimensional feature block 32 when overlapping multiple times, and obtain multiple groups of the actual coordinates.

[0137] During the actual calibration process, in order to improve the accuracy, when the distance between the light spots of the infrared indicator light 31 and the laser ranging component 10 emitting laser light is less than the preset threshold, this application needs to control the light spots of the infrared indicator light 31 and the laser ranging component 10 emitting laser light to overlap multiple times. If PID control is used throughout the process, when the distance between the light spots of the infrared indicator light 31 and the laser ranging component 10 emitting laser light is less than the preset threshold in this application, the differential component (I, Integral) in the PID control algorithm will affect the accuracy of this process. Therefore, this application first uses the PID algorithm to control the robotic arm 40, and when the distance between the light spots of the infrared indicator light 31 and the laser ranging component 10 emitting laser light is less than the preset threshold, it switches to the PD control algorithm to reduce the influence brought by the differential.

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

[0139] The first formula: ;

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

[0141] The second formula: ;

[0142] 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 the three-dimensional feature block 32 or the infrared indicator light 31;

[0143] Based on the sum of squared errors and the least squares method, the deviation compensation matrix is fitted.

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

[0145] In some embodiments, the use of 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 includes:

[0146] Calculate a second transformation matrix for the coordinate system of the 3D camera 20 and the coordinate system of the robotic arm 40 based on the relative positions of the 3D camera 20 and the robotic arm 40;

[0147] Use the initial coordinates corresponding to the depth image, multiply 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.

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

[0149] 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 capture the infrared indicator light 31 and the light spot.

[0150] Embodiment 2: The present application provides a physiotherapy robot, and the deviation compensation matrix of the physiotherapy robot is calibrated using the three-dimensional calibration method of the six-axis physiotherapy robot in Embodiment 1.

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

[0152] Those skilled in the art should 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 completely hardware embodiment, a completely 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.) containing computer-usable program code.

[0153] 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 process and / or block in the flowcharts and / or block diagrams, and the combination of processes and / or blocks in the flowcharts and / or block diagrams, can be implemented 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 implementing the functions specified in Figure 1 one process or multiple processes and / or blocks Figure 1 one block or multiple blocks.

[0154] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means embodying the function specified in the flowchart(s) Figure 1 a flowchart or flowcharts and / or block(s) Figure 1 or block(s) specified.

[0155] These computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process, whereby the instructions executed on the computer or other programmable apparatus provide steps for implementing the function specified in the flowchart(s) Figure 1 a flowchart or flowcharts and / or block(s) Figure 1 or block(s) specified.

[0156] Although the preferred embodiments of the present invention have been described, additional changes and modifications can be made by those skilled in the art once they learn of the basic inventive concept. Therefore, the appended claims are intended to be construed to include the preferred embodiments as well as all changes and modifications falling within the scope of the present invention.

[0157] It is apparent that those skilled in the art can make various changes and modifications to the present invention without departing from the spirit and scope of the 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 changes and modifications.

Claims

1. A three-dimensional calibration method for a six-axis physiotherapy robot, characterized in that: The method comprises: Executing a calibration program to obtain a deviation compensation matrix of the six-axis physiotherapy robot, wherein the deviation compensation matrix is ​​used to control the movement of the robot arm; the executing calibration program includes: Use the depth image collected by the 3D camera to obtain the theoretical coordinates of the three-dimensional feature blocks of each calibration point; Controlling the infrared indicator light to light up and the laser ranging component to emit laser, wherein the laser forms a light spot on the three-dimensional feature block; Using the infrared image captured by the infrared camera, the robotic arm is adjusted so that the infrared indicator light and the laser light spot emitted by the laser ranging component overlap; 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; Using the least square method, based on the multiple sets of theoretical coordinates and actual coordinates, a deviation compensation matrix is ​​obtained by fitting; Light up the infrared indicator light of each assessment point in turn; Based on the depth image captured by the 3D camera of the six-axis physiotherapy robot and the deviation compensation matrix, the robotic arm is controlled to move to each evaluation point to obtain the infrared indicator light image of each evaluation point; Using the target indicator light image to evaluate whether the deviation compensation matrix meets the preset calibration requirements; When the deviation compensation matrix meets the preset calibration requirements, storing the deviation compensation matrix; and Controlling the laser ranging component to emit laser to the three-dimensional calibration block of the evaluation point to form a light spot on the three-dimensional calibration block; The three-dimensional calibration block corresponding to the evaluation point is provided with an infrared indicator light, the working end of the robotic arm is provided with a laser ranging component and the 3D camera, and the evaluation point is used to control the overlap of the light spot and the infrared indicator light.

2. The three-dimensional calibration method of the six-axis physiotherapy robot according to claim 1, characterized in that: The using the target indicator light image to evaluate whether the deviation compensation matrix meets the preset calibration requirements includes: Calculating the distance between the light spot in the target indicator light image and the infrared indicator light; When the distance is less than the distance threshold, it is confirmed that the deviation compensation matrix meets the preset calibration requirement.

3. The three-dimensional calibration method of the six-axis physiotherapy robot according to claim 1, characterized in that: Also includes: When the difference between the height measured by the laser and the height measured by the 3D camera is less than the height difference threshold, confirming that the deviation compensation matrix meets the preset calibration requirement; The laser measured height is the height measured by the laser ranging component, and the 3D camera measured height is the height calculated from the depth map collected by the 3D camera.

4. 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: Using a laser distance measuring component to measure a first height of a three-dimensional feature block between a working end of the robotic arm and a calibration point; When controlling the working end of the robotic arm to maintain the first height, controlling the infrared camera to continuously collect images; The mechanical arm is iteratively adjusted based on multiple frames of infrared images until the light spot of the laser ranging component on the three-dimensional calibration block overlaps with the infrared indicator light, and the infrared camera is controlled to collect the image of the target indicator light.

5. The three-dimensional calibration method of the six-axis physiotherapy robot according to claim 1, characterized in that: Also includes: Using the infrared image collected by the infrared camera, the robotic arm is adjusted so that when the infrared indicator light corresponding to the current calibration point overlaps with the spot of the laser emitted by the laser ranging component, the infrared indicator lights corresponding to other three-dimensional feature blocks are turned off.

6. The three-dimensional calibration method of the six-axis physiotherapy robot according to claim 1, characterized in that: The least square method is used to fit a deviation compensation matrix based on multiple sets of theoretical coordinates and actual coordinates, including: ； in, is the actual coordinate, is the deviation compensation matrix, is the theoretical coordinate; ； 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, N is 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 1, characterized in that: Also includes: When the deviation compensation matrix does not meet the preset calibration requirements, the calibration procedure is re-executed to update the deviation compensation matrix of the six-axis physiotherapy robot.

8. A physical therapy robot, characterized in that: The deviation compensation matrix of the physiotherapy robot is calibrated using the three-dimensional calibration method for a six-axis physiotherapy robot as described in any one of claims 1-7.

Images