Research on Intraoperative Organ Motion Tracking Method Based on Fusion of Inertial and Electromagnetic Navigation

The intraoperative movement of target organs is currently a key and difficult problem that restricts clinical surgery diagnosis and treatment, especially radiotherapy. Unexpected posture changes can cause the deviation of the target area during the operation, thereby affecting the treatment effect and even causing severe complications. Therefore, it is necessary to track the movement of target organs in real-time and accurately to improve the effect and safety of the surgical treatment. This paper proposes an intraoperative organ motion tracking method based on the fusion of inertial navigation and electromagnetic navigation., which can directly measure the target organ’s movement under non-invasive or minimally invasive conditions using the human natural cavity. The proposed method for identifying the magnetic field’s interference state uses the extended Kalman filter to fuse the inertial measurement unit and the electromagnetic positioning information to achieve target organ movement tracking under different interference conditions. It effectively improved the real-time and robustness of intraoperative organ tracking. Simultaneously, based on the 9-axis inertial measurement unit and a 6-DOF electromagnetic positioning system, the catheter-type target organ tracker was developed, and then under the simulated electromagnetic interference environment, dynamic and static verification experiments were carried out, respectively. The maximum tracking error of displacement and attitude is 2.75 mm and 0.127 rad, respectively, under severe electromagnetic disturbance through experiments. Under non-electromagnetic interference conditions, its displacement and attitude’s maximum tracking errors are 0.94 mm and 0.011 rad, respectively. The results prove that the target organ tracking method based on the fusion of inertial navigation and electromagnetic navigation is feasible. It can realize the tracking and measurement of the target organ’s movement within the clinical permission error range.


I. INTRODUCTION
Intraoperative dynamic real-time tracking of target organs is a critical issue and difficulty that currently restricts clinical surgery diagnosis and treatment [1]. Especially for cancer radiotherapy surgery, it requires that the radiation can precisely cover the tumor cells while avoiding excessive damage to normal tissues. However, in clinical practice, factors such as respiration, digestion, postural changes, and instrument disturbances may cause unintended movements The associate editor coordinating the review of this manuscript and approving it for publication was Ze Ji . of target organs and even cause tumors to leave the planned target volume (PTV) range [2]- [4], resulting in tumor remnants. And also expose a large amount of healthy tissues to the radiation range, causing inevitable unnecessary damage or even serious side effects. Therefore, there is an urgent clinical need for a reliable intraoperative organ dynamic movement tracking technology to obtain real-time tumor position and posture. This helps the treatment device to be able to point to the target more precisely, narrow the PTV boundary range, avoid damage to normal human tissues, and improve the precision and effect of surgical treatment [5], [6]. VOLUME 9, 2021 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ For the problem of intraoperative target organ movement, many scholars and medical institutions have proposed various technical solutions, such as the passive pressure technique based on constrained organ motion space and the method of deep inspiration breath-hold [7], [8], Gating technique based on the perception of organ motion cycle pattern [9] and movement prediction compensation algorithm [10], and methods such as four-dimensional CT and slow CT based on image scanning technology [11]. However, it is difficult to obtain the real movement of the intraoperative target area in real-time for all the above schemes. Therefore, in recent years, dynamic target organ real-time tracking methods are proposed to track the tumor position and posture in real-time through the device, divided into three modes: image tracking, direct tracking, and indirect tracking. Among them, image tracking is the direct tracking of target area movement by intraoperative ultrasound, C-arm fluoroscopy, magnetic resonance imaging, and other imaging methods. Its use is cumbersome and has the mutual constraints of imaging accuracy, additional radiation dose, and response rate [12]- [14]. The primary method of direct tracking is to implant the gold pellet into the patient as a marker and detect the tracking by X-ray imaging, which has a high measurement accuracy and good real-time performance. Still, the implantation process is invasive, and the patient needs to be exposed to a high additional radiation dose [15]- [18]. Indirect tracking is a non-invasive and simple method to infer the movement of the target area by acquiring parameters such as body surface markers or respiratory heart rate in real-time, but it is difficult to accurately obtain the correct position of the target area through the body surface state alone because the relationship between the body surface and the internal organ target area movement is complicated and not constant [19]- [21]. For direct tracking and indirect tracking methods in terms of marker movement measurement, in vitro markers are mainly sensed by optical localization or mechanical motion localization methods, which have high accuracy and reliability under the right detection conditions. However, when the marker target is in vivo, optical localization methods cannot operate due to the occlusion problems [22], [23]. In this case, it is necessary to use imaging methods such as ultrasound, CT, or electromagnetic positioning to achieve the movement measurement of the markers in vivo. However, similar to optical positioning systems' occlusion problem, electromagnetic positioning suffers from the problem of magnetic field interference affecting the measurement accuracy. Therefore, to address the problem of insufficient stability of a single sensor, Max Q.-H. Meng et al. proposed a method to solve the problem of intraoperative occlusion as well as limited field of view of optical tracking systems by fusing multiple monocular optical sensors in an array [24], and Zhang Y. D. et al. proposed a method based on the fusion of intraoperative ultrasound images with electromagnetic localization to achieve movement tracking of intraoperative local lesion locations [25]. Jiang-Sun et al. oriented to the problem of image navigation during surgery improved the intraoperative navigation rate and alignment accuracy by ultrasound and magnetic resonance image fusion method [26]. All of the above researches have made a lot of useful explorations in intraoperative navigation. Still, they have not yet solved the problem of dynamic and accurate perception of target organ movement position under intraoperative interference environment.
In order to solve the above shortcomings, this paper proposed a real-time tracking technique for intraoperative target organ position based on the fusion of inertial navigation and electromagnetic navigation. The high refresh rate and good anti-interference characteristics of inertial navigation are combined with the advantages of high positioning accuracy of electromagnetic navigation to solve self-drift and intraoperative magnetic field transient perturbation, respectively. Besides, a target organ movement sensing sensor was designed to achieve direct sensing of the target organ under non-invasive or minimally invasive conditions by using the human body's natural cavity to improve the intraoperative target organ position tracking accuracy and real-time performance. Although the introduction of an electromagnetic navigation system needs to increase the cost of medical equipment, and it takes time to place sensors. However, it can reduce the additional damage to patients and medical personnel by getting rid of traditional particle implantation surgery on CT, X-ray, and other radioactive image navigation methods.

