Space-Time Localization and Registration on the Beating Heart

Abstract

This paper presents a framework for localizing a miniature epicardial crawling robot, HeartLander, on the beating heart using only 6-degree-of-freedom position measurements from an electromagnetic position tracker and a dynamic surface model of the heart. Using only this information, motion and observation models of the system are developed such that a particle filter can accurately estimate not only the location of the robot on the surface of the heart, but also the pose of the heart in the world coordinate frame as well as the current physiological phase of the heart. The presented framework is then demonstrated in simulation on a dynamic 3-D model of the human heart and a robot motion model which accurately mimics the behavior of the HeartLander robot.

I. Introduction

Due largely to the improvement in patient outcomes, minimally invasive cardiac therapies have become increasingly appealing in comparison to standard invasive cardiac surgeries. Although these methods provide many advantages for the patient, they possess a number of challenges due to the types of instruments and access points used. With no direct line of sight to the operation field, real-time medical imaging technologies, including magnetic resonance imaging (MRI) [1], fluoroscopy [2], and ultrasound [3], are often used to provide visual feedback for navigation.

Image guided surgery, which uses pre-operative medical images to provide a virtual view of the operating site, is also often used for visual feedback. In this framework the surgical device is often tracked using an electromagnetic position sensor, localized in and registered to the virtual model, and displayed in the visualization [4]–[6]. These methods, while possessing considerable power, can often be negatively affected by the dynamic nature of the heart. While the model of the heart is treated as a static body, deformations of almost 30 mm occur due to the physiological cycles of heartbeat and respiration [7]. For tools and surgical robots that move relatively freely in the cardiothoracic cavity, or inside the heart, accounting for this periodic motion poses a significant challenge due to the changing contact constraints. However, for a robot which adheres to the surface of the heart, this motion may be leveraged to improve localization and registration.

HeartLander, shown in Fig. 1, provides therapies to the heart by adhering to and moving over the epicardial surface. The robot is a miniature inchworm-style robot that adheres to the epicardial surface, inside the pericardium, using suction, and moves by extending and retracting drive wires connecting the two feet while alternating suction. Access to the heart is gained through a subxiphoid skin incision and a small incision in the pericardium at the apex of the heart. Previous work has successfully demonstrated the ability to access the pericardium, move over the surface of the heart, and reach targets accurately [8].

Fig. 1.

The HeartLander robot.

The current methods used for localizing HeartLander on the surface of the heart use several approximations which limit accuracy. Position measurements of the robot come from a 6-degree-of-freedom electromagnetic tracking sensor (microBIRD, Ascension Technology) embedded in the front foot of the robot. The surface of the heart, as previously mentioned, is a dynamic environment which undergoes periodic deformation due to both the heartbeat and respiration cycles. Because the system currently uses a static model of the heart generated from pre-operative CT images, these deformations are treated as noise and filtered out to estimate of the mean location of the robot [8]. This mean location is treated as the position of the robot on the surface of the static heart model. Registration between the map frame and measurement frame is found using markers placed on the chest wall which are identified in each frame. Transformations between the frames are then found using least squares methods.

Although HeartLander has shown considerable success in live animal testing, there remains the possibility of improving the accuracy of robot positioning. If, instead of rejecting the periodic deformations of the surface of the heart as noise, these motions, which vary over the surface of the heart, are treated as features which yield information about the current robot position on the heart, we may be able to improve localization accuracy. Assuming that we possess a map which fully defines how the surface of the heart moves through the physiological phases, this work presents a method for using the motion of the surface to localize on the surface. The concept was demonstrated in 2-D in [9]. In the present work, it is implemented and demonstrated in simulation on a 3-D surface model of the heart.

II. Related Work

Methods for registering and localizing in and around the heart are highly dependent upon the instrument or robot being used. Tully et al. use an electromagnetic tracker located at the distal end of a highly articulated snake-like robot to first the shape of the robot [4], and then use inequality constrained kalman filtering to correct registration and localization parameters when the model is found to violate geometric constraints, such as intersecting the pre-operative surface model [5].

Therapies which target the endocardium using intracardiac echo (ICE) catheters use ultrasound images to localize. One method generates a point cloud by extracting heart surface points from 2-dimensional ultrasound images at the catheter tip by rotating the catheter about its longitudinal axis. After sufficient points have been collected the point cloud is registered to a pre-operative model of the left atrium using an iterative closest point (ICP) method [10]. Another method uses a particle filter to recursively estimate the pose of the ICE catheter in the left atrium [6]. In this work the probability of a catheter tip pose is calculated by comparing virtual ultrasound images constructed from a pre-operative model with the actual ultrasound images. The predicted catheter pose is then determined as the weighted sum of the particles, and the registration parameters are then calculated using this pose estimate and measurements from an electromagnetic position tracker.

