Sensorless Estimation of the Planar Distal Shape of a Tip-Actuated Endoscope

Abstract

Traditional endoscopes consist of a flexible body and a steerable tip with therapeutic capability. Although prior endoscopes have relied on operator pushing for actuation, recent robotic concepts have relied on the application of a tip force for guidance. In such case, the body of the endoscope can be passive and compliant; however, the body can have significant effect on mechanics of motion and may require modeling. As the endoscope body’s shape is often unknown, we have developed an estimation method to recover the approximate distal shape, local to the endoscope’s tip, where the tip position and orientation are the only sensed parameters in the system. We leverage a planar dynamic model and extended Kalman filter to obtain a constant-curvature shape estimate of a magnetically guided endoscope. We validated this estimator in both dynamic simulations and on a physical platform. We then used this estimate in a feed-forward control scheme and demonstrated improved trajectory following. This methodology can enable the use of inverse-dynamic control for the tip-based actuation of an endoscope, without the need for shape sensing.

I. INTRODUCTION

THE use of magnetic fields for medical robot actuation has been demonstrated for applications in gastroenterology, opthamology, otolaryngology, and cardiology [1], [2]. Magnetic actuation has been used to control devices with [3] and without [4] a tether, as well as continuum devices that include flexible endoscopes, needles, catheters, and robots [5]–[8]. The modelling of continuum devices has been thoroughly investigated where kinematic or mechanics-based computations rely on computing the configuration of a device given a known robot base, e.g. physical point where robot is at least partially constrained, and applied loads [9]–[12]. Similar methods have been applied for continuum devices that rely on magnetic fields for actuation [5], [6], [13].

In the case of tip-actuated magnetically guided flexible endoscopes that are not equipped with shape sensors [7], the entire body of the endoscope is translated via the dragging of the tip, i.e. where the internal magnet is mounted. This results in a lack of knowledge of both the robot’s base and shape which complicates modelling. Such device would thus require sensors to facilitate robotic control which may increase system complexity, potentially add calibration steps, and add cost to a system that may otherwise be made single-use. This paper addresses a need for a mechanics-compensation methodology for tethered tip-actuated systems where the tether disturbs tip motion and knowledge of tether configuration is unknown. The development of a sensorless distal shape estimation method may allow for computed-torque control of such devices.

The distal portion of a flexible endoscope may be modelled if the shape is known. Shape sensing for continuum devices has been widely used with primary methodologies being fiber Bragg gratings (FBG), electromagnetic tracking, and intraoperative imaging (fluoroscopy, ultrasound) [14]; however, these modalities cannot be trivially used in devices. Shape estimation using FBG is expensive owing to the need of an optical spectrum interrogator with multiple channels [14]. Electromagnetic sensing cannot be used with magnetic actuation systems unless the tracking and actuation systems are compatible. The use of intraoperative imaging contributes to system complexity and cost and can expose patients to radiation. An ability to estimate the configuration of the distal tip of a flexible endoscope would facilitate the development of devices that contain the benefits of sensing but are simpler and cheaper to produce; this is especially relevant for the production of single-use devices. To our knowledge, a sensorless local shape estimation technique for robots with flexible bodies has not been investigated.

In this work, we propose a method for estimating the distal shape of a magnetic endoscope that is local to its tip whose position and orientation is directly sensed. This estimator is designed for tools that are used in the body, where no clear line-of-sight with an operator exists. The key motivation of this work is the development of a model estimate that can be used for feed-forward control; thus, inaccuracies in shape sensing are acceptable in the case that the estimator can be used to enhance control. This method provides model information that would otherwise be unknown to the system. The proposed method relies on the use of a two-link planar dynamic model where the links are attached via torsion spring, and each link is attached to a mass. We employ an extended Kalman filter (EKF) for estimating the angle between the links that dictates the estimated model’s shape. We validated the shape-sensing method in both simulation and on platform experiments. We then conducted a set of trajectory control trials in simulation where the use of feed-forward control using the estimate is demonstrated to enhance motion. This method is applicable to tip-actuated tethered robots where a motion model can be developed for tip actuation, thus it is not limited to magnetic guidance.

II. SYSTEM OVERVIEW

