Laparoscope operation method and system combined with admittance controller

Abstract

The invention belongs to the technical field of laparoscope operation, and particularly discloses a laparoscope operation method and system combined with an admittance controller. The method includes the steps that a robot is used for achieving laparoscope posture control under RCM constraint; joint torque applied by a doctor is obtained, if the joint torque is larger than a threshold value, the expected pose of the robot is calculated in combination with an admittance controller and the current pose, and otherwise, the expected pose of the robot is calculated in combination with the current pose and the mirror moving instruction; an expected joint angle is calculated according to the expected pose, and a robot kinematics model with an arm angle as a redundant parameter is constructed according to the expected joint angle; and according to the kinematic model, establishing a robot joint control framework under a non-blocking process to perform position control on joints of a robot, thereby realizing laparoscope operation. According to the method, the laparoscope can be kept to meet the RCM constraint all the time, the passivity and the bounded property of free movement of the redundant robot can be achieved, the operation precision is high, and the stability is good.

Description

Technical Field

[0001] The present invention belongs to the technical field of laparoscopic mirror movement, and more specifically, relates to a laparoscopic mirror movement method and system combined with an admittance controller. Background Art

[0002] In traditional minimally invasive abdominal surgery, the surgeon first uses surgical instruments to drill several tiny incisions in the patient's abdomen, so that one end of the slender surgical instrument can pass through the incision to reach the vicinity of the patient's internal organs, and the other end of the surgical instrument is operated by the surgeon. The laparoscope is an indispensable medical instrument in abdominal surgery. It can be inserted into the patient's abdominal cavity through the RCM point to provide a field of view for the surgeon. In order to cooperate with the surgeon's surgical progress, an assistant holding the laparoscope is required to continuously adjust the laparoscope posture and the distance from the lens to the organ, so as to provide a suitable and good field of view to ensure the smooth progress of the operation. However, in the actual operation, there are the following problems: (1) During a long operation, the surgeon holding the laparoscope will experience physiological shaking or small movements of the laparoscope, which brings interference and challenges to minimally invasive abdominal surgery. (2) It is difficult to move the laparoscope frequently and slightly, and when the surgeon needs to perform subtle operations, it cannot be guaranteed to provide the most appropriate field of view. (3) Holding the laparoscope will generate tangential force on the patient's abdominal incision, causing the wound to expand and even endangering the patient's life.

[0003] To solve the above problems, Kim designed a serial spherical mechanism that always satisfies the RCM constraint in the article “Design of a compact 5-DOF surgical robot of a spherical mechanism”, which can keep the instrument at the end of the mechanism always under the RCM constraint, avoid shear force on the incision, and thus improve the safety of the operation. Azimian adopted a task priority method in the article “On constrained manipulation in robotics-assisted minimally invasive surgery”, that is, the RCM constraint is at the first priority, and the second priority is trajectory tracking through the tool tip. The system can provide a more appropriate field of view for the surgeon while maintaining the RCM constraint. From derived a new Jacobian matrix and called it the RCM Jacobian matrix, which eliminates the kinematic constraints from the control loop, and then safely performs the task through Cartesian impedance control.

[0004] However, the mechanism designed by Kim will take up a large amount of surgical space, which will interfere with the operation of the surgeon, and the mechanism has only five degrees of freedom, making it difficult to achieve other control tasks beyond the RCM constraint. Azimian's method only uses position control, which will result in a large contact force under rigid interaction, which will pose a safety hazard to minimally invasive abdominal surgery. In addition, the existing technology can only ensure that the end effector gives feedback when it contacts the external environment, which means that the camera can only be moved by moving the end during surgery, which requires the doctor to have accurate spatial cognition, and it is difficult to achieve the desired robot posture by moving the end effector. Summary of the invention

[0005] In view of the above defects or improvement needs of the prior art, the present invention provides a laparoscopic mirror movement method and system combined with an admittance controller, wherein the redundant robot's own characteristics and the laparoscopic mirror movement control characteristics are combined to design a laparoscopic mirror movement method combined with an admittance controller that integrates the intention of the surgeon and the intention deviation of the assistant, which uses a redundant robot to realize laparoscopic posture control under RCM constraints, and at the same time, the robot's joint torque sensor is used to monitor the torque applied by the doctor, and the admittance controller is combined to output the desired posture of the redundant robot; the kinematic model of the SRS configuration redundant robot is set, and the robot joint control framework under the non-blocking process is established, and the position of the robot's joints is controlled, thereby completing the laparoscopic mirror movement task. It can not only keep the laparoscope always meeting the RCM constraints, but also realize the passivity and boundedness of the free movement of the redundant robot.

[0006] To achieve the above object, according to one aspect of the present invention, a laparoscopic mirror operation method combined with an admittance controller is proposed, comprising the following steps:

[0007] Step 1: Use the robot to achieve laparoscopic posture control under RCM constraints;

