A Visual Servo Control Method for Robotic Arms Considering RCM Constraints

Abstract

This invention provides a visual servo control method for a robotic arm considering RCM constraints, comprising: reading joint angles; calculating the positions and actual insertion depths of the endoscope base, camera optical center, and puncture point in the world coordinate system; extracting target feature points from the image in real time; calculating the lateral error of the puncture point in the local coordinate system of the endoscope base; constructing an RCM-constrained Jacobian matrix of joint speed and the rate of change of this error; using the null projection method, solving for the particular solution of speed and the null basis with RCM constraints as the highest priority; solving for the visual adjustment speed and null projection matrix that satisfy the RCM constraints; solving for the depth adjustment speed; synthesizing the desired speed; converting the desired speed into equality constraints, and converting the joint speed limit, collision avoidance limit, and safe depth into inequality constraints; constructing a quadratic function with the goal of approaching the joint center; and solving for the optimal joint speed to drive the robotic arm.

Description

Technical Field

[0001] This application relates to the field of robot control technology, and in particular to a visual servoing method for a robotic arm that considers RCM constraints. Background Technology

[0002] With the development of medical robot technology, robot-assisted minimally invasive surgery has been widely used due to its advantages such as less trauma, faster recovery, and more precise operation. To reduce the burden on surgeons who frequently adjust their field of vision during surgery, visual servo control methods have been introduced into endoscopic robot systems. This allows the endoscope to automatically track surgical instruments or surgical areas based on image features and maintain the target at the desired position within the field of vision. Endoscopic robots differ from general free-space visual servo robots in that their motion is simultaneously constrained by a remote center of motion (RCM). That is, the endoscope rod can only pitch, yaw, and rotate around the incision point, and move forward and backward along the rod's axis. Therefore, in endoscopic visual servo control, how to strictly satisfy the remote center of motion constraint while achieving image tracking is a key issue affecting system safety and control performance.

[0003] Existing endoscopic visual servo control methods generally suffer from the following shortcomings when handling RCM constraints and visual tracking tasks. First, existing methods often treat remote center of motion constraints as soft constraints, secondary priority tasks, or scalar error terms. When image errors are large or target movements are intense, the visual task and remote center of motion constraints are prone to interference, thus violating the RCM constraints. Second, existing methods often struggle to effectively decouple field of view control from insertion depth control. When the endoscope performs image centering movements, it often exhibits undesirable depth changes, affecting the stability of the surgical field. Third, existing methods focus primarily on upper-level trajectory generation, lacking a low-level solution framework that can unify the handling of safety hard constraints and joint posture optimization. Summary of the Invention

[0004] The purpose of this invention is to provide a visual servo control method for a robotic arm that considers RCM constraints, comprising the following steps: Step S100: Obtain the joint angle vector of the robotic arm at the current moment; calculate the position information of the endoscope base, camera optical center and puncture point in the world coordinate system through robot forward kinematics, and calculate the actual insertion depth of the endoscope at the current moment; at the same time, acquire the images collected by the endoscope in real time and extract the target feature points. Step S200: Under the local reference coordinate system established by the endoscope base, calculate the lateral error component of the puncture point relative to the local reference coordinate system, and construct the RCM constrained Jacobian matrix between the joint velocity and the lateral error change rate. Step S300: Taking RCM constraint as the highest priority task, based on the RCM constraint Jacobian matrix, a lateral error attenuation law is set, and the velocity particular solution satisfying the RCM constraint and the corresponding null basis are solved; taking image centering as the second priority task, the image Jacobian matrix is ​​projected onto the null basis, and the visual accommodation speed under RCM constraint is solved, and the null projection matrix of this visual task is extracted; taking depth adjustment as the third priority task, the depth Jacobian matrix is ​​projected onto the visual null projection matrix, and the depth adjustment speed under RCM constraint and image centering task conditions is solved; the velocity particular solution, visual accommodation speed and depth adjustment speed are synthesized to obtain the desired speed for adjusting the endoscope pose. In step S400, the generated desired speed command is converted into equality constraints, and the absolute speed limit of the robotic arm's joints, the anti-collision limit of the joint position, and the safe insertion depth boundary of the endoscope are converted into inequality constraints. Taking the robotic arm's tendency to move towards the joint center position as the optimization objective, a quadratic objective function is constructed, and the current optimal joint speed command is obtained by solving it, and the robotic arm is driven to move.

