Endoscope robot control method and system, electronic equipment, medium and program product

Abstract

The invention provides an endoscope robot control method and system, electronic equipment, a medium and a program product, and an endoscope robot comprises a first mechanical joint, a second mechanical joint, a third mechanical joint, a fourth mechanical joint, a first virtual joint and a second virtual joint. The endoscope robot control method comprises the following steps: determining a first translation distance, a first rotation angle, a second rotation angle, a third rotation angle, a second translation distance and a fourth rotation angle according to a target coordinate point of an endoscope robot; and controlling the position of the endoscope robot to be updated to the target coordinate point according to the first translation distance, the first rotation angle, the second rotation angle, the third rotation angle, the second translation distance and the fourth rotation angle. Operation can be observed from different angles by controlling the movement of the endoscope robot combining the mechanical joint and the virtual joint, the manufacturing process of the endoscope robot is simplified, and meanwhile the stability of the structure of the endoscope robot can be guaranteed under the condition that the degree of freedom is not sacrificed.

Description

Technical Field

[0001] The present disclosure relates to the technical field of surgical robots, and in particular to an endoscopic robot control method, system, electronic equipment, medium and program product. Background Art

[0002] In surgical robot technology, the robot passing through the natural cavity will be equipped with a robotic arm with a multi-degree-of-freedom robot structure to drive the endoscope (referred to as the endoscopic robot) to observe the real-time image of the natural cavity. Existing endoscopic robots are generally composed of three subsystems: a visual system, a robotic arm, and a control system. The visual system is composed of a lens, a light source, an image processing unit, and a display. The robotic arm system is generally composed of a series joint composed of mechanical connecting rods, pulleys, etc., a drive rope, a screw, a reducer and other transmission mechanisms, and a motor. The control system is generally composed of a motion controller, a motor servo controller, and a host computer. The lens is usually installed at the end of the robotic arm, and the movement of the robotic arm drives the lens to move, changing the position of the lens and the observation angle to meet the visual observation required by the clinic.

[0003] Due to the narrow, long and tortuous nature of natural cavities, and the need to observe images at different angles during surgery, the design of endoscopic robots is often flexible, has multiple degrees of freedom, and a small diameter. In the prior art, the number of degrees of freedom and the diameter of the endoscopic robot are often mutually restrictive factors. Since more degrees of freedom mean more joints and mechanical structures are needed, more space needs to be occupied and the complexity of the design and production process is increased, and the performance of the robot is reduced due to multiple joints connected in series. In the prior art, the degree of freedom is often reduced in exchange for structural stability or more mechanical structures are added, but this suffers from the disadvantages of reduced performance and reduced structural stability. Summary of the invention

[0004] The technical problem to be solved by the present disclosure is to overcome the defects in the prior art that the number of degrees of freedom and the diameter of the endoscopic robot are mutually restricted, the process is complicated and affects the performance of the robot, and an endoscopic robot control method, system, electronic equipment, medium and program product are provided.

[0005] The present invention solves the above technical problems through the following technical solutions:

[0006] The present disclosure provides an endoscopic robot control method, wherein the endoscopic robot comprises a first mechanical joint, a second mechanical joint, a third mechanical joint, a fourth mechanical joint, a first virtual joint, and a second virtual joint, wherein the first virtual joint is realized by image scaling of the endoscopic robot, and the second virtual joint is realized by image rotation of the endoscopic robot, and the endoscopic robot control method comprises:

[0007] Determine, according to the target coordinate point of the endoscopic robot, a first translation distance of the first mechanical joint, a first rotation angle of the second mechanical joint, a second rotation angle of the third mechanical joint, a third rotation angle of the fourth mechanical joint, a second translation distance of the first virtual joint, and a fourth rotation angle of the second virtual joint;

[0008] The position of the endoscopic robot is controlled to be updated to the target coordinate point according to the first translation distance, the first rotation angle, the second rotation angle, the third rotation angle, the second translation distance and the fourth rotation angle.

[0009] Preferably, the step of controlling the position of the endoscopic robot to update to the target coordinate point according to the first translation distance, the first rotation angle, the second rotation angle, the third rotation angle, the second translation distance and the fourth rotation angle comprises:

[0010] Determine the optimal imaging distance based on the focal length and image distance;

[0011] Determine a zoom factor according to the optimal imaging distance and the second translation distance;

[0012] Determine the third image coordinates corresponding to the first virtual joint after translation according to the zoom factor, the first image coordinates corresponding to the initial position of the first virtual joint, the fourth rotation angle and the optimal imaging distance; determine the fourth image coordinates corresponding to the second virtual joint after rotation according to the zoom factor, the second image coordinates corresponding to the initial position of the second virtual joint, the fourth rotation angle and the optimal imaging distance.

[0013] Preferably, the step of determining the third image coordinates corresponding to the first virtual joint after translation according to the zoom factor, the first image coordinates corresponding to the initial position of the first virtual joint, the fourth rotation angle and the optimal imaging distance, and the step of determining the fourth image coordinates corresponding to the second virtual joint after rotation according to the zoom factor, the second image coordinates corresponding to the initial position of the second virtual joint, the fourth rotation angle and the optimal imaging distance comprises:

[0014] The third image coordinates and the fourth image coordinates are determined by the following formula:

[0015]

[0016] Among them, z c Used to characterize the optimal imaging distance; An x-axis coordinate for representing the third image coordinate or the fourth image coordinate; Used to represent the y-axis coordinate of the third image coordinate or the fourth image coordinate; θ is used to represent the fourth rotation angle; k is used to represent the zoom factor; x is used to represent the x-axis coordinate of the first image coordinate or the second image coordinate; y is used to represent the y-axis coordinate of the first image coordinate or the second image coordinate.

[0017] Preferably, the step of determining the optimal imaging distance according to the focal length and the image distance comprises:

[0018] The optimal imaging distance is determined by the following formula:

[0019]

[0020] Among them, z c It is used to characterize the optimal imaging distance; f is used to characterize the focal length; d is used to characterize the image distance;

[0021] and / or,

[0022] The step of determining the zoom factor according to the optimal imaging distance and the second translation distance comprises:

[0023] The zoom factor is determined by the following formula:

[0024]

[0025] Among them, k is used to represent the scaling factor; z c It is used to characterize the optimal imaging distance; δ is used to characterize the second translation distance.

[0026] Preferably, the step of determining the first translation distance of the first mechanical joint, the first rotation angle of the second mechanical joint, the second rotation angle of the third mechanical joint, the third rotation angle of the fourth mechanical joint, the second translation distance of the first virtual joint, and the fourth rotation angle of the second virtual joint according to the target coordinate point of the endoscopic robot comprises:

[0027] Determine a first matrix according to the target coordinate point, where the first matrix is ​​used to represent the spatial position and orientation of the target coordinate point;

[0028] Acquire a second matrix, where the second matrix is ​​used to characterize the joint position relationship among the first mechanical joint, the second mechanical joint, the third mechanical joint, the fourth mechanical joint, the first virtual joint, and the second virtual joint;

[0029] The first translation distance of the first mechanical joint, the first rotation angle of the second mechanical joint, the second rotation angle of the third mechanical joint, the third rotation angle of the fourth mechanical joint, the second translation distance of the first virtual joint, and the fourth rotation angle of the second virtual joint are determined according to the first matrix and the second matrix.

[0030] Preferably, the step of determining the first matrix according to the target coordinate point comprises:

[0031] The first matrix is ​​determined according to the target coordinate point using the following formula:

[0032]

[0033] T 11 =-sin(q4)sin(q6)sin(q2+q3)+cos(q6)cos(q2+q3);

[0034] T 12 =-sin(q4)sin(q6)sin(q2+q3)+cos(q6)cos(q2+q3);