[0008] Step 2: Obtain the joint torque applied by the doctor. If the joint torque is greater than the threshold, the desired posture of the robot is calculated by combining the admittance controller and the current posture. Otherwise, the desired posture of the robot is calculated by combining the current posture and the mirror movement instruction.

[0009] Step 3, calculating the expected joint angle according to the expected posture, and constructing a robot kinematic model with the arm angle as a redundant parameter;

[0010] Step 4: Establish a robot joint control framework under a non-blocking process according to the kinematic model to control the position of the robot joints, thereby realizing laparoscopic mirror movement.

[0011] As further preferred, step one specifically includes:

[0012] The robot is used to achieve laparoscopic posture control under RCM constraints, while also compensating for the gravity of the robot's end load and intuitively moving the laparoscope in Cartesian space.

[0013] As a further preferred embodiment, step one further includes: collecting camera movement instructions, wherein the camera movement instructions include:

[0014] Move the mirror along the X direction, move the mirror along the Y direction, move the mirror along the axis translation and rotate the mirror along the axis.

[0015] As a further preferred embodiment, the condition matrix H for realizing the laparoscopic posture control under the RCM constraint includes:

[0016]

[0017] in, is the first-order derivative of matrix C with respect to time, C is an arbitrary basis matrix in the robot constraint task space, κ1 and κ2 are the set convergence parameters, and x c It is the projection of the directed vector formed by the line connecting the RCM point and the TCP at the end of the robot on the plane perpendicular to the axis of the end of the robot. For x c The first derivative with respect to time.

[0018] As a further preferred embodiment, in step 2, according to the desired robot joint angular acceleration The basis matrix C in the robot's constraint task space, the conditional matrix H for the robot to implement the RCM constraint, and a matrix satisfying CF in the robot's free motion space T = 0, the arbitrary basis matrix F, the damping matrix D set in the robot's free motion space f , the robot joint angular velocity at the last moment The robot joint torque sensor measures the joint torque value τ(t) at time t to construct the admittance controller.

[0019] As further preferred, in step 2, the nanocontroller includes:

[0020]

[0021]

[0022] Where M is the weighted coefficient matrix set in the admittance controller, is the weighted right pseudo-inverse matrix of matrix C, is the weighted right pseudo-inverse matrix of the matrix F.

[0023] As a further preferred embodiment, in step 2, the calculation model of the joint torque after gravity compensation includes:

[0024] τ=τ(t)-τ0,τ0=J T [g x g y g z m gx m gy m gz ] T

[0025]

[0026] Wherein, τ is the joint torque value after gravity compensation is achieved, τ0 is the joint torque value generated by the laparoscope and the robot end load due to gravity when the robot end is rigidly connected to the laparoscope, and J T is the transposed matrix of the robot’s Jacobian matrix, g x , g y and g z are the components of the gravity vector of the laparoscope and the robot end load along the x, y and z directions under the robot TCF, respectively, x , l y and l z are the distances between the center of mass of the laparoscope and the robot end load and the TCP in the robot TCF along the x, y and z directions, respectively, m gx 、m gy and m gz They are the components of the torque generated by the gravity vector of the laparoscope and the robot end load on the robot along the x, y and z directions under the robot TCF.

[0027] As a further preferred embodiment, step 4 further comprises:

[0028] (401) setting arm angle redundant parameters, setting the robot kinematics model, and obtaining multiple joint angles in combination with the desired posture;

[0029] (402) In combination with the current joint angle, according to the minimum energy principle, the optimal joint angle and the most desired joint angle are selected from the multiple joint angles.

[0030] According to another aspect of the present invention, there is also provided a laparoscopic mirror movement system in combination with an admittance controller, comprising:

[0031] A first control module, for implementing laparoscopic posture control under RCM constraints using a robot;

[0032] The second control module is used to obtain the joint torque applied by the doctor. If the joint torque is greater than the threshold, the desired posture of the robot is calculated by combining the admittance controller and the current posture. Otherwise, the desired posture of the robot is calculated by combining the current posture and the mirror movement instruction.

[0033] a third control module, which calculates the expected joint angle according to the expected posture, and constructs a robot kinematic model with the arm angle as a redundant parameter;

[0034] The fourth control module is used to establish a robot joint control framework under a non-blocking process according to the kinematic model to perform position control on the robot joints, thereby realizing laparoscopic mirror movement.

[0035] As further preferred, the nanocontroller includes:

[0036]

[0037] Where M is the weighted coefficient matrix set in the admittance controller, is the weighted right pseudo-inverse matrix of matrix C, is the weighted right pseudo-inverse matrix of matrix F, is the desired robot joint angular acceleration, C is the basis matrix in the robot constraint task space, H is the condition matrix for the robot to implement RCM constraints, and F is a matrix in the robot free motion space that satisfies CF T = 0, any basis matrix, D f is the damping matrix set in the robot's free motion space, is the robot joint angular velocity at the previous moment, and τ(t) is the joint torque value measured by the robot joint torque sensor at time t.

