Six-Dimensional Localization of a Robotic Capsule Endoscope Using Magnetoquasistatic Field

This study presents a six-dimensional (6D) localization method and theoretical analysis for a robotic capsule endoscope (RCE). Herein, the challenge of implementing 6D localization for an RCE, with the simultaneous localization of 3D position and 3D orientation, is resolved using optimization with an induced magnetostatic field and RCE kinematics. The optimization is achieved using three transmitting coils (Txs) that are placed in a foundation bed to generate the magnetoquasistatic field and three orthogonal receiving coils (Rxs) that are embedded in the RCE, which is aligned with a capsule coordinate system (CCS), to detect the magnetoquasistatic field. For the 3D position estimation, a nonlinear net magnetic field model is linearized, and a polynomial equation optimization is formulated. The local existence and uniqueness of the solution in the region of interest (ROI) are proved based on the model and simulation. For the 3D orientation estimation, a rotation matrix describing the RCE orientation is computed using the measured magnetic field at the three orthogonal Rxs, and the orthogonality of the rotation matrix is enhanced by using the polar coordinate decomposition. Both simulations and experiments verify the suitability of the proposed method. The maximum error for position and orientation are 1.68+/−0.76 mm and 1.74+/−1.06°, respectively, under a 5 Hz sampling rate in an applicable spherical ROI with a diameter of 10 cm.


I. INTRODUCTION
Various methodologies for robotic capsule endoscope (RCE) localization have been comprehensively studied to date. Posture information provided by the localization system, such as the feedback control achieved by incorporating an external magnetic actuation (EMA) system, can significantly enhance the performance of an RCE [1]- [3]. However, the existing localization system is still in a state of underdevelopment, owing to limitations such as a narrow workspace, low accuracy, and slow response time.
Several previous studies have developed localization systems by utilizing the Hall-effect sensor group. In 2005, Hu et al. built a two-dimensional (2D) sensor array to achieve five-dimensional (5D) localization [4]. In 2016, Son et al.
The associate editor coordinating the review of this manuscript and approving it for publication was Wei Xu . developed a new algorithm for the 2D sensor array system, and a distinction between the magnetic dipole and the magnetic field generated by the EMA system was obtained [5]. However, the limitation of an intrinsic Hall-effect sensor in terms of short sensing distance remains unaddressed. A localization system utilizing visual images has also been extensively studied; sequential visual images obtained during diagnostics using an embedded camera are employed to develop the localization algorithm [6]- [9]. However, uncertain image quality and a continuous increase in error resulted in low accuracy. As a promising approach, a localization system utilizing radio frequency (RF) signals has also been proposed [10]- [14]. However, the high-frequency RF signals are highly attenuated and distorted, and environmental conditions have affected the localization performance. A localization system based on a rotational generator coil has also been developed. In 2001, Paperno et al. introduced a position VOLUME 10,2022 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ and orientation tracking method for a single-axis receiving coil [15]. Furthermore, in 2017, Abbott et al. introduced a localizing and propelling method for a spiral-shaped capsule by applying an external rotating permanent magnet [16]. However, the system has a limited sampling rate due to the rotating rate of the generator coil. In 2017, Sharma et al discussed the communication behavior for magnetic induction (MI)-based technology in nonconventional media [17]. They pointed out that MI-based technology exhibits several unique and promising qualities, such as a large communications range, constant channel behavior, and negligible propagation delay. In 2019, Kisseleff et al. investigated the application of MI-based technology [18]. MI-based technology has been verified to be adaptive for localization purposes, and a large amount of critical information was provided, such as the optimal resonance frequency for the minimum path loss, in their paper. Previously, we proposed a three-dimensional (3D) position localization method based on three Txs and one Rx [19]. This method was developed under the assumption of a known prior alignment orientation of an RCE by the given external actuation magnetic field. However, because the gastrointestinal tract environment sometimes prevents the RCE from aligning accurately with the desired direction, a 6D localization method must be developed to mitigate the limitations of practical hindering. Building on the previous methodology, specifically the theoretical analysis of nonlinear magnetoquasistatic fields for localization, we newly propose a 6D localization method in this study. Fig. 1(a) shows a schematic of the proposed localization system composed of three Txs and three Rxs. The circuit block diagram of the controller and the overall signal flow of the developed hardware system are presented in Fig. 1(b).
The novelty of the study can be summarized with the following contributions; (1) a 6D localization (position and rotation) method by using polynomial governing equation of the net magnetic field generated by a sizable current loop, (2) volumetric magnetic field analysis and investigation by employing nonlinear geometry model, (3) investigation of the colinear and coplanar magnetic field based on the linearized system model and its uniqueness proof to obtain localization, and (4) experimental validation resulting in a more realistic magnetic field model compared to the ground truth.
The remainder of this paper is organized as follows. We present the proposed method and theoretical analysis in Section II. In Section III, the existence and uniqueness of our approach are investigated. The hardware experiments are introduced in Section IV. In Section V, the performance of the system is discussed based on the experimental results. Finally, concluding remarks are presented in Section V.

