Binocular vision-based auxiliary mirror-holding robot control method and system, and medium

Abstract

The application discloses a binocular vision-based auxiliary mirror-holding robot control method and system and a medium, and comprises the following steps: acquiring a left camera image and a right camera image of a binocular endoscope, and acquiring mechanical arm joint angle data of an auxiliary mirror-holding robot; determining mark bit two-dimensional information and instrument depth information of a surgical instrument according to the left and right camera images; determining camera pose information, insertion depth information and RCM position information of the binocular endoscope according to the mechanical arm joint angle data; performing multi-target joint control according to the mark bit two-dimensional information, the instrument depth information, the camera pose information, the insertion depth information and the RCM position information to obtain a mechanical arm end desired speed; determining a mechanical arm desired joint angle according to the mechanical arm end desired speed; and controlling the auxiliary mirror-holding robot according to the mechanical arm desired joint angle. The application improves the accuracy of auxiliary mirror-holding robot control and the safety of minimally invasive surgery, and can be applied to the technical field of medical instrument control.

Description

Technical Field

[0001] This invention relates to the field of medical device control technology, and in particular to a control method, system and medium for an assistive mirror-holding robot based on binocular vision. Background Technology

[0002] Minimally invasive surgery significantly reduces surgical incisions, accelerates postoperative recovery, lowers the probability of complications, and conserves medical resources. The application of surgical robotics technology has further propelled the development of minimally invasive surgery. Using robotic arms to assist in holding the endoscope avoids safety risks caused by subjective factors such as hand tremors and visual fatigue in the assistant holding the endoscope. While master-slave surgical robot systems integrate the endoscope, with the surgeon controlling the surgical field and performing the surgery simultaneously, they lack autonomy. In contrast, endoscope-holding robots offer advantages such as rapid response and stable movement, allowing them to autonomously control the surgical field for the surgeon, improving surgical efficiency and safety.

[0003] Traditional endoscopic-assisted robotic systems rely solely on a monocular endoscope for instrument tracking, failing to fully integrate and utilize the left and right image information from a binocular endoscope. This results in a lack of quantitative assessment of instrument depth and an inability to accurately control the safe distance between surgical instruments and the endoscope, impacting the accuracy of endoscopic-assisted robotic control and the safety of minimally invasive surgery. Furthermore, the limited control objectives of traditional endoscopic-assisted robots increase system errors, further affecting the accuracy of endoscopic-assisted robotic control and the safety of minimally invasive surgery. Summary of the Invention

[0004] The purpose of this invention is to at least partially solve one of the technical problems existing in the prior art.

[0005] Therefore, one objective of this invention is to provide a binocular vision-based control method for an assisted mirror-holding robot, which improves the accuracy of the assisted mirror-holding robot control and the safety of minimally invasive surgery.

[0006] Another objective of this invention is to provide a binocular vision-based assisted mirror-holding robot control system.

[0007] To achieve the above-mentioned technical objectives, the technical solutions adopted in the embodiments of the present invention include:

[0008] In a first aspect, embodiments of the present invention provide a control method for an assisted mirror-holding robot based on binocular vision, comprising the following steps:

[0009] Acquire images from the left and right cameras of the binocular endoscope, and acquire joint angle data of the robotic arm of the endoscope-assisted robot;

[0010] The two-dimensional information of the surgical instrument markers and the instrument depth information are determined based on the images from the left and right cameras.

[0011] The camera pose information, insertion depth information, and RCM position information of the binocular endoscope are determined based on the joint angle data of the robotic arm. Multi-target joint control is then performed based on the two-dimensional information of the marker position, the instrument depth information, the camera pose information, the insertion depth information, and the RCM position information to obtain the desired speed of the robotic arm end effector.

[0012] The desired joint angle of the robotic arm is determined based on the desired speed at the end of the robotic arm, and then the auxiliary mirror-holding robot is controlled based on the desired joint angle of the robotic arm.

[0013] Furthermore, in one embodiment of the present invention, the two-dimensional information of the marker bits includes first two-dimensional information of the marker bits and second two-dimensional information of the marker bits. The step of determining the two-dimensional information of the marker bits and the instrument depth information of the surgical instrument based on the left camera image and the right camera image specifically includes:

[0014] The first two-dimensional information of the surgical instrument's marker position in the left camera image is extracted by the key point detection model, and the second two-dimensional information of the surgical instrument's marker position in the right camera image is extracted by the key point detection module.

[0015] Based on the two-dimensional information of the first marker, the two-dimensional information of the second marker, and the intrinsic and extrinsic parameter matrix of the binocular endoscope, the instrument depth information of the surgical instrument is determined by triangulation.

[0016] Furthermore, in one embodiment of the present invention, the auxiliary mirror-holding robot control method further includes the following steps:

[0017] The binocular endoscope is stereoscopically calibrated so that the pixel rows of the left camera image and the pixel rows of the right camera are strictly aligned one by one.

[0018] Furthermore, in one embodiment of the present invention, the step of determining the camera pose information, insertion depth information, and RCM position information of the binocular endoscope based on the joint angle data of the robotic arm specifically includes:

[0019] The camera pose information of the binocular endoscope is determined by using the robot's joint angle data and a robot forward kinematics model.

[0020] The insertion depth information of the binocular endoscope is determined based on the camera pose information and the initial RCM position;

[0021] The RCM position information is determined based on the camera pose information and the insertion depth information.

[0022] Furthermore, in one embodiment of the present invention, the step of performing multi-target joint control based on the two-dimensional information of the marker, the instrument depth information, the camera pose information, the insertion depth information, and the RCM position information to obtain the desired speed of the robotic arm end effector specifically includes:

