Posture Detection of Dual-Hemisphere Capsule Robot Based on Magnetic Tracking Effects and ORB-AEKF Algorithm

Abstract

Posture detection is essential for capsule robots to be manipulated in a relatively closed gastrointestinal (GI) tract and to fulfill some medical operations. In this paper, a posture detection technique for a magnetic-actuated dual-hemisphere capsule robot (DHCR) is proposed. In this method, the DHCR realizes fixed-point posture adjustment based on tracking effects, and feature points are recognized and matched with the help of the ORB algorithm on the GI image acquired by a vision sensor. The system model is derived from the dynamic model and feature point information. Then, the posture is optimized by using the adaptive extended Kalman filter (AEKF) algorithm. As a result, the posture detection method based on the tracking effects and the ORB-AEKF algorithm is formed. The effectiveness and superiority of the proposed method are verified through experiments, which provide a good foundation for the subsequent, accurate closed-loop control of the DHCR.

1. Introduction

Magnetic-actuated capsule robots (MACRs) hold great potential for medical applications in the gastrointestinal (GI) tract [1,2]. Magnetic actuation is considered one of the most effective strategies because it allows for remote manipulation with high controllability, and it is more biocompatible and can penetrate human tissues without causing harm [3,4]. However, the individual variability in the GI tract tissue in terms of size, shape, and location, coupled with its relatively closed character, undoubtedly increases the difficulty of navigating MACRs in the GI tract. The actuation mechanism and posture estimation of MACRs play an important role in navigation maneuvers in the GI tract.

In general, the types of magnetic actuation can be divided into the magnetic traction type, which uses external permanent magnets [5] or electromagnets [6]; the magnetic force–torque hybrid type [7]; and the pure magnetic torque type, which uses paired coils [8]. The mechanisms of the first two types rely on static magnetic fields to establish a static equilibrium. However, this approach suffers from the coupling of magnetic force and torque, making it susceptible to disturbances that can affect the posture control of MACRs. In contrast, the mechanism of the third type employs dynamic magnetic fields to create a dynamic equilibrium. Based on this, a dual-hemisphere capsule robot (DHCR) system is proposed [9,10], which is actuated by a spatial universal rotating magnetic field (SURMF) generated by three-axis orthogonal square Helmholtz coils (TOSHCs). The DHCR exhibits a tracking effect [11] with good anti-interference capability and stability. This approach significantly enhances the accuracy of posture control compared to a static approach. However, the complex GI environment, the orientation error of the SURMF, and the dynamic response of the DHCR may all affect the actual axial orientation. Thus, realizing high-precision posture estimation remains a key challenge for clinical applications.

Currently, researchers are conducting in-depth studies on the posture detection of MACRs in the GI tract using different sensors and localization techniques [12], such as radio frequency, magnetic, inertial, medical imaging-based, and visual localization. Radio frequency can be used as a means of measurement using electromagnetic waves. Ye [13] used a radio frequency system for capsule localization. The advantage of this system is that it only requires a reading device to be installed outside the human body, allowing for remote measurements and an easy manipulation process. The human body is a heterogeneous and highly attenuating environment for radio frequency signals, which affects the quality of the read signal and reduces localization accuracy. Inertial detection technology relies on inertial measuring units such as gyroscopes and accelerometers. It has the advantages of autonomous observation and continuous dynamic localization. Vedaei [14] embedded inertial measuring units into a capsule to achieve the feedback of motion data. However, the inertial detection error may grow over time when performing continuous dynamic measurements. Thus, inertial detection techniques are rarely used alone and usually need to be combined with other techniques in practical applications. Magnetic localization utilizes sensors to detect magnetic sources, and its methods can be classified into two types. For example, Xu [15] employed an external magnetic sensor array for the pose parameters of MACRs, which is a passive method; Popek [16] proposed a method for estimating the posture of MACRs with embedded Hall effect sensors, using a rotating dipole field, which is an active method. The external sensor arrangement in the first method can be limited by the working space. Built-in sensors in the second method consume more limited on-board space and power. Meanwhile, the magnetic sensors may experience interference with the magnetic field used to drive the MACRs, affecting detection accuracy. Medical imaging-based localization techniques mainly include the use of X-ray, computed tomography, and magnetic resonance imaging. Marya [17] used this technique to image MACRs, providing data to support the assessment of the accuracy of the localization technique. It should be noted that the human body should not be irradiated frequently or for long periods of time to avoid radiation damage. Visual localization [18] uses a video of the GI tract captured by MACRs as the localization source. Because there are no internal or external sensor devices, MACRs save internal space. In addition, endoscopists in general also clarify the subsequent operations to be performed based on the GI video to realize the complete navigation task. Although this method enables visualization, the detection of pose accuracy in the complex GI tract environment remains a challenge. There are also several localization methods that have been heavily researched, such as the ultrasonic [19], the deep learning and neural network-based [20,21], and the multi-sensor fusion localization methods [22]. For the DHCR proposed in this study, no gyroscope sensors or Hall sensors are installed inside the capsule, considering the size and energy constraints, so vision technology is utilized to achieve posture detection.

