Position Estimation of an Epicardial Crawling Robot on the Beating Heart by Modeling of Physiological Motion

Abstract

HeartLander, a small mobile robot designed to provide treatments to the surface of the beating heart, overcomes a major difficulty of minimally invasive cardiac surgery, providing a stable operating platform. This is achieved inherently in the way the robot adheres to and crawls over the surface of the heart. This mode of operation does not require physiological motion compensation to provide this stable environment; however, modeling of physiological motion is advantageous in providing more accurate position estimation as well as synchronization of motion to the physiological cycles. The work presented uses an Extended Kalman Filter framework to estimate parameters of non-stationary Fourier series models of the motion of the heart due to the respiratory and cardiac cycles as well as the position of the robot as it moves over the surface of the heart. The proposed method is demonstrated in the laboratory with HeartLander operating on a physiological motion simulator. Improved performance is demonstrated in comparison to the filtering methods previously used with HeartLander. The use of detected physiological cycle phases to synchronize locomotion of HeartLander is also described.

I. Introduction

HeartLander is a small mobile robot designed to provide minimally invasive therapies to the heart. Access to the heart is gained through a subxiphoid skin incision and a small incision to the pericardium. HeartLander then attaches to the surface of the heart, the epicardium, inside of the pericardium. Because HeartLander attaches itself to the surface of the heart there is no need for cardiac immobilizers or for active compensation of organ motion to provide a stable operating platform.

HeartLander, shown in Fig. 1, has two “feet” which attach to the epicardium using suction. Flexible drive wires control the distance between the two feet. Alternating suction between the feet and extending/retracting the wires causes HeartLander moves in an inchworm-like manner. A flexible tether connects the suction lines, drive wires, and a 6-DOF magnetic tracking sensor (microBIRD, Ascension Technologies, Burlington, VT) from HeartLander to offboard control hardware, allowing the robot to be small and robust enough to operate in this challenging environment.

Fig. 1.

Physiological motion simulator. (Inset) HeartLander epicardial crawling robot.

Previous work has demonstrated the ability of HeartLander to access the anterior, posterior, and lateral surfaces of the heart, and to position itself accurately while the heart is beating [1]. This work used a low-pass filter to remove the heartbeat motion and a series of notch filters to remove the respiration component from position data recorded from the tracking sensor to allow for effective targeting and control. Previous work has also shown that the variation in intrapericardial pressure applied to HeartLander due to the respiratory and cardiac cycles significantly affects locomotion efficiency [2]. In this study accelerometers placed on the chest of a porcine model were used to detect the physiological phases and to time locomotion to coincide with periods of minimum interpericardial pressure, thereby maximizing efficiency.

Although the previous techniques met with success in the operation of HeartLander, there remains much more information that can be extracted from the magnetic tracker already incorporated within the robot, including information about the motion of the heart due to the physiological cyclical movement, which is currently lost by the filtering techniques previously used.

Considerable research has been conducted on modeling and prediction of the motion of the heart for motion compensation in beating-heart surgery. Points of interest on the heart have been tracked using generalized linear predictors [3], model-based control based on previous heartbeat cycles [4], and linear parameter variant FIR models [5]. Fourier series models have also been used to describe the periodic motion of the heart [6], with parameters sometimes estimated using least mean squares [7] or an Extended Kalman Filter (EKF) [8–9]. All of these methods for modeling or predicting the motion of the heart are intended to enable active motion compensation, providing a stable operating environment by moving surgical instruments in unison with the heart.

With a stable operating environment inherently provided by HeartLander, the goal of modeling physiological motion becomes localizing the robot as it moves over the surface of the heart. The work presented extends the EKF framework proposed in [9], in which physiological motion is modeled using time-varying Fourier series, to include a model of the motion of HeartLander. Combining these models enables more accurate estimates of the position of HeartLander on the surface of the heart, overcoming the lag inherent in the previous filtering techniques, simplifying the detection of physiological phases for motion synchronization, and providing an adaptive framework that is able to quickly overcome unmodeled disturbances.