[0023] The tracking control velocity components are determined based on the two-dimensional information of the flag bits and the image Jacobian matrix.

[0024] The current instrument depth and the previous instrument depth are determined based on the instrument depth information, and the instrument depth control speed component is determined by the PD control algorithm based on the current instrument depth and the previous instrument depth.

[0025] The image orientation error of the binocular endoscope at the current moment and the previous moment is determined based on the camera pose information, and the hand-eye coordination control speed component is determined by the PD control algorithm based on the image orientation error.

[0026] The current insertion depth and the previous insertion depth of the binocular endoscope are determined based on the insertion depth information, and the insertion depth control speed component is determined by the PD control algorithm based on the current insertion depth and the previous insertion depth.

[0027] The current RCM position and the previous RCM position are determined based on the RCM position information, and the RCM constraint control velocity component is determined by the PD control algorithm based on the current RCM position and the previous RCM position.

[0028] The desired speed of the robotic arm end effector is determined based on the tracking control speed component, the instrument depth control speed component, the hand-eye coordination control speed component, the insertion depth control speed component, and the RCM constraint control speed component.

[0029] Furthermore, in one embodiment of the present invention, the step of determining the desired speed of the robotic arm end effector based on the tracking control speed component, the instrument depth control speed component, the hand-eye coordination control speed component, the insertion depth control speed component, and the RCM constraint control speed component specifically includes:

[0030] The desired camera speed of the binocular endoscope is obtained by weighted summation of the tracking control speed component, the instrument depth control speed component, the hand-eye coordination control speed component, and the insertion depth control speed component.

[0031] The end effector velocity components of the robotic arm are determined based on the desired velocity of the camera and the RCM constraint model of the velocity level.

[0032] The desired end-effector velocity is obtained by weighted summation of the end-effector velocity component and the RCM constraint control velocity component.

[0033] Furthermore, in one embodiment of the present invention, the step of determining the desired joint angle of the robotic arm based on the desired speed of the robotic arm end effector, and then controlling the auxiliary mirror-holding robot based on the desired joint angle of the robotic arm, specifically includes:

[0034] The joint movement speed of the robotic arm is determined based on the desired speed at the end of the robotic arm.

[0035] The desired joint angle of the robotic arm is determined based on the joint movement speed and the current joint angle of the robotic arm.

[0036] The rotation of the motor of the auxiliary mirror-holding robot is controlled according to the desired joint angle of the robotic arm.

[0037] Secondly, embodiments of the present invention provide a binocular vision-based assisted mirror-holding robot control system, comprising:

[0038] The data collection module is used to acquire images from the left and right cameras of the binocular endoscope, and to acquire joint angle data of the robotic arm of the assistive endoscope-holding robot.

[0039] The visual information processing module is used to determine the two-dimensional information of the marker position and the depth information of the surgical instrument based on the image from the left camera and the image from the right camera.

[0040] The multi-target joint control module is used to determine the camera pose information, insertion depth information and RCM position information of the binocular endoscope based on the joint angle data of the robotic arm, and to perform multi-target joint control based on the two-dimensional information of the marker position, the instrument depth information, the camera pose information, the insertion depth information and the RCM position information to obtain the desired speed of the robotic arm end effector.

[0041] The robotic arm motion control module is used to determine the desired joint angle of the robotic arm based on the desired speed of the robotic arm end effector, and then control the auxiliary mirror-holding robot based on the desired joint angle of the robotic arm.

[0042] Thirdly, embodiments of the present invention provide a binocular vision-based auxiliary control device for a mirror-holding robot, comprising:

[0043] At least one processor;

[0044] At least one memory for storing at least one program;

[0045] When the at least one program is executed by the at least one processor, the at least one processor implements the above-described binocular vision-based assisted mirror-holding robot control method.

[0046] Fourthly, embodiments of the present invention also provide a computer-readable storage medium storing a processor-executable program, which, when executed by a processor, is used to perform the above-described binocular vision-based assisted mirror-holding robot control method.

[0047] The advantages and beneficial effects of the present invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention:

[0048] This invention acquires images from the left and right cameras of a binocular endoscope and obtains joint angle data of the robotic arm of an assisted endoscope-holding robot. Based on the left and right camera images, it determines the two-dimensional information of the surgical instrument's marker position and the instrument's depth information. Then, based on the joint angle data of the robotic arm, it determines the camera pose information, insertion depth information, and RCM position information of the binocular endoscope. Based on the two-dimensional information of the marker position, the instrument depth information, the camera pose information, the insertion depth information, and the RCM position information, it performs multi-target joint control to obtain the desired speed of the robotic arm's end effector. Then, based on the desired speed of the robotic arm's end effector, it determines the desired joint angle of the robotic arm, and finally, it controls the assisted endoscope-holding robot based on the desired joint angle of the robotic arm. This invention determines the two-dimensional information of the surgical instrument's marker position and the instrument's depth information based on the images from the left and right cameras. It also determines the camera pose information, insertion depth information, and RCM position information of the binocular endoscope based on the joint angle data of the robotic arm. This enables quantitative assessment of the instrument depth and the insertion depth of the binocular endoscope. Furthermore, multi-target joint control is performed based on the two-dimensional information of the marker position, the instrument depth information, the camera pose information, the insertion depth information, and the RCM position information. This allows for accurate control of the safe distance between the binocular endoscope and the surgical instruments, enabling autonomous adjustment of the surgical field of view and improving the accuracy of the assisted endoscope-holding robot control and the safety of minimally invasive surgery. Attached Figure Description

[0049] To more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings used in the embodiments of the present invention are described below. It should be understood that the drawings described below are only for the convenience of clearly describing some embodiments of the technical solutions of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0050] Figure 1 A flowchart illustrating the steps of a binocular vision-based assisted mirror-holding robot control method provided in an embodiment of the present invention;