[0035] T 13 = -sin(q2+q3)cos(q4);

[0036] T 14 =-L2sin(q2)-L3sin(q2+q3)-(L4+q5)sin(q2+q3)cos(q4);

[0037] T 21 = sin(q6)cos(q4);

[0038] T 22 =cos(q4)cos(q6);

[0039] T 23 = -sin(q4);

[0040] T 24 =(-L4-q5)sin(q4);

[0041] T 31 =sin(q4)sin(q6)cos(q2+q3)+sin(q2+q3)cos(q6);

[0042] T 32 =sin(q4)sin(q6)cos(q2+q3)-sin(q6)sin(q2+q3);

[0043] T 34=L1+L2cos(q2)+L3cos(q2+q3)+q1

[0044] +(L4+q5)cos(q4)cos(q2+q3);

[0045] T 41 =0;

[0046] T 42 =0;

[0047] T 43 =0;

[0048] T 44 =1;

[0049] Among them, T is used to represent the first matrix, T 11 The formula for representing the first row and first column of the first matrix, T 12 The formula for representing the first row and second column of the first matrix, T 13 The formula for representing the first row and third column of the first matrix, T 14 The formula for representing the first row and fourth column of the first matrix, T 21 The formula for representing the second row and first column of the first matrix, T 22 The formula for representing the second row and second column of the first matrix, T 23 The formula for representing the second row and third column of the first matrix, T 24 The formula for representing the second row and fourth column of the first matrix, T 31 The formula for representing the third row and first column of the first matrix, T 32 The formula for representing the third row and second column of the first matrix, T 33 The formula for representing the third row and third column of the first matrix, T 34 The formula for representing the third row and fourth column of the first matrix, T 41 The formula for representing the fourth row and first column of the first matrix, T 42 The formula for representing the fourth row and second column of the first matrix, T 43 The formula for representing the fourth row and third column of the first matrix, T 44A formula for characterizing the fourth row and fourth column of the first matrix, q1 is used to characterize the first translation distance of the first mechanical joint, q2 is used to characterize the first rotation angle of the second mechanical joint, q3 is used to characterize the second rotation angle of the third mechanical joint, q4 is used to characterize the third rotation angle of the fourth mechanical joint, q5 is used to characterize the second translation distance of the first virtual joint, and q6 is used to characterize the fourth rotation angle of the second virtual joint; L1 is used to characterize the connecting rod length of the first mechanical joint and the second mechanical joint, L2 is used to characterize the connecting rod length of the second mechanical joint and the third mechanical joint, L3 is used to characterize the connecting rod length of the third mechanical joint and the fourth mechanical joint, and L4 is used to characterize the connecting rod length of the first virtual joint and the second virtual joint;

[0050] The step of obtaining the second matrix comprises:

[0051] The second matrix is ​​obtained by the following formula:

[0052]

[0053] Among them, T act Used to represent the second matrix, T act11 To characterize T 11 The value of T act12 To characterize T 12 The value of T act13 To characterize T 13 The value of T act14 To characterize T 14 The value of T act21 To characterize T 21 The value of T act22 To characterize T 22 The value of T act23 To characterize T 23 The value of T act24 To characterize T 24 The value of T act31 To characterize T 31 The value of T act32 To characterize T 32 The value of T act33 To characterize T 33 The value of T act34 To characterize T 34 The value of T act41 To characterize T 41 The value of T act42 To characterize T 42 The value of T act43 To characterize T 43 The value of T act44 To characterize T 44 The value of

[0054] The step of determining the first translation distance of the first mechanical joint, the first rotation angle of the second mechanical joint, the second rotation angle of the third mechanical joint, the third rotation angle of the fourth mechanical joint, the second translation distance of the first virtual joint, and the fourth rotation angle of the second virtual joint according to the first matrix and the second matrix comprises:

[0055] The first translation distance of the first mechanical joint, the first rotation angle of the second mechanical joint, the second rotation angle of the third mechanical joint, the third rotation angle of the fourth mechanical joint, the second translation distance of the first virtual joint, and the fourth rotation angle of the second virtual joint are determined by the following formula:

[0056] q1=T 34 -L1-L2cos(q2)-(L3+(L4+q5)cos(q4)cos(q2+q3);

[0057] q2=-arcsin((T 14 +((L4+q5)cos(q4)+L3)sin(q2+q3)) / L2);

[0058] q3=arctan(sin(q2+q3) / cos(q2+q3))-q2;

[0059] q4=-arcsin(T 23 );

[0060] q5=-T 24 / sin(q4)-L4;

[0061] q6=arctan(T 21 ,T 22 );

[0062] Among them, q1 is used to characterize the first translation distance of the first mechanical joint, q2 is used to characterize the first rotation angle of the second mechanical joint, q3 is used to characterize the second rotation angle of the third mechanical joint, q4 is used to characterize the third rotation angle of the fourth mechanical joint, q5 is used to characterize the second translation distance of the first virtual joint, and q6 is used to characterize the fourth rotation angle of the second virtual joint.

[0063] The present disclosure provides an endoscopic robot control system, the endoscopic robot comprising a first mechanical joint, a second mechanical joint, a third mechanical joint, a fourth mechanical joint, a first virtual joint and a second virtual joint, the first virtual joint being realized by endoscopic robot image scaling, the second virtual joint being realized by endoscopic robot image rotation, the endoscopic robot control system comprising:

[0064] a determination module, configured to determine, according to a target coordinate point of the endoscopic robot, a first translation distance of the first mechanical joint, a first rotation angle of the second mechanical joint, a second rotation angle of the third mechanical joint, a third rotation angle of the fourth mechanical joint, a second translation distance of the first virtual joint, and a fourth rotation angle of the second virtual joint;

[0065] A position update module is used to control the position of the endoscopic robot to update to the target coordinate point according to the first translation distance, the first rotation angle, the second rotation angle, the third rotation angle, the second translation distance and the fourth rotation angle.

[0066] The present disclosure provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and used to run on the processor, wherein the processor implements the above-mentioned endoscopic robot control method when executing the computer program.

[0067] The present disclosure provides a computer-readable storage medium having a computer program stored thereon, and the computer program implements the above-mentioned endoscopic robot control method when executed by a processor.

[0068] The present disclosure provides a computer program product, including a computer program, which implements the above-mentioned endoscopic robot control method when executed by a processor.

[0069] On the basis of being in accordance with the common sense in the art, the above-mentioned preferred conditions can be arbitrarily combined to obtain the preferred embodiments of the present disclosure.

[0070] The positive and progressive effects of this disclosure are:

[0071] The present invention can observe surgery from different angles by controlling the movement of an endoscopic robot including a combination of mechanical joints and virtual joints, thereby simplifying the manufacturing process of the endoscopic robot and ensuring the stability of the endoscopic robot structure without sacrificing the degree of freedom. BRIEF DESCRIPTION OF THE DRAWINGS

[0072] Figure 1 A structural diagram of an endoscopic robot provided in Example 1 of the present disclosure;

[0073] Figure 2 A flowchart of an endoscopic robot control method provided in Example 1 of the present disclosure;

[0074] Figure 3 A schematic diagram of transformation from a world coordinate system to a camera coordinate system of a specific example of an endoscopic robot control method provided in Embodiment 1 of the present disclosure;

[0075] Figure 4A schematic diagram of transformation from a camera coordinate system to an image coordinate system of a specific example of an endoscopic robot control method provided in Embodiment 1 of the present disclosure;

[0076] Figure 5 A schematic diagram of transformation from an image coordinate system to a pixel coordinate system of a specific example of an endoscopic robot control method provided in Embodiment 1 of the present disclosure;

[0077] Figure 6 A schematic diagram of an optimal imaging distance for a specific example of an endoscopic robot control method provided in Embodiment 1 of the present disclosure;

[0078] Figure 7 A flowchart of a specific example of an endoscopic robot control method provided in Embodiment 1 of the present disclosure;

[0079] Figure 8 A structural diagram of an endoscope robot control system provided in Example 2 of the present disclosure;

[0080] Fig. 9 This is a schematic diagram of the structure of an electronic device according to Embodiment 3 of the present disclosure. DETAILED DESCRIPTION

[0081] The present disclosure is further described below by way of examples, but the present disclosure is not limited to the scope of the examples.

[0082] Prefixes such as "first" and "second" are used in the embodiments of the present disclosure only to distinguish different description objects, and have no limiting effect on the position, order, priority, quantity or content of the described objects. The use of prefixes such as ordinal numbers to distinguish description objects in the embodiments of the present disclosure does not constitute a limitation on the described objects. For the statement of the described objects, please refer to the description in the context of the claims or embodiments, and no unnecessary limitation should be constituted due to the use of such prefixes. In addition, in the description of the present embodiment, unless otherwise specified, the meaning of "plurality" is two or more.

[0083] Example 1

[0084] This embodiment provides an endoscope robot control method, see Figure 1The endoscopic robot includes a first mechanical joint J1, a second mechanical joint J2, a third mechanical joint J3, a fourth mechanical joint J4, a first virtual joint J5 and a second virtual joint J6. J1 is a translation joint realized by a mechanical structure, and is driven by a corresponding motor. J2, J3 and J4 are rotation joints realized by a mechanical structure, and are driven by corresponding motors. The J5 joint is a virtual translation joint realized by an image, and is simulated by the scaling of the image. J6 is a virtual rotation joint realized by an image, and is simulated by the rotation of the image. The first virtual joint and the second virtual joint are displayed by mapping to an image coordinate system. The initial position of the first virtual joint corresponds to the first image coordinate, and the initial position of the second virtual joint corresponds to the second image coordinate. The translation of the first virtual joint is realized by scaling the first image coordinate, and the rotation of the second virtual joint is determined by the rotation of the second image coordinate.

[0085] In an optional embodiment, before step S1, the following steps are included:

[0086] S0. Determine the target coordinate point of the endoscope robot according to user instructions.

[0087] Among them, user instructions are set according to actual conditions.

[0088] See also Figure 2 , the endoscope robot control method includes:

[0089] S1. Determine the first translation distance of the first mechanical joint, the first rotation angle of the second mechanical joint, the second rotation angle of the third mechanical joint, the third rotation angle of the fourth mechanical joint, the second translation distance of the first virtual joint, and the fourth rotation angle of the second virtual joint according to the target coordinate point of the endoscopic robot.

[0090] S2. Control the position of the endoscopic robot to update to the target coordinate point according to the first translation distance, the first rotation angle, the second rotation angle, the third rotation angle, the second translation distance and the fourth rotation angle.

[0091] In this embodiment, the endoscopic robot is composed of 6 serial joints. In the master-slave remote control, the target coordinate point (Xe, Ye, Ze) of the endoscopic robot is inversely calculated to the distance or angle required for each joint to move, and the motor or image processor is synchronously controlled to realize the joint, so as to operate the endoscopic lens e to observe at different positions and angles. That is, by controlling the movement of the endoscopic robot including the combination of mechanical joints and virtual joints, the operation can be observed from different angles, which simplifies the manufacturing process of the endoscopic robot and ensures the stability of the structure of the endoscopic robot without sacrificing the degrees of freedom.

[0092] In an optional embodiment, step S2 includes:

[0093] S21, determining the optimal imaging distance according to the focal length and the image distance;

[0094] S22, determining a zoom factor according to the optimal imaging distance and the second translation distance;

[0095] S23. Determine the third image coordinates corresponding to the first virtual joint after translation according to the zoom factor, the first image coordinates, the fourth rotation angle and the optimal imaging distance; determine the fourth image coordinates corresponding to the second virtual joint after rotation according to the zoom factor, the second image coordinates, the fourth rotation angle and the optimal imaging distance.

[0096] In an optional implementation, step S23 includes:

[0097] S231. Determine the third image coordinates and the fourth image coordinates by the following formula:

[0098]

[0099] Among them, z c Used to characterize the optimal imaging distance; An x-axis coordinate for representing the third image coordinate or the fourth image coordinate; Used to represent the y-axis coordinate of the third image coordinate or the fourth image coordinate; θ is used to represent the fourth rotation angle; k is used to represent the zoom factor; x is used to represent the x-axis coordinate of the first image coordinate or the second image coordinate; y is used to represent the y-axis coordinate of the first image coordinate or the second image coordinate.

[0100] In an optional implementation, step S21:

[0101] The optimal imaging distance is determined by the following formula:

[0102]

[0103] Among them, z c It is used to characterize the optimal imaging distance; f is used to characterize the focal length; d is used to characterize the image distance.

[0104] In an optional implementation, step S22 includes:

[0105] The zoom factor is determined by the following formula:

[0106]

[0107] Among them, k is used to represent the scaling factor; z c It is used to characterize the optimal imaging distance; δ is used to characterize the second translation distance.

[0108] In an optional embodiment, step S1 includes:

[0109] S11. Determine a first matrix according to the target coordinate point.

[0110] The first matrix is ​​used to represent the spatial position and orientation of the target coordinate point.

[0111] S12. Obtain a second matrix.

[0112] The second matrix is ​​used to characterize the joint position relationship among the first mechanical joint, the second mechanical joint, the third mechanical joint, the fourth mechanical joint, the first virtual joint and the second virtual joint. The second matrix is ​​represented by a homogeneous matrix.

[0113] S13. Determine the first translation distance of the first mechanical joint, the first rotation angle of the second mechanical joint, the second rotation angle of the third mechanical joint, the third rotation angle of the fourth mechanical joint, the second translation distance of the first virtual joint, and the fourth rotation angle of the second virtual joint according to the first matrix and the second matrix.

[0114] Specifically, step S11 includes:

[0115] The first matrix is ​​determined according to the target coordinate point using the following formula:

[0116]

[0117] T 11 =-sin(q4)sin(q6)sin(q2+q3)+cos(q6)cos(q2+q3);

[0118] T 12 =-sin(q4)sin(q6)sin(q2+q3)+cos(q6)cos(q2+q3);

[0119] T 13 = -sin(q2+q3)cos(q4);

[0120] T 14 =-L2sin(q2)-L3sin(q2+q3)-(L4+q5)sin(q2+q3)cos(q4);

[0121] T 21 = sin(q6)cos(q4);

[0122] T 22 =cos(q4)cos(q6);

[0123] T 23 = -sin(q4);

[0124] T 24 =(-L4-q5)sin(q4);

[0125] T 31 =sin(q4)sin(q6)cos(q2+q3)+sin(q2+q3)cos(q6);

[0126] T 32 =sin(q4)sin(q6)cos(q2+q3)-sin(q6)sin(q2+q3);

[0127] T 34 =L1+L2cos(q2)+L3cos(q2+q3)+q1+(L4+q5)cos(q4);

[0128] T 41 =0;

[0129] T 42 =0;

[0130] T 43 =0;

[0131] T 44 =1;

[0132] Among them, T is used to represent the first matrix, T 11 The formula for representing the first row and first column of the first matrix, T 12 The formula used to represent the first row and second column of the first matrix, T 13 The formula used to represent the first row and third column of the first matrix, T 14 The formula used to represent the first row and fourth column of the first matrix, T 21 The formula used to represent the second row and first column of the first matrix, T 22 The formula used to represent the second row and second column of the first matrix, T 23 The formula used to represent the second row and third column of the first matrix, T 24 The formula used to represent the second row and fourth column of the first matrix, T 31 The formula used to represent the third row and first column of the first matrix, T 32 The formula used to represent the third row and second column of the first matrix, T 33 The formula used to represent the third row and third column of the first matrix, T 34 The formula used to represent the third row and fourth column of the first matrix, T 41 The formula used to represent the fourth row and first column of the first matrix, T 42 The formula used to represent the fourth row and second column of the first matrix, T 43 The formula used to represent the fourth row and third column of the first matrix, T 44The formula used to characterize the fourth row and fourth column of the first matrix, q1 is used to characterize the first translation distance of the first mechanical joint, q2 is used to characterize the first rotation angle of the second mechanical joint, q3 is used to characterize the second rotation angle of the third mechanical joint, q4 is used to characterize the third rotation angle of the fourth mechanical joint, q5 is used to characterize the second translation distance of the first virtual joint, and q6 is used to characterize the fourth rotation angle of the second virtual joint. L1 is used to characterize the connecting rod length of the first mechanical joint and the second mechanical joint, L2 is used to characterize the connecting rod length of the second mechanical joint and the third mechanical joint, L3 is used to characterize the connecting rod length of the third mechanical joint and the fourth mechanical joint, and L4 is used to characterize the connecting rod length of the first virtual joint and the second virtual joint.

[0133] Step S12 includes:

[0134] The second matrix is ​​obtained by the following formula:

[0135]

[0136] Among them, T act Used to represent the second matrix, T act11 To characterize T 11 The value of T act12 To characterize T 12 The value of T act13 To characterize T 13 The value of T act14 To characterize T 14 The value of T act21 To characterize T 21 The value of T act22 To characterize T 22 The value of T act23 To characterize T 23 The value of T act24 To characterize T 24 The value of T act31 To characterize T 31 The value of T act32 To characterize T 32 The value of T act33 To characterize T 33 The value of T act34 To characterize T 34 The value of T act41 To characterize T 41 The value of T act42 To characterize T 42 The value of T act43 To characterize T 43 The value of T act44 To characterize T 44 The value of .

[0137] Step S13 includes:

[0138] The first translation distance of the first mechanical joint, the first rotation angle of the second mechanical joint, the second rotation angle of the third mechanical joint, the third rotation angle of the fourth mechanical joint, the second translation distance of the first virtual joint, and the fourth rotation angle of the second virtual joint are determined by the following formula:

[0139] q1=T 34 -L1-L2cos(q2)-(L3+(L4+q5)cos(q4)cos(q2+q3).

[0140] q2=-arcsin((T 14 +((L4+q5)cos(q4)+L3)sin(q2+q3)) / L2).

[0141] q4=-arcsin(T 23 ).

[0142] q3=arctan(sin(q2+q3) / cos(q2+q3))-q2.

[0143] q5=-T 24 / sin(q4)-L4.

[0144] q6=arctan(T 21 ,T 22 ).

[0145] Among them, q1 is used to characterize the first translation distance of the first mechanical joint, q2 is used to characterize the first rotation angle of the second mechanical joint, q3 is used to characterize the second rotation angle of the third mechanical joint, q4 is used to characterize the third rotation angle of the fourth mechanical joint, q5 is used to characterize the second translation distance of the first virtual joint, and q6 is used to characterize the fourth rotation angle of the second virtual joint.

[0146] In this embodiment, the first translation distance of the first mechanical joint, the first rotation angle of the second mechanical joint, the second rotation angle of the third mechanical joint, the third rotation angle of the fourth mechanical joint, the second translation distance of the first virtual joint, and the fourth rotation angle of the second virtual joint are determined by inverse calculation of the first matrix and the second matrix, thereby improving the observation efficiency of the endoscopic robot.

[0147] In an optional embodiment, step S2 includes:

[0148] Map the second translation distance to pixel coordinates.

[0149] The updating of the position of the endoscopic robot is controlled according to the second translation distance in the pixel coordinate system.

[0150] In an optional implementation, step S2 may further include:

[0151] Map the fourth rotation angle to the pixel coordinate system.

[0152] The updating of the position of the endoscopic robot is controlled according to the second translation distance in the pixel coordinate system.

[0153] In an optional implementation, step S2 may further include:

[0154] According to the first translation distance, the first rotation angle, the second rotation angle, and the third rotation angle, the first motor operating angle corresponding to the first mechanical joint, the second motor operating angle corresponding to the second mechanical joint, the third motor operating angle corresponding to the third mechanical joint, and the fourth motor operating angle corresponding to the fourth mechanical joint are determined respectively.

[0155] The updating of the position of the endoscopic robot is controlled according to the first motor operating angle, the second motor operating angle, the third motor operating angle and the fourth motor operating angle.

[0156] In this embodiment, the mechanical joint can control the endoscopic robot to reach the preset position by determining the motor running angle, and the virtual joint can transform the virtual joint into an image coordinate system by scaling the rotation ratio and other methods, and then transform it into a pixel coordinate system, map the second translation distance and the fourth rotation angle to the pixel coordinate system, and then control the endoscopic robot to reach the preset position according to the second translation distance and the fourth rotation angle determined in the pixel coordinate system. This embodiment will realize a 6-DOF robot through the linkage processing of the image virtual joints of the mechanical joints without increasing the complexity of the mechanical structure, and balance the freedom of the endoscopic robot and the stability of the mechanical structure.

[0157] A specific example is introduced below to explain in detail the endoscope robot control method of this embodiment.

[0158] like Figure 1 As shown, the endoscopic robot in this example consists of 4 mechanical joints and 2 virtual joints.

[0159] Among them, J1, J2, J3, and J4 are mechanical joints, which respectively realize:

[0160] J1 is the translation axis (the first mechanical joint) to achieve the in-and-out freedom; J2 (the second mechanical joint) and J3 (the third mechanical joint) are the rotation axes to achieve the pitch freedom; J4 (the fourth mechanical joint) is the rotation axis to achieve the yaw freedom.

[0161] In addition, J5 and J6 are image virtual joints, and the lens e is installed on the end link of the endoscope robot arm, respectively realizing:

[0162] J5 is the image zoom axis (the first virtual joint), which realizes the translational freedom of the field of view point along the field of view direction.

[0163] J6 is the image rotation axis (the second virtual joint), which realizes the rotation freedom along the field of view.

[0164] The virtual coordinate axes of J5 and J6 have the same rotation and scaling effects as the image coordinate axes. The specific principles are as follows:

[0165] Without considering distortion, the imaging model of the camera is as follows.

[0166] For a point P in a three-dimensional rectangular coordinate system world coordinate system w (x w ,y w ,z w ), mapped to the pixel coordinate system o(u,v), is divided into three parts.

[0167] Part I Figure 3 As shown, the world coordinate system P w To the camera coordinate system P c (x c ,y c ,z c )

[0168]

[0169] Among them, R is a 3x3 rotation matrix and t is a 3x1 translation vector.

[0170] Part 2: Camera Coordinate System P c The transformation to the image coordinate system P(x,y) is as follows Figure 4 , the formula is:

[0171]

[0172] Where f is the focal length of the camera.

[0173] The third part is the transformation from the image coordinate system P(x,y) to the pixel coordinate system o(u,v), such as Figure 5 , the formula is:

[0174]

[0175] Where dx is the distance between each pixel in the x direction, and dy is the distance between each pixel in the y direction.

[0176] Combining the above three parts, we can get the mapping from the world coordinate system to the pixel coordinate system. The formula is as follows:

[0177]

[0178] Among them, uo, vo are the coordinate values ​​of the pixel at the center of the image.

[0179] The virtual coordinate axes of J5 and J6 can be equivalent to rotating and scaling the point P(x, y) of the image coordinate system around the center. The scaled point is Its rotation angle is θ and its scaling factor is k:

[0180]

[0181] Rearranging the equation into

[0182]

[0183] It can be considered that the Z-axis direction of the image coordinate system has been translated, and the Z-axis coordinate after translation is:

[0184]

[0185] Combined with the above change formula, then:

[0186]

[0187] Equivalent to

[0188]

[0189] in,

[0190]

[0191] This formula can be understood as rotating and translating the point in the image coordinates relative to the initial coordinate system. The rotation angle is θ and the translation amount is δ. It can be equivalent to rotating the point in the camera coordinates along the Z axis and scaling it k times, where the rotation angle is θ and the scaling factor is That is, the equivalent motion of the two virtual joints J5 and J6, namely translation and rotation, is achieved in this way.

[0192] The following is a specific process for determining the scaling factor k:

[0193] When the virtual joint J5 undergoes translational motion, it can be considered that the Z-axis direction of the camera coordinate system corresponding to J5 is translated and scaled by k times, and the Z-axis coordinate after translation is:

[0194]

[0195] That is, if there is a target z-axis target position The required scaling is Then assume that the translation δ relative to the initial position is:

[0196]

[0197] Therefore, we can obtain:

[0198]

[0199] For the scaling rotation angle θ, since the virtual joint rotation acts in the z-axis direction, the angle of virtual joint rotation is consistent with the angle of image rotation.

[0200] When J5 and J6 are translated or rotated, first, the camera coordinates of J5 and J6 are transformed as follows:

[0201]

[0202] Secondly, the camera coordinate system Will be transformed into the image coordinate system

[0203]

[0204] Finally, the image coordinate system Transform to pixel coordinate system

[0205]

[0206] Among them, dx is the distance between each pixel in the x direction; dy is the distance between each pixel in the y direction, and u0 and v0 are the coordinate values ​​of the pixel at the center of the image.

[0207] In virtual joint control, the rotation and translation coordinate systems are used as input conditions, that is, the rotation angle θ and the k-fold scaling factor need to be obtained.

[0208] The above Zc refers to the coordinate of the Z axis of the photographed object in the camera coordinate system. Since the Z coordinate of the photographed object changes with the position and orientation of the camera in actual imaging, it is necessary to select a virtual photographed object as a reference for the virtual joint movement. In the present invention, the optimal imaging distance of the camera is selected as Zc to adapt to most shooting situations. The relationship between the optimal imaging distance and the focal length and the image distance is related, such as Figure 6 As shown, the specific calculation results are calculated as follows:

[0209]

[0210] Among them, f is the focal length of the camera and d is the image distance of the camera.

[0211] Figure 7 For the control process of the endoscope robot in this example, usually:

[0212] S701. The host computer sends the target coordinate point of the endoscope robot as an instruction, that is, the observation position and angle where the endoscope needs to be.

[0213] S702, calculating the angles corresponding to the joints of the target coordinate points of the endoscopic robot by an inverse solution method.

[0214] The following is the specific inverse solution method:

[0215] like Figure 1 As shown in the figure, the reference coordinate system of each joint link is set according to the Modified Denavit-Hartenberg method (a parametric method for describing the geometric relationship between robot joints and links). The joint positions that J1 to J6 want to reach are recorded as q1, q2, q3, q4, q5, q6, and the corresponding link lengths in the figure are recorded as L1, L2, L3, L4.

[0216] The analytical method is used to express the relationship between the spatial position and orientation of point e and the position of each joint using homogeneous proof:

[0217]

[0218] T 11 =-sin(q4)sin(q6)sin(q2+q3)+cos(q6)cos(q2+q3);

[0219] T 12 =-sin(q4)sin(q6)sin(q2+q3)+cos(q6)cos(q2+q3);

[0220] T 13 = -sin(q2+q3)cos(q4);

[0221] T 14 =-L2sin(q2)-L3sin(q2+q3)-(L4+q5)sin(q2+q3)cos(q4);

[0222] T 21 = sin(q6)cos(q4);

[0223] T 22 =cos(q4)cos(q6);

[0224] T 23 = -sin(q4);

[0225] T 24 =(-L4-q5)sin(q4);

[0226] T 31 =sin(q4)sin(q6)cos(q2+q3)+sin(q2+q3)cos(q6);

[0227] T 32 =sin(q4)sin(q6)cos(q2+q3)-sin(q6)sin(q2+q3);

[0228] T 34 =L1+L2cos(q2)+L3cos(q2+q3)+q1+(L4+q5)cos(q4);

[0229] T 41 =0;

[0230] T 42 =0;

[0231] T 43 =0;

[0232] T 44 =1;

[0233] When the spatial position and orientation of point e are known, the positions of each joint are solved.

[0234] The motion range of the mechanical joint and the virtual joint is set in combination with the actual mechanical limit. For example, this example limits q1∈[0,20]mm(millimeter); q2,q3,q4∈[-60,60]deg(degree); q5∈[0,10]mm; q6∈(-180,180]deg.

[0235] The solution is:

[0236] q1=T 34 -L1-L2cos(q2)-(L3+(L4+q5)cos(q4)cos(q2+q3).

[0237] q2=-arcsin((T 14 +((L4+q5)cos(q4)+L3)sin(q2+q3)) / L2).

[0238] q3=arctan(sin(q2+q3) / cos(q2+q3))-q2.

[0239] q4=-arcsin(T 23 ).

[0240] q5=-T 24 / sin(q4)-L4.

[0241] q6=arctan(T 21 ,T 22 ).

[0242] The above solution values ​​are then restricted according to the motion ranges of the mechanical joints and virtual joints.

[0243] S703. For the J1, J2, J3, and J4 joints, calculations are performed according to the traditional mechanical structure relationship to calculate the angles at which the motors need to operate.

[0244] S704, converting the angle required for the motor to run into the code number required by the motor, and sending it to the motor servo as a control target instruction, and outputting the control angle through the motor servo actuator.

[0245] S705 . For the J5 and J6 joints, map the rotation angle and translation distance of the joints to the image coordinate system through a mapping method.

[0246] S706 , the image pixel processing unit maps the rotation angle and the translation distance to the pixel coordinate system, and performs corresponding rotation and translation calculations on the image pixel points.

[0247] Example 2

[0248] Corresponding to the aforementioned embodiment of the endoscopic robot control method, the present disclosure also provides an embodiment of an endoscopic robot control system.

[0249] The endoscopic robot comprises a first mechanical joint, a second mechanical joint, a third mechanical joint, a fourth mechanical joint, a first virtual joint and a second virtual joint. The first virtual joint and the second virtual joint are displayed by mapping to an image coordinate system. The initial position of the first virtual joint corresponds to the first image coordinate, and the initial position of the second virtual joint corresponds to the second image coordinate. The translation of the first virtual joint is achieved by scaling the first image coordinate, and the rotation of the second virtual joint is determined by the rotation of the second image coordinate, see Figure 8 , the endoscope robot control system includes:

[0250] Determination module 1 is used to determine the first translation distance of the first mechanical joint, the first rotation angle of the second mechanical joint, the second rotation angle of the third mechanical joint, the third rotation angle of the fourth mechanical joint, the second translation distance of the first virtual joint and the fourth rotation angle of the second virtual joint according to the target coordinate point of the endoscopic robot.

[0251] The position updating module 2 is used to control the position of the endoscopic robot to update to a target coordinate point according to the first translation distance, the first rotation angle, the second rotation angle, the third rotation angle, the second translation distance and the fourth rotation angle.

[0252] In an optional embodiment, the determination module 1 is also used to determine the optimal imaging distance based on the focal length and the image distance; it is also used to determine the zoom factor based on the optimal imaging distance and the second translation distance; it is also used to determine the third image coordinates corresponding to the first virtual joint after translation based on the zoom factor, the first image coordinates, the fourth rotation angle and the optimal imaging distance; it is also used to determine the fourth image coordinates corresponding to the second virtual joint after rotation based on the zoom factor, the second image coordinates, the fourth rotation angle and the optimal imaging distance.

[0253] In an optional implementation, the determination module 1 is further configured to determine the third image coordinates and the fourth image coordinates by using the following formula:

[0254]

[0255] Among them, z c Used to characterize the optimal imaging distance; An x-axis coordinate for representing the third image coordinate or the fourth image coordinate; Used to represent the y-axis coordinate of the third image coordinate or the fourth image coordinate; θ is used to represent the fourth rotation angle; k is used to represent the zoom factor; x is used to represent the x-axis coordinate of the first image coordinate or the second image coordinate; y is used to represent the y-axis coordinate of the first image coordinate or the second image coordinate.

[0256] In an optional implementation, the determination module 1 is further configured to determine the optimal imaging distance using the following formula:

[0257]

[0258] Among them, z c It is used to characterize the optimal imaging distance; f is used to characterize the focal length; d is used to characterize the image distance.

[0259] In an optional implementation, the determination module 1 is further configured to determine the scaling factor using the following formula:

[0260]

[0261] Among them, k is used to represent the scaling factor; z c It is used to characterize the optimal imaging distance; δ is used to characterize the second translation distance.

[0262] In an optional implementation, the determination module 1 is further used to determine a first matrix according to the target coordinate point, where the first matrix is ​​used to characterize the spatial position and orientation of the target coordinate point.

[0263] See also Figure 8 , the endoscope robot control system also includes:

[0264] The acquisition module 3 is used to acquire a second matrix. The second matrix is ​​used to characterize the joint position relationship between the first mechanical joint, the second mechanical joint, the third mechanical joint, the fourth mechanical joint, the first virtual joint and the second virtual joint.

[0265] Determination module 1 is also used to determine the first translation distance of the first mechanical joint, the first rotation angle of the second mechanical joint, the second rotation angle of the third mechanical joint, the third rotation angle of the fourth mechanical joint, the second translation distance of the first virtual joint and the fourth rotation angle of the second virtual joint according to the first matrix and the second matrix.

[0266] In an optional implementation, the determination module 1 is further configured to determine the first matrix according to the target coordinate point by using the following formula:

[0267]

[0268] T 11 =-sin(q4)sin(q6)sin(q2+q3)+cos(q6)cos(q2+q3);

[0269] T 12 =-sin(q4)sin(q6)sin(q2+q3)+cos(q6)cos(q2+q3);

[0270] T 13 = -sin(q2+q3)cos(q4);

[0271] T 14 =-L2sin(q2)-L3sin(q2+q3)-(L4+q5)sin(q2+q3)cos(q4);

[0272] T 21 = sin(q6)cos(q4);

[0273] T 22 =cos(q4)cos(q6);

[0274] T 23 = -sin(q4);

[0275] T 24 =(-L4-q5)sin(q4);

[0276] T 31 =sin(q4)sin(q6)cos(q2+q3)+sin(q2+q3)cos(q6);

[0277] T 32=sin(q4)sin(q6)cos(q2+q3)-sin(q6)sin(q2+q3);

[0278] T 34 =L1+L2cos(q2)+L3cos(q2+q3)+q1

[0279] +(L4+q5)cos(q4)cos(q2+q3);

[0280] T 41 =0;

[0281] T 42 =0;

[0282] T 43 =0;

[0283] T 44 =1;

[0284] Among them, T is used to represent the first matrix, T 11 The formula for representing the first row and first column of the first matrix, T 12 The formula used to represent the first row and second column of the first matrix, T 13 The formula used to represent the first row and third column of the first matrix, T 14 The formula used to represent the first row and fourth column of the first matrix, T 21 The formula used to represent the second row and first column of the first matrix, T 22 The formula used to represent the second row and second column of the first matrix, T 23 The formula used to represent the second row and third column of the first matrix, T 24 The formula used to represent the second row and fourth column of the first matrix, T 31 The formula used to represent the third row and first column of the first matrix, T 32 The formula used to represent the third row and second column of the first matrix, T 33 The formula used to represent the third row and third column of the first matrix, T 34 The formula used to represent the third row and fourth column of the first matrix, T 41 The formula used to represent the fourth row and first column of the first matrix, T 42 The formula used to represent the fourth row and second column of the first matrix, T 43 The formula used to represent the fourth row and third column of the first matrix, T 44The formula used to characterize the fourth row and fourth column of the first matrix, q1 is used to characterize the first translation distance of the first mechanical joint, q2 is used to characterize the first rotation angle of the second mechanical joint, q3 is used to characterize the second rotation angle of the third mechanical joint, q4 is used to characterize the third rotation angle of the fourth mechanical joint, q5 is used to characterize the second translation distance of the first virtual joint, q6 is used to characterize the fourth rotation angle of the second virtual joint; L1 is used to characterize the connecting rod length of the first mechanical joint and the second mechanical joint, L2 is used to characterize the connecting rod length of the second mechanical joint and the third mechanical joint, L3 is used to characterize the connecting rod length of the third mechanical joint and the fourth mechanical joint, and L4 is used to characterize the connecting rod length of the first virtual joint and the second virtual joint;

[0285] The acquisition module 3 is further used to obtain the second matrix through the following formula:

[0286]

[0287] Among them, T act Used to represent the second matrix, T act11 To characterize T 11 The value of T act12 To characterize T 12 The value of T act13 To characterize T 13 The value of T act14 To characterize T 14 The value of T act21 To characterize T 21 The value of T act22 To characterize T 22 The value of T act23 To characterize T 23 The value of T act24 To characterize T 24 The value of T act31 To characterize T 31 The value of T act32 To characterize T 32 The value of T act33 To characterize T 33 The value of T act34 To characterize T 34 The value of T act41 To characterize T 41 The value of T act42 To characterize T 42 The value of T act43 To characterize T 43 The value of T act44 To characterize T 44 The value of

[0288] The determination module 1 is further used to determine the first translation distance of the first mechanical joint, the first rotation angle of the second mechanical joint, the second rotation angle of the third mechanical joint, the third rotation angle of the fourth mechanical joint, the second translation distance of the first virtual joint, and the fourth rotation angle of the second virtual joint by the following formula:

[0289] q1=T 34 -L1-L2cos(q2)-(L3+(L4+q5)cos(q4)cos(q2+q3);

[0290] q2=-arcsin((T 14 +((L4+q5)cos(q4)+L3)sin(q2+q3)) / L2);

[0291] q4=-arcsin(T 23 );

[0292] q3=arctan(sin(q2+q3) / cos(q2+q3))-q2;

[0293] q5=-T 24 / sin(q4)-L4;

[0294] q6=arctan(T 21 ,T 22 );

[0295] Among them, q1 is used to characterize the first translation distance of the first mechanical joint, q2 is used to characterize the first rotation angle of the second mechanical joint, q3 is used to characterize the second rotation angle of the third mechanical joint, q4 is used to characterize the third rotation angle of the fourth mechanical joint, q5 is used to characterize the second translation distance of the first virtual joint, and q6 is used to characterize the fourth rotation angle of the second virtual joint.

[0296] In an alternative embodiment, see Figure 8 , the endoscope robot control system also includes:

[0297] The mapping module 4 is used to map the second translation distance to a pixel coordinate system.

[0298] The position updating module 2 is further used to control the updating of the position of the endoscopic robot according to the second translation distance in the pixel coordinate system.

[0299] In an optional implementation, the mapping module 4 is further configured to map the fourth rotation angle to a pixel coordinate system.

[0300] The position updating module 2 is further used to control the updating of the position of the endoscopic robot according to the second translation distance in the pixel coordinate system.

[0301] In an optional embodiment, the determination module 1 is also used to determine the first motor operating angle corresponding to the first mechanical joint, the second motor operating angle corresponding to the second mechanical joint, the third motor operating angle corresponding to the third mechanical joint, and the fourth motor operating angle corresponding to the fourth mechanical joint according to the first translation distance, the first rotation angle, the second rotation angle, and the third rotation angle.

[0302] The position updating module 2 is further used to control the updating of the position of the endoscopic robot according to the first motor operating angle, the second motor operating angle, the third motor operating angle and the fourth motor operating angle.

[0303] In an optional embodiment, the determination module 1 is further used to determine the target coordinate point of the endoscopic robot according to user instructions.

[0304] As for the system embodiment, since it basically corresponds to the method embodiment, the relevant parts can refer to the partial description of the method embodiment. The system embodiment described above is only illustrative, in which the units described as separate components may or may not be physically separated, and the components as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the disclosed solution.

[0305] Example 3

[0306] Fig. 9 This is a schematic diagram of the structure of an electronic device showing an example embodiment of the present disclosure, wherein the electronic device includes a memory, a processor, and a computer program stored in the memory and used to run on the processor, and when the processor executes the computer program, the endoscopic robot control method of any of the above embodiments is implemented. Fig. 9 The electronic device 90 shown is only an example and should not bring any limitation to the functions and scope of use of the embodiments of the present disclosure.

[0307] like Fig. 9 As shown, the electronic device 90 may be in the form of a general-purpose computing device, for example, it may be a server device. The components of the electronic device 90 may include, but are not limited to: at least one processor 91, at least one memory 92, and a bus 93 connecting different system components (including the memory 92 and the processor 91).

[0308] The bus 93 includes a data bus, an address bus, and a control bus.

[0309] The memory 92 may include a volatile memory, such as a random access memory (RAM) 921 and / or a cache memory 922 , and may further include a read-only memory (ROM) 923 .

[0310] The memory 92 may also include a program tool 925 (or utility) having a set (at least one) of program modules 924, such program modules 924 including but not limited to: an operating system, one or more application programs, other program modules and program data, each of which or some combination may include an implementation of a network environment.

[0311] The processor 91 executes various functional applications and data processing by running the computer program stored in the memory 92, such as the endoscopic robot control method provided in any of the above embodiments.

[0312] The electronic device 90 may also communicate with one or more external devices 94 (e.g., keyboards, pointing devices, etc.). Such communication may be performed via an input / output (I / O) interface 95. Furthermore, the electronic device 90 may also communicate with one or more networks (e.g., local area networks (LANs), wide area networks (WANs), and / or public networks, such as the Internet) via a network adapter 96. As shown, the network adapter 96 communicates with other modules of the electronic device 90 via a bus 93. It should be understood that, although not shown in the figure, other hardware and / or software modules may be used in conjunction with the electronic device 90, including but not limited to: microcode, device drivers, redundant processors, external disk drive arrays, RAID (disk array) systems, tape drives, and data backup storage systems, etc.

[0313] It should be noted that although several units / modules or sub-units / modules of the electronic device are mentioned in the above detailed description, this division is merely exemplary and not mandatory. In fact, according to the embodiments of the present disclosure, the features and functions of two or more units / modules described above can be embodied in one unit / module. Conversely, the features and functions of one unit / module described above can be further divided into multiple units / modules to be embodied.

[0314] Example 4

[0315] The embodiments of the present disclosure also provide a computer-readable storage medium having a computer program stored thereon, and when the program is executed by a processor, the endoscopic robot control method provided in any of the above embodiments is implemented.

[0316] The readable storage medium may include but is not limited to: a portable disk, a hard disk, a random access memory, a read-only memory, an erasable programmable read-only memory, an optical storage device, a magnetic storage device or any suitable combination of the above.

[0317] Example 5

[0318] The embodiments of the present disclosure also provide a computer program product, including a computer program, which implements any of the above-mentioned endoscopic robot control methods when executed by a processor.

[0319] Among them, the program code for executing the computer program product of the present disclosure can be written in any combination of one or more programming languages, and the program code can be executed completely on the user device, partially on the user device, as an independent software package, partially on the user device and partially on a remote device, or completely on the remote device.

[0320] Although the specific embodiments of the present disclosure are described above, those skilled in the art should understand that this is only an example, and the protection scope of the present disclosure is defined by the appended claims. Those skilled in the art may make various changes or modifications to these embodiments without departing from the principles and essence of the present disclosure, but these changes and modifications all fall within the protection scope of the present disclosure.

Claims

1. A method for controlling an endoscope robot, characterized in that: The endoscopic robot comprises a first mechanical joint, a second mechanical joint, a third mechanical joint, a fourth mechanical joint, a first virtual joint and a second virtual joint, wherein the first virtual joint is realized by image scaling of the endoscopic robot, and the second virtual joint is realized by image rotation of the endoscopic robot, and the endoscopic robot control method comprises: Determine, according to the target coordinate point of the endoscopic robot, a first translation distance of the first mechanical joint, a first rotation angle of the second mechanical joint, a second rotation angle of the third mechanical joint, a third rotation angle of the fourth mechanical joint, a second translation distance of the first virtual joint, and a fourth rotation angle of the second virtual joint; The position of the endoscopic robot is controlled to be updated to the target coordinate point according to the first translation distance, the first rotation angle, the second rotation angle, the third rotation angle, the second translation distance and the fourth rotation angle.

2. The endoscopic robot control method according to claim 1, characterized in that: The step of controlling the position of the endoscopic robot to update to the target coordinate point according to the first translation distance, the first rotation angle, the second rotation angle, the third rotation angle, the second translation distance and the fourth rotation angle comprises: Determine the optimal imaging distance based on the focal length and image distance; Determine a zoom factor according to the optimal imaging distance and the second translation distance; Determine the third image coordinates corresponding to the first virtual joint after translation according to the zoom factor, the first image coordinates corresponding to the initial position of the first virtual joint, the fourth rotation angle and the optimal imaging distance; determine the fourth image coordinates corresponding to the second virtual joint after rotation according to the zoom factor, the second image coordinates corresponding to the initial position of the second virtual joint, the fourth rotation angle and the optimal imaging distance.

3. The endoscopic robot control method according to claim 2, characterized in that: The step of determining the third image coordinates corresponding to the first virtual joint after translation according to the zoom factor, the first image coordinates corresponding to the initial position of the first virtual joint, the fourth rotation angle and the optimal imaging distance, and the step of determining the fourth image coordinates corresponding to the second virtual joint after rotation according to the zoom factor, the second image coordinates corresponding to the initial position of the second virtual joint, the fourth rotation angle and the optimal imaging distance comprises: The third image coordinates and the fourth image coordinates are determined by the following formula: Among them, z c Used to characterize the optimal imaging distance; An x-axis coordinate for representing the third image coordinate or the fourth image coordinate; Used to represent the y-axis coordinate of the third image coordinate or the fourth image coordinate; θ is used to represent the fourth rotation angle; k is used to represent the zoom factor; x is used to represent the x-axis coordinate of the first image coordinate or the second image coordinate; y is used to represent the y-axis coordinate of the first image coordinate or the second image coordinate.

4. The endoscopic robot control method according to claim 2, characterized in that: The step of determining the optimal imaging distance according to the focal length and the image distance comprises: The optimal imaging distance is determined by the following formula: Among them, z c It is used to characterize the optimal imaging distance; f is used to characterize the focal length; d is used to characterize the image distance; and / or, The step of determining the zoom factor according to the optimal imaging distance and the second translation distance comprises: The zoom factor is determined by the following formula: Among them, k is used to represent the scaling factor; z c It is used to characterize the optimal imaging distance; δ is used to characterize the second translation distance.

5. The endoscopic robot control method according to claim 1, characterized in that: The step of determining, according to the target coordinate point of the endoscopic robot, a first translation distance of the first mechanical joint, a first rotation angle of the second mechanical joint, a second rotation angle of the third mechanical joint, a third rotation angle of the fourth mechanical joint, a second translation distance of the first virtual joint, and a fourth rotation angle of the second virtual joint comprises: Determine a first matrix according to the target coordinate point, where the first matrix is ​​used to represent the spatial position and orientation of the target coordinate point; Acquire a second matrix, where the second matrix is ​​used to characterize the joint position relationship among the first mechanical joint, the second mechanical joint, the third mechanical joint, the fourth mechanical joint, the first virtual joint, and the second virtual joint; The first translation distance of the first mechanical joint, the first rotation angle of the second mechanical joint, the second rotation angle of the third mechanical joint, the third rotation angle of the fourth mechanical joint, the second translation distance of the first virtual joint, and the fourth rotation angle of the second virtual joint are determined according to the first matrix and the second matrix.

6. The endoscopic robot control method according to claim 5, characterized in that: The step of determining the first matrix according to the target coordinate point comprises: The first matrix is ​​determined according to the target coordinate point using the following formula: T 11 -sin(q4)sin(q6)sin(q2+q3)+cos(q6)cos(q2+q3)! T 12 -sin(q4)sin(q6)sin(q2+q3)+cos(q6)cos(q2+q3)! T 13 =-sin(q2+q3)cos(q4); <h2 style=";text-align:left;direction:ltr">T<h2 style=";text-align:left;direction:ltr"> 14 <h2 style=";text-align:left;direction:ltr"> = -L2sin(q2) - L3sin(q2+q3) - (L4+q5)sin(q2+q3)cos(q4) T 21 =sin(q6)cos(q4); T 22 =cos(q4)cos(q6); T 23 =-sin(q4); T 24 =(-L4-q5)sin(q4); T 31 <sin(q4)sin(q6)cos(q2+q3)+sin(q2+q3)cos(q6)! <h2 style=";text-align:left;direction:ltr">T<h2 style=";text-align:left;direction:ltr"> 32 <h2 style=";text-align:left;direction:ltr"> (sin(q4)sin(q6)cos(q2+q3)-sin(q6)sin(q2+q3)) T 34 =L1+L2cos(q2)+L3cos(q2+q3)+q1+(L4+q5)cos(q4)cos(q2+q3); T 41 ＝0； T 42 ＝0； T 43 ＝0； T 44 ＝1； Among them, T is used to represent the first matrix, T 11 The formula for representing the first row and first column of the first matrix, T 12 The formula for representing the first row and second column of the first matrix, T 13 The formula for representing the first row and third column of the first matrix, T 14 The formula for representing the first row and fourth column of the first matrix, T 21 The formula for representing the second row and first column of the first matrix, T 22 The formula for representing the second row and second column of the first matrix, T 23 The formula for representing the second row and third column of the first matrix, T 24 The formula for representing the second row and fourth column of the first matrix, T 31 The formula for representing the third row and first column of the first matrix, T 32 The formula for representing the third row and second column of the first matrix, T 33 The formula for representing the third row and third column of the first matrix, T 34 The formula for representing the third row and fourth column of the first matrix, T 41 The formula for representing the fourth row and first column of the first matrix, T 42 The formula for representing the fourth row and second column of the first matrix, T 43 The formula for representing the fourth row and third column of the first matrix, T 44 A formula for characterizing the fourth row and fourth column of the first matrix, q1 is used to characterize the first translation distance of the first mechanical joint, q2 is used to characterize the first rotation angle of the second mechanical joint, q3 is used to characterize the second rotation angle of the third mechanical joint, q4 is used to characterize the third rotation angle of the fourth mechanical joint, q5 is used to characterize the second translation distance of the first virtual joint, and q6 is used to characterize the fourth rotation angle of the second virtual joint; L1 is used to characterize the connecting rod length of the first mechanical joint and the second mechanical joint, L2 is used to characterize the connecting rod length of the second mechanical joint and the third mechanical joint, L3 is used to characterize the connecting rod length of the third mechanical joint and the fourth mechanical joint, and L4 is used to characterize the connecting rod length of the first virtual joint and the second virtual joint; The step of obtaining the second matrix comprises: The second matrix is ​​obtained by the following formula: Among them, T act Used to represent the second matrix, T act11 To characterize T 11 The value of T act12 To characterize T 12 The value of T act13 To characterize T 13 The value of T act14 To characterize T 14 The value of T act21 To characterize T 21 The value of T act22 To characterize T 22 The value of T act23 To characterize T 23 The value of T act24 To characterize T 24 The value of T act31 To characterize T 31 The value of T act32 To characterize T 32 The value of T act33 To characterize T 33 The value of T act34 To characterize T 34 The value of T act41 To characterize T 41 The value of T act42 To characterize T 42 The value of T act43 To characterize T 43 The value of T act44 To characterize T 44 The value of The step of determining the first translation distance of the first mechanical joint, the first rotation angle of the second mechanical joint, the second rotation angle of the third mechanical joint, the third rotation angle of the fourth mechanical joint, the second translation distance of the first virtual joint, and the fourth rotation angle of the second virtual joint according to the first matrix and the second matrix comprises: The first translation distance of the first mechanical joint, the first rotation angle of the second mechanical joint, the second rotation angle of the third mechanical joint, the third rotation angle of the fourth mechanical joint, the second translation distance of the first virtual joint, and the fourth rotation angle of the second virtual joint are determined by the following formula: q1 T 34 -L1-L2cos(q2)-(L3+(L4+q5)cos(q4)cos(q2+q3); <h2 style=";text-align:left;direction:ltr">q2=-arcsin((T<h2 style=";text-align:left;direction:ltr"> 14 <h2 style=";text-align:left;direction:ltr"> +((L4+q5)cos(q4)+L3)sin(q2+q3)) / L2); q3=arctan(sin(q2+q3) / cos(q2+q3))-q2; q4=-arcsin(T 23 ); q5=-T 24 / sin(q4)-L4; q6=arctan(T 21 ,T 22 ); Among them, q1 is used to characterize the first translation distance of the first mechanical joint, q2 is used to characterize the first rotation angle of the second mechanical joint, q3 is used to characterize the second rotation angle of the third mechanical joint, q4 is used to characterize the third rotation angle of the fourth mechanical joint, q5 is used to characterize the second translation distance of the first virtual joint, and q6 is used to characterize the fourth rotation angle of the second virtual joint.

7. An endoscope robot control system, characterized in that: The endoscopic robot comprises a first mechanical joint, a second mechanical joint, a third mechanical joint, a fourth mechanical joint, a first virtual joint and a second virtual joint, wherein the first virtual joint is realized by image scaling of the endoscopic robot, and the second virtual joint is realized by image rotation of the endoscopic robot, and the endoscopic robot control system comprises: a determination module, configured to determine, according to a target coordinate point of the endoscopic robot, a first translation distance of the first mechanical joint, a first rotation angle of the second mechanical joint, a second rotation angle of the third mechanical joint, a third rotation angle of the fourth mechanical joint, a second translation distance of the first virtual joint, and a fourth rotation angle of the second virtual joint; An updating position module is used to control the position of the endoscopic robot to update to the target coordinate point according to the first translation distance, the first rotation angle, the second rotation angle, the third rotation angle, the second translation distance and the fourth rotation angle.

8. An electronic device comprising a memory, a processor, and a computer program stored in the memory and used to run on the processor, characterized in that: When the processor executes the computer program, the endoscopic robot control method according to any one of claims 1 to 6 is implemented.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the endoscopic robot control method according to any one of claims 1 to 6 is implemented.

10. A computer program product, comprising a computer program, characterized in that When the computer program is executed by a processor, the endoscopic robot control method according to any one of claims 1 to 6 is implemented.

Images