In practical applications, factors such as sensor noise, nonlinear motion, and external interference can affect the accuracy of posture estimation. The dynamic response characteristics of the DHCR during posture adjustment are also not taken into account, resulting in large errors in some of the detected values [23]. Extended Kalman filtering (EKF) with the covariance matching method is denoted as adaptive extended Kalman filtering (AEKF), which can be used to solve this problem. Therefore, to improve the posture detection accuracy, this paper proposes a DHCR posture detection method based on the ORB-AEKF algorithm with the aid of the dynamic motion model. This method fuses magnetic control and visual localization techniques without additional sensors and provides the key support for the closed-loop control of DHCR.

This paper is organized as follows: Section 2 introduces the DHCR posture detection method. Section 3 describes the tracking effects mechanism and establishes the motion model. Section 4 elaborates on the ORB-AEKF algorithm. Section 5 experimentally validates the posture detection method. Finally, Section 6 gives a conclusion.

2. Posture Detection Method Overview

2.1. Magnetic-Actuated DHCR Diagnostic System

The magnetic-actuated DHCR diagnostic system consists of a SURMF generating device, a DHCR, and a human–machine interaction (HMI) communication control system, as shown in Figure 1.

The SURMF generating device (TOSHC, as shown in Figure 1a) produces a SURMF when a three-phase alternating current is applied to the coils. The specific current formula is shown in [10]. The SURMF with magnetic vector B rotates counterclockwise with angular velocity ω around the axis normal vector n_B= (θ, ϕ), where θ and ϕ are the yaw and the pitch angles of n_B, respectively.

The spherical DHCR comprises active and passive hemispheres, as shown in Figure 1b, connected by a ceramic bearing. The active hemisphere contains an NdFeB radial magnetizing ring actuator, whereas the passive hemisphere is underdriven. The magnetic dipole moment m and the magnetic vector B generate magnetic torque T rotating the active hemisphere via the magnetic ring. Based on tracking effects, the DHCR can align its axis with the applied magnetic field direction. As shown in Figure 2, two distinct working modes emerge based on DHCR-GI tract contact: (1) when only the passive hemisphere contacts the tract, the fixed-point posturing is performed in the passive mode; (2) when the active hemisphere contacts the GI tract, it is converted to the active mode, and rolling walking is performed. The passive hemisphere integrates an image acquisition module, wireless transmission module, and power supply for system communication, as shown in Figure 1c.

The HMI communication control system mainly contains an HMI and a SURMF controller to receive and display GI tract images for endoscopic diagnosis, as shown in Figure 1d. Then, by sending control signals to the SURMF controller, operators can adjust the orientation, intensity, and rotation speed of the SURMF to guide the DHCR to complete clinical procedures.

2.2. Posture Detection Method

The posture detection method consists of three main components: (1) DHCR posture calibration and adjustment based on tracking effects, (2) GI tract image acquisition by the vision system and feature points recognition based on the ORB algorithm, and (3) posture estimation based on the AEKF, as shown in Figure 3.

First, the DHCR initial posture calibration is performed. As the DHCR undergoes multimodal conversion in the GI tract, the starting posture calibration is realized based on the self-standing characteristic of tracking effects. This ensures that the DHCR aligns its axial in a vertical upward orientation. In addition, in the vertical orientation, the magnetic vector azimuthal difference is zero, which reduces the effect of phase difference caused by coil inductance. Therefore, the vertical orientation is taken as the initial posture, and the DHCR vision module captures GI tract images. Afterwards, when posture adjustment is performed based on the fixed-axis characteristic of tracking effects, the DHCR vision module is utilized to capture GI tract images during the process. During the movement of DHCR based on tracking effects, its axial posture is defined as the state vector, and the system state equation is constructed with the aid of the DHCR dynamics model.

