Target following control method, mirror-holding robot and computer-readable medium

Abstract

The present application relates to a target following control method, a mirror holding robot, and a computer-readable medium. In the target following control method, the origin of the coordinate system (i.e., {f n}) fixed to the endoscope coincides exactly with the RCM point. Without motion constraints in terms of position or velocity, a motion law in which the {f n} attitude changes while the position remains unchanged will naturally occur, that is, the endoscope has no displacement and velocity along the RCM coordinate system in the RCM coordinate system, so as to realize the control of the endoscope to maintain the position of the RCM point. The kinematic model established by the Jacobian matrix J obtained using the above D-H coordinate system can directly describe the relationship between the differential motion of the endoscope and the change in the angles of the rotation joints of the mirror holding arm during the control period, enabling the present application to avoid solving the inverse solution of the mirror holding arm and planning the spatial trajectory without RCM constraints. The motion caused by this control method naturally satisfies the RCM constraints, simplifies the solution of the rotation joint angles of the instrument holding arm, and provides accurate and real-time visual feedback to the operator.

Description

Technical Field

[0001] The present application relates to the technical field of medical device auxiliary control, and particularly to a target following control method, a mirror holding robot, and a computer-readable medium. Background Art

[0002] In endoscopic surgery, a mirror holding robot can assist a surgeon in automatically adjusting the surgical field of view. The mirror holding robot realizes the rapid positioning of the endoscope through visual feedback, and then autonomously adjusts the surgical field of view to assist the doctor in completing the surgical operation.

[0003] The target following of the mirror holding robot, that is, according to the target pose given by the visual positioning program, controlling the mirror holding robot to continuously move, and under the constraint of the RCM, making the endoscope continuously follow the movement of the target, so that the target is always in a suitable position in the field of view image.

[0004] In the prior art, the control method of the mirror holding robot is mostly realized based on the inverse kinematics of the mirror holding arm. It is necessary to establish a coordinate system at the endoscopic connector, the RCM point, and the end of the endoscope respectively. When solving the relationship between the endoscope lens speed and the mirror holding arm speed, it is necessary to consider the RCM constraints at the position and speed levels at the same time to obtain the movement law of the endoscope that maintains the RCM point; and the process of solving the inverse kinematics solution using the analytical method is cumbersome, and it is difficult to obtain an accurate analytical solution for some configurations. In addition, using the numerical solution iteration method to solve, the real-time performance of the algorithm is difficult to guarantee, and there are errors. Under the constraint of the RCM, the planning of tracking the target trajectory cannot be carried out like in free space. Summary of the Invention

[0005] Based on this, it is necessary to provide a target following control method, a mirror holding robot, and a computer-readable medium for the problems that the process of solving the inverse kinematics solution by the analytical method is cumbersome, it is difficult to obtain an accurate analytical solution for some configurations, using the numerical solution iteration method to solve, the real-time performance of the algorithm is difficult to guarantee, there are errors, and under the constraint of the RCM, the planning of tracking the target trajectory cannot be carried out like in free space.

[0006] The present application provides a target following control method applied to a mirror holding robot. The mirror holding robot includes a mirror holding arm for controlling an endoscope to move around an RCM point. The mirror holding arm has a plurality of linkages connected by rotating joints. The target following control method is characterized in that it includes:

[0007] Receiving image data input by the endoscope and generating a field of view image according to the image data;

[0008] Obtaining the coordinates of the image center point of the field of view image and the coordinates of the target center point of the target to be followed in the field of view image;

[0009] Determine whether the target center point enters the set threshold range centered on the image center point;

[0010] If the target point does not enter the set threshold range centered on the image center point, establish an image coordinate system with the image center point as the origin, establish an RCM coordinate system with the RCM point as the origin, and establish a mapping relationship from the image coordinate system to the RCM coordinate system;

[0011] Calculate the differential motion of the endoscope in one control period according to the projection of the target center point on the RCM coordinate system;

[0012] Input the differential motion of the endoscope in one control period into a kinematic model, so that the kinematic model outputs the change amount of each rotation joint angle of the endoscope holding arm in this control period;

[0013] Output a control signal according to the change amount of each rotation joint angle of the endoscope holding arm in the control period to control the movement of the image center point towards the target center point;

