Device and method for calibrating gesture precision of tail end of mechanical arm at main end of robotTechnical Field
The invention belongs to the technical field of robot mechanical arms, and particularly relates to a device and a method for calibrating the terminal gesture precision of a terminal mechanical arm of a main terminal mechanical arm of a laparoscopic robot.
Background
With the development of robotics, laparoscopic surgical robots are increasingly being used in clinic. The laparoscopic surgery robot mainly comprises a doctor control console and a patient surgery platform. The doctor control end is also called as the tail end of the robot main end mechanical arm, and an operator remotely controls the patient platform mechanical arm through the doctor control console to complete the operation. Thus, the accuracy of the movement of the main end will affect the accuracy of the movement of the patient table.
In order to ensure the operation precision, the doctor control console should have higher motion precision. The main factors affecting the accuracy of the mechanical arm are: machining errors, mechanical tolerances, assembly errors, speed reducer accuracy, joint flexibility, link flexibility, and the like. The actual precision and the theoretical precision of the mechanical arm at the main end have certain errors. The gesture precision calibration of the main end mechanical arm can well improve the motion precision of the main end mechanical arm. The master-slave control of the endoscope operation robot adopts a form of pose separation, the slave end position following motion adopts incremental control, and the pose following adopts master-slave pose absolute mapping, so that the precision of the master end pose is particularly important. The gesture movement precision of the tail end of the doctor console is improved, so that an operator can intuitively control the slave mechanical arm during operation, the operation precision is indirectly improved, and the operation risk is reduced.
Disclosure of Invention
Aiming at the defects in the prior art, the invention provides the device and the method for calibrating the terminal gesture precision of the mechanical arm at the main end of the robot, wherein the calibration device splits the terminal gesture motion of the mechanical arm at the main end of the robot into rotation motion around X, Y, Z three axes, and respectively calibrates and compensates the motion of each axis, thereby achieving the calibration and the error compensation of the terminal gesture precision.
The invention discloses a device for calibrating the gesture precision of the tail end of a robot main end mechanical arm, wherein the tail end of the robot main end mechanical arm comprises a pitching joint, a deflecting joint and a rolling joint; the calibration device comprises a bracket, a base, a main end rotating shaft and a rotatable cube, wherein one end of the bracket is fixed on the base, the other end of the bracket is connected with a first rotating surface of the rotatable cube, the rotatable cube is of a three-order magic cube structure, a hole matched with the main end rotating shaft is formed in one central shaft of the rotatable cube, the main end rotating shaft is inserted in the hole, and the main end rotating shaft is perpendicular to the first rotating surface; the axis of the yaw joint is a z axis, the axis of the pitch joint is a y axis, the axis of the roll joint is an x axis, the intersection points of the axes of the yaw joint, the pitch joint and the roll joint are at the tail end of the main end rotating shaft, and the tail end of the main end rotating shaft is at the center of the rotatable cube; the initial end of the main end rotating shaft is connected with the transverse rolling joint.
According to the device and the method for calibrating the gesture precision of the tail end of the mechanical arm at the main end of the robot, the rotatable cube is composed of the central shaft connected with six central blocks, eight corner blocks and twelve corner blocks, the second rotating surface and the first rotating surface of the rotatable cube are opposite surfaces, and the surfaces of the four corner blocks on the second rotating surface are respectively provided with a marker a, a marker b, a marker c and a marker d.
The invention discloses an error calibration method based on the robot main end mechanical arm tail end gesture precision calibration device, which is characterized by comprising the following steps of:
resetting the tail end of the robot main end mechanical arm, and placing a high-precision camera right in front of the tail end of the robot main end mechanical arm, wherein the second rotating surface of the rotatable cube is opposite to a camera of the high-precision camera;
calibrating an x-axis error: keeping motors of the pitching joint and the deflecting joint motionless, moving the rolling joint, recording the motor movement angle of the rolling joint and coordinate values of the corresponding marker a and the marker c, calculating to obtain an actual rotation angle of the x-axis, and calibrating x-axis error calibration based on the actual rotation angle and the theoretical rotation angle;
calibrating y-axis errors: keeping motors of the transverse rolling joint and the deflection joint motionless, moving the pitching joint, recording the motor movement angle of the pitching joint and coordinate values of the corresponding marker a and the marker c, calculating to obtain an actual rotation angle of a y axis, and calibrating y axis error calibration based on the actual rotation angle and the theoretical rotation angle;
calibrating a z-axis error: the motors of the roll joint and the pitch joint are kept motionless, the yaw joint is moved, the motor movement angle of the yaw joint and coordinate values of the corresponding marker a and the marker c are recorded, the actual rotation angle of the y axis is calculated, and the error calibration of the y axis is calibrated based on the actual rotation angle and the theoretical rotation angle.
According to the error calibration method of the robot main end mechanical arm tail end gesture precision calibration device, x-axis error calibration is performed: when the roll joint is moved, the roll joint is moved every 1 degree, the real-time pulse number of the motor is recorded every 1 degree, the position a (X, Y, Z) of the marker a and the position c (X, Y, Z) of the marker c are recorded every 1 degree, and the change of the linear slope of the two markers a and c in the Z0Y plane is utilized to obtain the actual rotation angle value;
calibrating y-axis errors: when the pitching joint moves, the pitching joint moves every 1 degree, the real-time pulse number of the motor is recorded every 1 degree, the position a (X, Y, Z) of the marker a and the position c (X, Y, Z) of the marker c are recorded every 1 degree, and the change of the linear slope of the two markers a and c in the Z0X plane is utilized to obtain the actual rotation angle value;
calibrating a z-axis error: when the deflection joint is moved, the deflection joint is moved every 1 degree, the real-time pulse number of the motor is recorded every 1 degree, the position b (X, Y, Z) of the marker a and the position d (X, Y, Z) of the marker c are recorded every 1 degree, and the change of the linear slope of the two markers b and d in the X0Y plane is utilized to obtain the actual rotation angle value;
the error calibration method of the robot main end mechanical arm tail end gesture precision calibration device further comprises the following steps:
correcting x-axis error: each time the x-axis error calibration is executed, the errors of an actual rotation angle and a theoretical rotation angle and the pulse number of the actual rotation angle are obtained, the pulse number is taken as a characteristic value, the theoretical rotation angle is taken as a target value, and training data are constructed; performing x-axis error calibration for multiple times to obtain multiple training data, forming a training data set, and training a linear regression model 'theoretical rotation angle=l×actual pulse number of motor+n' by using the training data set to obtain optimal correction coefficients L and N, and correcting the pulse number corresponding to the theoretical angle;
y-axis error correction: each time the y-axis error calibration is executed, the errors of an actual rotation angle and a theoretical rotation angle and the pulse number of the theoretical rotation angle are obtained, the pulse number is taken as a characteristic value, the theoretical rotation angle is taken as a target value, and training data are constructed; performing y-axis error calibration for multiple times to obtain multiple training data, forming a training data set, and training a linear regression model 'theoretical rotation angle=l×actual pulse number of motor+n' by using the training data set to obtain optimal correction coefficients L and N, and correcting the pulse number corresponding to the theoretical angle;
correcting z-axis error: each time the z-axis error calibration is executed, the errors of the actual rotation angle and the theoretical rotation angle and the pulse number of the theoretical rotation angle are obtained, the pulse number is taken as a characteristic value, the theoretical rotation angle is taken as a target value, and training data are constructed; and performing z-axis error calibration for multiple times to obtain multiple training data, forming a training data set, and training a linear regression model 'theoretical rotation angle=l×actual pulse number of motor+n' by using the training data set to obtain optimal correction coefficients L and N, and correcting the pulse number corresponding to the theoretical angle.
Compared with the prior art, the invention has the following advantages: according to the invention, after the rotation angles of the pitching joint, the deflecting joint and the rolling joint which determine the tail end gesture of the mechanical arm at the main end of the robot are calibrated and corrected, the gesture movement precision of the tail end of the operation arm of the doctor console is improved, the master-slave control precision of the laparoscopic surgery robot is ensured, and the precision of the laparoscopic surgery is indirectly improved.
The technical scheme of the invention is further described in detail through the drawings and the embodiments.
Drawings
FIG. 1 is a block diagram of the whole end of a robot main end mechanical arm and a calibration device in an embodiment of the invention.
FIG. 2 is a diagram showing the whole use of the end of the robot arm and the calibration device according to the embodiment of the invention.
FIG. 3 is a schematic diagram illustrating the assembly of the marking device and the main end terminal according to an embodiment of the present invention.
FIG. 4 is a schematic diagram of the accuracy of the marked X-axis rotation in an embodiment of the present invention.
FIG. 5 is a schematic diagram of the accuracy of the marked Y-axis rotation in an embodiment of the present invention.
FIG. 6 is a schematic diagram of the accuracy of the Z-axis rotation in an embodiment of the present invention.
FIG. 7 is a flowchart of a calibration error compensation algorithm according to an embodiment of the present invention.
FIG. 8 is a graph showing a fit of experimental data in an embodiment of the present invention.
Reference numerals illustrate:
1-a left-hand mechanical arm; 2-a camera; 3-a bracket;
4, a base; 5-pitch joint; 6-deflection joint;
7-rolling joints; 8-rotating the cube; 9-a label a;
10-a marker b; 11-a marker c; 12-a marker d;
13-main end spin-twisting shaft; 14-first corner block; 15-second corner block;
16-center block.
Detailed Description
As shown in fig. 1-3, the device for calibrating the gesture precision of the tail end of the mechanical arm at the main end of the robot consists of a bracket 3, a base 4 and a rotatable cube 8. The rotatable cube 8 is similar to a three-stage cube, and is composed of a central shaft connected with six central blocks, eight corner blocks and twelve edge blocks, and when the central shaft and the eight corner blocks are connected together, the rotatable cube 8 forms a whole, and any one surface can horizontally rotate without affecting other cubes.
The bracket 3 is connected with the outer sides of the first corner block 14 and the second corner block 15 of the rotatable cube 8. The bracket 3 is fixed on the base 4, and the base 4 is horizontally placed on the ground.
The center block 16 is provided with a hole matched with the main end rotating shaft 13, and the main end rotating shaft 13 can be inserted into the center shaft of the rotating cube 8 through the hole on the center block 16 to fix the tail end of the main end mechanical arm with the calibration device.
The main end mechanical arm is provided with a left hand mechanical arm and a right hand mechanical arm, only the left hand mechanical arm 1 is used for example, the axes of a pitching joint 5, a deflecting joint 6 and a rolling joint 7 of the left hand mechanical arm 1 are intersected at the tail end of a main end twisting shaft 13 and always move around the tail end of the main end twisting shaft 13, and the movement posture of the main end is determined by the three-axis movement. The end of the main end spin axis 13 coincides with the centre of the rotating cube 8.
The front surface of the rotating cube 8 is embedded with 4 targeting markers, namely a marker a9, a marker b10, a marker c11 and a marker d12. The camera 2 is a high-precision tracker, the distance testing precision is 0.01mm, the angle testing precision is 0.001 degrees, and the high-precision tracker is placed right in front of the left hand mechanical arm 1 and used for acquiring real-time coordinates of the marker.
When the gesture precision of the tail end of the main end mechanical arm is tested, joints of the left hand mechanical arm 1 except for a pitching joint 5, a deflecting joint 6 and a rolling joint 7 are fixed, gesture movement of the tail end of the main end mechanical arm is decomposed into rotary movement around an X axis, a Y axis and a Z axis, and the movement precision around each axis is calibrated respectively. The pitch joint 5, yaw joint 6, roll joint 7 encoders have 16-bit encoders with a rotational accuracy of 0.005 °. When the rotation precision around the X axis is calibrated, the rotating cube 8 is reset, squares of all the surfaces are overlapped, motors of the pitching joint 5 and the deflecting joint 6 are enabled to keep motionless, the rolling joint 7 is moved, the movement angle of the motors and coordinate values of the corresponding markers I9 and III 11 are recorded, and the actual rotation angle of the X axis is calculated. When the rotation precision around the Y axis is calibrated, the rotating cube 8 is reset, the motors of the yaw joint 6 and the roll joint 7 are enabled to keep motionless, the pitch joint 5 is moved, the motor movement angle and the coordinate values corresponding to the first marker 9 and the third marker 11 are recorded, and the actual rotation angle of the Y axis is calculated. When the rotation precision around the Z axis is calibrated, the rotating cube 8 is reset, the motors of the pitching joint 5 and the rolling joint 7 are enabled to keep motionless, the yaw joint 6 is moved, the motor movement angle and the coordinate values corresponding to the marker II 10 and the marker IV 12 are recorded, and the actual rotation angle of the Z axis is calculated.
Further, analyzing the measured value, taking the X-axis measured value as an example, testing an actual movement angle every 1 degree of movement of the roll joint 7, recording the number of motor pulses corresponding to the actual angle, fitting the obtained number of motor pulses and the corresponding theoretical angle by using a linear regression to correct a coefficient, correcting an actual pulse count value corresponding to the error theoretical angle, and re-testing the X-axis rotation angle by using the corrected pulse count value, wherein the steps are repeated, the angle error after fitting is obtained by measurement, and when the root mean square error of the angle error is smaller than a threshold value, the accuracy is considered to be in accordance with the requirement. The error measurement and compensation steps for the Y-axis and Z-axis are similar.
It can be seen that after the rotation angles of the pitch joint 5, the yaw joint 6 and the roll joint 7 which determine the terminal gesture are calibrated and corrected, the gesture movement precision of the terminal of the operation arm of the doctor console is improved, the master-slave control precision of the laparoscopic surgery robot is ensured, and the precision of the laparoscopic surgery is indirectly improved.
Specifically, the error calibration method based on the robot main end mechanical arm tail end gesture precision calibration device comprises the following steps:
step one, the left hand mechanical arm 1 is locked by the other joints except the three joints at the tail end, and the main end rotating shaft 13 is inserted into the central shaft of the rotating cube 8 through a hole on the central block 16, so that the fixture and the main end are fixed. The camera 2 is placed at the front two meters of one side of the cube 8 with the marking surface, and the position schematic diagram is shown in the whole structure diagram of the tail end of the main end mechanical arm of the robot and the calibration device in fig. 1.
Step two, calibrating the rotation precision around the X axis, referring to FIG. 4, in the embodiment of the invention, the rotation precision of the X axis is calibrated. So as to bend downThe motors of the upward joint 5 and the deflection joint 6 are kept still. The roll joint 7 is moved every 1 ° (or 5 ° (the specific degree may be selected according to actual practice), from-90 ° to 90 °, the number of pulses in real time is recorded every 1 ° of the motor, and the position a (X) of the marker a9 is recorded every 1 °9 ,Y9 ,Z9 ) Position c (X) of marker three 1111 ,Y11 ,Z11 ) The angle value actually walked is obtained by utilizing the change of the slope of the straight line formed by the points a and c in the Z0Y plane.
And thirdly, calibrating the rotation precision around the Y axis, wherein the rotation precision of the Y axis is shown schematically in the embodiment of the invention in FIG. 5, so that the motors of the deflection joint 6 and the roll joint 7 are kept motionless. Every 1 ° (or 5 ° (the specific degree may be selected according to actual practice)) the pitch joint 5 is moved from-45 ° to 45 °, the number of pulses in real time is recorded every 1 ° of the motor, and the position a (X) of the marker a9 is recorded every 1 °9 ,Y9 ,Z9 ) Position c (X) of marker c1111 ,Y11 ,Z11 ) The angle value actually walked is obtained by utilizing the change of the slope of a straight line formed by the two points a and c in the Z0X plane.
And step four, calibrating the rotation precision around the Z axis, referring to a schematic diagram of the rotation precision of the Z axis in the embodiment of the invention shown in fig. 6, so that the motors of the pitching joint 5 and the rolling joint 7 are kept motionless. The yaw joint 6 is moved every 1 ° (or 5 ° (the specific degree may be selected according to actual practice), from-45 ° to 45 °, the number of pulses in real time is recorded every 1 ° of the motor, and the position b (X) of the marker b10 is recorded every 1 °10 ,Y10 ,Z10 ) Position d (X) of marker d1212 ,Y12 ,Z12 ) And obtaining an actual walking angle value by utilizing the change of the slope of the straight line formed by the two points b and d in the X0Y plane.
And fifthly, compensating the precision error of the X axis, and setting the root mean square error threshold value to be 0.1. And (3) fitting the correction coefficient between the obtained theoretical angle and the corresponding actual pulse number through linear regression, and the actual pulse number +N of the motor with the theoretical angle=L, retesting the rotation precision of the X axis by using the corrected pulse value, repeating the step (II), measuring to obtain the error between the fitted actual angle and the theoretical angle, and considering the precision to meet the requirement when the root mean square error of the angle error is smaller than a threshold value.
Step six, the precision error compensation for the Y axis and the Z axis is similar to step five, and will not be described again here, and the error compensation algorithm flow chart is shown in the error compensation algorithm flow chart marked in the embodiment of the invention in fig. 7.
In summary, the calibration device designed by the invention divides the gesture of the tail end of the mechanical arm of the doctor console into rotary motions around the X axis, the Y axis and the Z axis by utilizing the characteristic that the rotation center of the device coincides with the gesture motion point at the tail end, respectively calibrates the rotary motion precision of the three axes, and compensates and corrects errors, thereby achieving the purposes of calibrating and correcting the gesture of the main end. The invention improves the gesture movement precision and master-slave movement control precision of the master end, and helps an operator to intuitively control the slave end mechanical arm during operation, thereby improving the precision of endoscopic surgery and reducing the risk of surgery.
Taking the X axis as an example, the precision error of the X axis is compensated, the data are collected at intervals of 5 degrees, and the collected data are shown in the following table:
TABLE 1
At this time, the angle root mean square error α= 0.0791, the motor pulse numbers corresponding to each theoretical angle and the actual angle are woven into a training data set, when the linear regression model is applied, the pulse numbers corresponding to the actual angle are used as independent variables (characteristics), the theoretical angle (target value) is used as dependent variable to train, the best primary linear correction coefficient L, N is obtained by fitting through the least square method, l=182.0629, n= 8.8070 and theoretical angle= 182.0629 are obtained after fitting, the actual pulse number of the motor is +8.8070, a fitted primary curve is shown in an experimental data fitting chart of fig. 8, the motor pulse numbers corresponding to the theoretical angle after fitting are used for re-measurement, and the angle error after re-measurement is shown in the table above. The fitted angle root mean square error phi=0.0551.
It can be seen that phi <0.1 after fitting meets the requirement, and phi < alpha, the rotation precision after fitting is improved, and the compensation algorithm flow is shown in the marked error compensation algorithm flow chart in the embodiment of the invention in fig. 7.
The foregoing description is only a preferred embodiment of the present invention, and is not intended to limit the present invention, and any simple modification, variation and equivalent structural changes made to the above embodiment according to the technical substance of the present invention still fall within the scope of the technical solution of the present invention.