II. Methods

A. Physiological Motion Simulator

In order to run experiments in the laboratory a physiological motion simulator was developed which emulates the motion encountered in surgical interventions. The simulator, shown in Fig. 1, consists of a beating heart phantom (Model 1283, The Chamberlain Group, Great Barrington, Mass.) affixed to a linear slide. The heart model simulates the deformation of the surface of the heart during the cardiac cycle using pneumatic actuation and allows for three heartbeat frequencies.

The linear portion of the motion phantom approximates the gross displacement of the heart due to the respiratory cycle. The slide is positioned such that its motion is along the inferior-superior axis, which is the dominant component of motion due to the respiratory cycle [10]. Frequency and amplitude of the simulated respiration are controllable.

Position data of HeartLander on the anterior surface of the beating heart model with both simulated respiration and cardiac cycles is shown in Fig. 2. Position data was collected using the microBird magnetic tracing sensor which has reported translational and angular accuracies of 1.4 mm and 0.5° respectively. The respiration component was set to an amplitude of 4 mm peak-to-peak at a frequency of 0.25 Hz. Spectral analysis of the simulated physiological motion, also shown in Fig. 2, showed the dominant frequency of the cardiac motion was approximately 1.16 Hz with multiple harmonics.

Fig. 2.

(Top) Inferior-Superior (IS) axis position of HeartLander on the anterior surface of the beating heart/respiration phantom. (Bottom) Power spectrum of IS position data. The respiration motion is composed of a single harmonic while the cardiac motion contains multiple harmonics.

B. Physiological Motion Model

The motion of the robot in each direction due to physiological cycles is modeled as a combination of two Fourier series, one for the cardiac component and the other for the respiration component, as in [9]. The translational position of the robot, F, is defined as the sum of Fourier series models of the respiratory component, R, and cardiac component, C, and a DC offset term E. The model of robot motion due to the physiological cycles in each direction is given as:

$$F=R+C+E$$ (1)

$$R=\sum _{n=1}^{H_r}\left[a_{n}sin\left(n\sum _{i=0}^k{\omega }_{r_i}{\delta }_{t_i}\right)+b_{n}cos\left(n\sum _{i=0}^k{\omega }_{r_i}{\delta }_{t_i}\right)\right]$$ (2)

$$C=\sum _{n=1}^{H_c}\left[c_{n}sin\left(n\sum _{i=0}^k{\omega }_{c_i}{\delta }_{t_i}\right)+d_{n}cos\left(n\sum _{i=0}^k{\omega }_{c_i}{\delta }_{t_i}\right)\right],$$ (3)

where H_r and H_c are the number of respiratory and cardiac harmonics, ω_r and ω_c are the respiratory and cardiac base frequencies, i is the time step index, k is the current time step index, and δt is the time step duration. Fourier series parameters for respiratory motion are a and b, and c and d for cardiac motion.

The DC offset component of the motion model, E, is the mean position of the point on the heart to which HeartLander is attached under physiological motion. This can also be interpreted as the location of HeartLander on the stationary heart. This interpretation of robot position allows the position of HeartLander to be registered to solid models created using preoperative medical imaging.

C. Robot Motion Model

HeartLander is controlled by extending or retracting drive wires attached to the left and right side of the front foot. The robot coordinate frame is chosen such that it coincides with the magnetic tracking coil at the midpoint of the two drive wires, with its x-axis pointing forward, y-axis left, and z-axis downward. With this choice of coordinate frame the linear velocity of the front foot, U, is then the average velocity of the drive wires, v_L and v_R, in the x-direction. Becasue the motion models of the physiological cycles are constructed in world coordinates, the front foot velocity is transformed into world coordinates. Quaternion format is used to represent the orientation of the magnetic tracker. Front foot velocity, Ė, is calculated by transforming the front foot velocity in robot coordinates, U, by projecting it along the robot x-axis in world coordinates.

$$U=\frac{v_L+v_R}{2}$$ (4)

