Six-axis physiotherapy robot three-dimensional calibration method and calibration platform

Abstract

The invention relates to a six-axis physiotherapy robot three-dimensional calibration method and platform, and the method comprises the steps: collecting a depth image of a three-dimensional feature block through a 3D camera, and obtaining the theoretical coordinates of the three-dimensional feature block based on the depth image; controlling an infrared indicating lamp to be lightened and a laser ranging assembly to emit laser; the mechanical arm is adjusted by using an image collected by the infrared camera, so that light spots of laser emitted by the infrared indicating lamp and the laser ranging assembly are overlapped; calculating actual coordinates of the three-dimensional feature block when light spots of laser emitted by the infrared indicating lamp and the laser ranging assembly are overlapped; fitting to obtain a deviation compensation matrix based on multiple groups of theoretical coordinates and actual coordinates; wherein the deviation compensation matrix is used for compensating a first transformation matrix, and the first transformation matrix is a transformation matrix from a 3D camera coordinate system to a base coordinate system. According to the invention, high-precision calibration of the six-axis physiotherapy robot is realized.

Description

Technical Field

[0001] The present invention relates to the field of physiotherapy robot manufacturing, and in particular 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, physiotherapy robots have very high requirements for the accuracy of visual calibration.

[0003] At present, there are two main 3D system layout schemes for six-axis therapy robots: hands on eyes and hands outside eyes. The 3D calibration schemes of the two layouts are very different. Both use fixed and mobile 3D cameras or calibration plates to collect images at different spatial positions. The calibration accuracy is generally between 2cm and 3cm.

[0004] The current calibration accuracy of physiotherapy robots is insufficient. Summary of the invention

[0005] The present invention aims to solve the problem of insufficient calibration accuracy of a physiotherapy robot.

[0006] In a first aspect, the present application provides a three-dimensional calibration method for a six-axis therapy robot, which is applied to a calibration platform, wherein the calibration platform comprises: a plurality of infrared indicator lights, a control device, an infrared camera arranged on a mechanical arm of the six-axis therapy robot, 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 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, the robotic arm is adjusted so that the infrared indicator light and the laser ranging component emit laser spots 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; 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.

[0007] In some embodiments, the step of adjusting the robotic arm using the infrared image captured by the infrared camera includes: 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.

[0008] In some embodiments, the calculation of 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 includes: Obtain the coordinates of the robotic arm of the six-axis therapy robot before the laser ranging component and the infrared camera are installed, and the coordinates of the robotic 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 robotic 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.

[0009] In some embodiments, the image captured by the infrared camera is used to adjust the robotic 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.

[0010] In some embodiments, lighting up each infrared indicator light in sequence, and when the current infrared indicator light is lit, controlling the infrared camera to collect an image of a single three-dimensional feature block, and adjusting the mechanical arm so that the infrared indicator light and the laser ranging component emit laser spots overlap, include: The mechanical arm is adjusted using a PID algorithm, and when the distance between the infrared indicator light and the laser ranging component emitting laser spots is less than a preset threshold, the mechanical arm is controlled to move along a preset trajectory using a PD algorithm so that the infrared indicator light and the laser ranging component emitting laser spots overlap multiple times; 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.

[0011] In some embodiments, fitting a deviation compensation matrix based on multiple sets of theoretical coordinates and actual coordinates includes: 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, 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.

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

[0013] In some embodiments, 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.

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

[0015] In a second aspect, the present application provides a calibration platform, which comprises: 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; and The 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 the above items is implemented.