[0038] In general, the above technical solution conceived by the present invention has the following technical advantages compared with the prior art:

[0039] 1. The present invention decouples the constrained task space and the free motion space by inputting the applied external force in the admittance controller and outputting the desired posture, which can not only ensure that the laparoscope always satisfies the RCM constraints, but also realize the passivity and boundedness of the free motion of the redundant robot.

[0040] 2. The present invention optimizes the preoperative positioning of the redundant robot to ensure that it does not occupy a large amount of space above the patient's abdomen, thereby allowing the robot to take advantage of its high precision and high stability without interfering with the surgeon's surgical operation.

[0041] 3. The present invention realizes gravity compensation of the laparoscope at the end of the robot through redundant robot joint torque sensors. At the same time, compared with the traditional end-attached six-dimensional force sensor, the use of joint torque sensors makes the robot end simpler and the disassembly and assembly of the laparoscope more convenient.

[0042] 4. The present invention takes into account the intention deviation between the surgeon and the assistant, that is, while ensuring that the assistant can control the laparoscope posture through the host computer software, the surgeon can also control the laparoscope posture by touching the joints of the redundant robot to better meet his own intentions. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] Figure 1 It is a flow chart of a laparoscopic scope movement method combined with an admittance controller according to an embodiment of the present invention;

[0044] Figure 2 It is a control logic diagram of a laparoscopic mirror movement method combined with an admittance controller involved in an embodiment of the present invention;

[0045] Figure 3 It is a flowchart of the upper computer software operation of Example 5 of the present invention;

[0046] Figure 4 is a schematic diagram of an admittance controller according to Embodiment 5 of the present invention;

[0047] Figure 5 It is a framework diagram of the robot model of embodiment 5 of the present invention. DETAILED DESCRIPTION

[0048] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0049] The present invention is a laparoscopic mirror movement method combined with an admittance controller. The redundant robot is used to realize the laparoscopic posture control under the RCM (Remote Center of Motion) constraint, and the upper computer software is designed to realize the intuitive mirror movement of the laparoscope in the Cartesian space; at the same time, the joint torque sensor of the robot is used to monitor the torque applied by the doctor, and the desired posture of the redundant robot is output in combination with the admittance controller; the kinematic model of the SRS configuration redundant robot is set, and the robot joint control framework under the non-blocking process is established, and the position of the robot joint is controlled, so as to complete the laparoscopic mirror movement task. Among them, the kinematic model is a model of the analytical method with the arm angle as the redundant parameter, and the principle of the robot joint control framework is to integrate the angle increment within a fixed time step based on the recursive Newton-Euler method. Please see the following embodiment or a combination of multiple embodiments for details.

[0050] Example 1

[0051] like Figure 1 and Figure 2 As shown, an embodiment of the present invention provides a laparoscopic mirror operation method combined with an admittance controller, comprising:

[0052] Step 1: Use redundant robots to realize the posture control of laparoscopy under the constraints of RCM (Remote Center of Motion), design the host computer software to realize the intuitive movement of the laparoscope in Cartesian space; at the same time, the joint torque sensor of the robot is used to monitor the torque applied by the doctor, and the desired posture of the redundant robot is output in combination with the admittance controller;

[0053] Step 2, set up the kinematic model of the SRS configuration redundant robot, establish the robot joint control framework under the non-blocking process, and control the position of the robot joints to complete the laparoscopic mirror movement. The kinematic model is an analytical method model with the arm angle as a redundant parameter, and the principle of the robot joint control framework is to integrate the angle increment within a fixed time step based on the recursive Newton-Euler method.

[0054] Specifically, the expression of the admittance controller is:

[0055]

[0056] Where M is the weighted coefficient matrix set in the admittance controller, is the desired robot joint angular acceleration, C is an arbitrary basis matrix in the redundant robot constraint task space, is the weighted right pseudo-inverse matrix of matrix C, H is the conditional matrix for redundant robots to implement RCM constraints, and F is a matrix in the free motion space of redundant robots that satisfies CF T = 0 for any basis matrix, is the weighted right pseudo-inverse matrix of matrix F, D f is the damping matrix set in the free motion space of the redundant robot, is the robot joint angular velocity at the previous moment, and τ(t) is the joint torque value measured by the robot joint torque sensor at time t.

[0057] Specifically, it also includes optimizing the torque on the right side of the equation in the admittance controller, and the expression of the optimized admittance controller is specifically:

[0058]

[0059] τ=τ(t)-τ0,τ0=J T [g x g y g z m gx m gy mgz ] T

[0060]

