Localization of Point-of-Interest Positions on Cardiac Surface for Robotic-Assisted Beating Heart Surgery

Abstract

One of the critical components of robotic-assisted beating heart surgery is precise localization of a point-of-interest (POI) position on cardiac surface, which needs to be tracked by the robotic instruments. This is challenging as the incoming sensor measurements, from which POI position is localized, might be noisy and incomplete. This paper presents two Bayesian filtering based localization approaches to localize POI position online from sonomicrometer measurements. Specifically, extended Kalman filter (EKF) and particle filter (PF) localization algorithms are explored to estimate the state of POI position. The estimations of upcoming heart motion generated by the generalized adaptive predictor, which is demonstrated in the authors’ past work, are also incorporated to generate an improved motion model. The proposed methods are validated with prerecorded in-vivo heart motion data.

I. INTRODUCTION

In the robotic-assisted beating heart surgery, robotic instruments replaces the conventional surgical tools and are under direct control of the surgeon through teleoperation [1]. The tools need to track a fast moving target with very high precision and cancel the relative motion between the surgical site on the heart and the surgical instruments. Thus, the surgeon operates on the heart as if it is stationary.

An essential aspect of the proposed concept is accurate localization of the point-of-interest (POI) on cardiac surface from the noisy sensor measurements, which are gathered by using a position tracking sensor. For precise motion tracking, it is important to provide clean POI position data to the control algorithms. Earlier studies in robotic-assisted heart surgery employed optical [1]–[4] and ultrasonic [5]–[7] position tracking sensors for the localization of POI position. Sonomicrometer is the choice of position tracking sensor in this study just as in the case of authors’ past work [8].

In this paper, a probabilistic formulation of the problem of robust online POI position localization is presented. The problem is studied by utilizing extended Kalman filter (EKF) and particle filter (PF) localization algorithms. Proposed framework aims to localize POI position from the sonomicrometer measurements. First, heart motion is modeled as Brownian motion and then approximated by a harmonic motion model. The estimations of upcoming heart motion generated by the generalized adaptive predictor (introduced in [8]) are also employed for an improved motion model to enhance localization accuracy. The probabilistic measurement models account for the noise in the sonomicrometer data. The proposed methods are evaluated by simulation studies with prerecorded in-vivo heart motion data. This study extends authors’ previous work [9] on probabilistic heart motion measurement with sonomicrometer by employing the adaptive filter predictions in the motion model.

The remainder of this paper is organized as follows. The formulation of the problem of POI position localization from sonomicrometer data is briefly described in Section II. The Brownian, harmonic, and improved motion models as well as the probabilistic measurement model are explained in Section III. The EKF and PF localization algorithms are introduced in Section IV. The results and discussions are presented in Section V. Finally, conclusion and possible future extensions are given in Section VI.

II. PROBLEM FORMULATION

A sonomicrometer accurately measures the distances within the moving soft tissue via ultrasound signals by a set of small piezoelectric crystals. Each sonomicrometer crystal can alternate between transmitting and receiving ultrasound signals. The distance between a pair of transmitting and receiving crystals are computed by measuring the time of flight of the sound wave. In measuring in-vivo heart motion, one crystal is sutured next to the POI and three or more crystal are placed on a rigid base forming a reference coordinate frame. The localization problem is formulated as computing the three-dimensional (3D) configuration of the crystal next to POI (with respect to the reference coordinate frame) from the measured distances between the crystal next to POI and the base crystals [10]. Fig. 1 provides a graphical schematic of the localization problem. This configuration yields ten different distance measurements with half of them being repeated measurements of the others.

Fig. 1.

POI localization problem: Five crystals are placed on a rigid base forming a reference coordinate frame and a sixth crystal is sutured next to POI on a beating heart. The localization algorithms take distance measurements between the POI and the base crystals, and infer POI position with respect to reference coordinate frame.

The instantaneous 3D triangulation method, provided by sonomicrometry sensor system (Sonometrics, Inc. ON, CA) performs online POI localization without any cleaning on the incoming sonomicrometer measurements. The system provides offline filtering. Yet, this is not feasible for in-vivo motion tracking; as sonomicrometer data must be processed online. As the channel measurements are not processed online in instantaneous triangulation, the existing noise in the data are incrementally reflected on the localized POI position as shown in Fig. 2. In this study, Bayesian state estimation algorithms are employed to remedy this problem and localize the POI position in a robust, online fashion.

Fig. 2.

. (a) The online unfiltered and offline filtered sonomicrometer measurements between POI and 4^th base crystal. (b) z-coordinate of the resulting 3D Positions via sonomicrometer triangulation.

III. PROBABILISTIC MOTION AND MEASUREMENT MODELS