[0014] The kinematic model is established according to a Jacobian matrix J describing the relationship between the joint space velocity of the endoscope holding arm and the velocity at the RCM point in the Cartesian space; the Jacobian matrix J is obtained based on the D-H coordinate system of the endoscope holding arm; the D-H coordinate system of the endoscope holding arm is represented by {f i}, where i (i = 0, 1, 2,..., n) is the serial number of the rotation joint of the endoscope holding arm;

[0015] In the D-H coordinate system of the endoscope holding arm, {f 0} is the reference coordinate system, and its pose remains unchanged with the world coordinate system; the origin of {f 0} coincides with the initial RCM point position, the z-axis of {f n} points to the direction pointed by the endoscope, and the x-axis of {f n} points to be determined according to the standard D-H rule. n} is determined according to the standard D-H rule.

[0016] This application also provides an endoscope holding robot, including:

[0017] A processor for executing the target following control method described above;

[0018] An endoscope electrically connected to the processor;

[0019] A display device for displaying the image captured by the lens of the endoscope, and the display device is communicatively connected to the processor.

[0020] An endoscope holding arm, and the endoscope is arranged on the endoscope holding arm;

[0021] The mirror-holding arm is electrically connected to the processor, and the processor controls the mirror-holding arm to adjust the captured image of the endoscope according to the image captured by the lens of the endoscope.

[0022] The present application also provides a computer-readable medium, on which a computer program is stored. When the computer program is executed by a processor, the target following control method described above is implemented.

[0023] The present application relates to a target following control method, a mirror-holding robot, and a computer-readable medium. The target following control method makes the origin of the coordinate system (i.e., {f n}) fixed to the endoscope coincide exactly with the RCM point. Without motion constraints at the position or velocity level, a motion law where the attitude of {f n} changes while the position remains unchanged will naturally occur. That is, the endoscope has no displacement and velocity along the RCM coordinate system in the RCM coordinate system, thus realizing the control of the endoscope to maintain the position of the RCM point. Using the Jacobian matrix J established by the above D-H coordinate system to obtain the kinematic model can directly describe the relationship between the differential motion of the endoscope and the change in the angles of the rotation joints of the mirror-holding arm during the control period, enabling the present application to avoid solving the inverse solution of the mirror-holding arm and planning the space trajectory without RCM constraints. The motion caused by this control method naturally satisfies the RCM constraints, simplifies the solution of the rotation joint angles of the instrument-holding arm, and provides accurate and real-time visual feedback to the operator. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] The drawings constituting a part of the present application are used to provide a further understanding of the present application, making other features, objectives, and advantages of the present application more obvious. The schematic embodiments and descriptions of the drawings of the present application are used to explain the present application and do not constitute an improper limitation of the present application.

[0025] Figure 1 is a schematic flowchart of a target following control method according to an embodiment of the present application,

[0026] Figure 2 is a schematic diagram of the D-H coordinate system in a target following control method according to an embodiment of the present application,

[0027] Figure 3 is a schematic diagram of the mapping from the image coordinate system to the RCM coordinate system in a target following control method according to an embodiment of the present application.

[0028] Figure 4 is a schematic diagram comparing the shooting angles of a rigid endoscope and a flexible endoscope in a target following control method according to an embodiment of the present application.

[0029] Figure 5It is a schematic diagram of the endoscopic coordinate system in the target following control method according to an embodiment of the present application.

[0030] Figure 6 It is a structural block diagram of a lens-holding robot provided by an embodiment of the present application.

[0031] Figure 7 It is a three-dimensional schematic diagram of the lens-holding robotic arm in the lens-holding robot provided by an embodiment of the present application.

[0032] Figure 8 It is a system topology diagram of the lens-holding robot provided by an embodiment of the present application.

[0033] Figure 9 It is a structural schematic diagram of the endoscope in the lens-holding robot provided by an embodiment of the present application.

[0034] Reference numerals:

[0035] 100 - lens-holding robot; 110 - processor; 120 - display device; 130 - endoscope; 131 - endoscope body; 132 - lens; 133 - bendable joint; 140 - lens-holding robotic arm. Detailed implementation manners

[0036] In order to make the objectives, technical solutions and advantages of the present application clearer, the present application will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0037] The present application provides a target following control method. It should be noted that the vision control method provided by the present application does not limit its execution subject. Optionally, the vision control method provided by the present application is applied to a lens-holding robot. When applied to the lens-holding robot, the lens-holding robot includes a lens-holding arm for manipulating the endoscope to move around an RCM point, and the lens-holding arm has a plurality of linkages connected by rotating joints.

