Continuum Robot Shape Estimation Using Magnetic Ball Chains

Abstract

Shape sensing of medical continuum robots is important both for closed-loop control as well as for enabling the clinician to visualize the robot inside the body. There is a need for inexpensive, but accurate shape sensing technologies. This paper proposes the use of magnetic ball chains as a means of generating shape-specific magnetic fields that can be detected by an external array of Hall effect sensors. Such a ball chain, encased in a flexible polymer sleeve, could be inserted inside the lumen of any continuum robot to provide real-time shape feedback. The sleeve could be removed, as needed, during the procedure to enable use of the entire lumen. To investigate this approach, a shape-sensing model for a steerable catheter tip is derived and an observability and sensitivity analysis are presented. Experiments show maximum estimation errors of 7.1% and mean of 2.9% of the tip position with respect to total length.

I Introduction

Continuum robots are designed to conform to three-dimensional curves, possessing the ability to alter their form by bending, rotating, and either extending or retracting their structural elements. These flexible capabilities render continuum robots highly suitable for applications that require delicate manipulation and spatial adaptability, such as minimally invasive surgical procedures. While kinematic models have been developed for the various types of continuum robots, effects such as nonlinear elasticity, friction and hysteresis as well as external forces applied by surrounding tissue, degrade the accuracy of these models.

Shape sensing can provide real-time feedback to augment the kinematic model prediction of robot state and so mitigate modeling inaccuracies. Approaches to real-time sensing that have been studied include fiber Bragg gratings (FBG), electromagnetic (EM) tracking, tendon-based sensing and imaging. FBG sensing is currently receiving much attention due to its ability to provide multiple accurate curvature measurements along a robot’s length, but it remains expensive owing to the cost of the sensors as well as the optical interrogator [1, 2, 3]. EM sensors can be used effectively in some clinical applications [4], but are limited to localizing a small number of points along a robot. Inferring shape and external applied forces from tendons and string encoders can provide accurate shape estimates, but requires careful robot design and sensor integration [5]. Shortcomings of image-based shape sensing include cost, resolution limitations and the use of ionizing radiation [4].

In contrast to these approaches, a real-time low-cost shape sensing system that could be easily inserted and removed from the robot is preferred because of its direct measurement, instant adaptability to the continuous shape change and ease of replaceability. Magnetic ball chains have recently been introduced as a new design for continuum robots [6]. Comprised of a chain of spherical permanent magnets encased in a flexible sleeve, an external magnetic field is used to steer the ball chain. As shown in Fig. 1, to create a shape sensor, the ball chain and its sleeve are inserted inside the lumen of a continuum robot and the external magnetic field is replaced by an array of Hall effect sensors. The continuum robot containing the ball chain can be actuated by any of the standard means such as tendons. The ball chain itself is inexpensive, contains no electronics and is easy to sterilize or recycle.

Figure 1: Magnetic ball chain shape sensing. (a) The sensor is comprised of a chain of spherical permanent magnets encased in a flexible sleeve which is inserted in a robot’s lumen. (b) An external array of Hall effect sensors measures the combined magnetic field of all balls from which the shape of the robot is computed.

The potential for tracking permanent magnets embedded in medical robots using external sensor arrays has been established by several researchers. For example, Son et al. [7] investigated tracking a capsule robot containing a single permanent magnet. Continuum robot shape estimation has also been performed by attaching a permanent magnet at the tip of each flexing section and interpolating robot shape between magnets [8, 9]. Here, we extend the concept by creating a removable chain of spherical magnets which simultaneously maximize the magnetic content and flexibility of the sensor. The goal of our approach is to avoid the need to interpolate robot shape between magnets while also enabling their removal so that the entire robot lumen can be made available for the clinical task after navigation is complete.

In this initial paper, we investigate a simplified shape sensing implementation in which it is assumed that the coordinate frame of the robot base is known and fixed with respect to the world frame. The task is to estimate the shape of a single flexing section that can bend $180^{\circ}$ in any direction with respect to its longitudinal axis. The remainder of the paper is arranged as follows. The shape sensing model is derived in the next section. The following two sections present observability and sensitivity analyses for inverting the model to estimate shape. Section V contains an experimental evaluation and conclusions appear in the final section.