Kalman and other Bayesian filters for state estimation employ probabilistic models of the process and the measurement [11]. In this work, 3D POI position is the dynamic system state. Process model represents the POI motion and describes how the current system state evolves from the previous state. Measurement model describes how a sonomicrometer measurement is generated given the system state. This section describes the probabilistic motion and measurement models used in the EKF and PF localization algorithms.

A. Motion Model

Three different process (i.e. POI motion) models are used that provide different levels of a priori information about the heart motion.

1). Brownian Motion Model:

The POI is assumed to be undergoing a Brownian random motion in this model. The state at time t consists of the 3D coordinates of the POI position and is of the form: q_t = [x_t, y_t, z_t]^T. Then, system is modeled as:

$$q_{t+1}=q_t+ϵ,$$ (1)

where ϵ is drawn from a multivariate normal distribution, $\mathcal{N}(0,\Sigma )$ with the process covariance $\Sigma \in {ℝ}^{3\times 3}$ is a diagonal matrix.

2). Harmonic Motion Model:

The heart motion is primarily the superposition of two effects: breathing motion and heart beating motion. Here, by utilizing the quasiperiodic nature of the heart motion [8], POI motion is approximated by a 2^nd-order compact Fourier series with constant offset, which considers only the main mode and first harmonic of breathing and heart beating motions. For a single coordinate of POI position this approximation is given as:

$$\begin{array}{l}{u}_{y_t}=C_0+\sum _{m=1}^2{C}_{b_m}cos\left(m{w}_{b_0}t+{\theta }_{b_m}\right)\\ +C_{h_m}cos\left(m{w}_{h_0}t+{\theta }_{h_m}\right),\end{array}$$ (2)

where C₀, $C_{b_m}$, ${\theta }_{b_m}$, $C_{h_m}$, ${\theta }_{h_m}$ are respectively the constant offset, the corresponding coefficients, and phases of breathing and heart motion components. $w_{b_0}=2\pi f_{b_0}$ and $w_{h_0}=2\pi f_{h_0}$ are the angular frequencies of the main modes of breathing motion and heart beating motion. Fig. 3 shows z-coordinate of POI motion and its harmonic approximation. At time t, the update equations of the heart motion and its harmonic approximation are given by:

$$p_{t+\Delta t}=p_t+\Delta p_t,$$ (3)

$$u_{t+\Delta t}=u_t+\Delta u_t,$$ (4)

where p_t, u_t, Δp_t, $\Delta u_t\in {ℝ}^{3\times 1}$ are respectively the actual heart motion, its harmonic approximation, and their corresponding increments. The above approximation allows the increment, Δp_t, to be also approximated by Δu_t; Δp_t ≈ Δu_t. Then, system is modeled as:

$$q_{t+1}=q_t+\Delta u_t+{ϵ}_r,$$ (5)

where ϵ_r is drawn from a multivariate normal distribution, $\mathcal{N}\left(0,{\Sigma }_r\right)$ with the process covariance ${\Sigma }_r\in {ℝ}^{3\times 3}$ is a diagonal matrix.

Fig. 3.

Shows z-axis of POI motion and harmonic approximation.

3). Generalized Adaptive Predictor as Motion Model:

A possible way to improve motion model is using the one-step estimates obtained by the generalized adaptive predictor, which recursively generates the best estimate of the next observation, given only current and past observations [8]. At time t, the update equation of the generalized motion model can be expressed as:

$$\Delta g_t=g_{t+1}-g_t,$$ (6)

where g_t+1 is the one step prediction generated by the adaptive predictor and g_t = q_t, current state. The process model is given by:

$$q_{t+1}=q_t+\Delta g_t+{ϵ}_g,$$ (7)

where ϵ_g is drawn from $\mathcal{N}\left(0,{\Sigma }_g\right)$ with the process covariance ${\Sigma }_g\in {ℝ}^{3\times 3}$ is a diagonal matrix.

B. Measurement Model

The sonomicrometer is susceptible to a peculiar form of error caused by the crystal geometry, obstruction of ultrasound transmission paths, and ultrasound echo effects [10]. The proposed measurement model captures these possible sources of error.

The ideal (noise-free) sonomicrometer measurements for each of the measurement channels zⁱ are given by the system state and base crystal positions as:

$$\begin{array}{l}{h}_i\left(q_t\right)=\Vert q_t-c_i\Vert ,\\ {\widehat{z}}_t^i=h_i\left(q_t\right),\end{array}$$ (8)

where c_i’s are 3D coordinates of base crystal positions with respect to reference coordinate frame and h_i(q_t) is the distance between the POI crystal and the i^th base crystal at time t. Then the measurement error, ${\alpha }_t=z_t-{\widehat{z}}_t$ yields the sonomicrometer measurement model, where $\left\{z_t^1,\cdots ,z_t^n\right\}$ is the set of sensor data at time t with n is the number of measurements (Fig. 1). The measurement likelihood is given by:

$$p\left({\alpha }_t\right)=p\left(z_t|q_t\right).$$ (9)

The small measurement error caused by crystal geometry can be approximated by a narrow Gaussian noise distribution, $p_{small}\left(z_t|q_t\right)~\mathcal{N}\left(\mu ,{\sigma }^2\right)$ with mean, μ, and variance, σ². The random unexplained noise, which describes the ultrasound echoes, is modeled using a uniform distribution, $p_{rand}\left(z_t|q_t\right)~\mathcal{U}\left({\alpha }_{min},{\alpha }_{max}\right)$, spread over the entire error range. These two different distributions are combined via a weighted average, defined by the parameters w_small and w_rand with w_small+w_rand = 1. The weights are determined according to the frequency of the error values for the corresponding distributions.

$$p\left(z_t|q_t\right)={\left(\begin{array}{c}{w}_{small}\\ w_{rand}\end{array}\right)}^T\cdot \left(\begin{array}{c}{p}_{small}\left(z_t|q_t\right)\\ p_{rand}\left(z_t|q_t\right)\end{array}\right).$$ (10)

IV. LOCALIZATION ALGORITHMS

In this section, EKF and PF localization algorithms for estimating POI position from sonomicrometer measurements are presented. The major advantage of the PF over the EKF is PF can incorporate non-Gaussian noise models, while EKF assumes Gaussian noise models. PF also approaches to the optimal Bayesian estimate, if the posterior distribution of the state is represented by sufficiently large number of particles, which makes it more accurate than EKF. However, the EKF is computationally very efficient compared to the PF.

Algorithms 1 and 2 show EKF and PF localization algorithms with harmonic motion model. Both algorithms are adapted from the generic EKF and PF algorithms [11]. The algorithms with the Brownian and generalized motion models are analogous and omitted due to space constraints. For these models, changes are made by incorporating the appropriate process models (i.e.(1) and (7)) to Lines 2–3 in Algorithm 1 and Lines 4–5 in Algorithm 2.

V. RESULTS AND DISCUSSION

The localization performances of the proposed models and algorithms are evaluated with a 60 seconds constant heart rate motion data collected at a sampling rate of 404 Hz from a calf model, where absolute sonomicrometer accuracy is 250 μm [8]. The breathing (f_r0) and heart beating components (f_h0) respectively have fundamental frequencies of 0.16 Hz and 1.63 Hz, corresponding to 98 beats/min. The peak-to-peak amplitude of the POI motion is 7.33 mm, with a RMS value of 3.51 mm. The offline 3D triangulation method provided by the proprietary software of the sonomicrometry system, which filters and cleans the online recorded sonomicrometer data, is also used to localize POI position. These values were used as the ground truth localization (referred as POI_Sono from here on) as currently we do not have an independent set of sensor measurements (such as from a vision sensor) that can be used for the validation purposes.

Algorithm 1.

EKF algorithm with harmonic motion model. μ_t and Σ_t are the mean and covariance that represent the belief of POI state at time t. ${\Sigma }_r\in {ℝ}^{3\times 3}$ and $Q\in {ℝ}^{n\times n}$ are respectively process noise and measurement noise covariance matrices. n is the number of sonomicrometer measurements at time t.

1: function POIlocalizationEKF(μt−1,Σt−1,Δut,zt)
2:	${\overline{\mu }}_t\leftarrow {\mu }_{t-1}+\Delta u_t$
3:	${\overline{\Sigma }}_t\leftarrow {\Sigma }_{t-1}+{\Sigma }_r$
4:	$K_t\leftarrow {\overline{\Sigma }}_t{H}_t^T{\left(H_t{\overline{\Sigma }}_t{H}_t^T+Q\right)}^{-1}$
5:	${\mu }_t\leftarrow {\overline{\mu }}_t+K_t\left(z_t-h\left({\overline{\mu }}_t\right)\right)$
6:	${\Sigma }_t\leftarrow \left(I-K_t{H}_t\right){\overline{\Sigma }}_t$
7:	return μt

Algorithm 2.

PF algorithm with harmonic motion model. ${\mathcal{X}}_t=\left\{q_t^{[m]},m=1\dots N\right\}$ is the set of particles that represent the belief of POI state at time t. N is the total number of particles.

