Disclosure of Invention
The invention provides a multilayer control system of a full-automatic venipuncture robot, aiming at the problem that the existing full-automatic venipuncture robot cannot realize autonomous decision making of actions in the puncturing process and further causes discomfort of a patient.
The invention discloses a full-automatic venipuncture robot multilayer control system, which comprises:
acquiring binocular camera images of the arm to be punctured by adopting two near-infrared cameras based on a parallax principle;
the upper computer performs three-dimensional correction and three-dimensional matching on the images of the binocular cameras to obtain the blood vessel depth of the arm to be punctured; mapping based on the blood vessel depth to obtain a blood vessel point cloud picture; extracting a blood vessel central line from the blood vessel point cloud picture, and further determining a final puncture decision;
the multi-axis motion control card obtains a track planning result according to the final puncture decision, distributes the motion amount to each axis of the multi-axis motion of the tail end, and sends a motion instruction to a driver corresponding to each axis, so that the drivers drive corresponding direct current servo motors to drive the tail end executor of the robot to act together;
in the action process of the end effector, a puncturing force signal of the end effector is collected through a multi-dimensional force sensor and is transmitted to an upper computer, and the upper computer adjusts a puncturing decision on the basis of the existing final puncturing decision; and simultaneously, feeding the current position of each direct current servo motor back to the multi-axis motion control card through an installed encoder, and adjusting the track planning result in real time by combining the adjusted puncture decision and the current position of the motor through the multi-axis motion control card until the puncture is finished.
According to the multilayer control system of the full-automatic venipuncture robot, the distance between the two near-infrared cameras is 58mm, and the two near-infrared cameras are symmetrically arranged at the position 300mm above an arm to be punctured.
According to the multi-layer control system of the full-automatic venipuncture robot of the present invention, the determining the final puncture decision comprises:
decision points are equidistantly arranged on a blood vessel central line, and are evaluated according to the diameter, the angle, the depth and the straightness of the blood vessel at each decision point to obtain the optimal puncture point, the puncture angle and the puncture depth, so that the final puncture decision is formed.
According to the full-automatic venipuncture robot multilayer control system of the invention,
the method for obtaining the trajectory planning result comprises the following steps: and (3) planning a track in a Cartesian space, obtaining the shortest path of puncture time by using a genetic algorithm, and then obtaining the motion amount of each axis through inverse kinematics calculation.
According to the full-automatic venipuncture robot multilayer control system of the invention,
the method for adjusting the puncture decision by the upper computer on the basis of the existing final puncture decision comprises the following steps:
the upper computer adjusts the puncture decision according to the real-time collected puncture force signal of the end effector; the position and the posture of the needle point of the end effector can be dynamically adjusted under the control of the impedance controller of the multi-axis motion control card, so that the minimum puncture force is achieved.
According to the full-automatic venipuncture robot multilayer control system of the invention,
when the puncture force signal of the end effector acquired by the upper computer in real time changes suddenly, the needle point is determined to puncture the upper wall of the blood vessel, and at the moment, the final puncture decision is adjusted to execute the needle picking action, so that the needle point reduces the elevation angle to form an included angle of 10 degrees with the skin on the surface of the arm under the control of the multi-axis motion control card, and then the needle point is horizontally pushed into the blood vessel.
The invention has the beneficial effects that: in the invention, the near infrared light is selected for vein development, so that the advantages of clear image and high imaging speed are achieved, the imaging of the vein is less influenced by hair and skin color, and therefore, two near infrared enhanced cameras are selected for vein three-dimensional information perception. In vein puncture, puncture force can reflect puncture state and patient's painful degree, therefore use the multidimensional force transducer to detect puncture force and realize puncture state perception and puncture strategy adjustment.
The body control layer is used for controlling the robot body to execute a puncture decision, the upper computer PC sends the puncture decision to the lower computer multi-axis motion control card, the multi-axis motion control card performs track planning, and the driver controls the motor to drive the end effector to complete a puncture action. In the motion process, the encoder is arranged on the output shaft of the motor and used for feeding back the position of the motor, so that the motion control card can conveniently carry out track planning.
The invention adopts the upper computer to carry out image processing, three-dimensional reconstruction and autonomous decision making, and has high operation speed; the body control of the bottom layer robot mainly comprises the steps of planning a track, controlling the robot body to execute a puncture action, and finishing the process in a multi-axis motion control card; the layered control of the tail end of the robot ensures that the two control layers are different in labor division and do not influence each other, and the overall working efficiency of the control system can be improved.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
It should be noted that the embodiments and features of the embodiments may be combined with each other without conflict.
The invention is further described with reference to the following drawings and specific examples, which are not intended to be limiting.
In a first embodiment, referring to fig. 1, the present invention provides a multi-layer control system for a full-automatic venipuncture robot, including:
acquiring binocular camera images of the arm to be punctured by adopting two near-infrared cameras based on a parallax principle;
the upper computer performs three-dimensional correction and three-dimensional matching on the images of the binocular cameras to obtain the blood vessel depth of the arm to be punctured; mapping based on the blood vessel depth to obtain a blood vessel point cloud picture; extracting a blood vessel central line from the blood vessel point cloud picture, and further determining a final puncture decision;
the multi-axis motion control card obtains a track planning result according to the final puncture decision, distributes the motion amount to each axis of the multi-axis motion of the tail end, and sends a motion instruction to a driver corresponding to each axis to enable the driver to drive a corresponding direct current servo motor, so that the tail end executor of the robot is driven to act together, and the puncture action is completed according to the planned path;
in the action process of the end effector, a puncturing force signal of the end effector is collected through a multi-dimensional force sensor and is transmitted to an upper computer, and the upper computer adjusts a puncturing decision on the basis of the existing final puncturing decision; and simultaneously, feeding the current position of each direct current servo motor back to the multi-axis motion control card through an installed encoder, and adjusting the track planning result in real time by combining the adjusted puncture decision and the current position of the motor through the multi-axis motion control card until the puncture is finished.
The implementation mode can realize multilayer control of the full-automatic venipuncture robot, divides the overall control into a sensing and decision-making layer, relates to two near-infrared cameras, an upper computer and a multi-dimensional force sensor, and can realize sensing and three-dimensional reconstruction of the vein position, autonomous puncture decision-making and puncture force sensing as a top control layer; the control layer relates to a multi-axis motion control card, a driver, a direct current servo motor and an encoder and serves as a bottom control layer; an executive layer relates to a robot end effector. And the implementation of hierarchical control can effectively improve the control efficiency and is beneficial to later development, debugging and maintenance.
Before the two near-infrared cameras are used, calibration is firstly carried out to obtain internal and external parameters; then respectively collecting the images of the arm to be punctured. The camera comprises a near-infrared enhanced camera, and the used light source is a near-infrared light source. The upper computer is used for image processing, autonomous decision making and man-machine interaction, can be used for displaying vein three-dimensional position information and parameter adjustment in real time, and performs puncture decision making according to the vein three-dimensional information; the puncture state, the puncture process and the position of the end effector can be displayed in real time; in addition, the man-machine interaction interface can also adjust image processing parameters, motion parameters and the like. The motion parameters in the body control layer can be adjusted in the interface, and the motion state of each axis is displayed in the interface. The multi-dimensional force sensor is used for sensing the change condition of the puncture force in the puncture process, and is used for sensing the puncture state and adjusting the puncture strategy. The encoder is used for sensing and feeding back the position of the direct current servo motor.
As shown in fig. 1, the communication mode between the sensing and decision layer and the body control layer can be bus communication, and an EtherCAT bus is adopted for communication, so that the communication efficiency is high, the expansibility is strong, and the function of the whole system in the later period can be expanded conveniently. Software development of the perception and decision layer and the body control layer can use C + + language in a unified mode, a human-computer interaction interface is compiled by QT, display of vein three-dimensional information and puncture states and adjustment of motion parameters are achieved, and compatibility and transportability of the system can be enhanced.
The image information collected by the near-infrared camera can be transmitted to the upper computer through the USB.
By way of example, the two near-infrared cameras are 58mm apart and symmetrically arranged 300mm above the arm to be punctured.
The vein three-dimensional reconstruction is carried out by adopting binocular vision, the near infrared light has obvious advantages in the field of vein development, so that the near infrared enhanced camera and the near infrared light source are selected to realize the three-dimensional reconstruction of the vein, and in the stereo matching algorithm, the SGBM algorithm is used, so that the effect is better and the speed is higher; designing an autonomous decision algorithm, and realizing autonomous decision according to vein three-dimensional information, wherein the algorithm is a technical key for displaying full-automatic venipuncture; the multi-dimensional force sensor is used for sensing the change condition of the puncture force, and the puncture state and the pain degree of a patient can be sensed.
Further, the determining a final puncture decision comprises:
decision points are equidistantly arranged on a blood vessel central line, and are evaluated according to the diameter, the angle, the depth and the straightness of the blood vessel at each decision point to obtain the optimal puncture point, the puncture angle and the puncture depth, so that the final puncture decision is formed.
Still further, the method for obtaining the trajectory planning result comprises the following steps: and (3) planning a track in a Cartesian space, obtaining the shortest path of puncture time by using a genetic algorithm, and then obtaining the motion amount of each axis through inverse kinematics calculation.
Still further, the method for adjusting the puncture decision by the upper computer on the basis of the existing final puncture decision comprises the following steps:
the upper computer adjusts the puncture decision according to the real-time collected puncture force signal of the end effector; the position and the posture of the needle point of the end effector can be dynamically adjusted under the control of the impedance controller of the multi-axis motion control card, so that the minimum puncture force is achieved in the puncture process, and the pain of a patient is relieved to the maximum extent.
And further, when the puncture force signal of the end effector acquired by the upper computer in real time changes suddenly, the needle point is determined to puncture the upper wall of the blood vessel, and at the moment, the final puncture decision is adjusted to execute the needle picking action, so that the needle point reduces the elevation angle to form an included angle of 10 degrees with the skin on the surface of the arm under the control of the multi-axis motion control card, and then is horizontally pushed into the blood vessel.
In the body control layer, the designed puncture flow completely simulates the venous puncture flow of nurses, when the blood vessel wall is punctured, the pick needle action is executed, the elevation angle of the needle is reduced, and the needle is horizontally pushed into the blood vessel.
As an example, the specific implementation of the present embodiment may include six drivers, six dc servo motors, and six encoders, thereby realizing the control of the degree of freedom of the full-automatic venipuncture robot 6.
In conclusion, the invention automatically identifies the vein of the elbow of the hand through computer vision, then carries out puncture pose decision, finally controls the mechanical arm to insert the needle into the vein, and realizes the automatic blood sampling function. The invention designs a multilayer control system on the basis of the existing mechanical body of the venipuncture robot to realize vein recognition and automatic puncture.
Although the invention herein has been described with reference to particular embodiments, it is to be understood that these embodiments are merely illustrative of the principles and applications of the present invention. It is therefore to be understood that numerous modifications may be made to the illustrative embodiments and that other arrangements may be devised without departing from the spirit and scope of the present invention as defined by the appended claims. It should be understood that features described in different dependent claims and herein may be combined in ways different from those described in the original claims. It is also to be understood that features described in connection with individual embodiments may be used in other described embodiments.