The methods discussed in this work were validated on our magnetic flexible endoscope (MFE) system, shown in Fig. 1, that has been introduced and discussed in prior works [7]. The MFE contains a permanent magnet at its tip, which we will refer to as the “intracorporeal” permanent magnet (IM) as this magnet will be inside a patient during an endoscopy. The IM is actuated by the application of forces and torques via the motion of an extracorporeal permanent magnet (EM) that is housed at the end-effector of a six DoF serial robot (RV6SDL, Mitsubishi, Inc., Japan). Both magnets have a remanence of 1.48 T. The MFE’s tip is rigid and contains a camera, LED, irrigation channel, and a intervention channel for the passing of endoscopic tools. The body of the MFE is a Pebax flexible sleeve that houses channels and electrical wires. The MFE’s body is over 1.5 m long and completely passive. The tip of the MFE is localized in six DoF using a localization algorithm that relies on magnetic fields and has been described in our previous work [7]. The software for the system is written in Python and C++ using the Robotic Operating System [15].

Fig. 1.

The MFE system with a camera used for color-marker detection.

The estimation strategy we present in this work was first developed and tested in a simulation environment (Gazebo [16]) that was developed in our prior work [17]. We utilized a prior-made custom-made magnetic interaction physics plugin [17] for Gazebo as well as a continuum tether approximation that consists of several small rigid links connected via universal joints. After validation in simulation, we conducted experiments on our physical platform. As this work pertains to the estimation of shape, we developed an image-based tether-configuration measurement system. We used a webcam (“Creative Live!”, Creative Labs, Singapore) to detect three color markers on the tether. Processing of images was done using OpenCV [18]. The conversion from pixels to positions in the robot’s frame was achieved by registering the camera’s output using a least-squares fitting [19]. We assume all color markers were on a plane; this is an acceptable assumption as the trajectory followed by the MFE was on the horizontal, while the camera faced the vertical direction, as shown in Fig. 1. The shape estimation can be updated at an approximate rate of 28 Hz.

III. MAGNETIC ACTUATION

The magnetic actuation of the MFE relies on moving the EM to impart field gradients and field direction-changes on the IM that result in applied magnetic forces and torques. We used the dipole model which has been shown to be a valid field approximation for axially-magnetized permanent magnets with a length-to-diameter ratio of 1 [20]. The magnetic actuation method is an extension of our group’s previous work in [17]; certain concepts and definitions are repeated here for completeness. The vectors that describe the positions of the IM and EM, respectively are p_i and p_e. The relative position between magnets is given by p = p_i – p_e. The point-dipole magnetic field of the EM is represented using b_e. The respective headings of the IM and EM are given by ${\widehat{\mathbf{m}}}_i$ and ${\widehat{\mathbf{m}}}_e$ where m indicates the magnetization vector of a dipole and the notation $\widehat{\mathbf{v}}$ indicates that the vector v is of unit length. The symbol δ indicates an infinitesimal change. The magnetic force and torque are defined as f_m = (m_i · ∇)b_e and τ_m = m_i × b_e, respectively. The expression for the commanded change in magnetic wrench, δw_c, is shown in Eq. (1).