II Shape Estimation Model

Figure 2: Schematic representation of magnetic ball chain with plane ($\phi$) and angle of bending ($\psi$) parameters. Two possible sensor array locations (I and II) are shown.

As shown in Fig. 2, the spheres align to form a chain due to their magnetic interaction [6]. Since the balls are spherical, we can consider each magnet as a perfect dipole [10]. When constrained to bend inside a continuum robot, they take on the curved shape of the robot.

In this paper, we assume that continuum robot flexure is of constant curvature, which we parameterize using angle of bending $\psi$ and plane angle $\phi$ as shown in Fig. 2 and assume that each ball’s dipole is locally tangent to the curve. Further assuming that the superimposition principle applies, the total field generated can be measured by an array of 3D magnetic field sensors as shown.

We define $\mathbf{q}_{j}\in\mathbb{R}^{3}$ to be the field measured by the $j$th sensor with respect to the global reference frame $\{G\}$, which equates to the magnetic field generated by the chain at the position of the sensor $\mathbf{p}_{s_{j}}$. We use the magnetic dipole model to compute the field measured by the sensor, for a given bending of the chain

$$\boldsymbol{\gamma}=\psi\left(\begin{array}[]{c}-\sin(\phi)\\ \cos(\phi)\end{array}\right).$$ (1)

The field measured by the $j$th sensor is

$$\mathbf{q}_{j}(\boldsymbol{\gamma})={}^{W}\mathbf{R}_{S_{j}}\sum_{i=1}^{n}\frac{\mu_{0}}{4\pi\|\mathbf{r}_{ij}\|^{4}}\cdot\left(3\mathbf{r}_{ij}\mathbf{r}_{ij}^{T}-I\right){}^{S_{j}}\boldsymbol{\mu}_{i}$$ (2)

where $n$ is the number of balls in the chain, $\mathbf{p}_{i}$ is the $i$th ball’s position, $\boldsymbol{\mu}_{i}$ its magnetic dipole moment, and we define

$$\mathbf{r}_{ij}=\ ^{S_{j}}\mathbf{p}_{i}+\ ^{S_{j}}\mathbf{p}_{0}-\ ^{S_{j}}\mathbf{p}_{s_{j}}.$$ (3)

Here $\|\cdot\|$ indicates the Euclidean norm and $\mu_{0}$ is the vacuum magnetic permeability.

The expression ${}^{F}\mathbf{v}$ indicates that a vector $\mathbf{v}\in\mathbb{R}^{3}$ is written with respect to the reference frame $F$, and ${}^{F_{1}}\mathbf{R}_{F_{2}}$ is the rotation from frames $F_{2}$ to $F_{1}$.

Under assumption of constant curvature, the position $\mathbf{p}_{i}$ and magnetic dipole $\boldsymbol{\mu}_{i}$ are

$${}^{S_{j}}\mathbf{p}_{i}={}^{S_{j}}\mathbf{R}_{G}\left({}^{G}\mathbf{p}_{0}+{}^{G}\mathbf{R}_{B}\sum_{k=1}^{i}\exp{\left(\frac{\left[\boldsymbol{\gamma}\right]^{\wedge}k}{n}\right)d\mathbf{e}_{3}}\right)$$ (4a)

$${}^{S_{j}}\boldsymbol{\mu}_{i}={}^{S_{j}}\mathbf{R}_{B}\sum_{k=1}^{i}\exp{\left(\frac{\left[\boldsymbol{\gamma}\right]^{\wedge}k}{n}\right)\mu_{i}\mathbf{e}_{3}}$$ (4b)

where $d$ is the diameter of the balls, $\mu_{i}$ the $i$th ball’s magnetic dipole intensity, and $\mathbf{e}_{k}$ is the $k$th element of the canonical basis of $\mathbb{R}^{3}$. The operator $\left[\boldsymbol{\gamma}\right]^{\wedge}$ denotes the cross product form of the extended vector $\boldsymbol{\gamma}^{\prime}=\left(\boldsymbol{\gamma}^{T}\ 0\right)^{T}$, i.e. $\left[\boldsymbol{\gamma}\right]^{\wedge}=\left(\boldsymbol{\gamma}^{\prime}\times\mathbf{e}_{1}\ \boldsymbol{\gamma}^{\prime}\times\mathbf{e}_{2}\ \boldsymbol{\gamma}^{\prime}\times\mathbf{e}_{3}\right)$.

