A laparoscopic surgery robot control method, system and storage medium

Abstract

The application provides a laparoscopic surgery robot control method, system and storage medium, and relates to the technical field of medical instruments.The method comprises the following steps: acquiring a control instruction;when the control instruction is a rotating lens instruction, closing a moving motor, acquiring a projection variable of a first main controller and a second main controller, and controlling a rotating motor according to the projection variable; when the control instruction is a moving lens instruction, closing the rotating motor, acquiring a space matrix variable of the first main controller and the second main controller, and controlling the moving motor according to the space matrix variable.According to different control instructions, different control strategies are selected, different projection variables and space matrix variables are acquired, and corresponding motors are controlled, so that the problem that only a rotating action occurs when a doctor wants to move a lens holding arm lens or only a moving action occurs when the doctor rotates the lens holding arm lens due to operation errors or other subjective factors can be effectively solved, the safety of surgery is ensured and improved.

Description

Technical Field

[0001] This invention relates to the field of medical device technology, and more specifically, to a control method, system, and storage medium for a laparoscopic surgical robot. Background Technology

[0002] Laparoscopic surgical robots are used in clinical laparoscopic surgery. Compared with manual laparoscopic surgery, laparoscopic surgical robots can reduce patient pain and reduce the workload of doctors, thereby improving the success rate of surgery.

[0003] In existing technologies, laparoscopic surgical robots control the movement or rotation of the endoscope arm lens by receiving control commands from the surgeon on the main hand controller, thereby performing related surgical procedures. However, due to surgeon errors or other subjective factors, the main hand controller may have slight deviations, potentially causing rotational movements when only the endoscope arm lens needs to be moved, or movement when only the endoscope arm lens needs to be rotated, thus affecting the safety of the surgery. Summary of the Invention

[0004] The problem addressed by this invention is how to improve the safety of operating laparoscopic surgical robots.

[0005] To address the aforementioned problems, in a first aspect, the present invention provides a method for controlling a laparoscopic surgical robot, comprising:

[0006] Obtain control commands;

[0007] When the control command is a lens rotation command, the system enters the lens rotation arm rotation mode, shuts down the movement motor, acquires the projection variables of the first main controller and the second main controller, and controls the rotation motor according to the projection variables. The first main controller and the second main controller are used to map the doctor's rotation movements.

[0008] When the control command is a lens movement command, the system enters the lens movement arm movement mode, shuts down the rotary motor, acquires the spatial matrix variables of the first main controller and the second main controller, and controls the movement motor according to the spatial matrix variables. The first main controller and the second main controller are used to map the doctor's movement actions.

[0009] Optionally, obtaining the projection variables of the first main controller and the second main controller on the display screen plane includes:

[0010] When entering the rotating lens arm lens mode, the first main controller and the second main controller respectively obtain the first initial coordinates in the display screen space coordinate system;

[0011] Based on the first initial coordinates, generate the initial projections of the first main controller and the second main controller on the display screen plane;

[0012] After entering the rotating lens arm lens mode, the rotation coordinates of the first main controller and the second main controller in the display screen space coordinate system are obtained respectively.

[0013] Based on the rotation coordinates, generate rotational projections of the first main controller and the second main controller on the display screen plane;

[0014] The projection variable is generated based on the initial projection, the rotated projection, and the projection calculation formula, wherein the projection calculation formula includes:

[0015]

[0016] Where, |θ ty | is the projection variable, A LR (1) is the vector of the initial projection in the x-axis direction, A LR (2) is the vector of the initial projection in the y-axis direction, B LR (1) is the vector of the rotational projection in the x-axis direction, B LR (2) is the vector of the rotational projection in the y-axis direction.

[0017] Optionally, controlling the rotary motor according to the projection variable includes:

[0018] Based on the projection variables and the lens rotation formula, the lens rotation angle is generated. The lens rotation formula includes:

[0019] Theta8 = Sign(A LR (1)*B LR (2)-A LR (2)*B LR (2))*|θ ty |;

[0020] Where Theta8 is the lens rotation angle, Sign is the sign function, and A LR (1) is the vector of the initial projection in the x-axis direction, A LR (2) is the vector of the initial projection in the y-axis direction, B LR (1) is the vector of the rotational projection in the x-axis direction, B LR (2) is the vector of the rotational projection in the y-axis direction, |θ ty | represents the projection variable;

[0021] The rotary motor is controlled according to the lens rotation angle.

[0022] Optionally, obtaining the spatial matrix variables of the first main controller and the second main controller in the display screen spatial coordinate system includes:

[0023] When entering the lens mode of the mobile lens arm, the first main controller and the second main controller respectively obtain the second initial coordinates in the display space coordinate system and generate an initial matrix column;

[0024] Obtain the initial pose matrix column of the lens-holding arm;

[0025] Generate the laparoscopic initial matrix column based on the initial matrix column and the initial pose matrix column;

[0026] After entering the moving lens arm mode, the first main controller and the second main controller respectively obtain the movement coordinates in the display screen space coordinate system and generate a movement matrix column;

