CN115192092B - Robotic autonomous biopsy sampling method for flexible dynamic environment in vivo - Google Patents

Abstract

The invention provides an in-vivo flexible dynamic environment-oriented robot autonomous biopsy sampling method, a system, a storage medium and electronic equipment, and relates to the technical field of biopsy histological diagnosis. In the invention, the three-dimensional positions of a focus point and a collision avoidance area selected by a doctor on a three-dimensional image and the three-dimensional positions of the tail ends of the biopsy forceps with multiple degrees of freedom are respectively converted into a robot base coordinate system; a multi-target movement fusion control method is adopted to control the tail end of the biopsy forceps to move from an initial position to a focus point; after reaching the vicinity of the focus point, acquiring the contact force between the tail end of the biopsy forceps and the tissue according to a preset autonomous sampling control system, and completing the clamping sampling operation; by adopting the multi-target motion fusion control method, the tail end of the multi-degree-of-freedom biopsy forceps is controlled to return to the initial position from the sampling end position. The three-dimensional dynamic tracking of the sampling point marked by the doctor can be realized, the autonomy of the sampling process can be realized, the sampling time is shortened, and the quality and the efficiency of sampling are improved.

Description

Robotic autonomous biopsy sampling method for flexible dynamic environment in vivo

Technical Field

The present invention relates to the technical field of biopsy histological diagnosis, and in particular to a robot autonomous biopsy sampling method, system, storage medium and electronic equipment for a flexible dynamic environment in vivo.

Background Art

Biopsy histological diagnosis plays an important role in clinical disease diagnosis and lesion assessment. It can timely detect local organ lesions and enable patients to receive early treatment. At present, the main method of clinical biopsy is to insert the sampler with the laparoscope into the designated position of the patient's body to clamp the lesion cells in the area.

At present, traditional biopsy sampling is done by surgeons who determine the depth information of a fixed position at a specific moment based on preoperative images or intraoperative laparoscopic images. However, during the entire operation, the sampling area will change dynamically over time, and repeated positioning and calibration will not only affect the efficiency of biopsy sampling but also the accuracy of sampling. In addition, the operation time and accuracy of biopsy sampling depend on the surgeon's experience and operation level. Repeated sampling due to operational errors and missed diagnosis due to sampling deviations are irreversible harm to patients.

In view of this, it is necessary to provide a robotic autonomous biopsy sampling solution that can ensure sampling quality and efficiency.

Summary of the invention

1. Technical issues to be solved

In view of the deficiencies in the prior art, the present invention provides a robotic autonomous biopsy sampling method, system, storage medium and electronic device for a flexible dynamic environment in the body, which solves the technical problem of being unable to guarantee sampling quality and efficiency.

(II) Technical solution

To achieve the above objectives, the present invention is implemented through the following technical solutions:

A robotic autonomous biopsy sampling method for an in vivo flexible dynamic environment, comprising:

S1, read the laparoscopic image, mark the lesion point and collision avoidance area to be sampled on the image frame according to the doctor's choice, and locate them on the three-dimensional image;

S2, respectively converting the three-dimensional positions of the lesion point and the collision avoidance area on the three-dimensional image, and the three-dimensional position of the end of the multi-degree-of-freedom biopsy forceps into the robot base coordinate system;

S3, using a multi-target motion fusion control method to control the end of the biopsy forceps to move from the initial position to the lesion point; the multi-target includes path tracking from the initial position to the target point, target point tracking and collision avoidance;

S4. After approaching the lesion, stop moving the end of the biopsy forceps and open the biopsy forceps to obtain the contact force between the end of the biopsy forceps and the tissue, and complete the clamping and sampling operation;

S5. The multi-target motion fusion control method is used again to control the end of the multi-degree-of-freedom biopsy forceps to return from the sampling end position to the initial position.

Preferably, the S1 comprises:

S11, reading the laparoscopic image, and marking the lesion point xp ₀ to be sampled and the collision avoidance area Ad on the initial image frame according to the doctor's choice;

S12, mark several key feature points Px and Pa near the lesion point and the collision avoidance area, and determine an area of size L×L with the lesion point and the collision avoidance area as the center respectively;

S13, performing depth estimation on the laparoscopic image to obtain a depth image corresponding to the endoscopic image; obtaining spatial information and color information of each pixel from the depth image and the endoscopic image to construct a three-dimensional point cloud;

S14, registering the continuous three-dimensional point clouds through feature extraction and matching, obtaining the coordinate transformation parameter rotation matrix R and translation vector t, and transforming the source point cloud to the same coordinate system as the target point cloud;

S15. Use the optical flow method to perform feature matching of the lesion point and the collision avoidance area for two adjacent image frames, use the average difference in pixel coordinates of the lesion point and the collision avoidance area point pair to obtain the moving direction and distance between the two frames, and obtain the laparoscope video position xp ^c (t) of the lesion point and the laparoscope video position Ad ^c (t) of the collision avoidance area that change with time.

Preferably, S2 includes:

S21. Calculate the spatial coordinate transformation matrix between the laparoscope and the robot coordinate system through the optical locator The laparoscope image three-dimensional coordinates xp ^c (t) of the lesion point are converted to the sampling robot base coordinate system to obtain the three-dimensional position xp ^r (t) of the lesion point in the robot coordinate system, and the laparoscope image three-dimensional coordinates Ad ^c (t) of the collision avoidance area are converted to the sampling robot base coordinate system to obtain the three-dimensional position Ad ^r (t) of the lesion point in the robot coordinate system;

S22, based on the coordinate transformation matrix from the end of the multi-degree-of-freedom biopsy forceps to the end of the robot Obtain the three-dimensional position ^xbr ( _ts ) of the end of the multi-degree-of-freedom biopsy forceps at the initial time _ts in the robot base coordinate system;

Among them, xr ^r (t _s ) is the three-dimensional pose of the end of the manipulator at the initial time t _s in the robot base coordinate system;

According to the spatial position of the lesion, a fixed RCM point xm is set. Under the constraint of the RCM point, the posture of the robot in the autonomous sampling process is limited to _qt ;

_Yt ＝ _Zt × _Xt-1