By combining (2) and (4), we obtain the total field measured by each sensor. For an array of $m$ sensors, we can combine all measurements as

$$\mathbf{q}(\boldsymbol{\gamma})=\left(\mathbf{q}_{1}(\boldsymbol{\gamma})^{T}\ \mathbf{q}_{2}(\boldsymbol{\gamma})^{T}\ \cdots\ \mathbf{q}_{m}(\boldsymbol{\gamma})^{T}\right)^{T}\in\mathbb{R}^{3m}.$$ (5)

For a sensor reading $\overline{\mathbf{q}}\in\mathbb{R}^{3m}$ we solve for the shape as

$$\hat{\boldsymbol{\gamma}}\ni\mathbf{K}(\hat{\boldsymbol{\gamma}})\left(\overline{\mathbf{q}}-\mathbf{q}(\hat{\boldsymbol{\gamma}})\right)=0,$$ (6)

where $\mathbf{K}(\hat{\boldsymbol{\gamma}})\in\mathbb{R}^{m\times m}$ is a matrix of gains which can be tuned, depending on the confidence level for each sensor. Since different areas of the workspace may have different signal/noise ratio for different sensors, we subdivided the workspace in different manifolds $\Gamma_{k}$ such that a certain gain $\mathbf{K}_{k}$ is calibrated for $\boldsymbol{\gamma}\in\Gamma_{k}$. In our implementation, the manifold $\Gamma_{k}$ has a center $\overline{\boldsymbol{\gamma}}_{k}$ so that

$$\boldsymbol{\gamma}\in\Gamma_{k}\Leftarrow\min_{k}\|\boldsymbol{\gamma}-\overline{\boldsymbol{\gamma}}_{k}\|.$$ (7)

We use Algorithm 1 below to iteratively solve (6) and adjust the sensors gains according to the previous solution. This technique of adjusting the confidence level of each sensor improves the accuracy of the results. Our method for calibrating the matrices $\mathbf{K}_{k}$ is described in experimental section V.

$\mathbf{K}(\hat{\boldsymbol{\gamma}})\leftarrow$ identity matrix

$\hat{\boldsymbol{\gamma}}^{(0)}\leftarrow$ solution of (6)

while $l\leq N$ do

       if $\boldsymbol{\gamma}^{(l)}\in\Gamma_{k}$ then

             $\mathbf{K}(\hat{\boldsymbol{\gamma}})\leftarrow\mathbf{K}_{k}$

             $\hat{\boldsymbol{\gamma}}^{(l)}\leftarrow$ solution of (6)

       end if

      

end while

Result: $\hat{\boldsymbol{\gamma}}^{(N)}$

Algorithm 1 Shape Estimation Algorithm

Equation (6) can be solved using gradient-based methods when the Jacobian matrix $\mathbf{J}(\boldsymbol{\gamma})=\frac{-\partial\mathbf{q}(\boldsymbol{\gamma})}{\partial\boldsymbol{\gamma}}$ is full rank. In particular, in the remainder of the paper, we use lsqnonlin solver from Matlab (Mathworks, Natick, MA) to solve for $\boldsymbol{\gamma}$ given a sensor reading of $\overline{\mathbf{q}}$. Since we assume no prior knowledge of the robot state, we use the rank of the Jacobian to compute the observability of the shape sensing system as described in the next section.

III Observability Analysis

The bending $\boldsymbol{\gamma}$ of the robot can be reconstructed from the set of sensor readings $\overline{\mathbf{q}}$ as long as the the Jacobian $\mathbf{J}(\boldsymbol{\gamma})$ is of full rank. Here, we define the reciprocal condition number of $\mathbf{J}(\boldsymbol{\gamma})$ as

$$\chi=\frac{\alpha_{m}}{\alpha_{M}},$$ (8)

where $\alpha_{m}$ and $\alpha_{M}$ are the respective minimum and maximum singular values of $\mathbf{J}(\boldsymbol{\gamma})$ and use this value to assess the observability of robot curvature. In the plots that follow, $\chi$ is expressed as a percentage.