[0061] Wherein, τ is the joint torque value after gravity compensation is achieved, τ0 is the joint torque value generated by the laparoscope and the robot end load due to gravity when the robot end is rigidly connected to the laparoscope, and J T is the transposed matrix of the robot’s Jacobian matrix, g x , g y and g z are the components of the gravity vector of the laparoscope and the robot end load along the x, y and z directions under the robot TCF (Tool Coordinate Frame), l x , l y and l z are the distances between the center of mass of the laparoscope and the robot end load and TCP (Tool Center Point) along the x, y and z directions under the robot TCF, respectively, m gx 、m gy and m gz They are the components of the torque generated by the gravity vector of the laparoscope and the robot end load on the robot along the x, y and z directions under the robot TCF.

[0062] Specifically, the condition matrix H for the redundant robot to implement the RCM constraint is:

[0063]

[0064] in, is the first-order derivative of the matrix C with respect to time, κ1 and κ2 are the set convergence parameters, and x c It is the projection of the directed vector formed by the line connecting the RCM point and the TCP at the end of the robot on the plane perpendicular to the axis of the end of the robot. For x c The first derivative with respect to time.

[0065] Specifically, it also includes:

[0066] By setting the kinematic model of the SRS redundant robot, x is calculated c , set the convergence parameters κ1 and κ2 in the admittance controller.

[0067] Example 2

[0068] The embodiment of the present invention further provides a laparoscopic mirror movement system combined with an admittance controller, comprising:

[0069] The posture calculation module is used for redundant robots to realize the posture control of laparoscopy under RCM constraints. The host computer software is designed to realize the intuitive movement of the laparoscope in Cartesian space. At the same time, the joint torque sensor of the robot is used to monitor the torque applied by the doctor, and the desired posture of the redundant robot is output in combination with the admittance controller.

[0070] The control module is used to set the kinematic model of the SRS redundant robot, establish the robot joint control framework under the non-blocking process, and perform position control on the robot joints, thereby completing the laparoscopic mirror movement. The kinematic model is a model of an analytical method with arm angles as redundant parameters, and the principle of the robot joint control framework is to integrate the angle increment within a fixed time step based on the recursive Newton-Euler method.

[0071] Specifically, the expression of the admittance controller is:

[0072]

[0073] Where M is the weighted coefficient matrix set in the admittance controller, is the desired robot joint angular acceleration, C is an arbitrary basis matrix in the redundant robot constraint task space, is the weighted right pseudo-inverse matrix of matrix C, H is the conditional matrix for redundant robots to implement RCM constraints, and F is a matrix in the free motion space of redundant robots that satisfies CF T = 0 for any basis matrix, is the weighted right pseudo-inverse matrix of matrix F, D f is the damping matrix set in the free motion space of the redundant robot, is the robot joint angular velocity at the previous moment, and τ(t) is the joint torque value measured by the robot joint torque sensor at time t.

[0074] Specifically, it also includes optimizing the torque on the right side of the equation in the admittance controller, and the expression of the optimized admittance controller is specifically:

[0075]

[0076] τ=τ(t)-τ0,τ0=J T [g x g y g z m gx m gy m gz ] T

[0077]

[0078] Wherein, τ is the joint torque value after gravity compensation is achieved, τ0 is the joint torque value generated by the laparoscope and the robot end load due to gravity when the robot end is rigidly connected to the laparoscope, and J T is the transposed matrix of the robot’s Jacobian matrix, g x , g y and g z are the components of the gravity vector of the laparoscope and the robot end load along the x, y and z directions under the robot TCF (Tool Coordinate Frame), l x , l y and l z are the distances between the center of mass of the laparoscope and the robot end load and TCP (Tool Center Point) along the x, y and z directions under the robot TCF, respectively, m gx 、m gy and m gz They are the components of the torque generated by the gravity vector of the laparoscope and the robot end load on the robot along the x, y and z directions under the robot TCF.

[0079] Specifically, the condition matrix H for the redundant robot to implement the RCM constraint is:

[0080]

[0081] in, is the first-order derivative of the matrix C with respect to time, κ1 and κ2 are the set convergence parameters, and x c It is the projection of the directed vector formed by the line connecting the RCM point and the TCP at the end of the robot on the plane perpendicular to the axis of the end of the robot. For x c The first derivative with respect to time.

[0082] Specifically, it also includes:

[0083] By setting the kinematic model of the SRS redundant robot, x is calculated c , set the convergence parameters κ1 and κ2 in the admittance controller.

[0084] Example 3

[0085] The embodiment of the present invention further provides a storage medium storing a plurality of instructions, wherein the instructions are used to implement the voice-controlled laparoscopic operation method.

[0086] Optionally, in this embodiment, the above storage medium may be located in any computer terminal in a computer terminal group in a computer network, or in any mobile terminal in a mobile terminal group.