II. DESIGN OF TARGET ORGAN POSTURE FUSION PERCEPTION SYSTEM
In order to solve the problem of obtaining the posture tracking of target organ under the premise of minimally invasive or non-invasive, the target organ posture fusion sensing system is proposed in the application context of prostate radioactive particle implantation surgery, which mainly consists of an electromagnetic tracking system, a catheter-type target organ tracker and an upper computer system. The catheter-type target organ fusion tracker is placed through the urethra into the mid-prostate region to synchronize the anterior catheter sensing module's movement with the target organ. After the fusion of inertial navigation and electromagnetic navigation data, the real-time position data of the target organ can be transmitted to the host system for intraoperative decision making or displayed intraoperatively in real-time through HoloLens and other mixed reality holographic devices, providing surgeons with a more intuitive and accurate picture of the actual movement of the organ and improving the quality and efficiency of surgery. The overall layout architecture is shown in Figure 1.
The tracking unit of the target organ intraoperative movement tracking system comprises a catheter-type target organ fusion tracker based on the fusion of inertial navigation and electromagnetic navigation and a magnetic field generator. The magnetic field generator is an essential component of the electromagnetic navigation system, which acts as an excitation source to generate a low intensity, alternating electromagnetic field in space, and uses the induced electric potential generated by the positioning coil in the magnetic field to calculate the spatial distribution of the magnetic field to derive the current location and attitude of the target. Because the principle of the magnetic field generator restricts its positioning space into a domed measurement interval and a rectangular measurement interval, the rectangular measurement interval has higher positioning accuracy and reliability, so the magnetic field generator needs to be placed close to the target organ during the operation to ensure that the target organ is located in the high-precision measurement space during the operation. The accuracy and stability of the electromagnetic positioning system are guaranteed.
The catheter-based target organ fusion tracker consists of a head support frame, upper support diaphragm, bottom support diaphragm, and posture fusion sensing module, with an overall size of less than 6 mm at the maximum diameter and the head support frame length of no more than 25 mm to ensure that it can be successfully implanted into the urethra. The upper and bottom support diaphragms can be expanded to both sides by filling saline through the Luer taper at the rear of the head support frame, respectively, thus ensuring that the front end of the movement-sensing catheter can be stably connected to the target organ. The front end of the catheter can be stably connected to the target organ. At the same time, the support diaphragms on both sides are independently driven, and the lifting height of the support diaphragms on both sides can be adjusted according to the actual positioning requirements, which can be combined with the overall rotation and linear movement of the catheter to achieve the 3-DOF adjustment of the catheter-type target organ fusion tracker in the human body cavity, and its specific structure is shown in Figure 2.
The posture fusion sensing module consists of the inertial measurement unit and the electromagnetic positioning unit, where the inertial measurement unit is a 9-axis MEMS sensor structure, consisting of a 3-axis MEMS gyroscope, a 3-axis MEMS accelerometer, a 3-axis Magnetometer and digital motion processor DMP, with the gyroscope range of ±2000 • /s, the zero bias of ≤ 0.32 rad/h, the accelerometer range of ±8 g, the zero bias of ≤20 mg, the Magnetometer  range of ±8 Gauss, the zero bias of ≤ 0.003 Gauss, the maximum output frequency of 500 Hz, and the overall size is 8.5mm × 5.5mm × 2mm. The 6-DOF positioning coil of Aurora electromagnetic tracking system of the NDI company is used in the electromagnetic positioning module. The coil size is 1.8mm × 9mm, the positioning measurement accuracy is 0.48 mm, the orientation measurement accuracy is 0.3 • , and the measurement frequency is 40 Hz. The posture fusion sensing module is embedded in the support frame of the catheter head after preoperative sterilization. It is sealed with silicone for intraoperative and sterilization requirements to maintain the internal sensor environment's stability. The specific configuration and layout of the sensor are shown in Figure 3.