q _t = [X _t Y _t Z _t ]

Among them, xb(t) is the position of the end of the biopsy forceps, _Xt represents the X-axis vector of the posture matrix, _Yt represents the Y-axis vector of the posture matrix, _Zt represents the Z-axis vector of the posture matrix, and xb( _ts ) is the position vector of ^xbr ( _ts ) at the initial time _ts .

Preferably, S3 includes:

S31, constructing an overall linear control system and establishing a state equation, and designing target controllers for each target realization requirement in a laparoscopic surgery scenario; the target controllers include a planning path tracking controller, a target guidance controller, and a collision avoidance controller;

S32, establishing corresponding motion control prediction models and target evaluation functions for the planned path tracking controller, the target guidance controller, and the collision avoidance controller, respectively, estimating the motion state of a future prediction time interval based on the system motion state at the current moment, and calculating the corresponding target evaluation function cumulative value;

S33, respectively calculating the target gradient of the cumulative value of the target evaluation function of each controller at the current moment; and according to the preset weight function, nesting and fusing the target gradient values corresponding to each controller in sequence from low to high according to the weight hierarchy sequence, and adding them to the total control input of the system;

S34, converting the fused position output into the joint angle of the surgical robot to realize the autonomous cutting operation when the robot moves from the initial position to the lesion point.

Preferably, the S31 includes:

S311. Construct an overall linear control system and establish the state equation;

y(t)＝Cx(t)+Du(t)

Among them, x(t) is the motion state of the system at time t, is the motion speed of the system at time t, u(t) is the total control input of the system at time t, y(t) is the total control output of the system at time t, A, B, C, D are the calculation parameters of the state equation;

S312. Design target controllers for each target realization requirement in the laparoscopic surgery scenario;

u _i (t) = f (y (t), r _i (t))

Among them, _ui (t) is the control input of controller i, which is a function of the total control output of the system at time t and the expected control target, and _ri (t) is the expected value of the control target at time t.

The target controller includes a planning path tracking controller, a target guidance controller, a cutting depth limit controller and a collision avoidance controller;

Among them, the controller is set for the planned path tracking control:

e _s (t) = r _s (t) - y (t)

r _s (t) = ψ (xb (t _s ), xp (t))

Where u _s (t) is the input of the planning path tracking controller, are the proportional coefficient and differential coefficient of the planned path tracking controller PD control, _rs (t) is the expected state of the planned path tracking controller at time t, and _es (t) is the deviation between the expected position of the object controlled by the planned path tracking controller and the total control output of the system;

To boot the controller against the target:

r _o (t) = x p (t)

Where u _o (t) is the input of the target guidance controller, are the proportional coefficient and differential coefficient controlled by the target guidance controller PD, r _o (t) is the position of the target point, e _o (t) is the speed at which the object controlled by the target guidance controller approaches the target, and t _f is the time the target guidance controller expects to complete the cutting task;

Set up the controller for collision avoidance control:

cd(t)＝‖y(t) _-ra (t)‖

_ra (t)＝H(Ad(t))

Where _ua (t) is the input of the collision avoidance controller, are the proportional coefficient and differential coefficient of the collision avoidance controller PD, _ra (t) is the position of the center point of the obstacle Ad(t) at time t, R is the radius of the spherical collision detection area with the center point of the obstacle as the radius, r is the radius of the spherical collision avoidance area with the center point of the obstacle as the radius, cd(t) is the distance between the total control output of the system and the center point of the obstacle, and ε is a very small constant;

Preferably, the S32 includes:

S321, establishing corresponding motion control prediction models for the planned path tracking controller, the target guidance controller, and the collision avoidance controller respectively;

in, is the predicted motion state of controller i at time t within the prediction interval, is the predicted motion speed of controller i at time t within the prediction interval, is the predicted control output of the control target i at time t within the prediction interval;

S322, establishing target evaluation functions for the planned path tracking controller, the target guidance controller, and the collision avoidance controller, respectively, and estimating the motion state of a future prediction time interval based on the system motion state at the current time _t0 in combination with the corresponding motion control prediction model, and calculating the corresponding target evaluation function cumulative value;

Where, _Ji (t ₀ ) is the cumulative value of the objective function of the i-th controller at time t ₀ , _Gi is the target evaluation function value of the i-th controller at a single time point in the prediction interval from t ₀ to t ₀ +T, and T is the prediction time interval;

Among them, in said S322:

Establish a target evaluation function for the planned path tracking control objective to evaluate the effectiveness of the planned path tracking controller within the prediction interval;

Where, _Js ( _t0 ) is the objective function value of the planned path tracking controller at time _t0 , _Gs is the objective evaluation function value of the planned path tracking controller at a single time point in the prediction interval _t0 to _t0 +T, is the predicted control output of the planned path tracking control target at time t within the prediction interval;

A target evaluation function is established for the target guidance control objective to evaluate the effectiveness of the target guidance controller within the prediction interval;

Where, J _o (t ₀ ) is the objective function value of the target guidance controller at time t ₀ , and G _o is the target evaluation function value of the target guidance controller at a single time point in the prediction interval t ₀ to t ₀ +T. is the predicted control output of the target-guided control target at time t within the prediction interval;

Establish a target evaluation function for the collision avoidance control objective to evaluate the effectiveness of the collision avoidance controller within the prediction interval;

Where, _Ja ( _t0 ) is the objective function value of the collision avoidance controller at time _t0 , _Ga is the objective evaluation function value of the collision avoidance controller at a single time point in the prediction interval _t0 to _t0 +T, is the predicted control output of the collision avoidance control target at time t within the prediction interval, and rz is a very small constant.

Preferably, the S33 includes:

S331, calculating the descent gradient of each controller objective evaluation function at the current state t ₀ respectively by optimization;

Among them, _gs ( _t0 ), _go ( _t0 ), _ga ( _t0 ) are the target evaluation function descent gradients of the planned path tracking controller, target guidance controller and collision avoidance controller at time _t0 respectively;

S332, inputting the parameters of each control target and the importance of the control target, and sorting the controllers according to the importance of the target to determine the priority of each controller;

M＝[ _gs , _g0 , _ga ]