[0087] Optionally, in this embodiment, the storage medium is configured to store program codes for executing the following steps: Step 1, using a redundant robot to implement laparoscopic posture control under RCM (Remote Center of Motion) constraints, designing host computer software to implement intuitive movement of the laparoscope in Cartesian space; at the same time, monitoring the torque applied by the doctor is implemented through the joint torque sensor of the robot, and outputting the desired posture of the redundant robot in combination with the admittance controller;

[0088] Step 2, set up the kinematic model of the SRS configuration redundant robot, establish the robot joint control framework under the non-blocking process, and control the position of the robot joints to complete the laparoscopic mirror movement. The kinematic model is an analytical method model with the arm angle as a redundant parameter, and the principle of the robot joint control framework is to integrate the angle increment within a fixed time step based on the recursive Newton-Euler method.

[0089] Specifically, the expression of the admittance controller is:

[0090]

[0091] Where M is the weighted coefficient matrix set in the admittance controller, is the desired robot joint angular acceleration, C is an arbitrary basis matrix in the redundant robot constraint task space, is the weighted right pseudo-inverse matrix of matrix C, H is the conditional matrix for redundant robots to implement RCM constraints, and F is a matrix in the free motion space of redundant robots that satisfies CF T = 0 for any basis matrix, is the weighted right pseudo-inverse matrix of matrix F, D f is the damping matrix set in the free motion space of the redundant robot, is the robot joint angular velocity at the previous moment, and τ(t) is the joint torque value measured by the robot joint torque sensor at time t.

[0092] Specifically, it also includes optimizing the torque on the right side of the equation in the admittance controller, and the expression of the optimized admittance controller is specifically:

[0093]

[0094] τ=τ(t)-τ0,τ0=J T [g x g y g z m gx m gy m gz ] T

[0095]

[0096] Wherein, τ is the joint torque value after gravity compensation is achieved, τ0 is the joint torque value generated by the laparoscope and the robot end load due to gravity when the robot end is rigidly connected to the laparoscope, and J T is the transposed matrix of the robot’s Jacobian matrix, g x , g y and g z are the components of the gravity vector of the laparoscope and the robot end load along the x, y and z directions under the robot TCF (Tool Coordinate Frame), l x , l y and l z are the distances between the center of mass of the laparoscope and the robot end load and TCP (Tool Center Point) along the x, y and z directions under the robot TCF, respectively, m gx 、m gy and m gz They are the components of the torque generated by the gravity vector of the laparoscope and the robot end load on the robot along the x, y and z directions under the robot TCF.

[0097] Specifically, the condition matrix H for the redundant robot to implement the RCM constraint is:

[0098]

[0099] in, is the first-order derivative of the matrix C with respect to time, κ1 and κ2 are the set convergence parameters, and x c It is the projection of the directed vector formed by the line connecting the RCM point and the TCP at the end of the robot on the plane perpendicular to the axis of the end of the robot. For x c The first derivative with respect to time.

[0100] Specifically, it also includes:

[0101] By setting the kinematic model of the SRS redundant robot, x is calculated c , set the convergence parameters κ1 and κ2 in the admittance controller.

[0102] Example 4

[0103] An embodiment of the present invention also proposes an electronic device, including a processor and a storage medium connected to the processor, wherein the storage medium stores a plurality of instructions, and the instructions can be loaded and executed by the processor so that the processor can execute the voice-controlled laparoscopic operation method.

[0104] Specifically, the electronic device of this embodiment may be a computer terminal, and the computer terminal may include: one or more processors, and a storage medium.

[0105] Among them, the storage medium can be used to store software programs and modules, such as a voice-controlled laparoscopic mirror operation method in an embodiment of the present invention, and the corresponding program instructions / modules. The processor executes various functional applications and data processing by running the software programs and modules stored in the storage medium, that is, realizing the above-mentioned voice-controlled laparoscopic mirror operation method. The storage medium may include a high-speed random storage medium, and may also include a non-volatile storage medium, such as one or more magnetic storage systems, flash memory, or other non-volatile solid-state storage media. In some instances, the storage medium may further include a storage medium remotely arranged relative to the processor, and these remote storage media may be connected to the terminal via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

[0106] The processor can call the information and application programs stored in the storage medium through the transmission system to execute the following steps: Step 1, use the redundant robot to realize the laparoscopic posture control under the RCM (Remote Center of Motion) constraint, design the upper computer software to realize the intuitive movement of the laparoscope in the Cartesian space; at the same time, the joint torque sensor of the robot is used to monitor the torque applied by the doctor, and the desired posture of the redundant robot is output in combination with the admittance controller;

[0107] Step 2, set up the kinematic model of the SRS configuration redundant robot, establish the robot joint control framework under the non-blocking process, and control the position of the robot joints to complete the laparoscopic mirror movement. The kinematic model is an analytical method model with the arm angle as a redundant parameter, and the principle of the robot joint control framework is to integrate the angle increment within a fixed time step based on the recursive Newton-Euler method.

[0108] Specifically, the expression of the admittance controller is:

[0109]

