A double-loop control-based endoscopic holding instrument following method and system

Abstract

The application provides an endoscope holding instrument following method and system based on double-loop control, comprising: obtaining an initial RGB video frame for distortion correction to obtain an RGB video frame; identifying based on the RGB video frame to obtain a tip two-dimensional pixel position and a tip three-dimensional coordinate position; dynamically adjusting based on the tip three-dimensional coordinate position to obtain an RCM point position; obtaining an expected coordinate position based on the RGB video frame, and obtaining an image plane error based on tip two-dimensional pixel position comparison processing; obtaining an end angle velocity instruction based on the image plane error and the RCM point position; obtaining an actual end pose, and obtaining a pose error based on the end angle velocity instruction; performing high-bandwidth control based on the pose error to obtain a joint angle velocity instruction; and controlling the endoscope holding instrument based on the end angle velocity instruction and the joint angle velocity instruction to realize the following of the endoscope holding instrument. The application can improve the accuracy of the endoscope following.

Description

Technical Field

[0001] This invention belongs to the field of endoscope control technology, and particularly relates to a method and system for following nasal endoscope holding instruments based on dual-ring control. Background Technology

[0002] Traditional endoscope-holding instrument tracking solutions adjust the endoscope angle through mechanical structures. However, traditional tracking solutions lack path planning and real-time closed-loop control, resulting in insufficient safety and tracking efficiency in complex scenarios.

[0003] To address the aforementioned issues, existing technologies employ multi-degree-of-freedom robotic arms to hold the endoscope and generate and transmit robot control commands by executing control algorithms on the endoscope view. Specifically, the control algorithms in these technologies adjust the endoscope orientation based on the distance between the surgical instrument tip and the center point of the endoscope view, improving the safety and tracking efficiency of instrument following. However, because these technologies rely solely on view errors to adjust the endoscope orientation, they suffer from slow response and tracking lag in relatively low-frequency situations with visual delays, reducing the accuracy of instrument following. Furthermore, while existing technologies increase the proportional gain of the controller to achieve a faster response, this approach is prone to oscillations near the equilibrium point, failing to balance speed and stability, and further reducing the accuracy of instrument following. Summary of the Invention

[0004] The present invention aims to provide a method and system for tracking nasal endoscope instruments based on dual-ring control, so as to solve the above-mentioned technical problems and improve the accuracy of tracking the endoscope instruments.

[0005] To address the aforementioned technical problems, this invention provides a method for following a nasal endoscope holding instrument based on dual-ring control, comprising the following steps:

[0006] Obtain an initial RGB video frame, and perform distortion correction processing based on the initial RGB video frame to obtain an RGB video frame;

[0007] Based on the RGB video frames, the two-dimensional pixel position and the three-dimensional coordinate position of the tip are obtained by recognition processing.

[0008] The RCM point position is obtained by dynamically adjusting the position of the tip's three-dimensional coordinates.

[0009] Based on the RGB video frame, the desired coordinate position is obtained, and the image plane error is obtained by comparing the desired coordinate position with the tip two-dimensional pixel position.

[0010] Based on the image plane error and the RCM point position, the end-effector angular velocity command is obtained;

[0011] The actual pose of the end effector is obtained, and the pose error is obtained based on the actual pose of the end effector and the end effector angular velocity command.

[0012] Based on the pose error, high-bandwidth control processing is performed to obtain joint angular velocity commands;

[0013] The end-effector angular velocity command and the joint angular velocity command are used to control the end-effector, thereby enabling the end-effector to follow the movement of the end-effector.

[0014] In the aforementioned scheme, the outer loop control is achieved by comparing the desired coordinate position with the current actual two-dimensional pixel position of the endoscope tip to obtain the image plane error, and then calculating the end-effector angular velocity command based on this image plane error, so that the instrument tip can move to the desired position in the image. In the same scheme, the inner loop control is achieved by obtaining the end-effector target pose based on the end-effector angular velocity command, comparing the end-effector target pose with the actual end-effector pose to obtain the pose error, and then using high-bandwidth control of the pose error and RCM point position to obtain the joint angular velocity command, thereby driving the movement of each joint of the endoscope-holding instrument, allowing the endoscope to rotate stably around the RCM point. Furthermore, in this scheme, the movement of the endoscope-holding instrument is used to close the inner loop by feeding back the actual end-effector posture, and also by changing the position of the instrument in the image, forming the outer loop by feeding back RGB video frames. Therefore, the dual-loop control of this scheme ensures that even with the relatively low frequency of the outer loop and the presence of visual delay, the high-frequency control of the inner loop can still guarantee the smooth movement of the endoscope-holding instrument, thereby improving the accuracy of the endoscope-holding instrument's following.

[0015] Further, the step of obtaining an initial RGB video frame and performing distortion correction processing based on the initial RGB video frame to obtain an RGB video frame includes: obtaining an initial RGB video frame and performing offline calibration based on the RGB video frame to obtain a camera intrinsic parameter matrix and distortion coefficients; and performing distortion correction processing based on the initial RGB video frame, the camera intrinsic parameter matrix, and the distortion coefficients under the standard Zhang Zhengyou model to obtain an RGB video frame.

[0016] In the above scheme, the actual camera imaging process is affected by the lens, resulting in radial and tangential distortion, thus the acquired initial RGB video frame has image distortion. This scheme corrects the distortion caused by image distortion in the initial RGB video frame by offline determination of the initial RGB image to obtain the camera intrinsic parameter matrix and distortion coefficients. The initial RGB image is then subjected to distortion correction using the standard Zhang Zhengyou model under the camera intrinsic parameter matrix and distortion coefficients. The final RGB video frame removes radial and tangential distortion, reduces visual deviation caused by distortion, facilitates subsequent processing, improves the accuracy of the tip's two-dimensional pixel position and tip's three-dimensional coordinate position obtained in subsequent processes, and thus improves the accuracy of the lens-holding instrument's tracking.

[0017] Further, the step of performing recognition processing based on the RGB video frame to obtain the tip's two-dimensional pixel position and tip's three-dimensional coordinate position includes: performing recognition processing based on the RGB video frame to obtain the tip's two-dimensional pixel position; performing mapping processing based on the tip's two-dimensional pixel position to obtain the initial tip's three-dimensional coordinate position; and performing coordinate system transformation processing based on the initial tip's three-dimensional coordinate position to obtain the tip's three-dimensional coordinate position.

[0018] Further, the step of mapping based on the two-dimensional pixel position of the tip to obtain the initial three-dimensional coordinate position of the tip includes: obtaining the camera principal pixel position, camera focal length, actual tip diameter, and tip pixel diameter; obtaining the tip depth based on the camera focal length, the actual tip diameter, and the tip pixel diameter; and performing integrated calculation based on the two-dimensional pixel position of the tip, the tip depth, the camera principal pixel position, and the camera focal length to obtain the initial three-dimensional coordinate position of the tip.