Several factors differentiate our current work from those presented. First, and most obvious, is that they all work from a static map of the environment so that the periodic motion of the heart is either rejected as noise or accounted for in the registration parameters. While the use of dynamic maps of the entire cardiothoracic cavity may be infeasible at the present time, considerable work has been done on generating models of the deforming heart including representations using splines [11], [12], finite element models [13], and statistical models [14].

A second difference is the measurements, or constraints, available in each system. In [4] the entire shape of the robot is used to constrain the robot with respect to the map. In [6], [10] two separate measurements are used to localize and estimate registration parameters. First the ultrasound images are used to localize within the map, then the position measurements from the electromagnetic tracker are used to register the model to the real world.

In our case, the only measurement available is 6-DOF pose from the electromagnetic tracker. Using only this measurement, along with the constraint that the robot must be on the surface of the heart, we estimate localization and registration parameters simultaneously.

III. Methods

A. Heart Surface Model

The work presented relies on possessing complete maps which describe the periodic motion of a surface. For our purposes, a map of a surface takes the form:

$$M=[\overrightarrow{x}(\phi ),\overrightarrow{n}(\phi )],$$ (1)

where φ ∈ (0, 1] is the phase, x⃗ are the Cartesian coordinates in map frame, and n⃗ are the surface normals.

In order to develop and test the following methods used for localizing on such surfaces, a surface model of a beating heart was derived from a spline-based model of the epicardium of a human subject [11]. The heart surface model is shown at four different phases of the cardiac cycle in Fig. 2.

Fig. 2.

Model of the epicardial surface at cardiac phases, φ, of (a) φ = 0, (b) φ = 0.25, (c) φ = 0.5, and (d) φ = 0.75. The red background in (b)–(d) denotes the shape of the heart at φ = 0.

B. Simulated System

The simulated system is represented by a given map, M, and the following state vector:

$$s_t={\left[\begin{array}{ccccc}{\overrightarrow{x}}_r^m(\phi )& {\overrightarrow{q}}_r^m(\phi )& \phi & {\overrightarrow{x}}_m^w& {\overrightarrow{q}}_m^w\end{array}\right]}^T,$$ (2)

where ${\overrightarrow{x}}_r^m(\phi )$ is the location of the robot on the surface in map coordinates, ${\overrightarrow{q}}_r^m(\phi )$, is the quaternion orientation of the robot in map coordinates, φ is the current phase, ${\overrightarrow{x}}_m^w$, is the location of the map in world coordinates, and ${\overrightarrow{q}}_m^w$ is the quaternion orientation of the map in world coordinates. Using this representation, the pose of the robot in the world frame is then:

$${\overrightarrow{x}}_r^w(\phi )={\overrightarrow{q}}_m^w{\overrightarrow{x}}_r^m(\phi ){\overrightarrow{q}}_m^{w^{-1}}+{\overrightarrow{x}}_m^w$$ (3)

$${\overrightarrow{q}}_r^w(\phi )={\overrightarrow{q}}_m^w{\overrightarrow{q}}_r^m(\phi )$$ (4)

The phase of the system is advanced by:

$${\phi }_t={\phi }_{t-1}=\omega dt,$$ (5)

where the velocity, ω, is assumed to be constant. The location and orientation of the map in world coordinates are also assumed to be constant.

Control inputs to the robot, u_t = (θ_t, d_t), rotates the robot about the surface normal through an angle, θ_t, and moves the robot along the surface a distance d_t in the robot’s x-direction. Using this framework, we wish to estimate the current state vector, s_t, using a particle filter.

C. Particle Filter Overview

This section gives a brief overview of the particle filter algorithm implemented in this work. A more in-depth treatment of the algorithm and related topics an be found in [15]. The particle filter is a nonparametric Bayes filter which represents the posterior distribution by a set of random samples drawn from the posterior. These samples of the posterior, or particles, are represented as:

$$S_t=[s_t^1,s_t^2,\dots ,s_t^n],$$ (6)

where each particle, $s_t^i$, is a hypothesis of the true state of the system at time t. The set of particles, S_t, then approximates the belief state, where the likelihood for each state hypothesis is proportional to its Bayes posterior:

$$\mathit{bel}(s_t)~p(s_t\mid z_{1:t},u_{1:t}),$$ (7)

where z_1:t are all past measurements, and u_1:t are all past control inputs. Because of this distribution the more probable regions of the state space will be more densely populated by particles. The particle filter, being a Bayes filter, recursively constructs the belief of the current state from the previous belief state. The general Bayes filter consists of two steps: prediction and correction. In the prediction step the predicted belief, $\overline{\mathit{bel}}(s_t)$, is determined by combining the transition probability using the current control input, u_t, and the prior belief, bel(s_t−1).

$$\overline{\mathit{bel}}(s_t)=\sum _{s_{t-1}}p(s_t\mid u_t,s_{t-1})\mathit{bel}(s_{t-1})$$ (8)

The predicted belief is then updated by incorporating the current measurement, z_t.

$$\mathit{bel}(s_t)=\eta p(z_t\mid s_t)\overline{\mathit{bel}}(s_t)$$ (9)

Because the particle filter represents the belief distribution by random samples from the posterior it differs slightly in form from the general Bayes filter, yet it is based on similar principles. In the prediction step, each particle, $s_{t-1}^i$, i ∈ (1, N), where N is the number of particles, is advanced using the motion model. The advanced particle is drawn from the state transition distribution.

$$s_i^t~p(s_t\mid u_t,s_{t-1}^i)$$ (10)

The set of particles generated by incorporating the state transitions is the filter’s representation of the predicted belief distribution, $\overline{\mathit{bel}}(s_t)$. In order to incorporate the current measurement, z_t, importance factors, or weights $w_t^i$, for each particle is calculated as:

$$w_t^i=w_{t-1}^{i}p(z_t\mid s_t^i)$$ (11)

The updated particle set, S̄_t, consists of each particle along with their respective weight. This set of particles represents the predicted distribution $\overline{\mathit{bel}}(s_t)$.

$${\overline{S}}_t={\overline{S}}_t+\langle s_t^i,w_t^i\rangle$$ (12)

In order to force the particle set from the predicted distribution, $\overline{\mathit{bel}}(s_t)$, to the posterior distribution, bel(s_t), the algorithm uses importance sampling. In importance sampling, N particles are drawn with replacement from the predicted set, where the probability of of drawing each sample is proportional to its weight, $w_t^i$. After resampling each of the particles weight is set to one, and the particle set S_t is distributed according to the posterior, bel(s_t).

D. Particle Filter Implementation

1) Particle Initialization

In order to reduce the size of the space spanned by the state vector, which reduces the number of particles to sufficiently cover this space, it is assumed that at initialization an estimate of the position and orientation of the heart, as well as a single measurement of the pose of the robot in the world frame, are available. Using this information, along with confidence bounds on the position and orientation of the heart, a fixed number of particles, N, are generated such that each particle’s registration parameters lie within the confidence bounds of the initial estimate and the location and orientation of the robot on the surface of the heart, when transformed to world coordinates, would produce the initial measurement.

2) Motion Model

Incorporation of the control inputs in the state transition distribution is achieved through use of a robot motion model. As previously described, the control input u_t = (θ_t, d_t), rotates the robot about the surface normal through an angle, θ_t, and moves the robot along the surface a distance d_t in the robot’s x-direction. In order to return a sample from the distribution $p(s_t\mid u_t,s_{t-1}^i)$, noise is injected into the motion model. The angle and distance each particle moves on the surface is:

$${\theta }_t^i={\theta }_t+{\delta }_{\theta }^i,$$ (13)

$$d_t^i=d_t+{\delta }_u^i,$$ (14)

where ${\delta }_{\theta }^i$ and ${\delta }_d^i$ are random samples drawn from $\mathcal{N}(0,{\sigma }_{\theta }^2)$ and $\mathcal{N}(0,{\sigma }_d^2)$ respectively. Also, the same method is used to advance the phase of each particle.

$${\phi }_t^i={\phi }_{t-1}^i+(\omega +{\delta }_{\omega }^i)dt,$$ (15)

where ${\delta }_{\omega }^i$ is a random sample from $\mathcal{N}(0,{\sigma }_{\omega }^2)$. The velocity of the cardiac phase, φ, is assumed to be known as this information can be measured in the clinical setting using an electrocardiogram (ECG).