[0016] The embodiment of the present application lights up the infrared indicator light and controls the laser ranging component to work, then controls the infrared camera to capture an infrared image, and controls and adjusts the robotic arm based on the infrared image so that the light spots of the infrared indicator light and the laser ranging component emit overlap; so as to calculate the actual coordinates of the three-dimensional feature block when the light spots of the infrared indicator light and the laser ranging component emit overlap; thereby, a deviation compensation matrix can be calculated based on the actual coordinates and the therapy coordinates to compensate for the first transformation matrix, thereby improving the calibration accuracy of the six-axis therapy robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 This is a structural schematic diagram of an embodiment of a calibration platform of the present application; Figure 2 This is a flowchart of an embodiment of a three-dimensional calibration method for a six-axis physiotherapy robot of the present application; Figure 3 This is a flowchart of another embodiment of the three-dimensional calibration method of the six-axis physiotherapy robot of the present application; Figure 4 This is a flowchart of another embodiment of the three-dimensional calibration method of the six-axis physiotherapy robot of the present application.

[0018] In the accompanying drawings, the components represented by the reference numerals are listed as follows: 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 DESCRIPTION

[0019] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present invention.

[0020] In the description of the present invention, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Thus, the features defined as "first" and "second" may explicitly or implicitly include one or more of the features. In the description of the present invention, the meaning of "plurality" is two or more, unless otherwise clearly and specifically defined.

[0021] In the description of the present invention, the term "for example" is used to mean "used as an example, illustration or explanation". Any embodiment described as "for example" in the present invention is not necessarily to be construed as being more preferred or advantageous than other embodiments. The following description is given to enable any technician in the field to implement and use the present invention. In the following description, details are listed for the purpose of explanation. It should be understood that a person of ordinary skill in the art can recognize that the present invention can be implemented without using these specific details. In other examples, well-known structures and processes will not be 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 is consistent with the widest scope consistent with the principles and features disclosed in the present invention.

[0022] Example 1: The calibration accuracy requirement of a six-axis therapy robot is very high, generally at the millimeter level. The present application provides a three-dimensional calibration method for a six-axis therapy robot, the purpose of which is to improve the calibration accuracy of the six-axis therapy robot.

[0023] Reference 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 arranged on a robotic arm 40 of a six-axis physiotherapy robot, and a plurality of three-dimensional feature blocks 32.

[0024] Exemplarily, the three-dimensional feature blocks 32 are distributed at certain intervals in the horizontal direction; Exemplarily, along the height direction, the heights of the three-dimensional feature blocks 32 are uneven.

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

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

[0027] Reference Figure 1 The six-axis physiotherapy robot includes a mechanical arm 40, and a laser ranging component 10 and a 3D camera 20 arranged at the working end of the mechanical arm 40; Reference Figure 2 In one embodiment, the three-dimensional calibration method of the six-axis therapy robot of the present application includes: S100, using the 3D camera 20 to collect a depth image of the three-dimensional feature block 32, and obtaining theoretical coordinates of the three-dimensional feature block 32 based on the depth image; It should be noted that the mechanical arm 40 of the six-axis therapy robot is controlled by six axes, namely, translation control of up and down, left and right, and front and back, and rotation control of pitch axis, roll axis, and yaw axis. Figure 1 In practical applications, the end of the robot arm 40 is used as the origin of the base coordinate system.

[0028] First, based on the installation position of the 3D camera 20 and the rotation position relationship of the robotic arm 40, a rotation matrix between the coordinate system of the 3D camera 20 and the coordinate system of the robotic arm 40 is established: ; ; ; ; in, , , Respectively represent the roll rotation matrix, pitch rotation matrix, and heading rotation matrix of the rotation matrix, is the rotation matrix between the 3D camera 20 coordinate system and the robot arm 40 coordinate system. , , They respectively represent the roll angle, pitch angle, and yaw angle in the relationship between the installation position of the 3D camera 20 and the rotation position of the robot arm 40 .

[0029] Based on the installation position relationship between the 3D camera 20 and the translation position of the robotic arm 40, the translation vector is added to the rotation matrix to obtain the transformation matrix between the 3D camera 20 coordinate system and the robotic arm 40 coordinate system: ; in, represents the transformation matrix between the 3D camera 20 coordinate system and the robot arm 40 coordinate system, That is, the rotation matrix between the 3D camera 20 coordinate system and the robot arm 40 coordinate system. It is a 3*3 matrix, so a dotted box is reserved in the formula. X, Y, and Z are the translation matrices between the 3D camera 20 coordinate system and the robot arm 40 coordinate system.

