A Novel Relative Position Estimation Method for Capsule Robot Moving in Gastrointestinal Tract

Recently, a variety of positioning and tracking methods have been proposed for capsule robots moving in the gastrointestinal (GI) tract to provide real-time unobstructed spatial pose results. However, the current absolute position-based result cannot match the GI structure due to its unstructured environment. To overcome this disadvantage and provide a proper position description method to match the GI tract, we here present a relative position estimation method for tracking the capsule robot, which uses the moving distance of the robot along the GI tract to indicate the position result. The procedure of the proposed method is as follows: firstly, the absolute position results of the capsule robot are obtained with the magnetic tracking method; then, the moving status of the robot along the GI tract is determined according to the moving direction; and finally, the movement trajectory of the capsule robot is fitted with the Bézier curve, where the moving distance can then be evaluated using the integral method. Compared to state-of-the-art capsule tracking methods, the proposed method can directly help to guide medical instruments by providing physicians the insertion distance in patients’ bodies, which cannot be done based on absolute position results. Moreover, as relative distance information was used, no reference tracking objects needed to be mounted onto the human body. The experimental results prove that the proposed method achieves a good distance estimation of the capsule robot moving in the simulation platform.


Introduction
According to a report of the World Health Organization (WHO), cancer is considered as the leading cause of death worldwide [1], accounting for the deaths of nearly 10 million people every year. In terms of mortality, colorectal cancer (CRC) is the third highest among all cancers, and the number of deaths were about 774,000 in 2015. Gastrointestinal (GI) diseases, especially in the small intestine and colon, are extremely harmful to the public's health. However, if it can be detected in its early stages and treated promptly, the survival rate could increase up to 94% [2].
Conventional endoscopes with cables is the most common tool used in examinations, but often bring pain to the patient during the examination. Moreover, this process requires narcotic drugs, which may be risky to the patient. In recent years, robot-assisted colonoscopy and the use of micro-robots in the GI tract have made significant progress [3][4][5]. Capsule robots, as a smart micro-tool, can enter the human GI tract for use in medical exploration and treatment. It causes minimal stimulation and damage to the human body, and is a new breakthrough in therapeutic medical technology and interventional therapy. After it is swallowed and enters into the GI track, it can photograph organs such as the oesophagus, stomach, small intestine, and large intestine, and thus help doctors in the diagnosis of patients [6][7][8]. Compared to traditional endoscopes, wireless capsule endoscopes offer a non-invasive solution, has broad application prospects, and is able to avoid the occurrence of pain. However, problems still exist in that it is hard to provide the position information of the capsule corresponding to each endoscopic image and feedback which the capsule poses parameters for in further treatment operations. In recent years, scholars have done much research and proposed a variety of localization methods [9,10]. The radio frequency signal-based localization method is one of the most common methods used [11][12][13], which consists of a reader and a tag, and requires positioning targets with tags and antennae. It is not suitable for narrow environments, such as in the gastrointestinal tract. Ultrasound-based methods [14,15] have been widely used in the biomedical area, but their accuracy is limited due to speed-of-sound variations in human tissue. Medical imaging methods [16,17] have used images from the endoscope to do localizations, but they depend on the accuracy of the sample before the test, and the matching of image features is difficult to achieve. In addition, nuclear medicine imaging technology, such as CTs and MRIs, can also be used for in vivo positioning, but long-term exposure is not conducive to human health.
The magnetic localization method [18][19][20][21] was also considered as preferred technology. Here, a permanent magnet is mounted onto the capsule robot, and its position can be obtained by sensing the magnetic field of the magnet. Ref. [22] employed a novel probe to perceive the position of magnetic materials. Simple facilities and a low-cost algorithm were adapted in this approach. In addition to medical applications, magnetic positioning technology has been widely used in other fields in recent years, such as indoor mobile phone positioning [23], and automatic vehicle navigation systems [24,25].
However, the above positioning methods can only provide the absolute position information of the capsule in the tracking space. In fact, the GI tract is constantly squirming, which means a certain position in the space may correspond to different GI positions. Therefore, under the conditions of GI tract peristalsis and fluttering of the belly, the absolute tracking results may fail to provide the correct tissue position, and thus affect the correctness of the diagnosis. Some researchers proposed relative positioning methods based on the human body coordinate system [26,27]. By using additional tracking objects mounted on the human body to serve as tracking references, these methods can easily transform the capsule position results into the body coordinates. Moreover, this method can eliminate the tracking error caused by the relative movement between the human body and the tracking sensor array. However, when the GI tract is peristaltic, the proposed positioning method still cannot provide the precise position of the target in the GI tract. In addition, some medical procedures may need relative length information to determine the location of the lesion, such as endoscopy to stop bleeding, the dissection of polyps, and endoscopic biopsy. Therefore, how one can accurately reflect the matching positioning results of the capsule robot and the intestinal anatomy is an important problem for current capsule robots to be applied in clinical applications.
Above all, neither the absolute localization algorithm based on the sensor coordinate system, nor the relative localization algorithm based on the human coordinate system can provide the appropriate tracking results that can be effectively used for further endoscopy operations. Some researchers have utilized the simultaneous localization and mapping (SLAM) method to obtain position information [28][29][30][31][32][33]. Usually, vision information has been used for tracking by applying the learning methods. However, the intestinal environment is full of mucus and food debris; thus, it is difficult to get the image information to match correctly. As a result, SLAM positioning may also be affected. Moreover, none of the above methods consider that the peristaltic process of the intestine causes a short retrospective process of the robot. To overcome this problem and provide a proper position description method to match the GI tract, we here propose a relative position method for tracking the movement of a capsule robot in the GI tract. As shown in Figure 1, the proposed method uses the moving distance of the robot relative to a marker position along the GI tract to indicate the current position result. First, the positioning method based on magnetic field strength was used to provide the absolute position, which could provide a high-precision, unobstructed wireless positioning result in real time [34][35][36]. Based on the absolute position, the movement path of the capsule robot was fitted with the Bézier curve. Finally, the distance information could then be estimated with the curve integration method. Compared to state-of-the-art capsule tracking methods, the proposed method can directly help guide medical instruments by providing physicians the insertion distance in patients' bodies, which cannot be done based on position results of absolute coordinates. Moreover, as relative distance information has been used, no reference tracking objects need to be mounted onto the human body.

