Visual navigation method in the narrow space of oral cavity and pharynx based on artificial feature target and guidance device

Abstract

The present invention discloses a visual navigation method for a narrow space of the oral cavity and pharynx based on an artificial feature target and a guiding device, S1: construct an artificial guiding auxiliary device surrounding the internal structure of the oral cavity, and design the attached artificial texture features of the inner wall as a visual aid; S2: based on the guiding auxiliary device and the artificial visual feature assistance in S1, in the roaming stage, by combining a lightweight visual odometer and a back-end optimization module, a method for estimating the pose of the end effector itself based on synchronous positioning and mapping technology is constructed; S3: construct an artificial feature target, in the execution stage, it is convenient for the algorithm in S4 to track and locate the specific surgical instrument within the field of view through the target area; S4: in the execution stage, a method for estimating the pose of a specific surgical instrument based on the target pose detection technology is constructed. The present invention can effectively cope with many practical challenges in the special operating environment of the surgical space, and construct a stable and high-precision pose solution for the end effector itself and the target surgical instrument.

Description

Technical Field

[0001] The invention relates to the technical field of computer vision, and in particular to a visual navigation method in a narrow space of the oral cavity, pharynx and throat based on an artificial feature target and a guiding device. Background Art

[0002] With the continuous development of modern medicine, digital medicine with the help of computer technology is gradually becoming a very valuable research direction. In recent years, it has developed particularly rapidly in specific task areas such as preoperative diagnosis based on medical images and surgical process assistance. Real-time video sequences acquired through single- or binocular endoscopes can provide surgeons with effective visual observation assistance around the lesion area during surgery, and are also the basis for building a visual navigation and positioning method for automated surgical robots. However, in special operating environments such as the oral cavity and throat, this positioning method based on visual perception still faces many practical challenges.

[0003] In the oral and pharyngeal environment, there is a lack of texture features to rely on, the operating space is narrow and the precision requirements are high. When collecting images at extremely close viewing distances, problems such as defocusing and mutual occlusion of targets are prone to occur. Due to these problems, the current traditional feature-based visual positioning method is difficult to maintain stable operating results on its own. Summary of the invention

[0004] In order to overcome the shortcomings of the above technologies, the purpose of the present invention is to provide a visual navigation method for the narrow space of the oral cavity and pharynx based on artificial feature targets and guidance devices, which can effectively cope with many practical challenges in the special operating environment of the surgical space, and construct a stable and high-precision posture solution for the end effector itself and the target surgical instrument, so as to provide a safe and effective state estimation service for the overall surgical robot system.

[0005] In order to achieve the above object, the technical solution adopted by the present invention is:

[0006] The method for visual navigation in the narrow space of the oral cavity and pharynx based on artificial feature targets and guidance devices comprises the following steps:

[0007] S1: When the surgical robot end effector approaches the lesion occurrence area inside the oral cavity and pharynx from the outside of the human body during the roaming stage, an artificial guidance auxiliary device surrounding the internal structure of the oral cavity is constructed to serve as a physical path guide during the approach stage, and an artificial auxiliary visual feature is set; the visual auxiliary feature is composed of a planar checkerboard target with different pattern designs, and is located on the inner wall surface of the guidance device;

[0008] S2: Based on the guidance auxiliary device and artificial auxiliary visual features in S1, during the roaming stage, a method for estimating the end effector's own pose based on simultaneous positioning and mapping technology is constructed by combining a lightweight visual odometer and a backend optimization module;

[0009] S3: The overall model of the end effector of the surgical robot is connected to various specific surgical instruments (such as electrothermal surgical forceps and laser devices) through an intermediate connector in a specific manner; a planar feature target with a variety of pattern designs is constructed and attached to the side wall surface of the intermediate connector, so that the algorithm in S4 can track and locate the specific surgical instruments within the field of view through the target area during the execution stage;

[0010] S4: Based on the planar feature targets on specific surgical instruments and corresponding connectors, during the execution phase, a specific surgical instrument pose estimation method based on target pose detection technology is constructed through the plane homography principle and traditional feature method.

[0011] The method for constructing the artificial guidance auxiliary device surrounding the internal structure of the oral cavity described in S1 is as follows:

[0012] The surrounding guide device is a hollow truncated cone with gradually decreasing inner and outer diameters, and the smallest outer diameter is no more than 15 mm, to meet the requirements of the internal structure of the adult oral and pharyngeal cavity; the device is composed of multiple truncated cone components of different specifications connected in series, and in actual use, they can be assembled arbitrarily to meet the needs of surgery in different areas of the oral and pharyngeal cavity; the inner surface of the hollow component is composed of a surrounding side wall parallel to the entry direction of the actuator, and a front narrow truncated cone plane perpendicular to the entry direction, on which a large number of specially designed plane feature patterns are arranged, providing sufficient feature point support for running visual algorithms through different observation angles. Furthermore, the plane feature patterns include different forms of targets such as QR codes, ArUco benchmarks, and multi-layer nested concentric rings.

[0013] During the roaming stage, the artificial guidance auxiliary device will directly guide the surgical robot end effector (controlled by a flexible body connection) from outside the patient's mouth, enter through the maximum inner diameter opening of the cone, and arrive at the minimum inner diameter exit along the contraction channel, that is, near the lesion area.

[0014] The end effector's own pose estimation method based on the synchronous positioning and mapping technology in S2 includes a front-end visual odometer module for providing an initial pose estimation at the current moment, and a back-end optimization module for continuously optimizing the overall estimation result in the operation channel;

[0015] The front-end visual odometry module is implemented based on the ORB (Oriented FAST and Rotated BRIEF) feature. In the continuous moments of the running channel, the visual odometry is used to calculate the relative pose transformation of the camera between adjacent moments based on the input image frame at the current moment, or to initialize the module at the initial moment based on the valid continuous image pair to reconstruct the basic sparse map points and the channel start.