[0019] In the above scheme, the position of the instrument tip in the image is identified based on RGB video frames to obtain the two-dimensional pixel position of the tip. After mapping the two-dimensional pixel position of the tip, the initial three-dimensional coordinate position of the tip is in the camera coordinate system. To apply it to the control of the lens-holding instrument, the initial three-dimensional coordinate position of the tip needs to be transformed to the coordinate system of the instrument base to guide the movement of the instrument. Therefore, this scheme avoids the problem of control errors caused by the inconsistency between the camera and instrument coordinate systems, and enhances the stability and robustness of the lens-holding instrument's following.

[0020] Furthermore, the step of dynamically adjusting the RCM point position based on the three-dimensional coordinate position of the tip to obtain the RCM point position includes: acquiring historical RCM point positions, and obtaining an initial RCM point position based on the historical RCM point positions and the three-dimensional coordinate position of the tip under a preset virtual potential field strategy; and performing constraint projection correction based on the initial RCM point position to obtain the RCM point position.

[0021] In the above scheme, the RCM point is the dynamic remote motion center point, and the endoscope mounted on the endoscope-holding instrument rotates around this RCM point. The initial RCM point position obtained under the preset virtual potential field strategy may contain errors and deviate from the constraint boundary of the cavity entrance. When the endoscope-holding instrument enters the cavity, the constraint boundary of the cavity entrance is formed based on the tissue at the cavity entrance that its shaft can contact. This scheme corrects the initial RCM position by constraint projection to limit the movement of the endoscope-holding instrument under the constraint boundary of the cavity entrance, thereby correcting the RCM point to accurately project it under the constraint boundary, avoiding undue pressure or damage to the cavity entrance, and improving the safety of the endoscope-holding instrument's tracking. Furthermore, this scheme achieves dynamic adjustment of the RCM point based on the three-dimensional coordinate position of the tip, enabling the RCM point to move dynamically and smoothly within the cavity entrance region to adapt to the offset of the endoscope-holding instrument, and expanding the endoscope's field of view, thus improving the stability and real-time performance of the endoscope-holding instrument's tracking.

[0022] Further, the step of obtaining the historical RCM point position and, based on the historical RCM point position and the tip's three-dimensional coordinate position, obtaining the initial RCM point position under a preset virtual potential field strategy includes: obtaining the cavity inlet surface intersection position based on the tip's three-dimensional coordinate position; obtaining the historical RCM point position and calculating the deviation based on the historical RCM point position and the cavity inlet surface intersection position to obtain a deviation value; obtaining the attractive force based on the deviation value under a preset attractive force coefficient; if the deviation value meets a preset repulsion threshold, obtaining the repulsive force based on the deviation value under a preset repulsion threshold and a preset repulsion force coefficient; performing calculation processing based on the historical RCM point position to obtain the historical fulcrum movement speed, and obtaining the damping force based on the historical fulcrum movement speed under a preset damping coefficient; integrating the attractive force, the repulsive force, and the damping force to obtain a resultant force; and calculating the initial RCM point position based on the resultant force, the historical RCM point position, and the historical fulcrum movement speed.

[0023] In the above scheme, when the instrument tip enters the cavity, there will be an intersection point with the cavity entrance surface. This intersection point is taken as the intersection point position of the cavity entrance surface. Based on the deviation value obtained from the historical RCM point position and the intersection point position of the cavity entrance surface, the distance between the historical RCM point and the instrument tip can be quantified. The larger the deviation value, the more the endoscope's viewing angle deviates from the instrument tip. In order to enable the endoscope to track the instrument tip well and prevent the endoscope from colliding with the instrument tip due to excessive distance, the deviation value needs to be controlled within a suitable range.

[0024] The above scheme, based on the attractive force obtained from the deviation value, allows the initial RCM point position to be closer to the instrument tip, thus improving the endoscopic field of view. If the deviation value of this scheme meets the preset repulsion threshold, it indicates that the distance between the historical RCM point and the instrument tip is too close. The repulsive force constructed by this scheme can control the initial RCM point position to remain within a safe distance from the instrument tip, preventing the endoscope from colliding with the instrument tip. The damping force constructed by this scheme based on the historical fulcrum movement speed can prevent the RCM point position from overshooting due to large position changes, thus avoiding oscillations of the RCM point position under the action of attractive and repulsive forces. The initial RCM point position obtained by this scheme based on the combined force of the constructed attractive, repulsive, and damping forces provides a better endoscopic field of view for tracking the instrument tip and ensures that the endoscope and instrument tip will not collide, increasing the safety of instrument tracking. Furthermore, the smooth change of the RCM point position contributes to the good stability of instrument tracking.

[0025] Further, obtaining the end-effector angular velocity command based on the image plane error and the RCM point position includes: generating camera angular velocity and translation speed commands based on the image plane error; obtaining angle increment and position increment under a preset sampling period based on the camera angular velocity and the translation speed commands; performing mapping processing based on the angle increment, the position increment, and the RCM point position to obtain the end-effector three-dimensional spatial attitude increment; and generating the end-effector angular velocity command based on the end-effector three-dimensional spatial attitude increment.

[0026] The end-effector angular velocity command obtained by the above scheme can control the angular velocity of the endoscope tip rotating around the RCM point, so that the tip of the instrument is closer to the desired coordinate position in the RGB video frame image, thereby providing a better endoscopic field of view and improving the accuracy of endoscope tracking. This scheme obtains the current endoscope target pose based on the endoscope's three-dimensional spatial pose increment, and generates an end-effector angular velocity command based on this target pose to control the endoscope tracking action.

[0027] Furthermore, the step of performing high-bandwidth control processing based on the pose error to obtain joint angular velocity commands includes: obtaining an end-effector acceleration correction amount based on the pose error; and performing joint mapping processing based on the end-effector acceleration correction amount to obtain joint angular velocity commands.

[0028] In the above scheme, based on the end-effector acceleration correction, the pseudo-inverse of the Jacobian matrix corresponding to the lens-holding device is used for mapping, and the required end-effector motion of the lens-holding device is mapped to the joint angular velocity commands of each joint of the lens-holding device. The joint angular velocity commands obtained by this scheme can control the lens-holding device to quickly approach the target pose corresponding to the end-effector angular velocity command, thereby improving the tracking efficiency of the lens-holding device.

[0029] Furthermore, the step of performing high-bandwidth control processing based on the pose error to obtain joint angular velocity commands includes: performing path planning under a preset environment model based on the pose error and the RCM point position to obtain an initial motion path set; performing optimization processing under a multi-objective cost function based on the initial motion path set to obtain an optimized motion path; and performing smooth interpolation processing based on the optimized motion path to generate joint angular velocity commands.

[0030] In the above scheme, path planning is performed based on the pose error and the RCM point position to obtain an initial motion path set consisting of multiple initial motion paths. To achieve better tracking of the eyepiece, a motion path with short position change distances, safe obstacle avoidance, and smooth trajectory is needed based on the initial motion path set. Therefore, this scheme constructs a multi-objective cost function considering path length, collision risk, and trajectory smoothness. Based on the optimized motion path obtained under the multi-objective cost function, joint angular velocity commands are generated, making the tracking process of the eyepiece towards the device tip smoother and more stable, with higher tracking efficiency.