III. INERTIAL NAVIGATION ATTITUDE TRACKING A. MEASUREMENT MODEL AND PRINCIPLE
The integrated motion sensor has a smaller package size that is well suited to the requirements of introducing motion sensors into the natural cavity of the human body. The linear acceleration and angular velocity of target organ motion can be measured in real-time with a high sampling rate by accelerometer and gyroscope. However, the accelerometer has good static characteristics, but the dynamic characteristics are susceptible to vibration and noise, which results in measurement deviations. The gyroscope has good tracking characteristics in measuring the rotational angular velocity. Still, its measurement angle is obtained by integrating the angular velocity, and the gyroscope sensor is susceptible to zero drift caused by temperature and other factors, which will also lead to serious measurement deviations. The inertial measurement unit has the problem of data drift and measurement noise in its measurement value, and the drift data will accumulate over time. Therefore, in the 6-DOF perception measurement of target organ movement based on the inertial VOLUME 9, 2021 measurement unit, the gyroscope, accelerometer, and magnetometer measurement models are defined as where,ω is the output of the measured angular velocity of the gyroscope rotation around the body axis; ω is the true angular velocity value of the target attitude of the measured target; b ω is the bias error of the gyroscope measurement; η ω is the gyroscope measurement noise;ã is the output of the accelerometer acceleration measurement along the body axis; a is the true acceleration velocity value of the target attitude of the measured target organ; a g is the component of the earth's gravity on the acceleration measurement axis, which needs to be removed when calculating the position based on the inertial measurement cell; b a is the bias error of the accelerometer measurement; η a is the measurement noise of the accelerometer;m is the measured value of the magnetometer; m is the magnetic intensity acting on each sensitive axis of the magnetometer; b m is the magnetometer measure the offset error; η m is the magnetometer measure the noise.
The bias error of gyroscope and accelerometer is a common problem and accumulates gradually with time, leading to an increase in inertial tracking error. The bias errors of the gyroscope and accelerometer are defined as the bias of the respective measured values and the random white noise in the bias.
where b ωp , b ap and b mp is the measurement offsets of the gyroscope, accelerometer, and magnetometer, respectively. b ωn , b an and b mn is the random white noise in the measurement bias of gyroscope, accelerometer, and magnetometer, respectively.

