Urethral dilation path planning system fusing visual slam and force perception

Abstract

The application relates to the technical field of intelligent urethral surgical instruments, and particularly discloses a urethral dilation path planning system fusing visual SLAM and force sensing, which synchronously collects urethral internal environment image data and contact force data between the instrument and the tissue through an image sensor and a force sensing unit integrated at the front end of a dilation instrument; a three-dimensional environment model of the urethra is constructed by using a visual SLAM technology, and the position of the instrument is located in real time; dynamic force distribution is generated by fusing the force data and the three-dimensional model, a safe dilation path is planned based on the distribution and the current position of the instrument; the advancing direction of the instrument and the dilation strength are dynamically adjusted through a closed-loop control system, so that the instrument moves along the planned path; meanwhile, a monitoring mechanism is used to detect path deviation and force abnormality in real time, and model updating and path re-planning are triggered.

Description

Technical Field

[0001] This invention relates to the field of intelligent urethral surgical instruments, specifically to a urethral dilation path planning system that integrates visual SLAM and force perception. Background Technology

[0002] In clinical urological surgery, urethral and ureteral stricture dilation is a common procedure. Currently, the use of dilators for urethral stricture relies heavily on the surgeon's experience and manual dexterity, leading to a high incidence of complications such as urethral injury and false urethral passages. Balloon dilators for ureteral stricture suffer from problems such as poor dilation results due to imperfect balloon positioning, and even iatrogenic injuries such as tearing of the normal ureteral mucosa. While some international medical centers use X-ray positioning to monitor balloon location, this method has the drawback of adverse effects on both medical staff and patients. Although current technology proposes a "visualized direct visualization" approach, it still lacks real-time perception of the local tissue mechanical properties of the urethral wall and cannot quantitatively assess changes in tissue stiffness during dilation. This makes it difficult for surgeons to predict the mechanical load on fragile tissues caused by the dilation procedure, and intraoperative safety and precision need improvement.

[0003] Current technology cannot quantitatively assess the local stiffness changes of the urethral wall tissue in real time during urethral dilation, and therefore cannot plan a safe path for the dilator to effectively pass through narrow areas while avoiding excessive pressure on highly rigid and fragile tissues. Specifically, it addresses the problem of how to integrate visually perceived three-dimensional anatomical structures with force-perceived local tissue mechanical properties during real-time operation, and dynamically generate a safe path based on this. Summary of the Invention

[0004] The purpose of this invention is to provide a urethral dilation path planning system that integrates visual SLAM and force perception to solve the problems mentioned above.

[0005] The objective of this invention can be achieved through the following technical solutions:

[0006] A urethral dilation path planning system integrating visual SLAM and force perception includes:

[0007] The data acquisition module is used to acquire real-time image data of the internal environment of the urethra, and at the same time, acquire real-time force data of the dilator as it travels in the urethra.

[0008] The environmental modeling and localization module uses real-time image data and a visual SLAM algorithm to construct a three-dimensional environmental model of the inside of the urethra and to locate the current position of the dilator in the three-dimensional environmental model in real time.

[0009] The path planning module fuses real-time force data with a 3D environment model to generate a dynamic force distribution map of the urethral stricture area, and calculates a safe dilation path within the urethra based on the dynamic force distribution map and the current position of the dilator.

[0010] The path control module dynamically adjusts the direction and force of the expansion device according to the safe expansion path, so that the expansion device moves along the safe expansion path, and feeds back the adjustment results to the data acquisition module in real time to update the data.

[0011] The monitoring and correction module continuously monitors the changes in the 3D environment model updated by the environment modeling and positioning module and the real-time force data feedback from the path control module during the movement of the dilatation device. When a path deviation or force abnormality is detected, the environment modeling and positioning module, the path planning module and the path control module are triggered to re-execute to correct the path until the dilatation device reaches the target narrow area and completes the dilatation operation.

[0012] As a further aspect of the present invention: the construction of a three-dimensional environment model of the urethra using a visual SLAM algorithm specifically includes:

[0013] The acquired real-time image data is preprocessed using a hybrid filtering method that combines anisotropic diffusion and local histogram equalization to enhance the texture features of urethral wall mucosal folds while suppressing spot noise in the urine environment.

[0014] An improved, illumination-invariant corner feature is extracted from preprocessed consecutive image frames. A high-dimensional descriptor is constructed by calculating its directional gradient histogram. Inter-frame feature matching is then performed to preliminarily estimate the motion of the endoscope.

[0015] The successfully matched feature points are triangulated using epipolar geometric constraints to reconstruct their three-dimensional spatial points. A sliding window bundle adjustment method, optimized only for new observation areas, is then used to fuse the three-dimensional spatial points in real time, thereby incrementally generating a three-dimensional environmental model of the urethra.

[0016] As a further aspect of the present invention: the current position of the real-time positioning expansion device in the three-dimensional environment model specifically includes:

[0017] At least three non-collinear markers that will be highlighted under a specific narrowband spectrum are pre-set at the tip of the expansion device;

[0018] Acquire real-time images containing marker points and match them with texture information in the constructed 3D environment model of the urethra to determine the initial pose;