II. 6D POSTURE RECOGNITION METHOD
To perform the 3D position estimation, a nonlinear magnetic model that is independent of the orientation of the RCE should be first established. The posture of an RCE is defined by its 3D location (x G , y G , z G ) and 3D orientation (α, β, γ ) in a global coordinate system (GCS), which is located at the center of ROI. For the 3D position estimation, a nonlinear net magnetic field model is linearized, and a polynomial equation optimization is newly formulated. The local existence and uniqueness of the solution in the ROI are proved based on the model and simulation. For the 3D orientation estimation, a rotation matrix describing the RCE orientation is computed using the measured magnetic field at three orthogonal Rxs. The orthogonality of the rotation matrix is enhanced using a polar coordinate decomposition. The existence and uniqueness of the rotation matrix are proved by checking the rank of three magnetic fields.

A. NET MAGNITUDE OF MAGNETIC FIELD
The configuration of three-Txs and three-Rxs provides us with nine signals, and these signals change if the orientation changes. However, the net magnitude of the magnetic field is invariable despite the orientation changes, indicating that the function for the magnetic field is independent with orientation. The integral form of governing equations that represent the magnetic flux density in the K-th local coordinate system (LCS) for a Tx coil based on the Biot-Savart law is defined as follows [19]: where D = P K ·P K +r 2 −2r (x K cos θ + y K sin θ). The letters µ 0 , N K , I K , and r K represent the permeability of vacuum, number for the coil turns, and the current and radius of K-th Tx, respectively.
The variables for x K , y K , and z K in equation (1) can be obtained by transforming P G = [ x G y G z G ] T in the GCS to P K in the K-th LCS, and the magnetic flux density B KG = [ b KG i b KG j b KG k ] T in the GCS can be obtained by transforming B K in the K-th LCS to the GCS as follows: where R K G is the rotation matrix from the GCS to the K-th LCS, and T K GORG is a translation vector of GCS and K-th LCS. We assume that a magnetoquasistatic field is generated by the K-th Tx, and the magnetic induction signal is detected by the Rxs. Then, we can transform the induction signal in each Rx to a magnetic flux density vector denoted by S KC = [ s KC i s KC j s KC k ] T , where s KC i , s KC j , and s KC k are the magnetic field component in the direction of three axes of the CCS that is formed by three orthogonal Rxs. With the configuration of the three-axis orthogonal Rx, the net magnitude of the magnetic field at the position of the RCE can be identified. By applying the Euclidean norm to B KG and S KC , the net magnitude of the magnetic flux density for the K-th Tx and at the position of the Rx can be obtained as follows:

B. SYSTEM RANK INVESTIGATION
To resolve three unknown independent parameters for the position of an RCE in the GCS, the system established by three Txs must have a minimum rank of 3. Suppose that the magnetic flux density in the GCS is superposed by three Txs, a system can be established as follows: where I K is the input current applied to each Tx, and B KG denotes the fundamental magnetic field computed using a unit input current (1 A). Applying the Euclidean norm to equation (4), the net magnitude becomes Functions B 1G ·B 1G , . . . , B 2G ·B 3G are independent of each other, owing to the different structures of their expansion. A nonlinear system with a maximum rank of 6 can thus be obtained. However, six equations must be included in the 6-rank system, and the Txs must be energized six times to create a 6-rank system. A 3-rank system can meet our need to guarantee computability and minimize time and resource consumption. There are two methods to create a 3-rank system that can resolve equation (5). One approach is to activate three Txs individually as follows: The other one is to activate two Txs individually and then simultaneously activate two Txs together as follows: Both nonlinear models in equations (6) and (7) can possess a rank of 3 and the computability for a 3D position. However, the nonlinear model (equation (6)) is applied in this study as the local uniqueness of the 3D solution of the nonlinear systems established by equation (6) exists in the ROI of our 3Tx-3Rx system. The analysis regarding the existence and uniqueness for systems (6) and (7) is presented in Section III.

C. LINEARIZED MODEL OF THE NET MAGNITUDE
The net magnitude of the magnetic flux density can be obtained by applying the Euclidean norm to equation (1). However, the ultimate formula is complicated and includes more than 100 terms. Even though the nonlinear solver can solve a complex nonlinear function, a considerably long computing time is unavoidable. Thus, a concise expression for the net magnitude is necessary.
For a circular Tx coil, the net magnitude of the magnetic field is consistent in an arbitrary slice along its center axis. By taking advantage of this symmetric property, a governing equation can be derived in a K-th polar coordinate system (PCS) with two independent variables: a length ρ K and an angle ξ K . Because the magnetic field is naturally continuous and smooth, its net magnitude B KG 2 can be optimized by a continuous and differentiable equation. Based on our experience, the following polynomial is the most suitable: Because the three Txs have the same order and shape, the order and coefficients of three functions B 1G 2 , B 2G 2 , and B 3G 2 are the same. The independence of the three functions is given by their unknown independent parameters ρ K and ξ K in the different K-th PCS, which can be obtained by transforming the unknown position vector P G in the GCS to P K in the K-th LCS using equation (2) and then converting P K to ρ K and ξ K in the K-th PCS by the following equation: The data set of the net magnitude in an arbitrary slice for a Tx can be simulated by applying equation (1) to equation (3). We utilized the Optimization Toolbox in MATLAB (Math-Works), and the coefficient in equation (8) can be optimized through the simulated data set.
The result is shown in Fig. 2. The simulated data set is represented by the blue dot, and a smooth blue surface represents equation (8) with the optimized coefficients. The red region represents the part of the net magnitude within the ROI. The coefficient of determination method is applied to quantify the goodness-of-fit of the optimization result between the data set and the polynomial [20]. Suppose that the data set of the net magnitude simulated by equations (1)-(3) is marked to be the ground truth vector g = [ g 1 . . . g n ] T . Each element of g is associated with a component of the fitted value vector given by the polynomial p = [ p 1 . . . p n ] T . Denoting the residual sum of squares and the total sum of squares as SS res and SS tot , respectively, the coefficient of determination method can be represented by equation (10). The value of R 2 in equation (10) is 0.9994. An R-square number close to 1 yields a nice fitting result of the optimization performance.

D. FORMULATION TO LEAST SQUARE PROBLEM
To compute the 3D position of the RCE, the nonlinear system (6) should be resolved inversely through an appropriate optimization method. In this section, we reformulate system (6) to a least-squares objective function. The rotation matrix for an RCE is defined from the GCS to the CCS using the Z-Y-X Euler angles method as follows: We represent the map between the net magnitude of the magnetic field established by the nonlinear system and at the position of the Rx. By multiplying each side by its respective transpose and applying a 0.5 power to each side, the following equation is obtained: By moving B SYS_K 2 to the left side and taking its absolute value, we have The solution set of equation (13) is guaranteed to be located at the minimal extremum of zero. By rewriting three independent functions in system (6) with their associated sensing value and taking the superposition, we can have where B SYS_K is modeled by a continuous and partial differentiable polynomial; therefore, the solution point of equation (14) can be approached by using Newton's method [21], which is a numerical interior-reflective method that is able to minimize a smooth and partial differentiable nonlinear function of multiple variables. To implement a valid partial differentiation for Newton's method, equation (14) should be squared to prevent the elimination of the scalar sensing value S KC 2 , The structure of equation (15) satisfies the form of a leastsquare objective function min f 2 2 = min( f 2 1 + f 2 2 + f 2 3 ). For the numerical process, the computing program is packaged by the lsqnonlin in MATLAB function, which is developed based on Newton's method, as described in [22] and [23].