Start point
Moving distance The structure of this paper is organized as follows: the absolute localization method based on the magnetic field is illustrated in Section 2. The algorithms for direction prediction, judgment, and moving distance calculations are developed in Section 3. Section 4 describes the experiments used to verify the accuracy of the proposed method. Finally, conclusions are drawn in Section 5.

Absolute Position Estimation Method
Magnetic tracking is the most widely studied method for wireless capsule robots. The magnetic permeability of the human body is almost the same as that of air, and the distribution of the static magnetic field is basically unaffected by human tissue. The volume of the permanent magnet can be small and easily embedded in the capsule robot, which provides conditions for miniaturization of the capsule robot. In addition, the field strength of the permanent magnet is stable, and no additional energy or peripheral circuitry is required; therefore, wireless tracking can be achieved. Due to the above advantages, the magnetic tracking method was used for the absolute positioning of wireless capsule robots in this paper.
As shown in Figure 2, the magnetic tracking system contains two main parts-a magnet, and a magnetic sensor array. The magnet can be embedded in a capsule robot to provide a magnetic field, which can be measured by the sensor array. By minimizing the difference between the sensing field from magnetic sensors and the theoretic field from the magnetic field model, the position (a, b, c) T and orientation (m, n, p) T of the capsule robot can be obtained using the optimization method.  As shown in Figure 2, the position of the ith sensor is (x i , y i , z i ) T , where (i = 1, 2, ..., N), and its theoretical magnetic field (that generated by the magnet) can be estimated based on the magnetic dipole model, as shown in the following equation: where (B ix , B iy , B iz ) T are the three components of the magnetic field parallel to the coordinate axis, µ r is the relative permeability of air, is a constant characterizing the magnet, which relates to its size and material, and P i and R i are defined as follows: H 0 = (m, n, p) T is a normalized vector characterizing the direction of the magnetic moment of the magnet and under the constrain: The error function is defined as follows: wherex is the estimated state vector of the magnet. The position and orientation (a, b, c, m, n, p) T of the magnet can then be estimated by using the optimization method.