[0031] This invention also provides a nasal endoscope instrument following system based on dual-ring control, comprising: an RGB video frame correction module for acquiring an initial RGB video frame and performing distortion correction processing based on the initial RGB video frame to obtain an RGB video frame; a video frame position recognition module for performing recognition processing based on the RGB video frame obtained by the RGB video frame correction module to obtain the tip's two-dimensional pixel position and tip's three-dimensional coordinate position; an RCM point position dynamic adjustment module for performing dynamic adjustment processing based on the tip's three-dimensional coordinate position obtained by the video frame position recognition module to obtain the RCM point position; and an outer ring image error acquisition module for acquiring a desired coordinate position based on the RGB video frame obtained by the video frame position recognition module, and comparing the desired coordinate position with the tip's two-dimensional pixel position obtained by the video frame position recognition module. The system performs a series of steps to obtain the following: Image plane error is obtained through image plane error acquisition; an outer ring end-effector command generation module is used to obtain an end-effector angular velocity command based on the image plane error obtained by the outer ring image error acquisition module; an inner ring pose error acquisition module is used to obtain the actual end-effector pose and, based on the actual end-effector pose and the end-effector angular velocity command obtained by the outer ring end-effector command generation module, obtain a pose error; an inner ring joint command generation module is used to perform high-bandwidth control processing based on the pose error obtained by the inner ring pose error acquisition module and the RCM point position obtained by the RCM point position dynamic adjustment module, to obtain a joint angular velocity command; and a lens-holding device following control module is used to control the lens-holding device based on the end-effector angular velocity command obtained by the outer ring end-effector command generation module and the joint angular velocity command obtained by the inner ring joint command generation module, to achieve lens-holding device following.

[0032] In the above scheme, the outer loop control is achieved by comparing the desired coordinate position with the current actual two-dimensional pixel position of the endoscope tip to obtain the image plane error, and then calculating the end-effector angular velocity command based on this image plane error, so that the instrument tip can move to the desired position in the image. In the same scheme, the inner loop control is achieved by obtaining the end-effector target pose based on the end-effector angular velocity command, comparing the end-effector target pose with the actual end-effector pose to obtain the pose error, and then using high-bandwidth control of the pose error and the RCM point position to obtain the joint angular velocity command, which drives the movement of each joint of the endoscope-holding instrument, allowing the endoscope to rotate stably around the RCM point. This dual-loop control scheme ensures that even with the relatively low frequency of the outer loop and the presence of visual delay, the high-frequency control of the inner loop can still guarantee the smooth movement of the endoscope-holding instrument, thereby improving the accuracy of the endoscope-holding instrument's tracking. Attached Figure Description

[0033] Figure 1 A flowchart illustrating the implementation of a nasal endoscope holding instrument following method based on dual-ring control, as provided in an embodiment of the present invention;

[0034] Figure 2 This is a schematic diagram of a nasal endoscope holding instrument following system architecture based on dual-ring control, provided in an embodiment of the present invention. Detailed Implementation

[0035] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0036] Please see Figure 1 This embodiment provides a method for following a nasal endoscope holding instrument based on dual-ring control, including the following steps:

[0037] Step S1: Obtain the initial RGB video frame, and perform distortion correction processing based on the initial RGB video frame to obtain the RGB video frame;

[0038] Step S2: Based on the RGB video frame, perform recognition processing to obtain the two-dimensional pixel position and three-dimensional coordinate position of the tip;

[0039] Step S3: Perform dynamic adjustment processing based on the three-dimensional coordinate position of the tip to obtain the RCM point position;

[0040] Step S4: Based on the RGB video frame, obtain the desired coordinate position, and compare the desired coordinate position with the tip two-dimensional pixel position to obtain the image plane error;

[0041] Step S5: Based on the image plane error and the RCM point position, obtain the end-effector angular velocity command;

[0042] Step S6: Obtain the actual pose of the end effector, and based on the actual pose of the end effector and the end effector angular velocity command, obtain the pose error;

[0043] Step S7: Based on the pose error, perform high-bandwidth control processing to obtain joint angular velocity commands;

[0044] Step S8: Control the end-effector based on the end-effector angular velocity command and the joint angular velocity command to achieve the following of the end-effector.

[0045] In the above embodiments, the outer loop control is achieved by comparing the desired coordinate position with the current actual two-dimensional pixel position of the endoscope tip to obtain the image plane error, and then calculating the end-effector angular velocity command based on the image plane error, so that the instrument tip can move to the desired position in the image. In the same embodiments, the inner loop control is achieved by obtaining the end-effector target pose based on the end-effector angular velocity command, comparing the end-effector target pose with the actual end-effector pose to obtain the pose error, and then using high-bandwidth control of the pose error and the RCM point position to obtain the joint angular velocity command, thereby driving the movement of each joint of the endoscope-holding instrument, allowing the endoscope to rotate stably around the RCM point. Furthermore, in this embodiment, the movement of the endoscope-holding instrument is used to close the inner loop by feeding back the actual end-effector posture, and also changes the position of the instrument in the image, forming the outer loop by feeding back RGB video frames. Therefore, the dual-loop control in this embodiment ensures that even with a relatively low frequency outer loop and visual delay, the high-frequency control of the inner loop can still guarantee the smooth movement of the endoscope-holding instrument, thereby improving the accuracy of the endoscope-holding instrument's tracking.

[0046] Further, the step of obtaining an initial RGB video frame and performing distortion correction processing based on the initial RGB video frame to obtain an RGB video frame includes: obtaining an initial RGB video frame and performing offline calibration based on the RGB video frame to obtain a camera intrinsic parameter matrix and distortion coefficients; and performing distortion correction processing based on the initial RGB video frame, the camera intrinsic parameter matrix, and the distortion coefficients under the standard Zhang Zhengyou model to obtain an RGB video frame.

[0047] In the above embodiments, the actual camera imaging process is affected by the lens, resulting in radial and tangential distortion. Therefore, the acquired initial RGB video frames have image distortion. In this embodiment, to correct the distortion caused by image distortion in the initial RGB video frames, the camera intrinsic parameter matrix and distortion coefficients are obtained through offline determination of the initial RGB image. The initial RGB image is then subjected to distortion correction using the standard Zhang Zhengyou model under the camera intrinsic parameter matrix and distortion coefficients. The final RGB video frames have removed radial and tangential distortion, reducing visual deviation caused by distortion, facilitating subsequent processing, and improving the accuracy of the tip's two-dimensional pixel position and tip's three-dimensional coordinate position obtained in subsequent processes, thereby improving the accuracy of the lens-holding instrument's tracking.

[0048] Further, the step of performing recognition processing based on the RGB video frame to obtain the tip's two-dimensional pixel position and tip's three-dimensional coordinate position includes: performing recognition processing based on the RGB video frame to obtain the tip's two-dimensional pixel position; performing mapping processing based on the tip's two-dimensional pixel position to obtain the initial tip's three-dimensional coordinate position; and performing coordinate system transformation processing based on the initial tip's three-dimensional coordinate position to obtain the tip's three-dimensional coordinate position.

