1. A robot trachea cannula acting force-displacement-vision hybrid control method is characterized in that firstly, a cannula standard path and an oral cavity mechanical model are utilized, corresponding points in the standard path are obtained through visual image mapping, theoretical and actual displacement and force information is read according to a robot device, the standard path and the mechanical model, safety of a region where the robot is located is judged by using a virtual clamp method, and the motion speed of a mechanical arm is regulated and controlled by using parallel PID regulation and control according to safety zones.

2. The force-displacement-vision hybrid control method for the robotic endotracheal tube according to claim 1, characterized in that the specific process comprises the following steps:

(1) establishing a mapping relation of acting force-displacement-vision in the oral cavity according to historical record data of the tracheal cannula of the robot;

(2) inputting the acting force-displacement-vision mapping relation in the step (1) and a set standard path into the robot;

(3) in the tracheal intubation process of the robot, an image of the interior of the oral cavity is collected, image features are extracted, and according to the mapping relation of acting force-displacement-vision in the step (1), the position and posture vector x of the tail end of the mechanical arm in the standard path corresponding to the image features is obtained₀And force and moment vectors f at the end of the arm₀；

(4) Reading the current position and attitude vector x of the end of the mechanical arm₁And the current force and moment vectors f of the end of the arm₁；

(5) And (3) judging the state of the robot trachea cannula corresponding to the data in the step (4) by adopting a virtual clamp method, wherein the method comprises the following steps:

(5-1) calculating the current position and attitude vector x of the tail end of the mechanical arm according to the step (3) and the step (4)₁Position and attitude vector x from the end of the arm in the standard path₀The difference of (a): x ═ x₁-x₀Current force and moment vector f at the end of the arm₁Force and moment vectors f to the end of the arm in the standard path₀The difference of (a): f ═ f₁-f₀；

(5-2) calculating and judging the objective function y as x according to the two differences in the step (5-1)^TAx+f^TBf, wherein the superscript T matrix is transposed, and A, B is a coefficient matrix obtained through fitting;

(5-3) setting a judgment threshold y for the state of the step (4)₁And y₂：

If y < y₁If the robot trachea cannula is in the safe area, the step (6) is carried out, and if y is₁≤y≤y₂If the robot trachea cannula is in the buffer area, the step (6) is carried out, and if y is>y₂If the robot trachea cannula is in the dangerous area, the robot is withdrawn;

(6) combining x and f in the step (5) into an offset vector P ═ x, f, and performing force-displacement parallel control on the robot as follows: the method comprises the steps of utilizing a linear mapping method to linearly map three values of an offset vector P, an accumulated value I of the offset vector P along with time and a difference D of the offset vector P into a speed-angular speed signal V for controlling the tracheal intubation of the robot, outputting the signal V to the robot if the tracheal intubation of the robot is in a safe area, and outputting a signal alpha V if the tracheal intubation of the robot is in a buffer area, wherein alpha is a preset parameter, and alpha is more than 0 and less than 1, so that the acting force-displacement-vision hybrid control of the tracheal intubation of the robot is realized.

Background

The traditional operation of inserting the laryngoscope mainly depends on the grasp and hand feeling of a doctor on the oral cavity and the respiratory tract due to the lack of a real-time oral cavity image, the intubation action is completed by experience, the time consumption is long, certain danger exists, and for an operating doctor treating respiratory infectious diseases, the close-range operation is in particular in the face of the risk of cross infection. And the robot automation technologies such as image processing, force feedback control and the like can accurately grasp the hand feeling by integrating force, displacement and visual information to protect doctors and patients. Therefore, it is desirable to establish a method for controlling a robotic endotracheal intubation.

Most of the current robot control methods rely on visual, mechanical or operator remote control for navigation and feedback. There are also few existing patents that have both visual navigation and force feedback. For example, in the patent "robot control device, robot system, robot and robot control method", the robot movement is controlled by simultaneously acquiring images and detecting forces, but the acquired visual images and forces are not directly mixed and used, and the mechanical information is more used as safe detection rather than actively guiding the movement with more force.

Disclosure of Invention

The invention aims to provide a force-displacement-vision hybrid control method for a robot tracheal cannula, which integrates force-displacement and vision information, obtains the internal body of an oral cavity through a camera, inputs an image into a computer to read the force and displacement information, and guides a trachea to be efficiently inserted into a specified part through two control algorithms of a virtual clamp and a parallel PID.

The invention provides an acting force-displacement-vision hybrid control method of a robot tracheal cannula, which comprises the steps of firstly utilizing a cannula standard path and an oral cavity mechanical model, obtaining a corresponding point in the standard path through visual image mapping, reading theoretical and actual displacement and force information according to a robot device, the standard path and the mechanical model respectively, judging the safety of a region where the robot device is located by using a virtual clamp method, and regulating and controlling the movement speed of a mechanical arm by using parallel PID (proportion integration differentiation) regulation and control according to a safety partition.

The acting force-displacement-vision hybrid control method for the trachea cannula of the robot comprises the following steps:

(1) establishing a mapping relation of acting force-displacement-vision in the oral cavity according to historical record data of the tracheal cannula of the robot;

(2) inputting the acting force-displacement-vision mapping relation in the step (1) and a set standard path into the robot;

(3) in the tracheal intubation process of the robot, an image of the interior of the oral cavity is collected, image features are extracted, and according to the mapping relation of acting force-displacement-vision in the step (1), the position and posture vector x of the tail end of the mechanical arm in the standard path corresponding to the image features is obtained₀And force and moment vectors f at the end of the arm₀；

(4) Reading the current position and attitude vector x of the end of the mechanical arm₁And the current force and moment vectors f of the end of the arm₁；