Since observability will depend on the relative orientation between the robot and the sensor plane, we consider the two scenarios shown in Fig. 2. Configuration I corresponds to the undeflected robot pointing toward the sensor plane. In Configuration II, the undeflected robot points in a direction parallel to the sensor plane. These two configurations can be viewed as a basis set for many clinically relevant robot-sensor configurations.

Figure 3 presents a numerical assessment of observability for Configurations I and II. In both configurations, the robot base is located at a distance of 15cm from the sensor plane and the robot was allowed to deflect in an arbitrary direction up to $180^{\circ}$, i.e., $(\phi,\psi)\in\left[-180,180\right]^{\circ}\cap\left[0,180\right]^{\circ}$. We consider a chain of ten N42 magnetic balls of diameter 6.35mm comparable to those described later in our experiments.

The sensors where positioned at ${}^{G}\mathbf{p}_{s_{j}}=40.5\cdot\text{rot}_{z}\left(j\frac{\pi}{2}\right)e_{1}$ mm, in global frame (see Fig. 2); here $\text{rot}_{z}\left(\theta\right)$ indicates the rotation of angle $\theta$ around the $z$ axis; $\theta=k\cdot 90^{\circ},k=0,1,2,3$.

Figure 3(a) plots the reciprocal condition number as function of bending plane and bending angle for Configuration I in which the chain points toward the sensor array when undeflected. Given the symmetry of this configuration with respect to $\phi$, we anticipate that observability will be independent of $\phi$. This can be seen to be true with the highest value of the observability approaching 100% when the chain is straight and falling off to between 30-40% when the chain is deflected $180^{\circ}$. (Note that owing to discretization and numerical rounding, a small amount of variation with respect to $\phi$ is seen in the contours.) This can be anticipated since when the chain is straight, the sensors are well positioned to triangulate on the location of the chain. As it bends, this capability becomes reduced, however, $\chi$ still remains above 30% indicating that shape estimation is still possible.

(a) Configuration I

(b) Configuration II

Figure 3: Contour plots of shape observability as measured by reciprocal condition number, $\chi$, for the two sensor array configurations shown in Fig. 2. Here, $\chi$ is expressed as a percentage.

When the sensor array is positioned as shown in Configuration II, the straight configuration is no longer symmetric with respect to all of the sensors resulting in a value of $\chi$ between 50-60%. In this configuration, however, bending the chain increases observability regardless of the value of $\phi$.

In particular, the highest values of $\chi$ occur when $\phi\in\{\pm 45,\pm 90,\pm 135\}^{\circ}$. For $\phi\in\pm 90^{\circ}$, the maxima occur when the robot is deflected $180^{\circ}$. These configurations correspond to the chain bending directly toward and away from the sensor array. These are again the configurations for which the balls are centrally located with respect to all of the sensors resulting in improved triangulation.

The maxima at $\phi\in\{\pm 45,\pm 135\}^{\circ}$ occur for $\psi\approx 130^{\circ}$. Given the number of balls and their position and orientation with respect to the sensor array, it is difficult to predict that these are especially observable configurations using intuition alone. The important result is that, while Configuration II would appear to be more challenging for shape estimation than Configuration I, the minimum value of $\chi$ is greater than 40%. This means that a small change in the chain’s configuration is more easily observed when the sensor array is placed to the side of the chain.

IV Sensitivity Analysis

Figure 4: Maximum tip position error (percentage of robot length) as a function of bending angle, $\psi$, for three noise levels in Configuration I (See Fig. 2).

The observability analysis of the preceding section assumes perfect measurements by the Hall effect sensors. In a real sensing system, noise corrupts measurements with small magnitude signals being degraded more than large magnitude signals. To understand how sensor noise would degrade our estimates of robot shape, we performed a sensitivity analysis for Configurations I and II as follows.