[0038] As Figure 1 shown, in an embodiment of the present application, the target following control method includes the following S100 to S700:

[0039] S100, receiving the image data input by the endoscope and generating a vision image according to the image data.

[0040] S200, obtaining the coordinates of the image center point of the vision image and the coordinates of the target center point of the target to be followed in the vision image.

[0041] S300, determining whether the target center point enters a set threshold range centered on the image center point.

[0042] S400. If the target point does not enter the set threshold range centered on the center point of the image, an image coordinate system is established with the center point of the image as the origin, an RCM coordinate system (i.e., the remote center of motion) is established with the RCM point as the origin, and a mapping relationship from the image coordinate system to the RCM coordinate system is established.

[0043] S500. Calculate the differential motion of the endoscope in one control cycle according to the projection of the target center point on the RCM coordinate system.

[0044] S600. Input the differential motion of the endoscope in the one control cycle into a kinematic model so that the kinematic model outputs the change amounts of the rotation joint angles of the endoscope holding arm in this control cycle.

[0045] S700. Output a control signal according to the change amounts of the rotation joint angles of the endoscope holding arm in the control cycle to control the movement of the image center point towards the target center point.

[0046] Specifically, in each control cycle, the above S100 to S700 are repeated until the target center point enters the set threshold range centered on the center point of the image, realizing the tracking of the target to be followed.

[0047] Specifically, the kinematic model is established according to a Jacobian matrix J that describes the relationship between the joint space velocity of the endoscope holding arm and the velocity at the RCM point in the Cartesian space. The Jacobian matrix J is obtained based on the D-H coordinate system of the endoscope holding arm.

[0048] As Figure 2 shown, using the standard D-H method, a coordinate system fixed on its connecting rod is established at each rotation joint of the endoscope holding arm. The D-H coordinate system of the endoscope holding arm is represented by {f i}, where i (i = 0, 1, 2,..., n) is the serial number of the rotation joint of the endoscope holding arm.

[0049] In the D-H coordinate system of the endoscope holding arm, {f 0} is the reference coordinate system, {f 0} keeps the pose unchanged with the world coordinate system and can be used as the reference coordinate system for the end pose of the endoscope holding robot. The origin of {f n} coincides with the position of the initial RCM point, the z-axis of {f n} points to the direction where the endoscope points, and the x-axis of {f n} points to be determined according to the standard D-H rule.

[0050] In this embodiment, by making the origin of the coordinate system (i.e., {f n}) fixed on the endoscope coincide exactly with the RCM point, without motion constraints at the position or velocity level, {f n will naturally be generated.}The motion law with unchanged position despite attitude change, that is, the endoscope has no displacement and velocity along the RCM coordinate system in the RCM coordinate system, so as to realize the endoscope control for maintaining the position of the RCM point. The kinematic model established by the Jacobian matrix J obtained using the above D-H coordinate system can directly describe the relationship between the differential motion of the endoscope and the change amount of the angles of the rotation joints of the endoscope holding arm during the control period, enabling this application to eliminate the need to solve the inverse solution of the endoscope holding arm and plan the spatial trajectory without the RCM constraint. The motion caused by this control method naturally satisfies the RCM constraint, simplifies the solution of the rotation joint angles of the instrument holding arm, and provides accurate and real-time visual feedback to the operator.

[0051] During a control period of the endoscope holding robot, first, the visual system obtains the two-dimensional coordinate system of the tracking target in the endoscopic image. In the traditional method, usually, this two-dimensional coordinate system and the calibrated camera parameters are used to estimate the three-dimensional coordinates of the target relative to the camera. However, since the optical system of the endoscope needs to change internal parameters such as the focal length during use, the above traditional method cannot be used.

[0052] As Figure 3 shown, in an embodiment of this application, the S400 includes the following S410 and S420.

[0053] S410, establish an image coordinate system, with the center point of the image as the origin, and the x-axis of the image coordinate system is defined as x parallel to one side of the field of view image img , and the y-axis of the image coordinate system is defined as y parallel to the other adjacent side of the field of view image img . Obtain the coordinates of the target point in the endoscopic image in the image coordinate system through the visual system, denoted as t.