(5) And (3) judging the state of the robot trachea cannula corresponding to the data in the step (4) by adopting a virtual clamp method, wherein the method comprises the following steps:

(5-1) calculating the current position and attitude vector x of the tail end of the mechanical arm according to the step (3) and the step (4)₁Position and attitude vector x from the end of the arm in the standard path₀The difference of (a): x ═ x₁-x₀Current force and moment vector f at the end of the arm₁Force and moment vectors f to the end of the arm in the standard path₀The difference of (a): f ═ f₁-f₀；

(5-2) calculating and judging the objective function y as x according to the two differences in the step (5-1)^TAx+f^TBf, wherein the superscript T matrix is transposed, and A, B is a coefficient matrix obtained through fitting;

(5-3) setting a judgment threshold y for the state of the step (4)₁And y₂：

If y < y₁If the robot trachea cannula is in the safe area, the step (6) is carried out, and if y is₁≤y≤y₂If the robot trachea cannula is in the buffer area, the step (6) is carried out, and if y is>y₂If the robot trachea cannula is in the dangerous area, the robot is withdrawn;

(6) combining x and f in the step (5) into an offset vector P ═ x, f, and performing force-displacement parallel control on the robot as follows: the method comprises the steps of utilizing a linear mapping method to linearly map three values of an offset vector P, an accumulated value I of the offset vector P along with time and a difference D of the offset vector P into a speed-angular speed signal V for controlling the tracheal intubation of the robot, outputting the signal V to the robot if the tracheal intubation of the robot is in a safe area, and outputting a signal alpha V if the tracheal intubation of the robot is in a buffer area, wherein alpha is a preset parameter, and alpha is more than 0 and less than 1, so that the acting force-displacement-vision hybrid control of the tracheal intubation of the robot is realized.

The invention provides a force-displacement-vision hybrid control method for a robot trachea cannula, which has the following advantages:

the acting force-displacement-vision hybrid control method of the robot tracheal intubation combines the force, displacement and vision information in the tracheal intubation, realizes the safe and efficient insertion of a laryngoscope and a catheter through a virtual clamp, a parallel PID control method and a threshold control method, ensures the accuracy of the intubation posture, and provides a solid foundation for the robot automatic tracheal intubation.

Drawings

Fig. 1 is a flow chart of a force-displacement-vision hybrid control method for a robotic endotracheal tube according to the present invention.

Detailed Description

The invention provides an acting force-displacement-vision hybrid control method of a robot tracheal cannula, which comprises the steps of firstly utilizing a cannula standard path and an oral cavity mechanical model, obtaining a corresponding point in the standard path through visual image mapping, reading theoretical and actual displacement and force information according to a robot device, the standard path and the mechanical model respectively, judging the safety of a region where the robot device is located by using a virtual clamp method, and regulating and controlling the movement speed of a mechanical arm by using parallel PID (proportion integration differentiation) regulation and control according to a safety partition.

The flow chart of the acting force-displacement-vision hybrid control method for the tracheal intubation of the robot is shown in fig. 1, and the method specifically comprises the following steps:

(1) establishing a mapping relation of acting force-displacement-vision in the oral cavity according to historical record data of the tracheal cannula of the robot;

(2) inputting the acting force-displacement-vision mapping relation in the step (1) and a set standard path into the robot;

(3) in the tracheal intubation process of the robot, a mechanical arm camera is used for collecting an image in the oral cavity, image characteristics are extracted, and according to the mapping relation of acting force-displacement-vision in the step (1), the position and posture vector x of the tail end of the mechanical arm in the standard path corresponding to the image characteristics is obtained₀And force and moment vectors f at the end of the arm₀；

(4) Reading the current position and attitude vector x of the end of the robot arm from the robot and force sensor₁And the current force and moment vectors f of the end of the arm₁；

(5) And (3) judging the state of the robot trachea cannula corresponding to the data in the step (4) by adopting a virtual clamp method, wherein the method comprises the following steps:

(5-1) calculating the current position and attitude vector x of the tail end of the mechanical arm according to the step (3) and the step (4)₁Position and attitude vector x from the end of the arm in the standard path₀The difference of (a): x ═ x₁-x₀Current force and moment vector f at the end of the arm₁Force and moment vectors f to the end of the arm in the standard path₀The difference of (a): f ═ f₁-f₀；

(5-2) calculating and judging the objective function y as x according to the two differences in the step (5-1)^TAx+f^TBf, wherein the superscript T matrix is transposed, and A, B is a coefficient matrix obtained through fitting;

(5-3) setting judgment on the state in the step (4)Off threshold y₁And y₂：

The state of the robot tracheal cannula comprises a safety zone, a buffer zone or a danger zone. If y < y₁If the robot trachea cannula is in the safe area, the step (6) is carried out, and if y is₁≤y≤y₂If the robot trachea cannula is in the buffer area, the step (6) is carried out, and if y is>y₂If the robot trachea cannula is in the dangerous area, the robot is withdrawn;

(6) combining x and f in the step (5) into an offset vector P ═ x, f, and performing force-displacement parallel (PID for short) control on the robot as follows: the method comprises the steps of utilizing a linear mapping method to linearly map three values of an offset vector P, an accumulated value I of the offset vector P along with time and a difference D of the offset vector P into a speed-angular speed signal V for controlling the tracheal intubation of the robot, outputting the signal V to the robot if the tracheal intubation of the robot is in a safe area, and outputting a signal alpha V if the tracheal intubation of the robot is in a buffer area, wherein alpha is a preset parameter and is more than 0 and less than 1, and in one embodiment of the invention, the value of alpha is 0.5, so that the acting force-displacement-vision hybrid control of the tracheal intubation of the robot is realized.