Then, after capturing GI tract images before and after posturing, the improved ORB algorithm is utilized to recognize and match feature points. The matching relationship and pixel coordinates of the feature points are obtained. Since there are no other sensors inside the DHCR, the feature point pixel coordinates become the system observed values, and the observation equations are constructed based on the functional relationship between the image and world coordinate systems.

Finally, because the state and observation equations of the DHCR system are nonlinear, the AEKF is used for posture estimation. After linearizing the state and observation equations, the current state estimation value (DHCR axis orientation) is iteratively updated using prior state estimates and current observations. The AEKF algorithm considers visual detection errors, effectively integrates prediction and observation information, and ultimately optimizes state estimation.

In summary, the above progress constitutes a DHCR posture detection method based on the ORB-AEKF algorithm. This method incorporates the DHCR system motion characteristics on the basis of visual detection, improving the posture detection effect.

3. Tracking Effects Mechanism and Motion Model Establishment

3.1. Tracking Effects Analysis

After placing the DHCR in the SURMF, the DHCR is guided to adjust its posture by controlling the SURMF, achieving multimodal motion. Assuming that the angle between the SURMF orientation n_B and the magnetic ring orientation n is σ, a coordinate system O-x₁y₁z₁ is introduced, where axis Oy₁ aligns with the line orthogonal to both n and n_B, and the axis Oz₁ coincides with axis n, as shown in Figure 4.

The rotating magnetic vector B can be represented in the O-x₁y₁z₁ coordinate system as follows:

$${\mathit{B}}_2=\left(\begin{array}{ccc}\cos \sigma & 0& -\sin \sigma \\ 0& 1& 0\\ \sin \sigma & 0& \cos \sigma \end{array}\right)\left(\begin{array}{c}{B}_0\cos \omega t\\ B_0\sin \omega t\\ 0\end{array}\right)=\left(\begin{array}{c}{B}_0\cos \sigma \cos \omega t\\ B_0\sin \omega t\\ B_0\sin \sigma \cos \omega t\end{array}\right)$$

(1)

where B₀ is the magnetic flux density. A previous study [11] showed that the magnetic ring synchronously rotates with B but lags by a hysteresis angle δ. Thus, the magnetic dipole moment m can be expressed in the O-x₁y₁z₁ coordinate system as follows:

$$\mathit{m}={\left(\begin{array}{ccc}{m}_0\cos (\omega t-\delta )& m_0\sin (\omega t-\delta )& 0\end{array}\right)}^{\mathrm{T}}$$

(2)

where m₀ is the modulus of the magnetic moment and δ is the slip angle.

The coupling magnetic torque T can be expressed in the O-x₁y₁z₁ coordinate system as follows:

$$\mathit{T}=\mathit{m}\times {\mathit{B}}_2={\left(\begin{array}{ccc}{T}_{x1}& T_{y1}& T_{z1}\end{array}\right)}^{\mathrm{T}}=m_0{B}_0\left(\begin{array}{c}\sin (\omega t-\delta )\cos \omega t\sin \sigma \\ -\cos (\omega t-\delta )\cos \omega t\sin \sigma \\ \cos (\omega t-\delta )\sin \omega t-\sin (\omega t-\delta )\cos \omega t\cos \sigma \end{array}\right)$$

(3)

The magnetic torque component T_z1 drives m to rotate around the axis Oz₁, while T_y1 and T_x1 drive the magnetic ring to deflect around the axis Oy₁ and Ox₁, respectively. Although T_x1 and T_y1 are time-varying, their periodic nature causes oscillations around an equilibrium point with a period of τ′ = π /ω. By applying the period-averaging method, the equivalent magnetic torque components ${\overline{T}}_{x1}$ and ${\overline{T}}_{y1}$ are derived as follows:

$$\left(\begin{array}{c}{\overline{T}}_{x1}\\ {\overline{T}}_{y1}\end{array}\right)=m_0{B}_0\left(\begin{array}{c}{\displaystyle \frac{1}{\tau \text{'}}}{\displaystyle {\int }_0^{\tau \text{'}}{T}_{x1}\mathrm{d}t}\\ {\displaystyle \frac{1}{\tau \text{'}}}{\displaystyle {\int }_0^{\tau \text{'}}{T}_{y1}\mathrm{d}t}\end{array}\right)={\displaystyle \frac{m_0{B}_0}{2}}\left(\begin{array}{c}\sin \sigma \sin \delta \\ -\sin \sigma \cos \delta \end{array}\right)$$