[0016] The basic principle and process of the front-end visual odometer module are described as follows: extract image key points based on ORB features; obtain inter-frame feature correspondence through brute force matching and screening mechanisms; construct a Frame2Frame multi-view geometric model between valid initialized monocular image pairs, obtain the initial adjacent moment pose transformation based on epipolar constraints, and reconstruct the 3D coordinates of feature points by triangulation to determine the unique monocular scene scale information; for monocular image input at any time, construct a feature matching relationship from image pixels to known reconstructed space points, construct a Model2Frame geometric model and solve the pose transformation based on the Pnp (Perspective-n-Point) method; provide initial pose estimation to the optimization module and insert sparse map feature points;

[0017] The back-end optimization module is implemented based on the multi-style bundle adjustment method. During the process of camera calibration internal parameter estimation, initial pose conversion estimation and map point reconstruction, the errors in the operation channel will continue to accumulate, causing the subsequent estimation results to gradually deviate from the actual situation. The optimization module is used to constrain the overall estimation results in the channel by constructing different inter-frame common view relationships in this process to eliminate error accumulation;

[0018] The bundle adjustment modes used include the following:

[0019] Thoroughly tune the overall optimization goals, that is, the pose state nodes and related map points corresponding to all moments in the running channel. In the initialization stage of the algorithm, use the most luxurious method (that is, for the initialization image frame pairs, the initial construction of map points and other overall optimization goals, based on the method of minimizing reprojection errors, perform thorough optimization) to obtain the most accurate initial state, map points and monocular scale estimation, so as to minimize subsequent error accumulation;

[0020] For the local optimization target, all other associated nodes and observed map points around the current state node determined by the common view relationship search are thoroughly optimized. At any time in the running channel, after each map point is inserted, this local area is optimized;

[0021] Only for the optimization of pose state nodes, after obtaining the preliminary pose estimation through the front-end visual odometer and before inserting the map points, the estimation results are preliminarily optimized;

[0022] All the above optimization processes are based on minimizing the reprojection error from map points to corresponding image frames.

[0023] The basic construction method of the surgical instrument intermediate connector and the artificial feature target in S3 is as follows:

[0024] The surgical instrument intermediate connector is used as a transition part from the flexible body connecting arm to the instrument body, and is composed of a cube with four strict rectangular planes on the outside to construct a texture support based on an artificial feature pattern;

[0025] The characteristic patterns include QR codes, ArUco benchmarks, and multi-layer nested concentric rings. Multiple observed planes can provide sufficient texture support for the visual positioning algorithm. The characteristic patterns are set on the surface of the target surgical instrument end component.

[0026] The front and rear ends of the assembly are respectively connected to the corresponding surgical instrument body and the flexible body connecting arm, and are a regular rectangular parallelepiped, with a side length of the square cross section not exceeding 4 mm, so as to adapt to the general size of universal end surgical instruments;

[0027] The outer side walls of the component are four strictly rectangular planes, and the surfaces are respectively filled with a QR code target pattern and any other artificial target feature pattern, that is, all feature points are extracted strictly in the same plane, which conforms to the assumption of the homography principle. At the same time, it is believed that in the observation angle after arbitrary rotation, the position and posture of the surgical instrument within the current field of view can still be estimated through the corresponding side wall plane group features.

[0028] The target surgical instrument pose estimation method based on the special feature pattern described in S4 mainly comprises the following steps:

[0029] Step 1: In the algorithm initialization process, the homography mapping relationship between the endoscope camera with a fixed posture in the execution phase and the target plane corresponding to the surgical instrument is calculated;

[0030] Step 2: At any subsequent time, extract feature points from the current image frame input and match features to the initialization frame;

[0031] Step 3: Based on the world coordinate system set with reference to the target plane and moving with the instrument body, reconstruct the three-dimensional world coordinates of the feature points of the current frame based on the reference;

[0032] Step 4: Based on the known imaging plane pixel coordinates of the feature points in the current frame and the reconstructed three-dimensional coordinates, construct the multi-view geometry model of Model2Frame, and calculate the pose transformation of the device relative to the camera device at the current moment based on the Pnp method.

[0033] The step 1 comprises the following steps:

[0034] Step 1.1: Select the initialization image frame, manually select any feature point in the QR code feature pattern on a single surface of the target component, and obtain the corresponding two-dimensional pixel point coordinates (x c ,y c );

[0035] Step 1.2: Take one corner of the rectangular feature pattern on a fixed single plane as the coordinate origin, set the world coordinate system that moves with the instrument, and refer to the QR code feature pattern with known size and shape to obtain the corresponding 3D world coordinates (x w ,y w ,z w ), since all the extracted feature points are located on the same component sidewall plane, that is, the coordinate system plane X o Y, so the three-dimensional coordinates of the feature point (x w ,y w ,0);

[0036] Step 1.3: Repeat the above steps to select N pattern feature points on the current component plane, where N ≥ 4;

[0037] Step 1.4: Calculate the 2D pixel coordinates of the imaging plane from the feature points To the corresponding three-dimensional world coordinates The mapping relationship, where i is the index to the current corresponding feature point, can be represented by a homography matrix H.

[0038]

[0039] Continue to simplify and eliminate the scale factor z,

[0040]

[0041] The feature point extraction process in step 2 is based on SIFT, ORB, SURF and other methods. During the feature matching screening process, the Hamming distance between the binary description vectors of the feature points is calculated. According to the set distance threshold, which is generally twice the minimum distance, the matches with a relative distance greater than the threshold are extracted from the original results, and the first round of brute force screening of the matching results is completed at one time; then the random sampling consistency method is used to perform a secondary screening of the outlier matching results, and a group is randomly selected from the original matching samples to calculate and generate a relative pose transformation, and an error function based on the least squares method is constructed. Other samples that can fit the current transformation model are calculated and form a consistent set. The "correctness" of the current sample group is evaluated by the number of elements in the set, and all combinations in the original matching results are traversed to search for the maximum value of the consistent set and the corresponding matching sample combination as the final output.

[0042] The feature point extraction process is strictly limited to the fixed plane of the target component to ensure that all calculations comply with the homography principle of a single plane;

