A magnetically guided painless enema intelligent control system

Abstract

This application provides a magnetically guided painless enema intelligent control system, relating to the field of medical device technology. The system includes an environmental sensing unit, a data processing unit, a path planning unit, and a control execution unit. The environmental sensing unit acquires information through a monocular camera, IMU, and force sensor; the data processing unit integrates direct monocular SLAM with a depth estimation network to achieve real-time monocular dense mapping and uses federated filtering for multi-sensor fusion localization; the path planning unit performs real-time path planning based on the dense map and uses an improved RRT algorithm with integrated probe mobility constraints; the control execution unit generates dynamic magnetic traction for the in-body probe using an external five-axis robotic arm and electromagnetic coils, and uses an active disturbance rejection controller to achieve precise pose tracking. This application enables safe and compliant movement of the enema tube and precise drug delivery during the enema process, solving the comfort and safety issues of left-sided colonic drug delivery in patients with active enteritis, and improving patient compliance.

Description

Technical Field

[0001] This application relates to the field of medical device technology, and in particular to a magnetically guided painless enema intelligent control system. Background Technology

[0002] Enemas are a commonly used clinical method, and the classic Chinese herbal formula for healing ulcers, derived from the "Ding's Hemorrhoid Treatment Techniques" at Nanjing Municipal Hospital of Traditional Chinese Medicine, has excellent efficacy in treating active enteritis. Enema procedures are invasive, requiring insertion into the intestinal wall. In some cases of nonspecific enteritis involving the entire intestine, the enema tube tip needs to be inserted from the anus to the ileocecal junction for drug administration. Painless and precise enema administration is crucial for achieving treatment of the entire colon or left colon. Current enema techniques limit the scope and effectiveness of treating active enteritis: traditional enema devices often rely on manual control by the operator, making it difficult to accurately reach the lesion. Furthermore, inexperienced operators may use incorrect force, speed, or angle during insertion or infusion, leading to intestinal irritation or injury, causing discomfort and pain for the patient. Summary of the Invention

[0003] This application provides a magnetically guided painless enema intelligent control system. By comprehensively utilizing real-time three-dimensional mapping, sensor information fusion, path planning, and active disturbance rejection control technologies, it solves common problems in intestinal enema such as insufficient positioning accuracy, difficulty in path planning, and low accuracy in enema tube posture control, significantly improving the accuracy, safety, and comfort of enema treatment.

[0004] This application provides a magnetically guided painless enema intelligent control system, including: The environmental sensing unit is used to collect information about the intestinal environment and the motion and force information of the enema tube probe, including a monocular camera, an inertial measurement unit, and a force sensor. The data processing unit is communicatively connected to the environment perception unit and is used to receive and process the information collected by the environment perception unit to perform real-time monocular dense mapping and multi-sensor federated filtering localization. The real-time monocular dense mapping is achieved by fusing the semi-dense depth map generated by direct monocular SLAM with the dense depth map generated by the depth estimation network to generate a dense environment map for path planning. The path planning unit is communicatively connected to the data processing unit and is used to plan the real-time motion path of the enema tube probe based on the dense environment map and probe positioning information generated by the data processing unit, using an improved fast expanding random tree algorithm that integrates probe mobility constraints and path smoothing optimization strategies. The control execution unit, which is communicatively connected to the path planning unit, includes an external five-axis robotic arm and its mounted electromagnetic coil. It is used to generate dynamic magnetic traction for the enema probe in the body through the electromagnetic coil according to the planned path output by the path planning unit, and to achieve precise tracking and control of the probe's position and posture using a posture controller based on active disturbance rejection control.

[0005] Furthermore, the data processing unit includes: The direct monocular SLAM module is used to generate semi-dense depth maps and estimate camera pose. The depth estimation network module employs a weakly supervised depth estimation network with online adaptive capabilities to generate dense depth maps. The scale fitting module is used to perform linear fitting between the semi-dense depth map output by the direct monocular SLAM module and the dense depth map output by the depth estimation network module, so as to unify the depth scale. The data fusion module is used to fuse the semi-dense depth map and the dense depth map after unifying the scale to generate the dense environment map.