[0051] Figure 2 A schematic diagram illustrating an application scenario of the binocular vision-based assisted mirror-holding robot control method provided in this embodiment of the invention;

[0052] Figure 3 A schematic diagram illustrating the specific process of the binocular vision-based assisted mirror-holding robot control method provided in an embodiment of the present invention;

[0053] Figure 4 A schematic diagram of the RCM constraint and coordinate system relationship of a binocular endoscope provided in an embodiment of the present invention;

[0054] Figure 5 A structural block diagram of a binocular vision-based assisted mirror-holding robot control system provided in an embodiment of the present invention;

[0055] Figure 6 This is a structural block diagram of a binocular vision-based auxiliary mirror-holding robot control device provided in an embodiment of the present invention.

[0056] Figure labels: 1. Operating table; 2. Patient; 21. Patient's body surface; 3. Surgical instruments; 4. Assistive endoscope holding robot; 41. Robotic arm end effector; 5. Binocular endoscope; 6. RCM restraint point; 7. Video system; 8. Computer. Detailed Implementation

[0057] The embodiments of the present invention are described in detail below. Examples of these embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and should not be construed as limiting the present invention. The step numbers in the following embodiments are set only for ease of explanation, and there is no limitation on the order between the steps. The execution order of each step in the embodiments can be adaptively adjusted according to the understanding of those skilled in the art.

[0058] In the description of this invention, "multiple" means two or more. The use of "first" and "second" is for distinguishing technical features only and should not be construed as indicating or implying relative importance, or implicitly indicating the number of indicated technical features, or the order of the indicated technical features. Furthermore, unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art.

[0059] Reference Figure 1 This invention provides a binocular vision-based control method for an assisted mirror-holding robot, specifically including the following steps:

[0060] S101. Acquire the left and right camera images of the binocular endoscope, and acquire the joint angle data of the robotic arm of the assistive endoscope holding robot;

[0061] S102. Determine the two-dimensional information of the surgical instrument markers and the instrument depth information based on the images from the left and right cameras;

[0062] S103. Determine the camera pose information, insertion depth information, and RCM position information of the binocular endoscope based on the joint angle data of the robotic arm, and perform multi-target joint control based on the two-dimensional information of the marker position, the instrument depth information, the camera pose information, the insertion depth information, and the RCM position information to obtain the desired speed of the robotic arm end effector.

[0063] S104. Determine the desired joint angle of the robotic arm based on the desired speed at the end of the robotic arm, and then control the auxiliary mirror-holding robot based on the desired joint angle of the robotic arm.

[0064] like Figure 2 The diagram illustrates an application scenario of the binocular vision-based assisted endoscope-holding robot control method provided in this embodiment of the invention. It is understood that during endoscopic minimally invasive surgery, the assisted endoscope-holding robot 4 inserts the binocular endoscope 5 into the patient's body surface 21 on the operating table 1 via the RCM constraint point 6, and transmits and displays the acquired video images in real time in the video system 7. The computer 8 acquires the data required by the system and completes information processing in real time, then sends control commands to the robotic arm driver to drive the robotic arm joints to move, causing the robotic arm end effector 41 to reach the desired state. This replaces the endoscope-holding assistant in achieving autonomous control of the surgical field of view, facilitating the surgeon's operation of the surgical instruments 3 for minimally invasive surgery.

[0065] like Figure 3 The diagram shows a detailed flowchart of the binocular vision-based assisted endoscope robot control method provided in this embodiment of the invention. It should be noted that, since single-objective control cannot simultaneously meet the requirements of system integrity, robustness, and safety, this embodiment proposes a multi-objective joint control method. This method uses visual servo control to ensure that the surgical instrument marker is always located in the center of the surgical field of view, providing the surgeon with a good surgical field of view; it uses endoscopic instrument depth control and endoscopic insertion depth control to avoid endoscopic damage to patient tissues and organs; it uses hand-eye coordination field of view direction control to ensure real-time alignment of the camera image, avoiding hand-eye incoordination caused by camera orientation deviation; and it uses RCM constraint control to achieve minimally invasive procedures on the patient's body surface, avoiding tearing of the patient's skin. The following is based on... Figure 3 Specific embodiments of the present invention will be described in detail below.

[0066] Acquiring RGB images of the left camera inside the abdominal cavity during minimally invasive surgery using a binocular endoscope. and the RGB image from the right camera The joint angles θ = [θ1 θ2 K θ] of the m-degree-of-freedom robotic arm are read from the robot control box via commands. m ] T .

[0067] As an optional implementation, the auxiliary mirror-holding robot control method further includes the following steps:

[0068] Stereoscopic correction is performed on the binocular endoscope to ensure that the pixel rows of the left camera image are strictly aligned with the pixel rows of the right camera image.

[0069] Specifically, before extracting visual information, stereoscopic correction needs to be performed on the binocular endoscope imaging to ensure that the pixel rows of the left and right images are strictly aligned.

[0070] As a further optional implementation, the two-dimensional marker information includes first and second two-dimensional marker information. The step of determining the two-dimensional marker information and instrument depth information of the surgical instrument based on the left and right camera images specifically includes:

[0071] S1021. Extract the first two-dimensional information of the surgical instrument in the left camera image through the key point detection model, and extract the second two-dimensional information of the surgical instrument in the right camera image through the key point detection module.

[0072] S1022. Based on the two-dimensional information of the first marker, the two-dimensional information of the second marker, and the intrinsic and extrinsic parameter matrix of the binocular endoscope, the instrument depth information of the surgical instruments is determined by triangulation.