[0043] The feature extraction area is represented by a rectangular bounding box, and the target component area of ​​the device is continuously tracked using a kernel correlation filter tracking algorithm. The steps include: manually selecting and determining the area where the target to be tracked is located in the image, so as to determine an ideal Gaussian output response result; in the training stage, extracting the multi-channel directional gradient histogram features of the target area, and using the kernel function to calculate the kernel matrix generation vector corresponding to the current area feature; using the kernel matrix, the least squares method based on ridge regression and the ideal response result in the initial image frame, training and generating an optimized filter template; in the detection stage, densely sampling the image in the form of a circulant matrix, calculating the current area features and performing correlation operations with the trained filter template to obtain confidence, and the confidence of all samples together constitutes the output response of the current frame, and the position with the maximum response value is the target location;

[0044] Based on the above steps, an effective reconstruction channel is constructed by obtaining the matching relationship between the feature points extracted from the current frame and the corresponding points in the initial frame (the task described in step 2), and then mapping the pixel coordinates of the feature points in the initial frame to the corresponding plane world system coordinates of the target component (the task described in step 1).

[0045] Beneficial effects of the present invention:

[0046] The present invention constructs a self-state estimation system for an oral and pharyngeal surgical robot based on monocular visual perception and traditional features; through an artificial auxiliary guidance device, a front-end visual odometer based on ORB features, and a back-end optimization based on a multi-style bundle adjustment method, the problems of lack of texture features, continuous non-rigid deformation, one-way operation channels, and repetitive appearance representation in the special environment of the oral and pharyngeal area are effectively overcome, ensuring stable, accurate, and robust pose estimation of the end effector itself within continuous moments in the roaming phase; an effective and stable target pose detection process based on the plane homography principle and the kernel correlation tube filter tracking algorithm is constructed, and combined with an artificial target component, the pose estimation problem of multi-target instruments under extremely close observation distance in the oral and pharyngeal operating environment is effectively solved. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 It is a basic design drawing of the auxiliary guidance device and components of the artificial oral throat during the roaming stage.

[0048] Figure 2 It is a flowchart of the basic principle of the end effector's own pose estimation method.

[0049] Figure 3 It is a basic design drawing of the artificial target components and specific characteristic patterns of the target surgical instrument during the execution phase.

[0050] Figure 4It is a flowchart of the basic principles of target device detection and pose estimation methods. DETAILED DESCRIPTION

[0051] The present invention is further described in detail below in conjunction with embodiments.

[0052] The present invention constructs a complete navigation and positioning system for the end effector of an oral and pharyngeal surgery robot, which combines specially designed auxiliary device components, artificial feature patterns, medical NBI enhanced monocular endoscopes, and robot posture state estimation methods.

[0053] like Figure 1-Figure 4 As shown: the whole surgical execution process can be divided into two main stages: the roaming stage and the execution stage. In the roaming stage, the overall model of the end effector will be controlled and guided by the external auxiliary robot arm through the flexible connector, and will approach the actual lesion occurrence area inside the oral cavity and pharynx from the outside of the human body, and after obtaining the best observation posture, it will stop running and fix the posture state of the overall model; in the execution stage, the end effector will use the laser reflection device and surgical forceps and other surgical instruments to complete specific actions, and the monocular endoscope and auxiliary lighting will provide real-time observation of the surgical area, and finally complete the cutting and suturing of specific lesions such as tumors and polyps, complete the surgical task, and exit the area. The ultimate specific goal of the embodiment of the present invention is to provide posture estimation for the actuator itself during the roaming stage, and to provide posture estimation for observable target surgical instruments within the field of view during the specific execution stage of the operation.

[0054] The operating space inside the oral cavity and pharynx is narrow, and the types of sensor devices that can be carried by the actuator are severely limited. Multiple target objects are very easy to block each other, and there are higher requirements for solution accuracy during operation. In addition, the extremely close observation line of sight caused by the narrow space, the lack of texture features inside the organs, and the poor lighting conditions make it difficult to build a navigation and positioning method based on visual input in this environment. In order to address the above problems, the technical solution adopted by the present invention is to construct an artificial auxiliary guidance device and a characteristic target, based on the visual input of a monocular endoscope, and based on the synchronous positioning and mapping technology and the target posture detection technology to build a navigation and positioning solution for the surgical space, including the following steps:

[0055] S1, for the roaming stage when the surgical robot end effector approaches the lesion area inside the oral cavity and throat from the outside of the human body, an artificial guidance auxiliary device surrounding the internal structure of the oral cavity is constructed to serve as a physical path guide during the approach stage. Most importantly, the attached artificial texture features of the inner wall are designed as visual aids to address the problem of lack of features in the original operating environment;

[0056] S2, based on the guidance auxiliary device and artificial visual features, during the roaming phase, by combining a lightweight visual odometer with a backend optimization module, a method for estimating the end effector's own pose based on simultaneous localization and mapping technology is constructed;

[0057] S3, for various specific surgical instruments carried by the actuator, constructs artificial feature targets and connects them to the instrument body in a specific way. During the execution phase, it is convenient for the algorithm in S4 to track and locate the specific surgical instruments within the field of view through the target area;

[0058] S4, based on the artificial feature targets on the surgical instrument model, in the execution stage, a specific surgical instrument pose estimation method based on target pose detection technology is constructed through the plane homography principle and traditional feature method;

[0059] Furthermore, the artificial guidance auxiliary device surrounding the internal structure of the oral cavity described in S1 is a hollow truncated cone with a maximum outer diameter of not less than 75 mm, a minimum outer diameter of not more than 15 mm, and an overall outer diameter that is constantly shrinking, and is made by 3D printing technology. Among them, the 15 mm minimum outer diameter is designed with reference to the structural regulations of the human throat, that is, the maximum inner diameter of the throat of an average adult does not exceed 15 mm. During the roaming stage, the device will directly guide the end effector of the surgical robot (controlled by a flexible body connection) from the outside of the patient's mouth, enter through the opening of the maximum inner diameter of the truncated cone, and arrive at the exit of the minimum inner diameter along the contraction pipe, that is, near the area where the lesion occurs.