E. 3D ORIENTATION RECONSTRUCTION
The 3D position of the RCE can be estimated by solving the least-squares objective function represented in equation (15). The magnetic flux density vector for each Tx in the GCS can be simulated using equations (1)- (3). The map between the magnetic flux density detected by the Rxs and the magnetic field generated by the system (6) can be represented as follows: where , and S C = [ S 1C S 2C S 3C ]. Then, the rotational matrix R C G can be computed as follows: The rotation matrix R C G is orthogonal owing to the 3-axis orthogonal Rx. However, as equation (17) suffers from the inevitable sensing noise in the term of S C , only an orthogonal-like rotation matrix can be obtained as a consequence. To minimize the impact caused by the sensing noise, a real orthogonal rotation matrix can be obtained by applying a polar decomposition [24], where R C G_OR and H are the orthogonal rotation matrix and positive-semidefinite Hermitian matrix, respectively.
Then, the 3D Z-Y-X Euler angle α, β, and γ can be computed as follows: 13 , r 2 11 + r 2 12 ) α = a tan 2(r 12 cβ, r 11 cβ) γ = a tan 2(r 23 cβ, r 33 cβ) (19) The flow chart of the overall execution of the developed algorithm is shown in Fig. 3. It depicts the processing sequence and how each term in the algorithm is calculated. The objective of the algorithm is to estimate the position vector P G and the Euler angles α, β, and γ . First, system (6) is established using the measured current value and the developed polynomial governing equation. Second, the leastsquare objective function is established by the net magnitude detected by the Rx sensor S KC 2 and generated by the nonlinear system B SYS_K 2 . Third, the ground truth magnetic field B G is simulated using the estimated position vector P G from the least-square objective function. Finally, the rotation matrix is computed by comparing B G and the magnetic field S C that is detected by the 3-axis Rx, which then leads to the rotation angles α, β, and γ .

III. EXISTENCE AND UNIQUENESS
In this section, the existence and uniqueness of the solution set of the 3D position and 3D orientation computed by our method is investigated. For the solution of the 3D position, nonlinear systems (6) and (7) are visualized using computer simulation. The local uniqueness is observed in the ROI for nonlinear system (6). For the solution of the 3D orientation, the local uniqueness is proved using the 3-rank magnetic fields B G in the ROI.

A. EXISTENCE AND UNIQUENESS FOR 3D POSITION
The least-square objective function, function (15), indicates that the solution for the 3D position is the intersection of three independent solution sets of f 1 , f 2 , and f 3 . As shown in Fig. 4(a), two possible solutions are formed by the intersection of three separate solution sets for Tx1 to Tx3. Two possible solutions are located in the upper and lower regions, respectively. To comprehensively investigate the distribution of the primary and secondary solutions, we simulated the multilevel contour lines in the slice of the YZ-plane. Fig. 4(b) illustrates the solution sets for f 1 and f 2 in the YZ-plane, where the solution sets of f 2 and f 3 are coincident. The intersections of the multilevel contour lines indicate possible solutions for different levels of the sensing value. The updown structure of the primary and secondary solutions can provide a local uniqueness that is subject to the bound of the ROI. In contrast to system (6), system (7) is a 3-rank nonlinear system that uses only two Txs for the 3D position estimation. However, the up-down structure that appeared in system (6) does not apply to the case of system (7). Fig. 4(c) illustrates such a case in the center plane of two Txs, where solution sets of f 1 and f 3 are coincident, and several groups of primary and secondary solutions are subjected to a left-right structure. Thus, the local uniqueness can only be guaranteed in the ROI by the 3-rank nonlinear system (6).