[0073] Specifically, through key point detection models Extract two-dimensional information of instrument markers from the left and right images. left and r right This two-dimensional information represents the coordinates of the instrument marker in the image pixel coordinate system, that is:

[0074] r left ={r left,i =[u left,i v left,i ] T |i=1,2,KN s} (1)

[0075] r right ={r right,i =[u right,i v right,i ] T |i=1,2,KN s} (2)

[0076] Where, Ns This represents the number of identical instruments appearing in the left and right images.

[0077] Based on the two-dimensional information of the instrument markers extracted from the left and right images r left and r right Calculate the tracking feature points r in the left and right images. tips,left and r tips,right ,Right now:

[0078]

[0079]

[0080] Specifically, based on the two-dimensional information r of the instrument markers extracted from the left and right images. left and r right Calculate the depth of instruments under endoscopy The depth of the i-th instrument is z lap i:

[0081]

[0082] Where d is the baseline distance of the binocular endoscope, f x,left The focal length of the left camera, d x,left d x,right The physical dimension of each pixel in the x-axis direction of the left and right image physical coordinate systems, u 0,left u 0,right These are the image coordinates of the intersection points of the optical axes of the left and right cameras and the image.

[0083] As a further optional implementation, the step of determining the camera pose information, insertion depth information, and RCM position information of the binocular endoscope based on the robotic arm joint angle data specifically includes:

[0084] S1031. Determine the camera pose information of the binocular endoscope based on the joint angle data of the robotic arm and the robot's forward kinematics model;

[0085] S1032. Determine the insertion depth information of the binocular endoscope based on the camera pose information and the initial RCM position;

[0086] S1033. Determine the RCM position information based on the camera pose information and insertion depth information.

[0087] As a further optional implementation, the step of performing multi-target joint control based on the two-dimensional information of the marker position, the depth information of the instrument, the pose information of the camera, the insertion depth information, and the RCM position information to obtain the desired speed of the robotic arm end effector specifically includes:

[0088] S1034. Determine the tracking control velocity components based on the two-dimensional information of the flag bits and the image Jacobian matrix;

[0089] S1035. Determine the current instrument depth and the previous instrument depth based on the instrument depth information, and determine the instrument depth control speed component based on the current instrument depth and the previous instrument depth using the PD control algorithm.

[0090] S1036. Determine the image orientation error of the binocular endoscope at the current moment and the previous moment based on the camera pose information, and determine the hand-eye coordination control speed component based on the image orientation error through the PD control algorithm.

[0091] S1037. Determine the current insertion depth and the previous insertion depth of the binocular endoscope based on the insertion depth information, and determine the insertion depth control speed component based on the current insertion depth and the previous insertion depth using the PD control algorithm.

[0092] S1038. Determine the current RCM position and the previous RCM position based on the RCM position information, and determine the RCM constraint control velocity component based on the current RCM position and the previous RCM position using the PD control algorithm.

[0093] S1039. Determine the desired speed of the robotic arm end effector based on the tracking control speed component, the instrument depth control speed component, the hand-eye coordination control speed component, the insertion depth control speed component, and the RCM constraint control speed component.

[0094] Specifically, the camera intrinsic parameter matrix is ​​obtained through camera calibration, and the camera intrinsic parameter f is used. x,cam f y,cam u 0,cam v 0,cam Calculate the camera tracking control velocity component at the current time t.

[0095]

[0096] Specifically, when the left camera is selected as the controlled camera, cam = left; when the right camera is selected as the controlled camera, cam = right; tracking error. r ctr The center position of the image. It is the pseudo-inverse of the Jacobian matrix of the camera image.

[0097] To avoid shaky views caused by frequent camera movement, it is necessary to establish a view centered on the image r. ctr With the center of the circle, r buffA buffer zone with a radius is defined. When a tracked feature point in the image falls within this buffer zone, the camera stops tracking. Furthermore, the camera tracking speed should be proportional to the tracking error. Therefore, the camera tracking control speed component... Revised to

[0098]

[0099] Where H is the height of the image and W is the width of the image.

[0100] Specifically, the endoscopic instrument depth at the current time t and the expected instrument depth d des Calculate the instrument depth control error at the current time t.

[0101]

[0102] The depth control error of the instrument at the current time t The instrument depth control error at the previous time t-1 and instrument depth control buffer range d depth,buff Calculate the linear velocity z-axis component of the camera's instrument depth control velocity at the current time t.

[0103]

[0104] Among them, K p,depth K d,depth These are the PD control parameters.

[0105] Then the camera device depth control velocity component at the current time t is:

[0106] Specifically, the position of the endoscope in the base coordinate system at the current time t. and the initial position of the RCM point base p RCM0 Calculate the endoscopic insertion depth at the current time t.

[0107]

[0108] Endoscopic insertion depth at current time t Endoscope insertion depth at the previous time t-1 Minimum insertion depth of endoscope d in,min Maximum insertion depth of endoscope d in,max Calculate the insertion depth error at the current time t.

[0109]

[0110] and the insertion depth error at the previous time t-1

[0111]

[0112] The insertion depth error at the current time t Insertion depth error at the previous time t-1 Calculate the linear velocity z-axis component of the camera insertion depth control velocity component at the current time t.

[0113]

[0114] Among them, K p,in K d,in These are the PD control parameters.

[0115] Then the camera insertion depth control velocity component at the current time t is:

[0116] Specifically, this is achieved by manipulating the rotation matrix from the planar coordinate system {op} to the base coordinate system {base}. op R base And the rotation matrix from the base coordinate system {base} to the camera coordinate system {cam} at the current time t. Calculate the rotation matrix from the operation plane coordinate system {op} to the camera coordinate system {cam} at the current time t.

[0117]