$$\stackrel{.}{E}=\left[\begin{array}{c}{\stackrel{.}{E}}_x\\ {\stackrel{.}{E}}_y\\ {\stackrel{.}{E}}_z\end{array}\right]=\left[\begin{array}{c}{q_0}^2+{q_1}^2-{q_2}^2-{q_3}^2\\ 2(q_1{q}_2+q_0{q}_3)\\ 2(q_1{q}_3-q_0{q}_2)\end{array}\right]U$$ (5)

D. Kalman Filter Implementation

The state vector of the Kalman Filter is composed of Fourier series parameters for respiration, a and b, and cardiac motion, c and d, robot location and velocity, E and Ė, respiratory and cardiac frequencies, ω_r and ω_c, and quaternion orientation, q. The total number of states which define the system varies depending on the number of harmonics used in the cardiac and respiration Fourier series. In our implementation the number of harmonics in each direction is allowed to differ and is set in initialization.

$$H_r=\left[\begin{array}{c}{H}_{r_x}\\ H_{r_y}\\ H_{r_z}\end{array}\right],H_c=\left[\begin{array}{c}{H}_{c_x}\\ H_{c_y}\\ H_{c_z}\end{array}\right]$$ (6)

In the prediction step of the EKF all state parameters are assumed to be static except for robot location and velocity, E and Ė. The estimated robot velocity is calculated as in (5), and the robot position estimate is given by:

$$E_k=E_{k-1}+{\stackrel{.}{E}}_{k-1}\delta t_{k-1}.$$ (7)

The measurement vector, y(k), used in the update step of the EKF is composed of 6-DOF position from the magnetic tracking system and the cardiac and respiratory frequencies. In the current implementation the respiration and cardiac frequencies are constant and known; however, they are included for future clinical usage where electrocardiogram and ventilator measurements are available.

$$y(k)={\left[\begin{array}{ccccccccc}{\omega }_r& {\omega }_c& x& y& z& q_0& q_1& q_2& q_3\end{array}\right]}^T$$ (8)

E. Phase Estimation and Synchronization

Determination of the phases of the physiological cycles can be calculated because of the Fourier series used to model the cycles. For every harmonic the phase shift, φ_n, can be calculated using their respective Fourier parameters.

$${\phi }_{r_n}=\text{atan2(}-a_n,b_n\text{)}$$ (9)

$${\phi }_{c_n}=\text{atan2(}-c_n,d_n\text{)}$$ (10)

The physiological phase values are then calculated using the notation defined by Shechter et al. [10]. In this notation the cardiac cycle is parameterized with systole occurring in the range from 0 to 0.42 and diastole occurring in the range from 0.42 to 1. The respiration cycle is parameterized with expiration occurring in the range −1 to 0 and inspiration in the range from 0 to 1. Using this notation the respiratory and cardiac phase values, θ_r and θ_c, are estimated using the first-order phase shifts.

$${\theta }_r=\frac{\mathit{mod}\left({\sum }_{i=0}^k{\omega }_{r_i}{\delta }_{t_i}+{\phi }_{r_1},2\pi \right)}{\pi }-1$$ (11)

$${\theta }_c=\frac{\mathit{mod}\left({\sum }_{i=0}^k{\omega }_{c_i}{\delta }_{t_i}+{\phi }_{c_1},2\pi \right)}{2\pi }$$ (12)

Using the estimated phase values, motion of the robot can then be restricted to acceptable phase ranges of each cycle. Ranges in which HeartLander achieves maximum locomotion efficiency have been established as the final three quarters of expiration, −0.75 to 0, for respiratory phase and half of the cardiac cycle centered about end of diastole, 0.17 to 0.67 [1].

III. Results

A. Physiological Motion Estimation

Filter performance was first tested with HeartLander remaining stationary on the anterior surface of the beating heart phantom with the physiological motion simulator in the configuration stated in section II-A. All state parameters were initialized to zero except for the nine states in the measurement vector which were initialized to the measurement at the initial time step. Five harmonics where used in each direction for the cardiac motion, while only a single harmonic was used in the IS-direction for respiration, with no components in the other directions.