B. EXISTENCE AND UNIQUENESS FOR 3D ORIENTATION
The rotation matrix is computed by the matrix multiplication in equation (17); therefore, its uniqueness can be investigated by the linear system theorem. We rewrite equation (16) in the form of linear systems by taking the transpose of both sides and obtain: The solution R G C of linear system (20) exists if and only if the ranks of the coefficient matrix B T G and augmented matrix [ B T G S T C ] are the same, and system (20) has precisely one solution if rank([ B T G S T C ]) = rank(B T G ) = 3, which means B T G is a full row-rank matrix. As rank(B T G ) = rank(B G ), the uniqueness of R G C can be proved if three flux vectors satisfy rank(B G ) = 3. Geometrically, rank(B G ) = 1 and rank(B G ) = 2 indicate that three magnetic flux density vectors are colinear and coplanar, respectively. We can identify the location where three vectors are subjected to the colinear and coplanar constraint as follows.
First, we identify the location where rank(B G ) = 1. A flat container, as shown in Fig. 5(a), confines the magnetic vector generated by the K-th Tx at an arbitrary point P G . This flat container passes through the point P G and the origin of both the K-th LCS and GCS. Defining the location of the origin of K-th LCS by O KG = [ x OKG y OKG z OKG ] T , the flat parametric equation for the plane container can be expressed as Representing b K and c K by a K , we obtain By computing the null space for the coefficient matrix of three flat parametric equations, the position vector P G can be obtained as follows: The reduction procedure for equation (23) is omitted here for convenience. As shown in Fig 5(a), Plane1, Plane2, and Plane3 are the three flat containers for the magnetic fields generated by Tx1, Tx2, and Tx3, respectively, and their intersection results in a null vector P G . The null space vector P G represents the common solution space for the linear system established by three flat parametric equations. Therefore, the three magnetic fields are colinear if they are colinear with the null vector P G . The following equation can formulate such a constraint: f colinear_K = B KG · P G B KG 2 P G 2 = 1, K = 1, 2, 3 (24) Fig. 5(b) illustrates the magnetic field B KG and the null space field P G in an arbitrary flat container. The magnetic and null space fields are normalized as our concern is only the direction. To capture the location of the colinear condition exactly, we polarize Fig. 5(b) by applying equation (1−f colinear_K ) 0.2 and assigning a different value at each point using a greyscale color, such that the color at the location of the colinear condition converges to black sharply. As shown in Fig 6(a), a simulation in 3D space focusing on the three flat containers for three Txs is presented. As the structure of the polarized image is the same for each Tx, we can rotate three planes in Fig. 6(a) about their center axes by 180 • to determine the intersection. As shown in Fig. 6(b), the rotation forms three spheres and three intersection points. One of the three points is singular, and equation (24) is not satisfied, as the point is intersected by three straight lines in the ROI and is located at the GCS origin. Two regular points are intersected by three spheres and located outside of the ROI, and the colinear constraints for the three Txs are satisfied. Thus, we proved that the location for rank(B G ) = 1 does not exist in the ROI.
Second, we identify the location of rank(B G ) = 2. The coplanar constraint for three magnetic fields can be formulated by the following equation: Similar to the colinear condition, we apply the polarization by using the equation f coplanar 0.2 to extract the locations for the coplanar condition in some slices, and the result is illustrated in Fig. 7(a). We observe that the black curve is evident in all the slices. As the number of slices increases to infinity, a continuous surface can be created by the curvature, and it is considered the ultimate location for the coplanar condition. The result is shown in Fig. 7(b). We simulated the magnetic flux density on that surface for verification, and equation (25) is satisfied at all the points on that surface. We can observe that our ROI is located inside. Thus, we proved that the location for rank(B G ) = 2 does not exist in the ROI. Fig. 6(b) and Fig. 7(b) illustrate the location for rank(B G ) = 1 and rank(B G ) = 2. Both the colinear and coplanar conditions occur outside of ROI. Thus, magnetic fields generated by the three Txs inside the ROI satisfy rank(B G ) = 3, and the solution of R G C is proved to be unique.