[0118] The rotation matrix from the operation plane coordinate system to the camera coordinate system at the current time t. Calculate the image orientation error at the current time t.

[0119]

[0120] Image orientation error at current time t Image orientation error compared to the previous time t-1 Calculate the camera hand-eye coordination control velocity components and angular velocity z-axis components at the current time t.

[0121]

[0122] Among them, K p,coor K d,coor These are the PD control parameters.

[0123] Then the camera hand-eye coordination control velocity component at the current time t is:

[0124] Specifically, the endoscope must pass through the incision and can only move and rotate axially at the incision site, a process known as being constrained by the remote center of motion (RCM).

[0125] like Figure 4 The diagram shown illustrates the RCM constraints and coordinate system relationships of a binocular endoscope provided in an embodiment of the present invention. It includes the robotic arm end-effector coordinate system {m}, the RCM coordinate system {RCM}, the left camera coordinate system {l}, the right camera coordinate system {r}, and the endoscope coordinate system {lap}. The endoscope length is d. lap .

[0126] The homogeneous transformation matrix from the base coordinate system {base} to the endoscope coordinate system {lap} is: base T lap :

[0127]

[0128] The location of the RCM point is... base p RCM :

[0129] base p RCM =η in base p m +η out base p lap (18)

[0130] Where, η in η out These represent the length ratio of the endoscope inside and outside the body, respectively, and η in +η out =1.

[0131] In the robotic arm end-effector coordinate system {m}, the velocities in the RCM coordinate system {RCM} and the endoscope coordinate system {lap} are:

[0132] m v RCM = m v m + m ω m m p RCM (19)

[0133] m v lap = m v m + mω m m p lap (20)

[0134] in, m p RCM,y = m p RCM,x =0, m p RCM,z =η out d lap .

[0135] Since the endoscope is only allowed to move in the axial direction and not in the XY plane, there are constraints:

[0136]

[0137] The velocity in the end-effector coordinate system {m} and the velocity in the endoscope coordinate system {lap} satisfy the velocity-level RCM constraint model as follows:

[0138]

[0139] Endoscopic insertion depth at current time t Current position of the robotic arm's end effector at time t Rotation matrix from base coordinate system to end coordinate system at current time t Calculate the RCM point position at the current time t.

[0140]

[0141] in, Let t be the position of the RCM point in the end coordinate system at the current time t.

[0142] Then the position error of the RCM point at the current time t is

[0143]

[0144] The position error of the RCM point at the current time t The position error of the RCM point at the previous time t-1 Calculate the RCM constraint control velocity components at the current time t.

[0145]

[0146] As a further optional implementation, the step of determining the desired speed of the robotic arm's end effector based on the tracking control speed component, the instrument depth control speed component, the hand-eye coordination control speed component, the insertion depth control speed component, and the RCM constraint control speed component specifically includes:

[0147] S10391. The desired camera speed of the binocular endoscope is obtained by weighted summation of the tracking control speed component, the instrument depth control speed component, the hand-eye coordination control speed component, and the insertion depth control speed component.

[0148] S10392. Determine the end-effector velocity components based on the camera's desired velocity and the RCM constraint model of the velocity level.

[0149] S10393. The desired velocity of the robotic arm end effector is obtained by weighted summation of the end effector velocity component and the RCM constraint control velocity component.

[0150] Specifically, the speed component is controlled by the camera tracking at the current time t. The camera instrument depth control velocity component at current time t The camera insertion depth control velocity component at current time t The camera's hand-eye coordination control speed component at current time t Calculate the camera control speed at the current time t.

[0151]

[0152]

[0153] Where, λ track , λ depth , λ in , λ coor They are respectively The weight.

[0154] Velocity of the camera coordinate system Speed ​​of conversion to endoscopic coordinate system

[0155]

[0156] in, [ lap p cam ] × yes lap p cam An antisymmetric matrix.

[0157] Velocity of the endoscope coordinate system Endoscope speed transformed to end coordinate system

[0158]

[0159] Calculate the velocity in the end-point coordinate system using the velocity-level RCM constraint model.

[0160]

[0161] Therefore, the joint control velocity of multiple targets in the base coordinate system can be calculated.

[0162]

[0163] Furthermore, the end-control velocity in the base coordinate system at the current time t can be calculated.

[0164]

[0165] Where, λ multi , λ RCM They are respectively The weight.

[0166] As a further optional implementation, the step of determining the desired joint angle of the robotic arm based on the desired end-effector velocity, and then controlling the auxiliary mirror-holding robot based on the desired joint angle, specifically includes:

[0167] S1041. Determine the joint movement speed of the robotic arm based on the desired speed of the robotic arm's end effector;

[0168] S1042. Determine the desired joint angle of the robotic arm based on the joint movement speed and the current joint angle of the robotic arm.

[0169] S1043. Control the rotation of the motor of the auxiliary mirror-holding robot according to the expected joint angle of the robotic arm.

[0170] Specifically, the end-effector control velocity is determined using the base coordinate system at the current time t. And the robot Jacobian matrix J in the base coordinate system b Calculate the robot joint motion velocity at the current time t.

[0171]

[0172] Furthermore, the expected joint angle θ of the robot at the next time step t+1 can be calculated. t+1 :

[0173]

[0174] Where dt is the step size of a single cycle.

[0175] Finally, the expected joint angle θ of the robot at the next time step t+1 is calculated. t+1 The signal is sent to the robot controller to control the motor rotation, causing the robotic arm to move to the desired joint angle, thus ending one round of control.