[0049] Further, the step of mapping based on the two-dimensional pixel position of the tip to obtain the initial three-dimensional coordinate position of the tip includes: obtaining the camera principal pixel position, camera focal length, actual tip diameter, and tip pixel diameter; obtaining the tip depth based on the camera focal length, the actual tip diameter, and the tip pixel diameter; and performing integrated calculation based on the two-dimensional pixel position of the tip, the tip depth, the camera principal pixel position, and the camera focal length to obtain the initial three-dimensional coordinate position of the tip.

[0050] In the above embodiments, the position of the instrument tip in the image is identified based on RGB video frames to obtain the two-dimensional pixel position of the tip. After mapping the two-dimensional pixel position of the tip, the initial three-dimensional coordinate position of the tip is in the camera coordinate system. To apply it to the control of the lens-holding instrument, the initial three-dimensional coordinate position of the tip needs to be transformed to the coordinate system of the instrument base to guide the movement of the instrument. Therefore, this embodiment avoids the problem of control errors caused by the inconsistency between the camera and instrument coordinate systems, and enhances the stability and robustness of the lens-holding instrument's following.

[0051] In one embodiment, a single electronic nasal endoscope (such as the Olympus ENF-VH or a functionally equivalent commercial nasal endoscope) is used as the imaging device to acquire RGB video frames. The imaging device has a resolution of not less than 1920×1080 and a frame rate of ≥30 fps. This embodiment employs an industrial computing platform to implement the dual-loop control-based nasal endoscope holding instrument following method described in this invention. The industrial computing platform inputs the real-time video signal from the endoscope into an industrial control computer for processing via a video acquisition card. The latency of the industrial computing platform must be controlled at an extremely low level, i.e., the processing and communication latency should be controlled to below 10ms as much as possible to support the high-frequency closed-loop control described below.

[0052] The above embodiments utilize the camera intrinsic parameter matrix obtained through offline calibration. The distortion coefficients are used to remove radial and tangential distortion from each frame of the RGB video frame; the camera intrinsic parameter matrix described in this embodiment can be expressed as... ,in, These are the coordinates of the principal point, and and The focal length is expressed in pixels. The distortion correction process in this embodiment uses the standard Zhang Zhengyou model to ensure the accuracy of the resulting RGB video frames.

[0053] Preferably, this embodiment uses lightweight and efficient models such as YOLOv5s, YOLOv8, Faster R-CNN and SSD to perform recognition processing on RGB video frames.

[0054] In one embodiment, to ensure an outer loop control frequency of over 100 Hz, high-speed image tracking is added. This involves using an optical flow tracking algorithm to interpolate and predict the motion of the instrument tip in RGB video frames, thereby updating the two-dimensional pixel position of the tip at an equivalent frequency of 100 Hz. This embodiment can reduce the impact of latency on the tracking of the instrument holding device when the frame rate is limited.

[0055] In one embodiment, during the process of converting the two-dimensional pixel position of the tip into the initial three-dimensional coordinate position of the tip, if the diameter of the instrument tip and the pixel diameter are known, then the two-dimensional pixel position of the tip is... By back-projecting the pinhole imaging model into three-dimensional space, the initial three-dimensional coordinate position of the tip in the camera coordinate system can be obtained. This embodiment estimates the depth of the instrument tip relative to the camera using the instrument tip diameter D and the pixel diameter d of the instrument tip in the RGB video frame image. Then it can be substituted and Find and .in, and Let these be the pixel coordinates of the camera's principal point in the image plane of the RGB video frame. and These are the camera's focal lengths in pixels along the x and y axes, respectively.

[0056] In one embodiment, due to the initial tip three-dimensional coordinate position The initial three-dimensional coordinate position of the tip needs to be transformed to the robot arm base coordinate system to guide the movement of the lens-holding device. This embodiment uses a standard hand-eye calibration method: a checkerboard calibration board is used to obtain the pose transformation matrix of the camera coordinate system relative to the robot arm base coordinate system. The calibration method in this embodiment involves taking photos at multiple positions within the camera's field of view with a checkerboard pattern carried at the end of the lens-holding device, and then using the Tsai algorithm to solve for the pose transformation matrix. In this embodiment, after completing the calibration method, the initial three-dimensional coordinate position of the tip is determined. The three-dimensional coordinate position of the tip of the robotic arm in the base coordinate system is obtained through homogeneous coordinate transformation. .

[0057] Furthermore, the step of dynamically adjusting the RCM point position based on the three-dimensional coordinate position of the tip to obtain the RCM point position includes: acquiring historical RCM point positions, and obtaining an initial RCM point position based on the historical RCM point positions and the three-dimensional coordinate position of the tip under a preset virtual potential field strategy; and performing constraint projection correction based on the initial RCM point position to obtain the RCM point position.

[0058] In the above embodiments, the RCM point is the dynamic remote motion center point, and the endoscope mounted on the endoscope-holding instrument rotates around this RCM point. The initial RCM point position obtained under the preset virtual potential field strategy in this embodiment may have errors and deviate from the constraint boundary of the cavity entrance. When the endoscope-holding instrument enters the cavity, the constraint boundary of the cavity entrance is formed based on the tissue at the cavity entrance that its shaft can contact. This embodiment corrects the initial RCM position by constraint projection to limit the movement of the endoscope-holding instrument under the constraint boundary of the cavity entrance, thereby correcting the RCM point to accurately project it under the constraint boundary, avoiding undue pressure or damage to the cavity entrance, and improving the safety of the endoscope-holding instrument's tracking. Furthermore, this embodiment achieves dynamic adjustment of the RCM point based on the three-dimensional coordinate position of the tip, enabling the RCM point to move dynamically and smoothly within the cavity entrance region to adapt to the offset of the endoscope-holding instrument, and expanding the field of view of the endoscope-holding instrument, thus improving the stability and real-time performance of the endoscope-holding instrument's tracking.

[0059] Further, the step of obtaining the historical RCM point position and, based on the historical RCM point position and the tip's three-dimensional coordinate position, obtaining the initial RCM point position under a preset virtual potential field strategy includes: obtaining the cavity inlet surface intersection position based on the tip's three-dimensional coordinate position; obtaining the historical RCM point position and calculating the deviation based on the historical RCM point position and the cavity inlet surface intersection position to obtain a deviation value; obtaining the attractive force based on the deviation value under a preset attractive force coefficient; if the deviation value meets a preset repulsion threshold, obtaining the repulsive force based on the deviation value under a preset repulsion threshold and a preset repulsion force coefficient; performing calculation processing based on the historical RCM point position to obtain the historical fulcrum movement speed, and obtaining the damping force based on the historical fulcrum movement speed under a preset damping coefficient; integrating the attractive force, the repulsive force, and the damping force to obtain a resultant force; and calculating the initial RCM point position based on the resultant force, the historical RCM point position, and the historical fulcrum movement speed.

