Laparoscopic scope navigation method and system incorporating an admittance controller

Abstract

The present application belongs to the technical field of laparoscope operation, and particularly discloses a laparoscope operation method and system combined with an admittance controller. The method comprises the following steps: using a robot to realize laparoscope posture control under RCM constraint; acquiring joint torque applied by a doctor, and if the joint torque is greater than a threshold, calculating a desired pose of the robot in combination with an admittance controller and a current pose, otherwise, calculating the desired pose of the robot in combination with the current pose and a laparoscope operation instruction; calculating a desired joint angle according to the desired pose, and constructing a robot kinematics model taking arm angle as a redundant parameter according to the desired joint angle; and establishing a robot joint control framework under a non-blocking process according to the kinematics model, so as to realize position control of the joints of the robot, thereby realizing laparoscope operation. The present application can not only keep the laparoscope always meeting the RCM constraint, but also realize the passivity and boundedness of the free motion of the redundant robot, and has high operation precision and good stability.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of laparoscope operation, and more particularly relates to a laparoscope operation method and system combined with an admittance controller. BACKGROUND

[0002] In traditional laparoscopic minimally invasive surgery, the surgeon first uses surgical instruments to drill several small incisions in the patient's abdomen, enabling one end of the elongated surgical instrument to reach the vicinity of the patient's internal organs through the incision, and the other end of the surgical instrument is operated by the surgeon. The laparoscope is an essential medical instrument in laparoscopic surgery, which can be inserted into the patient's abdominal cavity through the RCM point to provide a view for the lead surgeon. In order to cooperate with the surgical process of the lead surgeon, a mirror-holding assistant is needed to continuously adjust the laparoscope posture and the distance from the lens to the organ, thereby providing a suitable and good view to ensure the smooth progress of the surgery. However, in the actual surgical process, there are the following problems: (1) During the long surgical process, the mirror-holding surgeon will have physiological tremor or small amplitude mirror operation, which brings interference and challenges to laparoscopic minimally invasive surgery. (2) It is difficult to achieve frequent and subtle movement of the laparoscope, and when the lead surgeon needs to perform subtle operations, it cannot guarantee to provide the most suitable view. (3) Holding the laparoscope will generate a tangential force on the patient's abdominal incision, causing the wound to expand and even endanger the patient's life safety.

[0003] In order to solve the above problems, Kim designed a serial spherical mechanism in the article "Design of a compact 5-DOF surgical robot of a spherical mechanism", which always satisfies the RCM constraint, so that the instrument at the end of the mechanism always maintains the RCM constraint, avoiding the tangential force on the incision, thereby improving the safety of the surgery. 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 to track the trajectory through the tool tip. This system can provide a more suitable view for the lead surgeon while maintaining the RCM constraint. From derived a new Jacobian matrix and called it RCM Jacobian matrix, which eliminates the kinematic constraint from the control loop, and then performs the task safely through Cartesian impedance control.

[0004] However, the mechanism designed by Kim occupies a large amount of surgical space, interferes with the operation of the main surgeon, and only has 5 degrees of freedom, which is difficult to achieve other control tasks in addition to RCM constraints. The method of Azimian only uses position control, which can cause a large contact force in the case of rigid interaction, which can pose a safety hazard to minimally invasive surgery in the abdominal cavity. In addition, the existing technology can only ensure that the end effector makes feedback when contacting the external environment, which can only be achieved by moving the end during the operation, requiring the surgeon to have accurate spatial cognition, and it is difficult to achieve the desired robot pose by moving the end effector. SUMMARY

[0005] In view of the above defects or improvement needs of the prior art, the present application provides a laparoscope mirror moving method and system combined with an admittance controller, wherein the laparoscope mirror moving method combined with the deviation of the intention of the main surgeon and the intention of the mirror holding assistant is designed according to the characteristics of the redundant robot itself and the control characteristics of the laparoscope mirror moving, which uses the redundant robot to achieve the laparoscope pose control under the RCM constraint, and monitors the torque exerted by the surgeon through the joint torque sensor of the robot, and combines the admittance controller to output the desired pose of the redundant robot. The kinematics model of the S-R-S configuration redundant robot is set, and the robot joint control framework under the non-blocking process is established, and the joints of the robot are position controlled, so as to complete the laparoscope mirror moving task. Both the laparoscope can always meet the RCM constraint, and the passive and bounded motion of the redundant robot can be achieved.