[0176] The steps and flow of the embodiments of the present invention have been described in detail above. It can be understood that the embodiments of the present invention determine the two-dimensional information of the marker position and the instrument depth information of the surgical instrument based on the images of the left and right cameras, and determine the camera pose information, insertion depth information and RCM position information of the binocular endoscope based on the joint angle data of the robotic arm. This can realize the quantitative assessment of the instrument depth and the insertion depth of the binocular endoscope. Then, based on the two-dimensional information of the marker position, the instrument depth information, the camera pose information, the insertion depth information and the RCM position information, multi-target joint control can be performed, thereby accurately controlling the safe distance between the binocular endoscope and the surgical instruments, realizing autonomous adjustment of the surgical field of view, improving the accuracy of the control of the assisted endoscope holding robot and the safety of minimally invasive surgery.

[0177] Reference Figure 5 This invention provides a binocular vision-based assisted mirror-holding robot control system, comprising:

[0178] The data collection module is used to acquire images from the left and right cameras of the binocular endoscope, and to acquire joint angle data of the robotic arm of the assistive endoscope-holding robot.

[0179] The visual information processing module is used to determine the two-dimensional information of the surgical instrument's marker position and the instrument's depth information based on the images from the left and right cameras.

[0180] The multi-target joint control module is used to determine the camera pose information, insertion depth information and RCM position information of the binocular endoscope based on the joint angle data of the robotic arm, and to perform multi-target joint control based on the two-dimensional information of the marker position, the instrument depth information, the camera pose information, the insertion depth information and the RCM position information to obtain the desired speed of the robotic arm end effector.

[0181] The robotic arm motion control module is used to determine the desired joint angle of the robotic arm based on the desired speed of the robotic arm end effector, and then control the auxiliary mirror-holding robot based on the desired joint angle of the robotic arm.

[0182] Specifically, the data collection module is used to: acquire images of the left and right cameras inside the abdominal cavity during minimally invasive surgery using a binocular endoscope camera; and obtain the robot's current joint angle data.

[0183] The visual information processing module is used to: extract the two-dimensional information of the instruments in the left and right images respectively; and calculate the depth information of the instruments under endoscopy using triangulation based on the two-dimensional information of the instruments in the left and right images and the intrinsic and extrinsic parameter matrices of the binocular camera.

[0184] The multi-objective joint control module is used for: calculating the camera tracking control velocity component based on the two-dimensional information of the instrument extracted by the left or right camera using the image Jacobian matrix; calculating the camera instrument depth control velocity component based on the current and previous depth of the instrument under the endoscope using PD control; calculating the insertion depth of the endoscope based on the robot's forward kinematics model and the initial RCM position, and calculating the camera insertion depth control velocity component using PD control; calculating the image orientation error of the camera based on the robot's forward kinematics model and the current and previous time points, and calculating the camera hand-eye coordination control velocity component using PD control; and calculating the RCM point error of the endoscope based on the robot's end-effector pose, endoscope insertion depth, and initial RCM position, and calculating the RCM constraint control velocity component using PD control.

[0185] The robotic arm motion control module is used to: calculate the joint velocity of the robotic arm through a velocity-level inverse kinematics model, thereby calculating the desired joint angle of the robotic arm and driving the robotic arm to move to the desired joint angle.

[0186] The content of the above method embodiments is applicable to this system embodiment. The specific functions implemented in this system embodiment are the same as those in the above method embodiments, and the beneficial effects achieved are also the same as those achieved in the above method embodiments.

[0187] Reference Figure 6 This invention provides a binocular vision-based auxiliary control device for a mirror-holding robot, comprising:

[0188] At least one processor;

[0189] At least one memory for storing at least one program;

[0190] When the above-mentioned at least one program is executed by the above-mentioned at least one processor, the above-mentioned at least one processor implements the above-mentioned binocular vision-based auxiliary mirror-holding robot control method.

[0191] The content of the above method embodiments is applicable to the device embodiments. The specific functions implemented by the device embodiments are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those achieved by the above method embodiments.

[0192] This invention also provides a computer-readable storage medium storing a processor-executable program that, when executed by a processor, performs the aforementioned binocular vision-based assisted mirror-holding robot control method.

[0193] This invention provides a computer-readable storage medium that can execute a binocular vision-based assisted mirror-holding robot control method provided in the method embodiments of this invention. It can execute any combination of the implementation steps of the method embodiments and has the corresponding functions and beneficial effects of the method.

[0194] This invention also discloses a computer program product or computer program, which includes computer instructions stored in a computer-readable storage medium. A processor of a computer device can read the computer instructions from the computer-readable storage medium, and the processor executes the computer instructions, causing the computer device to perform... Figure 1 The method shown.

[0195] In some alternative embodiments, the functions / operations mentioned in the block diagrams may not occur in the order shown in the operation diagrams. For example, depending on the functions / operations involved, two consecutively shown blocks may actually be executed substantially simultaneously, or the aforementioned blocks may sometimes be executed in reverse order. Furthermore, the embodiments presented and described in the flowcharts of this invention are provided by way of example to provide a more comprehensive understanding of the technology. The disclosed methods are not limited to the operations and logic flows presented herein. Alternative embodiments are contemplated in which the order of various operations is changed and sub-operations described as part of a larger operation are executed independently.

[0196] Furthermore, although the invention has been described in the context of functional modules, it should be understood that, unless otherwise stated, one or more of the aforementioned functions and / or features may be integrated into a single physical device and / or software module, or one or more functions and / or features may be implemented in a separate physical device or software module. It is also understood that a detailed discussion of the actual implementation of each module is unnecessary for understanding the invention. Rather, given the properties, functions, and internal relationships of the various functional modules in the apparatus disclosed herein, the actual implementation of the module will be understood within the scope of conventional skill of an engineer. Therefore, those skilled in the art can implement the invention as set forth in the claims using ordinary techniques without excessive experimentation. It is also understood that the specific concepts disclosed are merely illustrative and not intended to limit the scope of the invention, which is determined by the full scope of the appended claims and their equivalents.