Where M is the weight hierarchy sequence of the target controller;

S333, nesting and calculating the fused target gradient value in order from low to high weight levels, and obtaining the target gradient value after nesting and fusion of the three controllers;

in, is the normalized function of the gradient g, α represents the layering parameter, and w _L (t ₀ ) is the target gradient value after the three controllers are nested and fused;

S334, summing the target gradient values after nested fusion to the fused controller, thereby achieving motion fusion of multiple different target motion controllers;

u(t)＝u _s (t)+u _o (t)+u _a (t)-Kw _L (t)

Among them, K is the proportionality coefficient.

Preferably, converting the fused position output into the joint angle of the surgical robot in S34 specifically refers to:

xb(t)＝y(t)

θ(t)＝ζ(q(t))

Among them, the function ζ represents the conversion of the attitude rotation matrix into the Euler angle in the Cartesian coordinate system, θ(t) is the Euler angle in the Cartesian coordinate system, is the speed of change of Euler angles, is the rotation speed of the robot joint angle, and J ^-1 (Θ) is the Jacobian matrix.

Preferably, S4 includes:

After approaching the lesion, the biopsy forceps are opened, the robot autonomous sampling controller is started, and a constant speed of v ₀ of the biopsy forceps is given in the direction of q _t. When the force sensor detects the force, the biopsy forceps starts to decelerate when approaching the lesion area. When the biopsy forceps touches the surface of the lesion area, the force sensor measures the force f _d (t) of the biopsy forceps contacting the tissue surface, and the PID control method is used to make the biopsy forceps start to decelerate until the force sensor obtains the expected stable _fe and stops.

The autonomous sampling control system refers to:

y _f (t)＝C _f x _f (t)+D _f u _f (t)

e(t)＝ _fe - _fd (t)

in, is the speed of the autonomous sampling control system, _xf (t) is the state of the autonomous sampling control system, _uf (t) is the control input of the autonomous sampling control system, _yf (t) is the control output of the autonomous sampling control system, _Af , _Bf , _Cf and _Df are the state equation parameters of the control system, _uf is the control input of the system, e(t) is the error between the expected force and the actual detected force, _kp , _ki and _kd are the PID parameters of the system.

The motion speed output and posture change speed obtained by the controller are converted into the joint angles of the robot to realize autonomous sampling of the robot:

xb(t)＝ _yf (t)

θ(t)＝ζ(q(t))

in, is the rotation speed of the robot joint angle, and J ^-1 (Θ) is the Jacobian matrix.

A robotic autonomous biopsy sampling system for flexible dynamic environments in vivo, comprising:

The marking module is used to read the laparoscopic image, mark the lesion points and collision avoidance areas to be sampled on the image frame according to the doctor's choice, and locate them on the three-dimensional image;

A conversion module, used to convert the three-dimensional positions of the lesion point and the collision avoidance area on the three-dimensional image, and the three-dimensional position of the end of the multi-degree-of-freedom biopsy forceps into the robot base coordinate system;

A mobile module is used to control the end of the biopsy forceps to move from the initial position to the lesion point using a multi-target motion fusion control method; the multi-target includes path tracking, target point tracking and collision avoidance from the initial position to the target point;

The sampling module is used to obtain the contact force between the end of the biopsy forceps and the tissue after reaching the vicinity of the lesion point according to the preset autonomous sampling control system to complete the clamping sampling operation;

The return module is used to use the multi-objective motion fusion control method again to control the end of the multi-degree-of-freedom biopsy forceps to return from the sampling end position to the initial position.

A storage medium stores a computer program for robotic autonomous biopsy sampling in a flexible dynamic environment in vivo, wherein the computer program enables a computer to execute the robotic autonomous biopsy sampling method as described above.

An electronic device, comprising:

one or more processors;

Memory; and

One or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, the programs including instructions for performing the robotic autonomous biopsy sampling method as described above.

(III) Beneficial effects

The present invention provides a robot autonomous biopsy sampling method, system, storage medium and electronic device for flexible dynamic environment in vivo. Compared with the prior art, it has the following beneficial effects:

In the present invention, the lesion point and the collision avoidance area to be sampled are marked on the image frame according to the doctor's choice, and positioned on the three-dimensional image; the three-dimensional positions of the lesion point and the collision avoidance area on the three-dimensional image, and the three-dimensional position of the end of the multi-degree-of-freedom biopsy forceps are respectively converted to the robot base coordinate system; a multi-target motion fusion control method is adopted to control the end of the biopsy forceps to move from the initial position to the lesion point position; after reaching the vicinity of the lesion point position, the contact force between the end of the biopsy forceps and the tissue is obtained according to the preset autonomous sampling control system to complete the clamping sampling operation; the multi-target motion fusion control method is adopted again to control the end of the multi-degree-of-freedom biopsy forceps to return to the initial position from the sampling end position. Not only can the three-dimensional dynamic tracking of the sampling point marked by the doctor be realized, but also the autonomy of the sampling process can be realized, the sampling time can be shortened, and the quality and efficiency of sampling can be improved.

BRIEF DESCRIPTION OF THE DRAWINGS

In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

FIG1 is a schematic flow chart of a method for autonomous biopsy sampling by a robot in a flexible dynamic environment in vivo provided by an embodiment of the present invention.

DETAILED DESCRIPTION

In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention are clearly and completely described. Obviously, the described embodiments are 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 ordinary technicians in this field without creative work are within the scope of protection of the present invention.

The embodiments of the present application solve the technical problem of being unable to guarantee sampling quality and efficiency by providing a robotic autonomous biopsy sampling method, system, storage medium and electronic device for a flexible dynamic environment in the body.

The technical solution in the embodiment of the present application is to solve the above technical problems, and the overall idea is as follows:

In view of the shortcomings of existing biopsy sampling points that cannot be dynamically updated with images and the sampling effect is highly related to the doctor's experience, a robotic autonomous biopsy sampling method for flexible dynamic environments in the body was constructed. It can not only achieve three-dimensional dynamic tracking of the sampling points marked by the doctor, but also realize the autonomy of the sampling process, shorten the sampling time, and improve the quality and efficiency of sampling.