B. ATTITUDE ALGORITHM BASED ON COMPLEMENTARY FILTERING OF INERTIAL SENSORS
The catheter-type attitude sensor achieves a fixed connection movement with the target organ through the support diaphragm and obtains the target organ's real-time movement velocity. However, to obtain the target organ's actual attitude, it is necessary to convert the carrier coordinate system b in which the inertial sensor is located to the reference coordinate system n in the real world. However, to obtain the target organ's actual attitude, it is necessary to convert the carrier coordinate system b in which the inertial sensor is located to the reference coordinate system n in the real world.
To avoid the singularity question of gimbal lock in the solution of Euler angular differential equation, use the quaternion method to calculate the attitude angle of the carrier. Thus, the rotation matrix C b n based on the quaternion can be described as: The target attitude quaternion is updated based on the rotation rate of each axis of the gyroscope.
where ω b = 0 ω x ω y ω z is the rotational angular velocity of the carrier target. After integrating the above equation and processing based on the first-order Runge-Kutta method, the equation for each attitude angle is obtained as follows: The attitude reference value can be obtained from the acceleration sensor, and when the carrier coordinate system of the acceleration sensor is stationary relative to the reference coordinate system, the acceleration components a x , a y , a z of each axis measured by the acceleration sensor meet: According to equation (7), the reference values of roll angle φ and pitch angle θ can be measured based on the measured value of acceleration sensor, and the calculation formula is as follows  To eliminate the problems of noise interference and drift error in the inertial sensor measurement model mentioned above. Through the analysis of the gyroscope and accelerometer measurement data, it can be learned that the measurement angle data is obtained by integrating the gyroscope data, so it is accurate in a short period. Still, with the increase of time, the accuracy will decline due to drift errors, and its static characteristics are poor mainly because of the existence of lowfrequency noise. For the accelerometer, its error is relatively large in a short period when subjected to movement. Still, with the increase of measurement time, its average acceleration value is accurate, its dynamic characteristics are poor mainly because of high-frequency noise. Therefore, combining the respective signal characteristics of the gyroscope and the accelerometer, based on the complementary filtering algorithm, high-pass and low-pass filters are introduced in the frequency domain angle, respectively. The complementary filtering and fusion of the data information of the gyroscope and the accelerometer improves the signal-to-noise ratio and eliminates the zero error, thereby improving the accuracy and reliability of the attitude calculation of the inertial measurement unit.
As for the process of complementary filtering, take the roll angle φ as an example, set the transfer functions of the firstorder high-pass filter and the low-pass filter as H (S), L(S), respectively. If the transfer function satisfies H (S)+L(S) =1, the attitude angle obtained by complementary filtering can be expressed aŝ where φ is the reference value of attitude angle calculated based on acceleration sensor. The above equation can effectively eliminate the low-frequency and high-frequency signal interference in the gyroscope and accelerometer measurement data and improve the attitude measurement accuracy.

IV. INERTIAL AND ELECTROMAGNETIC NAVIGATION FUSION A. INERTIAL AND ELECTROMAGNETIC NAVIGATION COORDINATE SYSTEM CALIBRATION
The target tracking system based on the fusion of inertial navigation and electromagnetic tracking uses inertial measurement units and electromagnetic navigation to track the same target. However, due to the inherent displacement and attitude bias between the body coordinate systems of each sensing system. Therefore, it is necessary to perform coordinate system correction before the fusion track.
For the inertial sensing system, which is based on inertial force to achieve the measurement of target organ movement, the coordinate system of the inertial measurement unit is defined as O IMU and the geographic coordinate system it is based on is defined as O WGS . Based on magnetic sensing, the electromagnetic tracking system can perform a 6-DOF attitude measurement. The measurement unit is mainly composed of the electromagnetic field generator and the sensing coil, and the coordinate system of the magnetic field generator is defined as O ENS and the coordinate system of the sensing coil is defined as O COIL . The definition of each coordinate system is shown in Figure 5. For the coordinate system correction of the inertial sensor and electromagnetic tracking system, the robot handeye calibration method is introduced, and the transformation relationship IMU COIL T between the inertial sensor coordinate system O IMU and the sensing coil coordinate system O COIL is solved by the method AX=XB. According to the installation relationship of the sensor, it is known that the magnetic field generator does not change its position relative to the earth during the two movements, that is, the coordinate transformation relationship WGS ENS T between the magnetic field generator coordinate system O ENS and the geographic coordinate system O WGS is always constant. Therefore, it can be concluded by Since the position of the inertial measurement unit and the electromagnetic positioning coil are relatively fixed, the coordinate transformation relationship IMU COIL T between them is also always constant, so it can be concluded as follows After obtaining AX=XB, the fused attitude tracking system is rotated around the determined axis to obtain two sets of measurements separately, and the parameters of the coordinate system transformation between the inertial measurement unit and the electromagnetic tracking system are obtained by solving for X [27].