[0030] Since the coordinate system of the calibrated data is the robot arm 40 coordinate system, and the coordinate system used in the actual control process of the robot arm 40 is the base coordinate system, it is also necessary to calculate the change matrix M_T2B between the robot arm 40 coordinate system and the base coordinate system. ), the same calculation using Euler angles can be calculated: ; in, is the change matrix between the robot arm 40 coordinate system and the base coordinate system.

[0031] In summary, we can obtain the output data of the robot arm 40 base coordinate system And 3D camera 20 coordinate system measurement data The transformation relationship between M_C2B: ; That is to say, , Afterwards, the 3D camera 20 is used to collect a depth image of the three-dimensional feature block 32, and the initial coordinates of the target point (such as the center point) of the three-dimensional feature block 32 are obtained based on the depth image, and then the initial coordinates are multiplied by , , the theoretical coordinates can be obtained.

[0032] 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 implemented through steps S200 to S400.

[0033] S200, controlling the infrared indicator light 31 to light up and the laser ranging component 10 to emit laser; 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, allowing the physical therapy robot to control the laser ranging component 10 to work so as to form a light spot on the three-dimensional feature block 32.

[0034] S300, using the image captured by the infrared camera 60, adjusting the mechanical arm 40 so that the infrared indicator light 31 and the laser spot emitted by the laser ranging component 10 overlap; After the infrared camera 60 captures the infrared image, image segmentation and recognition can be used to obtain the pixel center positions of the infrared indicator light 31 and the laser ranging light spot. 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 and iteratively controlled to move and capture infrared images until the difference between the two pixel center positions in the latest captured infrared image meets the requirement. It can be considered that the infrared indicator light 31 and the laser ranging component 10 emit laser light spots that overlap.

[0035] S400, calculating the actual coordinates of the three-dimensional feature block 32 when the infrared indicator light 31 and the laser spot emitted by the laser ranging component 10 overlap; In this embodiment, the actual coordinates of the three-dimensional feature block 32 can be the coordinates of the infrared indicator light 31 .

[0036] After obtaining the theoretical coordinate system, TCP (Tool Center Point) is used for measurement. Since a fixed posture is used for calibration, it is only necessary to confirm the translation vector of the robot arm 40 to calculate the actual coordinates of the three-dimensional feature block 32 .