The Relative Position Tracking Method
The movement of the capsule robot in the GI tract is a complex and irregular process. Its position and orientation information is related to the movement of the body, the vibration of the belly, and the motility of the GI tract. For the positioning problem when the human body moves, a positioning method based on the human body coordinate system was proposed in our previous work [27]. However, the problem of evaluating the distance of a capsule robot relative to a marker point based on the absolute position of the space with squirming of the GI is necessary for medial surgery, but has not been well-solved. The proposed method uses the length of the moving path between the current location and the landmark along the GI tract as the relative position of the robot. The proposed method consists of four steps: Firstly, effective sampling points will be obtained from the absolute tracking results. Secondly, the moving direction of the capsule robot will be evaluated by adjacent sampling points. Curve-fitting and integrating methods will then be performed with different direction paths to obtain the curve length. Finally, the length of the moving path can be estimated by superimposing or subtracting the lengths of the curves with different directions, respectively.

Moving States of the Robot
The movement of the capsule robot in the human body is mainly driven by peristalsis of the gastrointestinal tract. The basic mechanical movement of the intestine consisting of a contracted wavy ring was recognized by most people, which can be seen in Figure 3. The motility of the gastrointestinal tract is mainly to push the object that is moving forward (toward the direction of the exit). However, due to the contraction of the contracted wavy, the object behind the contracted wave would have a small direction of reversal. As shown in Figure 3, the rectangular blocks represent the capsule robot. The wavy curves indicate the intestine and its wave ring. Z is the direction of the exit, T 1 means the position of the capsule robot at present, and T 2.1 and T 2.2 denote the next possible position due to peristalsis of the GI tract. According to this situation, the moving direction of the capsule robot needs to be determined, and then the moving distance of the capsule robot relative to a certain feature point can be accumulated by increasing or decreasing the distance length. Although the GI twines around the human body, it also has some corners. Therefore, the angle between two adjacent moving direction vectors may vary within a certain range. Figure 4 describes the moment of capsule robots moving in the corner of the GI. As shown in Figure 4, at the next moment of point P, the capsule robot may move in direction V 2.1 or V 2.2 . It is assumed that moving in the direction of V 2.2 is normally to go forward. However, when moving in the direction of V 2.1 , the capsule robot goes backward. The angle between two adjacent moving direction vectors can be estimated as follows: where V 1 is the vector from the position of the capsule robot at point T 0 to the position of T 1 . V 2 is the vector from the position of T 1 to T 2 . Therefore, we can have: In addition, the positioning error of the magnetic sensor array we used was around 1.2 mm, which is shown in the experimental section. Therefore, to reduce the interference from the tracking result and obtain a smoother path, when the distance l from the last point to the current point was smaller than 4 mm, the position of the capsule robot was assumed to be unchanged, where In this paper, the reverse of the capsule robot is considered when θ ∈ [150 • , 210 • ]. The particular angle in our consideration was only used as a proof of concept, and with more advanced technology in the gastrointestinal kinematics model, we would have been able to define θ in more detail, considering that the distance between adjacent sampling points is only slightly larger than 4 mm. When the robot moves forward along the corner, the sampling points will not be reversed instantaneously, but slowly distributed along the curve. On the contrary, the reversal caused by the contraction of the gastrointestinal tract will lead to the instantaneous reversal of sampling points. According to a large number of gastrointestinal model tests, we believe that θ ∈ [150 • , 210 • ] can distinguish whether the capsule robot is turning forward or moving backwards.