The process covariance matrix in the prediction step of the EKF was initialized as a diagonal matrix containing σ_ω² = 10⁻⁹ for the frequency components, σ_E² = 10⁻⁶ for the robot position components, σ_Ė² = 10⁻⁶ for the robot velocity components, σ_q² = 10⁻² for the quaternion pose components, σ_r² = 10⁻⁶ for the respiratory parameters, and σ_c² = 10⁻⁶ for the cardiac parameters.

The measurement covariance matrix in the update step of the EKF was initialized as a diagonal matrix containing σ_pos² = 10⁻² for the position measurements, σ_qmeas² = 10⁻² for the quaternion pose measurements, and σ_ωmeas² = 10⁻³ for the frequency measurements. While measurements of cardiac and respiratory frequency would be available in the clinical setting because of forced respiration and electrocardiogram, in the laboratory setting these measurements are simulated as the frequencies are controllable.

A short section of the filter performance is shown in Fig. 3. The filter quickly converges to follow the ground truth position of the robot; however, the estimated position of the robot on a stationary heart, E, requires approximately two seconds to converge. In order to ensure that the Fourier series are accurately capturing the cardiac and respiratory motions the separated motions were reconstructed from the estimated state using (2) and (3), and are shown in Fig. 3.

Fig. 3.

(Top) IS axis measured and estimated position. HeartLander estimated position quickly converges. (Bottom) Reconstruction of respiration and cardiac cycles from estimated Fourier series parameters.

B. Robot Motion Estimation

After the functionality of the filter was confirmed for tracking HeartLander while stationary, motion trials were conducted on the beating heart phantom, shown in Figs. 4–5. Once again the motion simulator was configured as stated in section II.A: all parameters were initialized to zero except for those initialized to the measurement at the first time step. Again the filter required approximately two seconds to converge on the position of the robot. In the trial a step was executed at approximately 10 seconds. The motion model of the robot incorporated into the EKF allowed the filter to accurately track the position of the robot for the duration of the 0.5 second step. In the time after the initial step, and prior to the next extension step which occurs at approximately 18 s, the suction of the front foot was engaged, the suction of the back foot was disengaged, and the back foot was retracted towards the front foot. During this time the front foot slipped causing a deviation of the estimated robot position from the measured robot position at approximately 15 s. This slip caused the estimated cardiac and respiratory cycles to briefly diverge, but they quickly reconverged on the true physiological cycles.

Fig. 4.

Measured and estimated position while HeartLander moves over the beating heart phantom in (Top) X-direction, (Middle) Y-direction, and (Bottom) Z-Direction.

Fig. 5.

Reconstruction of respiration and cardiac cycles from estimated Fourier Series Parameters in (Top) X-direction, (Middle) Y-direction, and (Bottom) Z-Direction.

C. Filter Performance Comparison

In order to compare the performance of the EKF filter to other filtering techniques, measurement data from the movement trials were post processed using a series of filters used previously with HeartLander. A third-order Chebyshev type II low-pass filter with a 20-db cutoff frequency at 1.0 Hz was used to remove the heartbeat from the tracking data. The respiration component of the motion was removed using a series of two second order infinite impulse response notch filters. The first notch filter used a notch frequency at the primary respiration harmonic, 0.25 Hz, while the second filter had a notch frequency at twice the primary harmonic, 0.5 Hz [1].

A comparison of the low-pass/notch filtered position and EKF filtered position estimates are shown in Fig. 6. While the two filtering techniques show similar results when HeartLander remains stationary, the difference in performance is clear during the portion of the signal when HeartLander is stepping. The series of low-pass/notch filters introduced a lag of approximately 0.6 seconds while the EKF filter responded with no noticeable lag.

Fig. 6.

Comparison of EKF estimated position with position estimate from a series of low-pass and notch filters. The cascaded filters introduce a lag in estimated position.

D. Locomotion Synchronization