[0027] Generate a laparoscopic movement matrix column based on the initial pose matrix column and the movement matrix column;

[0028] The spatial matrix variable is generated by performing a difference operation on the initial matrix column of the laparoscopy and the moving matrix column of the laparoscopy.

[0029] Optionally, controlling the moving motor according to the spatial matrix variables includes:

[0030] Based on the spatial matrix variables, generate a relative pose matrix column;

[0031] Based on the relative pose matrix and the first angle calculation formula, the rotational joint angle is generated, wherein the first angle calculation formula includes:

[0032] Theta6=atan2(H[2,4],H[1,4]);

[0033] Where Theta6 is the rotational joint angle, H[2,4] is the y-axis matrix of the relative pose matrix column, and H[1,4] is the x-axis matrix of the relative pose matrix column;

[0034] Based on the relative pose matrix column, the rotational joint angle, and the second angle calculation formula, the pitch joint angle is generated. The second angle calculation formula includes:

[0035] Theta7=atan2(H[3,4]-d6,H[1,4]*cos(Theta6)+H[2,4]*sin(Theta6));

[0036] Where Theta7 is the pitch joint angle, Theta6 is the rotation joint angle, H[1,4] is the x-axis matrix of the relative pose matrix column, H[2,4] is the y-axis matrix of the relative pose matrix column, and H[3,4] is the z-axis matrix of the relative pose matrix column.

[0037] The inverse kinematic distance of the slide joint is generated based on the rotational joint angle and the pitch joint angle.

[0038] The moving motor is controlled based on the rotation joint angle, and / or the pitch joint angle, and / or the inverse kinematic distance of the slide joint.

[0039] Optionally, generating the inverse kinematic distance of the slide joint based on the rotational joint angle and the pitch joint angle includes:

[0040] When the pitch joint angle is not equal to 0, the inverse kinematic distance of the slide joint is generated according to the first distance calculation formula, which includes:

[0041] d9 = H[3,4] - d6 / sin(Theta7);

[0042] Wherein, d9 is the inverse kinematic distance of the slide joint, H[3,4] is the z-axis matrix of the relative pose matrix column, d6 is the distance from the elbow joint of the lens-holding arm to the telecentric fixed point of the lens-holding arm, and Theta7 is the pitch joint angle;

[0043] When the pitch joint angle is equal to 0 and the rotation joint angle is not equal to 0, the inverse kinematic distance of the slide joint is generated according to the second distance calculation formula, which includes:

[0044] d9=H[1,4] / cos(Theta6) / cos(Theta7);

[0045] Wherein, d9 is the inverse kinematic distance of the slide joint, H[1,4] is the x-axis matrix of the relative pose matrix column, Theta6 is the rotation joint angle, and Theta7 is the pitch joint angle;

[0046] When the pitch joint angle is equal to 0 and the rotation joint angle is equal to 0, the inverse kinematic distance of the slide joint is generated according to the third distance calculation formula, which includes:

[0047] d9=H[2,4] / sin(Theta6) / cos(Theta7);

[0048] Wherein, d9 is the inverse kinematic distance of the slide joint, H[2,4] is the y-axis matrix of the relative pose matrix column, Theta6 is the rotation joint angle, and Theta7 is the pitch joint angle.

[0049] Optionally, controlling the rotary motor according to the lens rotation angle includes:

[0050] When the lens rotation angle is greater than 0, the rotary motor is controlled to rotate forward, and the operating power of the rotary motor is controlled according to the absolute value of the lens rotation angle;

[0051] When the lens rotation angle is less than 0, the rotary motor is controlled to reverse, and the operating power of the rotary motor is controlled according to the absolute value of the lens rotation angle.

[0052] Optionally, controlling the rotary motor according to the lens rotation angle includes:

[0053] When the lens rotation angle is greater than 0, the rotary motor is controlled to reverse, and the operating power of the rotary motor is controlled according to the absolute value of the lens rotation angle;

[0054] When the lens rotation angle is less than 0, the rotary motor is controlled to rotate forward, and the operating power of the rotary motor is controlled according to the absolute value of the lens rotation angle.

[0055] In a second aspect, the present invention provides a laparoscopic surgical robot control system, comprising:

[0056] The acquisition module is used to acquire control commands;

[0057] The rotation module is used to enter the rotating lens arm mode, turn off the moving motor, obtain the projection variables of the first main controller and the second main controller on the display screen plane when the control command is a rotating lens command, and control the rotating motor according to the projection variables. The first main controller and the second main controller are used to map the doctor's rotation action.

[0058] The movement module is used to enter the lens-holding arm movement mode when the control command is a lens movement command, turn off the rotation motor, obtain the spatial matrix variables of the first main controller and the second main controller in the display screen spatial coordinate system, and control the movement motor according to the spatial matrix variables, wherein the first main controller and the second main controller are used to map the doctor's movement actions.

[0059] Thirdly, the present invention provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the laparoscopic surgical robot control method described above.

[0060] The beneficial effects of the laparoscopic surgical robot control method, system, and storage medium provided by this invention are as follows:

[0061] This invention selects different control strategies based on different control commands. When the control command is a lens rotation command, the movement motor controlling the movement of the endoscope arm lens is turned off; when the control command is a lens movement command, the rotation motor controlling the rotation of the endoscope arm lens is turned off. This effectively solves the problem that slight deviations in the main controller due to surgeon error or other subjective factors may cause rotational movements when only the endoscope arm lens is moved, or movement movements when only the endoscope arm lens is rotated, thus ensuring surgical safety. Furthermore, by obtaining the projection variables of the first and second main controllers on the laparoscopic surgical robot's display screen, the desired rotation angle can be accurately obtained, thereby controlling the rotation motor and improving surgical safety. Similarly, by obtaining the spatial matrix variables of the first and second main controllers within the laparoscopic surgical robot's display screen's spatial coordinate system, the desired movement direction and distance can be accurately obtained, thereby controlling the rotation motor and improving surgical safety. Attached Figure Description

[0062] Figure 1 A flowchart illustrating the laparoscopic surgical robot control method provided in an embodiment of the present invention;

[0063] Figure 2 This is a schematic diagram of the laparoscopic surgical robot control system provided in an embodiment of the present invention. Detailed Implementation

[0064] To make the above-mentioned objects, features, and advantages of the present invention more apparent and understandable, specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. Although some embodiments of the present invention are shown in the drawings, it should be understood that the present invention can be implemented in various forms and should not be construed as limited to the embodiments set forth herein. Rather, these embodiments are provided to provide a more thorough and complete understanding of the present invention. It should be understood that the accompanying drawings and embodiments of the present invention are for illustrative purposes only and are not intended to limit the scope of protection of the present invention.

[0065] It should be understood that the various steps described in the method embodiments of the present invention may be performed in different orders and / or in parallel. Furthermore, the method embodiments may include additional steps and / or omit the steps shown. The scope of the present invention is not limited in this respect.

[0066] The term "comprising" and its variations as used herein are open-ended, meaning "including but not limited to". The term "based on" means "at least partially based on". The term "one embodiment" means "at least one embodiment"; the term "another embodiment" means "at least one additional embodiment"; the term "some embodiments" means "at least some embodiments"; the term "optionally" means "optional embodiments". Definitions of other terms will be given in the following description. It should be noted that the concepts of "first", "second", etc., mentioned in this invention are used only to distinguish different devices, modules, or units, and are not intended to limit the order of functions performed by these devices, modules, or units or their interdependencies.

[0067] It should be noted that the terms "a" and "a plurality of" used in this invention are illustrative rather than restrictive. Those skilled in the art should understand that, unless explicitly stated otherwise in the context, they should be understood as "one or more". The concepts of "first" and "second" used in this invention are only used to distinguish different devices, systems, or units, and are not used to limit the order or interdependence of the functions performed by these devices, systems, or units.

[0068] To address the above problems, in one embodiment, such as Figure 1 As shown, the present invention provides a method for controlling a laparoscopic surgical robot, comprising:

[0069] Step S1: Obtain control commands;

[0070] Specifically, a default operating mode and control commands can be set. During initial operation, the laparoscopic surgical robot operates in the default mode. Upon receiving a control command from the surgeon, the robot switches from the default mode to the mode corresponding to the control command. For example, if the default operating mode is the moving arm lens mode and the control command is set to double-click the main gripper of the laparoscopic surgical robot, the robot switches from the moving arm lens mode to the rotating arm lens mode upon receiving the double-click command. Alternatively, if the default operating mode is the rotating arm lens mode and the control command is set to double-click the main gripper, the robot switches from the rotating arm lens mode to the moving arm lens mode upon receiving the double-click command. The control commands can be set according to the actual situation.

[0071] Step S2: When the control command is a lens rotation command, enter the lens rotation arm rotation mode, turn off the moving motor, obtain the projection variables of the first main controller and the second main controller on the display screen plane, and control the rotation motor according to the projection variables. The first main controller and the second main controller are used to map the doctor's rotation action.

[0072] Specifically, when the control command is a lens rotation command, the system enters the lens rotation mode, shuts down the motor controlling the movement of the laparoscopic surgical robot's lens arm, and acquires the projection variables of the first and second main controllers on the laparoscopic surgical robot's display screen. The rotation motor is then controlled based on these projection variables. In the lens rotation mode, the first and second main controllers respond to the surgeon's rotational movements, thereby controlling the rotation of the laparoscopic surgical robot's lens arm. The first and second main controllers receive external input commands, such as the surgeon's handle operations, and translate them into movements of the laparoscopic surgical robot's lens arm. That is, when the surgeon rotates the first and second main controllers, the first and second main controllers rotate accordingly, causing changes in their pose parameters. The projection of these changes on the display screen is the projection variable. By acquiring the projection variables of the first and second main controllers, the surgeon's rotational movements can be converted into physical signals, thereby controlling the rotation motor.

