Organ-mounted Robot Localization via Function Approximation

Abstract

Background

Organ-mounted robots adhere to the surface of a mobile organ as a platform for minimally invasive interventions, providing passive compensation of physiological motion. This approach is beneficial during surgery on the beating heart. Accurate localization in such applications requires accounting for the heartbeat and respiratory motion. Previous work has described methods for modeling quasi-periodic motion of a point and registering to a static preoperative map. The existing techniques, while accurate, require several respiratory cycles to converge.

Methods

This paper presents a general localization technique for this application, involving function approximation using radial basis function (RBF) interpolation.

Results

In an experiment in the porcine model in vivo, the technique yields mean localization accuracy of 1.25 mm with a 95% confidence interval of 0.22 mm.

Conclusions

The RBF approximation provides accurate estimates of robot location instantaneously.

1. Introduction

Organ-mounted robots are a new class of surgical robots that adhere to the surface of a mobile organ as a platform for minimally invasive interventions ¹. While recent years have seen the development of bone-mounted robots to ease the problem of registration ^2,3, organ-mounted robots are intended to also provide passive compensation of the physiological motion of mobile organs. The approach is especially effective for surgery on the beating heart. Examples include Cerberus, a deployable epicardial parallel wire robot ⁴, HeartLander, an inchworm-type crawling robot, and Lamprey, a passive repositionable probe with a single suction foot ¹. For such systems, accurate localization requires accounting for motion due to heartbeat and respiration.

Some methods for prediction of motion of the heart, such as autoregressive representations ^5,6, do not directly model the motion. However, the majority of approaches involve fitting predictive models to the motion, usually using frequency-based models ^7–9. Existing approaches to registration and localization of surgical tools in and around the heart are highly dependent upon the tools and sensing modalities used. Constrained Kalman filtering ^10,11, Iterative Closest Point (ICP) ¹², and particle filtering ¹³ have been employed in snake-like robots and intracardiac echocardiographic (ICE) catheter systems. In each of these approaches, however, the tools have, at most, intermittent contact with the heart and the periodic motion of the heart is filtered out as noise. The present application is fundamentally different, because the robots in question are mounted on the heart surface.

A previous paper from our group presented methods for Fourier modeling of the periodic motion of a point on the surface of the heart due to both heartbeat and respiration ¹. A subsequent paper dealt with registration of motion models of such points to a static preoperative map of the heart surface ¹⁴. Using a simple or straightforward implementation of these methods alone, accurate estimates of the position of the robot on the surface of the heart at a given position can be achieved, but only once the frequency-based models have converged, which due to the low frequency of respiration (about 0.2 Hz) can require up to 15 s.

This paper, the third in this line of research, presents a more developed approach to the general problem of estimating the position of the organ-mounted robot on the beating heart at all times. We refer to this post-registration estimation of position on the heart as localization. The paper first briefly poses the localization problem solely based on the previously presented motion model in Section 2.1, an approach that, as mentioned above, suffers from slow convergence.

Next, in Section 2.2, we reinterpret the problem as function approximation. This method uses models fit to motion observed at locations on the heart visited earlier in the same intervention to predict where on the surface of the heart new observations will fall. These estimates outperform model-based prediction over short time horizons, and may help in reducing the time required to perform interventions. Finally, Section 3 presents experimental results from the porcine model in vivo comparing the approaches and characterizing performance.

2. Algorithmic Methods

2.1. Localization via Motion Prediction

Localizing on a registered surface is trivial in most cases. In the case of a static surface with a static map, the spatial registration parameters provide full information to associate a single measurement of robot pose to a location on the surface of the heart. Registration fully constrains this problem.

In the case of a periodically deforming surface with a static map, however, the problem is less trivial. If the measurements of robot pose have the same phase as the map, the problem is once again fully constrained. If the measurements are not the same phase as the map, we must project the measurements to the correct phase.

Using the previously derived model for periodic motion ¹ and assuming that spatiotemporal registration has been performed have been performed per ¹⁴, the estimated location of the robot on the heart, ũ, is defined as the nearest location in the map,

$$\stackrel{\sim }{\mathbf{u}}=\underset{\mathbf{u}}{\text{argmin}}\Vert \mathcal{S}(\mathbf{u})-{\mathbf{T}}^{\ast }{\mathbf{p}}^{{\varphi }^{\ast }}\Vert$$ (1)

where the predicted pose is

$${\mathbf{p}}^{{\varphi }^{\ast }}=\mathrm{P}({\varphi }_C^{\ast },{\varphi }_R^{\ast })={\mathcal{M}}_{\mathbf{q}}^{\mathbf{R}}({\mathbf{q}}_{\overline{\mathrm{H}}}^{\mathrm{P}}){\mathbf{W}}_{\overline{\mathbf{t}}}^T\varphi +{\mathbf{t}}_{\overline{\mathrm{H}}}^{\mathrm{P}}$$ (2)