[0019] Extract the two-dimensional pixel coordinates of the highlighted marker points in the current image frame, and couple the two-dimensional coordinates with the pre-defined three-dimensional geometric relationship of the marker points;

[0020] By solving the perspective lattice projection equation, the six-degree-of-freedom spatial pose of the dilator tip relative to the constructed three-dimensional environment model inside the urethra can be directly calculated, and the current position of the dilator in the three-dimensional environment model can be located in real time.

[0021] As a further aspect of the present invention: the dynamic force distribution map of the generated urethral stricture region specifically includes:

[0022] The collected multi-dimensional force signals are spatially registered and mapped onto the geometric surface of the urethral wall corresponding to the three-dimensional environment model.

[0023] Discrete force measurement points are subjected to inverse distance weighting to generate a continuous force distribution surface covering the entire urethral wall;

[0024] Real-time acquisition of tissue deformation data in different regions of the urethral wall, combined with a continuous force distribution surface, yields a stiffness distribution map reflecting the real-time mechanical properties of the urethral wall tissue.

[0025] The stiffness distribution map is superimposed on the continuous force distribution surface to form a dynamic force distribution map.

[0026] As a further aspect of the present invention: the calculation of a safe urethral dilation path based on the dynamic force distribution map and the current position of the dilator specifically includes:

[0027] In the three-dimensional environment model, a three-dimensional path search space is established with the current position of the expansion device as the starting point of the path and the center of the narrow target area as the ending point.

[0028] Areas in the dynamic force distribution map where both force values ​​and tissue stiffness are higher than the preset safety threshold are marked as high mechanical load no-go zones, and passage costs are set for them in the search space.

[0029] While avoiding all high mechanical load restrictions, multiple candidate paths from the starting point to the end point are generated iteratively;

[0030] Ultimately, the smoothest curve with the lowest overall travel cost and conforming to the physiological curvature of the urethra was selected as the safe dilation path.

[0031] As a further aspect of the present invention: the dynamic adjustment of the travel direction and expansion force of the expansion device according to the safe expansion path specifically includes:

[0032] The safe expansion path is discretized into a series of dense path points, and a reference contact force matching the stiffness of the urethral tissue is preset for each point.

[0033] The system compares the three-dimensional spatial deviation between the current position of the expansion device and the target path point in real time, and compares the real-time force data with the reference contact force.

[0034] Based on spatial deviation and force deviation, a composite control command is generated that includes directional correction and force adjustment.

[0035] The composite control command is converted into a drive signal, which controls the micro-movements of the deflection mechanism and the expansion mechanism at the end of the instrument, respectively, so as to achieve coordinated dynamic adjustment of the direction of travel and the expansion force.

[0036] As a further aspect of the present invention: the real-time feedback adjustment result to the data acquisition module to update the data specifically includes:

[0037] The actual posture data of the device and the real-time force data output by the path control module are transmitted back to the data acquisition module in real time through a high-priority data channel.

[0038] After receiving the feedback data, the data acquisition module immediately timestamps and aligns it with the original acquired images and force signals.

[0039] Using the synchronized feedback data, including the actual spatial position of the instrument and its interaction force with the urethral wall, the relevant areas in the image data and the corresponding events in the force data sequence are marked at the data acquisition end;

[0040] The tagged data is immediately sent to the environment modeling and localization module to trigger real-time updates of the 3D environment model and dynamic force distribution map.

[0041] As a further aspect of the present invention: the detection of path deviation or force anomaly specifically includes:

[0042] The actual coordinates of the instrument tip in the three-dimensional environment model are received in real time from the environment modeling and positioning module, and the actual coordinates are compared with the expected coordinates of the safe expansion path to calculate the Euclidean space distance deviation.

[0043] It synchronously receives real-time contact force data fed back from the path control module and calculates the difference between the real-time contact force data and the preset safety force threshold.

[0044] The spatial distance deviation and the contact force difference are input into a bivariate decision-maker. When the spatial distance deviation continues to exceed the tolerance range, or the contact force difference shows a sharp increase in pressure, the bivariate decision-maker determines that a path deviation or force abnormality has occurred.

[0045] After determining an anomaly, the bivariate decision maker immediately generates a trigger signal containing the anomaly type and level code.

[0046] As a further aspect of the present invention: the triggering environment modeling and localization module, the path planning module, and the path control module are re-executed to perform path correction, specifically including:

[0047] The trigger signal is sent to the environment modeling and localization module and the path planning module simultaneously via the system interrupt request line.

[0048] After receiving the signal, the environmental modeling and localization module immediately initiates a local pose optimization based on the latest image data and outputs the corrected current position of the instrument.

[0049] Based on the corrected location and the latest dynamic force distribution map, the path planning module quickly replans a new safe expansion path from the current location to the target point;

[0050] The new safe expansion path is immediately transmitted to the path control module, replacing the original path and controlling the expansion device to continue moving, thus completing a closed-loop path correction.

[0051] The beneficial effects of this invention are:

[0052] (1) By integrating visual and force perception data to generate a dynamic force distribution map, it is possible to identify vulnerable areas with high mechanical load in the urethral wall in real time and actively avoid them in path planning, thereby effectively reducing the risk of iatrogenic damage such as tissue tearing and perforation during the operation.