$$\begin{gathered} \delta {\mathbf{w}}_c=\left[\begin{array}{c}\hfill \delta {\mathbf{f}}_m\hfill \\ \hfill \delta {\tau }_m\hfill \end{array}\right]=\left[\begin{array}{ccc}\hfill \frac{\partial f_m}{\partial \mathbf{p}}\hfill & \hfill \frac{\partial f_m}{\partial {\widehat{\mathbf{m}}}_e}\hfill & \hfill \frac{\partial f_m}{\partial {\widehat{\mathbf{m}}}_i}\hfill \\ \hfill \frac{\partial {\tau }_{\mathbf{m}}}{\partial \mathbf{p}}\hfill & \hfill \frac{\partial {\tau }_{\mathbf{m}}}{\partial {\widehat{\mathbf{m}}}_e}\hfill & \hfill \frac{\partial {\tau }_{\mathbf{m}}}{\partial {\widehat{\mathbf{m}}}_i}\hfill \end{array}\right]\left[\begin{array}{c}\hfill \delta \mathbf{p}\hfill \\ \hfill \delta {\widehat{\mathbf{m}}}_e\hfill \\ \hfill \delta {\widehat{\mathbf{m}}}_i\hfill \end{array}\right] \\ ={\mathbf{J}}_F\left(\left[\begin{array}{c}\hfill \delta {\mathbf{p}}_i\hfill \\ \hfill 0\hfill \\ \hfill \delta {\widehat{\mathbf{m}}}_i\hfill \end{array}\right]+\left[\begin{array}{ccc}\hfill -{\mathbf{I}}_3\hfill & \hfill 0\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill {\mathbf{I}}_3\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill \end{array}\right]\left[\begin{array}{c}\hfill \delta {\mathbf{p}}_e\hfill \\ \hfill \delta {\widehat{\mathbf{m}}}_e\hfill \\ \hfill 0\hfill \end{array}\right]\right) \\ ={\mathbf{J}}_F\left[\begin{array}{c}\hfill \delta {\mathbf{p}}_i\hfill \\ \hfill 0\hfill \\ \hfill \delta {\widehat{\mathbf{m}}}_i\hfill \end{array}\right]+{\mathbf{J}}_F{\mathbf{I}}_i\left[\begin{array}{cc}\hfill {\mathbf{I}}_3\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill S{({\widehat{\mathbf{m}}}_e)}^{\mathrm{T}}\hfill \end{array}\right]{\mathbf{J}}_R\delta \mathbf{q} \\ ={\mathbf{J}}_F\left[\begin{array}{c}\hfill \delta {\mathbf{p}}_i\hfill \\ \hfill 0\hfill \\ \hfill \delta {\widehat{\mathbf{m}}}_i\hfill \end{array}\right]+{\mathbf{J}}_{\mathit{FA}}\delta \mathbf{q} \end{gathered}$$ (1)

The serial robot’s geometric Jacobian is indicated via J_R, I_n indicates an identity matrix $\in {\mathbf{ℝ}}^{n\times n}$, S(·) denotes the skew-symmetric form of the cross-product operation, I_i indicates the matrix of I₃ terms in the expression prior, and δq is the vector of infinitesimal changes in joint positions. To actuate a magnetic device we wish to determine the joint rates that will induce a desired wrench. The commanded wrench is determined by applying a velocity and position controller as defined in Eq. (2).

$$\delta {\mathbf{w}}_c=\left[\begin{array}{c}\hfill {\mathit{K}}_{\mathit{pvel}}{\mathbf{e}}_v\widehat{\mathbf{t}}+{\mathit{K}}_{\mathit{pos}}{\mathbf{e}}_p\widehat{\mathbf{n}}\hfill \\ \hfill {\mathit{K}}_{\mathit{orient}}{\mathbf{e}}_o\hfill \end{array}\right]$$ (2)

Here, $\widehat{\mathbf{t}}$ and $\widehat{\mathbf{n}}$ indicate the tangent and normal directions, on the horizontal, to the desired trajectory, respectively. The velocity error is depicted via ${\mathbf{e}}_v={\stackrel{.}{\mathbf{p}}}_{i_{\mathit{desired}}}-{\stackrel{.}{\mathbf{p}}}_i$, orientation error via ${\mathbf{e}}_o={\widehat{\mathbf{m}}}_i\times \widehat{\mathbf{t}}$ where $\widehat{\mathbf{t}}$ is the tangential direction to the desired path, and position error via e_p = p_i – p_i. The commanded trajectory is made via a Bezier curve. This commanded wrench step is not a function of tether mechanics in any way; rather, it is the same relation one would use when commanding an untethered device. Given a desired step in wrench, we define an optimization relation that favors achieving the commanded wrench, managing joint rates and joint-limit proximity [21], and maintaining a desired height of the EM; this relation is shown in Eq. (3). This is not a constrained minimization and relies on the subjective tuning of constants to give preference to the optimization of certain parameters over others.

$$\begin{gathered} \underset{\delta \mathbf{q}}{\min }({\Vert {\mathbf{W}}_w(\delta {\mathbf{w}}_c-{\mathbf{J}}_{\mathit{FA}}\delta \mathbf{q})\Vert }^2+\alpha {\Vert {\mathbf{W}}_{\mathbf{q}}^{-1}\delta \mathbf{q}\Vert }^2\phantom{)}\cdots \\ \phantom{(}+\beta {\Vert ({\mathbf{z}}_d-{\mathbf{W}}_z{\mathbf{J}}_R\delta \mathbf{q})\Vert }^2) \end{gathered}$$ (3)