[0060] The device is generally composed of multiple detachable sub-components: multiple hollow frustums with similar structures and fixed inner diameters (i.e., the hollow body is in the same cylindrical shape), such as five hollow frustums with inner diameters of 13, 17, 23, 33, and 49 mm, respectively. The inner diameter of a single component is fixed, the outer diameter is constantly shrinking, the thickness of the side wall of the device body is not less than 1 mm at the smallest point, from the outside of the oral cavity to the inside of the throat, representing different travel sections of the approach stage, and the overall travel can be modified by disassembling and assembling the sub-components of the device to cope with surgical tasks in different areas near the outside or deep inside the oral throat, which also makes the overall design and subsequent actual operation more flexible.

[0061] In addition to guiding the end effector to enter, another main purpose of the device is to solve the problems of lack of texture features and continuous non-rigid deformation of tissues in the special operating space of the original oral cavity and throat. Through the artificial texture features and rigid structural walls of the inner wall of the device, a stable operating environment support is provided for the end effector's own pose estimation method based on visual input and traditional features. Specially designed feature patterns are attached to the side walls of each component of the device parallel to the running direction; because the inner diameter of the hollow part of the device body is fixed and the maximum outer diameter is constantly shrinking, multiple annular truncated cones with a minimum width of not less than 2 mm can actually be formed at the connection and opening of each segment component. The plane of the truncated cone faces the entry direction and is perpendicular to the running direction, and a special texture pattern is set at the same time. Through the densely arranged texture patterns on the inner wall of the device, combined with different orientations, the end effector can be provided with visual feature assistance to the maximum extent during the entry stage, preventing the extraction and matching results of effective feature points in a single image frame from being insufficient due to problems such as skewed observation angle and lens distortion, which affects the final algorithm operation.

[0062] Furthermore, the end effector's own pose estimation method based on the simultaneous positioning and mapping technology in S2 consists of a front-end visual odometer module that gives the initial pose estimation at the current moment, and a back-end optimization module that is used to continuously optimize the overall estimation results in the operating channel.

[0063] The front-end visual odometer module is implemented based on the ORB (Oriented FAST and Rotated BRIEF) feature. In the continuous moments of the running channel, the visual odometer is used to calculate the relative pose transformation of the camera between adjacent moments based on the current moment input image frame, or initialize the module at the initial moment based on the valid continuous image pair, and reconstruct the basic sparse map points and the channel start.

[0064] Furthermore, the basic principles and processes of the front-end visual odometer module are described as follows: extract image key points based on ORB features; obtain inter-frame feature correspondences through brute force matching and screening mechanisms; construct a Frame2Frame multi-view geometric model between valid initialized monocular image pairs, obtain the initial adjacent moment pose transformation based on epipolar constraints, and reconstruct the 3D coordinates of feature points by triangulation to determine the unique monocular scene scale information; for monocular image input at any time, construct a feature matching relationship from image pixels to known reconstructed space points, construct a Model2Frame geometric model and solve the pose transformation based on the Pnp (Perspective-n-Point) method; provide an initial pose estimate to the optimization module and insert sparse map feature points.

[0065] The backend optimization module is implemented based on the multi-style bundle adjustment method. During the process of camera calibration internal parameter estimation, initial pose transformation estimation and map point reconstruction, the errors in the operation channel will continue to accumulate, causing the subsequent estimation results to gradually deviate from the actual situation. The optimization module is used to constrain the overall estimation results in the channel by constructing different inter-frame common view relationships in this process to eliminate error accumulation.

[0066] Furthermore, the surgical task for lesions in the oral and pharyngeal areas is performed in a nearly external, narrow and unidirectional channel. Usually, it is only necessary to provide the end effector with its own posture estimation service when it enters the roaming stage. After the operation, it can be directly evacuated from the surgical area under guidance. Therefore, the task scenario is "unidirectional" and there is no actual channel closed loop. At the same time, the throat area is a narrow and nearly straight channel with strong repeatability in texture features, which can easily form an incorrect closed loop due to incorrect feature matching. Therefore, based on the above two special factors, in the construction of the back-end module of this embodiment, the global constraint construction part based on closed loop detection and repositioning is excluded, and the construction, maintenance and optimization of the common view relationship are mainly centered on the following three forms:

[0067] Full optimization is used in the initialization phase of the algorithm to thoroughly tune the corresponding pose estimation results of continuous valid image pairs and all initial map points generated internally, as the starting point of the entire running channel, to define a globally consistent monocular scale as the basis for subsequent estimation; Motion Only optimization is used to optimize the pose state at continuous moments in the running channel and the latest estimation results output by the front-end visual odometer; Local optimization is based on the newly inserted map points and the established common view relationship. During the local mapping process, the current frame and all other frames and map points related to it are optimized once, and other irrelevant sequence frames are excluded from this optimization.

[0068] Furthermore, the artificial target pattern in S3 at least includes a QR code, an ArUco reference, and multiple layers of nested concentric rings. The above characteristic pattern is set on the surface of the target surgical instrument end component, and optionally, the component has the following characteristics:

[0069] The front and rear ends of the assembly are respectively connected to the corresponding surgical instrument body and the flexible body connecting arm, and are a regular rectangular parallelepiped, with a side length of the square cross section not exceeding 4 mm, so as to adapt to the general size of universal end surgical instruments;

[0070] The outer side walls of the component are four strictly rectangular planes, and the surfaces are respectively filled with a QR code target pattern and any other artificial target feature pattern, that is, all feature points are extracted strictly in the same plane, which conforms to the assumption of the homography principle. At the same time, it is believed that in the observation angle after arbitrary rotation, the position and posture of the surgical instrument within the current field of view can still be estimated through the corresponding side wall plane group features.

[0071] Furthermore, the target surgical instrument pose estimation method based on the special feature pattern described in S4 mainly includes the following steps:

[0072] Step 1: In the algorithm initialization process, the homography mapping relationship between the endoscope camera with a fixed posture in the execution phase and the target plane corresponding to the surgical instrument is calculated;