[0053] (2) The closed-loop control system combining real-time positioning and safety path can dynamically correct the direction and force of the instrument, ensuring that the instrument always moves accurately along the predetermined safety path, reducing the over-reliance on the doctor's personal experience and feel, and improving the consistency and controllability of the surgery. Attached Figure Description

[0054] The invention will now be further described with reference to the accompanying drawings.

[0055] Figure 1 This is a flowchart of the system of the present invention. Detailed Implementation

[0056] 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.

[0057] Please see Figure 1 As shown, this invention is a urethral dilation path planning system integrating visual SLAM and force perception, comprising:

[0058] The data acquisition module is used to acquire real-time image data of the internal environment of the urethra, and at the same time, acquire real-time force data of the dilator as it travels in the urethra.

[0059] The environmental modeling and localization module uses real-time image data and a visual SLAM algorithm to construct a three-dimensional environmental model of the inside of the urethra and to locate the current position of the dilator in the three-dimensional environmental model in real time.

[0060] The path planning module fuses real-time force data with a 3D environment model to generate a dynamic force distribution map of the urethral stricture area, and calculates a safe dilation path within the urethra based on the dynamic force distribution map and the current position of the dilator.

[0061] The path control module dynamically adjusts the direction and force of the expansion device according to the safe expansion path, so that the expansion device moves along the safe expansion path, and feeds back the adjustment results to the data acquisition module in real time to update the data.

[0062] The monitoring and correction module continuously monitors the changes in the 3D environment model updated by the environment modeling and positioning module and the real-time force data feedback from the path control module during the movement of the dilatation device. When a path deviation or force abnormality is detected, the environment modeling and positioning module, the path planning module and the path control module are triggered to re-execute to correct the path until the dilatation device reaches the target narrow area and completes the dilatation operation.

[0063] In the data acquisition module, a miniature image sensor integrated into the front end of the dilation device enables real-time image data acquisition. This miniature image sensor continuously captures consecutive video frames of the urethra during advancement. To adapt to the fluid environment within the urethra and obtain clear images of the tissue structures, a narrowband light source of a specific wavelength is used for illumination. This light source effectively penetrates the fluid medium and enhances the contrast between the folds and blood vessel distribution on the urethral mucosa. The acquired raw image data is transmitted via an internal fiber optic bundle to an external processing unit, forming a real-time image data stream for subsequent processing.

[0064] While acquiring real-time image data, miniature force sensing units distributed throughout the head of the dilator collect contact force data between the instrument and the urethral wall. These force sensing units, built using microelectromechanical systems (MEMS) technology, convert the radial contact pressure and axial friction force on the instrument surface into corresponding electrical signals. The force sensing units are arranged in an array on the instrument surface to obtain the force conditions at different axial positions. The acquired multiple raw analog electrical signals undergo preliminary amplification and filtering inside the instrument to eliminate noise interference introduced by high-frequency mechanical vibrations during operation.

[0065] The pre-processed multi-channel force signals are transmitted to an external signal acquisition card via shielded wires embedded within the instrument catheter. This card converts the analog signals into digital quantities and, according to a preset calibration curve, converts the voltage values ​​into standard mechanical units, thus generating real-time force data with spatial distribution characteristics. This force data not only contains information about the magnitude of the force but also associates it with the specific location on the instrument surface through the sensor number from which it originated.

[0066] In the environment modeling and localization module, the acquired real-time image data of the urethra is first subjected to anisotropic diffusion filtering. This method adjusts the diffusion intensity by analyzing the local gradient magnitude of the image, maintaining strong diffusion in uniform regions with small gradients to suppress spot noise, and weakening diffusion in edge regions with large gradients to preserve the texture details of the urethral mucosal folds. Subsequently, local histogram equalization is performed on the filtered image, dividing the image into multiple overlapping sub-regions and redistributing pixel gray values ​​within each sub-region to enhance local contrast. These two processing steps are performed sequentially, together constituting a hybrid filtering method, ultimately outputting a preprocessed image sequence.

[0067] Improved illumination-invariant corner features are extracted from preprocessed consecutive image frames. These features are obtained by comparing the intensity of multiple circular sampling patterns within the neighborhood of a pixel, maintaining stability against illumination changes. For each extracted corner feature, the histogram of the directional gradient of its surrounding pixel region is calculated. The directional range of 0-180 degrees is uniformly divided into 9 intervals, and the cumulative gradient magnitude within each interval is calculated to form a 36-dimensional feature descriptor. Inter-frame feature matching is performed by comparing the Euclidean distance between feature descriptors in two consecutive images. Based on the successfully matched feature point pairs, the fundamental matrix is ​​estimated using a random sample consensus algorithm, which is then used to decompose and obtain the preliminary motion posture of the endoscope.

[0068] Successfully matched feature points are correlated with each other based on their pixel coordinates and camera intrinsic parameters using epipolar geometric constraints. The 3D spatial coordinates of each feature point are calculated using linear triangulation. A sliding window bundle adjustment method is employed for optimization, with a window size of 10 keyframes. Joint optimization is performed only on all camera poses and 3D spatial points within the window. The objective function is the sum of squares of all reprojection errors, where the reprojection error refers to the difference between the coordinates of the 3D spatial points projected onto the image plane and the actual observed feature point coordinates. This objective function is iteratively minimized using the Levenberg-Marquardt method, outputting the optimized camera pose and 3D spatial point coordinates.