[0006] Furthermore, the depth estimation network module uses the previous frame image... As input, output the disparity map of the current frame. And through the disparity map With the next frame of the supervision image Calculate the loss function; the loss function includes a reconstruction error term. L recons and smoothing terms L smooth Specifically: (1) (2) In the formula, i For the nth output metric of the network, N The total number of scales, i.e., the number of image network output layers involved in the loss calculation. Let be the pixel intensity of the target image at the th scale. x Pixel coordinates represent the position of a pixel in an image. Dis i ( x Let be the disparity value at pixel position λ at the λ-th scale. Let be the pixel value of the target image at pixel coordinates at the th scale. For spatial gradient operators; The total loss value of the depth estimation network module is the reconstruction error term. L recons and smoothing terms Lsmooth Weighted sum: (3) In the formula, L This represents the total loss value.

[0007] Furthermore, the direct-method monocular SLAM module employs the LSD-SLAM algorithm to estimate camera pose by minimizing optical errors; the optical errors r ( p ) is defined as: (4) In the formula, R , t This represents the rotation and translation transformation relationship between two frames. This represents the projection function, which projects the image... I j A pixel on p According to inverse depth D ( p Projected onto image I i superior; The cost function Cos for localization t ( R , T Based on the aforementioned optical error r ( p The construction is represented as: (5) In the formula, This represents the Huber kernel function, used to increase the robustness of the cost function; The uncertainty of the system is expressed by the following formula, which is used to calculate the uncertainty based on the uncertainty propagation formula. Value: (6) In the formula, and These represent the uncertainty of the image grayscale value and the uncertainty of the inverse depth of a certain pixel, respectively.

[0008] Furthermore, the data processing unit employs a federated filtering algorithm without a reset mode to fuse information from the inertial measurement unit, the monocular camera, and the force sensor to achieve navigation and positioning of the enema tube probe, and also has the function of autonomous detection and isolation of sensor faults.

[0009] Furthermore, the improved RRT algorithm used in the path planning unit generates nodes by randomly sampling in the task space and performs heuristic search guided by the target. At the same time, it integrates the detection capability and maneuverability constraints of the enema tube probe during the path growth process and performs smooth optimization on the generated initial path to obtain a final path with shorter length and continuous curvature.

[0010] Furthermore, the pose controller based on active disturbance rejection control is designed to detect the vertical deflection angle of the enema tube probe. α To control its vertical deflection, the attitude model is as follows: (7) In the formula, The vertical deflection angle is... The deflection angular rate, For uncertainty and disturbance; Taking the first derivative of equation (7), we get: (8) In the formula, The deflection torque is the force that causes the torsional force to rotate vertically. Let be the moment of inertia of the probe about its vertical deflection axis. The angular acceleration of the enema probe around its "up and down deflection axis" is given. The rate of change of the vertical deflection angular velocity ω is expressed as a percentage of the angular acceleration. Let be the time rate of change of the uncertainty term and the disturbance term 𝑓(⋅); Based on the upper and lower deflection torques and the external magnetic permeability angle The relational expression is obtained as follows: (9) Simplifying equation (9) yields: (10) In the formula, f 1 represents the total uncertainty / total disturbance term after merging in equation (10), and g represents the control input 𝛿 Angular acceleration Control gain / input gain, ; The system equations for the pose controller are obtained as follows: (11) In the formula, x For system state variables, For state variables, y This is the system's output, specifically the actual output deflection angle of the enema probe.

[0011] Furthermore, the pose controller includes a tracking differentiator TD, an extended state observer ESO, and a nonlinear state error feedback control law NLSEF; The tracking differentiator TD is used to schedule the transition process and extract the differential signal, and its design is as follows: (12) In the formula, It is the input deflection angle command. and These are the tracking signal output by TD and its differential form, respectively, and the speed factor that determines the tracking speed. and These are the tracking output of TD and the differential form of the TD output, respectively. The filter factor; For time step; The fastest control synthesis function is expressed as shown in equation (13); It is a symbolic function, and its expression is shown in equation (14); (13) (14) In the formula, d To switch threshold parameters, r To track the velocity factor, h 0 is the filter factor. a 0 is an auxiliary intermediate variable. x 2 represents the second input quantity of fhan, indicating the velocity / differential correlation quantity. a 1 is used as an auxiliary intermediate variable to generate a continuous nonlinear structure with "fast convergence characteristics". a 2 is an auxiliary intermediate variable used to form another expression when there is a large error. a These are the key piecewise composite variables.

[0012] Furthermore, the extended state observer (ESO) is designed as follows: (15) In the formula, the system output quantity This is the actual output deflection angle of the enema probe. Input quantity It is the amount of deflection by the external magnetic force; This represents the error between the observed value and the state. Decide The degree of nonlinearity, Indicate its linear interval; The output error feedback gain parameter; It is a nonlinear error feedback function; z1 represents the state estimate of the system output 𝑦 by the ESO; z 2 represents the state estimate of the first derivative of the ESO output; z 3 represents the estimator of the extended state of ESO; These are their respective first derivatives with respect to time; b 0 represents the estimated value of the system input channel gain (control gain); The expression is as follows: (16) In the formula, fal ( e , a , ) is a nonlinear function defined according to equation (16). a It is an exponentiation.

[0013] Furthermore, the nonlinear state error feedback control law NLSEF is used to generate preliminary control quantities based on the tracking error, and its design is as follows: (18) In the formula, e 1 represents the state error of the position / angle channel. e 2 represents the state error of the speed channel. u 0 represents the error feedback control value. For a linear interval, These are the proportional and differential gains, respectively; In error feedback control quantity u Based on 0, the observed disturbance is used for compensation to obtain the control variable. u : (17)

[0014] The magnetically guided painless enema intelligent control system provided in this application has at least the following beneficial effects: 1. This application achieves real-time, dense, and high-precision 3D mapping in complex intestinal environments by integrating monocular visual SLAM with a weakly supervised depth estimation network. It solves the problems of inaccurate reconstruction of weakly textured regions and inconsistent depth map scales, providing an accurate and reliable environmental model for subsequent navigation, positioning, and path planning.

[0015] 2. This application adopts a multi-sensor information fusion scheme based on federated filtering, which integrates visual, inertial and force information. This not only significantly improves the estimation accuracy of the position and attitude of the enema tube probe, but also enhances the robustness and fault tolerance of the system through a resetless mode and intelligent fault detection and isolation mechanism, ensuring the continuity of navigation when some sensors fail.

[0016] 3. The improved RRT path planning algorithm adopted in this application integrates specific mobility constraints of the enema probe on the basis of the standard algorithm, and introduces a goal-oriented heuristic strategy and a path smoothing optimization mechanism. This enables the system to plan a safe (obstacle avoidance) and efficient (short and smooth path) movement trajectory for the probe in the tortuous and varied intestine, effectively avoiding the risk of tissue damage that may be caused by traditional manual operation.

[0017] 4. This application utilizes a pose controller based on Active Disturbance Rejection Control (ADRC) technology, which can estimate and compensate for various internal and external disturbances, such as intestinal peristalsis and tissue resistance, in real time without relying on an accurate intestinal dynamics model. Through the coordinated operation of a tracking differentiator, an expansion state observer, and nonlinear feedback, rapid, accurate, and compliant tracking control of the enema probe's pose is achieved, greatly reducing abnormal contact forces between the probe and the intestinal wall, thereby significantly improving patient comfort and achieving "painless" operation.

[0018] 5. This application reduces reliance on the operator's personal experience and manual skills through automated and intelligent operation, making the enema treatment process more standardized and controllable. Precise positioning and drug delivery capabilities allow the medication to act more accurately on the lesion area (such as the left colon or ileocecal junction), which not only improves the treatment effect for diseases such as active enteritis but also provides a feasible technical means for the treatment of a wider range of proximal intestinal lesions.

[0019] 6. This application provides a modern technological platform for the precise targeted delivery of classic Chinese medicine prescriptions into the intestine, which helps to promote the standardization and precision of traditional Chinese medicine treatment and reflects the innovative application value of artificial intelligence and digital technology in the field of modern medical devices. Attached Figure Description

[0020] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with this application and, together with the description, serve to explain the principles of this application.

[0021] Figure 1 A schematic diagram of a magnetically guided painless enema intelligent control system provided in an embodiment of this application; Figure 2 A structural block diagram of a magnetically guided painless enema intelligent control system provided in an embodiment of this application; Figure 3 A structural diagram of the data processing unit provided in the embodiments of this application; Figure 4 A block diagram of a real-time monocular dense mapping system provided in the embodiments of this application; Figure 5 A schematic diagram of the depth estimation network structure provided in the embodiments of this application; Figure 6 The standard RRT and its motion planning results are provided for the embodiments of this application; wherein, (a) is the standard RRT; and (b) is the motion planning results. Figure 7 A schematic diagram of real-time intestinal environment path planning based on RRT provided for embodiments of this application; Figure 8 This is a structural block diagram of a pose controller based on active disturbance rejection control provided in an embodiment of this application.

[0022] Explanation of reference numerals in the attached figures: 100. Environmental sensing unit; 101. Monocular camera; 102. Inertial measurement unit; 103. Force sensor; 200. Data Processing Unit; 201. Direct Method Monocular SLAM Module; 202. Depth Estimation Network Module; 203. Scale Fitting Module; 204. Data Fusion Module; 300. Path planning unit; 400. Control and execution unit; 401. Robotic arm (external five-axis robotic arm); 402. Electromagnet; 4021. Electromagnetic coil; 4022. External magnet; 500. Power supply system; 600. Display terminal; 700. Syringe; 800. Enema tube probe; 801. First catheter; 802. Second catheter; 803. Endoscope components; 804. Magnet; 805. Liquid outlet.

[0023] The accompanying drawings illustrate specific embodiments of this application, which will be described in more detail below. These drawings and descriptions are not intended to limit the scope of the concept in any way, but rather to illustrate the concepts of this application to those skilled in the art through reference to specific embodiments. Detailed Implementation

[0024] Exemplary embodiments will now be described in detail, examples of which are illustrated in the accompanying drawings. When the following description relates to the drawings, unless otherwise indicated, the same numbers in different drawings denote the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with this application. Rather, they are merely examples of apparatuses and methods consistent with some aspects of this application as detailed in the appended claims.

[0025] The collection, storage, use, processing, transmission, provision, and disclosure of financial data or user data involved in the technical solution of this application all comply with the provisions of relevant laws and regulations and do not violate public order and good morals.

[0026] It should be noted that in the embodiments of this application, certain software, components, models and other existing solutions in the industry may be mentioned. These should be regarded as exemplary and are only intended to illustrate the feasibility of implementing the technical solution of this application. However, it does not mean that the applicant has used or necessarily used the solution.

[0027] The technical solution of this application and how the technical solution of this application solves the above-mentioned technical problems are described in detail below with specific embodiments. These specific embodiments can be combined with each other, and the same or similar concepts or processes may not be described again in some embodiments. The embodiments of this application will now be described with reference to the accompanying drawings.

[0028] This application provides a magnetically guided painless enema intelligent control system. Through multiple sensors and control algorithms, it achieves safe and smooth movement of the enema tube and precise drug delivery during the enema process, addressing comfort and safety issues for patients with active enteritis during examinations, meeting the needs of patients with proximal intestinal lesions, and demonstrating innovative applications combining artificial intelligence and digitalization. Figure 1 As shown, the magnetically guided painless enema intelligent control system includes an environmental sensing unit 100, a data processing unit 200, a path planning unit 300, and a control execution unit 400. The environmental sensing unit 100 is used to collect information about the intestinal environment and the motion and force information of the enema tube probe, including a monocular camera 101, an inertial measurement unit 102, and a force sensor 103. The data processing unit 200 is communicatively connected to the environmental sensing unit 100 and is used to receive and process the information collected by the environmental sensing unit 100 to perform real-time monocular dense mapping and multi-sensor federated filtering localization. Real-time monocular dense mapping is achieved by fusing a semi-dense depth map generated by direct monocular SLAM with a dense depth map generated by a depth estimation network to generate a dense environmental map for path planning. The path planning unit 300... The processing unit 200 is communicatively connected to the data processing unit 200 and is used to plan the real-time motion path of the enema tube probe based on the dense environment map and probe positioning information generated by the data processing unit 200. It employs an improved fast expanding random tree algorithm that integrates probe mobility constraints and path smoothing optimization strategies. The control execution unit 400 is communicatively connected to the path planning unit 300 and includes an external five-axis robotic arm and its mounted electromagnetic coil. It is used to generate dynamic magnetic traction of the enema probe in the body through the electromagnetic coil according to the planned path output by the path planning unit 300, and uses a pose controller based on active disturbance rejection control to achieve precise tracking and control of the probe pose.

[0029] In one exemplary embodiment, such as Figure 2As shown, the magnetically guided painless enema intelligent control system, in its specific operation, is equipped with a power supply system 500, a display terminal 600, and a syringe 700. The enema tube probe 800 includes a first catheter 801, a second catheter 802, an endoscope component 803, and a magnet 804. One end of the first catheter 801 is connected to the syringe 700, and the other end is connected to the endoscope component 803. The endoscope component 803 has a liquid outlet 805, through which the liquid injected by the syringe 700 flows out. The endoscope component 803 is equipped with a monocular camera 101 and an inertial measurement unit 102. The connecting wires of the monocular camera 101 and the inertial measurement unit 102 are connected to the display terminal 600 via the second catheter 802 to display relevant information. The display terminal 600 can be implemented as a computer. The control execution unit 400 includes a robotic arm 401 (external five-axis robotic arm) and an electromagnet 402. The electromagnet 402 includes an electromagnetic coil 4021 and an external magnet 4022 disposed inside the electromagnetic coil 4021. The robotic arm 401 drives the electromagnet 402 to operate, and the electromagnet 402 generates magnetic lines of force with N and S poles, thereby attracting the magnet 804 inside the body. The monocular camera 101 mounted on the endoscope component 803 collects images of the intestinal environment, and the inertial measurement unit 102 monitors the probe's position and orientation information. The syringe 700 delivers the drug solution through the liquid outlet 805 connected to the first catheter 801. The display terminal 600 displays the real-time operating status of the system and various data collected. At the same time, the force sensor 103, which measures in kg, monitors the traction force in real time to ensure that the traction force is appropriate during the enema process and avoid intestinal damage, ultimately achieving painless and precise drug delivery.

[0030] In some embodiments, such as Figure 3 As shown, the data processing unit 200 includes: Direct monocular SLAM module 201 is used to generate semi-dense depth maps and estimate camera pose. The depth estimation network module 202 employs a weakly supervised depth estimation network with online adaptive capabilities to generate dense depth maps; The scale fitting module 203 is used to perform linear fitting between the semi-dense depth map output by the direct monocular SLAM module 201 and the dense depth map output by the depth estimation network module, so as to unify the depth scale. The data fusion module 204 is used to fuse the semi-dense depth map and the dense depth map after unification to generate a dense environment map.

[0031] Specifically, such as Figure 4The diagram shown is a block diagram of the real-time monocular dense mapping system provided in this application embodiment. This system employs monocular visual SLAM technology combined with a weakly supervised depth estimation network to perform real-time 3D mapping in complex intestinal environments. By fusing the depth estimation network and SLAM technology, it overcomes the inaccuracy of traditional visual SLAM in reconstructing weakly textured regions, providing an accurate and dense 3D map of the intestinal environment. This mapping method uses joint CPU and GPU processing, and by fusing depth data output from depth estimation and SLAM, it solves the problem of inconsistent depth map scales, thereby providing high-precision environmental modeling and ensuring accurate navigation of the enema tube probe. The real-time monocular dense mapping system mainly includes four modules: direct monocular SLAM, a depth estimation network with online adaptive capabilities, a scale fitting module, and a data fusion module.

[0032] Visual SLAM runs two threads on the CPU: localization and mapping. The localization thread estimates the camera pose of each frame in real time and defines keyframes; the mapping thread calculates the depth value of each keyframe, mainly including depth information from high-gradient points. Unlike feature-point SLAM, this embodiment uses direct SLAM because the point cloud generated by feature-point SLAM is relatively sparse, while direct SLAM provides a semi-dense point cloud, which can be better integrated into the final depth map. The depth estimation network module runs on the GPU. A training sample selection algorithm is designed to save images that meet the requirements into a sample pool, supporting online updates of the neural network. When the sample pool reaches a certain size, the system uses this data to update the network parameters, continuously performing data selection and depth estimation during this process. After the network parameters are updated, the system clears the sample pool and enters the next round of updates. The scale fitting module linearly fits the dense depth map output by the depth estimation network with the semi-dense depth map generated by SLAM, calculates the scale relationship between the two, and applies it to the camera pose and the SLAM depth map. The data fusion section combines the semi-dense depth map output by SLAM with the dense depth map predicted by the network. By evaluating the depth accuracy of high gradient points, a more accurate depth map is ultimately generated.

[0033] In some embodiments, the depth estimation network module 202 uses the previous frame image. As input, output the disparity map of the current frame. And through disparity maps With the next frame of the supervision image Calculate the loss function; the loss function includes the reconstruction error term. L recons and smoothing terms L smooth .

[0034] Specifically, the depth estimation network module 202 uses ResNet-50 as the base network for feature extraction, downsampling the image to 1 / 32 of the original. Then, five deconvolutional layers upsample the image to restore it to its original size. Figure 1 The image size is specified. Two skip connections are used to supplement the image's detail information. The network does not directly output a depth map, but instead outputs a disparity map, which is then converted into a depth map. Assume the previous input frame is... The output disparity map is The supervised image is The network's loss function consists of reconstruction error and a smoothing term, where the reconstruction error and smoothing term are defined as follows: (1) (2) In the formula, i For the nth output metric of the network, N The total number of scales, i.e., the number of image network output layers involved in the loss calculation. Let be the pixel intensity of the target image at the th scale. x Pixel coordinates represent the position of a pixel in an image. Dis i ( x Let be the disparity value at pixel position λ at the λ-th scale. Let be the pixel value of the target image at pixel coordinates at the th scale. For spatial gradient operators; The final loss value of the network is the weighted sum of the two terms: (3) In the formula, L This represents the total loss value.

[0035] For example, such as Figure 5The diagram shown is a schematic of the depth estimation network structure provided in this embodiment. This network uses ResNet-50 as its basic feature extraction network and consists of three core structures: an encoding layer, a decoding layer, and skip connections. The encoding layer includes ResNet-50's inherent bottleneck modules such as conv1, conv2_x, conv3_x, conv4_x, and conv5_x, responsible for downsampling the input RGB image layer by layer, compressing the image scale to 1 / 32 of the original image, and extracting multi-scale, high-dimensional image features. The decoding layer consists of five deconvolutional layers, upconv0 to upconv4, used to progressively upsample the low-resolution feature map output by the encoding layer, ultimately restoring it to the same size as the input RGB image. The skip connections span the encoding and decoding layers, directly transferring the detailed features extracted at different stages of the encoding process to the corresponding stages of the decoding layer, effectively compensating for the image texture and edge information lost during the upsampling process. Its working principle is as follows: The input RGB image is first fed into the encoding layer, where it undergoes convolution, batch normalization, and activation operations sequentially through the bottleneck module of ResNet-50 to achieve feature depth extraction and dimensionality enhancement, while simultaneously downsampling the image. The encoded feature map then enters the decoding layer, where the feature map size is gradually expanded through deconvolution layers. Each deconvolution operation combines the same-scale encoded features passed in via skip connections, supplementing detailed information and reducing the semantic gap through feature fusion. The network does not directly output a depth map, but instead outputs a disparity map (Dis) through the activation function of the last layer of the decoding layer. Based on the geometric mapping relationship between disparity and depth, the disparity map is then transformed into the final required depth map. During training, the network uses the weighted sum of reconstruction error and smoothing term as the loss function, and iteratively optimizes the network parameters using the gradient descent algorithm. The reconstruction error constrains the depth information after the disparity map transformation to be consistent with the real scene, while the smoothing term ensures the continuity of the depth map, ultimately achieving dense and accurate depth estimation, providing reliable data support for 3D intestinal mapping.

[0036] In some embodiments, the direct monocular SLAM module 201 employs the LSD-SLAM algorithm to estimate camera pose by minimizing optical errors. LSD-SLAM mapping offers the advantage of high accuracy, but its disadvantage is that the generated map is semi-dense, containing only object outlines and edge information. LSD-SLAM estimates camera pose in real-time through a localization thread and transforms it into pose in the world coordinate system; the mapping thread calculates pixel depth information through multi-view stereo matching. Its core principle is to calculate inverse depth values ​​through epipolar search and fuse them according to uncertainty descriptions to improve depth accuracy. LSD-SLAM calculates the uncertainty of inverse depth in three steps: calculating epipolar positions, finding pixel matching points, and calculating inverse depth based on disparity. Finally, the observed inverse depth and the prior inverse depth are fused to obtain a more accurate depth value. Furthermore, LSD-SLAM defines a localization cost function based on optical errors, combines it with uncertainty propagation formulas to optimize depth accuracy, and enhance system robustness.

[0037] In this embodiment, optical error r ( p ) is defined as: (4) In the formula, R , t This represents the rotation and translation transformation relationship between two frames. This represents the projection function, which projects the image... I j A pixel on p According to inverse depth D ( p Projected onto image I i superior; The cost function Cos for localization t ( R , T Based on optical errors r ( p The construction is represented as: (5) In the formula, This represents the Huber kernel function, used to increase the robustness of the cost function; The uncertainty of the system is expressed by the following formula, which is used to calculate the uncertainty based on the uncertainty propagation formula. Value: (6) In the formula, and These represent the uncertainty of the image grayscale value and the uncertainty of the inverse depth of a certain pixel, respectively.

[0038] In some embodiments, the data processing unit 200 employs a federated filtering algorithm without a reset mode to fuse information from the inertial measurement unit, monocular camera, and force sensor to achieve navigation and positioning of the enema tube probe, and has the function of autonomous detection and isolation of sensor faults.

[0039] To ensure accurate positioning and navigation of the enema tube in the complex intestinal environment, this embodiment designs a data fusion scheme based on an inertial measurement unit (IMU), a visual sensor, and a force sensor. Through an adaptive variable structure scheme and a multi-sensor information fusion method, the system can accurately estimate the position and orientation of the enema tube in real-time, overcoming the problem of single sensors being susceptible to noise and external disturbances, and ensuring rapid response and high-precision positioning during the enema process.

[0040] Multi-information fusion integrated navigation systems employ federated filtering for navigation error estimation, which necessitates establishing a linear dynamic equation set for the navigation system, namely the state equation and the measurement equation, while also satisfying the conditions for applying the Kalman filter.

[0041] In multi-information fusion integrated navigation systems, the validity of measurement information processed by each sub-filter must be determined in real time to decide which local state estimates to use to calculate the overall state estimate. The multi-information fusion integrated navigation system should be able to automatically detect sensor faults, isolate faulty sensors, reassemble the remaining intact sensors, and continue navigation, maintaining the continuity and stability of navigation parameter output.

[0042] Therefore, the design of filters in multi-information fusion integrated navigation systems should be equipped with corresponding fault detection and isolation algorithms. Once a fault is detected, it must be isolated, and finally, system information reconstruction should be performed to prevent the entire system from failing due to the fault. To ensure satisfactory fault tolerance of the multi-information fusion integrated navigation system, this embodiment adopts a reset-free mode federated filtering implementation scheme. The reset-free mode has strong fault tolerance and fault detection capabilities, good real-time performance, and the simplest control. Each sensor can work independently, avoiding cross-contamination of sub-filters caused by the failure of one sensor.

[0043] In some embodiments, the path planning unit 300 employs an improved RRT algorithm, which generates nodes by randomly sampling in the task space and performs a heuristic search guided by the target. At the same time, it integrates the detection capability and maneuverability constraints of the enema tube probe during the path growth process and performs smooth optimization on the generated initial path to obtain a final path with shorter length and continuous curvature.

[0044] Specifically, this embodiment employs an improved Rapid Expanding Random Tree (RRT) algorithm for enema tube path planning. The improved RRT algorithm not only considers the complexity of the intestine but also incorporates enema tube performance constraints, such as detection and maneuverability, and designs a goal-oriented heuristic path planning strategy. Through dynamic real-time motion planning, the system can generate the optimal path in real time based on the intestinal environment and the real-time state of the enema tube, optimizing path smoothness and length. This effectively avoids potential obstacles during the enema process and ensures the smooth progress of the treatment.

[0045] RRT directs the search and node expansion toward unknown regions by randomly sampling in the task space G. Figure 7 The basic principle of RRT growth is given. When expanding a new node, firstly, a point xrand is randomly selected in G. Then, the node xnear that is closest to xrand in the current RRT is searched. Finally, a new node xnew and the path Trnear,new between the two nodes are grown according to a fixed or random step size. During the growth of the tree, it is determined whether the newly expanded node xnew has reached the target point or target region. Once it has reached it, the search backtracks from the node to xinit (the starting point), thus obtaining a complete path composed of several control inputs and reference waypoints.

[0046] like Figure 6 The figure shown is a diagram of the standard RRT and its motion planning results provided in an embodiment of this application. Figure 6 Both subgraphs use the x-axis to represent the horizontal spatial coordinates of the intestinal environment and the y-axis to represent the vertical spatial coordinates (unit: km), presenting the motion planning output path of the standard RRT algorithm in a simulated intestinal space. From Figure 6 As can be seen, the path generated by the standard RRT algorithm through random sampling of extended nodes has obvious abrupt turns and node redundancy problems. Furthermore, it does not take into account the constraints of the detection and mobility of the enema tube probe, resulting in insufficient path smoothness. It is difficult to adapt to the complex physiological structure of the intestine, which is curved and narrow, and it is easy for the probe to collide with the intestinal wall or irritate the intestinal tissue.

[0047] Figure 7 This is a schematic diagram of real-time path planning in the intestinal environment based on the improved RRT algorithm of this application. Its coordinate system is the same as... Figure 6To maintain consistency, the initial position of the enema tube probe (probe 91000017), the target lesion area, and multiple "replanning" markers are clearly marked in the figure. Combining the 3D intestinal environment map output by real-time monocular dense mapping technology with the positioning information fused from multiple sensors, the improved RRT algorithm first processes the intestinal space through gridding, introducing probe mobility constraints during path search, such as bending angle limits and movement speed thresholds, to avoid generating path segments that exceed the probe's movement capabilities. At the same time, a goal-oriented heuristic strategy guides nodes to converge towards the lesion area, reducing invalid sampling and redundant nodes, significantly improving planning efficiency. To address potential interference in the intestinal environment, such as spatial changes caused by intestinal peristalsis, the algorithm supports real-time replanning, dynamically adjusting the smoothness and length of the path by continuously receiving updated data from the environmental sensing unit, ensuring that the planned path always conforms to the intestinal contour and remains continuous and smooth.

[0048] from Figure 7 The planning results show that the improved path has no obvious abrupt turns, and the path nodes are evenly distributed. It can accurately avoid potential obstacles in the intestine, such as intestinal folds and lesion protrusions. Moreover, the path's adaptability to the dynamic intestinal environment is ensured through multiple real-time replanning. This path planning result, together with the magnetic traction function of the external five-axis robotic arm and the self-disturbance-resistant robust adaptive control technology of the control execution unit, enables the enema tube probe to move safely and smoothly along the planned path, accurately reaching the target lesion area to complete drug delivery. This effectively solves the problems of inaccurate and poor adaptability of traditional enema path planning, and fully ensures the accuracy, safety, and patient comfort of enema treatment.

[0049] In some embodiments, such as Figure 8 As shown, the pose controller based on active disturbance rejection control targets the vertical deflection angle of the enema tube probe. α The pose controller is controlled by a tracking differentiator (TD), an extended state observer (ESO), and a nonlinear state error feedback control law (NLSEF).

[0050] Due to the complexity of the intestinal environment and the inability to obtain an accurate dynamic model of the enema tube probe, this embodiment introduces Active Disturbance Rejection Control (ADRC) technology to achieve precise pose control of the enema tube. Through an ADRC controller and a nonlinear feedback control law, this invention can accurately track the enema tube under the influence of external magnetic force. The introduction of ADRC significantly improves the system's resistance to disturbances, enabling the enema tube to achieve precise positioning and path tracking even without an accurate dynamic model, thereby ensuring the accuracy and safety of the enema process.

[0051] Compared to other control methods, Active Disturbance Rejection Control (ADRC) has low dependence on the system model, can estimate the "total disturbance" of the system and provide real-time feedback, and can compensate for the feedback signal to achieve overall disturbance reduction. It is suitable for engineering control problems of nonlinear systems with multiple disturbances.

[0052] Based on the theoretical research of the Trace Differentiator (TD), Extended State Observer (ESO), and Nonlinear State Error Feedback (NLSEF) modules in the ADRC structure, an active disturbance rejection attitude controller and position controller for an enema probe are designed. Taking the attitude controller that realizes the vertical deflection of the enema probe as an example, the specific implementation steps are explained in detail.

[0053] Step 1: State transition.

[0054] During the process of the enema probe moving in and out of the intestine, the problem of probe attitude control is mainly achieved by controlling the up / down / left / right deflection angle of the probe through magnetic force. ADRC takes the numerous internal or external disturbances affecting the model as the sum of disturbances, makes full use of the characteristic of ADRC not relying on the system model, estimates the sum of disturbances of the system in real time through ESO, and performs feedback and compensation through NLSEF.

[0055] The attitude deflection angle of the enema probe is used as the main variable of the attitude controller, and its vertical deflection attitude model is as follows: (7) In the formula, The vertical deflection angle is... The deflection angular rate, This is due to uncertainty and disturbance.

[0056] Step 2: Determine the total perturbation.

[0057] Taking the first derivative of equation (7), we get: (8) In the formula, The deflection torque is the force that causes the torsional force to rotate vertically. Let be the moment of inertia of the probe about its vertical deflection axis. The angular acceleration of the enema probe around its "up and down deflection axis" is given. The rate of change of the vertical deflection angular velocity ω is expressed as a percentage of the angular acceleration. Let be the time rate of change of the uncertainty term and the disturbance term 𝑓(⋅).

[0058] Based on the up and down deflection torque and the angle of the large / small dial. The relational expression can be obtained as follows: (9) In the formula, This is the constant / offset term (intercept term) in the relationship between the upper and lower deflection torques and α. This is the coefficient / gain of the up and down deflection torque with respect to the dial angle n.

[0059] make The first term in the formula, but: (10) In the formula, f 1 represents the total uncertainty / total disturbance term after merging in equation (10), and g represents the control input n to angular acceleration. Control gain / input gain, ; Step 3: Summarize the system equations.

[0060] Based on the above analysis, the system equation for the enema probe's active disturbance rejection attitude controller can be obtained as follows: (11) In the formula, x For system state variables, For state variables, y This is the system's output, specifically the actual output deflection angle of the enema probe.

[0061] Step 4: ADRC design.

[0062] The design structure of the enema probe's self-disturbance rejection attitude controller is as follows: Figure 6 As shown, the equations and functions of each step are designed in detail.

[0063] (1) Tracking Differentiator (12) In the formula, It is the input deflection angle command. and These are the tracking signal output by TD and its differential form, respectively, and the speed factor that determines the tracking speed. and These are the tracking output of TD and the differential form of the TD output, respectively. The filter factor; The time step is expressed in seconds (s). The fastest control synthesis function is expressed as shown in equation (13); For symbolic functions, their expression is shown in equation (14): (13) (14) In the formula, d To switch threshold parameters, r To track the velocity factor, h 0 is the filter factor. a 0 is an auxiliary intermediate variable. x 2 represents the second input quantity of fhan, indicating the velocity / differential correlation quantity. a 1 is used as an auxiliary intermediate variable to generate a continuous nonlinear structure with "fast convergence characteristics". a 2 is an auxiliary intermediate variable used to form another expression when there is a large error. a These are the key piecewise composite variables.

[0064] (2) Extended State Observer (15) In the formula, the system output quantity This is the actual output deflection angle of the enema probe. Input quantity It is the amount of deflection caused by the external magnetic force. This is the error between the observed value and the state. Decide The degree of nonlinearity, It represents its linear interval. This is the output error feedback gain parameter. The expression for is shown in equation (16). It is a nonlinear error feedback function; z 1 represents the state estimate of the system output 𝑦 by the ESO; z 2 represents the state estimate of the first derivative of the ESO output; z 3 represents the estimator of the extended state of ESO; These are their respective first derivatives with respect to time; b 0 represents the estimated value of the system input channel gain (control gain).

[0065] (16) In the formula, fal ( e , a , ) is a nonlinear function defined according to equation (16). a It is an exponentiation.

[0066] (3) Generation of control quantities In error feedback control quantity Based on this, the observed disturbance is used for compensation, and the control quantity is shown in equation (17): (17) (4) Nonlinear state control law (18) In the formula, e 1 represents the state error of the position / angle channel. e 2 represents the state error of the speed channel. Similar to a proportional element, This is equivalent to a differential element. These are the proportional and differential gains, respectively. It is a linear interval.

[0067] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features therein. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of this application.

Claims

1. A magnetically guided painless enema intelligent control system, characterized in that, include: The environmental sensing unit is used to collect information about the intestinal environment and the motion and force information of the enema tube probe, including a monocular camera, an inertial measurement unit, and a force sensor. The data processing unit is communicatively connected to the environmental sensing unit and is used to receive and process the information collected by the environmental sensing unit in order to perform real-time monocular dense mapping and multi-sensor federated filtering localization. The real-time monocular dense mapping is achieved by fusing the semi-dense depth map generated by direct monocular SLAM with the dense depth map generated by the depth estimation network to generate a dense environment map for path planning. The path planning unit is communicatively connected to the data processing unit and is used to plan the real-time motion path of the enema tube probe based on the dense environment map and probe positioning information generated by the data processing unit, using an improved fast expanding random tree algorithm that integrates probe mobility constraints and path smoothing optimization strategies. The control execution unit, which is communicatively connected to the path planning unit, includes an external five-axis robotic arm and its mounted electromagnetic coil. It is used to generate dynamic magnetic traction for the enema probe in the body through the electromagnetic coil according to the planned path output by the path planning unit, and to achieve precise tracking and control of the probe's position and posture using a posture controller based on active disturbance rejection control.

2. The magnetically guided painless enema intelligent control system according to claim 1, characterized in that, The data processing unit includes: The direct monocular SLAM module is used to generate semi-dense depth maps and estimate camera pose. The depth estimation network module employs a weakly supervised depth estimation network with online adaptive capabilities to generate dense depth maps. The scale fitting module is used to perform linear fitting between the semi-dense depth map output by the direct monocular SLAM module and the dense depth map output by the depth estimation network module, so as to unify the depth scale. The data fusion module is used to fuse the semi-dense depth map and the dense depth map after unifying the scale to generate the dense environment map.

3. The magnetically guided painless enema intelligent control system according to claim 2, characterized in that, The depth estimation network module is based on the previous frame image. As input, output the disparity map of the current frame. And through the disparity map With the next frame of the supervision image Calculate the loss function; the loss function includes a reconstruction error term. L recons and smoothing terms L smooth Specifically: （1） （2） In the formula, i For the nth output metric of the network, N The total number of scales, i.e., the number of image network output layers involved in the loss calculation. Let be the pixel intensity of the target image at the th scale. x Pixel coordinates represent the position of a pixel in an image. Dis i ( x Let be the disparity value at pixel position λ at the λ-th scale. Let be the pixel value of the target image at pixel coordinates at the th scale. For spatial gradient operators; The total loss value of the depth estimation network module is the reconstruction error term. L recons and smoothing terms L smooth Weighted sum: （3） In the formula, L This represents the total loss value.

4. The magnetically guided painless enema intelligent control system according to claim 2, characterized in that, The direct-method monocular SLAM module employs the LSD-SLAM algorithm to estimate camera pose by minimizing optical errors; the optical errors... r ( p ) is defined as: （4） In the formula, R , t This represents the rotation and translation transformation relationship between two frames. This represents the projection function, which projects the image... I j A pixel on p According to inverse depth D ( p Projected onto image I i superior; The cost function Cos for localization t ( R , T Based on the aforementioned optical error r ( p The construction is represented as: （5） In the formula, This represents the Huber kernel function, used to increase the robustness of the cost function; The uncertainty of the system is expressed by the following formula, which is used to calculate the uncertainty based on the uncertainty propagation formula. Value: （6） In the formula, and These represent the uncertainty of the image grayscale value and the uncertainty of the inverse depth of a certain pixel, respectively.

5. The magnetically guided painless enema intelligent control system according to claim 1, characterized in that, The data processing unit employs a federated filtering algorithm without a reset mode to fuse information from the inertial measurement unit, the monocular camera, and the force sensor to achieve navigation and positioning of the enema tube probe, and also has the function of autonomous detection and isolation of sensor faults.

6. The magnetically guided painless enema intelligent control system according to claim 1, characterized in that, The improved RRT algorithm used in the path planning unit generates nodes by randomly sampling in the task space and performs heuristic search guided by the target. At the same time, it integrates the detection capability and maneuverability constraints of the enema tube probe during the path growth process and performs smooth optimization on the generated initial path to obtain a final path with shorter length and continuous curvature.

7. The magnetically guided painless enema intelligent control system according to claim 1, characterized in that, The pose controller based on active disturbance rejection control is designed to control the vertical deflection angle of the enema tube probe. α To control its vertical deflection, the attitude model is as follows: （7） In the formula, The vertical deflection angle is... The deflection angular rate, For uncertainty and disturbance; Taking the first derivative of equation (7), we get: （8） In the formula, The deflection torque is the force that causes the torsional force to rotate vertically. Let be the moment of inertia of the probe about its vertical deflection axis. The angular acceleration of the enema probe about its vertical deflection axis. The rate of change of the vertical deflection angular velocity ω is expressed as a percentage of the angular acceleration. Let be the time rate of change of the uncertainty term and the disturbance term 𝑓(⋅); Based on the relationship between the vertical deflection torque and the external magnetic permeability angle n, we obtain: （9） In the formula, This refers to the constant / bias term in the relationship between the upper and lower deflection torques and φ. This is the coefficient / gain of the up and down deflection torque with respect to the dial angle n; Simplifying equation (9) yields: （10） In the formula, f 1 represents the total uncertainty / total disturbance term after merging in equation (10), and g represents the control input n. Angular acceleration Control gain / input gain, ; The system equations for the pose controller are obtained as follows: （11） In the formula, x For system state variables, For state variables, y This is the system's output, specifically the actual output deflection angle of the enema probe.

8. The magnetically guided painless enema intelligent control system according to claim 7, characterized in that, The pose controller includes a tracking differentiator TD, an extended state observer ESO, and a nonlinear state error feedback control law NLSEF; The tracking differentiator TD is used to schedule the transition process and extract the differential signal, and its design is as follows: （12） In the formula, It is the input deflection angle command. and These are the tracking signal output by TD and its differential form, respectively, and the speed factor that determines the tracking speed. and These are the tracking output of TD and the differential form of the TD output, respectively. The filter factor; For time step; The fastest control synthesis function is expressed as shown in equation (13); It is a symbolic function, and its expression is shown in equation (14); （13） （14） In the formula, d To switch threshold parameters, r To track the velocity factor, h 0 is the filter factor. a 0 is an auxiliary intermediate variable. x 2 represents the second input quantity of fhan, indicating the velocity / differential correlation quantity. a 1 is an auxiliary intermediate variable. a 2 is an auxiliary intermediate variable used to form another expression when there is a large error. a These are the key piecewise composite variables.

9. The magnetically guided painless enema intelligent control system according to claim 8, characterized in that, The extended state observer (ESO) is designed as follows: （15） In the formula, the system output quantity This is the actual output deflection angle of the enema probe. Input quantity It is the amount of deflection by the external magnetic force; This represents the error between the observed value and the state. Decide The degree of nonlinearity, Indicate its linear interval; The output error feedback gain parameter; It is a nonlinear error feedback function; z 1 represents the state estimate of the system output 𝑦 by the ESO; z 2 represents the state estimate of the first derivative of the ESO output; z 3 represents the estimator of the extended state of ESO; These are their respective first derivatives with respect to time; b 0 represents the estimated value of the system input channel gain; The expression is as follows: （16） In the formula, fal ( e , a , ) is a nonlinear function defined according to equation (16). a It is an exponentiation.

10. The magnetically guided painless enema intelligent control system according to claim 9, characterized in that, The nonlinear state error feedback control law NLSEF is used to generate preliminary control quantities based on the tracking error, and its design is as follows: （18） In the formula, e 1 represents the state error of the position / angle channel. e 2 represents the state error of the speed channel. u 0 represents the error feedback control value. For a linear interval, These are the proportional and differential gains, respectively; In error feedback control quantity u Based on 0, the observed disturbance is used for compensation to obtain the control variable. u : （17）。

Images