Here, W_w is a diagonal weight matrix that allows for tuning the preference of achieving desired magnetic forces or torques, W_q is a weight matrix that punishes joint motion as joints approach their limits, z_d = [0,0, p_ez–des, 0,0,0]^T where p_ez–des is the desired EM height, and W_z is a matrix of zeros $\in {\mathbf{ℝ}}^{6\times 6}$ apart from its (3,3) index which is set to 1.0. Here, the use of constants α and β facilitates giving relative optimization preference between EM height control and joint rate minimization. The constant β may be set to unity; however, it is used as a variable here for conceptual clarity. Finally, ${\mathbf{J}}_R\in {\mathbf{ℝ}}^{6\times 6}$ is the geometric Jacobian of the serial manipulator. The resultant joint rate is computed analytically and the command is sent to the robot controller.

IV. ESTIMATION METHOD

Our proposed method for the 2D local shape estimation of the MFE relies on fusing the following elements: (1) the sensed position and orientation of the MFE’s tip, (2) the magnetic wrench that is applied on the MFE’s tip, which is estimated using the dipole model, and (3) a dynamic model of the MFE’s tip and body in a near vicinity of the tip (approximately 15 cm). We chose to use a variant of the Kalman filter as it is a recursive Bayesian estimator that requires a single integration of dynamics per time step. We note that we found this dynamic model to be a good fit for our application; however, it is not the only model that can be used. We begin by describing the dynamic model, then the EKF, and finally we discuss the choice of model parameters.

A. Dynamic Modelling

The proposed dynamic model consists of two disks, each rigidly connected to a link, with the links connected via a torsion spring; shown in Fig. 2. The following are descriptions of parameters used for dynamic modelling: m_i indicates the respective mass of disks where the pose of mass 1 corresponds to the pose of the MFE’s tip, I_i indicates the moment of inertia of a respective disk, k_s is the torsion spring constant, L_i is the respective length of a link connected to mass m_i, b_i is a linear damping coefficient, and D_i is an angular damping coefficient. The states of the system consist of the MFE tip (m₁) position, linear velocity, orientation, and angular velocity, as well as the orientation and angular velocity of Mass 2 (m₂). Feedback is available on the tip’s pose and twist, but not on the state of m₂. These states are defined explicitly in Eq. (4) and Eq. (5).

Fig. 2.

Schematic of a dynamic two-link and two-mass model with a best-fit curve of constant curvature.

$$\mathbf{x}={[{\mathbf{p}}_1,{\stackrel{.}{\mathbf{p}}}_1,{\theta }_1,{\stackrel{.}{\theta }}_1,{\theta }_2,{\stackrel{.}{\theta }}_2]}^{\mathrm{T}}\in {\mathbf{ℝ}}^8$$ (4)

$$\mathbf{y}={[{\mathbf{p}}_1,{\stackrel{.}{\mathbf{p}}}_1,{\theta }_1,{\stackrel{.}{\theta }}_1]}^{\mathrm{T}}\in {\mathbf{ℝ}}^6$$ (5)

The dynamic model was written by deriving the system’s Lagrangian that is shown in Eq. (6).

$$\begin{gathered} L=K-V=\frac{1}{2}(m_1{\stackrel{.}{\mathbf{p}}}_1^{\mathrm{T}}{\stackrel{.}{\mathbf{p}}}_1+m_2{\stackrel{.}{\mathbf{p}}}_2^{\mathrm{T}}{\stackrel{.}{\mathbf{p}}}_2\phantom{)} \\ +\phantom{(}{I}_1{\stackrel{.}{\theta }}_1^2+I_2{\stackrel{.}{\theta }}_2^2-k_s{({\theta }_2-{\theta }_1)}^2) \end{gathered}$$ (6)

where

$${\mathbf{p}}_2={\mathbf{p}}_1-L_1\left[\begin{array}{c}\hfill \mathit{cos}({\theta }_1)\hfill \\ \hfill \mathit{sin}({\theta }_1)\hfill \end{array}\right]-L_2\left[\begin{array}{c}\hfill \mathit{cos}({\theta }_2)\hfill \\ \hfill \mathit{sin}({\theta }_2)\hfill \end{array}\right]$$ (7)

The Lagrangian expression in Eq. (6) was computed symbolically using Matlab (Mathworks Inc., USA) via Eq. (8) and compiled into a Python library, which was then interfaced-with using a ROS node.

$$\frac{d}{dt}\frac{\partial L}{\partial {\stackrel{.}{g}}_{c_i}}-\frac{\partial L}{\partial g_{c_i}}-n.c.=0$$ (8)