Segmentation Processing
As mentioned in the section above, the capsule robot generally moves towards the exit, but may sometimes reverse due to the squirming of the GI tract. Therefore, for calculating the moving distance of the capsule robot, different moving directions require a different approach. Figure 5 shows a moving sequence of the capsule robot. F 1 , F 2 and F 3 represent how the capsule robot was moving forward, and B 1 and B 2 represent the process of a short reverse of the capsule robot.
A moving directional flag (D.Flag) was set in the experimental program in order to provide the direction information of the micro-robot in real time. The D.Flag was initialized to 1, and transferred to −1 when the direction changed, which repeated during the whole examination progress. D.Flag = (−1) n , where n represents the changing times of the moving direction.  Figure 5. An example of a moving sequence of the capsule robot. F 1 , F 2 , and F 3 represent the capsule robot's movements forward, and B 1 and B 2 represent the process of the robot's movements backward.
In this part, F 1 , F 2 , and F 3 increase the moving length, but B 1 and B 2 decrease the whole distance. Therefore, the moving distance of the capsule robot under this sequence is calculated as: Then, the moving length of the capsule robot in the GI can be estimated as follows: where F m is m-th distance of the capsule robot moving forward, and B n is the n-th distance of the capsule robot moving backward. The swimming speed of micro-robot can also be obtained during the examination. In each period, the average swimming speed can be calculated asV = S/T, where S means the moving length, and T indicates the swimming time. In order to obtain instantaneous speed, the period should be divided into a smaller part.

Moving Distance
The above distance estimation method can have a distance estimation, but it still has drawbacks. The intestinal diameter of an adult is about 20-40 mm, while the diameter of a capsule is about 12 mm. This means that the capsule robot may shake backwards and forwards in one place, which can be seen in Figure 6. It can be seen that the red line matches the length of the GI better than the blue line.

Sample point Link line Fitting curve Gl
As shown in Figure 6, blue stars are the possible sampled position results. The robot's shaking may cause the length of the blue line to be longer than the actual length of the GI tract. Therefore, to minimize the error of distance accumulation and better match the moving length of the capsule robot, the Bézier curve was used to fit the sampling points to make the path smoother. As shown in Figure 6, the red line is the curve-fitting result. It can be seen that the red line matches the length of the GI better than the blue line.
The Bézier curve can be seen to produce a smooth curve that reduces distance error due to oscillation. In this paper, every section of F m and B n was fitted by a piece of a quadratic Bézier curve. The length of each curve was then calculated by integrating each fitting curve. Finally, the moving length of the capsule robot was gained by adding the distance of moving toward the exit and subtracting the distance moved backward.
The general form of the quadratic Bézier curve is presented as follows: where t ∈ [0, 1]. For each Bézier curve, positions of each point P i , (i = 0, 1, 2) can be obtained from the absolute tracking results. Therefore, each segment length can be obtained by integrating B t , which is: The final moving length of the capsule robot can be gained by adding each length of the Bézier curve, which is: (13) where distance + is the length of the capsule robot moving forward, while distance − is the length of the robot moving backward.
According to the above calculation process, the algorithm flow diagram is shown in Algorithm 1.
Algorithm 1 Distance calculation for the moving capsule.