[0060] In the above embodiments, when the instrument tip enters the cavity, there will be an intersection point with the cavity entrance surface. This intersection point is taken as the intersection point position of the cavity entrance surface. Based on the deviation value obtained from the historical RCM point position and the intersection point position of the cavity entrance surface, the distance between the historical RCM point and the instrument tip can be quantified. The larger the deviation value, the more the endoscope's viewing angle deviates from the instrument tip. In order for the endoscope to track the instrument tip well and not to cause the endoscope to collide with the instrument tip due to being too close, the deviation value needs to be controlled within a suitable range.

[0061] The above embodiments, based on the attractive force obtained from the deviation value, allow the initial RCM point position to be closer to the instrument tip, thereby improving the endoscopic field of view. If the deviation value in this embodiment meets the preset repulsion threshold, it indicates that the distance between the historical RCM point and the instrument tip is too close. The repulsive force constructed in this embodiment can control the initial RCM point position to remain within a safe distance from the instrument tip, preventing the endoscope from colliding with the instrument tip. The damping force constructed in this embodiment based on the historical fulcrum movement speed can prevent the RCM point position from overshooting due to large position changes, thus avoiding oscillations of the RCM point position under the action of attractive and repulsive forces. The initial RCM point position obtained from the combined force of the constructed attractive, repulsive, and damping forces provides a better endoscopic field of view for tracking the instrument tip and ensures that the endoscope and instrument tip do not collide, increasing the safety of instrument tracking. Furthermore, the smooth change in the RCM point position provides good stability for instrument tracking.

[0062] In one embodiment, in the virtual potential field strategy, attractive force... ,in, As the attractiveness coefficient, For historical RCM point locations, Location of the intersection of the nasal cavity inlet and the surface; repulsive force ,in, The repulsive force coefficient has a deviation value of , Preset repulsion threshold; damping force ,in, The damping coefficient is... The speed at which the historical pivot point moves.

[0063] In one embodiment, the resultant force Furthermore, this embodiment calculates the initial RCM point position based on the resultant force, the historical RCM point position, and the historical fulcrum movement speed, specifically through the following formula: , ,in, To preset virtual quality, The time for a single loop. For the speed of movement of the historical pivot, The initial RCM point position, This represents the current movement speed of the pivot point.

[0064] In one embodiment, the process of obtaining the RCM point position by performing constraint projection correction based on the initial RCM position specifically involves: if the cavity inlet surface is a plane, then the obtained initial RCM point position... The Z component is reset to H, i.e., the RCM position. ,in, The lateral component of the initial RCM point position. This represents the longitudinal component of the initial RCM point position. If the cavity inlet surface is a non-planar curved surface, then implicit surface equations can be used. , representing the cavity inlet constraint, and the initial RCM point position is projected onto the surface using numerical methods, where Specifically refers to the spatial coordinate components of that point in the X and Y directions, and the position of the tip's two-dimensional pixel. Different. When this point is the initial RCM point position, Under the constraint of the cavity inlet, the projection operation is as follows: That is, setting the z-component to H. The horizontal component remains unchanged from before projection. Furthermore, this embodiment can employ the gradient projection method to calculate the gradient of the surface at the current point. The RCM point position is then updated using the following iterative formula. .in, For function f in The Jacobian matrix at f. When f is a scalar implicit function. Equivalent to gradient vector Therefore, the norm term in the denominator is used to normalize the gradient, ensuring that the projection step size is appropriate.

[0065] In one embodiment, the initial RCM point can be accurately projected onto the curved surface through several iterations, i.e. At this time, among them, This is an implicit function for the curved surface. Furthermore, in this embodiment, when dealing with the nostril boundary of a slightly elliptical curved surface, a point cloud can be pre-collected and the initial RCM point positions can be found during runtime. The nearest point on the cloud surface to that point is used as the updated RCM point position.

[0066] Further, obtaining the end-effector angular velocity command based on the image plane error and the RCM point position includes: generating camera angular velocity and translation speed commands based on the image plane error; obtaining angle increment and position increment under a preset sampling period based on the camera angular velocity and the translation speed commands; performing mapping processing based on the angle increment, the position increment, and the RCM point position to obtain the end-effector three-dimensional spatial attitude increment; and generating the end-effector angular velocity command based on the end-effector three-dimensional spatial attitude increment.

[0067] The end-effector angular velocity command obtained in the above embodiments can control the angular velocity of the end-effector's rotation around the RCM point, so that the instrument tip is closer to the desired coordinate position in the RGB video frame image, thereby providing a better endoscopic field of view and improving the accuracy of end-effector tracking. This embodiment obtains the current end-effector target pose based on the end-effector's three-dimensional spatial pose increment, and generates an end-effector angular velocity command based on this target pose to control the end-effector's tracking action.

[0068] In one embodiment, image plane error ,in, For the desired coordinate position, This embodiment further addresses the image planar error by specifying the three-dimensional coordinate position of the tip. Perform PI control to obtain the end angular velocity command. Furthermore, the specific process by which the PI control obtains the end-effector angular velocity command in this embodiment is as follows: .in, This is the proportionality coefficient. is the integral coefficient.

[0069] Furthermore, the step of performing high-bandwidth control processing based on the pose error to obtain joint angular velocity commands includes: obtaining an end-effector acceleration correction amount based on the pose error; and performing joint mapping processing based on the end-effector acceleration correction amount to obtain joint angular velocity commands.

[0070] In the above embodiments, based on the end-effector acceleration correction, the pseudo-inverse of the Jacobian matrix corresponding to the lens-holding device is used for mapping, and the required end-effector motion of the lens-holding device is mapped to the joint angular velocity commands of each joint of the lens-holding device. The joint angular velocity commands obtained in this embodiment can control the lens-holding device to quickly approach the target pose corresponding to the end-effector angular velocity command, thereby improving the tracking efficiency of the lens-holding device.

[0071] In one embodiment, the pose error ,in, The end-point target pose corresponding to the end-point angular velocity command. This represents the actual pose of the end effector. In this embodiment, the inner loop control adjusts the pose error... High-bandwidth control, such as PID-based or feedforward-based servo control, is applied to calculate the instantaneous velocity / acceleration correction required at the end effector. Then, the desired end effector motion is mapped to the angular velocity commands of each joint using the pseudo-inverse of the robotic arm's Jacobian matrix. ;in, Representing the Jacobian matrix The pseudo-inverse, which is used to... Converted into angular velocity vectors of each joint The Jacobian matrix pseudo-inverse solution in this embodiment is applicable to redundant robotic arms, providing a minimum norm solution and avoiding joint singularities to some extent, thus reflecting the physical meaning of the end-effector-joint velocity mapping. This embodiment can also impose saturation constraints on the velocities, accelerations, and positions of each joint during the solution process, as well as integral limiting to prevent excessive accumulated errors.

[0072] Furthermore, the step of performing high-bandwidth control processing based on the pose error to obtain joint angular velocity commands includes: performing path planning under a preset environment model based on the pose error and the RCM point position to obtain an initial motion path set; performing optimization processing under a multi-objective cost function based on the initial motion path set to obtain an optimized motion path; and performing smooth interpolation processing based on the optimized motion path to generate joint angular velocity commands.