Specifically, in an embodiment of the present invention, the lesion point and the collision avoidance area to be sampled are marked on the image frame according to the doctor's choice and positioned on the three-dimensional image; the three-dimensional positions of the lesion point and the collision avoidance area on the three-dimensional image, as well as the three-dimensional position of the multi-degree-of-freedom biopsy forceps end are respectively converted to the robot base coordinate system; a multi-target motion fusion control method is used to control the end of the biopsy forceps to move from the initial position to the lesion point position; after reaching the vicinity of the lesion point position, the contact force between the end of the biopsy forceps and the tissue is obtained according to the preset autonomous sampling control system to complete the clamping sampling operation; the multi-target motion fusion control method is used again to control the end of the multi-degree-of-freedom biopsy forceps to return from the sampling end position to the initial position.

In order to better understand the above technical solution, the above technical solution will be described in detail below in conjunction with the accompanying drawings and specific implementation methods.

Example:

As shown in FIG1 , an embodiment of the present invention provides a robotic autonomous biopsy sampling method for a flexible dynamic environment in vivo, comprising:

S1, read the laparoscopic image, mark the lesion point and collision avoidance area to be sampled on the image frame according to the doctor's choice, and locate them on the three-dimensional image;

S2, respectively converting the three-dimensional positions of the lesion point and the collision avoidance area on the three-dimensional image, and the three-dimensional position of the end of the multi-degree-of-freedom biopsy forceps into the robot base coordinate system;

S3, using a multi-target motion fusion control method to control the end of the biopsy forceps to move from the initial position to the lesion point; the multi-target includes path tracking from the initial position to the target point, target point tracking and collision avoidance;

S4. After approaching the lesion, stop moving the end of the biopsy forceps and open the biopsy forceps to obtain the contact force between the end of the biopsy forceps and the tissue, and complete the clamping and sampling operation;

S5. The multi-target motion fusion control method is used again to control the end of the multi-degree-of-freedom biopsy forceps to return from the sampling end position to the initial position.

The embodiment of the present invention can not only realize the three-dimensional dynamic tracking of the sampling points marked by the doctor, but also realize the autonomy of the sampling process, shorten the sampling time, and improve the quality and efficiency of sampling.

The following will introduce each step of the above technical solution in detail with specific content:

In step S1, the laparoscopic image is read, and the lesion point and collision avoidance area to be sampled are marked on the image frame according to the doctor's choice, and positioned on the three-dimensional image, including:

S11, reading the laparoscopic image, and marking the lesion point xp ₀ to be sampled and the collision avoidance area Ad on the initial image frame according to the doctor's choice;

S12, mark several key feature points Px and Pa near the lesion point and the collision avoidance area, and determine an area of size L×L with the lesion point and the collision avoidance area as the center respectively;

S13, performing depth estimation on the laparoscopic image to obtain a depth image corresponding to the endoscopic image; obtaining spatial information and color information of each pixel from the depth image and the endoscopic image to construct a three-dimensional point cloud;

S14, registering the continuous three-dimensional point clouds through feature extraction and matching, obtaining the coordinate transformation parameter rotation matrix R and translation vector t, and transforming the source point cloud to the same coordinate system as the target point cloud;

S15. Use the optical flow method to perform feature matching of the lesion point and the collision avoidance area for two adjacent image frames, use the average difference in pixel coordinates of the lesion point and the collision avoidance area point pair to obtain the moving direction and distance between the two frames, and obtain the laparoscope video position xp ^c (t) of the lesion point and the laparoscope video position Ad ^c (t) of the collision avoidance area that change with time.

In this step, the doctor can mark the lesion points to be sampled on the image, which is intuitive, convenient and concise, and can improve the doctor's work efficiency.

In step S2, the three-dimensional positions of the lesion point and the collision avoidance area on the three-dimensional image, and the three-dimensional position of the end of the multi-degree-of-freedom biopsy forceps are respectively converted to the robot base coordinate system, including:

S21. Calculate the spatial coordinate transformation matrix between the laparoscope and the robot coordinate system through the optical locator The laparoscope image three-dimensional coordinates xp ^c (t) of the lesion point are converted to the sampling robot base coordinate system to obtain the three-dimensional position xp ^r (t) of the lesion point in the robot coordinate system, and the laparoscope image three-dimensional coordinates Ad ^c (t) of the collision avoidance area are converted to the sampling robot base coordinate system to obtain the three-dimensional position Ad ^r (t) of the lesion point in the robot coordinate system;

S22, based on the coordinate transformation matrix from the end of the multi-degree-of-freedom biopsy forceps to the end of the robot Obtain the three-dimensional position ^xbr ( _ts ) of the end of the multi-degree-of-freedom biopsy forceps at the initial time _ts in the robot base coordinate system;

Among them, xr ^r (t _s ) is the three-dimensional pose of the end of the manipulator at the initial time t _s in the robot base coordinate system;

According to the spatial position of the lesion, a fixed RCM point xm is set. Under the constraint of the RCM point, the posture of the robot in the autonomous sampling process is limited to _qt ;

_Yt ＝ _Zt × _Xt-1

q _t = [X _t Y _t Z _t ]

Among them, xb(t) is the position of the end of the biopsy forceps, _Xt represents the X-axis vector of the posture matrix, _Yt represents the Y-axis vector of the posture matrix, _Zt represents the Z-axis vector of the posture matrix, and xb( _ts ) is the position vector of ^xbr ( _ts ) at the initial time _ts .

In step S3, a multi-target motion fusion control method is used to control the end of the biopsy forceps to move from the initial position to the lesion point; wherein the multi-target includes path tracking, target point tracking and collision avoidance from the initial position to the target point; including:

S31. Construct an overall linear control system and establish a state equation, and design target controllers according to the various target realization requirements in the laparoscopic surgery scenario; the target controller includes a planning path tracking controller, a target guidance controller, and a collision avoidance controller.

S311. Construct an overall linear control system and establish the state equation;

y(t)＝Cx(t)+Du(t)

Among them, x(t) is the motion state of the system at time t, is the motion speed of the system at time t, u(t) is the total control input of the system at time t, y(t) is the total control output of the system at time t, A, B, C, D are the calculation parameters of the state equation;

