Model predictive control of a robotically actuated delivery sheath for beating heart compensation

Abstract

Minimally invasive surgery (MIS) during cardiovascular interventions reduces trauma and enables the treatment of high-risk patients who were initially denied surgery. However, restricted access, reduced visibility and control of the instrument at the treatment locations limits the performance and capabilities of such interventions during MIS. Therefore, the demand for technology such as steerable sheaths or catheters that assist the clinician during the procedure is increasing. In this study, we present and evaluate a robotically actuated delivery sheath (RADS) capable of autonomously and accurately compensating for beating heart motions by using a model-predictive control (MPC) strategy. We develop kinematic models and present online ultrasound segmentation of the RADS that are integrated with the MPC strategy. As a case study, we use pre-operative ultrasound images from a patient to extract motion profiles of the aortic heart valve (AHV). This allows the MPC strategy to anticipate for AHV motions. Further, mechanical hysteresis in the steering mechanism is compensated for in order to improve tip positioning accuracy. The novel integrated system is capable of controlling the articulating tip of the RADS to assist the clinician during cardiovascular surgery. Experiments demonstrate that the RADS follows the AHV motion with a mean positioning error of 1.68 mm. The presented modelling, imaging and control framework could be adapted and applied to a range of continuum-style robots and catheters for various cardiovascular interventions.

1. Introduction

According to the World Health Organization, cardiovascular diseases are among the leading causes of death globally (Alwan et al., 2011). Technological developments have the ability to improve and enable treatment of cardiovascular diseases (Himbert et al., 2009; Walther et al., 2009; Wong et al., 2010). Traditionally, open surgery via sternotomy is performed to gain surgical access to the heart, while a heart–lung machine provides life support. Accessibility to the heart is considered to be a major advantage during open surgery, which is at the expense of severe patient trauma. As an alternative, minimally invasive surgery (MIS) could reduce trauma and enable the treatment of high-risk patients who were initially denied surgery (Alfieri et al., 2007; Cribier et al., 2004; Himbert et al., 2009; Seeburger et al., 2010; Ye et al., 2010). However, limited access, vision and control of the instrument at the treatment locations impedes the performance of MIS. During interventions performed without a heart–lung machine, often beating heart motion compensation is desired and could potentially enable future cardiovascular interventions. Tracking of the beating heart requires the attention of the surgeon, whose accuracy deteriorates for complex repetitive motions up to 60 beats per minute (BPM) (Falk, 2002). Therefore, there is a demand for technology that assists the clinician via shared control during the procedure.

In this study, we focus on a model-predictive control (MPC) strategy that anticipates and compensates for the beating heart motion (Figure 1). The MPC strategy is used to compensate the beating heart motion by using a robotically actuated delivery sheath (RADS). Another advantage of MPC is the ability to integrate motion constraints on the instrument in order to prevent damage to sensitive tissue during the procedure. As a case study, we use transcatheter aortic valve implantation (TAVI) to demonstrate the potential of our MPC strategy. Further, applications such as ablation, valve repair surgery and mitral valve chordal replacement can potentially benefit from such an MPC strategy. TAVI can be performed via the transfemoral (TF) or transapical (TA) routes. In this case study, we focus on the TAVI-TA approach, which provides direct surgical access to the aortic and mitral heart valves (Johansson et al., 2011; Ye et al., 2010).

Fig. 1.

Model predictive control (MPC) can be used to steer the robotically actuated delivery sheath (RADS) in order to assist the clinician during cardiovascular surgery. The potential of MPC in cardiovascular surgery can be demonstrated by compensating for the aortic heart valve (AHV) motion in a representative case of transapical transcatheter aortic valve implantation. The RADS ① is inserted through the apex ② into the left ventricle ③ and oriented towards to the aortic annulus ④. The articulating tip ⑤ of the RADS can be steered inside the left ventricle under ultrasound ⑥ image guidance in two degrees of freedom by two pairs of antagonistically configured tension wires. Pre-operative ultrasound data ⑦ can be used as an input to the MPC strategy to anticipate and compensate the AHV motion during surgery.

1.1. Related work

Recent studies have demonstrated the capabilities of MIS in cardiovascular applications such as angioplasty and patent foramen ovale (Gosline et al., 2012; Jayender et al., 2008). These studies describe the use of robotic instruments in order to enable MIS. However, MIS in cardiovascular applications such as coronary artery bypass, ablation and valve surgery could significantly benefit from beating heart compensation. Active robotic stabilization at the treatment location could provide a virtually still scenario that allows the clinician to perform the primary task of the procedure as if the heart was stopped. Compensation of beating heart motions has been extensively covered in research. Studies that focused on the beating heart surface have been described (Bebek and Cavusoglu, 2007; Gangloff et al., 2006; Ginhoux et al., 2005; Nakamura et al., 2001; Ortmaier et al., 2005; Richa et al., 2010; Tuna et al., 2013). These studies cover various predictive strategies by filtering and control to anticipate the beating heart motions. Ortmaier et al. (2005) reported significant correlations between beating heart motions and electrocardiogram (ECG) signals, which were utilized for synchronization by Bebek and Cavusoglu (2007). The majority of these studies used predictive strategies to improve accuracy and to anticipate for occlusions in endoscopic camera images, while this study focuses on ultrasound-guided repair surgery in the beating heart.

Cardiovascular procedures such as ablation and valve-implantation or -repair surgery are often assisted by two-dimensional (2D) or three-dimensional (3D) ultrasound, which are used to evaluate cardiac functionalities (e.g. valve closing) (Walther et al., 2009). The presence of ultrasound imaging during the procedure provides the potential for online soft tissue and instrument tracking. Yuen et al. (2008) evaluated predictive filtering to enable ultrasound-guided robotic tracking of cardiac motions for potential use in ablation. A study by Kesner and Howe (2014) demonstrated capabilities of applying a constant force against a moving target by using ultrasound image feedback and force sensing. Whereas Bowthorpe et al. (2014) focused on predictive control using an ultrasound-guided tele-operated robot to anticipate for cardiac motions. All of these methods describe one dimensional motion compensation, which is along the lateral axis of the instrument. However, existing applications in valve implantation and repair surgery such as mitraclip placement could benefit from motion compensation in two or more degrees of freedom (DOFs) (Tamburino et al., 2010; Wong et al., 2010; Ye et al., 2010). Further, beating heart motion compensation in two or more DOFs could potentially enable future applications in minimally invasive cardiovascular surgery. In this study, we cover an integrated system capable of autonomously and accurately compensating for beating heart motions in two dimensions by using an MPC strategy. Further, our MPC strategy guided by ultrasound imaging considers active constraint handling in order to avoid damage to sensitive tissue. Major aspects in MPC of the integrated system are kinematic modelling of continuum-style robots and feedback on its pose.

Kinematic modelling is used to describe the RADS shape and articulating tip motion. Modelling of continuum-style robots have been investigated by several groups (Bardou et al., 2010; Camarillo et al., 2008; Ding et al., 2010; Dupont et al., 2010; Reiter et al., 2012; Rone and Ben-Tzvi, 2014; Webster and Jones, 2010). Various continuum-style robotic instruments with potential in cardiovascular applications have been described in literature. These devices can be mainly classified into tendon-driven robots and pre-bent concentric tubes. We describe the kinematics of the tendon-driven constant-curvature RADS using both robot-specific and -independent submappings (Webster and Jones, 2010). The kinematics presented in this study considers a rigid link which is attached to the flexible segment of the RADS. However, modelling mismatches and disturbances acting on the system affect the tip positioning performance of continuum-style robots.

Tracking of continuum-style robots plays an important role in order to reduce control tracking errors caused by modelling mismatches and external disturbances. In TAVI, the clinician is often assisted by 2D and 3D transoesophageal or transthoracic echocardiography (Walther et al., 2009). 2D ultrasound has been used for 2D and 3D tracking of flexible instruments by several groups (Hong et al., 2004; Neshat and Patel, 2008; Neubach and Shoham, 2010; Vrooijink et al., 2013, 2014). 3D ultrasound-based tracking of cardiac catheters (some equipped with markers) have also been described in the literature (Koolwal et al., 2010; Nadeau et al., 2015; Novotny et al., 2007; Stoll et al., 2012). In this study, we integrate an online and a robust tracking algorithm that uses 2D ultrasound images of the RADS.

1.2. Contributions

In this study, we demonstrate an integrated system that assists the clinician by compensating for beating heart motions, which could be used in existing and potential future cardiovascular interventions. By active stabilization of the instrument in the beating heart, a virtually still treatment location could be provided. This allows the clinician to perform the procedure as if the heart was stopped. Such a system requires instrument modelling and tracking, which is integrated in a control strategy capable of anticipating for beating heart motions. In this study, we incorporate the forward, inverse and differential kinematic models of the RADS, which considers a rigid link or a tool attached to the flexible tip segment. Subsequently, we integrate a robust 2D ultrasound image segmentation algorithm capable of online evaluation of the RADS tip position. Furthermore, we provide the control objective and corresponding MPC strategy, which considers kinematic modelling and ultrasound feedback. The MPC strategy anticipates for beating heart motions by incorporating models based on pre-operative patient data. In addition to the aforementioned contributions, the MPC strategy considers active constraint handling in order to prevent damage to sensitive tissue. To the best of the authors’ knowledge, we are the first to present an ultrasound-guided MPC strategy capable of compensation for beating heart motions in 2D using a RADS. As a case study, we focus on TAVI-TA by integrating an aortic heart valve (AHV) model in the MPC strategy, which is based on pre-operative 2D ultrasound patient images that describes the AHV motion during the cardiac cycle. The presented strategy for beating heart compensation could be applicable to a wide variety of existing and potential future cardiovascular interventions. Further, we improved the RADS tip positioning accuracy by compensating for instrument hysteresis. Three experimental scenarios using a water container setup demonstrate MPC of the novel integrated system that autonomously and accurately compensates for AHV motions.

2. Methods

This section presents techniques to enable tracking and MPC-based steering of the RADS. Details on the design of the RADS are provided by Vrooijink et al. (2014). We summarize the forward, inverse and differential kinematics which are used in MPC. Furthermore, we describe the 2D ultrasound segmentation method of the RADS used as feedback for MPC. The MPC strategy includes modelling of the heart valve motion based on pre-operative 2D ultrasound data. By combining the kinematic modelling and segmentation of the RADS with MPC, the system is able to anticipate the beating heart motion. Note, in the derivations presented, for notational simplicity, we leave out the discrete time variable denoted by k (except where needed for clarity). Further, the nomenclature describing the variables used in this study is provided in Appendix B.

2.1. Instrument modelling

2.1.1. Forward kinematics

The design and integration of the tendon-driven RADS used in this study is shown in Figure 2. Two pairs of antagonistically configured tension wires are used to actuate the articulating tip of the RADS in two DOFs. Each pair of wires is actuated by a single pulley and controls tip movement in a single DOF (Figure 2). The four-wire design allows for tip movement in two DOFs by using two actuators instead of the three that are required in an instrument with three wires. A kinematic model of the tendon-driven continuum-style robot can be described by a robot-specific and a robot-independent submapping (Webster and Jones, 2010). The robot-specific mapping relates the actuator space to the configuration space. The actuator space is given by the angles of the pulleys (${\psi }_x$ and ${\psi }_y$), while the configuration space is described by arc parameters such as arc curvature ($\kappa$), arc plane angle ($\varphi$) and arc length ($\ell$)). The robot-independent mapping transforms the configuration space to the task space (intermediate frame (${\Psi }_i$)).

Fig. 2.