1:	function POIlocalizationPF $\left({\mu }_{t-1},{\mathcal{X}}_{t-1},\Delta u_t,z_t\right)$
2:	${\overline{\mathcal{X}}}_t$, ${\mathcal{X}}_t\leftarrow \varnothing$
3:	for m = 1 → M do
4:	sample $r_t^{[m]}~N\left(0,{\Sigma }_r\right)$
5:	${\overline{\mu }}_t^{[m]}={\mu }_{t-1}+\Delta u_t+r_t^{[m]}$
6:	$w_t^{[m]}\leftarrow p(z_t|q_t^{[m]})$
7:	${\overline{\mathcal{X}}}_t\leftarrow {\overline{\mathcal{X}}}_t+\langle q_t^{[m]},w_t^{[m]}\rangle$
8:	${\mathcal{X}}_t\leftarrow \text{Low VarianceSampler}\left({\overline{\mathcal{X}}}_{\text{t}}\right)$
9:	${\mu }_t\leftarrow \mathbb{E}\left[{\mathcal{X}}_t\right]$
10:	return μt

The model parameters used in the two localization algorithms are selected empirically. In the EKF algorithm, the measurement noise covariance Q is a diagonal matrix with all values set to 0.1. To model the p_small and p_rand in (10), first ideal sonomicrometer measurements for each channel are calculated from POI_Sono and base crystal positions via (8). Measurement errors are computed between the ideal sonomicrometer measurements and online recorded sonomicrometer data for each channel. Measurements errors of individual channels are concatenated to get a normalized error histogram of the system. A normal disitribution is fitted to histogram to get p_small(z_t|q_t) ~ N(0.3,0.02) and a uniform distribution is fitted to entire error range to get p_rand(z_t|q_t) ~ U(−2,8) (Section III-B). Corresponding weights of the distributions in the measurement model are, w_small = 0.95,w_rand = 0.05. Fig. 4 shows the resulting distribution for the current data set.

Fig. 4.

Density distribution of the noise model superimposed on the normalized error histogram.

In Brownian motion model, the process covariance matrix Σ is set by finding the variance of the displacement distance between subsequent positions of the POI_Sono. This results in diagonal values [0.35, 0.41, 0.03]. In harmonic motion model, Σ_r is set by finding the variance of the displacement distance between subsequent positions of the approximation error; i.e.r_t = p_t − u_t via (3), (4). This results in diagonal values [0.35, 0.42, 0.03], and in generalized motion model Σ_g is set empirically with diagonal values [0.28,0.30,0.03].

The results are given in Table I. In the PF algorithm, 1000 particles are used to represent belief of the POI location and the algorithms were run 10 times with the worst results are reported. The root-mean-square (RMS) errors are calculated from the 3D distance between the POI position estimated by the proposed algorithms and POI_Sono. The RMS error between POI_Sono and online unfiltered POI positions from sonomicrometer is also given as reference. Localization results for the y-coordinate of the POI position by EKF algorithm with harmonic motion model is shown in Fig. 5.

TABLE I.

RMS POI Localization Errors

Localization Results	RMS Localization Error [mm]
Online Sonomicrometer POI	135.11
EKF with Brownian Model	1.38
EKF with Harmonic Model	1.32
EKF with Generalized Model	1.22
PF with Brownian Model	1.14
PF with Harmonic Model	1.05
PF with Generalized Model	1.08

Fig. 5.

Shows y-coordinate of the POI position estimated by EKF with harmonic model and POI position estimated by offline triangulation; POI_Sono.

The results show that the proposed algorithms were able to localize POI position accurately. In all experiments, PF outperformed EKF and employing harmonic motion model yielded better results than the Brownian motion model. This is no surprise since the harmonic approximation includes significant information about the state of the POI. Motion model based on the generalized adaptive filter also enhances the performance of the algorithms with Brownian motion model. Improved localization accuracy is expected as one-step predictions are based on current and past observations, and the motion model that integrates these predictions embodies information about the state of the POI. Fig. 5 shows the proposed models can accurately capture the sources of error in the sonomicrometer. Thus, localization algorithms can filter noisy sonomicrometer measurements and yield precise POI position estimates.

VI. CONCLUSIONS

This paper presents the formulation of a framework for the localization of a POI position on cardiac surface from sonomicrometer measurements. In the proposed framework, POI motion is modeled by different approaches and the uncertainties in the sonomicrometer are explicitly considered using a probabilistic formulation.

The results indicated that the proposed motion and measurement models were able to capture POI motion and uncertainties in the sonomicrometer accurately. The localization algorithms were also able to estimate POI position precisely.

Future works include a real-time implementation and a hardware validation of the proposed algorithms using data from a complementary sensor, e.g., a vision based sensor system, as the baseline truth.

Although it is not addressed here, computational load of the algorithms is another point of discussion. The EKF algorithm processes the 60 seconds heart motion data approximately in 0.6 seconds, whereas the PF algorithm with 1000 particles processes approximately in 3000 seconds. EKF is computationally much more tractable despite PF provides better localization results. The required speedup can potentially be achieved by employing parallel processing as PF algorithms can be trivially parallelized. A multi-threaded implementation can also provide additional speedups.