[0073] Step 2: At any subsequent time, extract feature points from the current image frame input and match features to the initialization frame;

[0074] Step 3: Based on the world coordinate system set with reference to the target plane and moving with the instrument body, reconstruct the three-dimensional world coordinates of the feature points of the current frame based on the reference;

[0075] Step 4: Based on the known imaging plane pixel coordinates of the feature points in the current frame and the reconstructed three-dimensional coordinates, construct the multi-view geometry model of Model2Frame, and calculate the pose transformation of the device relative to the camera device at the current moment based on the Pnp method.

[0076] Optionally, step 1 includes the following steps:

[0077] Step 1.1: Select the initialization image frame, manually select any feature point in the QR code feature pattern on a single surface of the target component, and obtain the corresponding two-dimensional pixel point coordinates (x c ,y c );

[0078] Step 1.2: Take one corner of the rectangular feature pattern on a fixed single plane as the coordinate origin, set the world coordinate system that moves with the instrument, and refer to the QR code feature pattern with known size and shape to obtain the corresponding 3D world coordinates (x w ,y w ,z w ), since all the extracted feature points are located on the same component sidewall plane, that is, the coordinate system plane X o Y, so the three-dimensional coordinates of the feature point (x w ,y w ,0);

[0079] Step 1.3: Repeat the above steps to select N pattern feature points on the current component plane, where N ≥ 4;

[0080] Step 1.4: Calculate the 2D pixel coordinates of the imaging plane from the feature points To the corresponding three-dimensional world coordinates The mapping relationship, where i is the index to the current corresponding feature point, can be represented by a homography matrix H.

[0081]

[0082] Continue to simplify and eliminate the scale factor z,

[0083]

[0084] Optionally, the feature point extraction process in step 2 is based on SIFT, ORB, SURF and other methods. In addition, during the feature matching screening process, the Hamming distance between the binary description vectors of the feature points is calculated, and according to the set distance threshold, which is generally twice the minimum distance, the matches with a relative distance greater than the threshold are extracted from the original results, and the first round of brute force screening for the matching results is completed at one time; then the random sampling consistency method is used to perform a secondary screening of the outlier matching results, and a group is randomly selected from the original matching samples to calculate and generate the relative pose transformation, and an error function based on the least squares method is constructed. Other samples that can fit the current conversion model are calculated and form a consistent set, and the "correctness" of the current sample group is evaluated by the number of elements in the set, and all combinations in the original matching results are traversed to search for the maximum value of the consistent set and the corresponding matching sample combination as the final output.

[0085] The feature point extraction process is strictly limited to a fixed plane of the target component to ensure that all calculations comply with the homography principle of a single plane.

[0086] Further, the feature extraction area is represented by a rectangular bounding box. In this embodiment, a kernel correlation filter tracking algorithm is used to continuously track the target component area of ​​the device, and the steps include: manually selecting and determining the area where the target to be tracked is located in the image, so as to determine an ideal Gaussian output response result; in the training stage, the multi-channel directional gradient histogram features of the target area are extracted, and the kernel function is used to calculate the kernel matrix generation vector corresponding to the current area feature; using the kernel matrix, the least squares method based on ridge regression and the ideal initial image frame response result, the optimized filter template is trained and generated; in the detection stage, the image is densely sampled in the form of a circulant matrix, the current area features are calculated and correlated with the trained filter template to obtain confidence, and the confidence of all samples together constitutes the output response of the current frame, and the position with the maximum response value is the target.

[0087] Based on the above steps, an effective reconstruction channel can be constructed by obtaining the matching relationship between the feature points extracted from the current frame and the corresponding points in the initial frame (the task described in step 2), and then mapping the pixel coordinates of the feature points in the initial frame to the corresponding plane world system coordinates of the target component (the task described in step 1).

[0088] Further, as described in step 4, based on the correspondence between the 2D pixel coordinates of this feature point and the 3D world coordinates, a Model2Frame multi-view geometry model can be constructed, and the Pnp method can be used to finally calculate the position and posture of the corresponding surgical instrument, that is, its target component relative to the camera device at the current moment.

[0089] Example:

[0090] The surgical robot system based on femtosecond laser knife is used to perform automatic cutting and suturing of various types of lesions in the oral and throat areas under the supervision and assistance of surgeons. The surgical robot system consists of a monitoring and control operating host, an external auxiliary robotic arm, a flexible connecting arm and an end effector. The end effector is equipped with a medical monocular endoscope with NBI enhanced display, electrothermal surgical forceps, a laser reflection device and other specific instruments and other components involved in the actual execution of the operation.

[0091] Figure 1 The basic design diagram of the artificial oral and pharyngeal guidance auxiliary device and its components of the present invention. Based on the component device, the visual navigation method for the end effector itself proposed in the present invention can provide effective continuous pose state estimation in a relatively stable operation channel with the support of sufficient environmental texture features. The front-end visual odometer based on ORB features is combined with the back-end optimization module based on the multi-style bundle adjustment method to finally construct a lightweight visual SLAM algorithm that adapts to the special narrow space of the oral and pharyngeal cavity. Its basic process is as follows: Figure 2 As shown, the following steps are included:

[0092] Step S1, receiving the image acquisition at the current moment, extracting pixel feature points based on ORB (Oriented FAST and Rotated BRIEF) features, and calculating and generating corresponding description vectors;

[0093] Step S2, searching for matching relationships between image frames based on feature extraction results within continuous time, and screening matching results based on brute force screening and random consistency principles, and finally obtaining valid matching samples;

[0094] The descriptor of the feature point is composed of a description vector, and the matching relationship is obtained by traversing all feature extraction samples in two consecutive image frames and calculating the Hamming distance between the description vectors.

[0095] Step S2-1 sets a threshold based on the distance between the description vectors, which is generally twice the minimum distance between matching samples. In the brute force matching process, matching samples with a relative distance greater than the threshold will be directly removed from the original sample set. This is an intuitive empirical method. However, in most cases, the distance of an absolutely correct matching result is naturally very small, so this is the most direct and effective method.