The registration parameters, ${\overrightarrow{x}}_m^w$ and ${\overrightarrow{q}}_m^m$, although stationary, also have noise added to ensure coverage of the space. The translational component of the registration parameter is advanced by:

$${\overrightarrow{x}}_{m_t}^{w^i}={\overrightarrow{x}}_{m_{t-1}}^{w^i}+{\delta }_{x_m^w}^i{\overrightarrow{v}}_x^i,$$ (16)

where ${\overrightarrow{v}}_x^i$ is a unit vector uniformly sampled from , and ${\delta }_{x_m^w}^i$ is a random sample drawn from $\mathcal{N}(0,{\sigma }_{x_m^{\omega }}^2)$. While the rotational registration component is advanced by:

$${\overrightarrow{q}}_{m_t}^{w^i}={\overrightarrow{q}}_{v_q}^i{\overrightarrow{q}}_{m_{t-1}}^{w^i},$$ (17)

$${\overrightarrow{q}}_{v_q}^i=\left[\begin{array}{c}cos\frac{{\alpha }^i}{2}\\ sin\frac{{\alpha }^i}{2}{\overrightarrow{v}}_q^i\end{array}\right],$$ (18)

where ${\overrightarrow{v}}_q^i$ is a unit vector uniformly sampled from , and αⁱ is a random sample drawn from $\mathcal{N}(0,{\sigma }_{\alpha }^2)$.

3) Measurement Model

The measurement assumed in this work is the 6-DOF pose of the robot in world frame. This measurement, z_t, is composed of a position vector, x⃗_z, and a quaternion orientation, q→_z, which are constructed from the ground truth robot state vector as in (3)–(4) and corrupted with Gaussian noise. The weight of each particle is calculated as the probability of the measurement given the particle state. The particle weight is given by:

$$w_t^i=w_{t-1}^{i}p(z_t\mid s_t^i)=w_{t-1}^{i}p({\overrightarrow{x}}_z\mid s_t^i)p({\overrightarrow{q}}_z\mid s_t^i)$$ (19)

$$p({\overrightarrow{x}}_z\mid s_t^i)={\eta }_{x}exp(\frac{\left|\right|{\overrightarrow{x}}_z-{\overrightarrow{x}}_r^{w^i}\left|\right|}{2{\sigma }_x^2})$$ (20)

$$p({\overrightarrow{q}}_z\mid s_t^i)={\eta }_{q}exp(\frac{{\beta }^i}{2{\sigma }_{\beta }^2})$$ (21)

$${\beta }^i=2arccos\left(\mid {\overrightarrow{q}}_z·{\overrightarrow{q}}_r^{w^i}\mid \right),$$ (22)

where βⁱ is the angle between the two rotations and a proper distance metric on SO(3) [16].

4) Resampling

The resampling procedure implemented in the system differs in two ways from the general resampling procedure previously mentioned. First, in order to decrease the risk of losing particle diversity resampling only occurs when the variance of the particle weights is sufficiently large, namely when the following holds:

$$\frac{\mathit{var}(w_t)}{{\overline{w}}_t}>\gamma ,$$ (23)

where w̄_t is the mean particle weight, and γ is a threshold value. The second strategy used to reduce the sampling error is known as low variance sampling [15]. Instead of just drawing independent samples based on each particle’s weight, this method ensures that any particle which has a weight greater than $\frac{1}{N}$, where N is the number of particles, is guaranteed to survive.

IV. Experiments

In order to test the feasibility of using the previously described particle filter framework to estimate localization and registration parameters 100 trials were run in the simulated system. For the trials the heart rate was set to 1.3 Hz. For each simulation a ground truth particle was randomly placed on the surface of the heart with randomly generated registration parameters. At each timestep the ground truth particle was commanded a random motion input such that the maximum robot velocity and path curvature were within the normal HeartLander operating parameters. Simulations were run for 500 time steps, or approximately 15 cardiac cycles.

In the experiment the number of particles was set to be N=2000. The particles were initialized such that each particles registration estimate was within 25 mm and 30° of the initialization estimate, and the phases were uniformly sampled. The motion and observation models as specified in section III-D were used with parameters set to: ${\sigma }_{\theta }^2=0.002\text{radians},{\sigma }_d^2=0.03\text{mm},{\sigma }_{\omega }^2=0.01\text{radians}$ per second, ${\sigma }_{x_m^w}^2=0.25\text{mm},{\sigma }_{\alpha }^2=0.002\text{radians}$. The observation model parameters are set to: η_x = η_q = 1, ${\sigma }_x^2=1.4\text{mm},{\sigma }_{\mathit{beta}}^2=0.009\text{radians}$. The variance resampling threshold was set to be γ = 0.01.