[0069] The locally optimized 3D spatial points are added to the global point cloud set and organized using an octree-based data structure. When the system detects that the image feature similarity between the current observation area and a historical area exceeds a preset threshold of 0.85, it determines that a loop closure has occurred. At this point, pose constraints are added between the pose nodes corresponding to the loop closure, and the global pose graph is optimized to eliminate accumulated errors. Finally, the dense point cloud is converted into a continuous 3D environment model of the urethra using the Poisson surface reconstruction method. This model represents the geometry of the urethra in the form of a triangular mesh.

[0070] The specific implementation process for setting the instrument markers is as follows: At least three non-collinear micro-fluorescent markers are embedded on the tip surface of the dilator. These markers are made of a material capable of producing high-brightness fluorescence under specific narrow-band spectral excitation. The relative positional relationship between the markers is determined through precise measurement during the instrument manufacturing stage, forming a fixed three-dimensional geometric configuration. During use, when illuminated by an excitation light source of a specific wavelength, the markers appear as distinct bright areas in the image, forming a significant contrast with the surrounding urethral tissue, facilitating subsequent identification and extraction.

[0071] After acquiring a real-time image containing marker points, the image is first converted from the RGB color space to the HSV color space, and the highlighted marker point regions are separated using saturation and luminance components. Simultaneously, texture information of visible surfaces is extracted from the constructed 3D environment model of the urethra to generate a virtual rendered image. By comparing the histogram features of directional gradients in the real-time image and the virtual rendered image, the iterative nearest-point algorithm is used to calculate the preliminary pose of the current image relative to the environment model. This pose serves as the initial value for subsequent precise localization.

[0072] In the current image frame, the separated marker regions are binarized, and connected component analysis is used to label each spot region. The moments of the pixel coordinates are calculated for each connected region. The area of ​​the region is calculated using the zeroth moment, and the centroid coordinates of the region are calculated using the first moment, which serve as the two-dimensional pixel coordinates of the marker point. Simultaneously, the ellipse fitting parameters for each marker point are calculated, including the major axis, minor axis, and orientation angle. These two-dimensional observation data are coupled with the pre-defined three-dimensional geometric relationships of the marker points to establish a list of correspondences between the known coordinates of each marker point in three-dimensional space and its observed coordinates in the two-dimensional image.

[0073] The precise pose of the dilatation device is calculated by solving the perspective lattice projection equation, transforming the problem into a nonlinear optimization problem of minimizing reprojection error. The objective function is defined as the sum of squared differences between the two-dimensional observation coordinates of all marked points and their coordinates after projecting their three-dimensional coordinates onto the image plane. The optimization variables are the six-DOF pose parameters of the dilatation device, including three translational components and three rotational components. The Levenberg-Marquardt algorithm is used to iteratively optimize the objective function. In each iteration, the Jacobian matrix of the objective function with respect to the pose parameters is calculated, and the pose parameters are updated until convergence. Finally, the six-DOF spatial pose of the dilatation device tip relative to the three-dimensional environment model inside the urethra is output, achieving real-time localization.

[0074] In the path planning module, the multi-dimensional force signals collected by the force sensing units are mapped to the urethral wall geometry corresponding to the 3D environment model. First, it is necessary to establish the correspondence between the spatial position of the force sensing units and the 3D coordinates of the urethral wall. Using the pre-calibrated 3D coordinates of the force sensing units on the dilation device surface, combined with the real-time acquired six-DOF pose of the dilation device, the 3D position of each force sensing unit in the global coordinate system is calculated. Based on the triangular mesh representation of the urethral wall geometry, the nearest triangular facet of the urethral wall surface is found for each force sensing unit. The force vector measured at this point is decomposed into a normal force perpendicular to the triangular facet and a tangential force parallel to the triangular facet. The magnitude of the normal force is recorded as the contact pressure value at the vertex of the triangular facet, and the magnitude of the tangential force is recorded as the frictional force value at the vertex of the triangular facet.

[0075] Inverse distance weighted interpolation is performed on discrete force measurement points distributed on the urethral wall surface to generate a continuous force distribution surface covering the entire urethral inner wall. For each vertex on the urethral wall triangular mesh, all force measurement points within a 3 mm radius are searched. A weighting coefficient is calculated based on the distance from each measurement point to the vertex, with the weight inversely proportional to the square of the distance. The force values ​​at each measurement point are weighted and averaged using these weighting coefficients to calculate the interpolated force value at that vertex. After completing this calculation for all vertices of the triangular mesh, the force value at any point within the mesh is calculated using bilinear interpolation based on the topological connectivity of the triangular mesh, forming a continuous force distribution surface.

[0076] Real-time acquisition of tissue deformation data from different regions of the urethral wall is performed, and tissue deformation is estimated by analyzing displacement changes on the urethral wall surface in a continuous image sequence. At each vertex of the urethral wall triangular mesh, the three-dimensional displacement between two adjacent image frames is calculated, and the corresponding contact pressure value is simultaneously acquired. Based on the fundamental principle of Hooke's Law, tissue stiffness is estimated as the ratio of contact pressure value to tissue deformation. For each triangular facet region, the average of the estimated stiffness values ​​at its three vertices is taken as the equivalent stiffness of that region. The stiffness values ​​of all triangular facets are mapped onto the urethral wall geometry to form a stiffness distribution map of the urethral wall tissue.