[0096] The screening method based on random sampling consistency in step S2-2 is a method for estimating model parameters in a sample set containing outliers by iteration; the screening process includes the following steps:

[0097] Set a maximum value N of the elements of a consistent set max =0;

[0098] A random group is selected from the original matching samples to calculate the relative transformation T of the pose, and an error function based on the least squares method is constructed to calculate other samples that can fit the current transformation model and form a consistent set S;

[0099] Calculate the number of elements n in the current consistent set (reflecting the "correctness" of the transformation generated based on the current sample group), if n ≥ N max , update N max ;

[0100] Traverse all matching samples and repeat the above steps until the maximum number of elements in the consistent set of this matching result and the corresponding sample set are found;

[0101] Step S3, based on the pixel feature points in the current image frame and the mapping to the corresponding three-dimensional map points obtained by feature matching, a 3D-2D (Model2Frame) multi-view geometric model is constructed, and the preliminary pose state estimation result is calculated and output by the Pnp (Perspective-n-points) method. The mathematical model of this problem can be described as follows:

[0102] sp=KP C =K(R CW P W +t CW )

[0103] In the model, p, P C , P W They represent the pixel coordinates of the feature points extracted in the current frame, the 3D coordinates of the corresponding spatial feature points in the camera coordinate system, and the 3D coordinates of the corresponding spatial feature points in the world coordinate system, s is the scale factor, K is the camera intrinsic parameter, and R CW and t CW They represent the rotation and translation of the camera coordinate system relative to the world coordinate system.

[0104] In addition, in the present invention, the initialization process of the monocular scene is independent of the general process of solving the pose state at continuous moments, and is implemented based on 2D-2D (Frame2Frame) epipolar geometry constraints and triangulation reconstruction. The problem model and basic steps are as follows:

[0105]

[0106] Where p1 and p2 represent the coordinates of the pixel feature points in the selected initial image pair, x1 and x2 are the projection coordinates of the pixel points p1 and p2 on the normalized imaging plane, the camera relative pose transformation consists of the rotation matrix R and the translation vector t, and K represents the intrinsic parameters of the camera device;

[0107] The above model is solved based on the eight-point method (feature matching samples of eight or more initial image pairs), and accurate monocular global consistency scale estimation is obtained in the initialization stage to reconstruct the initial map points.

[0108] The inside of the oral cavity and throat is similar to a long and narrow straight running channel. Considering the actual state of the surgical task scene, in general, the visual navigation algorithm only needs to provide posture estimation services in the entry part during the roaming stage. In the end, there is no closed-loop path connected to the first position in this one-way long and narrow running channel. Therefore, in the construction of the back-end optimization module of the present invention, the closed-loop detection function in the general visual SLAM framework is eliminated, and the channel optimization is implemented only through the local common view relationship. Among them, the main optimization forms include the following three:

[0109] Full optimization is used in the initialization phase of the algorithm to thoroughly tune the pose estimation results of consecutive valid image pairs and all the initial map points generated internally as the starting point of the entire running channel to define a globally consistent monocular scale as the basis for subsequent estimation;

[0110] Only optimize the motion (Motion Only) to optimize the posture state of continuous moments in the running channel, and optimize the latest estimation results output by the front-end visual odometer;

[0111] Local optimization (Local) is based on the newly inserted map points and the established co-viewing relationship. During the local mapping process, the current frame and all other frames and map points related to it are optimized once, and other irrelevant sequence frames are excluded from this optimization.

[0112] Step S4, the basic operation steps of this optimization module and the common view information maintenance are as follows:

[0113] Step S4-1, algorithm initialization stage, based on the redundant feature point extraction and matching results, construct the initial three-dimensional map points. The optimization in this stage adopts the bundle adjustment method for all objects: select the two corresponding posture state nodes of the initialization image frame and the current inserted map feature points;

[0114] Step S4-2, in the continuous moment of the running channel, find the corresponding relationship between the current frame and the constructed map point, record this observation: the total number of times the map point object is observed is added by one, and based on the same observed map point, the common view information between the local associated nodes is updated; based on this local common view relationship, by minimizing the reprojection error of the map point to each node image frame, the other nodes and map points associated with the current moment are optimized as a whole;

[0115] The main information of the co-viewing relationship includes: the total number of times the inserted map point is observed by the corresponding state nodes at different times, and the corresponding node ID; the number of map points observed between the corresponding state nodes at different times. With the above information, a local graph based on the state nodes at the current moment can be extracted by traversal search before any local optimization at the current moment is performed.

[0116] Step S4-3 inserts other valid feature points that do not form a matching relationship at the current moment, and optimizes the preliminary estimation result of the current position state based on the Motion-Only method.

[0117] Figure 3 The intermediate connector and related characteristic patterns of the specific surgical instrument of the embodiment of the present invention are designed. The intermediate connector is a transition from the flexible connecting arm to the instrument body, which is composed of a cube, and the outer wall is four strict rectangular planes with specially designed characteristic patterns attached. Based on the plane characteristic pattern, the present invention constructs a target posture detection method based on artificial characteristic patterns and plane homography principle. The basic process is as follows: Figure 4 As shown, the following steps are included:

[0118] Step S1, a specially designed artificial feature target is connected to a specific surgical instrument in a specific manner, and the actual three-dimensional coordinates of the corresponding pattern feature points can be obtained by referring to the moving target system;

[0119] Step S2, in the algorithm initialization stage, manually select feature points in the selected image frame to obtain the corresponding pixel plane 2D coordinates. At the same time, according to step S1, the corresponding three-dimensional coordinates of the feature points are known, thereby obtaining the mapping relationship from the pixel 2D coordinates to the corresponding 3D coordinates, that is, the homography matrix H;

[0120] Step S3, in the algorithm initialization stage, manually select the target tracking surgical instrument in the selected image frame through a rectangular bounding box, that is, the characteristic target plane on the corresponding middleware;