Here, g_ci denotes the i_th of the four generalized coordinates (p_1x, p_1y, θ₁, θ₂, where p₁ = [p_1x,p_1y]^T), and “n.c.” denotes the non-conservative forces applied on the system that include linear and angular viscous friction (each applied on both masses) and externally applied magnetic forces and torques. Eq. (8) was then solved for each ${\ddot{g}}_{c_i}$ to write a nonlinear state-transition expression as shown in Eq. (9). Here, w_m indicates the applied magnetic wrench that is estimated via the dipole model; this is possible owing to localization feedback of IM pose and kinematic feedback of the serial manipulator that is used to obtain EM pose. The complete expression for ${\ddot{g}}_{c_i}$ is omitted here for brevity. The nonlinear state-transition matrix is indicated using g(x_k–1,w_m); the complete expression is omitted for brevity. This state transition equation dictates how system state is updated based on time and input parameters, i.e. magnetic wrench.

$${\mathbf{x}}_k={\mathbf{x}}_{k-1}+{\stackrel{.}{\mathbf{x}}}_{k-1}\delta t=\mathbf{g}({\mathbf{x}}_{k-1},{\mathbf{w}}_m)$$ (9)

The expression for the measurement model, h(x_k–1), is shown in Eq. (5). The measurement, z_k, is identical to the measurement model and is obtained using a magnetic localization method described in Ref. [7] that provides six DoF pose feedback; velocities are obtained via discrete low-pass-filtered differentiation. As our estimation is in 2D, we utilize 2 DoFs of position feedback and 1 DoF of orientation feedback. The objective of this estimator is to recover the value of θ₂ and to use it to make a continuum-shape estimate. The functionality of the estimator can be conceptualized with the following question: “what is the state of the model that would result in the deflection that was observed?”. The possibility of this estimation is contingent on the observability of the state vector. As this system is nonlinear, we resort to determining local observability via linearizing the state-transition and measurement models. We verified the rank of the observability matrix O, Eq. (10), to be equal to the number of system states [22], where “n” denotes the number of system states. We found this model to be locally observable and remains observable when the only feedback information is the location of p₁.

$$\mathbf{O}(\mathbf{x},\mathbf{u})=\left[\begin{array}{c}\hfill \frac{\partial \mathbf{h}}{\partial \mathbf{x}}(\mathbf{x})\hfill \\ \hfill \frac{\partial \mathbf{h}}{\partial \mathbf{x}}(\mathbf{x})\frac{\partial \mathbf{g}}{\partial \mathbf{x}}(\mathbf{x},\mathbf{u})\hfill \\ \hfill ⋮\hfill \\ \hfill \frac{\partial \mathbf{h}}{\partial \mathbf{x}}(\mathbf{x})\frac{\partial \mathbf{g}}{\partial \mathbf{x}}{(\mathbf{x},\mathbf{u})}^{n-1}\hfill \end{array}\right]$$ (10)

The prediction step of the EKF relies on using the dynamic model to obtain an a priori state and state error covariance; these are marked via ${\stackrel{‒}{\mathbf{x}}}_k$ and ${\stackrel{‒}{\mathbf{P}}}_k$, respectively. These relations are shown in Eq. (11) and Eq. (12), where Q_k is the process noise covariance and P_k–1 is the state error covariance.

$${\stackrel{‒}{\mathbf{x}}}_k=\mathbf{g}({\mathbf{x}}_{k-1},{\mathbf{w}}_m)$$ (11)

$${\stackrel{‒}{\mathbf{P}}}_k=\frac{\partial g({\mathbf{x}}_{k-1})}{\partial \mathbf{x}}{\mathbf{P}}_{k-1}\frac{\partial g{({\mathbf{x}}_{k-1})}^T}{\partial \mathbf{x}}+{\mathbf{Q}}_k$$ (12)

The correction step of the EKF relies on using a priori state as predicted via dynamic model and fusing the prediction with sensory information to obtain an a posteriori state update. These relations are shown in Eq. (13), Eq. (14), and Eq. (15), where R_k is the measurement covariance. The Kalman gain, K_k, acts as a weight of confidence between the dynamic model and state measurement.