We generated simulated measurement data $\overline{\mathbf{q}}$ using the model in (2). We discretized the workspace using $(\phi,\psi)=(j\cdot 90^{\circ},k\cdot 30^{\circ}$), $\forall(j,k)\in[0,2]\cap[-6,6]\cap\mathbb{N}^{+}$. For each configuration in the workspace, we generated 100 samples, labeled as $l=1,2,\dots,100$, of noisy data by adding zero-mean Gaussian noise with a standard deviation $\sigma=k5/3\%$ of the norm of the measured data $\|\overline{\mathbf{q}}\|$, with $k=0,1,2$. We used these samples to estimate the maximum measurement errors for 0, 5 and 10% noise, since 99% of the samples of the normal distribution are within $3\sigma$.

For the $l$th sample of $k$th chain configuration $\overline{\mathbf{q}}_{k_{l}}$ we solved for the corresponding shape $\hat{\boldsymbol{\gamma}}_{k_{l}}$, following Algorithm 1 for $N=0$; in this case we have uniform sensors noise and do not need locally tuned gains for the sensors. Using (4), we compute the ground-truth position of the chain’s tip $\mathbf{p}_{k_{l}}=\mathbf{p}_{n}(\boldsymbol{\gamma}_{k_{l}})$ and the estimated tip position $\hat{\mathbf{p}}_{k_{l}}=\mathbf{p}_{n}(\hat{\boldsymbol{\gamma}}_{k_{l}})$. We report the error as the maximum norm error over the 100 samples of the tip position

$$e_{k}=\max_{l=1,2,\dots,100}\|\mathbf{p}_{k_{l}}-\hat{\mathbf{p}}_{k_{l}}\|.$$ (9)

Figure 4 depicts maximum tip position error for Configuration I. While, as expected, the error is independent of $\phi$, this plot shows that error is also independent of bending angle $\psi$. While $\chi$ falls as $\psi$ increases, the signal/noise ratio remains high enough that the tip position estimation error is not affected.

(a) Bending $\pm 180^{\circ}$ in the $y$-$z$ plane (motion toward and away from sensors, $\phi=0^{\circ}$).

(b) Bending $\pm 180^{\circ}$ in the $x$-$z$ plane (motion parallel to sensors, $\phi=90^{\circ}$).

Figure 5: Maximum tip position error (percentage of robot length) as a function of bending angle, $\psi$, for 3 noise levels in Configuration II (See Fig. 2).

With the sensor array in Configuration II, maximum tip position error is a function of both bending angle, $\psi$, and robot orientation angle, $\phi$. To illustrate these effects, we plot the results for two planes of bending, $\phi=0^{\circ}$ and $\phi=90^{\circ}$, in Fig. 5. From Fig. 5(b), the effect on noise on measurement accuracy can be clearly seen. As the chain bends away from the sensor plane, the signal magnitude falls sufficiently that accuracy degrades significantly. In contrast, Fig. 5(b) shows that robot bending in the plane parallel to the sensor has no effect on estimation accuracy since signal magnitude is comparable over all configurations.

From this analysis, we can conclude that acceptable maximum estimation errors, within 5% of chain length, would require sensors with a noise level below 5% of the signal. Furthermore, as mentioned in Section II, configuration-dependent weighting of sensor signals can be used to partially compensate for variations in sensor signal amplitude over the workspace.

V Experimental Validation

To evaluate the shape sensing system of Figs. 1 and 2, we 3D printed continuum “robots” of fixed constant curvatures and inserted magnetic ball chains in their lumens as shown in Fig. 6. This robot orientation matches Configuration I. Since the curvatures of these “robots” was known, the values could be used as ground truth and compared to the shape estimated by the sensing system.

The ball chain consisted of ten N42 spherical magnets of diameter $d=6.35$mm (K&J Magnetics), which matched the parameters used in the observability and sensitivity analyses presented above. The sensor array was comprised of four 3D Hall effect senors (MLX90393 Triaxis®, Melexis), positioned as described in Section III. The mock robot was mounted in a stand which provided a fixed set of possible orientation angles, $\phi$. Matching the analyses above, the stand was positioned such that the proximal ball was 15cm from the sensor plane.

Sensor data was collected for the configurations $(\phi,\psi)=(j\cdot 90^{\circ},k\cdot 30^{\circ}$), $\forall(j,k)\in[0,4]\cap[0,6]\cap\mathbb{N}^{+}$. At each configuration, 10 samples for each sensor channel were used for calibration and estimation, as described below.