(4)

Then, the equivalent total deflection torque ${\overline{T}}_{xy}$ and its equivalent azimuth $\overline{a}$ are derived as follows:

$$\begin{array}{l}{\overline{T}}_{xy}=-\sqrt{{\overline{T}}_{x1}^2+{\overline{T}}_{y1}^2}=-{\displaystyle \frac{B_0{m}_0}{2}}\sin \sigma \\ \overline{a}=\arctan {\displaystyle \frac{T_{x1}}{T_{y1}}}=\arctan (\tan \delta )=\delta \end{array}$$

(5)

The relationship curves between the equivalent magnetic torques and angle σ are shown in Figure 5. It can be seen that ${\overline{T}}_{xy}$ is basically close to ${\overline{T}}_{y1}$, which indicates that ${\overline{T}}_{y1}$ dominates the deflection motion so that n always tends to deflect toward n_B. However, when σ is not 0°, ${\overline{T}}_{x1}$ always exists. Therefore, the magnetic ring deflects around the Oy₁′ axis within the OAB plane, rather than directly around the Oy₁ axis within the Ox₁z₁ plane. The angle between the Ox₁z₁ plane and the OAB plane is $\overline{a}$, representing the deviation degree of n during motion.

Figure 6 illustrates the magnetic ring axis trajectory. As σ gradually approaches 0°, ${\overline{T}}_{xy}$ decreases to 0 mN⋅m, causing n to align with n_B. At this equilibrium state, since the DHCR is coaxial with the magnetic ring, its axis remains fixed. It indicates that after applying SURMF in any direction, the axis n of the DHCR can always track the axis n_B and ultimately maintain coaxial, forming tracking effects. The fixed-axis characteristic serves as the fundamental mechanism for DHCR to achieve fixed-point posture adjustment.

When a vertically oriented magnetic field is applied, the DHCR axis rapidly aligns with the vertical direction, achieving a self-standing state. The whole process demonstrates the unique self-standing characteristic of DHCR [10].

3.2. Motion Model Establishment

Posture detection in passive mode requires model-aided techniques, necessitating a clear dynamic model of the DHCR. The DHCR is a multi-rigid body structure with internal forces. To avoid solving internal forces, the Lagrangian method is used to construct the dynamic equations. The specific modeling process is shown in Appendix A. Eventually, the dynamic equation can be obtained as follows:

$$\mathit{M}\left(\mathit{q}\right)\ddot{\mathit{q}}+\mathit{C}(\mathit{q},\dot{\mathit{q}})\dot{\mathit{q}}+{\mathit{M}}_f(\dot{\mathit{q}})=\mathit{{\rm T}}$$

(6)

where q is Euler angles; T is the magnetic torque; ${\mathit{M}}_f(\dot{\mathit{q}})$ is the resistance torque; and $\mathit{M}\left(\mathit{q}\right)$ and $\mathit{C}(\mathit{q},\dot{\mathit{q}})$ are the inertia matrix, and the centrifugal force and Coriolis force matrix, respectively.

To avoid singularities inherent in the Euler or Cardan angle representations, the DHCR posture is described by quaternions p (in Appendix B). The conversion between Euler angles (α and β) and quaternions is given by the following:

$$\begin{array}{l}\alpha =\arctan {\displaystyle \frac{2(p_0{p}_1+p_2{p}_3)}{1-2({p_1}^2+{p_2}^2)}}\\ \beta =\arcsin (2(p_0{p}_2-p_3{p}_1))\end{array}$$

(7)

where p_i (i = 0, 1, 2, 3) is the Rodriguez parameter.

4. Posture Detector Design Based on ORB-AEKF Algorithm

The posture detection method consists of a quaternion-based state equation, an observation equation based on the ORB algorithm, and posture estimation based on the AEKF algorithm.

4.1. State Equation

The DHCR axis posture is expressed based on quaternions as the state vector. The state vector and its time derivative can be expressed as follows:

$$\begin{array}{l}{\mathit{X}}_t={\left(\begin{array}{cccc}{p}_0& p_1& p_2& p_3\end{array}\right)}^T\\ {\dot{\mathit{X}}}_t={\displaystyle \frac{1}{2}}\mathit{\Omega }(\omega ){\mathit{X}}_t\end{array}$$

(8)

where the angular velocity matrix Ω(ω) is defined in Appendix B.

Applying the first-order Runge–Kutta method to solve the differential equation yields the following discrete form of the state equation:

$$\mathit{X}(k+1)=f[k,\mathit{X}(k)]=[{\mathit{I}}_4+{\displaystyle \frac{1}{2}}\mathit{\Omega }(\omega )\Delta k]\mathit{X}(k)$$

(9)

Considering the model uncertainties and dynamic noise W, Equation (9) can be modified as follows:

$$\mathit{X}(k+1)=f[\mathit{X}(k)]+\mathit{W}(k)$$

(10)

where the dynamic noise W(k) is Gaussian white noise with mean zero and covariance matrix Q(k).

4.2. Observation Equation

4.2.1. GI Image Feature Point Recognition and Matching Based on the ORB Algorithm

Due to the volume and energy limitations, the DHCR does not incorporate gyroscopic sensors. Instead, posture information can be detected through the vision system. During the posture adjustment process of DHCR based on tracking effects, GI images can be obtained, as shown in Figure 7.

For GI image processing, we evaluated both traditional (ORB algorithm) and deep learning approaches (self-supervised learning [24] and SuperPoint [25]). After comparison, it is found that although deep learning methods have become mainstream in machine vision, there are still some limitations in the application process. First, the complex GI environment requires extensive labeled training data, increasing preparation difficulty. Second, substantial computational demands reduce real-time performance, particularly problematic given the need for GPU acceleration in resource-constrained scenarios. In contrast, the ORB algorithm is a lightweight feature extraction algorithm with a processing time of about 378 ms and significantly lower resource requirements than deep learning schemes. The ORB algorithm has better robustness in processing GI tract images, smaller computation, and a better quasi-matching effect, which has been validated in a series of experiments in GI tract environments with different light and blurring conditions [26]. Therefore, this paper selects the ORB algorithm for GI tract feature matching to provide data support for the subsequent posture estimation.

As shown in Figure 8, the ORB algorithm divides the processing of image feature information into two parts: the feature detection and the feature point description, which consists of the improved oFAST corner point detection algorithm and the rBRIEF descriptor. After the image features are detected, feature matching is required to establish the relationship between the images, which is achieved by comparing the similarity relationship between the feature points. After the binary descriptors are obtained from the ORB algorithm, Hamming distance is used to match feature points coarsely. In order to reduce the matching error, outlier feature matches are removed using a random sampling method. The processed GI images yield the feature matching results shown in Figure 9, from which we extract the precise feature point coordinates for subsequent posture estimation.

4.2.2. Observation Equation Establishment

The system observation vectors are the feature point pixel coordinates in the two-dimensional pixel coordinate system, which maintain a specific constraint relationship with the state vectors. To establish this relationship, we define the following coordinate systems.

The world coordinate system O-XYZ, camera coordinate system o_c-x_cy_cz_c, image physical coordinate system o_m-x_my_m, and image pixel coordinate system o′-uv are introduced, as shown in Figure 10. The camera is solidly connected to the passive hemisphere, and o_c-x_cy_cz_c is established with the center o_c of the camera as the origin, in which the optical axis is the o_cz_c axis. In o_m-x_my_m, the plane x_mo_my_m is perpendicular to the o_cz_c axis, and the intersection point is o_m. The o_mx_m and o_my_m axes parallel the o_cx_c and o_cy_c axes, respectively. The camera focal length is o_co_m, which is usually denoted by f. To describe the image pixel point location, the image pixel coordinate system o′-uv is constructed, where the coordinate system origin is located in the upper left corner of the image plane, and the o′u and o′v axes are parallel to the o_mx_m and o_my_m axes, respectively.

In the passive mode, the DHCR vision module remains fixed to the passive hemisphere, causing the camera pose to be equivalent to directly present the DHCR posture. Leveraging the isotropic homogeneity of the SURMF, the origin of the camera coordinate system can coincide with the origin of the world coordinate system. Then, o_c-x_cy_cz_c completes the coordinate conversion to O-XYZ through the rotation matrix R. Take a point P in space as an example to describe the mapping relationship from O-XYZ to o′-uv. For a spatial point P(X, Y, Z) in O-XYZ, with project image point p (u, v) in o′-uv, the transformation from O-XYZ to o′-uv can be expressed as follows:

$$z_c\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)=\left(\begin{array}{cc}\mathit{K}& \mathbf{0}\end{array}\right)\left(\begin{array}{cc}{\mathit{R}}_I& \mathbf{0}\\ 0& 1\end{array}\right)\left(\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right)=\left(\begin{array}{cccc}f/\mathrm{d}x& 0& u_0& 0\\ 0& f/\mathrm{d}y& v_0& 0\\ 0& 0& 1& 0\end{array}\right)\left(\begin{array}{cc}{\mathit{R}}_I& \mathit{0}\\ 0& 1\end{array}\right)\left(\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right)$$

(11)

where K is the camera internal reference matrix; R_I is the rotation matrix; dx and dy are the pixel physical dimensions; (u₀, v₀) is the point where the imaging plane intersects the optical axis, which is also called the main point and is theoretically located at the geometric center of the image; and z_c is the o_cz_c axis coordinate of P in o_c-x_cy_cz_c.

When the initial posture calibration of DHCR is performed based on tracking effects, R is 0. Assuming that the feature point is initially in o′-uv with the coordinates (u_s, v_s), according to Equation (11), the coordinates of the feature point P_s can be calculated as follows:

$${\mathit{P}}_s=\left(\begin{array}{c}{X}_s\\ Y_s\\ Z_s\end{array}\right)=\left(\begin{array}{c}{z}_s(u_s-u_0)dx/f\\ z_s(v_s-v_0)dy/f\\ z_s\end{array}\right)$$

(12)

Assuming that the pixel coordinate of the feature point obtained at moment k is (u_k, v_k), according to Equation (11), the following is obtained:

$$\left(\begin{array}{c}{u}_k\\ v_k\end{array}\right)={\displaystyle \frac{{\mathit{F}}_c\cdot {\mathit{R}}_{Ik}\cdot {\mathit{P}}_s}{{\mathit{E}}_1\cdot {\mathit{R}}_{Ik}\cdot {\mathit{P}}_s}}$$

(13)

where

$${\mathit{F}}_c=\left(\begin{array}{ccc}f/dx& 0& u_0\\ 0& f/dy& v_0\end{array}\right),{\mathit{E}}_1=\left(\begin{array}{ccc}0& 0& 1\end{array}\right)$$

(14)

and R_Ik derives from Equation (A6).

Based on Equation (13), the observation equation of the system is expressed as follows:

$$\mathit{Z}(k)=h[k,\mathit{X}(k)]$$

(15)

Clinical preparations (8-h fasting and consuming simethicone) improve the GI visualization [27], but illumination variations and motion blur persist, introducing observation noise V. The observation equation can be modified as follows:

$$\mathit{Z}(k)=h[k,\mathit{X}(k)]+\mathit{V}(k)$$

(16)

where the observation noise V(k) is Gaussian white noise with mean zero and covariance matrix R(k), calibrated per [26].

4.3. AEKF Algorithm

Posture estimation optimization is achieved by solving the posterior probability distribution of the state quantities. Methods such as the Kalman filter, EKF, unscented Kalman filter, and particle filter are usually used [28]. Since the DHCR dynamic system and the observation equations are nonlinear, the standard Kalman filter is inapplicable. The unscented Kalman filter can capture higher-order nonlinearities and is suitable for high-accuracy applications in controlled environments, but it is sensitive to uncertain noise. The particle filter gives more accurate results with sufficient samples but is computationally intensive and also faces sample impoverishment. In contrast, EKF is suitable for scenarios with uncertain noise, while the computational complexity is relatively small, and is the most widely used nonlinear state estimation technique. Therefore, EKF is selected for the posture estimation of DHCR in this paper.

The EKF linearizes the nonlinear system, involving Equations (10) and (16), through first-order Taylor expansion, enabling the application of the Kalman filter algorithm. An accurate a priori covariance matrix plays a key role in applying EKF for posture estimation, which involves the process noise covariance matrix Q and the observation noise covariance matrix R. The inaccurate noise variance estimation can lead to a decrease in estimation accuracy. The essence of the adaptive method is to correct the noise variance in real time, by automatically updating the noise covariance matrix in the algorithm to make it closer to the real characteristics of the noise. In this paper, the covariance matching method [29] is introduced to realize the noise adaptive.

The AEKF algorithm executes through three sequential phases: prediction, updating, and adaptive variance matching. In the prediction phase, the system state equations, observation equations, and error variance matrix are predicted. In the updating phase, the state prediction equation is modified, and the state error covariance matrix is updated based on the calculated Kalman gain. Finally, the adaptive variance matching of observation and process noise is realized by introducing scale factors to update the Kalman gain and prediction error variance matrices. The specific process is depicted in Appendix C.