S312. Design target controllers for each target realization requirement in the laparoscopic surgery scenario;

u _i (t) = f (y (t), r _i (t))

Among them, _ui (t) is the control input of controller i, which is a function of the total control output of the system at time t and the expected control target, and _ri (t) is the expected value of the control target at time t.

The target controller includes a planning path tracking controller, a target guidance controller, a cutting depth limit controller and a collision avoidance controller;

Among them, in order to ensure that the robot's autonomous cutting execution process meets the setting of the planned trajectory, the planned path tracking control setting controller specifically refers to:

e _s (t) = r _s (t) - y (t)

r _s (t) = ψ (xb (t _s ), xp (t))

Where u _s (t) is the input of the planning path tracking controller, are the proportional coefficient and differential coefficient of the planned path tracking controller PD control, _rs (t) is the expected state of the planned path tracking controller at time t, and _es (t) is the deviation between the expected position of the object controlled by the planned path tracking controller and the total control output of the system;

To ensure the tracking speed of the robot toward the target point along the cutting path, the control input is a function of the current position and the target position. The target guidance controller specifically refers to:

r _o (t) = x p (t)

Where u _o (t) is the input of the target guidance controller, are the proportional coefficient and differential coefficient controlled by the target guidance controller PD, r _o (t) is the position of the target point, e _o (t) is the speed at which the object controlled by the target guidance controller approaches the target, and t _f is the time the target guidance controller expects to complete the cutting task;

In order to ensure the shortest distance between the end of the device and the collision avoidance area and to ensure the safety of the non-target area to the greatest extent, the control input is a function related to the current position and the obstacle position. The collision avoidance control setting controller specifically refers to:

cd(t)＝‖y(t) _-ra (t)‖

_ra (t)＝H(Ad(t))

Where _ua (t) is the input of the collision avoidance controller, are the proportional coefficient and differential coefficient of the collision avoidance controller PD, _ra (t) is the position of the center point of the obstacle Ad(t) at time t, R is the radius of the spherical collision detection area with the center point of the obstacle as the radius, r is the radius of the spherical collision avoidance area with the center point of the obstacle as the radius, cd(t) is the distance between the total control output of the system and the center point of the obstacle, and ε is a very small constant, which ensures that when entering the collision avoidance area, there is a large outward repulsion speed.

S32. Establish corresponding motion control prediction models and target evaluation functions for the planned path tracking controller, target guidance controller and collision avoidance controller respectively, estimate the motion state of a future predicted time interval based on the system motion state at the current moment, and calculate the corresponding cumulative value of the target evaluation function.

S321, establishing corresponding motion control prediction models for the planned path tracking controller, the target guidance controller, and the collision avoidance controller respectively;

in, is the predicted motion state of controller i at time t within the prediction interval, is the predicted motion speed of controller i at time t within the prediction interval, is the predicted control output of the control target i at time t within the prediction interval;

S322, establishing target evaluation functions for the planned path tracking controller, the target guidance controller, and the collision avoidance controller, respectively, and estimating the motion state of a future prediction time interval based on the system motion state at the current time _t0 in combination with the corresponding motion control prediction model, and calculating the corresponding target evaluation function cumulative value;

Where, _Ji (t ₀ ) is the cumulative value of the objective function of the i-th controller at time t ₀ , _Gi is the target evaluation function value of the i-th controller at a single time point in the prediction interval from t ₀ to t ₀ +T, and T is the prediction time interval;

Among them, in said S322:

Establish a target evaluation function for the planned path tracking control objective to evaluate the effectiveness of the planned path tracking controller within the prediction interval;

Where, _Js ( _t0 ) is the objective function value of the planned path tracking controller at time _t0 , _Gs is the objective evaluation function value of the planned path tracking controller at a single time point in the prediction interval _t0 to _t0 +T, is the predicted control output of the planned path tracking control target at time t within the prediction interval;

A target evaluation function is established for the target guidance control objective to evaluate the effectiveness of the target guidance controller within the prediction interval;

Where, J _o (t ₀ ) is the objective function value of the target guidance controller at time t ₀ , and G _o is the target evaluation function value of the target guidance controller at a single time point in the prediction interval t ₀ to t ₀ +T. is the predicted control output of the target-guided control target at time t within the prediction interval;

Establish a target evaluation function for the collision avoidance control objective to evaluate the effectiveness of the collision avoidance controller within the prediction interval;

Where, _Ja ( _t0 ) is the objective function value of the collision avoidance controller at time _t0 , _Ga is the objective evaluation function value of the collision avoidance controller at a single time point in the prediction interval _t0 to _t0 +T, is the predicted control output of the collision avoidance control target at time t within the prediction interval, and rz is a very small constant.

S33. Calculate the target gradient of the cumulative value of the target evaluation function of each controller at the current moment respectively; and according to the preset weight function, nest and fuse the target gradient values corresponding to each controller in sequence from low to high according to the weight hierarchy sequence, and add them to the total control input of the system.

S331, calculating the descent gradient of each controller objective evaluation function at the current state t ₀ respectively by optimization;

Among them, _gs ( _t0 ), _go ( _t0 ), _ga ( _t0 ) are the target evaluation function descent gradients of the planned path tracking controller, target guidance controller and collision avoidance controller at time _t0 respectively;

S332, input the parameters of each control target and the importance of the control target, sort the controllers according to the importance of the target to determine the priority of each controller (assuming that in terms of priority, collision avoidance controller>target guidance controller>planning path tracking controller);

M＝[ _gs , _g0 , _ga ]

Where M is the weight hierarchy sequence of the target controller;

S333, nesting and calculating the fused target gradient value in order from low to high weight levels, and obtaining the target gradient value after nesting and fusion of the three controllers;

in, is the normalized function of the gradient g, α represents the layering parameter, and w _L (t ₀ ) is the target gradient value after the three controllers are nested and fused;

S334, summing the target gradient values after nested fusion to the fused controller, thereby achieving motion fusion of multiple different target motion controllers;

u(t)＝u _s (t)+u _o (t)+u _a (t)-Kw _L (t)

Where K is the proportionality coefficient.