$${\mathbf{K}}_k={\stackrel{‒}{\mathbf{P}}}_k\frac{\partial h{({\stackrel{‒}{\mathbf{x}}}_k)}^{\mathrm{T}}}{\partial \mathbf{x}}{(\frac{\partial h({\stackrel{‒}{\mathbf{x}}}_k)}{\partial \mathbf{x}}{\stackrel{‒}{\mathbf{P}}}_k\frac{\partial h{({\stackrel{‒}{\mathbf{x}}}_k)}^{\mathrm{T}}}{\partial \mathbf{x}}+{\mathbf{R}}_k)}^{-1}$$ (13)

$${\mathbf{x}}_k={\stackrel{‒}{\mathbf{x}}}_k+{\mathbf{K}}_k({\mathbf{z}}_k-h({\stackrel{‒}{\mathbf{x}}}_k))$$ (14)

$${\mathbf{P}}_k=(\mathbf{I}-{\mathbf{K}}_k\frac{\partial h({\stackrel{‒}{\mathbf{x}}}_k)}{\partial \mathbf{x}}){\stackrel{‒}{\mathbf{P}}}_k$$ (15)

Finally, a curve with a constant radius is fitted to the masses such that it is tangent to the heading of Mass 1, and passes through the position of Mass 2. An arc with a constant curvature, which results in a pure and constant bending assumption, was used in this work as it is sufficient for obtaining an approximate shape that can be used in feed-forward control. The constant-curvature assumption is not necessarily a valid one; however, it may provide information that is useful for control which is more valuable than no estimate at all.

B. Model Parameters

The selection of parameter values for the proposed dynamic model presents a unique problem owing to the disconnect in the mechanics of our model and the physical system. Typically continuum robots are modelled with a known base, while our system consists of a known tip onto which an actuating wrench is applied. Our model does not accurately depict the phenomenon of a long tether being dragged, but rather can usefully depicts local mechanics. We began with choosing a torsional spring constant. We conducted a set of 10 experiments where a serial arm with a force sensor at its end-effector was used to deflect the tip of the MFE; this resulted in a torsional spring constant of 0.033 ± 0.005 Nm/rad. This value over-estimates the true constant as the tether would translate when a force would be applied at the tip. We thus scaled this constant by 0.5 in simulations and by 0.3 in platform experiments; values that were chosen based on observed performance. The rest of dynamic parameters were subjectively chosen based on observing the estimated state: disk-radii r₁ = 0.02 and r₂ = 0.02, link lengths L₁ = 0.02 and L₂ = 0.08, masses m₁ = 0.05kg and m₂ = 0.4kg, linear damping coefficients b₁ = 0.005 and b₂ = 0.001 N · s/m, angular damping coefficients D₁ = 0.005 and D₂ = 0.05 N · m · s.

The measurement covariance matrix R_k was defined based on localization error variance as computed in Ref. [7]; the linear position noise was up to 5 mm while the angular position noise was up to 6.0°. The process noise covariance was set to an diagonal matrix with identical components of 0.2. We found that a low value of process noise resulted in a lag in the estimate. The estimator does not appear to be sensitive to process noise when a larger value is chosen. The state covariance was initialized to be a diagonal matrix with small values.

C. Feed-Forward Magnetic Control

Feed forward magnetic control is implemented by augmenting Eq. (2) to Eq. (16) such that a torque compensation term is applied. The torque compensation is a proportional control that is a function of the torsion spring angle. This proposed controller may apply a feed-forward torque when the heading error of endoscope is null.

$$\begin{gathered} \delta {\mathbf{w}}_{ff}=\delta {\mathbf{w}}_c+\delta {\mathbf{w}}_{\mathit{spring}} \\ =\left[\begin{array}{c}\hfill {\mathit{K}}_{p_{vel}}{\mathbf{e}}_v\widehat{\mathbf{t}}+{\mathit{K}}_{\mathit{pos}}{\mathbf{e}}_p\widehat{\mathbf{n}}\hfill \\ \hfill K_{\mathit{orient}}{\mathbf{e}}_o\hfill \end{array}\right] \\ +{[0,0,0,0,0,-{\mathit{K}}_{p_s}{k}_s({\theta }_2-{\theta }_1)]}^{\mathrm{T}} \end{gathered}$$ (16)

V. EXPERIMENTAL VALIDATION

The experimental validation in this work consists of shape estimation validation experiments as well as feed-forward control validation experiments. The shape estimation experiments were conducted both in simulation and a physical platform. Feed-forward control experiments were conducted in simulation.