[0054] S420, define the RCM coordinate system, with its origin as the RCM point. The x-axis of the RCM coordinate system is defined as x parallel to x img , and the y-axis of the RCM coordinate system is defined as y parallel to y rcm , project t onto a virtual hemispherical surface with the origin of the RCM coordinate system as the center of the sphere and a diameter greater than the diagonal length of the field of view image, denoted as t'. img rcm

[0055] As Figure 3 shown, in an embodiment of this application, the S500 includes the following S510 and S530.

[0056] S510, calculate the projections of t' on the x-axis and y-axis of the RCM coordinate system respectively rcm rcm

[0057] S520, project the t' onto the x-axis and y-axis of the RCM coordinate system respectively rcm ​rcm The projection on the axis is multiplied by the defined velocity conversion coefficient to obtain the velocity components of the endoscope rotating around the RCM coordinate system on the x rcm axis and the y rcm axis.

[0058] S530, multiply the velocity components of the endoscope rotating around the RCM coordinate system on the x rcm axis and the y rcm axis by the control period to obtain the differential motion of the endoscope rotating around the RCM coordinate system on the x rcm axis and the y rcm axis in one control period.

[0059] In an embodiment of the present application, if n = 6, it means that the endoscope holding arm of the present application has 6 rotating joints, and the kinematic model is specifically:

[0060]

[0061] where, J - represents the inverse matrix of the Jacobian matrix J. d x , d y , d z represent the linear velocities of the endoscope holding arm clamping the endoscope along the x, y, and z axes of {f 6}. δ x , δ y , δ z represent the angular velocities of the endoscope holding arm clamping the endoscope rotating around the x, y, and z axes of {f 6}. dθ 1 to dθ 6 represent the angular velocities of the rotating joints of the endoscope holding arm.

[0062] If n ≠ 6, it means that the number of rotating joints of the endoscope holding arm of the present application is greater than 6 or less than 6, and the kinematic model is specifically:

[0063]

[0064] where, J + represents the generalized inverse matrix of the Jacobian matrix J. d x , d y , d z represent the linear velocities of the endoscope holding arm clamping the endoscope along the x, y, and z axes of {f n}. δ x , δ y , δ z represent the angular velocities of the endoscope holding arm clamping the endoscope rotating around the x, y, and z axes of {f n}. dθ 1 to dθ n represent the angular velocities of the rotating joints of the endoscope holding arm.

[0065] In one embodiment of the present application, the S600 includes the following S610.

[0066] S610 assigns the differential motions of the endoscope rotating around the RCM coordinate system on the x rcm axis and the y rcm axis in a control period to δ x and δ y in Formula 1 or Formula 2 respectively, and calculates the angular change amounts of the respective rotating joints of the endoscope holding arm in this control period.

[0067] Specifically, when the number of rotating joints of the endoscope holding arm is 6, the differential motions of the endoscope rotating around the RCM coordinate system on the x rcm axis and the y rcm axis in a control period are assigned to Formula 1. When the number of rotating joints of the endoscope holding arm is greater than 6 or less than 6, the differential motions of the endoscope rotating around the RCM coordinate system on the x rcm axis and the y rcm axis in a control period are assigned to Formula 2.

[0068] Specifically, as Figure 2 shown, the method for solving the Jacobian matrix J in the kinematic model in the present application includes the following steps:

[0069] 1) Represent the homogeneous transformation matrix (4×4 matrix) A between each coordinate system in the D-H coordinate system with four column vectors n, o, a, and p, where A 1 is the homogeneous transformation matrix between {f 0} and {f 1}, A 2 is the homogeneous transformation matrix between {f 1} and {f 2}. A 2 , A 3 …A n and so on.

[0070] 2) Calculate the homogeneous transformation matrix where and so on until

[0071] 3) To each row and column element in can be represented by the symbol , then in the Jacobian matrix , J 1i =-n x p y +n y p x , J2i =-o x p y +o y p x , J 3i =-a y p x +a y p x , J 4i =n z , J 5i =o z , J 6i =a z . When solving for the first column in J, the n, o, a, p, etc. involved are from the The second column is from the And so on, the solution of the Jacobian matrix J is completed.

[0072] In an embodiment of the present application, when the number of rotating joints of the mirror - holding arm is 6, for the Jacobian matrix J, there is:

[0073]

[0074] Multiply both ends of Equation 3 on the left by the inverse matrix J⁻¹ of the Jacobian matrix J - The above - mentioned Equation 1 is obtained.