[0197] If the aforementioned functions are implemented as software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this invention, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0198] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as a sequenced list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by, or in conjunction with, an instruction execution system, apparatus, or device (such as a computer-based system, a processor-included system, or other system that can fetch and execute instructions from, an instruction execution system, apparatus, or device). For the purposes of this specification, "computer-readable medium" can be any means that can contain, store, communicate, propagate, or transmit programs for use by, or in conjunction with, an instruction execution system, apparatus, or device.

[0199] More specific examples of computer-readable media (a non-exhaustive list) include: electrical connections (electronic devices) having one or more wires, portable computer disk drives (magnetic devices), random access memory (RAM), read-only memory (ROM), erasable and editable read-only memory (EPROM or flash memory), fiber optic devices, and portable optical disc read-only memory (CDROM). Furthermore, computer-readable media can even be paper or other suitable media on which the aforementioned program can be printed, because the aforementioned program can be obtained electronically, for example, by optically scanning the paper or other medium, followed by editing, interpreting, or, if necessary, processing in other suitable ways, and then stored in computer memory.

[0200] It should be understood that various parts of the present invention can be implemented in hardware, software, firmware, or a combination thereof. In the above embodiments, multiple steps or methods can be implemented in software or firmware stored in memory and executed by a suitable instruction execution system. For example, if implemented in hardware, as in another embodiment, it can be implemented using any one or a combination of the following techniques known in the art: discrete logic circuits having logic gates for implementing logical functions on data signals, application-specific integrated circuits (ASICs) having suitable combinational logic gates, programmable gate arrays (PGAs), field-programmable gate arrays (FPGAs), etc.

[0201] In the foregoing description of this specification, references to terms such as "one embodiment," "another embodiment," or "some embodiments" indicate that a specific feature, structure, material, or characteristic described in connection with an embodiment or example is included in at least one embodiment or example of the present invention. In this specification, illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples.

[0202] Although embodiments of the invention have been shown and described, those skilled in the art will understand that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the claims and their equivalents.

[0203] The above is a detailed description of the preferred embodiments of the present invention. However, the present invention is not limited to the above embodiments. Those skilled in the art can make various equivalent modifications or substitutions without departing from the spirit of the present invention. All such equivalent modifications or substitutions are included within the scope defined by the claims of this application.

Claims

1. A binocular vision-based assisted mirror-holding robot control system, characterized in that, include: The data collection module is used to acquire images from the left and right cameras of the binocular endoscope, and to acquire joint angle data of the robotic arm of the assistive endoscope-holding robot. The visual information processing module is used to determine the two-dimensional information of the marker position and the depth information of the surgical instrument based on the image from the left camera and the image from the right camera. The multi-target joint control module is used to determine the camera pose information, insertion depth information and RCM position information of the binocular endoscope based on the joint angle data of the robotic arm, and to perform multi-target joint control based on the two-dimensional information of the marker position, the instrument depth information, the camera pose information, the insertion depth information and the RCM position information to obtain the desired speed of the robotic arm end effector. The robotic arm motion control module is used to determine the desired joint angle of the robotic arm based on the desired speed of the robotic arm end effector, and then control the auxiliary mirror-holding robot based on the desired joint angle of the robotic arm. The step of performing multi-target joint control based on the two-dimensional information of the marker, the depth information of the instrument, the pose information of the camera, the insertion depth information, and the RCM position information to obtain the desired speed of the robotic arm end effector specifically includes: The tracking control velocity components are determined based on the two-dimensional information of the flag bits and the image Jacobian matrix. The current instrument depth and the previous instrument depth are determined based on the instrument depth information, and the instrument depth control speed component is determined by the PD control algorithm based on the current instrument depth and the previous instrument depth. The image orientation error of the binocular endoscope at the current moment and the previous moment is determined based on the camera pose information, and the hand-eye coordination control speed component is determined by the PD control algorithm based on the image orientation error. The current insertion depth and the previous insertion depth of the binocular endoscope are determined based on the insertion depth information, and the insertion depth control speed component is determined by the PD control algorithm based on the current insertion depth and the previous insertion depth. The current RCM position and the previous RCM position are determined based on the RCM position information, and the RCM constraint control velocity component is determined by the PD control algorithm based on the current RCM position and the previous RCM position. The desired speed of the robotic arm end effector is determined based on the tracking control speed component, the instrument depth control speed component, the hand-eye coordination control speed component, the insertion depth control speed component, and the RCM constraint control speed component.

2. The binocular vision-based assisted mirror-holding robot control system according to claim 1, characterized in that, The two-dimensional information of the flag bit includes two-dimensional information of the first flag bit and two-dimensional information of the second flag bit. The visual information processing module is specifically used for: The first two-dimensional information of the surgical instrument's marker position in the left camera image is extracted by the key point detection model, and the second two-dimensional information of the surgical instrument's marker position in the right camera image is extracted by the key point detection module. Based on the two-dimensional information of the first marker, the two-dimensional information of the second marker, and the intrinsic and extrinsic parameter matrix of the binocular endoscope, the instrument depth information of the surgical instrument is determined by triangulation.

3. The binocular vision-based assisted mirror-holding robot control system according to claim 1, characterized in that, The visual information processing module is also used for: The binocular endoscope is stereoscopically calibrated so that the pixel rows of the left camera image and the pixel rows of the right camera image are strictly aligned one by one.

4. The binocular vision-based assisted mirror-holding robot control system according to claim 1, characterized in that, The step of determining the camera pose information, insertion depth information, and RCM position information of the binocular endoscope based on the joint angle data of the robotic arm specifically includes: The camera pose information of the binocular endoscope is determined by using the robot's joint angle data and a robot forward kinematics model. The insertion depth information of the binocular endoscope is determined based on the camera pose information and the initial RCM position; The RCM position information is determined based on the camera pose information and the insertion depth information.