The respiration and heartbeat synchronization method introduced in section II-E was evaluated by calculating the percentage of steps which fell within previously determined acceptable phase values for HeartLander motion trials on the physiological motion simulator. In each trial HeartLander was placed on the anterior surface of the beating heart model and completed 150 steps under joystick control over the surface of the heart. In the synchronized trial step events were only allowed when the cardiac and respiration phase values fell within the acceptable range. In the unsynchronized trial no limits were placed on when step events can occur.

The external magnetic tracking unit was oriented that its y-axis coincided with the simulated breathing motion and therefore the Fourier parameters for the y-direction were used to determine respiration phase value. The principal component of cardiac motion was observed to occur in the y-direction for the navigable portions of the beating heart simulator and was also used to determine cardiac phase value.

Ground truth physiological phase values for when each step occurred were determined by post processing the magnetic tracker position. In order to determine respiration phase the end of expiration of each cycle was identified on position data which was filtered with a zero-phase-lag low pass filter to remove the cardiac component. Cardiac phase was determined by identifying the end of diastole of position data which was filtered with zero-phase-lag low and high pass filters to remove the respiration component and sensor noise.

Results from the synchronization trials are shown as normalized histograms in Fig. 7. Each step event was binned using the ground truth labeled cardiac and respiration phase values. The acceptable phase ranges for stepping are shown as vertical dashed lines on the histograms. For the unsynchronized trials the distribution of the steps over the physiological cycles are near uniform with 35% of the steps falling in the acceptable respiration phase range and 63% falling within the acceptable cardiac phase range. With synchronization implemented 94% of the steps fall within the acceptable respiration phase range and 77% falling within the acceptable cardiac phase range.

Fig. 7.

Normalized histograms of physiological cycle phase values for (Top Left) respiration phase without synchronization, (Top Right) cardiac phase without synchronization, (Bottom Left) respiration phase with synchronization, and (Bottom Right) cardiac phase with synchronization.

IV. Discussion

The results obtained clearly show that in the absence of robot motion the filter is able to rapidly lock on to the dynamics of the physiological motion simulator as indicated by the reconstructed of the separated cycles in Fig. 3. This falls in line with the work presented in [9] in which a point of interest on the heart is tracked using an EKF with visual feedback used as the position measurements. The work also shows the feasibility of using the filter to track the position of the robot, as well as the physiological motion, throughout the operation. While the respiratory component of motion in our simulator is constant for the entire heart, the cardiac component of motion varies in amplitude and direction as the robot moves. As the robot moves the filter quickly adjusts the parameters of the Fourier series to account for these changes. The EKF filter improves localization over previously used filtering techniques, giving an approximately zero-lag estimate in the position of the robot.

Detection of physiological phases for step synchronization was quite successful (94%) for the respiratory motion, while less so (77%) for cardiac motion. The respiration cycle in the motion simulator is much smoother, slower, and larger in amplitude than the cardiac cycle, making its detection much easier. Refinement of the physiological phase detection methods is ongoing.

Future work is planned to extend and refine the methods presented for localization of HeartLander. Laboratory tests are planned to test the targeting accuracy of HeartLander on the beating heart phantom using the EKF for localization and registration. In these tests the beating heart phantom is registered to a solid model of the stationary heart prior to testing. Target sites for treatment are identified on the solid model and are used as goals for semi-autonomous navigation of HeartLander.

Laboratory testing is also planned on the effects of transforming the EKF framework from working in the global frame to a heart-centered reference frame for the cardiac component of motion. In this framework the respiration motion will still be modeled in the global frame; however, the cardiac motion will be modeled using spherical coordinates centered within the heart. This frame of reference may prove to be more natural in describing the deformation of the heart as HeartLander navigates the entire surface of the heart.

Future work will also include testing the feasibility of the methods in a porcine model, where it is expected that a respiratory component of heart motion will be present in all directions and will include a rotational component, the cardiac motion will have considerably higher amplitude [10], and physiological motion may include non-periodic components and have variable frequencies.