[0075] In an embodiment of the present application, when the number of rotating joints of the mirror - holding arm is greater than 6 or less than 6, for the Jacobian matrix J, there is:

[0076]

[0077] Multiply both ends of Equation 4 on the left by the generalized inverse matrix J⁺ of the Jacobian matrix J + The above - mentioned Equation 2 is obtained.

[0078] In an embodiment of the present application, for the mirror - holding robot that holds a rigid endoscope, the calibration of the RCM point includes the following steps.

[0079] 1) First, move the robotic arm so that its end drives the endoscope to insert into the RCM point.

[0080] 2) Zero - force dragging or individually controlling the movement of each rotating joint can be used to make the end of the endoscope reach the inserted RCM point.

[0081] 3) After inserting the endoscope into the RCM point and adjusting the initial position, click the calibration button through the upper computer interface, then the control system can calculate the relative position of the RCM point with respect to the mirror - holding robot based on the angles at each rotating joint and the known link lengths.

[0082] Among them, zero-force dragging means establishing the dynamic model of the mirror-holding robot, and according to the dynamic model, the torque output by the joint motors of the mirror-holding robot in real time can compensate for the gravity of the rods of the mirror-holding robot, the friction at the joints, etc., so that the operator can directly drag the mirror-holding robot to move.

[0083] As Figure 4 shown, when the mirror-holding arm holds the self-designed flexible endoscope, when the curvature of the flexible endoscope is small, target tracking similar to that of a rigid endoscope can be achieved. When the curvature of the flexible endoscope is large, more poses of the target can be observed.

[0084] Utilizing the above characteristics of the flexible endoscope, in an embodiment of the present application, before S300, the target following control method further includes the following S210 to S240.

[0085] S210, calculate the straight-line distance between the target center point and the image center point.

[0086] S220, determine whether the straight-line distance is greater than a set distance threshold.

[0087] S230, if the straight-line distance is less than or equal to the set distance threshold, output a control signal to drive the deformation of the bendable joint of the endoscope to control the movement of the image center point towards the target center point.

[0088] S240, if the straight-line distance is greater than the set distance threshold, execute the determination of whether the target center point enters the set threshold range centered on the image center point.

[0089] In this example, when the target to be followed moves within a small range, only the deformation of the bendable joint of the endoscope is used to move the lens of the endoscope to observe the lesion. When the target to be followed moves within a large range or is observed at a special angle, the mirror-holding arm is used to move the endoscope. Adopting such a control method reduces the movement of the external rotating joints of the mirror-holding arm and causes less interference to the operator.

[0090] As Figure 5 shown, in an embodiment of the present application, a segmented target tracking method combining a bendable joint and an instrument arm can also be adopted. The segmented target tracking method includes the following S251 to S253.

[0091] S251, establish a laparoscope coordinate system at the end of the endoscope, and the z-axis z cam of the coordinate system coincides with the optical axis of the endoscope, and coincides with the x-axis x cam and y-axis y cam of the coordinate system to satisfy the right-hand rule.

[0092] S252. Obtain the position of the target center point in the endoscopic coordinate system, and drive the deformable joint of the endoscope to control the lens to follow the target center point.

[0093] S253. If the curvature of the deformable joint of the endoscope has reached the maximum value allowed by it, but the target to be followed is still moving, activate the degrees of freedom of the extracorporeal endoscope holding arm to move the entire flexible endoscope, so as to have the ability to realign the target.

[0094] This application also provides a robotic endoscope holder.

[0095] As Figure 6 and Figure 7 shown, in an embodiment of this application, the robotic endoscope holder 100 includes: a processor 110, an endoscope 120, a display device 130, and an endoscope holding arm 140.

[0096] The processor is used to execute the target following control method mentioned above. The endoscope is electrically connected to the processor. The display device is used to display the image captured by the lens of the endoscope, and the display device is communicatively connected to the processor.

[0097] Specifically, the display device 120 includes a touch screen.

[0098] The endoscope is arranged on the endoscope holding arm. The endoscope holding arm is used to control the endoscope to move around an RCM point (i.e., the remote center of motion). The endoscope holding arm has a plurality of linkages connected by rotating joints. The robotic endoscope holding arm 140 is installed on a movable trolley for convenient position transfer. The endoscope holding arm is electrically connected to the processor, and the processor controls the endoscope holding arm to adjust the captured image of the endoscope according to the image captured by the lens of the endoscope.