S34, converting the fused position output into the joint angle of the surgical robot, so as to realize the autonomous cutting operation of the robot during the movement from the initial position to the lesion point. Wherein, converting the fused position output into the joint angle of the surgical robot specifically refers to:

xb(t)＝y(t)

θ(t)＝ζ(q(t))

Among them, the function ζ represents the conversion of the attitude rotation matrix into the Euler angle in the Cartesian coordinate system, θ(t) is the Euler angle in the Cartesian coordinate system, is the speed of change of Euler angles, is the rotation speed of the robot joint angle, and J ^-1 (Θ) is the Jacobian matrix.

In step S4, after approaching the lesion point, stop moving the end of the biopsy forceps and open the biopsy forceps to obtain the contact force between the end of the biopsy forceps and the tissue, and complete the clamping sampling operation; including:

After approaching the lesion, the biopsy forceps are opened, the robot autonomous sampling controller is started, and a constant speed of v ₀ of the biopsy forceps is given in the direction of q _t. When the force sensor detects the force, the biopsy forceps starts to decelerate when approaching the lesion area. When the biopsy forceps touches the surface of the lesion area, the force sensor measures the force f _d (t) of the biopsy forceps contacting the tissue surface, and the PID control method is used to make the biopsy forceps start to decelerate until the force sensor obtains the expected stable _fe and stops.

The autonomous sampling control system refers to:

y _f (t)＝C _f x _f (t)+D _f u _f (t)

e(t)＝ _fe - _fd (t)

in, is the speed of the autonomous sampling control system, _xf (t) is the state of the autonomous sampling control system, _uf (t) is the control input of the autonomous sampling control system, _yf (t) is the control output of the autonomous sampling control system, _Af , _Bf , _Cf and _Df are the state equation parameters of the control system, _uf is the control input of the system, e(t) is the error between the expected force and the actual detected force, _kp , _ki and _kd are the PID parameters of the system.

The motion speed output and posture change speed obtained by the controller are converted into the joint angles of the robot to realize autonomous sampling of the robot:

xb(t)＝ _yf (t)

θ(t)＝ζ(q(t))

in, is the rotation speed of the robot joint angle, and J ^-1 (Θ) is the Jacobian matrix.

In step S5, the multi-target motion fusion control method is used again to control the end of the multi-degree-of-freedom biopsy forceps to return from the sampling end position to the initial position; including

The biopsy forceps completes the clamping sampling operation, exits the sampling control system, and performs a return operation. According to the initial position xb(t _s ) of the end of the biopsy forceps, the robot performs collision-free target tracking control from the current end of the multi-degree-of-freedom biopsy forceps to the initial position of the end of the biopsy forceps. The multi-target motion fusion method introduced in step S3 is used to realize the fusion control of path tracking from the sampling end position xb(t _e ) to xb(t _s ), target point xb(t _s ) tracking and collision avoidance Ad(t), thereby realizing the intraoperative self-service sampling operation of laparoscopic surgery, which will not be repeated here.

Therefore, the autonomous sampling method provided by the embodiment of the present invention breaks away from the reliance of traditional sampling on the doctor's experience and operating ability, and improves the efficiency and accuracy of biopsy sampling.

The embodiment of the present invention provides a robotic autonomous biopsy sampling system for a flexible dynamic environment in vivo, comprising:

The marking module is used to read the laparoscopic image, mark the lesion points and collision avoidance areas to be sampled on the image frame according to the doctor's choice, and locate them on the three-dimensional image;

A conversion module, used to convert the three-dimensional positions of the lesion point and the collision avoidance area on the three-dimensional image, and the three-dimensional position of the end of the multi-degree-of-freedom biopsy forceps into the robot base coordinate system;

A mobile module is used to control the end of the biopsy forceps to move from the initial position to the lesion point using a multi-target motion fusion control method; the multi-target includes path tracking, target point tracking and collision avoidance from the initial position to the target point;

The sampling module is used to obtain the contact force between the end of the biopsy forceps and the tissue after reaching the vicinity of the lesion point according to the preset autonomous sampling control system to complete the clamping sampling operation;

The return module is used to use the multi-objective motion fusion control method again to control the end of the multi-degree-of-freedom biopsy forceps to return from the sampling end position to the initial position.

An embodiment of the present invention provides a storage medium storing a computer program for robotic autonomous biopsy sampling in a flexible dynamic environment in vivo, wherein the computer program enables a computer to execute the robotic autonomous biopsy sampling method as described above.

An embodiment of the present invention further provides an electronic device, including:

one or more processors;

Memory; and

One or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, the programs including a method for executing the robotic autonomous biopsy sampling method as described above

It is understandable that the robotic autonomous biopsy sampling system, storage medium and electronic device for a flexible and dynamic environment in vivo provided in the embodiments of the present invention correspond to the robotic autonomous biopsy sampling method for a flexible and dynamic environment in vivo provided in the embodiments of the present invention. The explanations, examples and beneficial effects of the relevant contents can refer to the corresponding parts in the robotic autonomous biopsy sampling method and will not be repeated here.

In summary, compared with the prior art, the present invention has the following beneficial effects:

1. The embodiment of the present invention can not only realize the three-dimensional dynamic tracking of the sampling points marked by the doctor, but also realize the autonomy of the sampling process, shorten the sampling time, and improve the quality and efficiency of sampling.

2. In the embodiment of the present invention, the doctor can mark the lesion point to be sampled on the image, which is intuitive, convenient and concise, and can improve the doctor's work efficiency.

3. The autonomous sampling method provided by the embodiment of the present invention breaks away from the reliance of traditional sampling on the doctor's experience and operating ability, and improves the efficiency and accuracy of biopsy sampling.

It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the presence of other identical elements in the process, method, article or device including the elements.

The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features thereof may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims (6)