B. CORRECTION OF SENSOR BIAS ERROR
Due to inertial sensors' characteristics, which the bias error will increase with time, it is necessary to correct it to avoid serious attitude tracking deviations during long periods of operation. The electromagnetic navigation system used in this paper is based entirely on the electromagnetic positioning principle, which avoids the problem of accumulated errors and provides accurate target position information in a non-disturbing environment. Therefore, this paper uses electromagnetic positioning data to correct the inertial measurement unit's error and bias. The ideal value of angular velocity can be obtained by the first derivative of time of the updated amount of attitude angle between the two refresh data of electromagnetic positioning data, and for acceleration, the ideal value at the current attitude can be calculated using the cosine algorithm based on the positional data of the electromagnetic navigation system.
where φ and θ are the roll angle and pitch angle of the target in the ideal state calculated based on the electromagnetic positioning data, the bias error model of the inertial sensor with bias and noise is obtained by comparing the ideal value with the measured value of the inertial sensor.
where ω I and a I represent the angular velocity and acceleration measurements values based on inertial sensors, respectively, and ω E and a E represent the ideal values of angular velocity and acceleration based on electromagnetic positioning systems, respectively. n ω and n g are the random noise in the gyroscope and accelerometer. In order to remove the noise in the bias error model of the inertial sensor, an FIR filter with an order of 15 is designed. Its output is the inertial sensor's bias, and its input is the filter coefficient and the ideal and measured values of the inertial sensor.
where the filter coefficient h(k) satisfies the condition that h(k) = 1. The bias error in the inertial sensor measurements is obtained when the noise is removed using the FIR filter. The correction process of inertial sensor bias error based on electromagnetic positioning sensor is shown in Figure 6.
C. DATA SYNCHRONIZATION The electromagnetic tracking system has high measurement accuracy for target position and attitude, and each measurement result is independent of the previous measurement result, but the maximum measurement rate is 40 Hz due to the measurement principle, which is much lower than the 200 Hz measurement frequency of the inertial measurement unit. Therefore, to realize the two data's fusion, the acquired inertial measurement data and electromagnetic tracking data are synchronized.
Since the electromagnetic tracking system's measurement data has the characteristic of low refresh frequency, and the measurement is accurate under non-interference. Therefore, in the process of data synchronization, the position data measured by the previous electromagnetic tracking system is updated by the inertial measurement unit in the interval between two measurement data updates of the electromagnetic tracking system, and when the electromagnetic tracking system measures the data in the next time, the new electromagnetic positioning data is used as the reference for data update. Within 0.025 s of the update cycle of the electromagnetic tracking system, the inertial measurement unit is less affected by the cumulative error, and its data credibility is higher, so the synchronization of electromagnetic positioning data based on the inertial measurement unit is a further improvement on the original positioning accuracy of the electromagnetic tracking system, and effectively improve the refresh rate of the electromagnetic tracking system, improve the smoothness of navigation in operation. Meanwhile, each time, When the data of electromagnetic tracking system data is updated, the drift error based on the inertial measurement unit will be cleaned without cumulative effect.

D. DATA FUSION OF INERTIAL AND ELECTROMAGNETIC TRACKING SYSTEM
The measurement data obtained from the electromagnetic tracking and inertial measurement units are fused based on the extended Kalman filter method to improve the fusion tracking system's tracking accuracy and anti-interference capability. The electromagnetic tracking system can obtain high accuracy position and attitude data without electromagnetic interference; the gyroscope in the inertial measurement unit can measure the angular velocity of the target, and the attitude change can be obtained by integrating and accumulating the angular velocity; while the acceleration sensor can discern the current target attitude by gravitational acceleration and get the displacement of the target after its integration and accumulation. Therefore, in the process of data fusion, the inertial measurement unit is used to navigate the electromagnetic tracking system between the two signals to improve the measurement frequency of the electromagnetic tracking system and avoid the data drift of the inertial navigation system. When interference is detected in the electromagnetic tracking system, the autonomous navigation is based entirely on the inertial measurement unit to improve the fusion tracking system's adaptability.

1) STATE ESTIMATION
For nonlinear estimation, the extended Kalman filter method is used in this paper to track the organ's position and attitude. The state variableX k is composed of the target attitude quaternion q = q 0 q 1 q 2 q 3 T , the target position p = X Y Z T , The state estimation equation is as follows: where ω is the angular velocity measured by the gyroscope after removing the bias error, a is the acceleration data measured by the accelerometer, and the data measured by the inertial measurement unit is corrected by the sensor bias error b. n b is the noise during the sensor bias update. The incremental update of angle obtains the attitude measurement of the target, but the acceleration of gravity g needs to be offset before the movement state analysis and calculation of the accelerometer.
After eliminating the effect of gravitational acceleration, the velocity value v can be obtained by integrating the obtained acceleration measurements once against time, and the displacement value p can be obtained by integrating twice. The state matrix F can be obtained by Taylor expanding the above equation and cut-off in the first-order.
The priori error covariance matrix of the extended Kalman filter is where Q is the noise in the state transition: where q i , p i , v i , and b i are the updated system covariance matrices of attitude angle, position, velocity, and bias error, respectively, with the noise model defined as white noise: The noise variance parameters are obtained by comparing the measured and estimated values of angular velocity and acceleration. The noise variance parameter σ 2 qi for the attitude estimation process is 0.00003 deg 2 , the noise variance parameter σ 2 pi for the position estimation process is 0.00065 mm 2 , and the noise variance parameter σ 2 vi for the velocity estimation process is 0.00007 mm 2 /s 2 .