5. The binocular vision-based assisted mirror-holding robot control system according to claim 1, characterized in that, The step of determining the desired end-effector velocity based on the tracking control velocity component, the instrument depth control velocity component, the hand-eye coordination control velocity component, the insertion depth control velocity component, and the RCM constraint control velocity component specifically includes: The desired camera speed of the binocular endoscope is obtained by weighted summation of the tracking control speed component, the instrument depth control speed component, the hand-eye coordination control speed component, and the insertion depth control speed component. The end effector velocity components of the robotic arm are determined based on the desired velocity of the camera and the RCM constraint model of the velocity level. The desired end-effector velocity is obtained by weighted summation of the end-effector velocity component and the RCM constraint control velocity component.

6. A binocular vision-based assisted mirror-holding robot control system according to any one of claims 1 to 5, characterized in that, The robotic arm motion control module is specifically used for: The joint movement speed of the robotic arm is determined based on the desired speed at the end of the robotic arm. The desired joint angle of the robotic arm is determined based on the joint movement speed and the current joint angle of the robotic arm. The rotation of the motor of the auxiliary mirror-holding robot is controlled according to the desired joint angle of the robotic arm.

7. A control device for an auxiliary mirror-holding robot based on binocular vision, characterized in that, include: At least one processor; At least one memory for storing at least one program; When the at least one program is executed by the at least one processor, the at least one processor performs the following steps: Acquire images from the left and right cameras of the binocular endoscope, and acquire joint angle data of the robotic arm of the endoscope-assisted robot; The two-dimensional information of the surgical instrument markers and the instrument depth information are determined based on the images from the left and right cameras. The camera pose information, insertion depth information, and RCM position information of the binocular endoscope are determined based on the joint angle data of the robotic arm. Multi-target joint control is then performed based on the two-dimensional information of the marker position, the instrument depth information, the camera pose information, the insertion depth information, and the RCM position information to obtain the desired speed of the robotic arm end effector. The desired joint angle of the robotic arm is determined based on the desired speed at the end of the robotic arm, and then the auxiliary mirror-holding robot is controlled based on the desired joint angle of the robotic arm. The step of performing multi-target joint control based on the two-dimensional information of the marker, the depth information of the instrument, the pose information of the camera, the insertion depth information, and the RCM position information to obtain the desired speed of the robotic arm end effector specifically includes: The tracking control velocity components are determined based on the two-dimensional information of the flag bits and the image Jacobian matrix. The current instrument depth and the previous instrument depth are determined based on the instrument depth information, and the instrument depth control speed component is determined by the PD control algorithm based on the current instrument depth and the previous instrument depth. The image orientation error of the binocular endoscope at the current moment and the previous moment is determined based on the camera pose information, and the hand-eye coordination control speed component is determined by the PD control algorithm based on the image orientation error. The current insertion depth and the previous insertion depth of the binocular endoscope are determined based on the insertion depth information, and the insertion depth control speed component is determined by the PD control algorithm based on the current insertion depth and the previous insertion depth. The current RCM position and the previous RCM position are determined based on the RCM position information, and the RCM constraint control velocity component is determined by the PD control algorithm based on the current RCM position and the previous RCM position. The desired speed of the robotic arm end effector is determined based on the tracking control speed component, the instrument depth control speed component, the hand-eye coordination control speed component, the insertion depth control speed component, and the RCM constraint control speed component.

8. A computer-readable storage medium storing a processor-executable program, characterized in that, The processor-executable program, when executed by the processor, is used to perform the following steps: Acquire images from the left and right cameras of the binocular endoscope, and acquire joint angle data of the robotic arm of the endoscope-assisted robot; The two-dimensional information of the surgical instrument markers and the instrument depth information are determined based on the images from the left and right cameras. The camera pose information, insertion depth information, and RCM position information of the binocular endoscope are determined based on the joint angle data of the robotic arm. Multi-target joint control is then performed based on the two-dimensional information of the marker position, the instrument depth information, the camera pose information, the insertion depth information, and the RCM position information to obtain the desired speed of the robotic arm end effector. The desired joint angle of the robotic arm is determined based on the desired speed at the end of the robotic arm, and then the auxiliary mirror-holding robot is controlled based on the desired joint angle of the robotic arm. The step of performing multi-target joint control based on the two-dimensional information of the marker, the depth information of the instrument, the pose information of the camera, the insertion depth information, and the RCM position information to obtain the desired speed of the robotic arm end effector specifically includes: The tracking control velocity components are determined based on the two-dimensional information of the flag bits and the image Jacobian matrix. The current instrument depth and the previous instrument depth are determined based on the instrument depth information, and the instrument depth control speed component is determined by the PD control algorithm based on the current instrument depth and the previous instrument depth. The image orientation error of the binocular endoscope at the current moment and the previous moment is determined based on the camera pose information, and the hand-eye coordination control speed component is determined by the PD control algorithm based on the image orientation error. The current insertion depth and the previous insertion depth of the binocular endoscope are determined based on the insertion depth information, and the insertion depth control speed component is determined by the PD control algorithm based on the current insertion depth and the previous insertion depth. The current RCM position and the previous RCM position are determined based on the RCM position information, and the RCM constraint control velocity component is determined by the PD control algorithm based on the current RCM position and the previous RCM position. The desired speed of the robotic arm end effector is determined based on the tracking control speed component, the instrument depth control speed component, the hand-eye coordination control speed component, the insertion depth control speed component, and the RCM constraint control speed component.

Images