[0121] Step S4, at any continuous moment in the running channel, extract the feature target in the picture based on the kernel correlation filtering algorithm, extract the pixel feature points in this limited area, and search for the feature matching of the image frame initialized and selected in steps S2 and S3;

[0122] Step S5, based on the feature matching to the initially selected image frame and the homography matrix calculation result, reconstructing the corresponding three-dimensional coordinates of the currently extracted pixel feature point;

[0123] Step S6, based on the known 2D coordinates of the pixel feature points and the corresponding 3D coordinates at the current moment, a multi-view geometric model of Model2Frame is constructed, and the position state of the target surgical instrument, i.e., the mobile target system, at the current moment is calculated based on the Pnp method.

[0124] The homography principle of the initialization stage can be summarized as follows:

[0125] For pixel feature points in the same plane, especially when the plane coincides with a plane of the coordinate system, in the context of the embodiment of the present invention, the feature plane coincides with the XoY plane of the moving target system, then the Z-axis value of the three-dimensional coordinate of the feature point corresponding to the world system is uniquely determined to be 0, that is, the 3D coordinate (X w ,Y w ,0) and its corresponding pixel plane coordinates (X C ,Y C ), based on the camera device sensor intrinsic parameter matrix A, there are the following conversions:

[0126]

[0127] The mapping relationship H can be simplified as follows:

[0128]

[0129] Each embodiment in this specification is described in a related manner, and the same or similar parts between the embodiments can be referred to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the system embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment.

Claims

1. A visual navigation method for the narrow space of the oral cavity and pharynx based on artificial feature targets and guidance devices, characterized in that: The steps include: S1: When the surgical robot end effector approaches the lesion occurrence area inside the oral cavity and pharynx from the outside of the human body during the roaming stage, an artificial guidance auxiliary device surrounding the internal structure of the oral cavity is constructed to serve as a physical path guide during the approach stage, and an artificial auxiliary visual feature is set; the visual auxiliary feature is composed of a planar checkerboard target with different pattern designs, and is located on the inner wall surface of the guidance device; S2: Based on the guidance auxiliary device and artificial auxiliary visual features in S1, during the roaming stage, a method for estimating the end effector's own pose based on simultaneous positioning and mapping technology is constructed by combining a lightweight visual odometer and a backend optimization module; S3; the overall model of the end effector of the surgical robot is connected to the surgical instrument through an intermediate connector; a planar feature target with a variety of pattern designs is constructed and attached to the side wall surface of the intermediate connector, so that in the execution stage, the algorithm in S4 can track and locate the specific surgical instrument within the field of view through the target area; S4: Based on the planar feature targets on the specific surgical instruments and the corresponding connectors, in the execution stage, a specific surgical instrument pose estimation method based on target pose detection technology is constructed through the plane homography principle and traditional feature method; The end effector's own pose estimation method based on the synchronous positioning and mapping technology in S2 includes a front-end visual odometer module for providing an initial pose estimation at the current moment, and a back-end optimization module for continuously optimizing the overall estimation result in the operation channel; The front-end visual odometer module is implemented based on the ORB (Oriented FAST and Rotated BRIEF) feature. In the continuous moments of the running channel, the visual odometer is used to calculate the relative pose transformation of the camera between adjacent moments based on the current moment input image frame, or initialize the module at the initial moment based on the valid continuous image pair to reconstruct the basic sparse map point and the channel start; The basic principle and process of the front-end visual odometer module are described as follows: Extract image key points based on ORB features; obtain inter-frame feature correspondence through brute force matching and screening mechanisms; Between the effective initialized monocular image pairs, a multi-view geometric model of Frame2Frame is constructed, the initial adjacent moment pose transformation is obtained based on the epipolar constraint, and the 3D coordinates of the feature points are reconstructed by triangulation to determine the unique monocular scene scale information; for the monocular image input at any time, the feature matching relationship from the image pixel point to the known reconstructed space point is constructed, the geometric model of Model2Frame is constructed and the pose transformation is solved based on the Pnp (Perspective-n-Point) method; the initial pose estimation is provided to the optimization module, and the sparse map feature points are inserted; The back-end optimization module is implemented based on the multi-style bundle adjustment method. During the process of camera calibration internal parameter estimation, initial pose conversion estimation and map point reconstruction, the errors in the operation channel will continue to accumulate, causing the subsequent estimation results to gradually deviate from the actual situation. The optimization module is used to constrain the overall estimation results in the channel by constructing different inter-frame common view relationships in this process to eliminate error accumulation; The bundle adjustment modes used include the following: Thoroughly tune the overall optimization goals, that is, the pose state nodes and related map points corresponding to all moments in the running channel. In the initialization stage of the algorithm, use the most luxurious method (that is, for the initialization image frame pairs, the initial construction of map points and other overall optimization goals, based on the method of minimizing reprojection errors, perform thorough optimization) to obtain the most accurate initial state, map points and monocular scale estimation, so as to minimize subsequent error accumulation; For the local optimization target, all other associated nodes and observed map points around the current state node determined by the common view relationship search are thoroughly optimized. At any time in the running channel, after each map point is inserted, this local area is optimized; Only for the optimization of pose state nodes, after obtaining the preliminary pose estimation through the front-end visual odometer and before inserting the map points, the estimation results are preliminarily optimized; All the above optimization processes are based on minimizing the reprojection error from map points to corresponding image frames; The specific surgical instrument posture estimation method based on the target posture detection technology in S4 mainly includes the following steps: Step 1: In the algorithm initialization process, the homography mapping relationship between the endoscope camera with a fixed posture in the execution phase and the target plane corresponding to the surgical instrument is calculated; Step 2: At any subsequent time, extract feature points from the current image frame input and match features to the initialization frame; Step 3: Reconstruct the three-dimensional world coordinates of the feature points of the current frame based on the world coordinate system set with reference to the target plane and moving with the instrument body; Step 4: Based on the known imaging plane pixel coordinates of the feature points in the current frame and the reconstructed three-dimensional coordinates, construct the multi-view geometry model of Model2Frame, and calculate the pose transformation of the device relative to the camera device at the current moment based on the Pnp method.