2) MEASUREMENT UPDATE
The Kalman gain matrix can be updated by the following equation:K where P k is the priori error covariance matrix of the system, H is the Jacobi matrix that transforms the system measurements and state quantities, S is the intermediate value for calculating the Kalman gain, and R is the covariance matrix of measurement noise errors of the sensor itself.
To enable the extended Kalman filter to track normal movement and abrupt movement, the quaternion measurement noise for attitude perception is divided into normal measurement noise covariance r n and adaptive noise covariance r a .
mp and mv are the error covariance matrices for position and velocity tracking based on the electromagnetic tracking system, respectively. When the electromagnetic tracking system is not disturbed, its position and attitude measurement accuracy are high, and its measurement noise covariance is chosen as a small parameter. In this case, the Kalman filter uses the signal obtained from the electromagnetic tracking system as the reference measurement value for the correction of the predicted value. When serious electromagnetic interference is detected in the electromagnetic tracking system, the measurement noise covariance is set to a larger value, at which point it can be determined that the measured value of the electromagnetic tracking system is no longer corrected for the estimated value. In turn, the measurement update equation of the system is obtained as follows: The structure of the fusion tracking method based on inertial and electromagnetic navigation is shown in Figure 8.

3) INTERFERENCE STATE IDENTIFICATION
The electromagnetic tracking system senses the change of magnetic field through the electromagnetic positioning coil, so as to calculate the current position and attitude. Therefore, it is possible to measure the occluded environment, such as inside the body. Still, due to the constraint of its positioning principle, when the electromagnetic interference causes a disturbance in the measurement magnetic field area, the electromagnetic positioning system will still output the deviated position and cannot know that it has been seriously disturbed. Therefore, the interference state decision method of the electromagnetic system based on the mutual discrimination of inertial navigation and electromagnetic navigation signals is proposed, which is divided into four measurement states as follows: a) The stationary state is a set of data output by the electromagnetic tracking system within the period t a whose standard deviation σ EMA is less than the set value. Combined with the actual clinical situation, it can be determined that the current state is static non-device interference. b) Interference eliminated means that when it is determined as interference, the standard deviation σ EM −G of the difference between electromagnetic positioning and inertial navigation measurement data during the continuous time period t b is less than the set value, which means that the electromagnetic tracking system data is similar to the independent inertial navigation measurement data, and the electromagnetic interference is judged to have been eliminated in the case of judged interference. c) Status signal means the external signal S 0 with interference and the external signal S 1 without interference, which is directly input by the upper computer system or medical staff according to the current operating environment to determine whether there is electromagnetic interference.
d) The interference state means that the standard deviation σ EM −G of the difference between electromagnetic positioning and inertial navigation measurement data is greater than the set value within a short period t d . This represents a large deviation between the electromagnetic tracking system data and the independent inertial navigation measurement data, which is judged as the system is currently subject to electromagnetic interference and requires autonomous navigation through the inertial measurement unit.
In addition to using the difference between the inertial measurement unit and the electromagnetic navigation data, the magnetometer's measurement data can also be used to determine whether there is interference from ferromagnetic equipment.
First, determine the average magnetic field strength m 0 in the non-interference state in the current environment. According to the experimental statistics, when the magnetic field strength m 1 ≥ m 0 + 500µT , it can be determined that there is ferromagnetic interference currently.