[0037] Specifically, the calibration tool TCP measurement can obtain the cone alignment position of the working end of the robot arm 40 in a fixed posture before installing the calibration tool. ( ), and then obtain the cone alignment position of the laser rangefinder after installing the calibration tool ( After the infrared indicator light 31 and the laser ranging component 10 emit laser spots overlap, the three-dimensional coordinate position of the current robot arm 40 is obtained. ( ), after correction, the actual three-dimensional feature information can be obtained ( ).

[0038] ; ; in, The translation vector of the robot arm 40.

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

[0040] In this embodiment, the number of the three-dimensional feature blocks 32 is set to 13 for illustration. In other embodiments, the number of the three-dimensional feature blocks 32 may be other numbers, which are not limited here. Thus, in this embodiment, 13 sets of theoretical three-dimensional feature information of the three-dimensional feature platform are obtained. and the actual 3D feature information value , perform linear least squares iterative fitting. The optimization compensation matrix M_B2R contains 12 unknowns, and four sets of data are required for fitting. The values ​​of the 13 feature points after error compensation are and the actual three-dimensional space value Difference and square error Calculation, by randomly selecting a sufficient amount of sample data, linear least squares iterative fitting is performed until the compensation matrix M_B2R is optimized to meet the accuracy requirements.

[0041] ; ; The embodiment of the present application lights up the infrared indicator light 31 and controls the laser ranging component 10 to work, then controls the infrared camera 60 to capture an infrared image, and controls and adjusts the robotic arm 40 based on the infrared image so that the infrared indicator light 31 and the laser ranging component 10 emit a laser spot that overlaps; calculates the actual coordinates of the three-dimensional feature block 32 when the infrared indicator light 31 and the laser ranging component 10 emit a laser spot that overlaps; and thereby calculates a deviation compensation matrix based on the actual coordinates and the therapy coordinates to compensate for the first transformation matrix, thereby improving the calibration accuracy of the six-axis therapy robot.

[0042] The embodiment of the present application lights up the infrared indicator light 31 and controls the laser ranging component 10 to work, then controls the infrared camera 60 to capture an infrared image, and controls and adjusts the robotic arm 40 based on the infrared image so that the infrared indicator light 31 and the laser ranging component 10 emit a laser spot that overlaps; calculates the actual coordinates of the three-dimensional feature block 32 when the infrared indicator light 31 and the laser ranging component 10 emit a laser spot that overlaps; and thereby calculates a deviation compensation matrix based on the actual coordinates and the therapy coordinates to compensate for the first transformation matrix, thereby improving the calibration accuracy of the six-axis therapy robot.

[0043] 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, N is the number of three-dimensional feature blocks 32 or infrared indicator lights 31; The deviation compensation matrix is ​​obtained by fitting based on the sum-square error and the least square method.

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

[0045] In some embodiments, the method of using the 3D camera 20 to collect a 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: Based on the relative positions of the 3D camera 20 and the robotic arm 40, a second transformation matrix of the 3D camera 20 coordinate system and the robotic arm 40 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 arm 40 coordinate system and the base coordinate system.

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

[0047] The infrared indicator light 31 is located at the image center of the three-dimensional feature block 32, so that the infrared camera 60 can better capture the infrared indicator light 31 and the light spot.

[0048] Embodiment 2: The present application provides a calibration platform, which comprises: An infrared camera 60 is used to be installed on a mechanical arm 40 of a six-axis physiotherapy robot; A plurality of infrared indicator lights 31 and a plurality of three-dimensional feature blocks 32, wherein the plurality of infrared indicator lights 31 are respectively arranged on the plurality of three-dimensional feature blocks 32; and The control device 50 is connected to the infrared indicator light 31, the mechanical arm 40 of the six-axis therapy robot, the laser ranging component 10 and the infrared camera 60. When the control device 50 executes the calibration program, the following three-dimensional calibration method of the six-axis therapy robot is implemented: S100, using the 3D camera 20 to collect a depth image of the three-dimensional feature block 32, and obtaining theoretical coordinates of the three-dimensional feature block 32 based on the depth image; S200, controlling the infrared indicator light 31 to light up and the laser ranging component 10 to emit laser; S300, using the image captured by the infrared camera 60, adjusting the mechanical arm 40 so that the infrared indicator light 31 and the laser spot emitted by the laser ranging component 10 overlap; S400, calculating the actual coordinates of the three-dimensional feature block 32 when the infrared indicator light 31 and the laser spot emitted by the laser ranging component 10 overlap; S500, fitting a deviation compensation matrix based on the multiple sets of theoretical coordinates and actual coordinates; wherein the deviation compensation matrix is ​​used to compensate a first transformation matrix, and the first transformation matrix is ​​a transformation matrix from the coordinate system of the 3D camera 20 to the base coordinate system.

[0049] The embodiment of the present application lights up the infrared indicator light 31 and controls the laser ranging component 10 to work, then controls the infrared camera 60 to capture an infrared image, and controls and adjusts the robotic arm 40 based on the infrared image so that the infrared indicator light 31 and the laser ranging component 10 emit a laser spot that overlaps; calculates the actual coordinates of the three-dimensional feature block 32 when the infrared indicator light 31 and the laser ranging component 10 emit a laser spot that overlaps; and thereby calculates a deviation compensation matrix based on the actual coordinates and the therapy coordinates to compensate for the first transformation matrix, thereby improving the calibration accuracy of the six-axis therapy robot.

[0050] In some embodiments, the infrared image captured by the infrared camera 60 is used to adjust the robotic arm 40, including: controlling the end height of the robotic arm 40 to be fixed, and iteratively adjusting the robotic arm 40 based on multiple frames of infrared images so that the distance between the infrared indicator light 31 and the spot of the laser emitted by the laser ranging component 10 is less than a distance threshold.

[0051] In some embodiments, the calculation of the actual coordinates of the three-dimensional feature block 32 when the infrared indicator light 31 and the laser ranging component 10 emit laser spots overlap includes: obtaining the coordinates of the robotic arm 40 of the six-axis physiotherapy robot before the laser ranging component 10 and the infrared camera 60 are installed, and the coordinates of the robotic arm 40 of the six-axis physiotherapy robot after the laser ranging component 10 and the infrared camera 60 are installed, and confirming the translation vector of the robotic arm 40 of the six-axis physiotherapy robot; adding the coordinate value when the infrared indicator light 31 and the laser ranging component 10 emit laser spots overlap to the translation vector to obtain the actual coordinates.

[0052] In some embodiments, S300, the image captured by the infrared camera 60 is used to adjust the robotic arm 40 so that the infrared indicator light 31 and the laser spot emitted by the laser ranging component 10 overlap, including: S301, lighting each infrared indicator light 31 in turn, so that when the current infrared indicator light 31 is lit, the infrared camera 60 is controlled to capture the image of a single three-dimensional feature block 32, and the robotic arm 40 is adjusted so that the infrared indicator light 31 and the laser spot emitted by the laser ranging component 10 overlap; S302, after calculating the actual coordinates of the three-dimensional feature block 32 when the current infrared indicator light 31 and the laser spot emitted by the laser ranging component 10 overlap, the current infrared indicator light 31 is turned off to obtain the actual coordinates of the three-dimensional feature block 32 corresponding to each infrared indicator light 31.

[0053] In some embodiments, S301, lighting up each infrared indicator light 31 in turn, so that when the current infrared indicator light 31 is lit, the infrared camera 60 is controlled to collect an image of a single three-dimensional feature block 32, and the robotic arm 40 is adjusted so that the infrared indicator light 31 and the laser ranging component 10 emit a laser spot that overlap, including: S3011, using the PID algorithm to adjust the robotic arm 40, when the distance between the infrared indicator light 31 and the laser ranging component 10 emits a laser spot that 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 infrared indicator light 31 and the laser ranging component 10 emit a laser spot that overlap multiple times; S3012, the actual coordinates of the three-dimensional feature block 32 are calculated when the infrared indicator light 31 and the laser ranging component 10 emit a laser spot that overlap, including: S3013, calculating the actual coordinates of the three-dimensional feature block 32 when overlapping multiple times, and obtaining multiple groups of the actual coordinates.

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

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

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

[0057] It should be noted that in the above embodiments, the description of each embodiment has its own emphasis, and for parts that are not described in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0058] It will be appreciated by those skilled in the art that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Furthermore, the present invention may 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.

[0059] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded computer, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0060] These computer program instructions may also be stored in a computer readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture including an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0061] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process in the computer or other programmable device. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0062] Although the preferred embodiments of the present invention have been described, those skilled in the art may make other changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present invention.

[0063] Obviously, those skilled in the art can make various changes and modifications 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 equivalents, 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, 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; 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 method of sequentially lighting up each infrared indicator light, and controlling the infrared camera to collect an image of a single three-dimensional feature block when the current infrared indicator light is lit, and adjusting the mechanical arm so that the infrared indicator light and the laser ranging component emit laser spots overlap, comprises: The mechanical arm is adjusted using a PID algorithm, and when the distance between the infrared indicator light and the laser ranging component emitting laser spots is less than a preset threshold, the mechanical arm is controlled to move along a preset trajectory using a PD algorithm so that the infrared indicator light and the laser ranging component emitting laser spots overlap multiple times; 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 fitting of the deviation compensation matrix based on the multiple sets of theoretical coordinates and actual coordinates includes: 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, 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 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