1. A robotic autonomous biopsy sampling system for flexible dynamic environments in vivo, comprising: A marking module, used to execute S1, read the laparoscopic image, mark the lesion point and collision avoidance area to be sampled on the image frame according to the doctor's choice, and locate them on the three-dimensional image; A conversion module, used to execute S2, and convert the three-dimensional positions of the lesion point and the collision avoidance area on the three-dimensional image, and the three-dimensional position of the end of the multi-degree-of-freedom biopsy forceps into the robot base coordinate system; The mobile module is used to execute S3 and use a multi-target motion fusion control method to control the end of the biopsy forceps to move from the initial position to the lesion point; the multi-target includes path tracking, target point tracking and collision avoidance from the initial position to the target point; The sampling module is used to execute S4, stop moving the end of the biopsy forceps and open the biopsy forceps after approaching the lesion point, obtain the contact force between the end of the biopsy forceps and the tissue according to the preset autonomous sampling control system, and complete the clamping sampling operation; A return module is used to execute S5, and again use the multi-objective motion fusion control method to control the end of the multi-degree-of-freedom biopsy forceps to return from the sampling end position to the initial position; The S3 includes: S31, constructing an overall linear control system and establishing a state equation, and designing target controllers for each target realization requirement in a laparoscopic surgery scenario; the target controllers include a planning path tracking controller, a target guidance controller, and a collision avoidance controller; S32, establishing corresponding motion control prediction models and target evaluation functions for the planned path tracking controller, the target guidance controller, and the collision avoidance controller, respectively, estimating the motion state of a future prediction time interval based on the system motion state at the current moment, and calculating the corresponding target evaluation function cumulative value; S33, respectively calculating the target gradient of the cumulative value of the target evaluation function of each controller at the current moment; and according to the preset weight function, nesting and fusing the target gradient values corresponding to each controller in sequence from low to high according to the weight hierarchy sequence, and adding them to the total control input of the system; S34, converting the fused position output into the joint angle of the surgical robot, so as to realize the autonomous cutting operation of the robot during the movement from the initial position to the lesion point position; The S31 includes: S311. Construct an overall linear control system and establish the state equation; y(t)＝Cx(t)+Du(t) Among them, x(t) is the motion state of the system at time t, is the motion speed of the system at time t, u(t) is the total control input of the system at time t, y(t) is the total control output of the system at time t, A, B, C, D are the calculation parameters of the state equation; S312. Design target controllers for each target realization requirement in the laparoscopic surgery scenario; u _i (t) = f (y (t), r _i (t)) Where, _ui (t) is the control input of controller i, which is a function of the total control output of the system at time t and the expected control target, and _ri (t) is the expected value of the control target at time t; The target controller includes a planning path tracking controller, a target guidance controller, a cutting depth limit controller and a collision avoidance controller; Among them, the controller is set for the planned path tracking control: e _s (t) = r _s (t) - y (t) r _s (t) = ψ (xb (t _s ), xp (t)) Where u _s (t) is the input of the planning path tracking controller, are the proportional coefficient and differential coefficient of the planned path tracking controller PD control, _rs (t) is the expected state of the planned path tracking controller at time t, and _es (t) is the deviation between the expected position of the object controlled by the planned path tracking controller and the total control output of the system; To boot the controller against the target: r _o (t) = x p (t) Where u _o (t) is the input of the target guidance controller, are the proportional coefficient and differential coefficient controlled by the target guidance controller PD, r _o (t) is the position of the target point, e _o (t) is the speed at which the object controlled by the target guidance controller approaches the target, and t _f is the time the target guidance controller expects to complete the cutting task; Set up the controller for collision avoidance control: cd(t)＝‖y(t) _-ra (t)‖ _ra (t)＝Ｈ(Ad(t)) Where _ua (t) is the input of the collision avoidance controller, are the proportional coefficient and differential coefficient of the collision avoidance controller PD, _ra (t) is the position of the center point of the obstacle Ad(t) at time t, R is the radius of the spherical collision detection area with the center point of the obstacle as the radius, r is the radius of the spherical collision avoidance area with the center point of the obstacle as the radius, cd(t) is the distance between the total control output of the system and the center point of the obstacle, and ε is a very small constant; The S32 includes: S321, establishing corresponding motion control prediction models for the planned path tracking controller, the target guidance controller, and the collision avoidance controller respectively; in, is the predicted motion state of controller i at time t within the prediction interval, is the predicted motion speed of controller i at time t within the prediction interval, is the predicted control output of the control target i at time t within the prediction interval; S322, establishing target evaluation functions for the planned path tracking controller, the target guidance controller, and the collision avoidance controller, respectively, and estimating the motion state of a future prediction time interval based on the system motion state at the current time _t0 in combination with the corresponding motion control prediction model, and calculating the corresponding target evaluation function cumulative value; Where, _Ji (t ₀ ) is the cumulative value of the objective function of the i-th controller at time t ₀ , _Gi is the target evaluation function value of the i-th controller at a single time point in the prediction interval from t ₀ to t ₀ +T, and T is the prediction time interval; Among them, in said S322: Establish a target evaluation function for the planned path tracking control objective to evaluate the effectiveness of the planned path tracking controller within the prediction interval; Where, _Js ( _t0 ) is the objective function value of the planned path tracking controller at time _t0 , _Gs is the objective evaluation function value of the planned path tracking controller at a single time point in the prediction interval _t0 to _t0 +T, is the predicted control output of the planned path tracking control target at time t within the prediction interval; A target evaluation function is established for the target guidance control objective to evaluate the effectiveness of the target guidance controller within the prediction interval; Where, J _o (t ₀ ) is the objective function value of the target guidance controller at time t ₀ , and G _o is the target evaluation function value of the target guidance controller at a single time point in the prediction interval t ₀ to t ₀ +T. is the predicted control output of the target-guided control target at time t within the prediction interval; Establish a target evaluation function for the collision avoidance control objective to evaluate the effectiveness of the collision avoidance controller within the prediction interval; Where, _Ja ( _t0 ) is the objective function value of the collision avoidance controller at time _t0 , _Ga is the objective evaluation function value of the collision avoidance controller at a single time point in the prediction interval _t0 to _t0 +T, is the predicted control output of the collision avoidance control target at time t within the prediction interval, and rz is a very small constant; The S33 includes: S331, calculating the descent gradient of each controller objective evaluation function at the current state t ₀ respectively by optimization; Among them, _gs ( _t0 ), _go ( _t0 ), _ga ( _t0 ) are the target evaluation function descent gradients of the planned path tracking controller, target guidance controller and collision avoidance controller at time _t0 respectively; S332, inputting the parameters of each control target and the importance of the control target, and sorting the controllers according to the importance of the target to determine the priority of each controller; M＝[ _gs , _g0 , _ga ] Where M is the weight hierarchy sequence of the target controller; S333, nesting and calculating the fused target gradient value in order from low to high weight levels, and obtaining the target gradient value after nesting and fusion of the three controllers; in, is the normalized function of the gradient g, α represents the layering parameter, and w _L (t ₀ ) is the target gradient value after the three controllers are nested and fused; S334, summing the target gradient values after nested fusion to the fused controller, thereby achieving motion fusion of multiple different target motion controllers; u(t)＝u _s (t)+u _o (t)+u _a (t)-Kw _L (t) Where K is the proportionality coefficient; The conversion of the fused position output into the joint angle of the surgical robot in S34 specifically refers to: xb(t)＝y(t) θ(t)＝ζ(q(t)) Among them, the function ζ represents the conversion of the attitude rotation matrix into the Euler angle in the Cartesian coordinate system, θ(t) is the Euler angle in the Cartesian coordinate system, is the speed of change of Euler angles, is the rotation speed of the robot joint angle, and J ^-1 (Θ) is the Jacobian matrix. 2. The robotic autonomous biopsy sampling system according to claim 1, wherein S1 comprises: S11, reading the laparoscopic image, and marking the lesion point xp ₀ to be sampled and the collision avoidance area Ad on the initial image frame according to the doctor's choice; S12, mark several key feature points Px and Pa near the lesion point and the collision avoidance area, and determine an area of size L×L with the lesion point and the collision avoidance area as the center respectively; S13, performing depth estimation on the laparoscopic image to obtain a depth image corresponding to the endoscopic image; obtaining spatial information and color information of each pixel from the depth image and the endoscopic image to construct a three-dimensional point cloud; S14, registering the continuous three-dimensional point clouds through feature extraction and matching, obtaining the coordinate transformation parameter rotation matrix R and translation vector t, and transforming the source point cloud to the same coordinate system as the target point cloud; S15. Use the optical flow method to perform feature matching of the lesion point and the collision avoidance area for two adjacent image frames, use the average difference in pixel coordinates of the lesion point and the collision avoidance area point pair to obtain the moving direction and distance between the two frames, and obtain the laparoscope video position xp ^c (t) of the lesion point and the laparoscope video position Ad ^c (t) of the collision avoidance area that change with time. 3. The robotic autonomous biopsy sampling system according to claim 2, wherein S2 comprises: S21. Calculate the spatial coordinate transformation matrix between the laparoscope and the robot coordinate system through the optical locator The laparoscope image three-dimensional coordinates xp ^c (t) of the lesion point are converted to the sampling robot base coordinate system to obtain the three-dimensional position xp ^r (t) of the lesion point in the robot coordinate system, and the laparoscope image three-dimensional coordinates Ad ^c (t) of the collision avoidance area are converted to the sampling robot base coordinate system to obtain the three-dimensional position Ad ^r (t) of the lesion point in the robot coordinate system; S22, based on the coordinate transformation matrix from the end of the multi-degree-of-freedom biopsy forceps to the end of the robot Obtain the three-dimensional position ^xbr ( _ts ) of the end of the multi-degree-of-freedom biopsy forceps at the initial time _ts in the robot base coordinate system; Among them, xr ^r (t _s ) is the three-dimensional pose of the end of the manipulator at the initial time t _s in the robot base coordinate system; According to the spatial position of the lesion, a fixed RCM point xm is set. Under the constraint of the RCM point, the posture of the robot in the autonomous sampling process is limited to _qt ; _Yt ＝ _Zt × _Xt-1 q _t = [X _t Y _t Z _t ] Among them, xb(t) is the position of the end of the biopsy forceps, _Xt represents the X-axis vector of the posture matrix, _Yt represents the Y-axis vector of the posture matrix, _Zt represents the Z-axis vector of the posture matrix, and xb( _ts ) is the position vector of ^xbr ( _ts ) at the initial time _ts . 4. The robotic autonomous biopsy sampling system according to claim 1 or 3, wherein S4 comprises: After approaching the lesion, the biopsy forceps are opened, the robot autonomous sampling controller is started, and a constant speed of v ₀ of the biopsy forceps is given in the direction of q _t. When the force sensor detects the force, the biopsy forceps starts to decelerate when approaching the lesion area. When the biopsy forceps touches the surface of the lesion area, the force sensor measures the force f _d (t) of the biopsy forceps contacting the tissue surface, and the PID control method is used to make the biopsy forceps start to decelerate until the force sensor obtains the expected stable _fe and stops. The autonomous sampling control system refers to: y _f (t)＝C _f x _f (t)+D _f u _f (t) e(t)＝ _fe - _fd (t) in, is the speed of the autonomous sampling control system, x _f (t) is the state of the autonomous sampling control system, u _f (t) is the control input of the autonomous sampling control system, y _f (t) is the control output of the autonomous sampling control system, A _f , B _f , C _f and D _f are the state equation parameters of the control system, e(t) is the error between the expected force and the actual detected force, k _p , k _i and k _d are the PID parameters of the system; The motion speed output and posture change speed obtained by the controller are converted into the joint angles of the robot to realize autonomous sampling of the robot: xb(t)＝ _yf (t) θ(t)＝ζ(q(t)) in, is the rotation speed of the robot joint angle, and J ^-1 (Θ) is the Jacobian matrix. 5. A storage medium, characterized in that it stores a computer program for robotic autonomous biopsy sampling in a flexible dynamic environment in vivo, wherein the computer program enables a computer to control the robotic autonomous biopsy sampling system as described in any one of claims 1 to 4 to perform robotic autonomous biopsy sampling. 6. An electronic device, comprising: one or more processors; Memory; and One or more programs, wherein the one or more programs are stored in the memory and are configured to be executed by the one or more processors, so that the one or more processors control the robotic autonomous biopsy sampling system according to any one of claims 1 to 4 to perform robotic autonomous biopsy sampling.