V. TARGET ORGAN POSTURE TRACKING EXPERIMENT
To verify the effectiveness of the proposed target organ movement tracking method based on inertial navigation and electromagnetic navigation fusion, a fusion tracking system was established, including the NDI Aurora electromagnetic tracker for electromagnetic tracking, the ICM-20948 integrated 9-axis movement processing sensor for inertial navigation.
In order to verify the motion tracking accuracy of the fusion tracking system in an electromagnetic interference environment, the sensor's current real movement attitude and position need to be known while tracking based on the fusion tracking sensor. Therefore, the UR3e collaborative robotic arm is introduced, and the fusion tracking sensing unit is placed on the flange at the end of the arm, and the UR robotic arm drives the end of the catheter-type target organ fusion tracker to move according to the predefined trajectory. The repeatable positioning accuracy of the UR collaborative robotic arm is better than 0.03 mm, which is higher than the requirements for verifying the sensor measurement accuracy and the clinical requirements for the perception of the target organ motion accuracy. Therefore, the TCP movement attitude data of the end output through the robotic arm is used as the real position reference of the sensor in the experiment. The installation layout structure is shown in Figure 9.

A. SENSOR BIAS ERROR EXPERIMENT
For the verification of the effectiveness of the bias correction algorithm and parameter measurement problem of the fusion of inertial and electromagnetic navigation, the fused tracking sensing unit is kept stationary in a non-electromagnetic  interference environment for continuous measurement, and the output data of each axis of the sensor in a static environment are obtained as shown in Figure 10 and Figure 11.
In terms of the angular velocity output from the gyroscope, it can be seen through Figure 10 that the measurement accuracy is high and stable in the x-axis and y-axis directions, but there is a significant offset error in the z-axis direction that needs to be eliminated.
From Figure 11, it can be seen that the measurement data output from the accelerometer contains the effect of gravitational acceleration. It is necessary to eliminate the component of gravitational acceleration on each axis to obtain the real output value based on the sensor's actual attitude in  the static environment. However, after removing the effect of gravitational acceleration, each axis's output data still has obvious measurement noise compared with the gyroscope signal, which needs to be removed. Otherwise, it will lead to serious cumulative data drift in the position solution.
In order to verify the validity of the error correction algorithm, the standard error RMSE of the pose tracking is calculated by the error correction and non-correction algorithms, respectively, based on the measurement data in the static environment. The measurement results are shown in Table 1.
In the static non-interference environment, the fusion tracking system's output data is the attitude state based on the updated correction of the electromagnetic tracking system, and each group of measurement data is independent of each other to avoid the cumulative error problem of the inertial measurement unit.

B. STATIC VERIFICATION EXPERIMENTS UNDER ELECTROMAGNETIC DISTURBANCE ENVIRONMENT
To verify the anti-interference ability of the target organ tracker under an electromagnetic disturbance environment.  Keep the sensor unit in a static state, apply disturbance signals to the electromagnetic tracking environment, and increase the tracking deviation of the electromagnetic tracking system. Thus, the output result of the fusion tracking system in terms of displacement is shown in Figure 12.
The static test experiment in the electromagnetic disturbance environment can be concluded that in the initial state of 0-15s without electromagnetic disturbance, the fusion tracking system measurement result (GEM) takes the electromagnetic positioning result (EM) as the updated reference, and the output value is similar to the true value (REAL). When subjected to severe electromagnetic disturbance, the output result of the fusion tracking system still takes the electromagnetic positioning signal as the reference due to the relatively small error in the initial 15 s-18 s of the electromagnetic disturbance, so there is a certain amount of disturbance deviation in the output signal, and the maximum positioning error is 2.25 mm at this time. When the electromagnetic disturbance fluctuates further, at the moment of t = 18s, the output result of the electromagnetic tracking system and the output result of the inertial measurement unit satisfy the judgment condition of σ EM −G ≥ 3. It is determined that the electromagnetic tracking system is seriously disturbed, and the fusion tracking system uses the measurement signal of the independent inertial measurement unit as the output data to avoid further increase of the measurement deviation caused by electromagnetic interference. However, with the further extension of the disturbance time, the output results based on the inertial measurement unit navigation gradually increased due to drift and other factors, resulting in a certain amount of measurement deviation, but its deviation value was 3.47 mm in the maximum positioning error in a long-time static test experiment within the 70 s, which can meet the required clinical requirements. When the electromagnetic disturbance is over t ≥ 70s, the output signal of the electromagnetic tracking system is relatively stable to meet the judgment condition of σ EMA < 1, it is determined that the current sensor is in a stationary non-interference state, and the fusion tracking system uses the electromagnetic positioning output data as the reference signal to update, so as to eliminate the accumulated error of the inertial measurement unit and improve the measurement accuracy and antiinterference capability of the fusion tracking system.