and where 𝒮 represents the preoperative medical image dataset, ${\mathbf{t}}_{\overline{\mathrm{H}}}^{\mathrm{P}}$ and ${\mathbf{q}}_{\overline{\mathrm{H}}}^{\mathrm{P}}$ are the position and orientation in quaternion form of the robot in the frame of a reference tracker probe placed on the chest wall of the patient, ${\mathbf{W}}_{\overline{\mathbf{t}}}^T$ is a matrix of Fourier series parameters for translation,¹⁴ T^* is the rigid transformation that represents intraoperative registration, and ${\varphi }_C^{\ast }$ and ${\varphi }_R^{\ast }$ are the ground-truth cardiac and respiration phases at the instant when preoperative images were taken, determined as in ¹⁴.

This formulation for localization is identical to the matching procedure used in ICP for registration; however, in that case it was assumed the models had converged and provided reasonable estimates of robot pose. In the case of using this formulation for localization, it is clear that localization error relies directly on the accuracy of the model. As we have shown ¹, convergence for these models requires up to 15 s (about 3 respiration cycles).

In this scheme the accuracy of prediction is a function of how long the robot stays at a single location. This dependence is demonstrated graphically in Fig. 1. The longer the robot observes the motion, the more accurate the model and the more accurate the prediction. Depending on the acceptable level of accuracy, this may require remaining stationary for up to 3 respiration cycles.

Figure 1.

Using motion prediction models for localization requires predicting the future location of the robot at the phase of the heart model. The two diagrams depict the evolution of the predicted location on the surface, ũ, as projected by the motion models, shown as dashed lines, learned from observed motion, shown as solid red lines. Models fit to (a) short sections of motion data can differ significantly from (b) converged models. This figure illustrates the operation of Section 2.1.

2.2. Localization via Function Approximation

In order to bypass the long convergence time required to accurately localize using motion prediction, the localization problem can be reinterpreted to utilize all available data. Accurate registration requires accurate models of the motion of points spread over the surface of the heart. These models enable prediction of robot pose at these points for any phase, as well as where the points lie on the surface of the static heart. Using these data, localization can be posed as function approximation.

This approach to localization is depicted in Fig. 2. Given a measurement of the robot pose at the current phase, z^ϕ_k, the predicted location of the robot on the heart, ũ, can be estimated by approximating the function which maps from the existing models’ predictions of pose at the current phase, $\left\{{\mathbf{p}}_1^{{\varphi }_k},{\mathbf{p}}_2^{{\varphi }_k},\dots ,{\mathbf{p}}_n^{{\varphi }_k}\right\}$, to their corresponding locations on the heart,{u₁, u₂, …, u_n}.

Figure 2.

Using models of heart motion observed at various locations, the localization problem can be posed as an interpolation or function approximation. In this scheme we learn a function that projects the currently measured robot pose, z^ϕ_k, to a predicted location on the surface of the heart, ũ, using models of the motion of previously visited locations on the heart, ${\mathbf{p}}_i^{{\varphi }_k}$. This figure depicts the approach presented in Section 2.4.

2.3. Radial Basis Function Approximation

In order to approximate the mapping to the surface of the heart, we employ radial basis function (RBF) interpolation. This method of scattered data interpolation is ubiquitous, being used in applications including solving partial differential equations ¹⁵, surface reconstruction ¹⁵, and nonlinear registration and surface estimation in medical imaging ^16–18. We briefly review RBFs here and refer the reader to Buhmann ¹⁹ for more information.

Radial basis functions approximate the real-valued function, g : ℝ^d → ℝ, with s : ℝ^d → ℝ, given the values {g(x_i) : i = 1,2, …, n} at the centers of interpolation {x_i : i = 1,2, …, n}. The interpolant is of the form

$$\mathrm{s}(x)=\sum _{i=1}^n{\lambda }_i\kappa \left(r(x_i,x)\right),$$ (3)

where r(·, ·) is a distance metric on ℝ^d, usually the Euclidean norm,

$$r(x_i,x)=\Vert x_i-x\Vert .$$ (4)

The kernel function, κ, is a positive definite function whose value depends only on the distance from the center. Common basis functions include

$$\text{Multiquadric:}\kappa (r)=\sqrt{1+{(\epsilon r)}^2}$$ (5)

$$\text{Gaussian:}\kappa (r)=e^{-{(\epsilon r)}^2}$$ (6)

$$\text{Thin-plate}\text{spline:}\kappa (r)=r^{2}logr$$ (7)

$$\text{Linear:}\kappa (r)=r$$ (8)

Real-valued weights, λ_i, satisfy the interpolation conditions at the centers,

$$\mathrm{s}(x_i)-\mathrm{g}(x_i)=0,\forall i.$$ (9)