In summary, combined with the tracking effects motion mechanism, the DHCR detection method based on ORB-AEKF is formed. The specific algorithm flow block diagram is shown in Figure 11.

5. Experiment

5.1. Experimental Platform

To verify the effectiveness of the posture estimation method proposed in this paper, an experimental platform is built, comprising a DHCR, a TOSHC, an HMI communication system, and a simulated GI tract, as shown in Figure 12. The DHCR prototype has been developed, integrating a radial magnetized NdFeB magnetic ring, miniature ceramic bearing, battery, image acquisition module, and radio frequency transmitting module to a sphere with a dual-hemisphere structure. During experiments, the DHCR is placed in the simulated GI tract. The simulated stomach model used is made of silicone, and it is approximately 25 cm in length, 10 cm in width, and 8 cm in height, with a lumen volume of 1 L. In addition, during the experiments, the surface of the stomach model is lined with isolated porcine colon to simulate real tissue. During operation, the DHCR transmits the GI tract image to the computer. After inputting parameters to the SURMF controller, the controller feeds three-phase currents into the TOSHC to generate SURMF. The DHCR can be actuated by SURMF based on tracking effects to realize multimodal motions.

The maximum working magnetic field strength of the TOSHC is maintained below 15 mT, with a maximum rotational frequency of 10 Hz. The magnetic field radiation and thermal effects meet the safety requirements [30,31].

5.2. Tracking Effects Verification Experiments

Tracking effects is the core mechanism by which the DHCR performs fixed-point posturing. During experiments, when the SURMF with different axial orientations is applied, the DHCR axis orientation can be rapidly deflected from initial orientations to the set target orientation, as shown in Figure 13.

Due to the complex GI environment, the DHCR may experience various disturbances. External disturbances are simulated through manual interference to test the immunity to interference in the fixed-axis characteristics of DHCR, as shown in Figure 14.

In the anti-interference experiment, the applied SURMF axis is vertically upward. The DHCR axis quickly aligns the vertical orientation after applying the magnetic field, as shown in Figure 14a. Physical disturbance is simulated through manual interference, causing axis deviation from vertical, as shown in Figure 14b. After disturbance cessation, the DHCR axis can quickly return to vertical orientation, as shown in Figure 14c. The experiments show that the DHCR exhibits excellent stability and anti-interference properties, enabling it to maintain posture stability against natural peristalsis and other GI tract disturbances while performing modal functions.

5.3. ORB-AEKF-Based Posture Detection Verification Experiments

5.3.1. Simulated GI Condition Experiments

To validate the DHCR posture detection method, a DHCR posture measurement experimental setup is constructed, as shown in Figure 15. Two cameras are set up along the x-axis and y-axis to observe and record the DHCR axis n during posture adjustment. The angles (η_x and η_y) between the axial projections n_yz and n_xz and the vertical z-axis are measured in each camera view, as shown in Figure 16. Based on the geometric relationship, the DHCR posture is calculated as follows:

$$\left\{\begin{array}{l}\alpha =\arctan ({\displaystyle \frac{\tan {\eta }_x}{\tan {\eta }_y}})\\ \beta =\arctan ({\displaystyle \frac{1}{\sqrt{{\tan }^2{\eta }_x+{\tan }^2{\eta }_{\mathrm{y}}}}})\end{array}\right.$$

(17)

After averaging the above equation over several calculations, the posture angle obtained is used as a reference value to compare with the posture estimate.

Prior to the experiments, camera calibration was performed using Zhang's method to obtain the camera internal reference matrix K:

$$\mathit{K}=\left(\begin{array}{ccc}466.14& 0& 320\\ 0& 466.14& 240\\ 0& 0& 1\end{array}\right)$$

(18)

The DHCR posture detection procedure comprised the following steps: (1) The DHCR is placed in the simulated GI tract, with verification of proper image acquisition from both the embedded DHCR camera and external Cameras 1–2. (2) The TOSHC generated a vertically upward SURMF. Based on the self-standing characteristic, the DHCR starting posture calibration is performed, and the corresponding GI image is recorded. (3) Multiple target magnetic field orientations are set. Based on the fixed-axis characteristic, the DHCR performs fixed-point alignment, with GI images recorded during adjustment process, as shown in Figure 17. (4) The ORB-AEKF algorithm processes the recorded images to extract the DHCR’s axis posture. Key calculation parameters are listed in

Table 1. Posture detection calculation parameters.

Parameter	Value
Magnetic flux density B0	6 mT
Angular velocity ω	16π rad/s
Scale factor ϑ	0.8
Process noise covariance matrix Q	diag(0.001, 0.001, 0.001, 0.001)
Observation noise covariance matrix R	[5;5]

.

To evaluate the robustness of the proposed posture detection method, we compared its performance with a conventional EKF-based approach that lacked adaptive covariance matching. Figure 18 presents the comparative results through plotted curves of the DHCR's axial yaw and pitch angles. The data reveal that while both methods successfully capture the general posture trends of the DHCR, their performance diverges with increasing deflection angles. During initial stages with minimal deflection, both methods produce nearly identical results. However, as the deflection angle grows, the ORB-AEKF-based method demonstrates superior accuracy, with its estimates aligning more closely with the reference values than those generated by the standard EKF approach.

To quantitatively evaluate the detection accuracy of each posture detection method, we compared their performance using three key metrics: maximum error, mean error, and error variance.

Table 2. Error statistics of DHCR posture detection methods.

Detection Method	Statistical Information	Yaw Angle	Pitch Angle
Posture detection based on ORB	error maximum value	4.87°	6.62°
	error mean value	1.67°	2.44°
	error variance	1.95	3.56
Posture detection based on ORB-EKF	error maximum value	4.79°	1.14°
	error mean value	1.53°	0.40°
	error variance	1.57	0.13
Posture detection based on ORB-AEKF	error maximum value	1.55°	0.76°
	error mean value	0.76°	0.31°
	error variance	0.16	0.07

presents these statistical measures for all tested methods, enabling direct comparison of their precision and stability.

Table 2 shows that the posture detection method based on ORB-AEKF achieves significant improvements across all error metrics, with reductions in maximum error, mean error, and variance compared to alternative methods. The average error values of yaw angle and pitch angle are 0.76° and 0.31°, respectively, and the variance is minimized, which indicates that the posture detection results are robust. The results prove the superior performance of the proposed posture detection method. The yaw angle detection error is slightly lower than the pitch angle detection error, most likely due to gravity center offset [32]. The gravity center offset torque becomes a dynamic disturbance torque in the yaw motion since the yaw magnetic torque and the gravity center offset torque are orthogonal vectors, which cause a larger detecting error in the yaw angle. Meanwhile, the gravity center offset torque and the pitch magnetic torque are isotropic, which will not interfere with the pitch motion, so the pitch angle detecting error is lower.

5.3.2. Posture Detection Application to the Navigation in Porcine Colon

To strengthen the claim of clinical applicability, proposed posture detection is applied in a porcine colon environment so that testing conditions more closely resemble in vivo scenarios, as shown in Figure 19. The DHCR is placed inside the porcine colon, and the DHCR movement is recorded by external and internal cameras. The actual value of the DHCR posture moving in the porcine colon is more challenging to measure. Navigation performance serves as an indirect validation metric, as the system calculates navigation direction from detected posture. For curved intestines, the dark zone is an important feature region. The navigation orientation from its mass center is derived based on using the improved OTSU algorithm. Different navigation orientations are set up to carry out the experiments, and the results are shown in Figure 20. The results show that the average error between the calculated and the actual values is within 2.3°. This error is superimposed by the errors of the posture detection method and the dark zone mass-center extraction method. The average error of the dark zone mass-center extraction method is about 1.3°. It is inferred that the posture detection error is about 1°. The marginally higher errors observed in biological tissue versus simulations likely resulted from real tissue deformations affecting DHCR motion and increased image matching complexity in vivo. The above experiments verify the feasibility of the proposed posture detection method.

6. Conclusions

The innovation of this paper is a posture detection method based on the ORB-AEKF algorithm for the magnetic-actuated DHCR. The method achieves posture calibration and adjustment based on tracking effects, combines GI tract image information acquired by the vision system with the ORB-based feature recognition and matching algorithm, and ultimately realizes the optimal estimation of the DHCR axial posture by using AEKF. This solution only requires a visual sensor, eliminating the need for additional inertial or magnetic sensors that would occupy the DHCR's internal space, which is conducive to further miniaturization. In addition, this method effectively fuses the dynamic response characteristics and visual observation information to enhance the DHCR posture estimation. The average detection errors of the yaw angle and pitch angle are 0.76° and 0.31° in simulated GI conditions. This method is applied with good results in porcine colon navigation experiments, providing an effective sensing feedback scheme for realizing closed-loop control of DHCR.