[0077] A complete dynamic force distribution map is constructed by overlaying the stiffness distribution map with a continuous force distribution surface. At each vertex of the urethral wall triangular mesh, three physical quantities are stored: contact pressure value, frictional force value, and local tissue stiffness value. Contact pressure values ​​are represented using the red channel, with larger values ​​resulting in darker colors; frictional force values ​​are represented using the green channel, with larger values ​​resulting in darker colors; and tissue stiffness values ​​are represented using the blue channel, with larger values ​​resulting in darker colors. This color-coding method visualizes the mechanical information. The dynamic force distribution map is updated in real time as new force and image data are input, with the update frequency matching the image acquisition frequency.

[0078] A three-dimensional path search space is established, with the current position of the dilator as the starting point and the center of the target narrow area as the ending point. First, the urethral lumen space from the starting point to the ending point is extracted from the three-dimensional environment model inside the urethra. This space is discretized into a dense three-dimensional grid of points with a resolution of 0.1 mm. Each grid point records its three-dimensional coordinates, tissue type, and accessibility status. Based on the geometric constraints of the urethral wall, grid points located outside the urethral wall are marked as inaccessible, while grid points located inside the urethral lumen are marked as accessible. The boundary of the search space is determined by the inner surface of the urethral wall.

[0079] Regions in the dynamic force distribution map where both force values ​​and tissue stiffness exceed preset safety thresholds are marked as high mechanical load restricted areas. The safety thresholds for contact pressure, friction force, and tissue stiffness are set to 0.5 Newtons, 0.3 Newtons, and 0.2 Newtons / mm. For each vertex of the urethral wall triangular mesh, if its contact pressure exceeds 0.5 Newtons and its tissue stiffness exceeds 0.2 Newtons / mm, the triangular facet region containing that vertex is marked as a high mechanical load region. In the 3D path search space, the mesh points corresponding to these high mechanical load regions are set as high-travel-cost points with a travel-cost value of 1000; the basic travel-cost of other reachable mesh points is set to 1.

[0080] A random path exploration algorithm is employed to iteratively generate multiple candidate paths from the starting point to the endpoint while avoiding all high-load restricted areas. Starting from the starting point, an exploration direction is randomly generated. A new path point is then extended along this direction, and it is checked whether this path point is located in a high-load restricted area or an inaccessible region. If not, the point is added to the path; if yes, the point is discarded and a new exploration direction is selected. During path extension, exploration directions with smaller angles to the endpoint are prioritized. When the path reaches the endpoint, this candidate path is recorded. This process is repeated 100 times, generating 100 different candidate paths.

[0081] The smoothest curve with the lowest overall travel cost and conforming to the physiological curvature of the urethra is selected as the safe expansion path from all candidate paths. First, the overall travel cost of each candidate path is calculated, which is the sum of the travel costs of all grid points traversed by the path. Then, the curvature of each path is calculated by evaluating the sum of the turning angles between adjacent segments. A curvature threshold of 180 degrees is set, and paths with curvature exceeding this threshold are excluded. Among the remaining paths, the path with the lowest overall travel cost is selected as the initial safe path. This path is then smoothed using a B-spline curve, and the final safe expansion path is generated through cubic B-spline interpolation, ensuring the continuity and smoothness of the path.

[0082] In the path control module, the safe expansion path is discretized into a series of dense path points, with a spacing of 0.2 mm between adjacent path points. A reference contact force is preset for each path point, calculated based on the tissue stiffness value of the corresponding urethral wall location. The mapping relationship between tissue stiffness value and reference contact force is achieved through a pre-established lookup table, which records the reference contact force values ​​corresponding to stiffness ranges from 0.1 N / mm to 1.0 N / mm. For every 0.1 N / mm increase in stiffness value, the reference contact force increases by 0.05 N, but the maximum reference contact force does not exceed 0.8 N. The reference contact force of the path points is pre-calculated and stored during the path discretization process for use in real-time control.

[0083] The system compares the current position of the expansion device with the target path point in real time, obtaining the positional deviation by calculating the straight-line distance between the two points in the three-dimensional coordinate system. Simultaneously, it compares the real-time force data with the reference contact force, calculating the difference between the two to obtain the force deviation. The calculation period for the positional deviation is 10 milliseconds, and the calculation period for the force deviation is 5 milliseconds. The positional deviation is calculated using the Euclidean distance formula, taking the square root of the sum of the squares of the differences between the current position and the target path point along the three coordinate axes. The force deviation is calculated directly from the arithmetic difference between the real-time contact force and the reference contact force.

[0084] A composite control command, comprising directional correction and force adjustment, is generated based on spatial and force deviations. The directional correction is calculated using a proportional-integral-derivative (PID) control algorithm, with a proportional gain of 0.8, an integral time constant of 0.1 seconds, and a derivative time constant of 0.05 seconds. The force adjustment is calculated using a proportional control algorithm, with a proportional gain of 0.5. When the position deviation is greater than 0.5 mm, the calculation of the directional correction takes precedence over the force adjustment; when the position deviation is less than or equal to 0.5 mm, the calculation of the force adjustment takes precedence over the directional correction. The composite control command is updated every 5 milliseconds and contains two floating-point data points, representing the directional correction angle and the force adjustment percentage, respectively.