[0073] In the above embodiments, path planning is performed based on the pose error and the RCM point position to obtain an initial motion path set consisting of multiple initial motion paths. To achieve better tracking of the eyepiece device, a motion path with short position change distances, safe obstacle avoidance, and smooth trajectory is needed based on the initial motion path set. Therefore, this embodiment constructs a multi-objective cost function considering path length, collision risk, and trajectory smoothness. Based on the optimized motion path obtained under the multi-objective cost function, joint angular velocity commands are generated, making the tracking process of the eyepiece device on the device tip smoother and more stable, with higher tracking efficiency.

[0074] In one embodiment, the multi-objective cost function is: ,in, For path cost, For path length, As a result of the collision, Cost for trajectory smoothness; , and Different weighting coefficients are used. In this embodiment, the collision cost can be calculated using a collision detection function. For example, the closer the motion path is to an obstacle in the space, the higher the cost is assigned (0 if there is no collision). The smoothness cost can be defined as the integral of the curvature of the motion path or the rate of change of the joint acceleration of the mirror-holding device, to penalize drastic changes in the motion path and make the optimized motion path smoother.

[0075] In one embodiment, the process of generating joint angular velocity commands by performing smooth interpolation based on the optimized motion path specifically includes: performing B-spline fitting on the optimized motion path to smooth the polygonal optimized motion path into a continuous curve, and further using the gradient descent algorithm to fine-tune the path node positions, remove redundant inflection points, and ensure a smooth transition of the end-effector posture; and in this embodiment, velocity, acceleration, and jerk constraints can be applied to the joint space trajectory of the robotic arm to smoothly interpolate the motion curves of each joint, avoiding damage to patient tissues or screen vibration caused by the vigorous movement of the end-effector.

[0076] Please refer to Figure 2The present invention also provides a nasal endoscope instrument following system based on dual-ring control, comprising: an RGB video frame correction module for acquiring an initial RGB video frame and performing distortion correction processing based on the initial RGB video frame to obtain an RGB video frame; a video frame position recognition module for performing recognition processing based on the RGB video frame obtained by the RGB video frame correction module to obtain the tip's two-dimensional pixel position and tip's three-dimensional coordinate position; an RCM point position dynamic adjustment module for performing dynamic adjustment processing based on the tip's three-dimensional coordinate position obtained by the video frame position recognition module to obtain the RCM point position; and an outer ring image error acquisition module for acquiring a desired coordinate position based on the RGB video frame obtained by the video frame position recognition module, and performing image error acquisition based on the desired coordinate position and the tip's two-dimensional pixel position obtained by the video frame position recognition module. The system performs comparative processing to obtain image plane error; an outer loop end-effector command generation module is used to obtain end-effector angular velocity commands based on the image plane error obtained by the outer loop image error acquisition module; an inner loop pose error acquisition module is used to obtain the actual end-effector pose and obtain pose error based on the actual end-effector pose and the end-effector angular velocity commands obtained by the outer loop end-effector command generation module; an inner loop joint command generation module is used to perform high-bandwidth control processing based on the pose error obtained by the inner loop pose error acquisition module and the RCM point position obtained by the RCM point position dynamic adjustment module to obtain joint angular velocity commands; and a lens-holding device following control module is used to control the lens-holding device based on the end-effector angular velocity commands obtained by the outer loop end-effector command generation module and the joint angular velocity commands obtained by the inner loop joint command generation module to achieve lens-holding device following.

[0077] In the above embodiments, the outer loop control is achieved by comparing the desired coordinate position with the current actual two-dimensional pixel position of the endoscope tip to obtain the image plane error, and then calculating the end-effector angular velocity command based on the image plane error, so that the instrument tip can move to the desired position in the image. In the same embodiments, the inner loop control is achieved by obtaining the end-effector target pose based on the end-effector angular velocity command, comparing the end-effector target pose with the actual end-effector pose to obtain the pose error, and then performing high-bandwidth control on the pose error and the RCM point position to obtain the joint angular velocity command, thereby driving the movement of each joint of the endoscope-holding instrument and enabling the endoscope to rotate stably around the RCM point. This dual-loop control ensures that even with a relatively low frequency outer loop and visual delay, the high-frequency control of the inner loop can still guarantee the smooth movement of the endoscope-holding instrument, thus improving the accuracy of the endoscope-holding instrument's following.

[0078] In one embodiment, the nasal endoscope-holding instrument following method based on dual-ring control described in this invention is applied to a dual-robotic arm collaborative system. This system includes an auxiliary robotic arm and a surgical robotic arm. The auxiliary robotic arm is a self-developed 7-DOF tandem robotic arm with a maximum joint speed of 120° / s and an end-effector load of 3kg, used for holding the endoscope. The surgical robotic arm is a self-developed 5-DOF robotic arm with a dedicated nasal endoscope holder interface at its end for manipulating surgical instruments. In this embodiment, the endoscope used in the collaborative system is an Olympus ENF-VH with a resolution of 1920×1080 and a frame rate of 30 fps. Important safety parameters of the system include: a minimum safe distance of 3 mm between the RCM point and the instrument, and a limit of 5 mm / s for the pivot point movement speed.

[0079] In the above embodiments, coarse positioning is performed before executing the endoscope-holding instrument following method of the present invention. The coarse positioning includes: controlling the movement of the auxiliary robotic arm so that the endoscope lens is roughly aligned with the center of the patient's nasal cavity entrance, with the front end of the lens slightly extending into the nostril, maintaining consistency with the direction of the nasal cavity passage, while controlling the surgical robotic arm to remain stationary or cooperate to avoid it; recording the current position of the endoscope front end and the center of the nostril, and initializing the RCM point; fixing the endoscope at the initialized RCM point.

[0080] In the above embodiment, the endoscope light source is turned on, and the endoscope video is acquired as the initial RGB video frame; the auxiliary robotic arm drives the endoscope to observe the pre-set checkerboard in the surgical area, and the camera intrinsic parameter matrix and distortion coefficient are obtained for calibration; after calibration, the initial RGB video frame is preprocessed with real-time distortion correction and color enhancement to obtain the RGB video frame; the YOLOv5s model is loaded, the position of the surgical instrument tip in the RGB video frame is detected, the two-dimensional pixel position of the tip is obtained, and the two-dimensional pixel position of the tip is mapped to the three-dimensional coordinate position of the camera tip in the camera coordinate system. Then, the three-dimensional coordinate position of the camera tip is transformed to the coordinate system of the robotic arm base to obtain the three-dimensional coordinate position of the tip.

[0081] In the above embodiments, the outer loop control calculates the deviation of the tip's three-dimensional coordinate position from the image center in the video frame, and generates an end-effector angular velocity command. The inner loop control converts the end-effector angular velocity command into joint angular velocity commands to control the movement of each joint of the auxiliary robotic arm, driving the endoscope to rotate around the RCM point and follow. Simultaneously, based on the tip's three-dimensional coordinate position, the intersection point is calculated at the nasal cavity inlet plane to obtain the intersection point position of the cavity inlet plane. Under a preset virtual potential field strategy, the RCM point is updated to ensure that the RCM point moves continuously along the instrument direction. In this embodiment, when the instrument moves slowly, the RCM point follows stably. If the instrument moves rapidly and significantly, the movement speed of the RCM point is limited to 5 mm / s to update the RCM point position. The outer loop control allows the endoscope's field of view to tolerate a certain error briefly, and then smoothly catches up within multiple cycles, avoiding severe shaking of the endoscope's field of view due to excessive speed.