C. DYNAMIC VALIDATION EXPERIMENTS
For the position measurement of the fusion tracking system in the motion environment, the sensor module is driven by the UR robotic arm to move in a straight line along with space without changing the movement attitude, and its movement process is to move from the origin (0, 0, 0) to (-20, -20, 0) at a speed of 20 mm/min, and then to (0, -20, -20) at a speed of 30 mm/min after stopping for 20 s. To avoid the problem of deviation between the robot's set path and the actual running trajectory, use the URScript control command sent to the UR robot through port 30003 to read the real TCP position data returned in real-time as the reference position of the sensor. And the electromagnetic disturbance signal is added to simulate the movement measurement characteristics of the fusion tracking system based on inertial navigation and electromagnetic navigation in the environment where the magnetic field is disturbed, and the measurement results are shown in Figure 13.
The dynamic displacement experiment shows that the fusion tracking system (GEM) can effectively track the position during the phase when the electromagnetic positioning signal (EM) is disturbed (30 s-90 s, 120 s-150 s), and the positioning error is less than 2.75 mm compared with the real value (Real). At the same time, in the initial stage of magnetic field disturbance, the fusion tracking is carried out by means of inputting external state signals, which avoids the problem that the fusion tracking output results are shifted with the disturbance electromagnetic positioning signal in the initial stage of the disturbance in the static test experiment, and the positioning tracking is smoother and avoids the problem of positioning signal step caused by state switching.
In terms of attitude tracking, the origin of the sensor module coordinate system is set as the TCP point of the robotic arm, and the end of the robotic arm is driven by the program to make a fixed-point rotary movement around the axis. The posture tracking experiment is realized without  changing the displacement, and the movement schematic is shown in Figure 14.
The attitude tracking results in the electromagnetic disturbance environment are shown in Figure 15. It can be concluded from the experiments on the attitude tracking of the sensors in the dynamic disturbance environment that the tracking results based on the fusion of inertial navigation and electromagnetic navigation have high accuracy with attitude tracking error less than 0.012 rad in the case of non-electromagnetic disturbance. In the case of magnetic field disturbance in the phases of 18 s-34 s and 47 s-58 s, the fusion tracking system performs autonomous navigation with inertial measurement units, and its measurement accuracy decreases with time. In the long time electromagnetic disturbance state experiments, the output results of the attitude fusion tracking system in the direction of the attitude around the x-axis and y-axis can be corrected by accelerometer measurements in the direction of gravity, while in the rotational attitude around the z-axis, there is a more apparent cumulative error because it is difficult to correct, so in the attitude tracking, the tracking error is less than 0.061 rad in the direction of rotation around the x-axis and y-axis, and the maximum tracking error is 0.127 rad in the z-axis rotation direction. The reliability of the fusion method based on inertial navigation and electromagnetic navigation and the feasibility of real-time detection of the intraoperative movement of target organs by the catheter-type fusion tracker were verified through the measurement experiments of the fusion tracking system under dynamic and static conditions, respectively, in the interference environment.

VI. CONCLUSION
This paper proposes a method of inertial navigation and electromagnetic navigation fusion for intraoperative target organ movement perception. And a catheter-type target organ fusion tracker is designed and processed, which can directly VOLUME 9, 2021 measure target organ movement through the human body's natural cavity under non-invasive or minimally invasive conditions. The fusion of inertial navigation and electromagnetic navigation improves the response speed and robustness of intraoperative target organ movement tracking. The static error experiment of the inertial measurement unit is carried out to eliminate its bias error. Meanwhile, a fusion method of inertial navigation and electromagnetic navigation of intraoperative target organs based on extended Kalman filter is proposed. In the case of no electromagnetic disturbance, the fusion tracking system uses electromagnetic positioning to update the inertial measurement unit to avoid the cumulative drift. And using the inertial measurement unit to synchronize the data between two updates of the electromagnetic positioning data improves the electromagnetic positioning refresh rate. In the case of electromagnetic disturbance, the update of electromagnetic positioning data is stopped. The inertial measurement unit is used for independent navigation to reduce the impact of magnetic field disturbance on the output results. The dynamic and static verification experiments conducted in the simulated intraoperative environment show that the maximum tracking errors of displacement and attitude are 2.75 mm and 0.127 rad, respectively, under severe electromagnetic disturbance, and 0.94 mm and 0.011 rad, respectively under non-electromagnetic disturbance conditions. The results show that the intraoperative tracking method based on the fusion of inertial navigation and electromagnetic navigation is feasible and can achieve the tracking measurement of intraoperative target organ movement and attitude within the range of clinically permissible error.