[0110]

[0111] Where M is the weighted coefficient matrix set in the admittance controller, is the desired robot joint angular acceleration, C is an arbitrary basis matrix in the redundant robot constraint task space, is the weighted right pseudo-inverse matrix of matrix C, H is the conditional matrix for redundant robots to implement RCM constraints, and F is a matrix in the free motion space of redundant robots that satisfies CF T= 0 for any basis matrix, is the weighted right pseudo-inverse matrix of matrix F, D f is the damping matrix set in the free motion space of the redundant robot, is the robot joint angular velocity at the previous moment, and τ(t) is the joint torque value measured by the robot joint torque sensor at time t.

[0112] Specifically, it also includes optimizing the torque on the right side of the equation in the admittance controller, and the expression of the optimized admittance controller is specifically:

[0113]

[0114] τ=τ(t)-τ0,τ0=J T [g x g y g z m gx m gy m gz ] T

[0115]

[0116] Wherein, τ is the joint torque value after gravity compensation is achieved, τ0 is the joint torque value generated by the laparoscope and the robot end load due to gravity when the robot end is rigidly connected to the laparoscope, and J T is the transposed matrix of the robot’s Jacobian matrix, g x , g y and g z are the components of the gravity vector of the laparoscope and the robot end load along the x, y and z directions under the robot TCF (Tool Coordinate Frame), l x , l y and l z are the distances between the center of mass of the laparoscope and the robot end load and TCP (Tool Center Point) along the x, y and z directions under the robot TCF, respectively, m gx 、m gy and m gz They are the components of the torque generated by the gravity vector of the laparoscope and the robot end load on the robot along the x, y and z directions under the robot TCF.

[0117] Specifically, the condition matrix H for the redundant robot to implement the RCM constraint is:

[0118]

[0119] in, is the first-order derivative of the matrix C with respect to time, κ1 and κ2 are the set convergence parameters, and x c It is the projection of the directed vector formed by the line connecting the RCM point and the TCP at the end of the robot on the plane perpendicular to the axis of the end of the robot. For x c The first derivative with respect to time.

[0120] Specifically, it also includes:

[0121] By setting the kinematic model of the SRS redundant robot, x is calculated c , set the convergence parameters κ1 and κ2 in the admittance controller.

[0122] Example 5

[0123] The embodiment of the present invention also provides a host computer software for realizing gravity compensation and collecting mirror movement instructions. When the end of the robot is rigidly connected to a load such as a laparoscope, the robot joint torque sensor will also record the torque caused by the load, which will cause serious interference to the subsequent measurement of the torque applied by the doctor on the robot. Therefore, gravity compensation of the end load must be performed.

[0124] like Figure 3 As shown, specifically, the expression of the joint torque value after gravity compensation is:

[0125] τ=τ(t)-τ0,τ0=J T [g x g y g z m gx m gy m gz ] T

[0126]

[0127] Wherein, τ is the joint torque value after gravity compensation, τ(t) is the joint torque value measured by the robot joint torque sensor at time t, τ0 is the joint torque value generated by the laparoscope and the robot end load due to gravity when the robot end is rigidly connected to the laparoscope, and J T is the transposed matrix of the robot’s Jacobian matrix, g x , g y and g z are the components of the gravity vector of the laparoscope and the robot end load along the x, y and z directions under the robot TCF (Tool Coordinate Frame), respectively. x , l y and l zare the distances between the center of mass of the laparoscope and the robot end load and TCP (Tool Center Point) along the x, y and z directions under the robot TCF, respectively, m gx 、m gy and m gz They are the components of the torque generated by the gravity vector of the laparoscope and the robot end load on the robot along the x, y and z directions under the robot TCF.

[0128] Through communication with professional doctors engaged in minimally invasive surgery, we learned that only one degree of freedom can be controlled at a time in the laparoscopic mirror movement task. Therefore, in the host computer software, we first need to set the laparoscopic mirror movement direction instruction set. In order to make the mirror movement process more intuitive, we preset the following four instructions in the host computer software: "Mirror movement along the X direction", "Mirror movement along the Y direction", "Mirror movement along the axis translation" and "Mirror movement along the axis rotation", each of which includes a specified positive direction and negative direction, and corresponds to a specific degree of freedom of the laparoscope.

[0129] After obtaining the doctor's instructions for moving the robot, the desired posture can be calculated in combination with the robot's current posture. However, during laparoscopic surgery, the surgeon may touch the robot or the laparoscope at the end of the robot. If the laparoscope maintains its current posture, a rigid collision will occur, which will endanger the smooth progress of the operation and even the patient's life. Therefore, the robot must be tested for torque. Since we have achieved gravity compensation for the end load, the torque value measured by the robot joint torque sensor is the torque value applied by the doctor.