[0082] In the above embodiments, when a significant change in the endoscopic field of view is required (e.g., when the instrument tip enters the sinus cavity), this embodiment uses the current state of the auxiliary robotic arm as a starting point and employs an improved RRT* algorithm to search for a collision-free path in the environment model. Considering the narrowness of the sinus ostium, this embodiment increases the collision cost weight in the multi-objective cost function, making the planned optimized motion path further away from the sinus bone wall. Furthermore, after planning the optimized motion path, the inner loop control drives the auxiliary robotic arm to move along the optimized path, while the outer loop control continues to correct minor image deviations to achieve precise alignment. Moreover, during the tracking process of the auxiliary robotic arm, if a deviation occurs or the instrument tip moves unexpectedly and rapidly, the outer loop control will immediately close the loop to correct the field of view and, if necessary, replan the motion path.

[0083] In the above embodiment, during continuous tracking by the assisted robotic arm, continuous outer-loop control and inner-loop control are employed. The outer-loop control adjusts the endoscopic field of view based on the image error corresponding to each frame in the video frame, while the inner-loop control adjusts the end-effector movement of the assisted robotic arm on a millisecond-level basis. For example, when a slight tremor at the instrument tip causes a field of view error, the outer-loop control uses PID control to quickly generate a reverse fine-tuning command. After the assisted robotic arm executes the reverse fine-tuning command, the inner-loop control maintains a stable endoscopic field of view. The dual-loop cooperation can control the static aiming error to within 1 pixel and the dynamic tracking error to within a few pixels and eliminate it within 0.1 seconds. In this embodiment, throughout the tracking process, the assisted robotic arm and the surgical robotic arm maintain good coordination through a collaborative strategy. The assisted robotic arm always moves around the current RCM point without interfering with the surgical robotic arm. When the surgical robotic arm moves at high speed, causing the image error in the outer-loop control to exceed the threshold, the surgical arm is instructed to decelerate, prioritizing the alignment of the endoscopic field of view.

[0084] In the above embodiment, the instrument tip is withdrawn after the surgical robotic arm completes its task; in this embodiment, the auxiliary robotic arm stops following the endoscope after recognizing that there are no instruments in the endoscope's field of view, and keeps the endoscope stationary; subsequently, the auxiliary robotic arm is controlled to slowly move the endoscope out of the cavity and back to a safe position, and the surgical robotic arm returns to its initial state.

[0085] In one embodiment, since the endoscopic field of view is more complex during sinus surgery and the instrument tip in the endoscopic field of view may be partially obscured, YOLOv5s is replaced with the more accurate Faster R-CNN (ResNet-50 backbone) to more robustly identify instrument tips in complex backgrounds, but the inference speed is slightly slower (about 10 fps). Therefore, the system combines acceleration techniques such as image pyramids and ROI region cropping, and adds optical flow prediction to maintain the outer loop control update frequency above 30Hz to ensure stable field of view.

[0086] In the above embodiments, the sinus passages are narrower and the surrounding bone walls are more sensitive. This embodiment increases the weight of the collision cost term in the multi-objective cost function to penalize paths that are close to obstacles, while reducing the smoothness weight in the multi-objective cost function, making the optimized motion path obtained from path planning more inclined towards safe obstacle avoidance. This embodiment also adds obstacle avoidance redundancy in actual implementation: before the auxiliary robotic arm moves according to the optimized motion path, the surgical arm will fine-tune the instrument position to leave more space. Through the above adjustments, this embodiment ensures that the endoscope does not get too close to the sinus wall.

[0087] In the above embodiments, due to the small internal space of the sinuses, more precise control is required. In this embodiment, the inner loop control incorporates joint acceleration limiting constraints to prevent vibration of the robotic arm or patient discomfort caused by high-speed movement of the auxiliary robotic arm. The PI control parameters used in the outer loop control are also optimized to suppress oscillations within a small range of tremors in the endoscopic field of view. These improvements in this embodiment result in smoother and more accurate adjustment of the endoscopic field of view within confined spaces.

[0088] In one embodiment, the dynamic adjustment of the RCM point position was optimized for posterior nasal cavity tumor surgery. Because the tumor is located deep within the nasal cavity, and the surgical robotic arm has a long instrument with a large range of motion, this embodiment utilizes an adaptive control algorithm for dynamic adjustment of the RCM point position. The adaptive control algorithm in this embodiment adjusts the attraction and damping coefficients in real time based on the speed and direction of the instrument tip, achieving more sensitive and stable RCM point following. For example, when the instrument moves forward rapidly, the attraction coefficient is increased to enhance the RCM point following speed; when a sudden stop or reversal of the instrument tip is detected, the damping coefficient is increased to prevent RCM point overshoot. This embodiment's RCM control based on the adaptive control algorithm exhibits better stability than a virtual potential field strategy with fixed parameters during a wide range of dynamic adjustments.

[0089] In the above embodiments, due to the long duration of tumor resection surgery, the patient's head may experience slight movements. This embodiment adds an intraoperative position detection function: the offset angle of the nasal cavity inlet posture is estimated by the changes in feature points (such as nasal septum landmarks) in the endoscopic image. If the nasal cavity inlet plane is detected to have deviated from the initial calibration posture by more than 5°, an automatic recalibration procedure is triggered. In this embodiment, the auxiliary robotic arm withdraws the endoscope slightly and recalculates the camera intrinsic parameter matrix and distortion coefficients to correct for coordinate system errors caused by patient movement, thereby ensuring the accuracy of subsequent endoscopic instrument tracking.

[0090] In the above embodiments, due to the fragility of tumor tissue, this embodiment increases the minimum safe distance between the RCM point and the instrument from 3mm to 5mm. Specifically, this embodiment presets a repulsion threshold of 5mm in the virtual potential field strategy. Furthermore, during tracking, if a deviation value less than 5mm is detected, the auxiliary robotic arm immediately decelerates or stops to provide greater buffering when the RCM point approaches the instrument, reducing the risk of the endoscope compressing the tissue. Through these adjustments and measures such as reducing the maximum speed of the robotic arm, this embodiment further ensures surgical safety.

[0091] The above description represents the preferred embodiments of the present invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principles of the present invention, and these improvements and modifications are also considered to be within the scope of protection of the present invention.

Claims