Fig. 3 illustrates the progression of the particle filter through a typical run. The ground truth location of the robot is shown by the large black dot in each image. Fig. 3(a) shows the initialization of the filter, and demonstrates the randomized coverage of our state space. Fig. 3(b)–(g) shows the particle filter after from iterations 10–90, as the particles begins to form clusters around states with high likelihoods, and the most likely clusters become more dense. The reason a few separate clusters form is due to certain geometric symmetries in the surface. Certain distinct regions of the surface under different registration and phase can produce similar world-frame motion, and keeping a diverse set of hypothesis is the correct thing to do for the particle filter. Fig. 3(h) shows the particle filter after 115 iterations. At this point, we have gathered enough observations to greatly reduce the uncertainly in our location, and only a single cluster remain, achieving convergence. From this point onward, the particle filter retains the correct localization until the end of the run.

Fig. 3.

Representative results of localizing on the surface of a beating heart using a particle filter. The ground truth position of the robot is shown by the large black dot. Each particle is represented by a small dot whose color denotes the particle weight. Low weights correspond to blue and high to red, with the color scale spanning the weights of the current particles. Plots correspond to (a) Filter initialization with particles randomly distributed over the surface, (b) after 10 iterations, (c) after 30 iterations, (d) after 40 iterations, (e) after 50 iterations, (f) after 70 iterations, (g) after 90 iterations, and (h) after 115 iterations

To make a prediction for the map-frame location of the ground truth using the particle filter at any time during the run, we take the weighted average of all particles. The reason we use an average of particles rather than simply outputting the single highest-weighted particle is for stability: as the weight on each particle is updated after every observation, the highest-weighted particle is likely to change often, and our localization output will be rather noisy.

We recorded the state estimation error made by the particle filter prediction output at each iteration of a run, and averaged the error over 100 runs to obtain a statistically significant result. Each of the 100 runs are completely independent with independent initializations. The results are shown in Fig 4(a)–(d). The error means and standard deviations observed at the end of the trials were phase error of 0.01 ± 0.01, position error of 0.40 ± 0.21 mm, and registration errors of 0.33 ± 0.14 mm and 0.38 ± 0.16°.

Fig. 4.

Results of localization averaged over 100 runs for (a) phase error, (b) map frame position error, (c) registration position error, and (d) registration orientation error. Mean errors are denoted by solid lines and one standard deviation by dashed lines. Errors for each run are calculated between the ground truth and the weighted average of particles. Position errors are calculated using the euclidean distance, phase and angle errors are absolute differences.

V. Discussion

While the results from the simulations are quite promising, there is significant work remaining in order to advance the presented work into real-world use. Our initialization procedure, which provided an initial estimate of the registration parameters, significantly reduced the number of particles required to adequately cover the state space, yet a large number of particles are still required to ensure convergence. Providing a more accurate estimate of the registration parameters could decrease the number of particles, as could providing bounds on the initial position of the robot on the surface of the heart. This is likely feasible in procedures as HeartLander is generally placed on the anterior surface near the apex of the heart. Further reductions may be achieved by incorporating cardiac phase measurements from an electrocardiogram (ECG).

Improvements to the algorithm may also show improvement over the existing framework. Inherent in the system is an ambiguity in the cause of error. It is not possible to determine if the error is due to an error in the location of the robot on the surface of the heart or in the registration parameters. The particle filter relies on the noise in the motion model to cover the state space and settle at a global minimum. A Rao-Blackwellized particle filter, which updates the registration parameters in a deterministic step, similar to the registration calculation in [6], [10], will likely improve performance.

The major hurdle to overcome is the construction of accurate dynamic pre-operative maps for use in actual interventions. As previously mentioned, there is considerable work in the medical imaging community addressing this and the authors remain confident that such maps will be more readily available in the near future.

VI. Conclusions

The presented work acts as a proof-of-concept for localizing on the beating heart using only a 6-DOF position measurement and a map of the heart. Although implementation in the operating room will require further work, the results produced show that not only is localization of mobile robots on a periodically deforming surface feasible, a well-tuned particle filter provides low-error estimates of the true location within a few hundred iterations, and within a few periods of the cyclical deformation. The motion model and observation model can be modeled with the limited data on currently available robot technologies such as simple 6-degree-of-freedom pose.