[0099] As Figure 8 shown, in an embodiment of this application, the robotic endoscope holder further includes a control system, a host computer interface program for human-computer interaction, and a visual positioning program that can acquire endoscopic images, identify and track targets (such as surgical instruments, lesion areas, etc.) according to the endoscopic images, and calculate the pose of the targets relative to the endoscope. For clamping a flexible endoscope, on the basis of the above-described arrangement of the degrees of freedom of the robotic endoscope holder, the bending degree of freedom of the endoscope itself is increased.

[0100] As Figure 9 shown, in an embodiment of this application, the endoscope 130 includes: an endoscope body 131, a lens 132, and a deformable joint 133.

[0101] The endoscope body 131 is provided with a driving device. The lens 132 is used to capture images. One end of the deformable joint 133 is connected to the endoscope body 131, and the other end is connected to the lens 132.

[0102] The driving device is communicatively connected to the processor 110, and is configured to output a torque in response to a control signal output by the processor 110, the torque driving the deformable joint 133 to deform so that the center of the image captured by the lens 132 moves towards the target point.

[0103] A computer-readable medium has a computer program stored thereon, wherein when the computer program is executed by a processor, it implements the target following control method mentioned above.

[0104] The technical features of the above-described embodiments can be combined arbitrarily, and there is no limitation on the execution order of the method steps. For the sake of brevity of description, not all possible combinations of the technical features in the above-described embodiments are described. However, as long as there is no contradiction in the combination of these technical features, it should be considered as falling within the scope described in this specification.

[0105] The above-described embodiments merely represent several implementation manners of the present application. The description is relatively specific and detailed, but it should not be construed as a limitation on the scope of the patent of the present application. It should be noted that for those of ordinary skill in the art, without departing from the concept of the present application, several modifications and improvements can be made, and these all belong to the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the appended claims.

Claims

1. A target following control method, characterized in that: Applied to a scope holding robot, the scope holding robot includes a scope holding arm for manipulating the endoscope to move around an RCM point, the scope holding arm having a plurality of connecting rods connected by a rotating joint; characterized in that the target following control method includes: receiving image data input by an endoscope and generating a field of view image according to the image data; Acquire the coordinates of the image center point of the field of view image and the coordinates of the target center point of the target to be followed in the field of view image; Determine whether the target center point is within a set threshold range centered on the image center point; If the target center point does not enter the set threshold range centered on the image center point, an image coordinate system is established with the image center point as the origin, an RCM coordinate system is established with the RCM point as the origin, and a mapping relationship between the image coordinate system and the RCM coordinate system is established; According to the projection of the target center point on the RCM coordinate system, the differential motion of the endoscope in one control cycle is calculated; Inputting the differential motion of the endoscope in the control cycle into a kinematic model, so that the kinematic model outputs the angle variation of each rotating joint of the endoscope holding arm in the control cycle; Outputting a control signal according to the angle variation of each rotating joint of the mirror holding arm within the control period to control the image center point to move toward the target center point; The kinematic model is established based on a Jacobian matrix J describing the relationship between the joint space velocity of the mirror holding arm and the velocity at the RCM point in Cartesian space; the Jacobian matrix J is obtained based on the DH coordinate system of the mirror holding arm; the DH coordinate system of the mirror holding arm is Indicates, where i is the rotation joint number of the mirror holding arm, i = 0, 1, 2, ..., n; In the DH coordinate system of the mirror holding arm, is the reference coordinate system, Keep the pose unchanged with the world coordinate system; The origin of coincides with the initial RCM point position. The z-axis points in the direction of the endoscope. The x-axis direction is determined according to standard DH rules.

2. The target following control method according to claim 1, characterized in that: If the target center point does not enter the set threshold range centered on the image center point, an image coordinate system is established with the image center point as the origin, an RCM coordinate system is established with the RCM point as the origin, and a mapping relationship between the image coordinate system and the RCM coordinate system is established, including: Establish an image coordinate system with the center of the image as the origin, and the x-axis of the image coordinate system is defined as parallel to one side of the field of view image. The y-axis of the image coordinate system is defined as parallel to the other adjacent side of the field of view image. ; Obtain the coordinates of the target center point in the laparoscope image in the image coordinate system through the visual system, denoted as t; Define the RCM coordinate system, whose origin is the RCM point, and the x-axis of the RCM coordinate system is defined as Parallel , the y-axis of the RCM coordinate system is defined as Parallel , project t onto a virtual hemispherical surface with the origin of the RCM coordinate system as the center and a diameter greater than the diagonal length of the field of view image, recorded as t'.