An overview of the robotically actuated delivery sheath (RADS). The articulating tip of the RADS is actuated in two degrees of freedom by two pairs (red and green) of antagonistic-configured tension wires driven by a two pulleys with radii ($r_p$) and angles (${\psi }_x$ and ${\psi }_y$). Three coordinate systems are assigned to describe the tip pose of the RADS: ${\Psi }_0$ is the reference frame fixed to the shaft, ${\Psi }_i$ is the intermediate frame assigned to the arc section and frame (${\Psi }_t$) is fixed to the articulating tip. Displacement of the tension wires ($t_1,\dots ,t_4$) by ${\delta }_x$ and ${\delta }_y$ (inset centre) results in instrument bending along the x- and y-axes (frame (${\Psi }_0$)), respectively. The arc of the RADS with parameters, bend angle ($\theta$), backbone length ($\ell$), radius (r) and curvature ($\kappa$) lies in a plane described by the arc plane (inset right). The orientation of the arc plane about the z-axis of the reference frame (${\Psi }_0$) is denoted by angle ($\varphi$). Further, the tendon distance to the backbone arc ($\ell$) is denoted $d_b$. A rigid link (not completely shown) of length ($l_t$) is attached to the arc (frame (${\Psi }_i$)) of the RADS.

In order to evaluate the arc parameters of the configuration space, the relation between tendon manipulation at the base and the resulting arc needs to be described. In the derivation presented, we denote for notational simplicity: $c_{*}=\cos (*)$ and $s_{*}=\sin (*)$. The RADS used in this study is actuated using four tendons ($t_i$), where $(i=1,\dots ,4$) with corresponding tendon lengths ($l_i$) as shown in Figure 2 (inset centre). The relationship between the arc length of the RADS ($\ell$) and the arc length of a single tendon ($l_i$) is given by

$$\ell =l_i+\theta d_b{c}_{{\varphi }_i}$$ (1)

where $\theta$ is the bend angle, $d_b$ denotes the distance between the backbone and tendon and ${\varphi }_i$ describes the angle between the bending direction of the RADS and the location of a single tendon ($t_i$). Note that the distance ($d_b$) in our instrument is equal for all tendons (Figure 2). Further, the bend angle ($\theta$) is related to the curvature by $\theta =\kappa \ell$.

Given the tendon configuration as illustrated in Figure 2, we can formulate the individual tendon angles by $c_{{\varphi }_1}=c_{\varphi }$, $c_{{\varphi }_2}=s_{\varphi }$, $c_{{\varphi }_3}=-c_{\varphi }$ and $c_{{\varphi }_4}=-s_{\varphi }$. By combining the individual tendon angles with (1), we can obtain an expression between the arc length ($\ell$) of the RADS and the individual tendon lengths ($l_i$), which yields

$$\ell =\frac{l_1+l_2+l_3+l_4}{4}$$ (2)

The antagonistic configuration allows for pairing of the tendons described by $t_1$ and $t_3$, and $t_2$ and $t_4$. By using the tendon pairs and (2), the arc plane angle ($\varphi$) is obtained according to

$$\varphi =\arctan \left(\frac{l_4-l_2}{l_3-l_1}\right)$$ (3)

while the curvature is evaluated as

$$\kappa =\frac{(l_1-3l_2+l_3+l_4)\sqrt{{(l_4-l_2)}^2+{(l_3-l_1)}^2})}{d_b(l_1+l_2+l_3+l_4)(l_4-l_2)}$$ (4)

Note that the arc parameters ($\ell$, $\varphi$ and $\kappa$) described in (2)–(4) are functions of the individual tendon lengths ($l_i$). These individual tendon lengths ($l_i$) are manipulated by displacements (${\delta }_x$) and (${\delta }_y$), which are introduced by two actuated pulleys (Figure 2). The relation between tendon displacements (${\delta }_x$ and ${\delta }_y$) and pulley angles (${\psi }_x$ and ${\psi }_y$) are given by ${\delta }_x=r_p{\psi }_x$ and ${\delta }_y=r_p{\psi }_y$, where $r_p$ describes the pulley radius (equal radii for both pulleys). Hence, we can use (1) to formulate an expression for all tendon lengths ($l_i$) as a function of the pulley angles (${\psi }_x$ and ${\psi }_y$) by $l_1=\ell -r_p{\psi }_x$, $l_2=\ell -r_p{\psi }_y$, $l_3=\ell +r_p{\psi }_x$ and $l_4=\ell +r_p{\psi }_y$. Substituting the expressions for the tendon lengths ($l_i$) into (3) and (4) yields

$$\varphi =\arctan \left(\frac{{\psi }_y}{{\psi }_x}\right)$$ (5)

and

$$\kappa =\frac{r_p\sqrt{{\psi }_x^2+{\psi }_y^2}}{\ell d_b}$$ (6)

respectively. Hence, we obtain the arc parameters ($\kappa$, $\varphi$ and $\ell$) of the configuration space as a function of the pulley angles (${\psi }_x$ and ${\psi }_y$) of the actuator space.

The robot-independent mapping is given by the homogeneous transformation matrix (${\mathbf{H}}_i^0\in \mathrm{SE}3$), where

$${\mathbf{H}}_i^0=\left[\begin{array}{cccc}{c}_{\varphi }{c}_{\kappa \ell }\hfill & -s_{\varphi }\hfill & c_{\varphi }{s}_{\kappa \ell }\hfill & \frac{c_{\varphi }(1-c_{\kappa \ell })}{\kappa }\hfill \\ \multicolumn{1}{c}{s_{\varphi }{c}_{\kappa \ell }}& c_{\varphi }\hfill & s_{\varphi }{s}_{\kappa \ell }\hfill & \frac{s_{\varphi }(1-c_{\kappa \ell })}{\kappa }\hfill \\ \multicolumn{1}{c}{-s_{\kappa \ell }}& 0\hfill & c_{\kappa \ell }\hfill & \frac{s_{\kappa \ell }}{\kappa }\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 0\hfill & 1\hfill \end{array}\right]$$ (7)

which describes the mapping between the intermediate frame (${\Psi }_i$) and the reference frame (${\Psi }_0$).

Remark (Discontinuity for $\kappa \ne 0$). Discontinuities in kinematics can be addressed by substituting differentiable cardinal sine and versine functions according to