2. The method for visual navigation in the narrow space of the oral cavity and pharynx based on artificial feature targets and guidance devices according to claim 1 is characterized in that: The method for constructing the artificial guidance auxiliary device surrounding the internal structure of the oral cavity described in S1 is as follows: The surrounding guide device is a hollow truncated cone, with gradually decreasing inner and outer diameters, and the smallest outer diameter is no more than 15 mm, to meet the requirements of the internal structure of the adult oral and pharyngeal cavity; the device is composed of a plurality of truncated cone components of different specifications connected in series, and in actual use, they can be assembled arbitrarily to meet the needs of surgery in different areas of the oral and pharyngeal cavity; the inner surface of the hollow component is composed of a surrounding side wall parallel to the entry direction of the actuator, and a front narrow truncated cone plane perpendicular to the entry direction, on which a large number of specially designed plane feature patterns are arranged, which provide sufficient feature point support for the operation of the visual algorithm through different observation angles, and the plane feature patterns include targets in different forms such as QR codes, ArUco benchmarks, and multi-layer nested concentric rings; During the roaming stage, the artificial guidance auxiliary device will directly guide the surgical robot end effector (controlled by a flexible body connection) from outside the patient's mouth, enter through the maximum inner diameter opening of the cone, and arrive at the minimum inner diameter exit along the contraction channel, that is, near the lesion area.

3. The method for visual navigation in the narrow space of the oral cavity and pharynx based on artificial feature targets and guidance devices according to claim 1, characterized in that: The basic construction method of the surgical instrument intermediate connector and the artificial feature target in S3 is as follows: The surgical instrument intermediate connector is used as a transition part from the flexible body connecting arm to the instrument body, and is composed of a cube with four strict rectangular planes on the outside to construct a texture support based on an artificial feature pattern; The characteristic patterns include QR codes, ArUco benchmarks, and multi-layer nested concentric rings. Multiple observed planes can provide sufficient texture support for the visual positioning algorithm. The characteristic patterns are set on the surface of the target surgical instrument end component. The front and rear ends of the assembly are respectively connected to the corresponding surgical instrument body and the flexible body connecting arm, and are a regular rectangular parallelepiped, with a side length of the square cross section not exceeding 4 mm, so as to adapt to the general size of universal end surgical instruments; The outer side walls of the component are four strictly rectangular planes, and the surfaces are respectively filled with a QR code target pattern and any other artificial target feature pattern, that is, all feature points are extracted strictly in the same plane, which conforms to the assumption of the homography principle. At the same time, it is believed that in the observation angle after arbitrary rotation, the position and posture of the surgical instrument within the current field of view can still be estimated through the corresponding side wall plane group features.

4. The method for visual navigation in the narrow space of the oral cavity and pharynx based on artificial feature targets and guidance devices according to claim 1, characterized in that: The step 1 comprises the following steps: Step 1.1: Select the initialization image frame, manually select any feature point in the QR code feature pattern on a single surface of the target component, and obtain the corresponding two-dimensional pixel point coordinates (x c ,y c ); Step 1.2: Take one corner of the rectangular feature pattern on a fixed single plane as the coordinate origin, set the world coordinate system that moves with the instrument, refer to the QR code feature pattern with known size and shape, and obtain the corresponding 3D world coordinates (x w ,y w ,z w ), since all the extracted feature points are located on the same component sidewall plane, that is, the coordinate system plane X o Y, so the three-dimensional coordinates of the feature point (x w ,y w ,0); Step 1.3: Repeat the above steps to select N pattern feature points on the current component plane, where N ≥ 4; Step 1.4: Calculate the 2D pixel coordinates of the imaging plane from the feature points To the corresponding three-dimensional world coordinates The mapping relationship, where i is the index to the current corresponding feature point, can be represented by a homography matrix H, Continue to simplify and eliminate the scale factor z, 5. The method for visual navigation in the narrow space of the oral cavity and pharynx based on artificial feature targets and guidance devices according to claim 1, characterized in that: The feature point extraction process in step 2 is based on SIFT, ORB, SURF and other methods. In the feature matching screening process, the Hamming distance between the binary description vectors of the feature points is calculated. According to the set distance threshold, which is generally twice the minimum distance, the matches with a relative distance greater than the threshold are extracted from the original results, and the first round of brute force screening for the matching results is completed at one time; then, the random sampling consistency method is used to perform a secondary screening on the outlier matching results, and a group is randomly selected from the original matching samples to calculate and generate the relative pose transformation, and an error function based on the least squares method is constructed. Other samples that can fit the current transformation model are calculated and form a consistent set, and the "correctness" of the current sample group is evaluated. All combinations in the original matching results are traversed, and the maximum value of the consistent set and the corresponding matching sample combination are searched as the final output.

6. The method for visual navigation in the narrow space of the oral cavity and pharynx based on artificial feature targets and guidance devices according to claim 1, characterized in that: The feature point extraction process is strictly limited to the fixed plane of the target component to ensure that all calculations comply with the homography principle of a single plane; The feature extraction area is represented by a rectangular bounding box, and the target component area of ​​the device is continuously tracked using a kernel correlation filter tracking algorithm. The steps include: manually selecting and determining the area where the target to be tracked is located in the image, so as to determine an ideal Gaussian output response result; in the training stage, extracting the multi-channel directional gradient histogram features of the target area, and using the kernel function to calculate the kernel matrix generation vector corresponding to the current area feature; using the kernel matrix, the least squares method based on ridge regression and the ideal response result in the initial image frame, training and generating an optimized filter template; in the detection stage, densely sampling the image in the form of a circulant matrix, calculating the current area feature and performing correlation operations with the trained filter template to obtain confidence, and the confidence of all samples together constitutes the output response of the current frame, and the position with the maximum response value is the location of the target; Based on the above steps, an effective reconstruction channel is constructed by obtaining the matching relationship between the feature points extracted from the current frame and the corresponding points in the initial frame, and then mapping the pixel coordinates of the feature points in the initial frame to the corresponding plane world system coordinates of the target component.

Images