[0006] To achieve the above-mentioned purpose, according to one aspect of the present application, a laparoscope mirror moving method combined with an admittance controller is provided, comprising the following steps:

[0007] Step one, using a robot to achieve laparoscope pose control under RCM constraint;

[0008] Step two, acquiring the joint torque exerted by the surgeon, if the joint torque is greater than the threshold, calculating the desired pose of the robot combined with the admittance controller and the current pose, otherwise, calculating the desired pose of the robot combined with the current pose and the mirror moving instruction;

[0009] Step three, calculating the expected joint angle according to the desired pose, and constructing the robot kinematics model taking the arm angle as the redundant parameter;

[0010] Step four, establishing the robot joint control framework under the non-blocking process according to the kinematics model, to position control the joints of the robot, so as to achieve the laparoscope mirror moving.

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

[0012] The robot is used to realize the laparoscope pose control under the RCM constraint, and the gravity compensation of the robot end load and the intuitive laparoscope operation in the Cartesian space.

[0013] As a further preferred, the step one further comprises: collecting the laparoscope operation instruction, wherein the laparoscope operation instruction comprises:

[0014] The laparoscope operation in the X direction, the laparoscope operation in the Y direction, the laparoscope translation operation along the axis, and the laparoscope rotation operation along the axis.

[0015] As a further preferred, the condition matrix H for realizing the laparoscope pose control under the RCM constraint comprises:

[0016]

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

[0018] As a further preferred, in the step two, according to the desired robot joint angle acceleration The basis matrix C in the robot constraint task space, the condition matrix H for realizing the RCM constraint of the robot, an arbitrary basis matrix F in the robot free motion space which satisfies CF T = 0, the set damping matrix D in the robot free motion space f , the robot joint angle velocity The joint torque value τ(t) measured by the robot joint torque sensor at time t is used to construct the admittance controller.

[0019] As a further preferred, in the step two, the admittance controller comprises:

[0020]

[0021]

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

[0023] As a further preferred, in the step two, the calculation model of the gravity-compensated joint torque comprises:

[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, τ 0 is the joint torque value of the laparoscope and the robot end effector load due to gravity when the laparoscope is rigidly linked to the robot end effector, J T is the transpose matrix of the Jacobian matrix of the robot, g x , g y and g z are the components of the gravity vector of the laparoscope and the robot end effector load in the x, y and z directions under the robot TCF, l x , l y and l z are the components of the distance between the center of mass of the laparoscope and the robot end effector load and the TCP in the x, y and z directions under the robot TCF, m gx , m gy and m gz are the components of the torque generated by the gravity vector of the laparoscope and the robot end effector load on the robot in the x, y and z directions under the robot TCF.

[0027] As a further preferred, step four further comprises:

[0028] (401) setting an arm angle redundancy parameter, setting a robot kinematics model, and obtaining a plurality of joint angles in combination with a desired pose;

[0029] (402) in combination with the current joint angle, filtering out the optimal joint angle from the plurality of joint angles as the desired joint angle according to the minimum energy principle.

[0030] According to another aspect of the present application, there is also provided a laparoscope operating system in combination with an admittance controller, comprising:

[0031] a first control module for using a robot to realize laparoscope pose control under RCM constraint;

[0032] a second control module for obtaining joint torque exerted by a doctor, and if the joint torque is greater than a threshold value, calculating a desired pose of the robot in combination with the admittance controller and the current pose, otherwise, calculating the desired pose of the robot in combination with the current pose and the laparoscope operating instruction;

[0033] A third control module is configured to calculate desired joint angles according to the desired pose and to construct a robot kinematics model with the arm angles as redundant parameters;

[0034] A fourth control module is configured to establish a robot joint control framework under a non-blocking process according to the kinematics model to perform position control on the joints of the robot, thereby achieving laparoscope operation.

[0035] As a further preferred, the said controller comprises:

[0036]

[0037] wherein M is a weighting coefficient matrix set in the admittance controller, is a weighted right pseudo-inverse matrix of the matrix C, is a weighted right pseudo-inverse matrix of the matrix F, is a desired robot joint angular acceleration, C is a base matrix in a robot constrained task space, H is a condition matrix for the robot to realize RCM constraints, F is an arbitrary base matrix in a robot free motion space satisfying CF T = 0, D f is a damping matrix set in the robot free motion space, is a robot joint angular velocity at the previous time, and τ(t) is a joint torque value measured by a robot joint torque sensor at time t.

[0038] Overall, compared with the prior art, the above technical solutions conceived by the present application mainly have the following technical advantages:

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

[0040] 2. The present application 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 exert its own advantages of high precision and high stability, and also does not interfere with the surgical operation of the main operator.

[0041] 3. The present application realizes gravity compensation of the robot end laparoscope through the joint torque sensor of the redundant robot, and compared with the traditional end additional six-dimensional force sensor, the use of the joint torque sensor makes the robot end more simple and the disassembly of the laparoscope more convenient.

[0042] 4.The application considers the intention deviation of the main operator and the mirror assistant, that is, to ensure that the mirror assistant can control the laparoscope posture through the host computer software, and at the same time, the main operator can also control the laparoscope posture by touching each joint of the redundant robot to realize the control of the laparoscope posture more in line with his own intention. BRIEF DESCRIPTION OF DRAWINGS

[0043] Figure 1 is a flow chart of a laparoscope operating method combined with an admittance controller according to an embodiment of the application;

[0044] Figure 2 is a control logic diagram of a laparoscope operating method combined with an admittance controller according to an embodiment of the application;

[0045] Figure 3 is a flow chart of the host computer software work of embodiment 5 of the application;

[0046] Figure 4 is a schematic diagram of the admittance controller of embodiment 5 of the application;

[0047] Figure 5 is a frame diagram of the robot model of embodiment 5 of the application. DETAILED DESCRIPTION

[0048] In order to make the purpose, technical scheme and advantages of the application clearer, the application will be further described in detail below in combination with the drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the application and do not limit the application. In addition, the technical features involved in each embodiment of the application described below can be combined with each other as long as they do not conflict with each other.

[0049] The laparoscope operating method combined with an admittance controller according to the application uses a redundant robot to realize the laparoscope posture control under the RCM (Remote Center of Motion) constraint, designs the host computer software to realize the intuitive laparoscope operation in the Cartesian space; at the same time, the joint torque sensor of the robot is used to realize the monitoring of the torque exerted by the doctor, and the admittance controller is combined to output the desired pose of the redundant robot; the kinematics model of the S-R-S configuration redundant robot is set, the robot joint control framework under the non-blocking process is established, the position control of the joints of the robot is realized, and thus the laparoscope operating task is completed. The kinematics model is a model of the analytical method taking the arm angle as the redundant parameter, and the principle of the robot joint control framework is to integrate the angle increment in the fixed time step based on the recursive Newton-Euler method. For details, please refer to the following embodiment or the combination scheme of multiple embodiments.

[0050] Embodiment 1

[0051] As Figure 1 andFigure 2 As shown, the embodiment of the present application provides a laparoscope operation method combined with an admittance controller, comprising:

[0052] Step 1, using a redundant robot to realize laparoscope posture control under RCM (Remote Center of Motion) constraint, designing an upper computer software to realize intuitive laparoscope operation in Cartesian space; at the same time, realizing monitoring of the torque exerted by a doctor through a joint torque sensor of the robot, and combining an admittance controller to output the expected pose of the redundant robot;

[0053] Step 2, setting a kinematics model of the S-R-S configuration redundant robot, establishing a robot joint control framework under a non-blocking process, and performing position control on the joints of the robot, so as to complete the laparoscope operation. The kinematics model is a model of an analytical method taking arm angle as a redundant parameter, and the principle of the robot joint control framework is to integrate the angle increment in a fixed time step based on the recursive Newton-Euler method.

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

[0055]

[0056] Wherein, M is a weighting coefficient matrix set in the admittance controller, is the expected 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 the matrix C, H is a condition matrix of the redundant robot realizing RCM constraint, F is an arbitrary basis matrix in the redundant robot free motion space satisfying CF T = 0, is the weighted right pseudo-inverse matrix of the matrix F, D f is a damping matrix set in the redundant robot free motion space, is the robot joint angular velocity at the last moment, τ(t) is the joint torque value measured by the robot joint torque sensor at the moment 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, τ0 is the joint torque value of the laparoscope and the robot end effector load due to gravity when the robot end effector is rigidly linked to the laparoscope, J T is the transpose matrix of the Jacobian matrix of the robot, g x , g y and g z are the gravity vector components of the laparoscope and the robot end effector load in the x, y and z directions under the robot TCF (Tool Coordinate Frame), l x , l y and l z are the distance components between the center of mass of the laparoscope and the robot end effector load and the TCP (Tool Center Point) in the x, y and z directions under the robot TCF, m gx , m gy and m gz are the torque components of the gravity vector of the laparoscope and the robot end effector load on the robot in the x, y and z directions under the robot TCF.

[0062] Specifically, the condition matrix H of the RCM constraint of the redundant robot is specifically:

[0063]

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

[0065] Specifically, the method further comprises:

[0066] The kinematics model of the S-R-S configuration redundant robot is set to calculate x c , and the convergence parameters κ1 and κ2 in the admittance controller are set.

[0067] Embodiment 2

[0068] The embodiment of the application further provides a laparoscope operating system combined with an admittance controller, comprising:

[0069] The pose solving module is used for realizing the laparoscope pose control under the RCM constraint of the redundant robot, and a host computer software is designed to realize the intuitive operation of the laparoscope in the Cartesian space. Meanwhile, the joint torque sensor of the robot is used to monitor the torque exerted by the doctor, and the desired pose of the redundant robot is output in combination with the admittance controller.

[0070] The control module is used for setting the kinematics model of the S-R-S configuration redundant robot, establishing the robot joint control framework under the non-blocking process, and performing position control on the joints of the robot, so as to complete the laparoscope operation. The kinematics model is a model of the analytical method taking the arm angle as the redundant parameter, and the principle of the robot joint control framework is to integrate the angle increment in the fixed time step based on the recursive Newton-Euler method.

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

[0072]

[0073] wherein M is a weighting 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 the matrix C, H is a condition matrix of the redundant robot for realizing the RCM constraint, F is an arbitrary basis matrix in the redundant robot free motion space satisfying CF T = 0, is the weighted right pseudo-inverse matrix of the matrix F, D f is a damping matrix set in the redundant robot free motion space, is the robot joint angular velocity at the last moment, τ(t) is the joint torque value measured by the robot joint torque sensor at the moment t.

[0074] Specifically, the torque on the right side of the equation in the admittance controller is optimized, and the expression of the optimized admittance controller is specifically as follows:

[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 realizing gravity compensation, τ0 is the joint torque value of the laparoscope and the robot end load due to gravity when the robot end is rigidly connected to the laparoscope, J T is the transpose matrix of the Jacobian matrix of the robot, g x , g y and g z are the gravity vector components of the laparoscope and the robot end load in the x, y and z directions under the robot TCF (Tool Coordinate Frame), l x , l y and l z are the distance components between the center of mass of the laparoscope and the robot end load and the TCP (Tool Center Point) in the x, y and z directions under the robot TCF, m gx , m gy and m gz are the torque components of the gravity vector of the laparoscope and the robot end load on the robot in the x, y and z directions under the robot TCF.

[0079] Specifically, the condition matrix H of the RCM constraint realized by the redundant robot is specifically:

[0080]

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

[0082] Specifically, it further comprises:

[0083] The kinematics model of the S-R-S configuration redundant robot is set to calculate x c , and the convergence parameters κ1 and κ2 in the admittance controller are set.

[0084] Embodiment 3

[0085] The embodiment of the application also provides a storage medium storing a plurality of instructions for realizing the voice-controlled laparoscope moving method.

[0086] Optionally, in the embodiment, the storage medium can be located in any one of the computer terminals in the computer terminal group in the computer network, or in any one of the mobile terminals in the mobile terminal group.

[0087] Optionally, in the embodiment, the storage medium is configured to store program code for performing the following steps: step 1, using a redundant robot to achieve laparoscope pose control under RCM (Remote Center of Motion) constraint, designing an upper computer software to achieve intuitive laparoscope operation in Cartesian space; at the same time, monitoring the torque exerted by the doctor through the joint torque sensor of the robot, and combining with the output of the mobility controller to output the expected pose of the redundant robot;

[0088] Step 2, setting up a kinematics model of the S-R-S configuration redundant robot, establishing a robot joint control framework under a non-blocking process, and performing position control on the joints of the robot to complete the laparoscope operation. The kinematics model is a model of an analytical method taking arm angle as a redundant parameter, and the principle of the robot joint control framework is to integrate the angle increment in a fixed time step based on the recursive Newton-Euler method.

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

[0090]

[0091] Wherein M is a weighting coefficient matrix set in the mobility controller, is the expected robot joint angular acceleration, C is an arbitrary basis matrix in the task space of the redundant robot constraint, is the weighted right pseudo-inverse matrix of matrix C, H is the condition matrix of the redundant robot to realize RCM constraint, F is an arbitrary basis matrix in the free motion space of the redundant robot that satisfies CF T = 0, is the weighted right pseudo-inverse matrix of matrix F, D f is a damping matrix set in the free motion space of the redundant robot, is the robot joint angular velocity at the last moment, τ(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 mobility controller, and the expression of the optimized mobility 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 realizing gravity compensation, τ0 is the joint torque value of the laparoscope and the robot end load due to gravity when the robot end is rigidly linked to the laparoscope, J T is the transpose matrix of the Jacobian matrix of the robot, g x , g y and g z are the gravity vector components of the laparoscope and the robot end load in 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 the TCP (Tool Center Point) in the x, y and z directions under the robot TCF, m gx , m gy and m gz are the torque components of the gravity vector of the laparoscope and the robot end load on the robot in the x, y and z directions under the robot TCF.

[0097] Specifically, the condition matrix H of the RCM constraint realized by the redundant robot is specifically:

[0098]

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

[0100] Specifically, it further comprises:

[0101] The kinematics model of the S-R-S configuration redundant robot is set to calculate x c , and the convergence parameters κ1, κ2 in the admittance controller are set.

[0102] Embodiment 4

[0103] The embodiment of the application also provides an electronic device comprising a processor and a storage medium connected to the processor, wherein the storage medium stores a plurality of instructions, the instructions can be loaded and executed by the processor, so that the processor can execute the voice-controlled laparoscope moving method.

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

[0105] The storage medium can be configured to store software programs and modules, such as a voice-controlled laparoscope moving method according to an embodiment of the present application, corresponding program instructions / modules, and the processor can execute various functions, applications, and data processing by running the software programs and modules stored in the storage medium, that is, implement the voice-controlled laparoscope moving method described above. The storage medium can include a high-speed random storage medium and can also include a non-volatile storage medium, such as one or more magnetic storage systems, flash memories, or other non-volatile solid-state storage media. In some examples, the storage medium can further include storage media remotely arranged with respect to the processor, and these remote storage media can be connected to the terminal through a network. Examples of the network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and a combination thereof.

[0106] The processor can call information and application programs stored in the storage medium through the transmission system to perform the following steps: Step 1, using a redundant robot to achieve laparoscope pose control under RCM (Remote Center of Motion) constraint, designing host computer software to achieve intuitive laparoscope moving in Cartesian space; at the same time, monitoring the torque exerted by the doctor through the joint torque sensor of the robot, and combining the inductance controller to output the expected pose of the redundant robot;

[0107] Step 2, setting up a kinematics model of the S-R-S configuration redundant robot, establishing a robot joint control framework under a non-blocking process, and performing position control on the joints of the robot to complete the laparoscope moving. The kinematics model is a model of an analytical method with 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 inductance controller is:

[0109]

[0110]

[0111] wherein M is a weighting coefficient matrix set in the inductance controller, is the expected 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 the matrix C, H is a condition matrix for the redundant robot to achieve RCM constraint, and F is a matrix satisfying CF Tarbitrary basis matrix with =0, is the weighted right pseudo-inverse matrix of matrix F, D f is the damping matrix set in the redundant robot free motion space, is the joint angular velocity of the robot at the last time, τ(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 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 realizing gravity compensation, τ0 is the joint torque value generated by the gravity of the laparoscope and the robot end load when the robot end rigidly links the laparoscope, J T is the transpose matrix of the Jacobian matrix of the robot, g x , g y and g z are respectively the gravity vector components of the laparoscope and the robot end load in the x, y and z directions under the robot TCF (Tool Coordinate Frame), l x , l y and l z are respectively the distance components between the center of mass of the laparoscope and the robot end load and the TCP (Tool Center Point) in the x, y and z directions under the robot TCF, m gx , m gy and m gz are respectively the torque components generated by the gravity vector of the laparoscope and the robot end load on the robot in the x, y and z directions under the robot TCF.

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

[0118]

[0119] wherein, ​is the first order derivative of matrix C with respect to time, κ1, κ2 are set convergence parameters, x c is the projection of the directed vector formed by the RCM point and the robot end TCP connection on the plane perpendicular to the robot end axis, is x c , the first order derivative of time.

[0120] Specifically, further comprising:

[0121] The kinematics model of the S-R-S configuration redundant robot is set to calculate x c , and the convergence parameters κ1, κ2 in the admittance controller are set.

[0122] Embodiment 5

[0123] The embodiment of the application also provides a host computer software for realizing gravity compensation and collecting a lens moving instruction. When the robot end is rigidly connected with a load such as a laparoscope, the robot joint torque sensor will also record the torque caused by the load, which will seriously interfere with the measurement of the torque exerted by the doctor on the robot subsequently, and therefore the gravity of the end load must be compensated.

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

[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 realizing gravity compensation, τ (t) is the joint torque value measured by the robot joint torque sensor at time t, τ 0 is the joint torque value of the laparoscope and the robot end load due to gravity when the robot end is rigidly connected with the laparoscope, J T is the transpose matrix of the Jacobian matrix of the robot, g x , g y and g z are respectively the gravity vectors of the laparoscope and the robot end load in the x, y and z directions under the robot TCF (Tool Coordinate Frame), l x , l y and l z ​are the components of the distance between the center of mass of the laparoscope and the TCP (Tool Center Point) along x, y and z directions under the robot TCF, m gx , m gy and m gz are the components of the moment generated by the gravity vector of the laparoscope and the robot end load along x, y and z directions under the robot TCF.

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

[0129] After obtaining the laparoscope operation instruction of the doctor, the desired pose can be calculated in combination with the current pose of the robot. However, in the process of laparoscopic surgery, the lead surgeon may touch the robot or the laparoscope at the end of the robot. If the laparoscope maintains the current pose, a rigid collision will occur, which will endanger the smooth progress of the surgery and even the life safety of the patient. Therefore, the robot must be subjected to moment detection. Since we have realized the gravity compensation of the end load, the moment value measured by the joint moment sensor is the moment value applied by the doctor.

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

[0131]

[0132] wherein M is a weighting 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 the matrix C, H is a condition matrix for the redundant robot to realize the RCM constraint, F is an arbitrary basis matrix in the free motion space of the redundant robot satisfying CF T = 0, is the weighted right pseudo-inverse matrix of the matrix F, D f is a damping matrix set in the free motion space of the redundant robot, is the joint angular velocity of the robot at the last time, τ(t) is the joint torque value measured by the joint torque sensor of the robot at time t.

[0133] Specifically, the torque on the right side of the admittance controller is optimized, 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 realizing gravity compensation, τ0 is the joint torque value generated by the gravity of the laparoscope and the robot end load when the robot end rigidly links the laparoscope, J T is the transpose matrix of the Jacobian matrix of the robot, g x , g y and g z are the gravity vectors of the laparoscope and the robot end load in the x, y and z directions of the robot TCF (Tool Coordinate Frame), respectively, l 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 (Tool Center Point) in the x, y and z directions of the robot TCF, respectively, m gx , m gy and m gz are the moments generated by the gravity vectors of the laparoscope and the robot end load on the robot in the x, y and z directions of the robot TCF, respectively.

[0138] Specifically, the condition matrix H of the RCM constraint realized by the redundant robot is specifically:

[0139]

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

[0141] Specifically, further comprising:

[0142] Calculate x by setting the kinematics model of the S-R-S configuration redundant robot c Set the convergence parameters κ1, κ2 in the admittance controller.

[0143] As shown in Figure 5 After obtaining the desired pose, the robot model needs to be further set to solve the optimal joint angle. First, set the arm angle as a redundant parameter, set the robot kinematics model, and solve multiple joint angles in combination with the desired pose; then, in combination with the current joint angle, the optimal joint angle is selected according to the minimum energy principle.

[0144] As shown in Figure 2 The present application realizes the gravity compensation and collection of the distal laparoscope and load, and the identification of the movement instruction of the laparoscope input by the doctor through the design of the upper computer software. If the joint torque value measured in the joint torque sensor is less than the set joint torque threshold value, the movement instruction is output to the set robot kinematics model, and the desired pose is solved in combination with the current pose; otherwise, if the joint torque value measured in the joint torque sensor is greater than the set joint torque threshold value, it is considered that the main doctor has contacted or even collided with the robot and the laparoscope at this time, and the joint torque value is output to the admittance controller, and the desired pose is solved in combination with the current pose. The desired joint angle is obtained by inputting the desired pose into the robot kinematics model, a non-blocking process robot control framework is built, and the robot laparoscope task is realized through position control.

[0145] The above-mentioned embodiment numbers of the present application are only for description, and do not represent the advantages and disadvantages of the embodiments.

[0146] In the above-mentioned embodiments of the present application, the description of each embodiment has its own emphasis, and the parts not described in detail in a certain embodiment can be referred to the related description of other embodiments.

[0147] In the several embodiments provided by the present application, it should be understood that the disclosed technology can be implemented in other ways. Of course, the embodiments described above are only schematic. For example, the division of units is only a logical function division, and there can be another division manner in actual implementation. For example, a plurality of units or components can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the coupling or direct coupling or communication connection between the units shown or discussed can be indirect coupling or communication connection through some interfaces, units or modules, and can be electrical or other forms.

[0148] The units described as separate components may or may not be physically separate, and the components displayed as units may or may not be physical units, that is, may be located in one place, or may be distributed to multiple network units. Part or all of the units can be selected according to actual needs to achieve the purpose of the embodiment scheme.

[0149] In addition, each functional unit in each embodiment of the present application can be integrated in one processing unit, or each unit can be physically present alone, or two or more units can be integrated in one unit. The integrated unit can be realized in the form of hardware or in the form of a software functional unit.

[0150] The integrated unit, if realized in the form of a software functional unit and sold or used as an independent product, can be stored in a computer-readable storage medium. Based on this understanding, the technical solutions of the present application or the part of the prior art that contributes essentially or the whole or part of the technical solutions can be embodied in the form of a software product, which is stored in a storage medium and includes a plurality of instructions for causing a computer device (which can be a personal computer, a server or a network device, etc.) to execute all or part of the steps of the method described in each embodiment of the present application. The foregoing storage medium includes a U disk, a read-only storage medium (ROM, Read-Only Memory), a random access storage medium (RAM, Random Access Memory), a mobile hard disk, a magnetic disk or an optical disk, and various program code storage media.

[0151] Those skilled in the art will readily understand that the above description is only a preferred embodiment of the present application and is not intended to limit the present application, and any modifications, equivalent replacements and improvements made within the spirit and principles of the present application shall be included in the protection scope of the present application.

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 robot's desired posture is calculated by combining the admittance controller and the current posture. Otherwise, the robot's desired posture is calculated by combining the current posture and the mirror movement command. Step 3: Calculate the expected joint angles according to the expected posture, and construct 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 based on the kinematic model to control the position of the robot joints, thereby realizing laparoscopic mirror movement.

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

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

4. The laparoscopic mirror operation method in combination with an admittance controller according to claim 1, characterized in that: In step 1, the condition matrix H for achieving laparoscopic posture control under RCM constraints includes: in, is the first-order derivative of the 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 of the robot end on the plane perpendicular to the axis of the robot end. is 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 condition matrix H for the robot to implement RCM constraints, 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 value τ(t) measured by the joint torque sensor at time t is used to construct the admittance controller.

6. The laparoscopic mirror operation method in combination 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 matrix F.

7. The 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, τ0 is the joint torque value of the laparoscope and the robot end load due to gravity when the robot end is rigidly connected to the laparoscope, 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 along the x, y, and z directions under the robot TCF, respectively, m gx 、m gy and m gz 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 kinematic 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 is used to use the robot to realize the laparoscopic posture control under the RCM constraint; 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 desired joint angles according to the desired posture, and constructs a robot kinematic model with the arm angles as redundant parameters; The fourth control module is used to 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.

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