[0130] like Figure 4 As shown, the torque value measured by the joint torque sensor is input into the admittance control to ensure that the applied torque only acts on the free motion space, and the desired posture is calculated according to the admittance controller. Specifically, the expression of the admittance controller is:

[0131]

[0132] Where M is the weighted coefficient matrix set in the admittance controller, is the desired robot joint angular acceleration, C is an arbitrary basis matrix in the redundant robot constraint task space, is the weighted right pseudo-inverse matrix of matrix C, H is the conditional matrix for redundant robots to implement RCM constraints, and F is a matrix in the free motion space of redundant robots that satisfies CF T = 0 for any basis matrix, is the weighted right pseudo-inverse matrix of matrix F, D f is the damping matrix set in the free motion space of the redundant robot, is the robot joint angular velocity at the previous moment, and τ(t) is the joint torque value measured by the robot joint torque sensor at time t.

[0133] Specifically, it also includes optimizing the torque on the right side of the equation in the admittance controller, and the expression of the optimized admittance controller is specifically:

[0134]

[0135] τ=τ(t)-τ0,τ0=J T [g x g y g z m gx m gy m gz ] T

[0136]

[0137] Wherein, τ is the joint torque value after gravity compensation is achieved, τ0 is the joint torque value generated by the laparoscope and the robot end load due to gravity when the robot end is rigidly connected to the laparoscope, and J T is the transposed matrix of the robot’s Jacobian matrix, g x , g y and g z are the components of the gravity vector of the laparoscope and the robot end load along the x, y and z directions under the robot TCF (Tool Coordinate Frame), l x , l y and l z are the distances between the center of mass of the laparoscope and the robot end load and TCP (Tool Center Point) along the x, y and z directions under the robot TCF, respectively, m gx 、m gy and m gz They are the components of the torque generated by the gravity vector of the laparoscope and the robot end load on the robot along the x, y and z directions under the robot TCF.

[0138] Specifically, the condition matrix H for the redundant robot to implement the RCM constraint is:

[0139]

[0140] in, is the first-order derivative of the matrix C with respect to time, κ1 and κ2 are the set convergence parameters, and x c It is the projection of the directed vector formed by the line connecting the RCM point and the TCP at the end of the robot on the plane perpendicular to the axis of the end of the robot. For x c The first derivative with respect to time.

[0141] Specifically, it also includes:

[0142] By setting the kinematic model of the SRS redundant robot, x is calculated c , set the convergence parameters κ1 and κ2 in the admittance controller.

[0143] like Figure 5 As shown in the figure, after obtaining the desired posture, the robot model needs to be further set to solve the optimal joint angle. First, the arm angle is set as a redundant parameter, the robot kinematic model is set, and multiple joint angles are solved in combination with the desired posture; then, the optimal joint angle is selected based on the minimum energy principle in combination with the current joint angle.

[0144] like Figure 2 As shown, the present invention realizes gravity compensation of the terminal laparoscope and the load and collects and identifies the motion instructions of the laparoscopic mirror movement input by the doctor by designing the upper computer software. If the joint torque value measured in the joint torque sensor is less than the set joint torque threshold, the motion instruction is output to the set robot kinematic model, and the desired posture is solved in combination with the current posture; on the contrary, if the joint torque value measured in the joint torque sensor is greater than the set joint torque threshold, it is considered that the surgeon is in contact with the robot and the laparoscope or even collides at this time, and the joint torque value is output to the admittance controller, and the desired posture is solved in combination with the current posture. The desired posture is input into the robot kinematic model to solve the desired joint angle, and a non-blocking process robot control framework is built to achieve the robot mirror movement task through position control.

[0145] The serial numbers of the above embodiments of the present invention are only for description and do not represent the advantages or disadvantages of the embodiments.

[0146] In the above embodiments of the present invention, the description of each embodiment has its own emphasis. For parts that are not described in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0147] In the several embodiments provided by the present invention, it should be understood that the disclosed technical content can be implemented in other ways. Among them, the system embodiments described above are only schematic. For example, the division of the units is only a logical function division. There may be other division methods in actual implementation. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of units or modules, which can be electrical or other forms.