[0073] For example, control strategies such as PID control, model predictive control, forward kinematics, inverse kinematics, and force / torque control can be used to control the rotary motor; the first main controller and the second main controller can not only respond to the doctor's rotational movements, but also to the doctor's movement movements or other surgical operations; and the first main controller and the second main controller can be designed according to the actual situation, for example, the first main controller can be set as the left-hand controller of the laparoscopic surgical robot and the second main controller can be set as the right-hand controller of the laparoscopic surgical robot, or the first main controller can be set as the operating handle and the second main controller can be set as the control pedal.

[0074] Step S3: When the control command is a lens movement command, enter the lens movement arm movement mode, turn off the rotating motor, obtain the spatial matrix variables of the first main controller and the second main controller in the display screen spatial coordinate system, and control the moving motor according to the spatial matrix variables, wherein the first main controller and the second main controller are used to map the doctor's movement actions.

[0075] Specifically, when the control command is a camera movement command, the system enters the camera movement arm movement mode. The rotary motor controlling the rotation of the laparoscopic surgical robot's camera movement arm is shut down, and the spatial matrix variables of the first and second main controllers in the laparoscopic surgical robot's display screen coordinate system are acquired. The movement motor is then controlled based on these spatial matrix variables. In camera movement arm movement mode, the first and second main controllers respond to the surgeon's movements, thereby controlling the movement of the laparoscopic surgical robot's camera movement arm. The first and second main controllers receive external input commands, such as the surgeon's handle operations, and translate them into movements of the laparoscopic surgical robot's camera movement arm. That is, the surgeon moves the first and second main controllers, and the first and second main controllers follow the movement, causing changes in their pose parameters. The changes in these pose parameters in the display screen coordinate system are the spatial matrix variables. By acquiring the spatial matrix variables of the first and second main controllers, the surgeon's movements can be converted into physical signals, thereby controlling the movement motor.

[0076] For example, control strategies such as PID control, model predictive control, forward kinematics, inverse kinematics, and force / torque control can be used to control the moving motor.

[0077] By setting default operating modes and control commands, such as double-clicking the main hand gripper of the laparoscopic surgical robot, the control mode can be easily switched. Different operating modes and control strategies can be selected based on the control commands. When the control command is to rotate the lens, the robot enters the lens rotation mode, and the movement motor controlling the lens movement is turned off. Conversely, when the control command is to move the lens, the robot enters the lens movement mode, and the rotation motor controlling the lens rotation is turned off. This effectively solves the problem of slight deviations in the main hand controller due to surgeon errors or other subjective factors, which might result in rotational movement when only the lens is moved, or movement when only the lens is rotated, thus ensuring surgical safety. Furthermore, by obtaining the projection variables of the first and second main controllers on the laparoscopic surgical robot's display screen, the desired rotation angle can be accurately obtained, thereby controlling the rotation motor and improving surgical safety. Finally, by obtaining the spatial matrix variables of the first and second main controllers within the spatial coordinate system of the laparoscopic surgical robot's display screen, the desired movement direction and distance can be accurately obtained, thereby controlling the rotation motor and improving surgical safety.

[0078] Optionally, obtaining the projection variables of the first main controller and the second main controller on the display screen plane includes:

[0079] When entering the rotating lens arm lens mode, the first main controller and the second main controller respectively obtain the first initial coordinates in the display screen space coordinate system;

[0080] Based on the first initial coordinates, generate the initial projections of the first main controller and the second main controller on the display screen plane;

[0081] After entering the rotating lens arm lens mode, the rotation coordinates of the first main controller and the second main controller in the display screen space coordinate system are obtained respectively.

[0082] Based on the rotation coordinates, generate rotational projections of the first main controller and the second main controller on the display screen plane;

[0083] The projection variable is generated based on the initial projection, the rotated projection, and the projection calculation formula, wherein the projection calculation formula includes:

[0084]

[0085] Where, |θ ty | is the projection variable, A LR (1) is the vector of the initial projection in the x-axis direction, A LR (2) is the vector of the initial projection in the y-axis direction, B LR (1) is the vector of the rotational projection in the x-axis direction, B LR (2) is the vector of the rotational projection in the y-axis direction.