[0005] Furthermore, the process of constructing the accurate RCM constraint matrix in step S200 specifically includes: Step S210: Establish a local reference coordinate system at the endoscope base, and represent the position of the puncture point in the local reference coordinate system as follows: (1) in, and This is the lateral error component. The axial component of the puncture point along the endoscope axis; Step S220: Obtain the lateral error vector e xy , (2) Step S230: Obtain the volume velocity of the endoscope base in the local reference coordinate system. S b , (3) in,[ v x , v y , v z [ is the linear velocity vector in the local reference coordinate system, [ ω x , ω y , ω z] is the angular velocity vector in the local reference coordinate system; Step S240: Based on rigid body kinematics, construct the RCM constraint Jacobian matrix between the lateral error rate of change and the body velocity: (4) Wherein, the constraint matrix Represented as: (5) Furthermore, the solution process in step S300, which prioritizes the RCM constraint, specifically includes: Step S311, set the lateral error vector It satisfies the predetermined exponential decay law: (6) in, Here is the RCM error feedback gain matrix; Step S312: Combining the exponential decay law, solve for the particular solution of the volume velocity that satisfies the decay law. : (7) in, for The pseudo-inverse matrix; Step S313: Extract the null basis corresponding to the RCM task. The feasible volume velocity subspace that satisfies the RCM constraints is represented as: (8) in, Let be the null space coefficient vector to be determined. Furthermore, the solution process in step S300, which prioritizes image centering as the second priority task, specifically includes: Step S321, based on the current pixel coordinates of the target feature point Center coordinates of the desired image Constructing image feature errors: (9) Step S322, establish the image error convergence relationship: (10) in, The image feedback gain matrix; Step S323: Calculate the coordinate transfer matrix using the fixed transformation relationship from the local reference coordinate system of the endoscope base to the camera coordinate system, and obtain the image Jacobian matrix in the base coordinate system. ; Step S324: Project the image Jacobian matrix onto the null basis of the RCM task. In the middle, construct a reduced-order visual task matrix. , (11) Step S325: Calculate the visual accommodation speed under RCM constraints. μ v : (12) Step S326: Construct the null projection matrix for the visual task. : (13) in, for The pseudo-inverse matrix, It is a unit array.

[0006] Furthermore, step 3, which prioritizes depth adjustment as the third priority task and the final desired volume velocity synthesis process, specifically includes: Step S331: Calculate the depth error between the actual insertion depth and the expected insertion depth of the endoscope. e d : (14) in, The current insertion depth, The desired insertion depth; Step S332: In the coordinate system of the endoscope base, construct a matrix for extracting the axial motion component. M d : (15) Step S333, convert the matrix M d By projecting sequentially through the RCM null basis and the visual null space, the depth adjustment order mapping matrix is ​​obtained: (16) Step S334, calculate the deep coupling velocity term introduced by executing high-priority tasks: (17) Step S335, solve for the depth adjustment speed of the RCM constraint and image centering task: (18) in, The depth adjustment gain coefficient; Step S336: Combine the solution results at each stage to obtain the final desired volume velocity command in the endoscope base coordinate system. (19) Furthermore, the method for constructing the quadratic objective function in step S400 is as follows: Step S411, set the decision variable vector as follows: (20) in, For the joint speed of the robotic arm, and These are the positive and negative joint position anti-collision relaxation variables, respectively; Step S412: To minimize the reference velocity deviation and penalty relaxation variable towards the joint center position, construct a quadratic objective function: ,(twenty one) in, To approximate the reference velocity at the physical limit center of the joint, Penalize the weights for slack variables.

[0007] Furthermore, the reference speed The following potential energy gradient descent method is used to obtain the following: ,(twenty two) in, For the current joint angle, This is the center of the joint's range of motion. This is half the range of motion of the joint. This represents the joint-centered gain coefficient. The potential energy gradient is given.

[0008] Furthermore, in the step S400 of constructing equality constraints, constraints are established to drive the movement of the robotic arm, specifically including: Step S421, construct the following kinematic equation constraints: ,(twenty three) in, Let be the Jacobian matrix of the endoscope base in its own coordinate system. For the generated desired speed command; Step S422: Construct soft constraints on joint position combining the relaxation variables, hard constraints on absolute joint velocity limiting the maximum joint motion capacity, and hard constraints on depth boundaries limiting the safe advance and retreat range of the endoscope: wherein Combined with the joint position soft constraints of the aforementioned relaxation variables: ,(twenty four) (25) in, and These are the positive and negative joint position anti-collision relaxation variables, respectively. For the joint speed of the robotic arm, and These represent the upper and lower limits of the joint position. To control the cycle; Hard constraints on the absolute velocity of a joint to limit its maximum range of motion: (26) in, and These are the maximum and minimum permissible speeds of the joint, respectively. Hard constraints on depth boundaries that limit the safe advance and retreat range of the endoscope: (27) in, The preset minimum safe insertion depth, For maximum safe insertion depth, This represents the current actual insertion depth. To map joint velocities to the depth Jacobian matrix of the insertion depth change rate, To control the cycle.

[0009] Compared with the prior art, the present invention has the following advantages: (1) The present invention models the lateral deviation of the puncture point under the local reference coordinate system established by the endoscope base and constructs an accurate RCM constraint matrix; compared with the method of treating RCM constraint as a soft constraint, a secondary priority task or a single scalar error, the present invention can more directly and accurately describe the deviation of the puncture point relative to the endoscope axis, which is more conducive to strictly maintaining RCM constraint during visual servoing and improving the safety of movement at the incision. (2) The present invention adopts a strict hierarchical upper-level visual reference generation method based on null space projection, and solves RCM constraint, image centering and depth adjustment in order of priority; among them, the image centering task is completed in the feasible subspace that satisfies the RCM constraint, and the depth adjustment task is further completed in the visual task null space, which effectively reduces the mutual interference between tasks, better solves the problem of coupling between field control and depth control in the prior art, and improves the stability and coordination of visual servoing control. (3) The present invention adopts a quadratic programming optimization solution framework in the lower layer, transforms the desired speed command generated in the upper layer into an equality constraint, and incorporates the absolute speed constraint of the robotic arm joint, the anti-collision constraint of the joint position, and the boundary constraint of the safe insertion depth of the endoscope into the optimization solution process. Compared with the simple amplitude limiting method after the control output, the present invention can consider motion tracking requirements and system physical boundaries at the same time in the solution stage, which is more conducive to ensuring the feasibility of the control command and the overall operational safety. (4) The present invention introduces the goal of the robotic arm tending to the joint center position in the lower layer optimization, which helps to avoid the robotic arm working near the joint limit for a long time, thereby reducing the risk of joint overrun and improving the operational stability and robustness of the system in the continuous control process. Attached Figure Description

[0010] Figure 1 This is a schematic diagram of the overall process of the control method of the present invention.

[0011] Figure 2 This is a graph showing the change in system insertion depth over time when the control method of this invention is used.

[0012] Figure 3 This is a graph showing the change in system insertion depth error over time when the control method of this invention is used.

[0013] Figure 4 This is a graph showing the change of remote motion center error over time when using the control method of the present invention.

[0014] Figure 5 This is a graph showing the change of image error over time when the control method of the present invention is used.

[0015] Figure 6The graph shows the changes in RCM error and RCM error using scalar constraints over time when using the control method of this invention.

[0016] Figure 7 A graph showing the insertion depth over time under the condition of dynamic desired depth when using the control method of the present invention.

[0017] Figure 8 A graph showing the change of insertion depth error over time under the condition of dynamic desired depth when using the control method of the present invention. Detailed Implementation

[0018] Combination Figures 1 to 8 The present invention provides a vision servo control method for a robotic arm that considers RCM constraints, comprising the following steps: Step 1: Obtain system status information. Obtain the joint angle vector of the robotic arm at the current moment. Based on the robot's forward kinematics, the optical center of the endoscope camera in the world coordinate system is calculated. The position coordinates below Endoscope base in world coordinate system The position coordinates below And the unit direction vector of the endoscope rod in the world coordinate system. .

[0019] Suppose the puncture point in space is fixed at The position below is Then the current insertion depth of the endoscope This can be represented as the projection of the puncture point onto the camera's optical center along the axis of the rod, i.e. (1) in, For the camera's optical center in the world coordinate system The following position coordinates, For the endoscope base in the world coordinate system The following position coordinates, is the unit direction vector of the endoscope rod in the world coordinate system.

[0020] Simultaneously, obtain the current pixel coordinates of the target feature in the endoscopic image. Set the target desired pixel coordinates as And set the desired insertion depth of the endoscope to be This serves as the input for generating the upper-level visual reference, and then proceeds to step 2.

[0021] Step 2: Construct an accurate RCM constraint matrix in the coordinate system of the endoscope base. This step is used to establish a local reference coordinate system for the endoscope base. A two-dimensional lateral error model is constructed, and the mapping relationship between joint velocity and lateral error change rate is further established.

[0022] First, place the puncture point in the local reference coordinate system. The following is represented as: (2) in, and These are the two lateral error components of the puncture point relative to the endoscope base in the local reference coordinate system. This represents the axial projection component of the puncture point along the axis of the rod.

[0023] Let the lateral error vector be: (3) Establish a local reference coordinate system The speed at which it descends is: (4) According to the kinematics of a rigid body, the rate of change of the lateral error at the puncture point satisfies: (5) Wherein, the error mapping matrix is : (6) Step 3: Generation of upper-level visual references based on null-spatial projection. This step is based on strictly hierarchical null-spatial projection, calculating the desired speed according to the priority order of RCM constraints, image centering, and depth adjustment, thereby achieving multi-task decoupling.

[0024] Step 3-1, Highest Priority Task: RCM Constraint Task. To ensure that the lateral error at the puncture point decays to zero, the error feedback decay law is set as follows: (7) in, It is a positive definite feedback gain matrix; Combining the error mapping relationship described in step 2, the particular solution of the volume velocity satisfying the RCM constraint is: (8) Simultaneously, the null basis corresponding to this RCM task is extracted: (9) At this point, the feasible velocity subspace satisfying the RCM constraint can be expressed as: (10) in, This is the adjustment velocity vector in null space for subsequent low-priority tasks.

[0025] Step 3-2, Second Priority Task: Image Centering. Based on the known desired pixel coordinates... Calculate the image feature tracking error : (11) The expected convergence relation of image features in the two-dimensional pixel plane is defined as follows: (12) in, The image feedback gain matrix; Define normalized pixel coordinates , and order , ;in, and Let the pixel coordinates of the target be on the image plane. and The principal pixel coordinates of the camera. and The camera is in shaft and Equivalent focal length along the axial direction; Establish the image Jacobian matrix based on the camera model: (13) in, This is an estimate of the target depth; By utilizing the fixed transformation relationship from the endoscope base coordinate system to the camera coordinate system, the image Jacobian is transformed to the base coordinate system, thus obtaining the image Jacobian matrix in the base coordinate system. ; Project the image Jacobian matrix onto the null basis of the RCM task. In the middle, construct the reduced-order visual task matrix: (14) Solve for the visual accommodation speed without violating the RCM constraint: (15) in, The image feedback gain matrix; Further construct the null projection matrix for the visual task : (16) Step 3-3, Third Priority Task: Depth Adjustment. Calculate the depth error between the actual insertion depth of the endoscope and the desired insertion depth: (17) in, The current insertion depth, The desired insertion depth; In the coordinate system of the endoscope base, construct the matrix for extracting the axial motion component: (18) The matrix is ​​then projected sequentially through the RCM null space and the visual null space to obtain the depth adjustment mapping matrix: (19) Calculate the deep coupling velocity term introduced by executing high-priority tasks: (20) Solving for depth adjustment speed without interfering with RCM constraints and image centering tasks: ;(twenty one) in, This is the depth adjustment gain coefficient.

[0026] Steps 3-4: Synthesizing the desired speed command. The adjustment speeds of each task are synthesized to obtain the final desired speed command for the endoscope base: .(twenty two) Step 4: Establish and solve the lower-level quadratic programming optimization problem. This step, while satisfying the system's physical limits and safety boundaries, uses joint optimization to find the optimal joint velocity command for the robotic arm at the current moment. .

[0027] Step 4-1, Construct the objective function. Let the decision variables of the quadratic programming problem be: ,(twenty three) in, For robot joint speed, and These are positive position slack variables and negative position slack variables, respectively. To ensure the robotic arm always approaches the center of its posture during movement and avoids triggering the physical limits of the joints, a potential energy function based on the joint position is constructed: ,(twenty four) in, , This represents half the range of motion of the joint. and They are the first Upper and lower limits of each joint position; Calculate the gradient of the potential energy function with respect to the joint position: (25) Therefore, the reference velocity towards the center of the joint can be calculated: (26) in, Joint-centered gain coefficient Construct a quadratic objective function to minimize the deviation from the central reference velocity and the penalty slack variable: (27) Step 4-2: Construct kinematic equation constraints. Transform the desired volume velocity command synthesized in Step 3 into equation constraints, forcing the robotic arm's end effector to execute the upper-level velocity reference. (28) in, Let be the Jacobian matrix of the endoscope base in its local coordinate system.

[0028] Step 4-3: Construct insertion depth boundary constraints. To uniformly incorporate the safety constraints of the endoscope insertion depth into the optimization problem, it is necessary to construct a mapping relationship between the current insertion depth and the rate of change of the joint velocity. This is based on the unit direction vector of the endoscope rod axis obtained in Step 1. Calculate the unit direction vector Vertical projection matrix in the direction of the location: (29) in, It is a third-order identity matrix; Furthermore, let the position Jacobian matrix of the local rigid body reference point in the world coordinate system be... The Jacobian matrix of the position of the camera's optical center in the world coordinate system is: The axial length between the two reference points of the endoscope rod is: calculate (30) Then the unit direction vector of the rod axis The derivative mapping matrix with respect to joint angular velocity is: (31) Based on the definition of the current insertion depth in step 1: (32) Based on the chain rule, the mapping relationship between the depth change rate and the joint velocity can be obtained: (33) Among them, the depth Jacobian matrix for: (34) Based on discrete control period It can perform first-order prediction of the insertion depth at the next time step, and based on the preset minimum safe insertion depth. With maximum safe insertion depth Construct hard constraints for the insertion depth boundary: (35) The constraints are used to ensure that the endoscope remains within a preset safe depth range during visual tracking.

[0029] Step 4-4: Construct robot joint motion constraints. To prevent joints from going out of bounds or exceeding speed limits during robot movement, soft constraints on joint position and hard constraints on joint velocity are further added to the quadratic programming optimization problem.

[0030] Let the current joint angle vector be... The upper and lower limits of the joint position are respectively and Then, a positive position slack variable is introduced. With negative position slack variables Then, soft constraints for collision avoidance at joint positions can be constructed: (36) (37) The above constraints indicate that when the robot joints approach the position boundary, the constraints are allowed to be relaxed to a limited extent by slack variables, but the amount of relaxation will be penalized in the objective function to avoid the optimization problem from losing a feasible solution due to the boundary being too tight; Furthermore, let the upper and lower limits of the joint velocity be respectively... and Then, construct hard constraints on the absolute velocity of the joints: (38) The hard constraints are used to directly limit the velocity amplitude of each joint, ensuring that the control commands obtained do not exceed the maximum motion capability of the robot body.

[0031] Steps 4-5: Construct and solve the complete quadratic programming problem. Combining steps 4-1 to 4-4, the complete quadratic programming optimization problem can be constructed as follows: (39) The following constraints must be met: (40) (41) (42) (43) (44) Combining the objective function, equality constraints, and inequality constraints described above, the problem is transformed into a matrix form of a standard quadratic programming problem. The optimal joint velocity is then obtained through iterative solving using a quadratic programming solver. The data is then sent to the robotic arm's underlying controller to drive joint movement.

Claims

1. A visual servo control method for a robotic arm considering RCM constraints, characterized in that, Includes the following steps: Step S100: Obtain the joint angle vector of the robotic arm at the current moment; The robot's forward kinematics is used to calculate the position information of the endoscope base, camera optical center, and puncture point in the world coordinate system, and to calculate the actual insertion depth of the endoscope. At the same time, images captured by the endoscope are acquired in real time, and target feature points are extracted. Step S200: Under the local reference coordinate system established by the endoscope base, calculate the lateral error component of the puncture point relative to the local reference coordinate system, and construct the RCM constrained Jacobian matrix between the joint velocity and the lateral error change rate. Step S300: Taking RCM constraint as the highest priority task, based on the RCM constraint Jacobian matrix, set the lateral error decay law, solve the velocity particular solution that satisfies the RCM constraint and the corresponding null basis; taking image centering as the second priority task, project the image Jacobian matrix onto the null basis, solve the visual accommodation speed under the RCM constraint condition, and extract the null projection matrix of the visual task. With depth adjustment as the third priority task, the depth Jacobian matrix is ​​projected onto the visual null space projection matrix to solve the depth adjustment speed under RCM constraints and image centering task conditions. By combining the velocity specificity, visual accommodation velocity, and depth accommodation velocity, the desired velocity for adjusting the endoscope pose is obtained. Step S400: The generated desired speed command is converted into equality constraints, and the absolute speed limit of the robotic arm joints, the anti-collision limit of the joint position, and the safe insertion depth boundary of the endoscope are converted into inequality constraints; with the robotic arm tending to the joint center position as the optimization objective, a quadratic objective function is constructed, and the current optimal joint speed command is obtained by solving it, and the robotic arm is driven to move. The method for constructing the quadratic objective function in step S400 is as follows: Step S411, set the decision variable vector as follows: ，（20） in, For the joint speed of the robotic arm, and These are the positive and negative joint position anti-collision relaxation variables, respectively; Step S412: To minimize the reference velocity deviation and penalty relaxation variable towards the joint center position, construct a quadratic objective function: ，（21） in, To approximate the reference velocity at the physical limit center of the joint, Penalize the weights of slack variables; Reference speed The following potential energy gradient descent method is used to obtain the following: ，（22） in, For the current joint angle, This is the center of the joint's range of motion. This is half the range of motion of the joint. This represents the joint-centered gain coefficient. The potential energy gradient; Step S400 establishes constraints to drive the robotic arm's motion, specifically including: Step S421, construct the following kinematic equation constraints: ，（23） in, Let be the Jacobian matrix of the endoscope base in its own coordinate system. For the generated desired speed command; Step S422: Construct soft constraints on joint position incorporating relaxation variables, hard constraints on absolute joint velocity limiting maximum joint motion, and hard constraints on depth boundaries limiting the safe advance and retreat range of the endoscope; wherein... Soft constraints on joint position combined with relaxation variables: ，（24） ，（25） in, and These are the positive and negative joint position anti-collision relaxation variables, respectively. For the joint speed of the robotic arm, and These represent the upper and lower limits of the joint position. To control the cycle; Hard constraints on the absolute velocity of a joint that limit its maximum range of motion: ，（26） in, and These are the maximum and minimum permissible speeds of the joint, respectively. Hard constraints on depth boundaries that limit the safe advance and retreat range of the endoscope: ，（27） in, The preset minimum safe insertion depth, For maximum safe insertion depth, This represents the current actual insertion depth. To map joint velocities to the depth Jacobian matrix of the insertion depth change rate, To control the cycle.

2. The method according to claim 1, characterized in that, The process of constructing the accurate RCM constraint matrix in step S200 specifically includes: Step S210: Establish a local reference coordinate system at the endoscope base, and represent the position of the puncture point in the local reference coordinate system as follows: ，（1） in, and This is the lateral error component. The axial component of the puncture point along the endoscope axis; Step S220, obtaining a lateral error vector e xy , ，（2） Step S230, acquiring the body velocity S of the endoscope base in the local reference coordinate system b , ，（3） Among them, [v x ,v y ,v z [ω] is the linear velocity vector in the local reference coordinate system. x ,ω y ,ω z ] is the angular velocity vector in the local reference coordinate system; Step S240: Based on rigid body kinematics, construct the RCM constraint Jacobian matrix between the rate of change of lateral error and the body velocity: ，（4） Wherein, the error mapping matrix Represented as: 。（5） 3. The method according to claim 2, characterized in that, The solution process in step S300, which prioritizes the RCM constraint, specifically includes: Step S311, set the lateral error vector It satisfies the predetermined exponential decay law: ，（6） in, Here is the RCM error feedback gain matrix; Step S312: Combining the exponential decay law, solve for the particular solution of the volume velocity that satisfies the decay law. : ，（7） in, for The pseudo-inverse matrix; Step S313: Extract the null basis corresponding to the RCM task. The feasible volume velocity subspace that satisfies the RCM constraints is represented as: ，（8） in, Let be the null space coefficient vector to be determined.

4. The method according to claim 3, characterized in that, The solution process in step S300, which prioritizes image centering as the second priority task, specifically includes: Step S321, based on the current pixel coordinates of the target feature point Center coordinates of the desired image Constructing image feature errors: ，（9） Step S322, establish the image error convergence relationship: ，（10） in, The image feedback gain matrix; Step S323: Calculate the coordinate transfer matrix using the fixed transformation relationship from the local reference coordinate system of the endoscope base to the camera coordinate system, and obtain the image Jacobian matrix in the base coordinate system. ; Step S324: Project the image Jacobian matrix onto the null basis of the RCM task. In the middle, construct a reduced-order visual task matrix. , ；（11） Step S325: Calculate the visual accommodation speed μ under the RCM constraint. v : ；（12） Step S326: Construct the null projection matrix for the visual task. : ，（13） in, for The pseudo-inverse matrix, It is a unit array.

5. The method according to claim 4, characterized in that, Step 3, which prioritizes depth adjustment as the third priority task and the final desired volume velocity synthesis process, specifically includes: Step S331: Calculate the depth error e between the actual insertion depth of the endoscope and the expected insertion depth. d : ，（14） in, The current insertion depth, The desired insertion depth; Step S332: In the coordinate system of the endoscope base, construct a matrix M for extracting the axial motion component. d : ；（15） Step S333, transform matrix M d By projecting sequentially through the RCM null basis and the visual null space, the depth adjustment order mapping matrix is ​​obtained: ；（16） Step S334, calculate the deep coupling velocity term introduced by executing high-priority tasks: ；（17） Step S335, solve for the depth adjustment speed of the RCM constraint and image centering task: ，（18） in, The depth adjustment gain coefficient; Step S336: Combine the solution results at each stage to obtain the final desired volume velocity command in the endoscope base coordinate system. 。（19）。

Images