[0148] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0149] In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0150] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a number of instructions for a computer device (which can be a personal computer, a server or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, read-only storage medium (ROM, Read-Only Memory), random access storage medium (RAM, Random Access Memory), mobile hard disk, magnetic disk or optical disk and other media that can store program codes.

[0151] It will be easily understood by those skilled in the art that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A laparoscopic mirror operation method combined with an admittance controller, characterized in that: The following steps are involved: Step 1: Use the robot to achieve laparoscopic posture control under RCM constraints; Step 2: Obtain the joint torque applied by the doctor. If the joint torque is greater than the threshold, the desired posture of the robot is calculated by combining the admittance controller and the current posture. Otherwise, the desired posture of the robot is calculated by combining the current posture and the mirror movement instruction. Step 3, calculating the expected joint angle according to the expected posture, and constructing a robot kinematic model with the arm angle as a redundant parameter; Step 4: Establish a robot joint control framework under a non-blocking process according to the kinematic model to control the position of the robot joints, thereby realizing laparoscopic mirror movement.

2. A laparoscopic mirror operation method combined with an admittance controller according to claim 1, characterized in that: Step 1 specifically includes: The robot is used to achieve laparoscopic posture control under RCM constraints, while also compensating for the gravity of the robot's end load and intuitively moving the laparoscope in Cartesian space.

3. A laparoscopic mirror operation method in combination with an admittance controller according to claim 2, characterized in that: Step one also includes: collecting camera movement instructions, wherein the camera movement instructions include: Move the mirror along the X direction, move the mirror along the Y direction, move the mirror along the axis translation and rotate the mirror along the axis.

4. The laparoscopic mirror operation method combined with an admittance controller according to claim 1, characterized in that: In step 1, the condition matrix H for realizing the laparoscopic posture control under RCM constraints includes: in, is the first-order derivative of matrix C with respect to time, C is an arbitrary basis matrix in the robot constraint task space, κ1 and κ2 are the set convergence parameters, and x c It is the projection of the directed vector formed by the line connecting the RCM point and the TCP at the end of the robot on the plane perpendicular to the axis of the end of the robot. For x c The first derivative with respect to time.

5. The laparoscopic mirror operation method combined with an admittance controller according to claim 1, characterized in that: In step 2, according to the desired robot joint angular acceleration The basis matrix C in the robot's constraint task space, the conditional matrix H for the robot to implement the RCM constraint, and a matrix satisfying CF in the robot's free motion space T = 0, the arbitrary basis matrix F, the damping matrix D set in the robot's free motion space f , the robot joint angular velocity at the last moment The robot joint torque sensor measures the joint torque value τ(t) at time t to construct the admittance controller.

6. The laparoscopic mirror operation method combined with an admittance controller according to claim 5, characterized in that: In step 2, the nanocontroller includes: Where M is the weighted coefficient matrix set in the admittance controller, is the weighted right pseudo-inverse matrix of matrix C, is the weighted right pseudo-inverse matrix of the matrix F.

7. A laparoscopic mirror operation method in combination with an admittance controller according to claim 6, characterized in that: In step 2, the calculation model of the joint torque after gravity compensation includes: τ=τ(t)-τ0,τ0=J T [g x g y g z m gx m gy m gz ] T Wherein, τ is the joint torque value after gravity compensation is achieved, τ0 is the joint torque value generated by the laparoscope and the robot end load due to gravity when the robot end is rigidly connected to the laparoscope, and J T is the transposed matrix of the robot’s Jacobian matrix, g x , g y and g z are the components of the gravity vector of the laparoscope and the robot end load along the x, y and z directions under the robot TCF, respectively, x , l y and l z are the distances between the center of mass of the laparoscope and the robot end load and the TCP in the robot TCF along the x, y and z directions, respectively, m gx 、m gy and m gz They are the components of the torque generated by the gravity vector of the laparoscope and the robot end load on the robot along the x, y and z directions under the robot TCF.

8. The laparoscopic mirror operation method combined with an admittance controller according to claim 1, characterized in that: Step 4 also includes: (401) setting arm angle redundant parameters, setting the robot kinematics model, and obtaining multiple joint angles in combination with the desired posture; (402) In combination with the current joint angle, according to the minimum energy principle, the optimal joint angle and the most desired joint angle are selected from the multiple joint angles.

9. A laparoscopic mirror movement system combined with an admittance controller, characterized in that: include: A first control module, for implementing laparoscopic posture control under RCM constraints using a robot; The second control module is used to obtain the joint torque applied by the doctor. If the joint torque is greater than the threshold, the desired posture of the robot is calculated by combining the admittance controller and the current posture. Otherwise, the desired posture of the robot is calculated by combining the current posture and the mirror movement instruction. a third control module, which calculates the expected joint angle according to the expected posture, and constructs a robot kinematic model with the arm angle as a redundant parameter; The fourth control module is used to establish a robot joint control framework under a non-blocking process according to the kinematic model to perform position control on the robot joints, thereby realizing laparoscopic mirror movement.

10. The laparoscopic mirror movement system combined with an admittance controller according to claim 9, characterized in that: The nanocontroller comprises: Where M is the weighted coefficient matrix set in the admittance controller, is the weighted right pseudo-inverse matrix of matrix C, is the weighted right pseudo-inverse matrix of matrix F, is the desired robot joint angular acceleration, C is the basis matrix in the robot constraint task space, H is the condition matrix for the robot to implement RCM constraints, and F is a matrix in the robot free motion space that satisfies CF T = 0, any basis matrix, D f is the damping matrix set in the robot's free motion space, is the robot joint angular velocity at the previous moment, and τ(t) is the joint torque value measured by the robot joint torque sensor at time t.

Images