A. Validation of Shape Estimation

To validate the proposed shape estimation technique, we conducted experiments in simulation, as well as on a physical platform. The position-sensor points in simulation and on the physical platform were placed in approximately the same position on the MFE: at 6 cm, 10 cm, and 14.5 cm along the tether length. As the number of position sensors available to us on the physical platform was limited, we developed an error metric that relies on measuring the distance between each sensor point and the nearest point on our estimated curve. We define the error metric e_est in Eq. (17).

$$e_{\mathit{est}}=\frac{1}{y}\sum _{i=2}^y\frac{\Vert {\mathbf{p}}_{s_i}-{\mathbf{p}}_{m_i}\Vert }{d_i}$$ (17)

where p_si is the sensor position, p_mi is the nearest model point to p_si, y is the number of position sensor points (1 localization sensor at tip, 3 position sensors along tether shape, 4 total); the sensor points start at 2 as the tip of the model is a known sensory feedback, while the rest of the model shape is unknown. The error term contains a normalization that is implemented via a quotient of d_i; as a point closer to the tip has a smaller error (model tip is given via pose sensor), a small error near the tip may carry the same amount of information as a much larger error farther away from the tip. The value of d_i is the nominal distance of the sensed point along the body of the endoscope.

To evaluate the performance of our estimate, we compare our estimated shape to an assumption that the tether is straight behind the MFE’s tip; we refer to this as the “baseline estimate”. We used this baseline comparison since a crucial aspect of our approach is ensuring that the estimated shape is on the correct side of the baseline estimate. Knowing which side of the IM the tether is on gives insight into the direction of wrench disturbance it will induce on motion. The precise knowledge of the exact shape is not critical, nor do we expect to recover it. For this baseline estimate, we also compute the error metric as defined in Eq. (17); we refer to this error via e_baseline. The success criteria for this validation is the estimate outperforming the baseline estimate.

1). Simulation Study:

We conducted two experiments, 10 trials each, in simulation. In each experiment, the MFE was commanded to autonomously follow a trajectory using velocity control in the tangential direction to the path, and position control in the normal direction to the path. The total length of the tether used in simulation was 45 cm and the length of the sensed portion of the tether was 14.6 cm. The trajectory in the first experiment was a double-bend curve (segment lengths: 13, 18, 20 cm) and in the second, it was a sinusoidal (0.4 m length, 0.05 amplitude, single period). In Fig. 3 we show these trajectories along with the measured, estimated, and baseline shapes. The double-bend-trajectory experiment resulted in e_baseline = 0.95±0.03 and e_est = 0.26±0.02. The sinusoidal-trajectory experiment resulted in e_baseline = 0.54±0.01 and e_est = 0.21±0.01. The results are shown in Fig. 4 and Fig. 5 using box plots. The regions of higher error correspond to regions of tether deflection caused by the MFE passing through a bend. To validate the evaluate the effect of environmental friction on the method, the friction of the simulation model was increased by a factor of 5 (from 0.1 to 0.5) while all other model parameters were unchanged which resulted in the following errors: e_baseline = 1.00±0.04 and e_est = 0.37±0.03. For reference, Terry et al. reported a coefficient of friction of 0.016±0.002 in vivo between tissue and polycarbonate [23].

Fig. 3.

Trajectories that the MFE was commanded to follow during simulation experiments. Trajectory 1 (a,b) consists of two-bend path, and trajectory 2 (a,b) consists of a sinusoidal path.

Fig. 4.

Experimental results for the double-bend simulation trajectory. The green indicates e_baseline results and the blue indicates e_est results.

Fig. 5.

Experimental results for the sinusoidal simulation trajectory. The green indicates e_baseline results and the blue indicates e_est results.

2). Experiments on a Physical Platform:

The physical platform trials were conducted with the MFE being teleoperated along a single-bend trajectory (segments: 11 cm, 10 cm). Teleoperation here refers to a the EM being directly commanded via joystick and it was used rather than autonomous control owing to the high-friction environment of the acrylic tube that resulted in the autonomous controller applying too much vertical force. The use of sufficient lubricant resulted in difficulties with visual detection of color-markers. The use of this teleoperation is not clinically relevant but it allowed for the evaluation of our estimation algorithm. In the case of a color marker being occluded, the measurement node did not send a data update.