IV. DYNAMIC MOTION TRACKING EXPERIMENTS
Experiments were conducted using two different patterns to test the feasibility of the proposed 6D localization method. The trajectory tracking experiment focused on the RCE under a state of rotation and translation, and the scanning motion tracking experiment addressed the motion of the RCE under a state of rotation only. The concept map for the trajectory and the scanning motion is illustrated in Fig. 8(a), and the experimental hardware setup is given in Fig. 8(b).

A. TRAJECTORY TRACKING EXPERIMENT
In the trajectory tracking experiment, the RCE is driven by a robotic arm along a hybridized path by a helix and a sine function. The robotic arm also creates the rotating motion during translation. In particular, the α angle was rotated continuously while β and γ remained constant. During the locomotion of the RCE, 375 sets of data were uniformly collected and applied to the developed computing algorithm. Fig. 9(a) shows the experimental error between the reference and the 3D position, and 3D orientation estimation. The root-mean-square error (RMSE) for the 3D position and 3D orientation is presented in Table 1. The experimental result verified the feasibility of using the proposed localization method to track an RCE under translation and rotation.

B. SCANNING MOTION TRACKING EXPERIMENT
In the scanning motion tracking experiment, the RCE was fixed at six points and the robotic arm created a rotating motion toward the center axis of the ROI for each point. In particular, β and γ were rotated continuously while α remained constant. During the rotation process, 91 data sets were uniformly collected and applied to the developed computing algorithm.
This experiment aimed to verify the reconstruction performance of an RCE under a scanning motion; therefore, we highlight the tracking results of the rotation angles β and γ at six points in Fig. 9(b). We can observe that the scanning motion is reconstructed well at points 1-5. At point 6, the tracking result is limited by signal attenuation. The experimental RMSE of the 6D posture at six points is presented in Table 1. The experimental result verified the feasibility of using the proposed localization method to track an RCE under rotation only.

V. DISCUSSION
Overall, the experimental error increases when the Rx sensor is far away from the Txs, and fluctuates up and down nonlinearly. Despite the existence of the hardware noise, the offset between the detected field and the ground truth field can also cause such a floating error. The error can be further addressed by applying multiple signal processing methods such as the calibration of the Rx sensor and the application of the state-space Kalman filter. The resulting mean RMSE is estimated by considering all the experimental data. The mean error for the 3D position in the X, Y, and Z directions are 1.68 ± 0.76 mm, 0.98 ± 0.55 mm, and 1.42 ± 0.34 mm, respectively. The mean error of 3D orientation for α, β, and γ are 1.73 ± 1.06 • , 2.24 ± 1.03 • , and 1.54 ± 1.01 • , respectively. The developed localization system provides an updating rate of 5 Hz for the real-time tracking performance in a spherical ROI with a diameter of 10 cm. The updating rate can be further improved for an application with higher real-time requirements. For example, a relay with a higher switching frequency can be applied, or the mechanism of the sequential energizing for three Txs can be upgraded by employing the hardware and software filtering technique for the different frequency bands of three Txs in one channel.

VI. CONCLUSION
This study introduced a novel 6D real-time localization method and theoretical validation for a robotic capsule endoscope for an electromagnetic tracking system. The method can estimate both the position and orientation with the help of the newly introduced 3-axis Rxs. With the proposed localization method, a secondary solution for the estimation result is not required; furthermore, the proposed method allows for a position and orientation estimation independent of an external EMA system. The electromagnetic tracking system was modeled with the magnetic field of a current loop and a magnetic dipole, and the localization and analysis of mapping between the sensing value and dipole model were conducted based on the linear system theory [25]. In this study, a polynomial governing equation for the net magnitude was developed to better fit the magnetic field with the ground truth. Moreover, the mapping between the sensing value and polynomial model was created using the nonlinear system and analyzed by a computer simulation. A least-squares objective function for the nonlinear system was formulated and optimized using a commercial nonlinear solver. Investigations using 3D imaging could provide the uniqueness and distribution of the desired solution. Finally, the method could be verified through simulation and experiments, which can guarantee its further applicability in a real environment. We expect that the proposed localization method can be used for the position feedback control of robotic devices in medical applications in the future.