Writing the system in linear form,

$$K\mathrm{\lambda }=\mathrm{g},$$ (10)

where

$$K_{i,j}=\kappa \left(r\left(x_i-x_j\right)\right)$$ (11)

$$\mathrm{\lambda }={[{\lambda }_1,{\lambda }_2,\dots ,{\lambda }_n]}^T$$ (12)

$$\mathrm{g}={[\mathrm{g}(x_1),\mathrm{g}(x_2),\dots ,\mathrm{g}(x_n)]}^T,$$ (13)

and assuming the problem is well conditioned, the inverse of K exists. The weights are then found using the inverse,

$$\mathrm{\lambda }=K^{-1}\text{g.}$$ (14)

2.4. RBF’s for Organ-mounted Robot Localization

Although it may be possible to construct an RBF interpolant using a distance metric over an anisotropic space ²⁰, the problem is simplified by not considering the full state space – ℝ³ × 𝕊¹ × 𝕊¹ (3D position, ϕ_C, and ϕ_R) – and instead solving the problem at each instant using a more standard metric over ℝ³.

At a particular instant, ϕ_k = (ϕ_Ck, ϕ_Rk), the centers of the interpolant are the predicted positions of the n motion models used during registration, $\left\{{\mathbf{p}}_1^{{\varphi }_k},{\mathbf{p}}_2^{{\varphi }_k},\dots ,{\mathbf{p}}_n^{{\varphi }_k}\right\}$, and the query point is the current position measurement of the robot, z^ϕ_k.

Instead of directly estimating the map coordinates, we instead attempt to learn the deformation field, or the translations required to move each center to its location on the map. This is done to first avoid the complications of singularities or constraints in spherical coordinates, and also to effectively zero-mean the data. The function to be approximated, g(·), is the difference between the registered and current locations,

$$\mathrm{g}\left({\mathbf{p}}_i^{{\varphi }_k}\right)=\mathcal{S}({\mathbf{u}}_i)-{\mathbf{p}}_i^{{\varphi }_k}.$$ (15)

Localization is solved by estimating the deformation at the query point and identifying the closest point in the map to the deformed position estimate,

$$\stackrel{\sim }{\mathbf{u}}=\underset{\mathbf{u}}{\text{argmin}}\left({\mathbf{z}}^{{\varphi }_k}+\sum _{i=1}^n{\lambda }_i\kappa \left(\Vert {\mathbf{p}}_i^{{\varphi }_k}-{\mathbf{z}}^{{\varphi }_k}\Vert \right)\right).$$ (16)

3. Experiments

The previously described methods of localization, motion modeling and RBF approximation, were tested retrospectively on data collected during a live-animal closed-chest beating-heart procedure performed under an IACUC-approved protocol. The Lamprey robot ¹ was attached to the beating heart at 108 different points in sequence and collected a time trace 30 s in duration at each point using an electromagnetic tracker (microBIRD, Ascension Technology Corp., Shelburne, Vermont, USA). Virtual image guidance was used to guide uniform coverage of the heart. The data were processed and fit using frequency-based models presented in ¹, then registered to the anatomical model using the methods presented in ¹⁴. The experiments described below use leave-one-out or randomized leave-n-out sampling to define previously visited locations for interpolation.

3.1. RBF Localization Performance

Optimal localization performance with RBF approximation was achieved using a multiquadric basis function with a shape parameter ε = 1, and is shown in Fig. 4. This trial shows the mean error across all points for 20 s with estimation occurring every 0.1 s.

Figure 4.

Localization error using radial basis functions is essentially constant over time. This plot shows the best performance achieved, with 95% confidence interval, and uses the Cartesian distance metric, multiquadric radial basis function with ε = 1, and differential Cartesian coordinates for prediction variables.

In this trial, localization was estimated for each point in a leave-one-out fashion such that all remaining points were used as interpolation centers. The raw data for each point, which were used to construct the motion model, were used as query locations, and the error in localization was calculated as the Euclidean distance from the ground-truth location estimated during registration. The data shown in Fig. 4 are the mean and standard error across all points.

Mean localization accuracy across the entire run was 1.25 mm with a 95% confidence interval of ±0.22 mm. There is some fluctuation in the error at what appears to be the respiration period (5 s), the effect is less than 0.25 mm peak-to-peak.

3.2. Effects of Center Density

In order to determine the effects of the density of centers on localization error, experiments were conducted in which the number of interpolation centers used ranged from 100% to 20% of the total number of point observations. Uniform sampling over the surface of the heart was approximated via a sampling scheme that chose points to convert to centers which were farthest from the nearest existing interpolation center. Sampling was randomly initialized and continued until the desired number of centers was identified. For each subsampled set the average minimum inter-center distance was calculated as an approximation of center density. An example of 22 centers sampled from among the total of 108 is shown in Fig. 5.