Static Model Experiments
To verify the proposed method, static environmental experiments were carried out to test the measurement accuracy under different environments and conditions. Figure 7 shows the magnetic tracking system. A plane with a circle path and rectangular path was placed above the sensor array to carry out the test. The magnetic tracking system had a position error of 1.2 mm and orientation error of 3.2 • according to our test before the experiments. Figure 7. Experimental platform. The magnetic sensors array contained 8 HMC5883L sensors. Signals from the sensors were transferred to a PC via an IIC to a USD adapter. A plane with a circle path and rectangular path was placed above the sensor array to carry out the test. Figures 8-11 show different distance estimation results in the regular grooves model under different paths. Figure 8 is a Z-shape track, whose length is 380 mm. Figure 9 consists of straight lines and a semicircular arc, and its path length is 638 mm. Figure 10 shows a square path that consists of four straight lines with a length of 190 mm, and the total path length is 760 mm. Figure 11 includes two arcs and several straight lines, the total length being 700 mm. For each experiment, five tests with different speeds and disturbances were performed. The red lines in each sub-figure (b) represent the real-time trajectory tracking results of the capsule robot in 3D space. Real-time distance estimation straight results are shown in the bottom-right of the interface. For each of the sub-figures (a), lines with different colors represent the different moving speeds of the magnet. Some forward-and backward-moving disturbances were also added to some trails, which can be seen in figures with circle markers. A horizontal straight line represents the ground-truth length of the track. As shown in the figures, the distance values are all larger than the real value at the end of each curve. This was caused by the shaking of the magnet while moving during the experiments.      Tables 1 and 2, as well as Figure 13 show the distance measurement errors of the capsule robot in different trajectories. The error is defined as follows: where D est is the estimated distance, and D gt is the ground truth. It can be seen that there is no accumulated error with increasing measuring distance. The maximum distance error is 3.3%, and the final average is 1.27%, which is acceptable for further endoscopy insertion operations.   Figure 13. Experimental error results. The maximum distance error is 3.3%, and the final average is 1.27%, which is acceptable for further endoscopy insertion operations.

Dynamic Phantom Experiments
In order to simulate a more realistic dynamic environment of the capsule robot's moving state in the GI tract, an artificial intestinal phantom with a layer of diseased tissue, such as polyps and tumors made of silicone, was tested in the experiments. As shown in Figure 14a, at the exit of the GI tract, a rope was used to drag the robot for the purpose of simulating the robot's moving state caused by peristalsis of the human intestine, and all led to the robot's overall trend of moving forward. At the beginning of the movement, a blue marker was tied onto the rope, and the experiment stopped when the robot came out of the intestine. The length from the marker of the rope to the end when the robot came out acted as ground truth. Figure 14b shows the experimental results in the GI. The red horizontal line is the depth at where the robot was placed acted as ground truth, which was 840 mm. The distance estimation experiments were carried out three times, and the tracking results were 883.6 mm, 898.8 mm, and 882.6 mm, respectively. The average error was 5.7%. The results are highly close to the ground truth, but there was still some fluctuation. This was caused by the existence of certain errors in the positioning system itself, since the moving positions were mainly on the border of the tracking system, where the positioning error was slightly larger.  In each test, the robot was placed at a depth of 840 mm from the exit, and then carried out by dragging with a rope. In this experiment, the length of the rope acted as ground truth, which was then compared with the accuracy of the magnetic positioning system to judge the reliability of our system.

Comparison
Some articles have also researched the movement path of capsule robots [28][29][30][31][32][33]. The visual odometer, SLAM, and machine learning methods were adopted in the above article. A comparison between the experimental results of this paper and these methods is shown in Table 3. It can be seen that the proposed method achieved good performance. Table 3. Comparisons between the proposed method and state-of-the-art methods.

Error Rate
Proposed method in static environment 1.27% Proposed method in dynamic phantom 5.7% Large-scale direct monocular (LSD) SLAM [31] around 14.0% Oriented fast and rotated brief (ORB) SLAM [32] around 13.2% Magnetic localization [29] around 6.8% Visual localization [33] around 6.5% Fusion by deep visual and magnetic [37] around 4.3% Unsupervised visual Odometry and Depth Learning [30] around 6.2% Compared to the above methods, the proposed method was subjected to less interference. Since the intestinal environment was filled with mucus and physical debris and the intestines were creeping about in real time, methods using image information based on a static intestinal environment may fail to provide available results in clinical applications. However, the proposed method still has limitations, in that the magnetic tracking system has a small sensing distance. For real clinical applications, a larger sensor array with more sensors would be needed.

Conclusions
Although positioning and tracking algorithms and techniques of capsule robots have been extensively studied, but there is still a lack of methods that can provide position results that match with the anatomy of the GI. In this paper, a method for measuring the movement length of a capsule robot in the human body has been proposed. The magnetic tracking system and curve-fitting method were used to obtain the moving distance result. Experimental results verified the proposed method. A maximum error of 3.3% was obtained. In the future, we will continue to improve the proposed method and further carry out animal experiments.