[0085] The composite control commands are converted into drive signals to control the micro-movements of the deflection and expansion mechanisms at the end of the instrument. The direction correction angle value is converted into a corresponding voltage signal via a digital-to-analog converter, with a voltage range of ±5 volts, corresponding to a deflection of ±30 degrees for the deflection mechanism. The force adjustment percentage is converted into a square wave signal with an adjustable duty cycle via pulse width modulation to control the stepper motor of the expansion mechanism. The response time of the deflection mechanism is 20 milliseconds, and the response time of the expansion mechanism is 30 milliseconds. The drive signal update frequency is 200 Hz to ensure real-time control.

[0086] The instrument's actual pose data and real-time force data are transmitted back to the data acquisition module in real time via a high-priority data channel. The data channel uses direct memory access for transmission at a rate of 1000 data packets per second. Each data packet contains the instrument's six-DOF pose data, three-axis force data, and a timestamp. The pose data uses three floating-point numbers to represent the position coordinates and four floating-point numbers to represent the quaternion orientation. The force data uses three floating-point numbers to represent the force components in three directions. The timestamp is accurate to the millisecond and is used for subsequent data synchronization.

[0087] Upon receiving the feedback data, the data acquisition module immediately synchronizes and aligns it with the original acquired image and force signal timestamps. Synchronization is achieved by comparing the timestamps of the feedback data and the original data; data with a time difference within ±10 milliseconds are considered to belong to the same moment. For cases where timestamps do not perfectly match, linear interpolation is used to interpolate the force data to align it with the image data timestamps. The correspondence between image data and force data is established by sharing the same timestamp, ensuring that subsequent processing can accurately correlate visual and mechanical information.

[0088] Using the synchronized feedback data, relevant regions in the image data and corresponding events in the force data sequence are marked at the data acquisition end. For image data, a rectangular region centered on the instrument tip is defined in the image based on the instrument's actual spatial position; this region is one-third the width and one-quarter the height of the image. For the force data sequence, time periods with force values ​​exceeding 0.3 Newtons are marked, and the maximum force value and corresponding time within those time periods are recorded. The marking information is appended to the raw data in metadata form and stored and transmitted along with the raw data.

[0089] The labeled data is immediately fed into the environment modeling and localization process to trigger real-time updates to the 3D environment model and dynamic force distribution map. Upon receiving labeled data, the type and extent of the labels are first checked. For image-labeled regions, only the image features within that region are re-extracted and matched to update the corresponding local region of the 3D environment model. For force data labeling events, the force distribution within that time period is recalculated, and the force and stiffness values ​​of the corresponding regions in the dynamic force distribution map are updated.

[0090] In the monitoring and correction module, the actual coordinates of the instrument tip in the 3D environment model, output from the environmental modeling and positioning processing flow, are received in real time. These coordinates include positional components in three directions. The actual coordinates are compared with the expected coordinates of the safe expansion path, and the differences between the two in each coordinate axis direction are calculated. Then, the square root of the sum of the squares of these differences is taken to obtain the Euclidean spatial distance deviation. The calculation period for the spatial distance deviation is 10 milliseconds. Simultaneously, real-time contact force data from the path control processing flow is received synchronously. This data includes two components: normal contact force and tangential friction force. The real-time contact force data is compared with a preset safety force threshold, which is set at 0.8 Newtons for the normal contact force and 0.5 Newtons for the tangential friction force. The arithmetic difference between the real-time contact force and the safety force threshold is calculated.

[0091] Spatial distance deviation and contact force difference are input into a bivariate decision-maker, which contains two independent decision conditions. The first condition concerns spatial distance deviation; a path deviation flag is triggered when the spatial distance deviation exceeds the tolerance range of 0.5 mm for more than 100 milliseconds. The second condition concerns contact force difference; a force anomaly flag is triggered when the normal contact force difference increases by more than 0.3 Newtons within 50 milliseconds, or the tangential friction force difference increases by more than 0.2 Newtons within 50 milliseconds. The bivariate decision-maker uses priority logic; an anomaly is determined when either flag is triggered. After an anomaly is determined, the decision-maker generates an anomaly type code based on the triggered flag type: path deviation is coded as 1, and force anomaly is coded as 2. Simultaneously, a level code of 1-3 is generated based on the severity of the deviation or difference.

[0092] Upon detecting an anomaly, the bivariate decision maker immediately sends a trigger signal via the system interrupt request line. This interrupt request line utilizes an independent hardware channel to ensure real-time signal transmission. The trigger signal contains two bytes of data: the first byte stores the anomaly type code, and the second byte stores the level code. The transmission delay of the interrupt signal does not exceed 1 millisecond. The environment modeling and localization processing flow and the path planning processing flow simultaneously receive this trigger signal through the interrupt controller. The interrupt controller ensures that both processes immediately pause their current tasks and switch to the anomaly handling routine upon receiving the signal.