3. The target following control method according to claim 2, characterized in that: The differential motion of the endoscope in one control cycle is calculated based on the projection of the target center point on the RCM coordinate system, including Calculate t' in the RCM coordinate system Axis and Projection on axis; The t' is respectively in the RCM coordinate system Axis and The projection on the axis is multiplied by the defined velocity conversion coefficient to obtain the rotation of the endoscope around the RCM coordinate system. Axis and Velocity components on the axis; The endoscope is rotated around the RCM coordinate system. Axis and The velocity component on the axis is multiplied by the control period to obtain the rotation of the endoscope around the RCM coordinate system in one control period. Axis and Differential motion on an axis.

4. The target following control method according to claim 3, characterized in that: The kinematic model is specifically: Formula 1 Where n=6, represents the inverse matrix of the Jacobian matrix J; Indicates that the endoscope is held by the endoscope arm. The linear velocity of the x, y, and z axis translation; Indicates that the endoscope is held by the endoscope holding arm. dθ1 to dθ6 represent the angular velocities of the rotation joints of the mirror holding arm; Alternatively, the kinematic model is specifically: Formula 2; Where n≠6, represents the generalized inverse matrix of the Jacobian matrix J; Indicates that the endoscope is held by the endoscope arm. The linear velocity of the x, y, and z axis translation; Indicates that the endoscope is held by the endoscope holding arm. Angular velocity of the x, y, and z axes; dθ1 to dθ n Indicates the angular velocity of each rotating joint of the mirror holding arm.

5. The target following control method according to claim 4, characterized in that: The step of inputting the differential motion of the endoscope in the control cycle into a kinematic model so that the kinematic model outputs the angle variation of each rotation joint of the endoscope holding arm in the control cycle includes: The endoscope rotates around the RCM coordinate system in one control cycle. Axis and The differential motion on the axis is assigned to the and , calculate the angle change of each rotation joint of the mirror holding arm during this control cycle.

6. The target following control method according to claim 4, characterized in that: The method for solving the Jacobian matrix J in the kinematic model includes: The homogeneous transformation matrix A between the coordinate systems in the DH coordinate system is represented by four column vectors n, o, a, and p, where yes and The homogeneous transformation matrix between ; And so on; Compute the homogeneous transformation matrix ,in , And so on, until we get ; to Each row and column element can be represented by the symbol Represents that the Jacobian matrix middle, , , , , , ; When solving the first column in J, the n, o, a, and p involved come from the , the second column comes from the , and so on, the Jacobian matrix J is solved.

7. The target following control method according to claim 1, characterized in that: Before determining whether the target center point enters a set threshold range centered on the image center point, the target following control method further includes: Calculating the straight-line distance between the target center point and the image center point; Determine whether the straight-line distance is greater than a set distance threshold; If the straight-line distance is less than or equal to the set distance threshold, a control signal is output to drive the bendable joint of the endoscope to deform, so as to control the center point of the image to move toward the center point of the target; If the straight-line distance is greater than the set distance threshold, the step of determining whether the target center point is within a set threshold range centered on the image center point is performed.

8. A mirror-holding robot, characterized in that: include: A processor, configured to execute the target following control method according to any one of claims 1 to 7; an endoscope, electrically connected to the processor; A display device, used to display the image captured by the lens of the endoscope, and the display device is communicatively connected with the processor; A mirror holding arm, on which the endoscope is arranged; The scope holding arm is electrically connected to the processor, and the processor controls the scope holding arm to adjust the image captured by the endoscope according to the image captured by the lens of the endoscope.

9. The mirror-holding robot according to claim 8, characterized in that: The endoscope comprises: The endoscope body is provided with a driving device; A lens, used to capture images; A bendable joint, one end of which is connected to the endoscope body and the other end of which is connected to the lens; The driving device is communicatively connected to the processor, and is used for outputting a torque that drives the bendable joint to deform so that the center point of the image captured by the lens moves toward the target center point in response to a control signal output by the processor.

10. A computer readable medium having a computer program stored thereon, wherein: When the computer program is executed by a processor, the target following control method according to any one of claims 1 to 7 is implemented.

Images