[0086] Specifically, the first initial coordinates are the position and orientation of the lens arm mapped by the first main controller and the second main controller when the lens arm is just entered into the rotating lens arm mode. The first initial coordinates of the first main controller are AL = [x L2 ;y L2 ;z L2 The first initial coordinates of the second main controller are AR = [x] R2 ;y R2 ;z R2 The initial projection, corresponding to the x, y, and z axes in the spatial coordinate system, is calculated based on the first initial coordinates of the first main controller and the first initial coordinates of the second main controller. This initial projection is the line vector A connecting the first and second main controllers. LR =[x R2 -x L2 ;y R2 -y L2 ;0],A LR (1) is the vector of the initial projection along the x-axis, i.e., x R2 -x L2A LR (2) is the vector of the initial projection along the y-axis, i.e., y R2 -y L2 B LR (1) and B LR (2) Similarly, this will not be elaborated further. After the doctor rotates the first and second main controllers by a certain angle, the rotation coordinates of the first main controller are BL = [x...]. L1 ;y R1 L;z L1 The rotation coordinates of the second main controller are BR = [x] R1 ;y R1 ;z R1 The rotation projection, i.e., the vector B connecting the rotation coordinates of the first and second main controllers, is calculated based on the first initial coordinates of the first main controller and the rotation coordinates of the second main controller. LR =[x R1 -x L1 ;y R1 -y L1 Finally, the projection variable is generated according to the projection calculation formula.

[0087] For example, the projected variable can also be calculated using the dot product formula, which includes:

[0088]

[0089] Optionally, controlling the rotary motor according to the projection variable includes:

[0090] Based on the projection variables and the lens rotation formula, the lens rotation angle is generated. The lens rotation formula includes:

[0091] Theta8 = Sign(A LR (1)*B LR (2)-A LR (2)*B LR (1))*|θ ty |;

[0092] Where Theta8 is the lens rotation angle, Sign is the sign function, and A LR (1) is the vector of the initial projection in the x-axis direction, A LR (2) is the vector of the initial projection in the y-axis direction, B LR (1) is the vector of the rotational projection in the x-axis direction, B LR (2) is the vector of the rotational projection in the y-axis direction, |θ ty | represents the projection variable;

[0093] The rotary motor is controlled according to the lens rotation angle.

[0094] Specifically, the lens rotation angle can be calculated using the lens rotation formula. Based on the lens rotation angle, the specific numerical value and direction of the lens rotation can be determined. The sign of Theta8 represents the direction of lens rotation, and the absolute value of Theta8 represents the specific numerical value of the lens rotation. The Sign function of the lens rotation formula is based on (A... LR (1)*B LR (2)-A LR (2)*B LR (1)) Outputs a positive or negative sign.

[0095] Optionally, obtaining the spatial matrix variables of the first main controller and the second main controller in the display screen spatial coordinate system includes:

[0096] When entering the lens mode of the mobile lens arm, the first main controller and the second main controller respectively obtain the second initial coordinates in the display space coordinate system and generate an initial matrix column;

[0097] Obtain the initial pose matrix column of the lens-holding arm;

[0098] Generate the laparoscopic initial matrix column based on the initial matrix column and the initial pose matrix column;

[0099] After entering the moving lens arm mode, the first main controller and the second main controller respectively obtain the movement coordinates in the display screen space coordinate system and generate a movement matrix column;

[0100] Generate a laparoscopic movement matrix column based on the initial pose matrix column and the movement matrix column;

[0101] The spatial matrix variable is generated by performing a difference operation on the initial matrix column of the laparoscopy and the moving matrix column of the laparoscopy.

[0102] Specifically, the second initial coordinates are the position and posture of the scope arm mapped by the first and second main controllers when the scope arm is first moved into the lens movement mode. The second initial coordinates of the first and second main controllers are obtained respectively, converted into second initial coordinate matrices, and the two second initial coordinate matrices are added to generate an initial matrix column. The initial matrix column includes: TempX = C[1,4]; TempY = C[2,4]; TempZ = C[3,4]. The initial pose matrix column of the scope arm is the pose matrix of the scope arm end relative to the scope arm elbow joint when the scope arm is first moved into the lens movement mode. It includes: D[1,4]; D[2,4]; D[3,4]. Based on the initial matrix column and the initial pose matrix column, the laparoscopic initial matrix column is generated, which is the pose matrix of the scope arm lens end relative to the lens end when the scope arm is first moved into the lens movement mode. The formula for generating the laparoscopic initial matrix column includes:

[0103] E[1,4]=-(D[1,1]*TempX+D[1,2]*TempY+D[1,3]*TempZ);

[0104] E[2,4]=-(D[2,1]*TempX+D[2,2]*TempY+D[2,3]*TempZ);

[0105] E[3,4]=-(D[3,1]*TempX+D[3,2]*TempY+D[3,3]*TempZ);

[0106] The movement coordinates of the first and second main controllers are acquired in real time, and a movement matrix column is generated. Similar to the initial matrix column, the laparoscopic movement matrix column is generated based on the initial pose matrix column and the movement matrix column. This represents the real-time pose matrix of the end of the laparoscopic arm relative to the end of the lens. The formula for generating the laparoscopic movement matrix column includes:

[0107] F[1,4]=-(D[1,1]*TempX+D[1,2]*TempY+D[1,3]*TempZ);

[0108] F[2,4]=-(D[2,1]*TempX+D[2,2]*TempY+D[2,3]*TempZ);

[0109] F[3,4]=-(D[3,1]*TempX+D[3,2]*TempY+D[3,3]*TempZ);

[0110] Finally, a difference operation is performed on the initial matrix column and the moving matrix column of the laparoscopy to generate a spatial matrix variable. The difference operation formula includes: G = FE, where G is the spatial matrix variable, F is the variable, and E is the variable.

[0111] Optionally, controlling the moving motor according to the spatial matrix variables includes:

[0112] Based on the spatial matrix variables, generate a relative pose matrix column;

[0113] Based on the relative pose matrix and the first angle calculation formula, the rotational joint angle is generated, wherein the first angle calculation formula includes:

[0114] Theta6=atan2(H[2,4],H[1,4]);

[0115] Where Theta6 is the rotational joint angle, H[2,4] is the y-axis matrix of the relative pose matrix column, and H[1,4] is the x-axis matrix of the relative pose matrix column;

[0116] Based on the relative pose matrix column, the rotational joint angle, and the second angle calculation formula, the pitch joint angle is generated. The second angle calculation formula includes:

[0117] Theta7=atan2(H[3,4]-d6,H[1,4]*cos(Theta6)+H[2,4]*sin(Theta6));

[0118] Where Theta7 is the pitch joint angle, Theta6 is the rotation joint angle, H[1,4] is the x-axis matrix of the relative pose matrix column, H[2,4] is the y-axis matrix of the relative pose matrix column, and H[3,4] is the z-axis matrix of the relative pose matrix column.

[0119] The inverse kinematic distance of the slide joint is generated based on the rotational joint angle and the pitch joint angle.

[0120] The moving motor is controlled based on the rotation joint angle, and / or the pitch joint angle, and / or the inverse kinematic distance of the slide joint.

[0121] Specifically, based on the spatial matrix variables, a relative pose matrix column is generated, namely, the pose matrix of the lens end of the lens-holding arm relative to the elbow joint of the lens-holding arm. The rotational joint angle and pitch joint angle are obtained using the first and second angle calculation formulas, and the inverse kinematic distance of the slide joint is generated. Finally, the moving motor is controlled based on the rotational joint angle and / or the pitch joint angle and / or the inverse kinematic distance of the slide joint. For example, the rotational joint angle and pitch joint angle can be negative values, i.e., the inverse kinematic angles of the rotational joint and / or the pitch joint, and the moving motor is controlled based on these inverse kinematic angles and / or the pitch joint.

[0122] Optionally, generating the inverse kinematic distance of the slide joint based on the rotational joint angle and the pitch joint angle includes:

[0123] When the pitch joint angle is not equal to 0, the inverse kinematic distance of the slide joint is generated according to the first distance calculation formula, which includes:

[0124] d9 = H[3,4] - d6 / sin(Theta7);

[0125] Wherein, d9 is the inverse kinematic distance of the slide joint, H[3,4] is the z-axis matrix of the relative pose matrix column, d6 is the distance from the elbow joint of the lens-holding arm to the telecentric fixed point of the lens-holding arm, and Theta7 is the pitch joint angle;

[0126] When the pitch joint angle is equal to 0 and the rotation joint angle is not equal to 0, the inverse kinematic distance of the slide joint is generated according to the second distance calculation formula, which includes:

[0127] d9=H[1,4] / cos(Theta6) / cos(Theta7);

[0128] Wherein, d9 is the inverse kinematic distance of the slide joint, H[1,4] is the x-axis matrix of the relative pose matrix column, Theta6 is the rotation joint angle, and Theta7 is the pitch joint angle;

[0129] When the pitch joint angle is equal to 0 and the rotation joint angle is equal to 0, the inverse kinematic distance of the slide joint is generated according to the third distance calculation formula, which includes:

[0130] d9=H[2,4] / sin(Theta6) / cos(Theta7);

[0131] Wherein, d9 is the inverse kinematic distance of the slide joint, H[2,4] is the y-axis matrix of the relative pose matrix column, Theta6 is the rotation joint angle, and Theta7 is the pitch joint angle.

[0132] Specifically, based on the magnitudes of the rotational and pitch joint angles, different distance calculation formulas are used to generate the inverse kinematic distance of the slide joint.

[0133] Optionally, controlling the rotary motor according to the lens rotation angle includes:

[0134] When the lens rotation angle is greater than 0, the rotary motor is controlled to rotate forward, and the operating power of the rotary motor is controlled according to the absolute value of the lens rotation angle;

[0135] When the lens rotation angle is less than 0, the rotary motor is controlled to reverse, and the operating power of the rotary motor is controlled according to the absolute value of the lens rotation angle.

[0136] Specifically, when the lens rotation angle is greater than 0, the rotary motor is controlled to rotate clockwise, causing the lens to rotate counterclockwise, and the operating power of the rotary motor is controlled according to the absolute value of the lens rotation angle; when the lens rotation angle is less than 0, the rotary motor is controlled to rotate counterclockwise, causing the lens to rotate clockwise, and the operating power of the rotary motor is controlled according to the absolute value of the lens rotation angle. For example, the rotation direction and operating power of the rotary motor can be controlled by setting the lens rotation angle according to actual conditions, and the speed and number of revolutions of the rotary motor can also be controlled according to the absolute value of the lens rotation angle, thereby achieving motor control.

[0137] Optionally, controlling the rotary motor according to the lens rotation angle includes:

[0138] When the lens rotation angle is greater than 0, the rotary motor is controlled to reverse, and the operating power of the rotary motor is controlled according to the absolute value of the lens rotation angle;

[0139] When the lens rotation angle is less than 0, the rotary motor is controlled to rotate forward, and the operating power of the rotary motor is controlled according to the absolute value of the lens rotation angle.

[0140] Specifically, when the lens rotation angle is greater than 0, the rotary motor is controlled to reverse, causing the lens to rotate counterclockwise, and the operating power of the rotary motor is controlled according to the absolute value of the lens rotation angle. When the lens rotation angle is less than 0, the rotary motor is controlled to rotate clockwise, causing the lens to rotate clockwise, and the operating power of the rotary motor is controlled according to the absolute value of the lens rotation angle. Alternatively, when the lens rotation angle is greater than 0, the rotary motor is controlled to reverse, causing the lens to rotate clockwise, and the operating power of the rotary motor is controlled according to the absolute value of the lens rotation angle; when the lens rotation angle is less than 0, the rotary motor is controlled to rotate clockwise, causing the lens to rotate counterclockwise, and the operating power of the rotary motor is controlled according to the absolute value of the lens rotation angle. For example, the rotation direction and operating power of the rotary motor can be set according to the actual situation, and the rotation speed and number of revolutions of the rotary motor can also be controlled according to the absolute value of the lens rotation angle, thereby achieving motor control.

[0141] In another embodiment, such as Figure 2 As shown, the present invention provides a laparoscopic surgical robot control system, comprising:

[0142] The acquisition module is used to acquire control commands;

[0143] The rotation module is used to enter the rotating lens arm mode, turn off the moving motor, obtain the projection variables of the first main controller and the second main controller on the display screen plane when the control command is a rotating lens command, and control the rotating motor according to the projection variables. The first main controller and the second main controller are used to map the doctor's rotation action.

[0144] The movement module is used to enter the lens-holding arm movement mode when the control command is a lens movement command, turn off the rotation motor, obtain the spatial matrix variables of the first main controller and the second main controller in the display screen spatial coordinate system, and control the movement motor according to the spatial matrix variables, wherein the first main controller and the second main controller are used to map the doctor's movement actions.

[0145] In yet another embodiment, the present invention provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the laparoscopic surgical robot control method as described above.

[0146] While the present invention has been disclosed above, its scope of protection is not limited thereto. Those skilled in the art can make various changes and modifications without departing from the spirit and scope of the present invention, and all such changes and modifications will fall within the scope of protection of the present invention.

Claims

1. A laparoscopic surgical robot control system, characterized in that, include: The acquisition module is used to acquire control commands; A rotation module is used to enter a rotating lens arm mode when the control command is a rotating lens command, shut down the movement motor, acquire the projection variables of the first main controller and the second main controller on the display screen plane, and control the rotating motor according to the projection variables. The first and second main controllers are used to map the doctor's rotational movements. Acquiring the projection variables of the first and second main controllers on the display screen plane includes: acquiring the first initial coordinates of the first and second main controllers in the display screen space coordinate system when entering the rotating lens arm mode; generating the initial projection of the first and second main controllers on the display screen plane based on the first initial coordinates; acquiring the rotation coordinates of the first and second main controllers in the display screen space coordinate system after entering the rotating lens arm mode; generating the rotation projection of the first and second main controllers on the display screen plane based on the rotation coordinates; and generating the projection variables based on the initial projection, the rotation projection, and a projection calculation formula, wherein the projection calculation formula includes: ； in, Let A be the projection variable. LR (1) is the vector of the initial projection in the x-axis direction, A LR (2) is the vector of the initial projection in the y-axis direction, B LR (1) is the vector of the rotational projection in the x-axis direction, B LR (2) is the vector of the rotational projection in the y-axis direction; A movement module is used to enter a moving endoscope arm lens mode when the control command is a moving lens command, shut down the rotating motor, acquire the spatial matrix variables of the first main controller and the second main controller in the display screen spatial coordinate system, and control the movement motor according to the spatial matrix variables. The first and second main controllers are used to map the doctor's movement actions. Acquiring the spatial matrix variables of the first and second main controllers in the display screen spatial coordinate system includes: acquiring the second initial coordinates of the first and second main controllers in the display screen spatial coordinate system when entering the moving endoscope arm lens mode, and generating an initial matrix column; acquiring the initial pose matrix column of the endoscope arm; generating a laparoscopic initial matrix column based on the initial matrix column and the initial pose matrix column; acquiring the movement coordinates of the first and second main controllers in the display screen spatial coordinate system after entering the moving endoscope arm lens mode, and generating a movement matrix column; generating a laparoscopic movement matrix column based on the initial pose matrix column and the movement matrix column; and performing a difference operation on the laparoscopic initial matrix column and the laparoscopic movement matrix column to generate the spatial matrix variables.

2. The laparoscopic surgical robot control system according to claim 1, characterized in that, The step of controlling the rotating motor according to the projection variable includes: Based on the projection variables and the lens rotation formula, the lens rotation angle is generated. The lens rotation formula includes: ； Where Theta8 is the lens rotation angle, Sign is the sign function, and A LR (1) is the vector of the initial projection in the x-axis direction, A LR (2) is the vector of the initial projection in the y-axis direction, B LR (1) is the vector of the rotational projection in the x-axis direction, B LR (2) is the vector of the rotational projection in the y-axis direction. For the projection variable; The rotary motor is controlled according to the lens rotation angle.

3. The laparoscopic surgical robot control system according to claim 1, characterized in that, The step of controlling the moving motor according to the spatial matrix variables includes: Based on the spatial matrix variables, generate a relative pose matrix column; Based on the relative pose matrix and the first angle calculation formula, the rotational joint angle is generated, wherein the first angle calculation formula includes: Theta6=atan2(H[2,4],H[1,4]); Where Theta6 is the rotational joint angle, H[2,4] is the y-axis matrix of the relative pose matrix column, and H[1,4] is the x-axis matrix of the relative pose matrix column; Based on the relative pose matrix column, the rotational joint angle, and the second angle calculation formula, the pitch joint angle is generated. The second angle calculation formula includes: Theta7=atan2(H[3,4]-d6,H[1,4]*cos(Theta6)+H[2,4]*sin(Theta6)); Where Theta7 is the pitch joint angle, Theta6 is the rotation joint angle, H[1,4] is the x-axis matrix of the relative pose matrix column, H[2,4] is the y-axis matrix of the relative pose matrix column, H[3,4] is the z-axis matrix of the relative pose matrix column, and d6 is the distance from the elbow joint of the lens-holding arm to the telecentric fixed point of the lens-holding arm. The inverse kinematic distance of the slide joint is generated based on the rotational joint angle and the pitch joint angle. The moving motor is controlled based on the rotation joint angle, and / or the pitch joint angle, and / or the inverse kinematic distance of the slide joint.

4. The laparoscopic surgical robot control system according to claim 2, characterized in that, The step of controlling the rotating motor according to the lens rotation angle includes: When the lens rotation angle is greater than 0, the rotary motor is controlled to rotate forward, and the operating power of the rotary motor is controlled according to the absolute value of the lens rotation angle; When the lens rotation angle is less than 0, the rotary motor is controlled to reverse, and the operating power of the rotary motor is controlled according to the absolute value of the lens rotation angle.

5. The laparoscopic surgical robot control system according to claim 2, characterized in that, The step of controlling the rotating motor according to the lens rotation angle includes: When the lens rotation angle is greater than 0, the rotary motor is controlled to reverse, and the operating power of the rotary motor is controlled according to the absolute value of the lens rotation angle; When the lens rotation angle is less than 0, the rotary motor is controlled to rotate forward, and the operating power of the rotary motor is controlled according to the absolute value of the lens rotation angle.

6. A computer-readable storage medium, characterized in that, The storage medium stores a computer program, which, when executed by a processor, implements a laparoscopic surgical robot control method, the control method comprising: Obtain control commands; When the control command is a lens rotation command, the system enters the lens rotation arm mode, shuts down the movement motor, acquires the projection variables of the first and second main controllers on the display screen plane, and controls the rotation motor according to the projection variables. The first and second main controllers are used to map the doctor's rotational movements. Acquiring the projection variables of the first and second main controllers on the display screen plane includes: acquiring the first initial coordinates of the first and second main controllers in the display screen space coordinate system when entering the lens rotation arm mode; generating the initial projection of the first and second main controllers on the display screen plane based on the first initial coordinates; acquiring the rotation coordinates of the first and second main controllers in the display screen space coordinate system after entering the lens rotation arm mode; generating the rotation projection of the first and second main controllers on the display screen plane based on the rotation coordinates; and generating the projection variables based on the initial projection, the rotation projection, and the projection calculation formula, whereby the projection calculation formula includes: ； in, Let A be the projection variable. LR (1) is the vector of the initial projection in the x-axis direction, A LR (2) is the vector of the initial projection in the y-axis direction, B LR (1) is the vector of the rotational projection in the x-axis direction, B LR (2) is the vector of the rotational projection in the y-axis direction; When the control command is a moving lens command, the system enters the moving endoscope arm lens mode, shuts down the rotary motor, acquires the spatial matrix variables of the first main controller and the second main controller in the display screen spatial coordinate system, and controls the moving motor according to the spatial matrix variables. The first and second main controllers are used to map the doctor's movement. Acquiring the spatial matrix variables of the first and second main controllers in the display screen spatial coordinate system includes: acquiring the second initial coordinates of the first and second main controllers in the display screen spatial coordinate system when entering the moving endoscope arm lens mode, and generating an initial matrix column; acquiring the initial pose matrix column of the endoscope arm; generating a laparoscopic initial matrix column based on the initial matrix column and the initial pose matrix column; acquiring the movement coordinates of the first and second main controllers in the display screen spatial coordinate system after entering the moving endoscope arm lens mode, and generating a movement matrix column; generating a laparoscopic movement matrix column based on the initial pose matrix column and the movement matrix column; and performing a difference operation on the laparoscopic initial matrix column and the laparoscopic movement matrix column to generate the spatial matrix variables.

Images