Figure 5.

Approximate uniform sampling of centers from a point cloud identifies points with the greatest distance to its nearest center. This example shows 22 centers, indicated by red stars ( ), sampled from among the total of 108.

For each set of sampled centers, prediction trials were conducted for all 108 points to estimate localization accuracy. Predictions were done for 20 s for each point at 10 Hz. Prediction errors were combined across all points and time to estimate a single mean prediction error for each set of centers. Results of these trials are shown in Table I and Fig. 6.

Table I.

Results from Center Density Experiment

Sampling Ratio	No. of Centers	Average Distancea (mm)	Mean Error (mm)	±95% Confidence Interval (mm)
100%	107	3.9	1.25	0.22
90%	97	4.6	1.30	0.22
80%	86	5.3	1.37	0.22
70%	76	5.9	1.34	0.24
60%	65	6.7	1.43	0.24
50%	54	7.6	1.54	0.24
40%	43	8.8	1.62	0.25
30%	32	10.9	1.74	0.26
20%	22	13.4	1.99	0.29

^aCalculated as average of minimum distance between centers.

Figure 6.

Interpolation error using Radial Basis Functions increases as the density of centers decreases. Square markers denote mean error with error bars representing 95% confidence interval of the mean estimate. Density is approximated as the average of the minimum distance between centers.

Reduction in the density of center locations caused an expected increase in localization error. The effect is highly linear as a function of density, increasing localization error from 1.25 mm to 1.99 mm as the average spacing of centers increased from 3.9 mm to 13.4 mm. The number of interpolation centers used decreased almost fivefold from 107 to 22. Even with this significant reduction in centers, localization error remains small.

3.3. Motion Prediction Comparison

Motion-prediction localization simulations were run to provide a comparison of the localization methods. Using the method described in Section 2.1, an amplitude-modulated Fourier series model of the motion of each point was learned and used to predict the robot location at the map, phase which was then used to estimate the robot’s location on the heart. All parameters of the model were initialized to zero. Estimation was conducted for 20 s at 100 Hz sampling rate. Error was calculated as the Euclidean distance between the ground-truth location estimated during registration and the predicted location, and averaged across all points to yield mean error, shown in Fig. 7.

Figure 7.

Localization error using motion prediction decreases as the motion model observes more data and becomes more accurate. RBF localization results are shown for the minimum and maximum center densities tested (20% and 100%, respectively).

Because the motion-prediction scheme relies on estimating a motion model, prediction accuracy is correlated with time. As more measurements are observed, the motion models become more accurate and reduce prediction error. Also shown in Fig. 7 are the mean and 95% confidence intervals for the minimum and maximum number of centers used in the previously described RBF approximation experiments. While the motion-prediction scheme provides much more accurate estimates of position over long time horizons, more accurate estimates of location are achieved over the first 2.5 s by RBF approximation using only 22 centers, and over the first 4.5 s by RBF approximation using all 108 centers.

4. Discussion

The two localization schemes presented, motion prediction and radial basis function approximation, should be viewed as complementary approaches that may both be used in real-time operation of the robot. Although both of these approaches are built upon our previously presented methods for modeling the periodic motion of the heart ¹, the two implementations use them in fundamentally different ways with complementary results. In the motion prediction scheme a single model is fit to the motion of the robot to predict where on the surface the robot lies, while in the RBF scheme models of the motion of many locations are used to interpolate the robot’s position.

The underlying difference in the two approaches is what data is used to make the predictions. In the model-based motion scheme only the data from the current location is used to predict the location of the robot on the heart, while the RBF scheme leverages data from all previously visited locations. While the data used in this paper was specifically collected for the presented experiments and densely covered the entire surface of the heart, the results show that the RBF method has advantages with as few as 20 sample locations. In real interventions these samples could be collected in a few minutes during registration.

The RBF approximation provides very accurate estimates of robot location instantaneously. This method can be used to provide reasonable estimates of robot location even when the robot is not attached to the heart. For applications that require less accurate positioning, RBF approximation may be sufficient on its own. For applications that require more accurate positioning, RBF approximation can be used over short time horizons and can then hand off estimation to the motion-prediction scheme over longer time horizons. RBF approximation may possibly even be used to ”jump-start” the motion-prediction models if an estimation scheme more complex than recursive least squares is employed for learning.

From a general perspective, the localization error achieved by using motion prediction is relatively small (approximately 3 mm) at the first time step. While this level of accuracy may be sufficient for many existing therapies, it is our expectation that the presented work will have its greatest effect in enabling new therapies that are not feasible with existing techniques.

Figure 3.

Data were acquired using Lamprey, a passively repositionable recording device that incorporates a single plastic ”foot” which houses a suction chamber on its bottom surface, an electromagnetic tracker probe (indicated by white arrow), and a flexible cable that protrudes from the body for manual repositioning.