[0093] Upon receiving the trigger signal, the environmental modeling and localization processing flow immediately initiates local pose optimization based on the latest 5 frames of image data. First, visual feature points at the current moment are extracted from the image data, and these feature points are matched with feature points in the most recent 10 keyframes. Then, a local optimization window is constructed, containing the pose parameters and 3D coordinates of the feature points in the current frame and the previous 4 frames. These parameters are optimized by minimizing the reprojection error, defined as the sum of the squared distances between the feature point's 3D coordinates projected onto the image plane and its actual observed coordinates. The Levenburg-Marquardt algorithm is used for iterative optimization, with a maximum of 10 iterations, ultimately outputting the corrected current position of the instrument.

[0094] The path planning process rapidly replans a new safe expansion path based on the corrected current instrument position and the latest dynamic force distribution map. First, a search space is established in the 3D environment model, extending from the current position to the target point. The search space encompasses a 20mm radius around the current position within the urethral lumen. Then, a bidirectional fast expanding random tree algorithm is used to simultaneously generate path trees from both the current position and the target point. A path connection is considered successful when the distance between two path trees is less than 1mm. The newly generated path undergoes a smoothness check to ensure that the path curvature does not exceed the physiological curvature limit of the urethra. Finally, the path is discretized to generate a new safe expansion path composed of path points spaced 0.2mm apart.

[0095] The new safety expansion path is transmitted to the path control processing flow via shared memory. Upon receiving the new path, the path control processing flow first verifies its integrity and reachability, checking that all path points are located within the urethral lumen and do not intersect with areas of high mechanical load. If verification is successful, the system immediately switches to the new path at the start of the next control cycle. The path switching process uses a gradual transition, progressively transitioning from the original path to the new path over three consecutive control cycles, with each cycle representing a 1 / 3 transition, ensuring smooth device movement. After the path switching is complete, the system returns to normal operation and continues monitoring the device's movement.

[0096] The working principle of this invention is as follows: Image sensors and force sensing units integrated at the front end of the dilation device synchronously acquire image data of the internal urethral environment and contact force data between the device and tissue; visual SLAM technology is used to extract, match, and reconstruct the image sequence to build a three-dimensional model of the internal urethral environment and locate the device position in real time; the force data is fused with the three-dimensional model to generate a dynamic force distribution map; based on this distribution map and the current position of the device, a path search algorithm is used to plan a safe dilation path that avoids areas with high mechanical loads; a closed-loop control system dynamically adjusts the device's direction of travel and dilation force to move along the planned path, and a monitoring mechanism is used to detect path deviations and force anomalies in real time, triggering model updates and path replanning, thus achieving precise and safe navigation and dilation operations of the device within the urethra.

[0097] The foregoing has provided a detailed description of one embodiment of the present invention, but this description is merely a preferred embodiment and should not be construed as limiting the scope of the invention. All equivalent variations and modifications made within the scope of the claims of this invention should still fall within the patent coverage of this invention.

Claims

1. A urethral dilation path planning system integrating visual SLAM and force perception, characterized in that, include: The data acquisition module is used to acquire real-time image data of the internal environment of the urethra, and at the same time, acquire real-time force data of the dilator as it travels in the urethra. The environmental modeling and localization module uses real-time image data and a visual SLAM algorithm to construct a three-dimensional environmental model of the inside of the urethra and to locate the current position of the dilator in the three-dimensional environmental model in real time. The path planning module fuses real-time force data with a 3D environment model to generate a dynamic force distribution map of the urethral stricture area, and calculates a safe dilation path within the urethra based on the dynamic force distribution map and the current position of the dilator. The path control module dynamically adjusts the direction and force of the expansion device according to the safe expansion path, so that the expansion device moves along the safe expansion path, and feeds back the adjustment results to the data acquisition module in real time to update the data. The monitoring and correction module continuously monitors the changes in the 3D environment model updated by the environment modeling and positioning module and the real-time force data feedback from the path control module during the movement of the dilatation device. When a path deviation or force abnormality is detected, the environment modeling and positioning module, the path planning module and the path control module are triggered to re-execute to correct the path until the dilatation device reaches the target narrow area and completes the dilatation operation.

2. The urethral dilation path planning system integrating visual SLAM and force perception according to claim 1, characterized in that, The construction of a three-dimensional environment model of the urethra using the visual SLAM algorithm specifically includes: The acquired real-time image data is preprocessed using a hybrid filtering method that combines anisotropic diffusion and local histogram equalization to enhance the texture features of urethral wall mucosal folds while suppressing spot noise in the urine environment. An improved, illumination-invariant corner feature is extracted from preprocessed consecutive image frames. A high-dimensional descriptor is constructed by calculating its directional gradient histogram. Inter-frame feature matching is then performed to preliminarily estimate the motion of the endoscope. The successfully matched feature points are triangulated using epipolar geometric constraints to reconstruct their three-dimensional spatial points. A sliding window bundle adjustment method, optimized only for new observation areas, is then used to fuse the three-dimensional spatial points in real time, thereby incrementally generating a three-dimensional environmental model of the urethra.