As our estimation algorithm is a stand-alone program independent of magnetic closed-loop control, shape estimation was carried out in the same manner as during simulation trials. As a human was in-the-loop, the estimated shape of the MFE was not displayed during the recording of data to prevent a bias in attempting to steer the estimate. An example camera-view and trajectory path is shown in Fig. 6. The experiment resulted in e_baseline = 1.06±0.05 and e_est = 0.87±0.07. The results are shown using box plots in Fig. 7. The region of high error in the latter portion of the trajectory is likely due to the unrealistically high friction in the experimental setup where sufficient lubrication could not be used owing to camera occlusion of markers on the tether. This limitation could be overcome by using a non-visual localization technique; however, this is non-trivial owing to our inability to use magnetic tracking and difficulty in integrating FBG sensors.

Fig. 6.

(a) A sample view from the camera used for tether shape measurement and (b) the trajectory followed by a teleoperated MFE.

Fig. 7.

Experimental results for the single-bend platform trajectory. The green indicates e_baseline results and the blue indicates e_est results.

B. Feed-Forward Control Validation

The following simulation experiments demonstrate the use of the estimated state in the magnetic control loop as described in Eq. (16). The spring angle was used to pass an additional torque term to the magnetic controller and the influence of this term on heading error was evaluated. The key benefit of this controller is that it applies a torque to compensate for tether disturbance even when the IM heading error is null. The MFE is tasked with traversing the double-bend curve (Fig. 3(1a,1b)). The torque compensation term, δw_spring, was only applied when the spring torque would reduce heading error. This expression is then used in Eq. (3) to compute robot joint commands. Instances occurred where the estimator predicted the tether to be on the wrong side of the true measurement. Adding a torque in such a case may result in error divergence. It should be noted that simply increasing heading-control gains may demonstrate similar improvements; however, increasing heading control gains influences the ability to control position as, in terms of control, the position and orientation of the IM are coupled. In other words, constantly applying high orientation control gains will induce a system preference for reducing heading error, at the expense of position control.

Ten simulation experiments were conducted with δw_spring, and ten without this torque-compensation term. The mean heading errors without and with compensation, respectively, were 11.8±1.7° and 7.6±1.0°; a 35% improvement. The respective position errors, normal to the path, were 3.8±0.5° and 4.6±0.5°. The position errors were small in both cases considering the total trajectory length was approximately 0.5 m. The slight increase in position error when using torque-compensation is likely due to the magnetic controller giving preference to applying magnetic torque over force which has a slight adverse effect on translation control. Further work is needed to investigate the possibility of force-compensation.

VI. DISCUSSION

The proposed local shape estimation method successfully performs better than a baseline estimate. This means that the estimated shape is on the correct side of the rear of the MFE’s tip, which is crucial for enabling the formulation of an estimated wrench disturbance that the tether is inducing on MFE motion. Our estimation algorithm operates at 28 Hz, thus providing rapid configuration updates. The estimator does not perfectly track the local tether shape as: (1) the dynamic model is not an exact representation of tether mechanics, (2) we use a constant-curvature assumption when generating the local shape when the real device shape does not necessarily form a local constant curve, and (3) our model captures disturbances that result from mechanics, while in reality, the MFE is also subject to disturbances from friction and environmental interaction; such effects cannot be captured using a dynamic model. A shortcoming of this work is the lack of a robust methodology for choosing dynamic model parameters. A system-specific subjective choice of parameters may be acceptable as this parameter choice must occur only once per estimated device, the parameters have physical meaning, and there are less than 20 of them. A more precise approach may involve the recording of a large data set of measured shape changes as induced via known applied wrenches. Given these same wrenches, a regression approach may be used to fit the dynamic model’s parameters. This may be useful for reducing estimation error, but is not necessary for using our proposed method. Future work is necessary to evaluate the performance of our proposed approach in a colonic simulator as well as in vivo.

VII. CONCLUSION

We have proposed an algorithm for the sensorless shape estimation of the distal end of a flexible endoscope. Our method uses information that was previously treated as disturbance noise to the path of the endoscope’s tip, and computes the most probable configuration of the flexible body of the endoscope. This is akin to computing simple inverse-kinematics by observing the deviation of the tip’s path. The method is applicable for dynamic models where a curvature parameter is observable. Our results suggest feasibility of the method and a potential for a future application in applying inverse-dynamic control to control the motion of tip-actuated endoscopes.