1. A method for following a nasal endoscope holding instrument based on dual-ring control, characterized in that, Includes the following steps: Obtain an initial RGB video frame, and perform distortion correction processing based on the initial RGB video frame to obtain an RGB video frame; Based on the RGB video frames, the two-dimensional pixel position and the three-dimensional coordinate position of the tip are obtained by recognition processing. Based on the three-dimensional coordinate position of the tip, the position of the intersection point of the cavity inlet surface is obtained; Obtain the historical RCM point positions, and calculate the deviation based on the intersection of the historical RCM point positions and the cavity inlet surface position to obtain the deviation value; Based on the deviation value, the attractive force is obtained under a preset attractive force coefficient; If the deviation value meets the preset repulsion threshold, then based on the deviation value, the repulsion force is obtained under the preset repulsion threshold and the preset repulsion force coefficient; The historical fulcrum movement speed is calculated based on the historical fulcrum movement speed, and the damping force is obtained based on the historical fulcrum movement speed under a preset damping coefficient. The resultant force is obtained by integrating the attractive force, the repulsive force, and the damping force. The initial RCM point position is obtained by calculating based on the resultant force, the historical RCM point position, and the historical pivot point movement speed. Based on the initial RCM point position, constraint projection correction is performed to obtain the RCM point position; Based on the RGB video frame, the desired coordinate position is obtained, and the image plane error is obtained by comparing the desired coordinate position with the tip two-dimensional pixel position. Based on the image plane error, generate camera angular velocity and translation speed commands; Based on the camera angular velocity and the translation speed command, the angle increment and position increment are obtained under a preset sampling period; Based on the angle increment, the position increment, and the RCM point position, a mapping process is performed to obtain the end-effector three-dimensional spatial attitude increment; Based on the aforementioned end-effector three-dimensional spatial attitude increment, an end-effector angular velocity command is generated; The actual pose of the end effector is obtained, and the pose error is obtained based on the actual pose of the end effector and the end effector angular velocity command. Based on the pose error, high-bandwidth control processing is performed to obtain joint angular velocity commands; The end-effector angular velocity command and the joint angular velocity command are used to control the end-effector, thereby enabling the end-effector to follow the movement of the end-effector.

2. The method for following a nasal endoscope holding instrument based on dual-ring control according to claim 1, characterized in that, The step of obtaining an initial RGB video frame and performing distortion correction processing based on the initial RGB video frame to obtain an RGB video frame includes: Acquire initial RGB video frames and perform offline calibration based on the RGB video frames to obtain the camera intrinsic parameter matrix and distortion coefficients; Based on the initial RGB video frame, the camera intrinsic parameter matrix, and the distortion coefficients, distortion correction processing is performed under the standard Zhang Zhengyou model to obtain the RGB video frame.

3. The method for following a nasal endoscope holding instrument based on dual-ring control according to claim 1, characterized in that, The recognition processing based on the RGB video frames to obtain the two-dimensional pixel position and three-dimensional coordinate position of the tip includes: The position of the tip two-dimensional pixel is obtained by performing recognition processing based on the RGB video frame; Based on the two-dimensional pixel position of the tip, a mapping process is performed to obtain the initial three-dimensional coordinate position of the tip; Based on the initial three-dimensional coordinate position of the tip, a coordinate system transformation is performed to obtain the three-dimensional coordinate position of the tip.

4. The method for following a nasal endoscope holding instrument based on dual-ring control according to claim 3, characterized in that, The process of mapping based on the two-dimensional pixel position of the tip to obtain the initial three-dimensional coordinate position of the tip includes: Obtain the camera principal pixel position, camera focal length, actual tip diameter, and tip pixel diameter; The tip depth is obtained based on the camera focal length, the actual diameter of the tip, and the pixel diameter of the tip; The initial three-dimensional coordinate position of the tip is obtained by integrating and calculating the tip's two-dimensional pixel position, tip depth, camera principal pixel position, and camera focal length.

5. The method for following a nasal endoscope holding instrument based on dual-ring control according to claim 1, characterized in that, The high-bandwidth control processing based on the pose error to obtain joint angular velocity commands includes: Based on the pose error, the end-effector acceleration correction amount is obtained; Based on the end-effector acceleration correction, joint mapping is performed to obtain the joint angular velocity command.

6. The method for following a nasal endoscope holding instrument based on dual-ring control according to claim 1, characterized in that, The high-bandwidth control processing based on the pose error to obtain joint angular velocity commands includes: Based on the pose error and the RCM point position, path planning is performed under a preset environment model to obtain an initial motion path set. Based on the initial motion path set, optimization is performed under a multi-objective cost function to obtain the optimized motion path; Based on the optimized motion path, smooth interpolation is performed to generate joint angular velocity commands.

7. A nasal endoscope holding instrument following system based on dual-ring control, characterized in that, A method for implementing a nasal endoscope holding instrument following method based on dual-ring control as described in any one of claims 1 to 6, comprising: An RGB video frame correction module is used to acquire an initial RGB video frame and perform distortion correction processing based on the initial RGB video frame to obtain an RGB video frame. The video frame position recognition module is used to perform recognition processing on the RGB video frame obtained by the RGB video frame correction module to obtain the two-dimensional pixel position and the three-dimensional coordinate position of the tip. The RCM point position dynamic adjustment module is used to obtain the position of the cavity entrance surface intersection point based on the three-dimensional coordinate position of the tip obtained by the video frame position recognition module; acquire historical RCM point positions, and calculate the deviation value based on the historical RCM point positions and the cavity entrance surface intersection point positions; obtain the attractive force based on the deviation value under a preset attractive force coefficient; if the deviation value meets a preset repulsion threshold, obtain the repulsion force based on the deviation value under a preset repulsion threshold and a preset repulsion force coefficient; calculate and process the historical fulcrum movement speed based on the historical RCM point positions, and obtain the damping force based on the historical fulcrum movement speed under a preset damping coefficient; integrate the attractive force, the repulsion force, and the damping force to obtain the resultant force; calculate the initial RCM point position based on the resultant force, the historical RCM point positions, and the historical fulcrum movement speed; and perform constraint projection correction based on the initial RCM point position to obtain the RCM point position. The outer ring image error acquisition module is used to obtain the desired coordinate position based on the RGB video frame obtained by the video frame position recognition module, and to compare and process the desired coordinate position with the tip two-dimensional pixel position obtained by the video frame position recognition module to obtain the image plane error. The outer loop end-of-loop command generation module is used to generate camera angular velocity and translation speed commands based on the image plane error obtained by the outer loop image error acquisition module; based on the camera angular velocity and translation speed commands, angle increment and position increment are obtained under a preset sampling period; based on the angle increment, the position increment and the RCM point position, mapping processing is performed to obtain the end-of-loop three-dimensional spatial attitude increment; based on the end-of-loop three-dimensional spatial attitude increment, an end-of-loop angular velocity command is generated. The inner loop pose error acquisition module is used to acquire the actual pose of the end effector and obtain the pose error based on the actual pose of the end effector and the end effector angular velocity command obtained by the outer loop end effector command generation module. The inner ring joint command generation module is used to perform high-bandwidth control processing based on the pose error obtained by the inner ring pose error acquisition module and the RCM point position obtained by the RCM point position dynamic adjustment module to obtain the joint angular velocity command. The lens-holding device following control module is used to control the lens-holding device based on the end angular velocity command obtained by the outer ring end command generation module and the joint angular velocity command obtained by the inner ring joint command generation module, so as to realize the following of the lens-holding device.

Images