3. The urethral dilation path planning system integrating visual SLAM and force perception according to claim 1, characterized in that, The current position of the real-time positioning expansion device in the three-dimensional environment model specifically includes: At least three non-collinear markers that will be highlighted under a specific narrowband spectrum are pre-set at the tip of the expansion device; Acquire real-time images containing marker points and match them with texture information in the constructed 3D environment model of the urethra to determine the initial pose; Extract the two-dimensional pixel coordinates of the highlighted marker points in the current image frame, and couple the two-dimensional coordinates with the pre-defined three-dimensional geometric relationship of the marker points; By solving the perspective lattice projection equation, the six-degree-of-freedom spatial pose of the dilator tip relative to the constructed three-dimensional environment model inside the urethra can be directly calculated, and the current position of the dilator in the three-dimensional environment model can be located in real time.

4. The urethral dilation path planning system integrating visual SLAM and force perception according to claim 1, characterized in that, The dynamic force distribution map of the region where urethral stricture is generated specifically includes: The collected multi-dimensional force signals are spatially registered and mapped onto the geometric surface of the urethral wall corresponding to the three-dimensional environment model. Discrete force measurement points are subjected to inverse distance weighting to generate a continuous force distribution surface covering the entire urethral wall; Real-time acquisition of tissue deformation data in different regions of the urethral wall, combined with a continuous force distribution surface, yields a stiffness distribution map reflecting the real-time mechanical properties of the urethral wall tissue. The stiffness distribution map is superimposed on the continuous force distribution surface to form a dynamic force distribution map.

5. A urethral dilation path planning system integrating visual SLAM and force perception according to claim 1, characterized in that, The calculation of a safe urethral dilation path based on the dynamic force distribution map and the current position of the dilator specifically includes: In the three-dimensional environment model, a three-dimensional path search space is established with the current position of the expansion device as the starting point of the path and the center of the narrow target area as the ending point. Areas in the dynamic force distribution map where both force values ​​and tissue stiffness are higher than the preset safety threshold are marked as high mechanical load no-go zones, and passage costs are set for them in the search space. While avoiding all high mechanical load restrictions, multiple candidate paths from the starting point to the end point are generated iteratively; Ultimately, the smoothest curve with the lowest overall travel cost and conforming to the physiological curvature of the urethra was selected as the safe dilation path.

6. The urethral dilation path planning system integrating visual SLAM and force perception according to claim 1, characterized in that, The method of dynamically adjusting the direction and force of the expansion device according to the safe expansion path specifically includes: The safe expansion path is discretized into a series of dense path points, and a reference contact force matching the stiffness of the urethral tissue is preset for each point. The system compares the three-dimensional spatial deviation between the current position of the expansion device and the target path point in real time, and compares the real-time force data with the reference contact force. Based on spatial deviation and force deviation, a composite control command is generated that includes directional correction and force adjustment. The composite control command is converted into a drive signal, which controls the micro-movements of the deflection mechanism and the expansion mechanism at the end of the instrument, respectively, so as to achieve coordinated dynamic adjustment of the direction of travel and the expansion force.

7. A urethral dilation path planning system integrating visual SLAM and force perception according to claim 1, characterized in that, The real-time feedback adjustment results to the data acquisition module to update the data specifically include: The actual posture data of the device and the real-time force data output by the path control module are transmitted back to the data acquisition module in real time through a high-priority data channel. After receiving the feedback data, the data acquisition module immediately timestamps and aligns it with the original acquired images and force signals. Using the synchronized feedback data, including the actual spatial position of the instrument and its interaction force with the urethral wall, the relevant areas in the image data and the corresponding events in the force data sequence are marked at the data acquisition end; The tagged data is immediately sent to the environment modeling and localization module to trigger real-time updates of the 3D environment model and dynamic force distribution map.

8. The urethral dilation path planning system integrating visual SLAM and force perception according to claim 1, characterized in that, The detected path deviation or force anomaly specifically includes: The actual coordinates of the instrument tip in the three-dimensional environment model are received in real time from the environment modeling and positioning module, and the actual coordinates are compared with the expected coordinates of the safe expansion path to calculate the Euclidean space distance deviation. It synchronously receives real-time contact force data fed back from the path control module and calculates the difference between the real-time contact force data and the preset safety force threshold. The spatial distance deviation and the contact force difference are input into a bivariate decision-maker. When the spatial distance deviation continues to exceed the tolerance range, or the contact force difference shows a sharp increase in pressure, the bivariate decision-maker determines that a path deviation or force abnormality has occurred. After determining an anomaly, the bivariate decision maker immediately generates a trigger signal containing the anomaly type and level code.

9. A urethral dilation path planning system integrating visual SLAM and force perception according to claim 1, characterized in that, The trigger environment modeling and localization module, path planning module, and path control module are re-executed to perform path correction, specifically including: The trigger signal is sent to the environment modeling and localization module and the path planning module simultaneously via the system interrupt request line. After receiving the signal, the environmental modeling and localization module immediately initiates a local pose optimization based on the latest image data and outputs the corrected current position of the instrument. Based on the corrected location and the latest dynamic force distribution map, the path planning module quickly replans a new safe expansion path from the current location to the target point; The new safe expansion path is immediately transmitted to the path control module, replacing the original path and controlling the expansion device to continue moving, thus completing a closed-loop path correction.

Images