Figure 6: Shape sensing experiments using mock continuum robots of known fixed curvature and orientation.

V-A Sensor Calibration

Based on the orientation of the chain with respect to the four sensors, the relative accuracy of the sensors varies. To account for this, the optimal gain matrix $\mathbf{K}_{k}$ associated with the manifold of $\Gamma_{k}$ as described in Section II, Algorithm 1, is computed and used for shape estimation. The center of the manifold $\overline{\boldsymbol{\gamma}}_{k}$ is computed from (1) for each combination of $(\phi,\psi)_{k}$.

(a) Results on vertical plane: $\phi=90$.

(b) Results on horizontal plane: $\phi=0^{\circ}$.

Figure 7: Experimental estimates of robot shape for Configuration I. Plots on left show estimated and actual ball locations. Plots on right compare sensor-computed tip position errors with predictions from sensitivity analysis of Section IV.

We compute the predicted value of the measurement from the model in (2), $\mathbf{q}(\overline{\boldsymbol{\gamma}}_{k})$, and the measurement reading, $\overline{\mathbf{q}}_{k_{t}}$, for each sample $t=1,2,\dots,10$. The maximum error for each component is given by

$$\mathbf{e}_{M}=\left(\max_{t}|\overline{\mathbf{q}}_{k_{t}}-\mathbf{q}(\overline{\boldsymbol{\gamma}}_{k})|\right)\in\mathbb{R}^{m}.$$ (10)

where $|\cdot|$ is absolute value and $m=12$, since four 3D sensors are used. Sensor gain is defined as

$$\mathbf{K}_{k}=\left(\text{diag}\left(\mathbf{e}_{M}\right)\right)^{-1}\in\mathbb{R}^{m\times m},$$ (11)

and so more trust is given to the channel with smaller maximum error.

V-B Shape Sensing

Algorithm 1 in Section II was applied to the collected data with number of iterations $N=2$. The results are reported in Fig. 7, where we show independently the bending on the the vertical $y$-$z$ plane ($\phi=90^{\circ}$, Fig. 7(a)) and the horizontal $x$-$z$ plane ($\phi=0^{\circ}$, Fig. 7(b)). The plots present the error between the ground truth configurations and the sensor-based estimates.

For comparison, we also plot the sensitivity analysis maximum error estimates for noise levels of 5% and 10% (see Section IV for details). Note that the 5% noise maximum errors bound the actual errors for almost all configurations in both plots. For the vertical plane, the only outlier is the maximum error of 7.1mm for $\psi=-120^{\circ}$ (11.2% of the length of the chain). The mean experimental error on the vertical plane is 2.2mm, less than half the size of a magnetic ball, corresponding to 3.5% of the total length of the chain.

In the horizontal plane of Fig. 7(b), the calibration was generally more effective in compensating for sensor noise. All experimental errors fell within the 5% noise level, with a maximum at $\psi=-90^{\circ}$ of 3.9mm (6.1% of the chain length). The mean error on this plane was 1.5mm (2.4% of the total length).

We also computed an average computation time over all data samples of 0.25s for Algorithm 1, indicating that it could be adapted for online use. Alternate real-time techniques such as extended Kalman filters could also be employed to handle dynamic configuration changes.

VI Conclusions

This paper introduced the concept of using a chain of spherical permanent magnets to generate a shape-specific magnetic field. When inserted in the lumen of a continuum robot, this field can be measured and decoded using an array of Hall effect sensors to estimate the robot shape. Our observability and sensitivity analyses confirm the potential of the method. Our experimental results, using low-cost ($25/sensor) Hall effect sensors, demonstrated mean tip position errors of 3.5% of robot length and maximum errors of less than $7.1\%$ of robot length. These results can likely be improved through the use of more accurate sensors.

While in the case considered here, the base coordinate frame of the robot was assumed known, future work will include estimation of the base frame and also consider robots comprised of multiple curving sections. Furthermore, we will consider optimizing the sensor number and arrangement with respect to the robot in order to optimally balance observability and sensitivity.

Initial testing not detailed here has shown that the magnetic field is not affected when the chain is inserted inside clinical-grade tendon-actuated catheters. Future work will study catheter compatibility in detail.