$$\mathrm{sinc}(\kappa \ell )=\{\begin{array}{cc}\frac{\sin (\kappa \ell )}{\kappa \ell }\hfill & \kappa \ell \ne 0\hfill \\ \multicolumn{1}{c}{1}& \kappa \ell =0\hfill \end{array}$$ (8)

and

$$\mathrm{verc}(\kappa \ell )=\{\begin{array}{cc}\frac{1-\cos (\kappa \ell )}{\kappa \ell }\hfill & \kappa \ell \ne 0\hfill \\ \multicolumn{1}{c}{0}& \kappa \ell =0\hfill \end{array}$$ (9)

which are defined for $\kappa =0$.

In order to describe the articulating tip frame (${\Psi }_t$) of the RADS with respect to the intermediate frame (${\Psi }_i$) as demonstrated in Figure 2, we use a second transformation. This transformation describes the rigid link attached to the arc section (intermediate frame (${\Psi }_i$)) of the RADS, which is given by homogenous transformation matrix (${\mathbf{H}}_t^i\in \mathrm{SE}3$), where

$${\mathbf{H}}_t^i=\left[\begin{array}{cc}{\mathbf{I}}_3\hfill & {\mathbf{L}}_t^i\hfill \\ \multicolumn{1}{c}{0_{1\times 3}}& 1\hfill \end{array}\right]$$ (10)

where $\mathbf{I}$ and $0$ represents an identity and a matrix filled with zeros, respectively. Further, ${\mathbf{L}}_t^i\in {\mathbb{R}}^3$, where ${\mathbf{L}}_t^i=[00l_t{]}^T$ describes the rigid link section by a translation along the z-axes of the intermediate frame (${\Psi }_i$). We describe the RADS articulating tip pose (${\mathbf{H}}_t^0$) in the reference frame (${\Psi }_0$) by a series of transformations according to ${\mathbf{H}}_t^0={\mathbf{H}}_i^0{\mathbf{H}}_t^i$, which completes the forward kinematics.

2.1.2. Inverse kinematics

The inverse kinematics is used to express the pulley angles (${\psi }_x$ and ${\psi }_y$) as a function of the reference tip position (${\mathbf{r}}_t^0\in {\mathbb{R}}^3$, where ${\mathbf{r}}_t^0=[r_x{r}_y{r}_z{]}^T$)). In order to evaluate the arc parameters of the configuration-space given a measured or reference tip position (${\mathbf{r}}_t^0$), we first determine the arc plane angle ($\varphi$) by

$$\varphi =\arctan \left(\frac{r_y}{r_x}\right)$$ (11)

Subsequently, we use the forward kinematics presented in (7) and (10) to derive an expression for the reference tip position (${\mathbf{r}}_t^0$) according to

$$\left[\begin{array}{c}{\mathbf{r}}_t^0\hfill \\ \multicolumn{1}{c}{1}\end{array}\right]={\mathbf{H}}_t^0\left[\begin{array}{c}{\mathbf{o}}_t\hfill \\ \multicolumn{1}{c}{1}\end{array}\right]=\left[\begin{array}{c}{c}_{\varphi }(\frac{(1-c_{\kappa \ell })}{\kappa }+l_t{S}_k\ell )\hfill \\ \multicolumn{1}{c}{S_{\varphi }(\frac{(1-c_{\kappa \ell })}{\kappa }+l_t{S}_k\ell )}\\ \multicolumn{1}{c}{\frac{S_k\ell }{\kappa }+l_t{C}_k\ell }\\ \multicolumn{1}{c}{1}\end{array}\right]$$ (12)

where ${\mathbf{o}}_t=[000{]}^T\in {\mathbb{R}}^3$ represents the origin of the articulating tip frame (${\Psi }_t$). Note, that the arc length ($\ell$) and rigid link length ($l_t$) are known. Thus, by substituting (11) into (12), the arc curvature ($\kappa$) can be solved numerically as described in Algorithm 1.

Algorithm 1 Numerical estimation of the arc curvature ($\kappa$)
Inputs:
${\mathbf{r}}_t^0=[r_x{r}_{y}0{]}^T$   ⊳ Robotically actuated delivery sheath tip position
${\epsilon }_{\mathrm{th}}$   ⊳ Specified tolerance
Parameters:
$\ell$   ⊳ Arc length
$l_t$   ⊳ Rigid link length
Outputs:
$\hat{\kappa }$   ⊳ Estimated arc curvature
Method:
1: $\varphi =\arctan \left(\frac{r_y}{r_x}\right)$   ⊳ Arc plane angle
2: $\tan (\hat{\theta })=\frac{|r_t^0|}{\ell +l_t}$
3: $\hat{\kappa }=\frac{2}{\ell +l_t}\tan (\hat{\theta })$   ⊳ Initial estimate $\hat{\kappa }$
4: while $e>{\epsilon }_{\mathrm{th}}$ do
5: $\left[\begin{array}{c}{\hat{\mathbf{r}}}_t^0\hfill \\ \multicolumn{1}{c}{1}\end{array}\right]={\mathbf{H}}_t^0(\hat{\kappa })\left[\begin{array}{c}{\mathbf{o}}_t\hfill \\ \multicolumn{1}{c}{1}\end{array}\right]$   ⊳ Compute tip position with $\hat{\kappa }$
6: $e=|{\hat{\mathbf{r}}}_t^0|-|{\mathbf{r}}_t^0|$   ⊳ Compute error (x- and y-axes)
7: $\hat{\kappa }=\hat{\kappa }+G·e$   ⊳ Re-estimate $\hat{\kappa }$, with $G=4450$ (empirically determined)
8: end while

We first estimate an initial value for the arc curvature ($\hat{\kappa }$) using trigonometric relations as depicted in Figure 3. Subsequently, we use the initial estimated arc curvature ($\hat{\kappa }$) to compute the forward kinematics described in (7) and (10). The error (e) between the estimate (${\hat{\mathbf{r}}}_t^0$) and reference (${\mathbf{r}}_t^0$) tip position is evaluated. The computed error (e) is used to correct the estimated arc curvature ($\hat{\kappa }$), where the feedback gain (G) is included to obtain a desired rate of convergence. The process is repeated until the error (e) is below a specified tolerance (${\epsilon }_{\mathrm{th}}$). Hence, we obtain an accurate estimation of the arc curvature ($\kappa$). Given the evaluated arc curvature ($\kappa$) and arc plane angle ($\varphi$), we can solve (1) for all individual tendon lengths ($l_i$) and determine the corresponding pulley angles (${\psi }_x$ and ${\psi }_y$), which completes the inverse kinematics.

Fig. 3.

Overview of the geometric relations used to estimate an initial value for the arc curvature ($\hat{\kappa }$) of the robotically actuated delivery sheath (RADS). The arc length ($\ell$) and rigid link length ($l_t$) combined with the RADS tip position (${\mathbf{|}\mathbf{r}}_t^0|$) are used to estimate the bend angle ($\hat{\theta }$). The estimated bend angle ($\hat{\theta }$) and the approximated length ($\frac{1}{2}(\ell +l_t)$) are used to evaluate an initial estimate of the arc curvature ($\hat{\kappa }$) of the RADS.

2.1.3. Differential kinematics

Differential kinematics is used to relate the change in instrument arc parameters ($\mathbf{u}=[\varphi \kappa {]}^T$) to the change in tip position, which is integrated in the MPC strategy. Similar to the reference tip position in (12), we can describe the tip position according to $\mathbf{p}=\mathbf{F}(\mathbf{\varphi },\mathbf{\kappa },\ell ,l_t)$, where $\mathbf{p}=[p_x{p}_y{p}_z{]}^T\in {\mathbb{R}}^3$ is the tip position in x-, y- and z-axes (frame (${\Psi }_0$)), which is described by a function ($\mathbf{F}(\mathbf{\varphi },\mathbf{\kappa },\ell ,l_t)$) of the arc parameters ($\varphi$ and $\kappa$). Given the instrument design, we consider the arc length ($\ell$) and rigid link length ($l_t$) to be constant in ($\mathbf{F}(\mathbf{\varphi },\mathbf{\kappa },\ell ,l_t)$). Note, that the arc parameters ($\varphi$ and $\kappa$) are controlled by pulley angles (${\psi }_x$ and ${\psi }_y$). An expression that relates the change in instrument arc parameters ($\stackrel{·}{\mathbf{u}}=[\stackrel{·}{\varphi }\stackrel{·}{\kappa }{]}^T$) to instrument tip velocity ($\stackrel{·}{\mathbf{p}}=[{\stackrel{·}{p}}_x{\stackrel{·}{p}}_y{\stackrel{·}{p}}_z{]}^T$) is given by

$$\stackrel{·}{\mathbf{p}}={\mathbf{J}}_R(\mathbf{u})\stackrel{·}{\mathbf{u}}$$ (13)

where

$$\begin{array}{c}{\mathbf{J}}_R(\mathbf{u})=\left[\begin{array}{cc}\frac{\partial {\mathbf{F}}_1}{\partial \varphi }\hfill & \frac{\partial {\mathbf{F}}_1}{\partial \kappa }\hfill \\ \multicolumn{1}{c}{\frac{\partial {\mathbf{F}}_2}{\partial \varphi }}& \frac{\partial {\mathbf{F}}_2}{\partial \kappa }\hfill \\ \multicolumn{1}{c}{\frac{\partial {\mathbf{F}}_3}{\partial \varphi }}& \frac{\partial {\mathbf{F}}_3}{\partial \kappa }\hfill \end{array}\right]=\hfill \\ \left[\begin{array}{cc}{s}_{\varphi }(\frac{c_{\kappa \ell }-1}{\kappa }-l_t{s}_{\kappa \ell })\hfill & c_{\varphi }(\frac{c_{\kappa \ell }-1}{{\kappa }^2}+\frac{\ell s_{\kappa \ell }}{\kappa }+\ell l_t{c}_{\kappa \ell })\hfill \\ \multicolumn{1}{c}{-c_{\varphi }(\frac{c_{\kappa \ell }-1}{\kappa }-l_t{s}_{\kappa \ell })}& s_{\varphi }(\frac{c_{\kappa \ell }-1}{{\kappa }^2}+\frac{\ell s_{\kappa \ell }}{\kappa }+\ell l_t{c}_{\kappa \ell })\hfill \\ \multicolumn{1}{c}{0}& \frac{\ell c_{\kappa \ell }}{\kappa }-\frac{s_{\kappa \ell }}{{\kappa }^2}-\ell l_t{s}_{\kappa \ell }\hfill \end{array}\right]\hfill \end{array}$$ (14)

uses the forward kinematics described in (12) to analytically derive the differential kinematics. Note, that similar to (7), discontinuities for $\kappa =0$ can be addressed by using derivatives of cardinal sine (sinc$(\kappa \ell )$) and versine (verc$(\kappa \ell )$) functions. Given the presented techniques that provide forward, inverse and differential kinematics, often unknown modelling mismatches and external disturbances degrade the control performance. Hence, we include ultrasound feedback in order to improve our control strategy.

2.2. Ultrasound image segmentation

This section elaborates on the segmentation techniques applied to evaluate the RADS centroid location in 2D ultrasound images. In order to view the tip of the RADS in ultrasound images, we insert the instrument in a container and use water as an acoustic transport medium. A radial cross-sectional view of the RADS is obtained by orientating the 2D ultrasound image plane perpendicular to the shaft of the instrument (Figure 4). The RADS tip is positioned in the ultrasound image plane by axial positioning, which is described in details in Section 2.3.4. Note, that by axial positioning, the segmented RADS tip frame in ultrasound images (frame (${\Psi }_u$)) is expressed in the fixed reference frame (${\Psi }_0$) by position feedback.

Fig. 4.

Ultrasound image segmentation to evaluate the centroid location ($p_x,p_y$) of the robotically actuated delivery sheath (RADS). The RADS tip (frame (${\Psi }_t$)) is positioned in the ultrasound image plane denoted by frame (${\Psi }_u$) using axial positioning along the x-axis (frame (${\Psi }_0$)). (a) Radial cross-sectional view of the RADS in 2D ultrasound images. (b) Gaussian filtering using a 2D kernel. (c) Canny edge detection with hysteretic thresholding. (d) Random sample consensus (RANSAC) to localize the centroid ($p_x$,$p_y$) of the RADS (centre of the blue circle). The green and red points are considered inliers and outliers, respectively.

A representative ultrasound image of the instrument tip is shown in Figure 4(a). We observe a semi-circular shape that describes the reflecting surface of the RADS. By evaluation of the pre-operative ultrasound data (Figure 6), we did not observe identical semi-circular shapes that represented anatomical structures. Therefore, we use segmentation techniques based on the circular shape parameters to localize the tip of the RADS in ultrasound images. Further, the acoustic impedance difference between RADS materials and cardiac tissue is considered to be significantly large, which enhances the visibility of the instrument (Aldrich, 2007; Vrooijink et al., 2014). This would enable instrument detection when interaction with cardiac tissue is considered.

Fig. 6.

A pre-operative two-dimensional transoesophageal echocardiogram (TEE) of the aortic heart valve (AHV) annular plane. The centre location of the aortic heart valve annulus is manually segmented for multiple cardiac cycles. The human evaluated annulus centre positions are used for fitting a motion model. The blue line demonstrates the manually segmented annulus position during the cardiac cycle. The corresponding red line describes the position of the fitted model. Further, the velocities and accelerations of the AHV models are provided.

Preprocessing is performed to enhance the visibility of the RADS in ultrasound images. In order to reduce speckle and to smoothen edges in the ultrasound image, a 2D Gaussian kernel is applied, as depicted in Figure 4(b). The contrast between instrument and the environment in ultrasound images is sufficient for edge detection. Hence, we use a Canny edge detector with hysteresis thresholding to obtain an edge map of the ultrasound image as shown in Figure 4(c) (Forsyth and Ponce, 2002). Hysteretic thresholding reduces the detection of irrelevant edges, that do not describe the semi-circular surface of the RADS. However, irrelevant edges, often introduced by surface deformations caused by artifacts and bending of the instrument may still exist. Hence, the centroid location is evaluated using a random sample consensus (RANSAC) strategy (Algorithm 2). RANSAC is robust to irrelevant edges that do not fit the parametric description of the semi-circular model representing the RADS (Forsyth and Ponce, 2002).

Algorithm 2 Random sample consensus robotically actuated delivery sheath localization
Inputs:
$A_c\leftarrow \{{\mathbf{x}}_{c,v|v=1,\dots ,w}\}$   ⊳ Set of detected edge points (${\mathbf{x}}_c$)
$f:H_3\to m_c$   ⊳ Computes the algebraic circle model parameters ($m_c$) from a set ($H_3$) of three randomly selected edge points
$C(m_c,{\mathbf{x}}_c)$   ⊳ Cost function for a single edge point (1 if ${\mathbf{x}}_c$ is an inlier to the algebraic circle parameters ($m_c$), 0 otherwise)
n   ⊳ Number of iterations
Outputs:
$m_c^{*}$   ⊳ Best model parameters
$S_c^{*}$   ⊳ Best consensus set (inliers)
$J_c^{*}$   ⊳ Best cost
Method:
1: for $i\leftarrow 1,n$ do
2: $H_{3,i}\leftarrow \mathrm{random}\_3\mathrm{pnts}(A_c)$   ⊳ (I) Hypothesis
3: $m_c,i\leftarrow f(H_{3,i})$
4: if$\mathrm{suffice}(m_{c,i})$then   ⊳ (II) Preliminary test
5:  $S_c,i\leftarrow \{\forall {\mathbf{x}}_c\in A_c|C(m_{c,i},{\mathbf{x}}_c)=1\}$
6:  $J_c,i\leftarrow \sum _{{\mathbf{x}}_c\in A_c}C(m_{c,i},{\mathbf{x}}_c)$
7:  if$J_c^{*}<J_c,i$then   ⊳ (III) Evaluation
8:   $J_c^{*}\leftarrow J_{c,i}$
9:   $m_c^{*}\leftarrow m_{c,i}$
10:   $S_c^{*}\leftarrow S_{c,i}$
11:  end if
12: end if
13: $i\leftarrow i+1$
14: end for

The evaluated set ($A_c$) of edge points (${\mathbf{x}}_c\in {\mathbb{R}}^2$) from the Canny edge detector is used as an input to the RANSAC algorithm. The RANSAC algorithm is an iterative process that consists of three steps (i.e. (I) hypothesis, (II) preliminary test and (III) evaluation). In (I), a hypothetical solution is found by fitting algebraic circle model parameters (m) for $f:H_3\to m_c$ to a set ($H_3$) of candidate inliers. The set ($H_3$) consists of three arbitrary selected edge points from the evaluated set ($A_c$). In (II), a preliminary test $(\mathrm{suffice}(m_{c,i}))$ is performed to evaluate the model parameters. The model parameters such as radius and centroid location should be consistent with those of the RADS in order to qualify as a potential solution. Further, all edge points of the edge map are evaluated using a cost function $(C(m_c,{\mathbf{x}}_c))$ to determine whether they are sufficiently close to the periphery of the circular shape. The fitted model is acceptable if a sufficiently large part of the semi-circular surface has been evaluated as the consensus set. In (III), an evaluation is performed in order to improve the previous solution. The model parameters and consensus set are both refined if the computed cost ($J_{c,i}$) of the current iteration exceeds the previous solution. After n iterations, the centroid location ($p_x,p_y$) of RADS is evaluated from the model parameters ($m_c$) and displayed as the centre of the circle (Figure 4(d)).

The performance of the RANSAC algorithm is evaluated and validated experimentally using a sequence of 600 ultrasound images. Experiments show that the localization error of the RADS decreases if the number (n) of iterations of the RANSAC algorithm are increased as depicted in Figure 5(b). In order to minimize computational costs and considering the ultrasound imaging resolution of approximately 0.12 mm per pixel, the number of RANSAC iterations should be limited to approximately 800, which is depicted green in Figure 4. Further, we compare the results of RADS segmentation for varying RANSAC iterations using manual segmentation as a ground truth (Figure 5(a)). The results show, that by increasing the RANSAC iterations, the segmentation error remains approximately constant (0.4 mm). Since there is no reason to believe that an increase in RANSAC iterations would deteriorate segmentation accuracy, we estimate that the insignificant increased error in Figure 5(a), could be attributed to imperfections in manual segmentations.

Fig. 5.

Experimental evaluation and validation of image segmentation using a sequence of 600 ultrasound images. (a) Compares the performance between varying random sample consensus (RANSAC) iterations using a ground truth obtained by manual segmentation. (b) Describes the relation between the number of iterations and the localization error using a ground truth obtained after ${10}^6$ RANSAC iterations.

The presented ultrasound segmentation method is used in our closed-loop control strategy to improve the performance. Note, that the presented 2D segmentation method can be expanded to 3D ultrasound imaging by parallel evaluation of slices along the instrument shaft.

2.3. Model predictive control

In this section we present the MPC architecture used to control the RADS in closed loop. The objective of the MPC architecture is to anticipate the AHV motion based on a model. This allows the clinician to focus on the primary task of the procedure, while compensation for AHV motions is provided. In order to develop an AHV model, we use the pre-operative 2D ultrasound images that describe the AHV motion of a human during the cardiac cycle. Further, we integrate forward, inverse and differential kinematics described in Section 2.1 to determine instrument motion. The 2D ultrasound segmentation presented in Section 2.2 is used to provide instrument feedback for control.

2.3.1. Model description

The continuous time representation of the model used in the MPC strategy incorporates the differential kinematics described in (13). A cascade configuration is used according to

$${\stackrel{·}{\mathbf{p}}}_{(t)}={\mathbf{J}}_R({\mathbf{u}}_{(t)}){\stackrel{·}{\mathbf{u}}}_{(t)}$$ (15)

$${\stackrel{··}{\mathbf{u}}}_{(t)}={\mathbf{v}}_{(t)}+{\mathbf{e}}_{(t)}$$ (16)

$${\mathbf{y}}_{(t)}={\mathbf{p}}_{(t)}+{\mathbf{e}}_{(t)}$$ (17)

where ${\mathbf{v}}_{(t)}$ is considered to be the MPC control input signal. However, the change in arc parameters ($\stackrel{·}{{\mathbf{u}}_{(t)}}$) is used to steer the RADS according to the MPC strategy. The system is subject to an inequality constraint according to

$${\mathbf{p}}_{\min }\le {\mathbf{p}}_{(t)}\le {\mathbf{p}}_{\max }\mathrm{for}\mathrm{all}t$$ (18)

where the instrument tip position is restricted. The instrument tip position is limited in order to prevent damage to the instrument arc section. Further, by adding restricted regions for the instrument tip position, damage to sensitive tissue can be avoided. In order to preserve dominant features required for ultrasound image segmentation, we add an additional inequality constraint. The constraint is used to limit the instrument tip velocity in x- and y-directions of frame (${\Psi }_0$) by

$${\stackrel{·}{\mathbf{p}}}_{\min }\le {\stackrel{·}{\mathbf{p}}}_{(t)}\le {\stackrel{·}{\mathbf{p}}}_{\max }\mathrm{for}\mathrm{all}t$$ (19)

The discrete time model integrated within the MPC strategy can be obtained by using forward Euler discretization of the continuous time model according to

$${\mathbf{p}}_{(k+1)}={\mathbf{p}}_{(k)}+{\mathbf{J}}_R({\mathbf{u}}_{(k)}){\mathbf{a}}_{(k)}$$ (20)

$${\mathbf{a}}_{(k+1)}={\mathbf{a}}_{(k)}+T_s^2{\mathbf{v}}_{(k)}+T_s^2{\mathbf{e}}_{(k)}$$ (21)

$${\mathbf{y}}_{(k)}={\mathbf{p}}_{(k)}+{\mathbf{e}}_{(k)}$$ (22)

where we substitute ${\mathbf{a}}_{(k)}={\mathbf{u}}_{(k+1)}-{\mathbf{u}}_{(k)}$ and $T_s$ denotes the sampling time. Similarly, the equivalent discrete time constraints are given by

$${\mathbf{p}}_{\min }\le {\mathbf{p}}_{(k)}\le {\mathbf{p}}_{\max }\mathrm{for}\mathrm{all}k$$ (23)

and

$${\stackrel{·}{\mathbf{p}}}_{\min }\le \frac{{\mathbf{p}}_{(k+1)}-{\mathbf{p}}_{(k)}}{T_s}\le {\stackrel{·}{\mathbf{p}}}_{\max }\mathrm{for\; all}k.$$ (24)

Note, that the constraints stacked into a vector can be evaluated component-wise. The presented discrete time model is integrated within MPC strategy to optimize the control objective.

2.3.2. Control objective

The main MPC objective is to anticipate the AHV motion, while considering a desired reference input. In order to capture these requirements, we can formulate a reference tracking objective according to

$$\mathbf{r}={\mathbf{r}}_{\mathrm{AHV}}+{\mathbf{r}}_d$$ (25)

where ${\mathbf{r}}_d$ describes the RADS desired tip position without a priori knowledge, while ${\mathbf{r}}_{\mathrm{AHV}}$ represents the position of the AHV with a priori knowledge obtained from pre-operative modelling. In this study, we assume that a priori knowledge on AHV position is sufficiently well-described by modelling, which is presented in Section 2.3.3. In order to achieve our control objective in MPC, we formulate a generalized predictive control (GPC) cost function (J) (van den Boom and Stoorvogel, 2012). This cost function evaluates, over a horizon, the reference tracking error and the control action by

$$\begin{array}{c}{J}_{(\mathbf{v},k)}=\sum _{j=N_m}^N{({\hat{\mathbf{y}}}_{(k+j|k)}-{\mathbf{r}}_{(k+j)})}^T({\hat{\mathbf{y}}}_{(k+j|k)}-{\mathbf{r}}_{(k+j)})\hfill \\ \multicolumn{1}{c}{+\sum _{j=1}^N{\mathbf{v}}_{(k+j-1|k)}^T{\mathbf{\lambda }}^T\mathbf{\lambda }{\mathbf{v}}_{(k+j-1|k)}}\end{array}$$ (26)

where $N=15$ describes the prediction horizon, $N_m$ is the minimum cost horizon and equals one, (${\hat{\mathbf{y}}}_{(k+j|k)}$) is the predicted instrument tip position (${\mathbf{y}}_{(k+j)}$) based on knowledge up to time (k) and $\mathbf{\lambda }$ is the control input weighting matrix given by

$$\mathbf{\lambda }=\left[\begin{array}{cc}1\times {10}^{-2}\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 1\times {10}^{-7}\hfill \end{array}\right]$$ (27)

which evaluates the control action ($\mathbf{v}$). The control input weighting matrix ($\mathbf{\lambda }$), the prediction horizon (N) and minimum cost-horizon ($N_m$) are empirically determined. By rewriting the cost function (26) as

$$J_{(\mathbf{v},k)}=\sum _{j=0}^{N-1}{\hat{\mathbf{z}}}_{(k+j|k)}^T{\mathbf{\Gamma }}_{(j)}{\hat{\mathbf{z}}}_{(k+j|k)}$$ (28)

we obtain the standard form used in MPC, where

$${\mathbf{z}}_{(k)}=\left[\begin{array}{c}{\hat{\mathbf{y}}}_{(k+1|k)}-{\mathbf{r}}_{(k+1)}\hfill \\ \multicolumn{1}{c}{\mathbf{\lambda }{\mathbf{v}}_{(k)}}\end{array}\right]$$ (29)

describes the reference tracking error and control action, while

$${\mathbf{\Gamma }}_{(j)}=\{\begin{array}{cc}\left[\begin{array}{cc}{0}_{3\times 3}\hfill & 0_{3\times 2}\hfill \\ \multicolumn{1}{c}{0_{2\times 3}}& {\mathbf{I}}_2\hfill \end{array}\right]\hfill & \mathrm{for}0\le \mathrm{j}<{\mathrm{N}}_{\mathrm{m}}-1\hfill \\ \multicolumn{1}{c}{\left[\begin{array}{cc}{\mathbf{I}}_3\hfill & 0_{3\times 2}\hfill \\ \multicolumn{1}{c}{0_{2\times 3}}& {\mathbf{I}}_2\hfill \end{array}\right]}& \mathrm{for}{\mathrm{N}}_{\mathrm{m}}-1\le \mathrm{j}<\mathrm{N}-1\hfill \end{array}$$ (30)

is a diagonal selection matrix (${\mathbf{\Gamma }}_{(j)}\in {\mathbb{R}}^{5\times 5}$) used to describe the horizon of the cost function. By minimizing the cost function ($J_{(\mathbf{v},k)}$), we optimize between RADS tracking accuracy and control effort, where the reference (${\mathbf{r}}_{\mathrm{AHV}}$) is given by AHV modelling.

2.3.3. AHV modelling

Modelling is used to anticipate the AHV motion in MPC. As a case study, we observe the motion of the AHV annulus during the human cardiac cycle. We use pre-operative 2D transoesophageal echocardiogram (TEE) to obtain the motion profile of a human AHV annulus (Figure 6). The motion profile obtained from 2D ultrasound images is used to demonstrate compensation of the AHV motion. In this study, we assume a constant shape of the aortic annulus. Hence, we obtain the centre position of the aortic annulus by manual segmentation (Figure 6).

The manual segmented aortic annulus position during the cardiac cycle is used to derive a model capable of describing the periodic motion. A two-term Fourier series according to

$$f_x=a_{0_x}+a_{1_x}{c}_{\frac{k}{S_r}\omega }+b_{1_x}{s}_{\frac{k}{S_r}\omega }+a_{2_x}{c}_{2\frac{k}{S_r}\omega }+b_{2_x}{s}_{2\frac{k}{S_r}\omega }$$ (31)

and

$$f_y=a_{0_y}+a_{1_y}{c}_{\frac{k}{S_r}\omega }+b_{1_y}{s}_{\frac{k}{S_r}\omega }+a_{2_y}{c}_{2\frac{k}{S_r}\omega }+b_{2_y}{s}_{2\frac{k}{S_r}\omega }$$ (32)

is used to describe the periodic aortic annulus motion, where k describes discrete time, $S_r$ denotes the number of samples per second and $\omega$ represents the frequency of the periodic function in radians per second. The frequency ($\omega$) is given by $\omega =\frac{{\omega }_x+{\omega }_y}{2}$, and relates to the heart rate ($H_r$) in beats per minute (BPM) according to $H_r=\frac{60\omega }{2\pi }$. The corresponding coefficients and frequencies ($a_{0_{*}}$, $a_{1_{*}}$, $b_{1_{*}}$, $a_{2_{*}}$, $b_{2_{*}}$ and ${\omega }_{*}$) are provided in Table 1. By considering the frequencies provided in Table 1, the observed heart rate ($H_r$) is 42 BPM. The goodness of data fitting is given by the coefficients of determination $R^2=0.86$ and $R^2=0.97$ along the x- and y-axes, respectively. Note, that an increase in the number of terms of the Fourier series described in (31) and (32) does not result in a significant improvement in the coefficients of determination. From the manual segmentation we observe displacements of $9.60\pm 3.23$ mm in x-axis, while the y-axis shows displacements of $12.25\pm 1.27$ mm. Further, from the fitted model we observe maximum velocities and accelerations of approximately 40 mm/s and 250 mm/${\mathrm{s}}^2$, respectively. The periodic aortic annulus motions described by $f_x$ and $f_y$ provides the tracking reference (${\mathbf{r}}_{\mathrm{AHV}}=[f_x{f}_{y}0{]}^T$) for MPC. In order to anticipate the motion of the aortic annulus, a priori knowledge of the tracking reference (${\mathbf{r}}_{\mathrm{AHV}}$) is considered during experimental evaluation using MPC.

Table 1.

Fourier coefficients used in (31) and (32) to describe the periodic aortic heart valve motion depicted in Figure 6. Subscript * denotes the corresponding x- or y-axes.

	$a_{0_{*}}$	$a_{1_{*}}$	$b_{1_{*}}$	$a_{2_{*}}$	$b_{2_{*}}$	${\omega }_{*}$
x-axis	0.0547	−1.7850	−2.4530	−0.5737	−0.7064	4.4050
y-axis	0.3157	−3.7290	−2.7090	−2.4190	0.4965	4.4240

2.3.4. Controller design

The MPC strategy used to control the articulating tip of the RADS is presented in Figure 7. The control variable ($\mathbf{v}$) is used to determine the change in arc parameters ($\mathbf{a}$), which are implemented in order to position the tip of the RADS. Subsequently, the tip position ($\mathbf{y}=[p_x{p}_{y}0{]}^T$) of the RADS is measured using a 2D ultrasound transducer as described in Section 2.2. However, ultrasound images are often prone to noise and the tip of the RADS may not always be detected during tracking. Hence, we add an extended Kalman filter to provide state estimation ($\hat{\mathbf{y}}=[{\hat{p}}_x{\hat{p}}_y{\hat{p}}_z{]}^T$) based on kinematics described in Section 2.1 and 2D ultrasound measurements (Bar-Shalom et al., 2001). The position error (${\mathbf{e}}_{\mathrm{rr}}=\mathbf{y}-\hat{\mathbf{y}}$) is evaluated in order to adapt the Kalman filter. Note, that the articulating tip of the RADS must intersect with the ultrasound image plane in order to provide position feedback. Hence, we limit the articulating tip motion to the ultrasound image plane by autonomous axial positioning of the RADS along the z-axis of frame (${\Psi }_0$) using a linear stage. The state estimate (${\hat{p}}_z$) provided by the Kalman filter is used as an input to axial positioning.

Fig. 7.

Model-predictive control (MPC) strategy is used to steer the robotically actuated delivery sheath (RADS). The referenced tip position by the clinician is denoted by ${\mathbf{r}}_d$, while ${\mathbf{r}}_{\mathrm{AHV}}$ describes the position of the aortic heart valve with a priori knowledge obtained from pre-operative ultrasound images. The tip position obtained by ultrasound image segmentation is denoted by $\mathbf{y}$, with corresponding filtered position described by $\hat{\mathbf{y}}$. The arc parameters denoted by $\mathbf{a}$ is provided as an input to the RADS and the Kalman filter. The resulting positioning error is given by ${\mathbf{e}}_{\mathrm{rr}}$, which is used to adapt the Kalman filter.

In order to describe the MPC strategy, we formulate a constrained standard predictive control problem (CSPCP), which uses a multiple-input and a multiple-output (MIMO) state-space representation of the system (Kinnaert, 1989; van den Boom and Stoorvogel, 2012). We can write the CSPCP using a MIMO model of the RADS in state-space realization according to the following:

$${\mathbf{x}}_{(k+1)}=\mathbf{A}{\mathbf{x}}_{(k)}+{\mathbf{B}}_1{\mathbf{e}}_{(k)}+{\mathbf{B}}_2{\mathbf{w}}_{(k)}+{\mathbf{B}}_3{\mathbf{v}}_{(k)}$$ (33)

$${\mathbf{y}}_{(k)}={\mathbf{C}}_1{\mathbf{x}}_{(k)}+{\mathbf{D}}_{11}{\mathbf{e}}_{(k)}+{\mathbf{D}}_{12}{\mathbf{w}}_{(k)}+{\mathbf{D}}_{13}{\mathbf{v}}_{(k)}$$ (34)

$${\mathbf{z}}_{(k)}={\mathbf{C}}_2{\mathbf{x}}_{(k)}+{\mathbf{D}}_{21}{\mathbf{e}}_{(k)}+{\mathbf{D}}_{22}{\mathbf{w}}_{(k)}+{\mathbf{D}}_{23}{\mathbf{v}}_{(k)}$$ (35)

$${\mathbf{\chi }}_{(k)}={\mathbf{C}}_4{\mathbf{x}}_{(k)}+{\mathbf{D}}_{41}{\mathbf{e}}_{(k)}+{\mathbf{D}}_{42}{\mathbf{w}}_{(k)}+{\mathbf{D}}_{43}{\mathbf{v}}_{(k)}\le {\mathbf{X}}_{(k)}$$ (36)

where we use the model described in (20) to (22). The state is given by ${\mathbf{x}}_{(k)}={\left[\begin{array}{cc}{\mathbf{p}}_{(k)}\hfill & {\mathbf{a}}_{(k)}\hfill \end{array}\right]}^T$, ${\mathbf{e}}_{(k)}$ represents zero mean white noise, while ${\mathbf{w}}_{(k)}={\left[\begin{array}{cc}{\mathbf{d}}_{(k)}\hfill & {\mathbf{r}}_{(k+1)}\hfill \end{array}\right]}^T$ combines all known external signals such as disturbance (${\mathbf{d}}_{(k)}$) and reference (${\mathbf{r}}_{(k+1)}$). Further, the generalized input is described by ${\mathbf{v}}_{(k)}$. The state-space matrices in (33) and (34) are given by

$$\begin{array}{c}\mathbf{A}=\left[\begin{array}{cc}{\mathbf{I}}_3\hfill & {\mathbf{J}}_R(\mathbf{u})\hfill \\ \multicolumn{1}{c}{0_{2\times 3}}& {\mathbf{I}}_2\hfill \end{array}\right],{\mathbf{B}}_1=\left[\begin{array}{cc}{0}_{3\times 3}\hfill & 0_{3\times 2}\hfill \\ \multicolumn{1}{c}{0_{2\times 3}}& T_s^2{\mathbf{I}}_2\hfill \end{array}\right],\hfill \\ \multicolumn{1}{c}{{\mathbf{B}}_2=\left[\begin{array}{cc}{T}_s{\mathbf{I}}_3\hfill & 0_{3\times 3}\hfill \\ \multicolumn{1}{c}{0_{2\times 3}}& 0_{2\times 3}\hfill \end{array}\right],{\mathbf{B}}_3=\left[\begin{array}{c}{0}_{3\times 2}\hfill \\ \multicolumn{1}{c}{T_s^2{\mathbf{I}}_2}\end{array}\right],{\mathbf{C}}_1=\left[\begin{array}{c}{\mathbf{C}}_0{0}_{3\times 2}\hfill \end{array}\right],}\\ \multicolumn{1}{c}{{\mathbf{D}}_{11}=\left[\begin{array}{c}{\mathbf{I}}_2{0}_{3\times 2}\hfill \end{array}\right],{\mathbf{D}}_{12}=0_{3\times 6},{\mathbf{D}}_{13}=0_{3\times 2}}\end{array}$$ (37)

where we use the state ($\mathbf{p}$) and the inverse kinematics described in Section 2.1.2 to provide the arc parameters ($\mathbf{u}$) for the differential kinematics incorporated in matrix ($\mathbf{A}$). Further, the output of the system is limited to the 2D ultrasound image plane, which is given in x- and y directions of frame (${\Psi }_0$) by

$${\mathbf{C}}_0=\left[\begin{array}{ccc}1\hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 1\hfill & 0\hfill \\ \multicolumn{1}{c}{0}& 0\hfill & 0\hfill \end{array}\right]$$ (38)

In this study, we do not model external disturbances, hence we denote ${\mathbf{d}}_{(k)}=[000{]}^T$, for all k. The matrices ${\mathbf{C}}_2$, ${\mathbf{D}}_{21}$, ${\mathbf{D}}_{22}$ and ${\mathbf{D}}_{23}$ associated with the cost signal (${\mathbf{z}}_{(k)}$) introduced in (35) can be obtained by substituting ${\hat{\mathbf{y}}}_{(k+1|k)}={\mathbf{C}}_1{\mathbf{x}}_{(k+1)}+{\mathbf{D}}_{11}{\hat{\mathbf{e}}}_{(k+1|k)}$ and ${\mathbf{v}}_{(k)}$ into (29), which can be rewritten as

$$\begin{array}{l}{\widehat{z}}_{(k)}=\underset{C_2}{\underbrace{\left[\begin{array}{c}{C}_{1}A\\ 0_{2\times 6}\end{array}\right]}}{x}_{(k)}+\underset{D_{21}}{\underbrace{\left[\begin{array}{c}{C}_1{B}_1\\ 0_{2\times 3}\end{array}\right]}}{e}_{(k)}\\ +\underset{D_{22}}{\underbrace{\left(\left[\begin{array}{c}{C}_1{B}_2\\ 0_{2\times 6}\end{array}\right]+\left[\begin{array}{cc}{0}_{3\times 3}& -I_{3\times 3}\\ 0_{2\times 3}& 0_{2\times 3}\end{array}\right]\right)}}{w}_{(k)}+\underset{D_{23}}{\underbrace{\left[\begin{array}{c}{C}_1{B}_3\\ \lambda \end{array}\right]}}{v}_{(k)}\end{array}$$ (39)

where we use the zero mean white noise estimate (${\hat{\mathbf{e}}}_{(k+1|k)}=0_{3\times 1}$).

The constraints presented in (23) and (24) can be combined and described by two one-sided constraint signals (${\mathbf{\chi }}_{1,(k)}$ and ${\mathbf{\chi }}_{2,(k)}$) stacked together according to

$$\begin{array}{l}\underset{{\chi }_{(\mathrm{k})}}{\underbrace{\left[\begin{array}{c}{\chi }_{1,(k)}\\ {\chi }_{2,(k)}\end{array}\right]}}=\left[\begin{array}{c}{x}_{(k+1)}\\ -x_{(k+1)}\end{array}\right]=\underset{{\mathrm{C}}_{\mathrm{4}}}{\underbrace{\left[\begin{array}{c}A\\ -A\end{array}\right]}}{x}_{(k)}+\underset{D_{\mathrm{41}}}{\underbrace{\left[\begin{array}{c}{B}_1\\ -B_1\end{array}\right]}}{e}_{(k)}+\\ \underset{D_{\mathrm{42}}}{\underbrace{\left[\begin{array}{c}{B}_2\\ -B_2\end{array}\right]}}{w}_{(k)}+\underset{D_{\mathrm{43}}}{\underbrace{\left[\begin{array}{c}{B}_3\\ -B_3\end{array}\right]}}{v}_{(k)}\le \underset{X_{(\mathrm{k})}}{\underbrace{\left[\begin{array}{c}{X}_{1,max}\\ -X_{2,min}\end{array}\right]}}\end{array}$$ (40)

where ${\mathbf{X}}_{1,\max }={\left[\begin{array}{cc}{\mathbf{p}}_{\max }\hfill & T_s{\mathbf{J}}_R^{+}(\mathbf{u}){\stackrel{·}{\mathbf{p}}}_{\max }\hfill \end{array}\right]}^T$ and ${\mathbf{X}}_{2,\min }={\left[\begin{array}{cc}{\mathbf{p}}_{\min }\hfill & T_s{\mathbf{J}}_R^{+}(\mathbf{u}){\stackrel{·}{\mathbf{p}}}_{\min }\hfill \end{array}\right]}^T$. Note, that by using the differential kinematic relation in (13), the constraint on the instrument tip velocity described in (24) can be rewritten as a constraint on the change in arc parameters (${\mathbf{a}}_{(k)}$) according to

$$T_s{\mathbf{J}}_R^{+}(\mathbf{u}){\stackrel{·}{\mathbf{p}}}_{\min }\le {\mathbf{a}}_{(k)}\le T_s{\mathbf{J}}_R^{+}(\mathbf{u}){\stackrel{·}{\mathbf{p}}}_{\max }\mathrm{for\; all}k$$ (41)

where ${\mathbf{J}}_R^{+}(\mathbf{u})$ is the Moore–Penrose pseudo-inverse of ${\mathbf{J}}_R(\mathbf{u})$. By adding the stacked inequality constraint, we complete the CSPCP presented in (33) to (36).

The CSPCP is used to formulate a model based on the concept of prediction, which is described in Appendix C. The prediction model is used to solve for the optimal control vector (${\tilde{\mathbf{v}}}_{(k)}$). By using predictions of the cost signal provided in (39) and the diagonal selection matrix described in (30), given the prediction interval $0\le j\le N-1$, we can formulate a signal vector (${\tilde{\mathbf{z}}}_k$) similar to (52) and provide a block diagonal selection matrix ($\overline{\mathbf{\Gamma }}$) by

$${\tilde{\mathbf{z}}}_{(k)}=\left[\begin{array}{c}{\hat{\mathbf{z}}}_{(k|k)}\hfill \\ \multicolumn{1}{c}{{\hat{\mathbf{z}}}_{(k+1|k)}}\\ \multicolumn{1}{c}{⋮}\\ \multicolumn{1}{c}{{\hat{\mathbf{z}}}_{(k+N-1|k)}}\end{array}\right]\mathrm{and}\overline{\mathbf{\Gamma }}=\left[\begin{array}{cccc}{\mathbf{\Gamma }}_{(1)}\hfill & 0_{5\times 5}\hfill & \dots \hfill & 0_{5\times 5}\hfill \\ \multicolumn{1}{c}{0_{5\times 5}}& {\mathbf{\Gamma }}_{(2)}\hfill & \ddots \hfill & ⋮\hfill \\ \multicolumn{1}{c}{⋮}& \ddots \hfill & \ddots \hfill & 0_{5\times 5}\hfill \\ \multicolumn{1}{c}{0_{5\times 5}}& \dots \hfill & 0_{5\times 5}\hfill & {\mathbf{\Gamma }}_{(N-1)}\hfill \end{array}\right]$$ (42)

respectively, which can be used to rewrite the cost-function presented in (28) according to

$$J_{(\mathbf{v},k)}={\tilde{\mathbf{z}}}_{(k)}^T\overline{\mathbf{\Gamma }}{\tilde{\mathbf{z}}}_{(k)}$$ (43)

The predicted cost signal (${\tilde{\mathbf{z}}}_k$) in (42) can be formulated as

$${\tilde{\mathbf{z}}}_{(k)}={\tilde{\mathbf{C}}}_2{\mathbf{x}}_{(k)}+{\tilde{\mathbf{D}}}_{21}{\mathbf{e}}_{(k)}+{\tilde{\mathbf{D}}}_{22}{\tilde{\mathbf{w}}}_{(k)}+{\tilde{\mathbf{D}}}_{23}{\tilde{\mathbf{v}}}_{(k)}$$ (44)

where matrices ${\tilde{\mathbf{C}}}_2$, ${\tilde{\mathbf{D}}}_{21}$, ${\tilde{\mathbf{D}}}_{22}$ and ${\tilde{\mathbf{D}}}_{23}$ can be obtained according (54) and (55). In addition to the constraint described in CSPCP according to (40), we add additional constraints to the prediction model to shape the control signal. In order to obtain a smooth and robust control action, we add a control horizon constraint to the prediction model. The equality constraint (${\mathbf{\Upsilon }}_{(k)}=0_{2\times 1}$) on the control horizon can be described by

$${\mathbf{v}}_{(k+j|k)}={\mathbf{\Upsilon }}_{(k)},\mathrm{for}{N}_c\le j<N$$ (45)

where the control action is assumed to be zero after control horizon ($N_c=10$), which is empirically determined. The corresponding prediction signal (${\tilde{\mathbf{\upsilon }}}_{(k)}$) according to (51) and (52) is given by

$${\tilde{\mathbf{\upsilon }}}_{(k)}={\tilde{\mathbf{C}}}_3{\mathbf{x}}_{(k)}+{\tilde{\mathbf{D}}}_{31}{\mathbf{e}}_{(k)}+{\tilde{\mathbf{D}}}_{32}{\tilde{\mathbf{w}}}_{(k)}+{\tilde{\mathbf{D}}}_{33}{\tilde{\mathbf{v}}}_{(k)}={\tilde{\mathbf{\Upsilon }}}_{(k)}$$ (46)

where the equality constraint prediction vector is given by ${\tilde{\mathbf{\Upsilon }}}_{(k)}=0_{10\times 1}$ and the corresponding matrices are described by ${\tilde{\mathbf{C}}}_3=0_{10\times 6}$, ${\tilde{\mathbf{D}}}_{31}=0_{10\times 3}$, ${\tilde{\mathbf{D}}}_{32}=0_{10\times 90}$ and

$${\tilde{\mathbf{D}}}_{33}=\left[\begin{array}{ccccccc}{0}_{2\times 2}\hfill & \cdots \hfill & 0_{2\times 2}\hfill & {\mathbf{I}}_2\hfill & 0_{2\times 2}\hfill & \cdots \hfill & 0_{2\times 2}\hfill \\ \multicolumn{1}{c}{0_{2\times 2}}& \cdots \hfill & 0_{2\times 2}\hfill & 0_{2\times 2}\hfill & {\mathbf{I}}_2\hfill & \ddots \hfill & ⋮\hfill \\ \multicolumn{1}{c}{⋮}& \hfill & ⋮\hfill & ⋮\hfill & \ddots \hfill & \ddots \hfill & 0_{2\times 2}\hfill \\ \multicolumn{1}{c}{0_{2\times 2}}& \cdots \hfill & 0_{2\times 2}\hfill & 0_{2\times 2}\hfill & \cdots \hfill & 0_{2\times 2}\hfill & {\mathbf{I}}_2\hfill \end{array}\right]$$ (47)

Further, a prediction vector (${\tilde{\mathbf{\chi }}}_{(k)}$) of the constraint signal (${\mathbf{\chi }}_{(k)}$) described in (40) can be constructed similar to (52), with corresponding prediction matrices computed according to (54) and (55). The prediction of the inequality constraint is given by

$${\tilde{\mathbf{\chi }}}_{(k)}={\tilde{\mathbf{C}}}_4{\mathbf{x}}_{(k)}+{\tilde{\mathbf{D}}}_{41}{\mathbf{e}}_{(k)}+{\tilde{\mathbf{D}}}_{42}{\tilde{\mathbf{w}}}_{(k)}+{\tilde{\mathbf{D}}}_{43}{\tilde{\mathbf{v}}}_{(k)}\le {\tilde{\mathbf{X}}}_{(k)}$$ (48)

which also completes the prediction model.

By minimizing the following cost function

$$\underset{\tilde{\mathbf{v}}}{min}{\tilde{\mathbf{z}}}_{(k)}^T\overline{\mathbf{\Gamma }}{\tilde{\mathbf{z}}}_{(k)}$$ (49)

subject to constraints described in (46) and (48), we can solve for the optimal control vector (${\tilde{\mathbf{v}}}_{(k)}$) that optimizes the CSPCP. The optimization described in (49) can be evaluated as a quadratic programming problem subject to constraints. For details on the derivations, we refer the reader to work of van den Boom and Stoorvogel (2012). We use qpOases software in C++ to solve the quadratic programming problem online (Ferreau et al., 2014). The solution is used to obtain the optimal control vector (${\tilde{\mathbf{v}}}_{(k)}$). We implement according to the receding horizon principle, where we apply (${\mathbf{a}}_{(k)}$) to steer the RADS according to the optimization.

2.3.5. Hysteresis compensation

Before the optimal solution is implemented, we compensate for hysteresis in the system. Hysteresis caused by backlash often occurs in cable-driven instruments such as endoscopes and catheters (Reilink et al., 2013). Hysteresis can be described by a positive or negative contact mode and a free mode. In contact mode, kinematics describes the relation between pulley angles (${\psi }_x$ and ${\psi }_y$) and RADS tip position ($\mathbf{p}$), which is not known in free mode. Further, the positive and negative contact modes corresponds to the direction in which the pulleys are engaged. Hysteresis often introduces inaccuracies in the response of the instrument. Hence, we use a classical approach to reduce hysteresis in the system according to

$$\mathbf{\Delta }\mathbf{c}={\left[\begin{array}{cc}\Delta {\psi }_x\hfill & \Delta {\psi }_y\hfill \end{array}\right]}^T+{\left[\begin{array}{cc}{f}_{(\Delta {\psi }_x)}\hfill & f_{(\Delta {\psi }_y)}\hfill \end{array}\right]}^T$$ (50)

where $\mathbf{\Delta }\mathbf{c}$ is the compensated angular velocity of the pulleys ($\Delta {\psi }_x$ and $\Delta {\psi }_y$) and

$$f_{(\Delta {\psi }_{*(k)})}=\{\begin{array}{cc}\mathrm{sign}(\Delta {\psi }_{*(k)})({\alpha }_{*}^{+}-{\alpha }_{*}^{-})\hfill & \mathrm{If\; sign}(\Delta {\psi }_{*(k)})\ne \mathrm{sign}(\Delta {\psi }_{*(k-1)})\hfill \\ \multicolumn{1}{c}{0}& \mathrm{Otherwise}\hfill \end{array}$$

is the function that represents the compensation term. In order to limit chatter, we only apply the compensation term if the velocity changes direction and exceeds a threshold. Compensation is provided by hysteretic parameters (${\alpha }_{*}^{+}$ and ${\alpha }_{*}^{-}$), which are the positive and negative contact positions, respectively. The positive and negative contact positions are experimentally evaluated by measuring the hysteresis between pulley angles and corresponding tip locations.

3. Experiments

In this section, we present experiments performed to evaluate the MPC strategy. First, we describe the components and layout of the experimental setup used to steer the RADS. Subsequently, a pre-operative AHV model is presented and integrated with the MPC strategy. This is followed by the experimental plan and results that demonstrate the capabilities of the MPC strategy.

3.1. Experimental setup and materials

The experimental setup used to evaluate the performance of the MPC is shown in Figure 8. The RADS used in our experiments is based on a cable-ring structure surrounded by a hinged tube (DEAM Corporation, Amsterdam, The Netherlands) (Breedveld, 2010). Two pairs of antagonistically configured tension wires facilitate articulating tip movement of the RADS in two DOFs (Vrooijink et al., 2014). The tension wires of the RADS are driven by an ECMax22 motor with a GP32/22 gearhead (Maxon Motor, Sachseln, Switzerland). Further, an LX30 translational stage (Misumi Group Inc., Tokyo, Japan) is used to translate the RADS along the longitudinal axis in order to compensate for the tip motion in the out-of-image-plane direction. The maximum velocity and acceleration along the longitudinal axis are 230 mm/s and 2800 mm/s², respectively. All motors are actuated by an Elmo Whistle 2.5/60 motor controller (Elmo Motion Control Ltd, Petach-Tikva, Israel). The RADS is inserted in a container filled with water in order to enable ultrasound-based tip tracking. A Siemens 18L6 transducer operating with a frequency of 16 MHz, a power level of −4 dB and a scanning depth of 4 cm on a Siemens Acuson S2000 ultrasound system (Siemens AG, Erlangen, Germany) is positioned at the tip of the RADS (in the container) to obtain ultrasound images as feedback. Note, that without any modifications to ultrasound image segmentation, the transthoracic echocardiography approach used in this study could be replaced by TEE. The ultrasound images are transferred via S-video output (in-plane resolution of approximately 0.12 mm per pixel) to a computer (2.80 GHz Intel i7, 8 GB internal memory and 64-bit Windows 7) with a frame rate of 25 frames per second. Further, this computer is used to implement and execute in C++ the MPC strategy which provides control signals to the motors and electronics. The MPC strategy uses a sampling time ($T_s$) of 0.04 s. In order to anticipate the beating heart motion, we integrate an AHV model within the MPC strategy.

Fig. 8.

The experimental setup used to control the robotically actuated delivery sheath (RADS) using a model-predictive control strategy. ① RADS. ② Container filled with water in which the RADS is inserted. ③ Ultrasound transducer. ④ Ultrasound image with a radial cross-sectional view of the RADS. ⑤ Motors and corresponding electronics used to control the articulating tip of the RADS. ⑥ Translation along the longitudinal axis of the RADS in order to position the tip in the two-dimensional ultrasound image plane. The top inset shows the flexible segment (articulating tip) of the RADS, which uses a hinged tube construction. The bottom inset depicts a longitudinal cross-section with dimensions of the RADS. An antagonistic configuration of a pair of tension wires (red) is actuated by a pulley-driven system. Each pair of tension wires (total of two pairs) is guided through the flexible shaft and through two incompressible brass tubes (yellow) to actuate a single degree of freedom of the articulating tip.

3.2. Experimental scenarios

A series of experiments have been conducted in order to evaluate the tracking accuracy of the MPC strategy in an integrated system using a reference signal described in (25). In previous research, a model-based approach was used to steer the RADS along a circular path using 2D ultrasound images as feedback (Vrooijink et al., 2014). The results of these experiments showed, while moving at 2 mm/s, mean positioning errors of approximately 2 mm along the x- and y-axes. In this study, we aim to improve these results and provide novel functionalities such as compensation for beating heart motions. Hence, we evaluate the performance using multiple scenarios such as tracking circular paths and AHV motions.

3.2.1. Circular path

The first set of experiments is performed in order to evaluate steering of the RADS along circular paths using the MPC strategy. Note, that no heart valve motion modelling is considered (${\mathbf{r}}_{\mathrm{AHV}}=0$) while the RADS moves along the circular paths described by the reference signal (${\mathbf{r}}_d$). The circle has a radius of 6 mm, while we use an articulating tip velocity of 2 mm/s. First, we evaluate a circular path without limitations on the RADS tip position. Subsequently, we limit the tip position to a maximum of 4 mm along the x-axis by integrating a state constraint to the MPC strategy (similar to (40)). By restricting the instrument motion to an allowable region, we demonstrate the ability of the MPC strategy to avoid sensitive tissue that could be present in surgery. These limitations can be integrated in the MPC strategy, allowing the controller to anticipate for the forbidden regions in order to avoid damage to sensitive tissue. In Figure 9(b), the allowed area of the RADS tip is depicted green, while the forbidden region is red.

Fig. 9.

Representative experimental model-predictive control results of the articulating tip of the robotically actuated delivery sheath during tracking of (a) circular reference path (${\mathbf{r}}_d$) and (${\mathbf{r}}_{\mathrm{AHV}}=$ 0), (b) constrained circular reference path (${\mathbf{r}}_d$) and (${\mathbf{r}}_{\mathrm{AHV}}=$0) and (c) and (d) aortic heart valve (AHV) motion trajectories according to ${\mathbf{r}}_{\mathrm{AHV}}=[f_x{f}_{y}0{]}^T$ and ${\mathbf{r}}_d=$0. The red line trajectory represents the reference path ( r) described in (25), while the blue line represents the actual path ( y$=[p_x{p}_{y}0{]}^T$) followed by the articulating tip.

3.2.2. Aortic heart valve motion tracking

The second set of experiments is used to evaluate the novel designed MPC strategy to steer the RADS with a priori knowledge on the AHV motion. Although, other medical applications such as mitral valve surgery could potentially benefit from the integrated system, we demonstrate the capabilities of the MPC strategy by tracking AHV motions in the annular plane. The beating heart motions have been obtained by analyzing the pre-operative 2D ultrasound images as depicted in Figure 6. We evaluate the performance of the MPC with a priori knowledge of the AHV motion described by tracking reference (${\mathbf{r}}_{\mathrm{AHV}}=[f_x{f}_{y}0{]}^T$) obtained by (31) and (32) and ${\mathbf{r}}_d=0$. In order to preserve the dominant features required for ultrasound image segmentation, we limit the tip velocity to 20 mm/s in x- and y-axes of frame (${\Psi }_0$) by integrating state constraints according to (24). A RADS tip velocity that exceeds 20 mm/s, will cause dominant features to disappear in ultrasound images. This results in unreliable segmentation of the RADS tip position. Further, we limit the maximum tip deflection to 10 mm in x- and y-directions (frame (${\Psi }_0$)) by (23).

3.3. Experimental results

The results of the experimental scenarios are reported in Table 2, while a single representative of each experiment can be found in Figure 9. In order to evaluate the tip tracking accuracy of the RADS, the experiments were repeated 12 times for circular trajectories and 30 times for AHV motions. The mean absolute errors (MAE) in the tracked tip position (${\epsilon }_x$ and ${\epsilon }_y$) during trajectory tracking are reported. On average, the RANSAC algorithm of the segmentation strategy completes $819\pm 101$ iterations (single CPU core implementation). In experiments, we observe a constant delay of approximately 200 ms in obtaining the 2D ultrasound images used for feedback. Further, a compensation algorithm is used to reduce the mechanical hysteresis in the system in order to improve tip positioning accuracy.

Table 2.

Experimental results of the robotically actuated delivery sheath tip tracking for circular paths and aortic heart valve motions using the model-predictive control. The mean absolute distance error ($\epsilon$) and position errors (${\epsilon }_x$ and ${\epsilon }_y$) along the x- and y-axes are provided. Further, the standard deviation for $N_r$ repetitions is reported.

Case	$N_r$	${\epsilon }_x$ [mm]	${\epsilon }_y$ [mm]	$\epsilon$ [mm]
		Circle	12	0.73±0.03	0.50±0.10	0.89±0.08
Constrained circle	12	0.74±0.11	0.61±0.17	0.97±0.17
Aortic heart valve	30	1.06±0.43	1.29±0.38	1.68±0.53

Experiments showed tracking of a circular trajectory with MAE of 0.73 and 0.50 mm in the x- and y-axes, respectively. The observed mean absolute distance error is 0.89 mm. A slightly higher error is observed in the x-axis compared to the y-axis. This could be attributed to the difference in mechanical properties such as friction and backlash of the two uncoupled pulley systems (each control a single DOF). Nonetheless, the results show a significant improvement in tracking performance compared to the mean positioning errors of approximately 2 mm along the x- and y-axes demonstrated in previous work (Vrooijink et al., 2014). Further, in the experiments that demonstrate the circular trajectory in which the instrument motion is limited, we observe a MAE of 0.74 and 0.61 mm in the x- and y-axes, respectively. The corresponding mean absolute distance error is 0.97 mm. The observed error correspond to the results of a circular trajectory without instrument limitations. Hence, we demonstrate that the MPC strategy is capable of avoiding areas that could potentially be sensitive tissue without degrading performance. By tracking AHV motions, we observe a MAE of 1.06 and 1.29 mm in the x- and y-axes, respectively. The observed mean absolute distance error is 1.68 mm. A higher error is observed in tracking AHV motions compared with circular trajectories. This could be explained by the fast moving AHV motions that impedes the accuracy of tracking. Further, we observe a decrease in tracking accuracy along the y-axis for AHV motions. The decrease in tracking accuracy could be explained by the instrument velocity which is limited to 20 mm/s in MPC in order to enable RADS detection in ultrasound images. The MPC strategy considers both the tip velocity limitation and AHV motion in order to optimize tracking accuracy.

Further, the ultrasound measurement delay introduced in the feedback signal of the MPC strategy is among the main contributor to the tracking error. This measurement delay is considered to be the combined result of pre-processing within the ultrasound system, transferring images to the computer using a capturing device and instrument segmentation. However, it cannot be ruled out that tracking accuracy is affected by (non-)linear friction, tendon elongation or crosstalk between the two tendon pairs (i.e. controlling one tendon pair influences the other tendon pair). Note, that we evaluate our system using pre-operative patient data which leads to simplifications with respect to heart-rate variability. However, these simplifications could potentially be eliminated by administering medicine that reduce the heart rate and apply over-pacing to effectively control the patient’s heart rate in a predictable manner. This improves the ability to anticipate the AHV motion. Nonetheless, our method demonstrates tracking of AHV motions based on pre-operative ultrasound patient data. This indicates that our system, with further development, could provide cardiac motion compensation to a wide class of cardiovascular applications performed without a heart–lung machine.

4. Conclusions and future work

In this study, we present and evaluate a novel RADS capable of autonomously and accurately compensating for AHV motions by using an MPC strategy. We develop and incorporate kinematic models of the RADS within the MPC strategy. In order to accurately evaluate the RADS tip position, we use a clinically available 2D ultrasound transducer which is orientated perpendicular to the tip of the RADS. An online segmentation algorithm is developed in order to provide feedback of the RADS tip position for the MPC strategy. Pre-operative ultrasound images of a patient are used to evaluate the AHV motion. Further, mechanical hysteresis is addressed by a compensation algorithm in order to improve the tip positioning accuracy. The novel integrated system is capable of controlling the articulating tip of the RADS in order to compensate for AHV motions. In experiments, we demonstrate evidence that the RADS tracks the AHV motions with a mean absolute distance error for AHV motions of 1.68 mm. Hence, we potentially improve and enable new applications in cardiovascular surgery performed without a heart–lung machine.

In future work, we intend to address accuracy problems introduced by measurement delay and instrument friction. Electro-magnetic instrument tip tracking will be integrated in order to reduce feedback delay and to improve robustness in control. The prototype device used in this study will be replaced by a flexible steerable catheter in order to enable applications in ablation, aortic and mitral valve surgery. Further, we plan to combine instrument and intra-operative AHV motion segmentation of 2D and 3D ultrasound images in order to improve the tracking performance in a clinically relevant scenario. We continuously aim to improve the robustness and accuracy of the integrated system. Nonetheless, the presented framework for modelling, imaging and control is applicable to a range of continuum-style robots and catheters. Our current study evaluates compensation of AHV motions using an MPC strategy. Hence, we have demonstrated the feasibility and potential for steerable catheters to compensate for cardiac motions in cardiovascular interventions such as aortic and mitral valve surgery.

Appendix A: Index to Multimedia Extension

Archives of IJRR multimedia extensions published prior to 2014 can be found at http://www.ijrr.org, after 2014 all videos are available on the IJRR YouTube channel at http://www.youtube.com/user/ijrrmultimedia

Extension 1.

Download video file ^{(12.7MB, mp4)}

DOI: 10.1177/0278364917691113.M1

This video demonstrates some representative results of tracking circular paths and aortic heart valve motions using model-predictive control.

Table of Multimedia Extension

Extension	Type	Description
1	Video	This video demonstrates some representative results of tracking circular paths and aortic heart valve motions using model-predictive control.

Appendix B: Nomenclature

k	Discrete time variable	[-]
${\psi }_{*}$	Pulley angle, where $*\in \{x,y\}$	[rad]
${\delta }_{*}$	Tendon displacement, where $*\in \{x,y\}$	[m]
$\kappa$	Arc curvature	[$\frac{1}{m}$]
r	Arc radius	[m]
$\varphi$	Arc plane angle	[rad]
$\ell$	Arc length	[m]
$\theta$	Arc bend angle	[rad]
$l_t$	Rigid link length	[m]
$r_p$	Pulley radius	[m]
$t_i$	Tension wires, where $i=1,\dots ,4$	[–]
$l_i$	Tendon lengths, where $i=1,\dots ,4$	[m]
${\varphi }_i$	Individual tendon angle, where $i=1,\dots ,4$	[m]
$d_b$	Distance between backbone and tendons	[m]
${\Psi }_{*}$	Coordinate system, where $*\in \{0,i,t,u\}$	[–]
${\mathbf{H}}_i^0$	Homogeneous transformation matrix expressing the intermediate frame in the reference frame (${\mathbf{H}}_i^0\in \mathrm{SE}3$)	[–]
${\mathbf{H}}_t^i$	Homogeneous transformation matrix expressing the tip frame in the intermediate frame (${\mathbf{H}}_t^i\in \mathrm{SE}3$)	[–]
${\mathbf{H}}_t^0$	Homogeneous transformation matrix expressing the tip frame in the reference frame (${\mathbf{H}}_t^0\in \mathrm{SE}3$)	[–]
${\mathbf{L}}_t^i$	Rigid link section by a translation along the z-axis of the intermediate frame (${\mathbf{L}}_t^i\in {\mathbb{R}}^3$)	[–]
$\mathbf{r}$, ${\mathbf{r}}_t^0$	Referenced tip position expressed in the reference frame ($\mathbf{r}$, ${\mathbf{r}}_t^0\in {\mathbb{R}}^3$)	[–]
$r_{*}$	Referenced tip position, where $*\in \{x,y,z\}$	[m]
${\mathbf{o}}_t$	Origin of the articulating tip frame (${\mathbf{o}}_t\in {\mathbb{R}}^3$)	[–]
$\mathbf{p}$, ${\mathbf{p}}_t^0$	Tip position expressed in the reference frame ($\mathbf{p}$, ${\mathbf{p}}_t^0\in {\mathbb{R}}^3$)	[–]
$p_{*}$	Tip position, where $*\in \{x,y,z\}$	[m]
$\mathbf{u}$	Arc parameters, $\mathbf{u}\in {\mathbb{R}}^2$	[–]
G	Rate of convergence feedback gain	[–]
e	Estimation error	[m]
${\epsilon }_{\mathrm{th}}$	Error tolerance	[m]
$A_c$	Set of detected edge points	[–]
${\mathbf{x}}_c$	Edge point obtained from Canny edge detector (${\mathbf{x}}_c\in {\mathbb{R}}^2$)	[–]
$H_3$	Set of three randomly selected edge points	[–]
$m_c$	Algebraic circle model parameters	[–]
n	Number of random sampling consensus iterations	[–]
$J_c$	Random sampling consensus cost	[–]
$S_c$	Random sampling consensus set (inliers)	[–]
$R^2$	Data fitting discrepancy	[–]
$\mathbf{e}$	Zero mean white noise ($\mathbf{e},\hat{\mathbf{e}}\in {\mathbb{R}}^2$)	[–]
$\mathbf{v}$	Control input signal ($\mathbf{v}\in {\mathbb{R}}^2$)	[–]
y	MPC tip position ($\mathbf{y}\in {\mathbb{R}}^3$)	[–]
${\mathbf{p}}_{\min },{\mathbf{p}}_{\max }$	Inequality constraint lower and upper position bound (${\mathbf{p}}_{\min },{\mathbf{p}}_{\max }\in {\mathbb{R}}^3$)	[m]
${\stackrel{·}{\mathbf{p}}}_{\min },{\stackrel{·}{\mathbf{p}}}_{\max }$	Inequality constraint lower and upper velocity bound (${\stackrel{·}{\mathbf{p}}}_{\min },{\stackrel{·}{\mathbf{p}}}_{\max }\in {\mathbb{R}}^3$)	[$m/s$]
$T_s$	Sampling time	[s]
$\mathbf{a}$	Variations in arc parameters ($\mathbf{a}\in {\mathbb{R}}^2$)	[–]
${\mathbf{r}}_{\mathrm{AHV}}$	Aortic heart valve annulus motion reference (${\mathbf{r}}_{\mathrm{AHV}}\in {\mathbb{R}}^3$)	[–]
${\mathbf{r}}_d$	Desired tip position (${\mathbf{r}}_d\in {\mathbb{R}}^3$)	[–]
J	Generalized predictive control cost	[–]
N	Prediction horizon	[–]
$N_m$	Minimum cost horizon	[–]
$N_c$	Control horizon	[–]
$\mathbf{\lambda }$	Control input weighting matrix ($\mathbf{\lambda }\in {\mathbb{R}}^{2\times 2}$)	[–]
$\mathbf{z}$	Cost signal ($\mathbf{z}\in {\mathbb{R}}^3$)	[–]
$\mathbf{\Gamma }$	Diagonal selection matrix ($\mathbf{\Gamma }\in {\mathbb{R}}^{5\times 5}$)	[–]
$f_{*}$	AHV reference position, where $*\in \{x,y\}$	[m]
$a_{*}$, $b_{*}$	The Fourier series coefficients	[–]
$\omega$, ${\omega }_{*}$	The Fourier series frequencies, where $*\in \{x,y\}$	[m]
$S_r$	Sample rate Fourier series	$[\frac{1}{s}]$
$H_r$	Heart rate	[bpm]
${\mathbf{e}}_{\mathrm{rr}}$	The tip position error (${\mathbf{e}}_{\mathrm{rr}}\in {\mathbb{R}}^3$)	[–]
$\mathbf{x}$	The MPC state variable ($\mathbf{x}\in {\mathbb{R}}^5$)	[–]
$\mathbf{w}$	The MPC external signal ($\mathbf{w}\in {\mathbb{R}}^6$)	[–]
$\mathbf{d}$	The MPC disturbance ($\mathbf{w}\in {\mathbb{R}}^3$)	[–]
$\mathbf{A},\mathbf{B},\mathbf{C},\mathbf{D}$	Multiple-input and multiple-output state space realization of the CSPCP	[–]
$\mathbf{\upsilon }$	Equality constraint signal ($\mathbf{\upsilon }\in {\mathbb{R}}^2$)	[–]
$\mathbf{\Upsilon }$	Equality constraint ($\mathbf{\Upsilon }\in {\mathbb{R}}^2$)	[–]
$\mathbf{\chi },{\mathbf{\chi }}_1,{\mathbf{\chi }}_2$	Inequality constraint signals (${\mathbf{\chi }}_1,{\mathbf{\chi }}_2\in {\mathbb{R}}^5,\mathbf{\chi }\in {\mathbb{R}}^{10}$)	[–]
$\mathbf{X},{\mathbf{X}}_1,{\mathbf{X}}_2$	Inequality constraints (${\mathbf{X}}_1,{\mathbf{X}}_2\in {\mathbb{R}}^5,\mathbf{X}\in {\mathbb{R}}^{10}$)	[–]
$\mathbf{\Delta }c$	Compensated angular pulley velocity ($\mathbf{\Delta }c\in {\mathbb{R}}^2$)	[–]
$\Delta {\psi }_{*}$	Angular pulley velocity, where $*\in \{x,y\}$	[rad/s]
${\alpha }_{*}^{+}$	Positive contact positions, where $*\in \{x,y\}$	[m]
${\alpha }_{*}^{-}$	Negative contact positions, where $*\in \{x,y\}$	[m]
${\epsilon }_{*}$	Mean absolute error in the tracked tip position, where $*\in \{x,y\}$	[m]
$\epsilon$	Mean absolute tip distance error	[m]
$N_r$	Number of experimental repetitions	[–]

Appendix C: Concept of prediction

Based on the model description (34), the concept of prediction can be introduced according to

$${\tilde{\mathbf{s}}}_{p,(k)}={\tilde{\mathbf{C}}}_p{\mathbf{x}}_{(k)}+{\tilde{\mathbf{D}}}_{p1}{\mathbf{e}}_{(k)}+{\tilde{\mathbf{D}}}_{p2}{\tilde{\mathbf{w}}}_{(k)}+{\tilde{\mathbf{D}}}_{p3}{\tilde{\mathbf{v}}}_{(k)}$$ (51)

where any signal ${\mathbf{s}}_{p,(k)}$ as presented in (34) can be used to formulate predictions at each time instant (k), that considers all signals over horizon N. The prediction of signal ${\mathbf{s}}_{p,(k)}$ is given by signal vector ${\tilde{\mathbf{s}}}_{p,(k)}$ according to

$${\tilde{\mathbf{s}}}_{p,(k)}=\left[\begin{array}{c}{\hat{\mathbf{s}}}_{p,(k|k)}\hfill \\ \multicolumn{1}{c}{{\hat{\mathbf{s}}}_{p,(k+1|k)}}\\ \multicolumn{1}{c}{⋮}\\ \multicolumn{1}{c}{{\hat{\mathbf{s}}}_{p,(k+N-1|k)}}\end{array}\right]$$ (52)

The prediction presented in (51) is based on future control and reference signals described by signal vectors

$${\tilde{\mathbf{v}}}_{(k)}=\left[\begin{array}{c}{\hat{\mathbf{v}}}_{(k|k)}\hfill \\ \multicolumn{1}{c}{{\hat{\mathbf{v}}}_{(k+1|k)}}\\ \multicolumn{1}{c}{⋮}\\ \multicolumn{1}{c}{{\hat{\mathbf{v}}}_{(k+N-1|k)}}\end{array}\right]\mathrm{and}{\tilde{\mathbf{w}}}_{(k)}=\left[\begin{array}{c}{\hat{\mathbf{w}}}_{(k|k)}\hfill \\ \multicolumn{1}{c}{{\hat{\mathbf{w}}}_{(k+1|k)}}\\ \multicolumn{1}{c}{⋮}\\ \multicolumn{1}{c}{{\hat{\mathbf{w}}}_{(k+N-1|k)}}\end{array}\right]$$ (53)

respectively. Note that ${\tilde{\mathbf{w}}}_{(k)}$ contains a priori knowledge of the AHV position. We can obtain matrices ${\tilde{\mathbf{C}}}_p$, $\widetilde{{\mathbf{D}}_{p1}}$, $\widetilde{{\mathbf{D}}_{p2}}$ and $\widetilde{{\mathbf{D}}_{p3}}$ by using successive substitution of the state equation (33) into (34) which yields

$${\tilde{\mathbf{C}}}_p=\left[\begin{array}{c}{\mathbf{C}}_p\hfill \\ \multicolumn{1}{c}{{\mathbf{C}}_p\mathbf{A}}\\ \multicolumn{1}{c}{{\mathbf{C}}_p{\mathbf{A}}^2}\\ \multicolumn{1}{c}{⋮}\\ \multicolumn{1}{c}{{\mathbf{C}}_p{\mathbf{A}}^{N-1}}\end{array}\right],{\tilde{\mathbf{D}}}_{p1}=\left[\begin{array}{c}{\mathbf{D}}_{p1}\hfill \\ \multicolumn{1}{c}{{\mathbf{C}}_p{\mathbf{B}}_1}\\ \multicolumn{1}{c}{{\mathbf{C}}_p\mathbf{A}{B}_1}\\ \multicolumn{1}{c}{⋮}\\ \multicolumn{1}{c}{{\mathbf{C}}_p{\mathbf{A}}^{N-2}{\mathbf{B}}_1}\end{array}\right]$$ (54)

and

$${\tilde{\mathbf{D}}}_{\mathrm{pq}}=\left[\begin{array}{ccccc}{\mathbf{D}}_{\mathrm{pq}}\hfill & 0\hfill & \cdots \hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{{\mathbf{C}}_p{\mathbf{B}}_q}& {\mathbf{D}}_{\mathrm{pq}}\hfill & \ddots \hfill & ⋮\hfill & ⋮\hfill \\ \multicolumn{1}{c}{{\mathbf{C}}_p\mathbf{A}{\mathbf{B}}_q}& {\mathbf{C}}_p{\mathbf{B}}_q\hfill & \ddots \hfill & 0\hfill & 0\hfill \\ \multicolumn{1}{c}{⋮}& \hfill & \ddots \hfill & {\mathbf{D}}_{\mathrm{pq}}\hfill & 0\hfill \\ \multicolumn{1}{c}{{\mathbf{C}}_p{\mathbf{A}}^{N-2}{\mathbf{B}}_q}& \hfill & \cdots \hfill & {\mathbf{C}}_p{\mathbf{B}}_q\hfill & {\mathbf{D}}_{\mathrm{pq}}\hfill \end{array}\right]$$ (55)

where subscript $q=2,3$ is used to indicate the corresponding matrix to complete the prediction. The concept of prediction is used to provide a prediction model of the CSPCP signals described in (34), (35) and (36).