Introduction

Minimally invasive surgery (MIS) is becoming the standard treatment for many surgical procedures. Shorter patient recovery time, reduced trauma, reduced inflammatory reaction and potentially lower medical costs are some of the claimed benefits. For example in the case of heart surgery, the procedure could be performed on a beating heart as opposed to open heart surgery, which more frequently needs an arrested heart and a cardiopulmonary bypass. If operated on an arrested heart, the heart is assessed in a non-physiological condition. In the beating heart approach, the surgeon can more easily evaluate the outcome of her/his actions and adjust the intervention on the spot to optimize the clinical outcome. Whereas the patient is seeing multiple and clear benefits, a surgeon faces an increased difficulty in execution of the procedure itself, due to reduced access and reduced dexterity. To simplify the problem, nowadays, one often resorts to rapid pacing, a technique for temporary heart arrest, which minimizes cardiac motion and therefore simplifies instrument positioning. For example, in the procedures such as transcatheter aortic valve replacement (TAVI) [5], rapid pacing is used when the arrest of left ventricular ejection is required. The periods where rapid pacing is used are, however, to be kept minimal in order to ensure haemodynamic stability [16]. To overcome the issues related to increased difficulty in catheter positioning, different robotic solutions have emerged. Current robotic cardiac catheters, such as the commercially available Artisan Control Catheter (Hansen Medical) or CorPath Vascular Robotic System (Corindus Vascular Robotics), do not provide sufficient velocities to allow for stable contact with intracardiac structures [14].

Hybrid rigid/continuum robotic mechanisms for cardiovascular interventions

To allow operation on a beating heart without the temporary heart arrest, a dynamic robotic system that guides the positioning and deployment of surgical instruments in the moving environment can be employed. For example in the work by Kesner [14], a motion compensation control strategy for a single-degree-of-freedom (DoF) robotic catheter was demonstrated. From a kinematic point of view, the catheter employed by Kesner et al. is geometrically constrained by the introductory sheath to move in one single DoF. For certain procedures, such as transapical approach to TAVI [5], the flexible instrument (catheter) is inserted into the heart in the apex area through the access port (trocar). By introducing the instrument through the trocar, the surgeon is effectively losing two degrees of freedom. Her/his motions are restricted to insertion/retraction, rotation and pivoting about the entry point; hence, additional degrees of freedom are required to correctly position and align the instruments. By using an instrument which offers two additional local DoFs past the trocar point, instrument dexterity can be restored, allowing for complete control over position and orientation of the tool [20]. The combination of a rigid robot with a tip-mounted active continuum robot proposed in [20] would restore the 6 DoFs required to correctly position and align the surgical instruments. A similar robotic mechanism was used in the work of Thienprapa [25] for operations on a beating heart. They showed that it is possible to accurately control the position of the tip of a robot also in the presence of trocar constraints. Despite operating on a beating heart, the control strategy did not involve active heart-beating compensation techniques and the overall system featured slow motion. The main difference between hybrid rigid/continuum approach and the more traditional mechanisms such as da Vinci system (Intuitive surgical) is in the compliant nature of the end-effector as opposed to the rigid in-body wrist mechanism of the da Vinci system. Hybrid rigid/flexible robotic systems offer advantages in terms of safety. In case of the da Vinci system, reports indicate a possibility of tissue damage and instrument damage when contacting tissue or colliding between instruments outside their field of view [15]. When operating a hybrid rigid/flexible robotic system, such problems are expected to be less prominent since a part of the impact can be absorbed by the flexible end-effector.

To the best knowledge of the authors, the use of hybrid rigid/continuum robotic systems to compensate for heart beating has not been attempted yet. It is therefore unknown what performance could be expected from these types of robotic systems when subjected to fast and periodic motions under the trocar constraint. In order to evaluate the behaviour of a hybrid rigid/continuum robotic system in these conditions, we have developed a robotic mechanism where a pneumatically driven continuum robot is mounted on a rigid robot arm. The application of a pneumatically driven continuum robot can be particularly appealing from the safety point of view due to their inherent compliant nature. Other advantages of pneumatic actuation are the ability to affect the shape of the robot only in the part where the actuators are embedded, and in the high bandwidth and velocity of such robotic mechanisms [9].

Related work on kinematics

In the recent years, a variety of different designs of pneumatically driven continuum robots and kinematic models have been proposed. To name a few, the deflection of the pneumatically driven robots under the presence of external loads was treated in [27]. Here a Cosserat rod model of the robot was used to account for deflections resulting from the gravitational loads. Another example includes modelling of a multi-segment robot through a lumped mass–spring–damper model [13]. The behaviour of pneumatically driven robots with variable lengths was described in [11]. This work targeted in particular the kinematics of robots which experience contraction and elongation during their actuation. Another concept of a pneumatically driven robot used as an active colonoscope was demonstrated in [4]. Here a constant curvature assumption was used to model the continuum robot kinematics. Whereas these aforementioned designs are not meant to be used as surgical tools for heart surgery mainly due to their large size, the main concepts of pneumatic actuation and kinematic modelling are also valid for the case of the continuum robot treated in this paper.

Since the goal of this paper is to evaluate the ability of the robotic system to follow complex trajectories under a complex set of constraints, we will deal mainly with free-space motion. The robot is here not subjected to external forces which may alter its kinematics, and a simpler constant curvature continuum robot model can therefore be used here. While there is a variety of different formulations of kinematic modelling through constant curvature representation of continuum robots, generally the problem is separated into two sub-domains: the so-called robot-independent mapping and the robot-specific mapping [28]. The robot-independent mapping describes the robot pose through geometric relations such as curvature and rotation. The robot-specific mapping then relates the actuation variables (pressure, cable length, etc) to the geometric parameters of the robot-independent mapping. However, it would be more desirable to obtain the kinematics and differential kinematic mapping of the actuation variables as a function of the robot pose. This would reduce the effort of having to describe the robot in two different configuration spaces. Furthermore, the robot configuration space of robot-independent mapping demonstrates singularities in cases when the robot is straight. These singularities appear also in the Jacobian matrix of the robot, where regularization methods of the robot Jacobian matrix need to be used to cope with the straight robot configurations [18]. Alternatively multi-modal approaches of constant curvature kinematics are developed to cope with these singular configurations [10, 11]. Here the constant curvature kinematics of the robot is approximated through a multivariate polynomial. While this approximation does not suffer from singularities, the limit on the order of the polynomial affects the overall accuracy of the model.

In this work, we propose a kinematic and differential kinematic model of the constant curvature formulation for the employed continuum robot, where both forward kinematics and differential kinematics are represented as a set of differential equations with boundaries defined at one end of the robot. The methodology is similar to the works presented in [23, 24] which both deal with calculation of differential kinematics for variable curvature robots, but the derivations are here simplified for the constant curvature case.

Contribution

The main goal of the work presented in this paper is to assert the behaviour of a hybrid robotic mechanism when commanded to follow periodic trajectories similar to the heart-beating motion. Hereby a pneumatically driven continuum robot will be employed to achieve additional two DoF past the trocar point.

In our work, the eTC (expressiongraph-based Task Controller [1]) framework is adopted to control the hybrid robotic system composed of rigid and continuum elements. The approach proposed in this work is general in the sense that it does not discriminate between flexible or rigid parts of the system as long as they are modelled in an adequate manner. Both the controller and the actual system are validated in a scenario where following fast trajectories are required. Here, position tracking of the instrument tip is shown to be possible while maintaining a rotational centre of motion. The analysis of the robotic system tracking behaviour deals with free-space motion, and a constant curvature model of the continuum robot is adopted to describe its shape. A novel formulation of the constant curvature kinematic and differential kinematic model is developed which does not exhibit singularities in case of a straight robot configuration.

The paper is built up as follows. Section “System overview” gives an overview of the system and the kinematic and differential kinematic modelling of a continuum robot. The control framework is described in section “Control framework”. The so-called surgical scene which summarizes the collection of objects and coordinate frames of interest along with imposed constraints is introduced in section “Scene and constraints description”. Simulation and experimental results are reported in section “Simulation and experiments”, and finally, conclusions are drawn in section “Conclusion”.

System overview

Figure 1 shows the proposed hybrid robotic system, where the arrows indicate the possible motions the robot is able to perform. Similar to manual operation, the rigid part of the robot should also only execute (i) tool insertion through the entry point, (ii) rotation of the tool about the tool axis and (iii) tool pivoting around the entry point. In addition, the tool tip can be bent in 2 directions. The hybrid robot employed in this work consists out of a pneumatically actuated continuum robot \(\textcircled {{2}}\), mounted on a longer shaft \(\textcircled {{3}}\) which is used to pass the surgical tool through the trocar (indicated by the red circle). This shaft is rigidly connected to a 3D-printed mounting stage \(\textcircled {{4}}\) which is finally mounted on the rigid robot \(\textcircled {{1}}\). The rigid robot employed in this study is a KUKA LWR robot featuring 7 DoF. The continuum robot control box \(\textcircled {{6}}\) contains four valves (SMC ITV-0050) to supply the pressure to the continuum robot through the long pressure tubes \(\textcircled {{5}}\). The servovalve uses an internal pressure controller that regulates the pressure with 0.01bar accuracy. An electromagnetic (em) field generator module (NDI, Aurora) \(\textcircled {{7}}\) is placed in the vicinity of the site of interest, giving absolute position measurements (with respect to the coordinate frame of the field generator) of the continuum robot.

Fig. 1
figure 1

Hybrid robotic system consisting of a flexible continuum robot mounted on a rigid robot arm. The rigid robot provides 6 DoFs to the tip, of which 2 are sacrificed to comply with the kinematic constraint at the entry point into the body. Two additional DoFs are offered by the continuum robot tip as such effectively restoring the instrument dexterity

Full size image

The remainder of this section describes the continuum robot design (“Continuum robot: design and actuation” section) and characterizes its kinematics (“Continuum robot: Kinematics” section) and differential kinematics (“Continuum robot: differential kinematics” section). Section “Workspace and compliance of the continuum robot” provides additional experiments demonstrating the workspace and compliance of the developed continuum robot.

Continuum robot: design and actuation

The design of the continuum robot follows the design principles outlined in [9]. The bandwidth of the robot measured in [9] exceeds 8 Hz with velocities of more than 400 mm/s, which indicates a potential for application of the robot in active heart-beating compensation control strategies.

The continuum robot, depicted in Fig. 2, is a composite structure actuated with four McKibben muscles (a type of pneumatic artificial muscles, PAM) embedded into a flexible nitinol (NiTi) tube. Characterized by a high power density, McKibben muscles are also inherently compliant. An extensive characterization of artificial muscles can be found in the literature [6, 12, 26]. Note that the surrounding NiTi tube acts as a return spring, so that the continuum robot straightens back if the pressure is released. In order to lower the bending stiffness of the structure while minimally affecting the compression stiffness in order to ensure axially stiff robot, the tube of overall length 66 mm and an external diameter of 7 and 6 mm internal diameter has been machined using wire-EDM (electro discharge machining), as shown in Fig. 2. The resulting tube is a symmetric structure with a bending section of 55 mm and two equally long rigid sections at each side. Each muscle has an initial diameter of 2.1 mm and 2.5 mm when pressurized with 6 bar. Muscles are fastened at each side of the continuum robot by two connection pieces. The connection pieces have four holes (2 mm diameter) to accommodate a pair of antagonistic McKibben muscles and one central working channel through which sensors can be inserted. Two em tracking sensors are embedded at both extremities of the continuum robot. These em-sensors allow measurement of the relative pose of the tip of the continuum robot with respect to its base.

Fig. 2
figure 2

The robot is built from a NiTi tube designed to achieve the desired bending. Four McKibben muscles are embedded inside the NiTi tube. At the connection pieces at the tip and the rear, two electromagnetic tracking sensors are attached

Full size image

The four muscles are symmetrically distributed within the NiTi tube, and their position inside the robot is denoted by \(\mathbf{r}_k= d \left[ \begin{matrix} cos\left( k\frac{\pi }{2}\right)&\ sin\left( k\frac{\pi }{2}\right)&\ 0 \end{matrix} \right] \), with \(k \in \{1,\dots ,4\}\). d is the distance of the muscle to the robot centreline. The actuation is realized pneumatically. An increase of muscle pressure \(p_k\) induces a radial expansion of the muscle, as well as an axial contraction, and thereby an axial pulling force \(F_k\). The relation between the force and the pressure can be approximated as a linear relation, where pressure to force constant \(K_F\) relates the muscle applied pressure to force:

$$\begin{aligned} F_k = K_F \ p_k. \end{aligned}$$
(1)

Since this muscle force acts at a distance from the robot centre, a torque is generated at the tip of the robot:

$$\begin{aligned} \mathbf{m}_k = \mathbf{r}_k \times \left[ \begin{matrix} 0&\ 0&\ - F_k \end{matrix} \right] ^T. \end{aligned}$$
(2)

By summing the contribution of all the muscles inside robot, the resulting tip applied torque is:

$$\begin{aligned} \mathbf{m} = \sum \limits _{k = 1}^4 \mathbf{m}_k. \end{aligned}$$
(3)

The tip applied torque results in a continuous torque across the robot resulting finally in a constant curvature robot shape. The curvature of the robot relates to the bending moment through the robot angular stiffness matrix \(\mathbf{K}_u = diag(K_b,K_b,K_t)\):

$$\begin{aligned} \mathbf{u} = \mathbf{K}_u \ \mathbf{m} = K \left[ \begin{matrix} p_4-p_2\,\,\, p_1-p_3 \,\,\, 0 \end{matrix} \right] ^T ,\quad K = d \ K_b \ K_F, \end{aligned}$$
(4)

where \(K_b\) is the robot bending stiffness and \(K_t\) is the robot torsion stiffness. From (4), it can be appreciated that with four muscles symmetrically distributed within the NiTi tube, the bending motions in two directions must be achieved by pressurizing a combination of two muscles. The control variables \({\varvec{q}}_c = \left[ \begin{matrix} q_1&q_2\end{matrix} \right] ^T\) of the continuum robot can be thus defined as:

$$\begin{aligned} \begin{aligned} {q_1}&= {p_1} - {p_3},\\ {q_2}&= {p_2} - {p_4}, \end{aligned} \end{aligned}$$
(5)

where \(q_j , j \in \{ 1,2 \}\) represent the control variables. Equation (5) allows an infinite number of combinations for muscle pressure \(\left( p_1,p_3\right) \) and \(\left( p_2,p_4 \right) \) that yield the same control variables. The pressure in the muscles can be then defined based on the requirement of pre-pressurization at 0.5 bar and minimal overall pressure in the robot:

$$\begin{aligned}&p_j= {\left\{ \begin{array}{ll} q_j+0.5, &{}\,\,\, q_j > 0\\ 0.5, &{}\,\,\, q_j \le 0 \end{array}\right. } \end{aligned}$$
(6)
$$\begin{aligned}&p_{j+2}= {\left\{ \begin{array}{ll} 0.5, &{}\,\,\, q_j > 0\\ q_j+0.5, &{}\,\,\, q_j \le 0 \end{array}\right. } \end{aligned}$$
(7)

The pre-pressurization of the muscle places the working range of the muscle in the area which does not experience the initial backlash which arises around zero pressure [8].

By combining (4) and (5), the relation between the curvature \(\mathbf{u}\) and the control variables \({\varvec{q}}_c\) can be written as:

$$\begin{aligned} \mathbf{u} =\ K \left[ \begin{matrix} -q_2&q_1&0 \end{matrix} \right] ^T. \end{aligned}$$
(8)

The parameter K was obtained experimentally by linearly increasing the control variable \(q_1\) and measuring the bending angle using the embedded em sensors. The obtained value for this parameter is \(K = 0.0024 1/\mathrm{Pa} \cdot \mathrm{mm}\).

Continuum robot: kinematics

The robot is schematically represented in Fig. 3. In order to describe the robot shape, a subset of Cosserat rod theory[2] is hereby adopted. The Cosserat rod model describes the robot deformation through its centreline parametrized by the arc-length coordinate s as shown in Fig. 3. For each arc-length variable s on the centreline, a coordinate frame \( \left\{ i \right\} \) is attached. The coordinate s varies from 0 (where \( \left\{ i \right\} \triangleq \left\{ b \right\} \)) to L, at the tip of the robot (\( \left\{ i \right\} \!\triangleq \! \left\{ t \right\} \)). The length L of the robot represents the length of the bending section of the robot, and for the developed continuum robot, \(L = 55\) mm. Lower sub-script will be used to denote a frame in which a specific variable is expressed. At this point, a convention will be adopted where the position of the first muscle (\(k=1\)) coincides with the x-axis of the \( \left\{ b \right\} \) frame. For notational convenience, if the reference frame is omitted, post-superscript \( \left\{ i \right\} \) will be implicitly assumed as most of the computations come as a function of arc-length variable s that corresponds to \(s=i\).

Fig. 3
figure 3

The robot is represented by its centreline and parametrized by the arc-length parameter s. A frame \( \left\{ i \right\} \) is attached to each point of the centreline. \( \left\{ i \right\} = \left\{ b \right\} \) and \( \left\{ i \right\} = \left\{ t \right\} \) are frames at, respectively, the base and the tip of the continuum robot. Four McKibben muscles are used to actuate the robot, and their position inside the robot cross section coincides with the principle axes of the robot. With the muscles pressurized, the robot takes a constant curvature shape

Full size image

The goal of the forward kinematics is to find the position \(_{b}\varvec{p}^{}(s)\) and orientation of \( \left\{ i \right\} \) w.r.t. a fixed coordinate frame \( \left\{ b \right\} \). With the constant curvature model, expressions (9) and (10) are sufficient to describe the pose \(_{b}\varvec{T}^{i} \in SE(3)\) of the section of the continuum robot and for \(s=i\), expressed in the base frame \({ \left\{ b \right\} }\):

$$\begin{aligned}&\frac{d_{b}\varvec{R}^{}}{\mathrm{d}s} = {_{b}\varvec{R}^{}} \ \hat{\mathbf{u}}, \end{aligned}$$
(9)
$$\begin{aligned}&\frac{d_{b}\varvec{p}^{}}{\mathrm{d}s} = {_{b}\varvec{R}^{}} \ {\mathbf{v}}, \end{aligned}$$
(10)

where \(\hat{\mathbf{u}}\) are the curvatures expressed as a matrix in a skew-symmetric form and \(\mathbf{v}\) represent linear strains. Linear strains can here be used to describe the robot extension or contraction. For an axially stiff robot, which does not contract or extend during the actuation (i.e. the continuum robot employed in this paper), the linear strains are \(\mathbf{v} = [\begin{matrix}0&0&1\end{matrix}]^T\). The homogeneous transformation matrix \(_{b}\varvec{T}^{i}\) between the base frame \( \left\{ b \right\} \) of the continuum robot, and a frame at the centreline \( \left\{ i \right\} \) can be written as:

$$\begin{aligned} _{b}\varvec{T}^{i} = \begin{bmatrix} {_{b}\varvec{R}^{i}}&\quad _{b}\varvec{p}^{i}\\ \varvec{ 0 }_{1\times 3}&\quad 1 \end{bmatrix}. \end{aligned}$$
(11)

The boundary conditions for Eqs. (9) and (10) are known at the base: \(_{b}\varvec{R}^{b} = \mathbf{I}_{3 \times 3} \) , \(_{b}\varvec{p}^{b}= \mathbf{0}_{3 \times 1}\). With the boundary conditions defined at the robot base where \(s=0\), Eqs. (9) and (10) can be solved by direct numerical integration from base to tip (\(s = 0 \rightarrow L\)).

Continuum robot: differential kinematics

The differential kinematics of a robot represents a relation between the change of the actuation variables and the infinitesimal displacement of the robot pose. A convenient way to describe the differential kinematics of the robot is through the spatial manipulator Jacobian matrix \(_{b}\varvec{J}^{i}_{6 \times 2}\) defined in [19]:

$$\begin{aligned} _{b}\varvec{J}^{i} = \left[ \left( \frac{\partial _{b}\varvec{T}^{i}}{\partial {q_1}} {_{b}\varvec{T}^{i}}^{-1} \right) ^{\vee } \left( \frac{\partial _{b}\varvec{T}^{i}}{\partial {q_2}} {_{b}\varvec{T}^{i}}^{-1} \right) ^{\vee } \right] , \end{aligned}$$
(12)

Here this matrix is of dimension \(6 \times 2\). The \(\vee \) operator defined in [19] parametrizes one column of the Jacobian matrix. The pose-differentiated matrix \(\frac{\partial _{b}\varvec{T}^{i}}{\partial q_j}\) is defined as:

$$\begin{aligned} \frac{\partial _{b}\varvec{T}^{i}}{\partial q_j} = \left[ {\begin{array}{*{20}{c}} {\frac{{\partial _{b}\varvec{R}^{i}}}{\partial q_j}}&{}\quad { \frac{{\partial _{b}\varvec{p}^{i}}}{\partial q_j}}\\ \mathbf{0}_{3\times 1}&{}\quad 0 \end{array}} \right] . \end{aligned}$$
(13)

In order to obtain (13), which contains partial derivatives of the robot position and orientation w.r.t. the actuation variables \({\varvec{q}}_c\), the equations of the forward kinematics Eqs. (9) and (10) together with the boundary conditions need to be differentiated w.r.t. \(q_j\). This gives a new set of differential equations for each control variable which define the differential kinematics problem. These equations come as a function of the arc-length parameter as well:

$$\begin{aligned} \frac{d}{\mathrm{d}s}\left( \frac{\partial _{b}\varvec{R}^{}}{\partial q_j}\right)= & {} \frac{\partial _{b}\varvec{R}^{}}{\partial q_j} {_{}\hat{\mathbf{u}}^{}} + \ _{b}\varvec{R}^{} \frac{\partial {_{}\hat{\mathbf{u}}^{}}}{\partial q_j}, \end{aligned}$$
(14)
$$\begin{aligned} \frac{d}{\mathrm{d}s}\left( \frac{\partial _{b}\varvec{p}^{}}{\partial q_j}\right)= & {} \frac{\partial {_{b}\varvec{R}^{}}}{\partial q_j} \mathbf{e}_z, \end{aligned}$$
(15)

where \(\frac{\partial {_{}\hat{\mathbf{u}}^{}}}{\partial q_j}\), here represented in a skew-symmetric form follow from differentiation of (8) w.r.t. \({\varvec{q}}_c\):

$$\begin{aligned} \frac{\partial \mathbf{u}}{\partial q_1} = \left[ \begin{matrix} 0&K&0 \end{matrix}\right] ^T, \quad \frac{\partial \mathbf{u}}{\partial q_2} = \left[ \begin{matrix} -K&0&0 \end{matrix}\right] ^T. \end{aligned}$$
(16)

The differential Eqs. (14) and (15) can be solved by direct numerical integration of the boundary conditions \(\frac{\partial _{b}\varvec{R}^{b}}{\partial q_j} = \mathbf{0}_{3 \times 3} , \frac{\partial _{b}\varvec{p}^{b}}{\partial q_j} = \mathbf{0}_{3 \times 1}\) from the base to the tip of the robot (\(s = 0 \rightarrow L\)). It can be appreciated from (14) that the rotation \(_{b}\varvec{R}^{}\) appears in the equations of differential kinematics. The equations of the differential kinematics are therefore coupled to the equations of forward kinematics. For this reason, Eqs. (14) and (15) need to be integrated simultaneously together with the equations of forward kinematics Eqs. (9) and (10).

As previously noted, this formulation of differential kinematics is free of singularities in case of a straight robot (\({\varvec{q}}_c =\left[ \begin{matrix} 0&0 \end{matrix}\right] ^T\)). For the developed continuum robot, the Jacobian matrix in straight configuration is:

$$\begin{aligned} _{b}\varvec{J}^{t} = \left[ {\begin{array}{*{20}{c}} {3.63}&{}\quad 0&{}\quad 0&{}\quad 0&{}\quad {-0.132}&{}\quad 0\\ 0&{}\quad {3.63}&{}\quad 0&{}\quad {0.132}&{}\quad 0&{}\quad 0 \end{array}} \right] ^T. \end{aligned}$$
(17)

Workspace and compliance of the continuum robot

The developed continuum robot achieves bending angles of approximately \(\pm 45^{\circ }\) (Fig. 4). The muscle shortening at these maximum angles is about 4 % of the non-actuated initial muscle length.

Fig. 4
figure 4

The workspace of the continuum robot

Full size image

The use of the pneumatic actuation allows for an additional level of compliance in the robotic system. In order to demonstrate the load- bearing capacity and the compliance of the robot, a 6-DoF force sensor (Nano43, ATI Industrial Automation) was installed between the instrument shaft and the 3D printed stage. Placed in the vicinity of the em-field generator, the set-up allows for measurement of tip deflections resulting from tip applied loads on the robot.

Figure 5 demonstrates the compliance of the robot. In this experiment, the robot was initially pressurized by setting \({\varvec{q}}_c = \left[ \begin{matrix} 4.28&0\end{matrix} \right] ^T bar\) and applying force on the tip of the robot. By applying force of \(\mathbf{F}=\left[ \begin{matrix} 0.05&0.4&0.55\end{matrix} \right] N\), the robot tip deflects by \(\varDelta _{}\varvec{p}^{}=\left[ \begin{matrix} 43.5&1.5&1\end{matrix} \right] \)mm from its initial (unloaded) configuration.

Fig. 5
figure 5

Demonstration of the robot deflection under external tip load. The pressure in the muscles of the robot is kept constant, while the load on the tip is gradually increased

Full size image

Control framework

The control of the robot is based on the eTC control framework, documented in [1]. This type of constraint-based control allows easy definition and evaluation of tasks defined in different coordinate spaces and in different reference frames. The eTC framework is a velocity-resolved control framework, and thus, the desired quantities and control laws are expressed in terms of desired velocities. The kinematic relation between the output space \(\varvec{y}\) and the space of the control variables, joint space \({\varvec{q}}\), and optionally the variables that describe uncontrolled DoFs \(\varvec{\chi }_{\varvec{u}}\) (e.g. the position of an object in the scene) can be written as:

$$\begin{aligned} \varvec{y}=\varvec{f}\left( {\varvec{q}},\,\varvec{\chi }_{\varvec{u}}\right) \end{aligned}$$
(18)

In a velocity-resolved approach, the derivation of control actions relies on the computed Jacobian matrices \(\varvec{A}{}\) (which includes the robot Jacobian) and \(\varvec{B}{}\), defined as:

$$\begin{aligned} \dot{\varvec{y}}=\varvec{A}{}\dot{\varvec{q}}+\varvec{B}\dot{\varvec{\chi }}_{\varvec{u},} \end{aligned}$$
(19)

where

$$\begin{aligned} \varvec{A}{}=\frac{\partial \varvec{f}\left( {\varvec{q}},\,\varvec{\chi }_{\varvec{u}}\right) }{\partial {\varvec{q}}}\dot{\varvec{q}}, \quad \varvec{B}{}=\frac{\partial \varvec{f}\left( {\varvec{q}},\,\varvec{\chi }_{\varvec{u}}\right) }{\partial \varvec{\chi }_{\varvec{u}}}\dot{\varvec{\chi }}_{\varvec{u}.} \end{aligned}$$
(20)

The first term of (19) relates the variation of controllable variables \(\dot{\varvec{q}}\), to the variation of outputs \(\dot{\varvec{y}}\), while the second term relates the variation of uncontrollable and/or estimated quantities to \(\dot{\varvec{y}}\).

In the case at hand, the second term will be used to represent a moving estimated frame of the heart. The robot position controller can be defined in terms of desired commanded velocities:

$$\begin{aligned} \dot{\varvec{y}}^{\circ }_{d}= \varvec{K}_{p}(\varvec{y}_{d}-\varvec{y})+\dot{\varvec{y}}_{d}, \end{aligned}$$
(21)

where \(\varvec{y}_{d}\) and \(\dot{\varvec{y}}_{d}\) are the desired generalized positions and velocities, \(\varvec{y}\) is the vector of position measurements, and \(\dot{\varvec{y}}^{\circ }_{d}\) are the calculated(desired) velocities in the output space. \(\varvec{K}_{p}\) is a (diagonal) matrix that contains the gains of the controller; its dimensions being \([1/\mathrm{s}]\). The gains here define the response time for each component of the constraint. Once the desired commanded velocities are determined, the problem of obtaining the control joint velocities is formulated as an optimization problem (see [7]):

$$\begin{aligned} \dot{\varvec{q}}= \mathop {\lim }\limits _{\varepsilon \rightarrow 0} \arg \min \left[ \varepsilon \left\| \dot{\varvec{q}}\right\| _{\varvec{H}}^2 + \left\| \varvec{A}\dot{\varvec{q}}-\dot{\varvec{y}}^{\circ }_{d}- \varvec{B}\dot{\varvec{\chi }}_{\varvec{u}}\right\| _{\varvec{W}}^2 \right] . \end{aligned}$$
(22)

where \(\varvec{W}\) is a diagonal weighting matrix that, in case of conflicting constraints, determines which constraint will be dominant. \(\varvec{H}\) is a matrix of joint weights that in case of redundancy determines which joint should be given the preference. \(\varepsilon \) is the regularization constant and represents a small positive value for which an approximate solution of (22) can be found. The vector of joint variables \({\varvec{q}}= \left[ \begin{matrix} {\varvec{q}}_r^T&;&{\varvec{q}}_c^T \end{matrix} \right] ^T\) here combines the joint variables of the rigid robot \({\varvec{q}}_r\) and the joint variables of the continuum robot \({\varvec{q}}_c\).

The weighting matrices \(\varvec{H}\) and \(\varvec{W}\) are also used to normalize joint variables and outputs that might have different physical dimensions. This is important for the proposed hybrid system as the joint variables of the continuum robot are expressed as pressure (physical dimension of \([\mathrm{bar}]\)). The same reasoning holds for matrix \(\varvec{W}\) that combines linear and rotational constraints.

In the case at hand, the optimization scheme (22) computes the vector \(\dot{\varvec{q}}\) of generalized joint velocities, which includes the joint velocities to be commanded to the rigid robot \([\mathrm{rad/s}]\) and the commanded pressure variation \([\mathrm{bar/s}]\) of the muscles in the continuum robot. The calculated velocities are directly given as a set-point for the rigid robot which is controlled at the velocity level. In the case of the continuum robot, these values are integrated to obtain the control variables \({\varvec{q}}_c\). The pressure for the control valves \(p_k\) can then be calculated according to Eqs. (6) and (7).

Scene and constraints description

In constraint-based approaches, the definition of tasks is achieved in two steps. First frames are defined as they are attached to the robot(s) and object(s) of interest, and then, the outputs \(\varvec{y}\) that are to be constrained are defined. Here, the objects of interest are the entry point (frame \( \left\{ k \right\} \)) and the beating heart (frame \( \left\{ f \right\} \)) at the point inside the patient’s body where the treatment has to be delivered. Frame \( \left\{ o \right\} \) indicates the stationary, reference position of the heart. Frame \( \left\{ f \right\} \) represents in fact the uncontrollable frame resulting from the uncertainty related to the motion of the heart itself w.r.t. the frame \( \left\{ o \right\} \). On the robot, the frames \( \left\{ w \right\} \), \( \left\{ b \right\} \) and \( \left\{ t \right\} \) are defined. The rigid part of the robot is located between frame \( \left\{ w \right\} \) and frame \( \left\{ b \right\} \). Latter is attached to the base of the flexible robot. Frame \( \left\{ t \right\} \) is conveniently attached to the tip of the continuum robot and indicates the frame of the surgical tool. Figure 6 depicts the hybrid robotic system and the corresponding objects of interest.

Fig. 6
figure 6

Representation of the scene and the frames which define the relation between the objects in the scene

Full size image
Table 1 Constraints used in the experiment
Full size table

A number of constraints follow from the envisioned surgical task: (i) the position and orientation of the tip frame relative w.r.t. the treatment site are to be controlled. These form a total of six constraints. (ii) The instrument shaft is to be controlled along the z-axis of frame \( \left\{ b \right\} \) at the entry point. This counts for two additional constraints. Table 1 summarizes the list of constraints and the frames in which the constraints are defined.

The first set of constraints, labelled as tip pose, implements a position Cartesian control. This set of constraints ensures that the desired pose, i.e. zero relative orientation and positions, is expressed in the object frame \( \left\{ o \right\} \). The heart beating is expressed through the relative motion of \( \left\{ f \right\} \) w.r.t. \( \left\{ o \right\} \) and is compensated through the feed-forward term \(\varvec{B}\dot{\varvec{\chi }}_{\varvec{u}_{ \left\{ f \right\} }}\).

The second set of constraints, labelled as trocar constraints, imposes that the shaft of the instrument, which is collinear to the z-axis of frame \( \left\{ b \right\} \), passes through the entry point. This behaviour is achieved by simply representing the entry point position \(\mathbf{k}\) in the frame \( \left\{ b \right\} \),

$$\begin{aligned} _b\mathbf{k} = \left( _{w}\varvec{T}^{b}\right) ^{-1}\ _w\mathbf{k}, \end{aligned}$$
(23)

selecting the x- and y-coordinates as part of the output vector,

$$\begin{aligned} \varvec{y}_{tr}=\begin{bmatrix}1&\quad 0&\quad 0\\0&\quad 1&\quad 0\end{bmatrix}\,_b\mathbf{k}, \end{aligned}$$
(24)

and imposing convergence to zero by means of (21) with null desired output vector \(\varvec{y}_{d}\) and output desired velocity \(\dot{\varvec{y}}_{d}\). For simplicity, the trocar frame \( \left\{ k \right\} \) is assumed to be stationary in space. Note that in real surgical applications also the trocar position might move due to the breathing and heart motion. To account for this motion, a similar methodology as in the tip pose constraint can be applied, where an additional feed-forward term \(\varvec{B}\dot{\chi }_{\varvec{u}_{}}\) could be used to compensate for the motion of the trocar.

Simulation and experiments

This section presents simulation and experimental results that were obtained in order to assess the performance of the proposed hybrid robotic system and the proposed control approach. The envisioned scenario follows from the case of transapical transcatheter valve implantation on a beating heart [5]. Overall, the procedure requires the system to be able to follow motion due to heartbeat, but should at the same time respect the constraint of the instrument entry point through the incision.

Figure 7 shows the experimental set-up. Here, the tip of the robot was moved in two different positions, under the trocar constraint. In configuration \(\textcircled {{1}}\), the tip of the robot is positioned on the location of the \( \left\{ o \right\} \) frame. Robot configuration \(\textcircled {{2}}\) corresponds to a singular robot configuration due to the fully extended elbow joint of the KUKA LWR robot. For a subset of the configuration space \(\textcircled {{3}}\) indicated by a dashed line, the grey space \(\textcircled {{4}}\) remained unreachable. In this area, the robot is unable to position the tip when keeping the orientation of the tool towards the ground and simultaneously maintaining the trocar constraint.

Fig. 7
figure 7

The actual experimental set-up. The tip of the continuum robot is maintaining the orientation towards the ground, while positioned in two different positions of the workspace. Simultaneously the rigid part of the robot is passing through the trocar

Full size image

Methods such as the one described in [22] can be used to estimate the location of an optimal pivot point. Here, the position of the tip frame \( \left\{ t \right\} \) and the frame \( \left\{ b \right\} \) are both measured by the em tracking sensors. By expressing the position of the em sensor in \( \left\{ t \right\} \) w.r.t. the sensor at \( \left\{ b \right\} \), an independent system for measuring the deflection of the continuum robot tip is obtained. This pose is then superimposed on the pose of the rigid robot. Latter can be measured by the different robot encoders. From this, the tip position measurement \(\varvec{y}\) of the hybrid robotic system is obtained.

Experimental parameters

The tip position and orientation of the tool-tip frame \( \left\{ t \right\} \) were commanded to follow the pose of frame \( \left\{ f \right\} \). Frame \( \left\{ f \right\} \) moves along the z-axis of the \( \left\{ o \right\} \) with a periodic motion, such that it resembles the heart beating. The incision point constraint is here represented by a stationary constraint by fixing the position of \( \left\{ k \right\} \) rigidly in space.

From the spectral decomposition of the heart motion performed by Cavusoglu and Bebek [3], the first harmonic of heart motion is found at 2 Hz with frequency components going up to 26 Hz . The results of this study indicate that the lower frequency components of heart motion typically result from breathing. Here a dominant mode can be found around approximately 0.37 Hz. Overall, the power density of motion at frequencies higher than 8 Hz can be neglected. In general, the heart demonstrates motion in three DoF, and each of the axes can be considered as independent [3, 21]. For the purposes of evaluation of the current robotic system to track dynamic trajectories, a more representative and simplified motion trajectory will be selected. The robot tip will be commanded to move along a single line, which coincides with the z-axis of the world frame. This motion will be represented with a single sinusoidal trajectory imposed on the \( \left\{ f \right\} \) frame:

(25)

where A is the amplitude of the sine wave (set to \(10\hbox { mm}\)), and \(\omega =2\pi \nu \) is the angular frequency.

The position and orientation of frame \( \left\{ k \right\} \) defines the position of the trocar w.r.t. the position of the base of the rigid robot at frame \( \left\{ w \right\} \), and the position of the frame \( \left\{ o \right\} \) is provided in Table 2.

Table 2 Pose of the object frame \( \left\{ o \right\} \) and the trocar \( \left\{ k \right\} \) w.r.t. the world frame \( \left\{ w \right\} \) located at the base of the rigid robot
Full size table

The controller is implemented on a Linux machine, with a loop rate running at 100 Hz. This corresponds to the maximum rate at which the low-level velocity control of the KUKA LWR can receive velocity set-points. The parameters of the controller from Eqs. (21) and (22) are given in Table 3.

Table 3 Controller parameters
Full size table

The parameters of the weighting matrix \(\varvec{W}_p\) for the tip pose constraint are chosen such that 1 mm in positioning error corresponds to \(0.058^{\circ }\) error in orientation. Overall a higher importance is given to orientation of the tool. The gains of the tip pose \(\varvec{K}_{p}\) constraints correspond to a time constant for the tip positioning of 2 s. Higher gains for the trocar constraint \(\varvec{K}_{t}\) indicate a shorter time constant for the trocar constraint controller of 0.33 s. Overall, higher importance is given to the trocar constraint than to the tip pose constraint by setting higher weights for \(\varvec{W}_t\).

In order to normalize the joint weights given in the \(\varvec{H}\) matrix, one can observe the effect of the control variables \({\varvec{q}}_c\) on the motion of the robot tip. Here \(1\mathrm{bar}/\mathrm{s}\) corresponds to an angular velocity of the tip of \(7.5{}^{\circ }/\mathrm{s}\). The joint weights in the \(\varvec{H}\) matrix can be normalized by choosing \(\varvec{H}_c\) to be \(diag(\frac{1}{7.5^2})\) with \(\varvec{H}_r = diag(1)\). While choosing these weights would result in one-to-one relation for all of the joint motion of the hybrid robot, we have set the joint weights of the continuum robot \(\varvec{H}_c\) to be much lower than the weights of the rigid robot \(\varvec{H}_r\). The cost of the rigid robot joints is therefore considered higher than the cost of the continuum robot joints. In this sense, the system will be inclined to give the preference to the continuum robot and move the continuum robot first if the robotic tasks is under-determined.

Simulation

First, the capability to track the above-defined trajectory has been evaluated in a simulation scenario. Such simulation gives a first indication of the ability of the controller to cope with fast trajectory following, but in an ideal scenario, i.e. in the absence of modelling errors. Both the rigid robot and the continuum robot are here replaced by an idealized system, where the rigid robot accurately follows the imposed velocity, and the kinematics of the continuum system is accurately described with the kinematic model given in section “Continuum robot: kinematics”.

Figure 8 shows the results of the trajectory following simulation where the tip pose is plotted for the two different sine waves, with frequencies \(\nu \), respectively, at 0.37 and 2 Hz. An increase in tracking error can be observed. The error is present in both cases; however, for the sine trajectory at 0.37 Hz, the maximum errors remain below 0.1 mm. For the more dynamic simulation at 2 Hz, the error increases to 1 mm.

Fig. 8
figure 8

Simulation results for two different values of reference trajectories at 0.37 and 2 Hz. The increase in the error for the 2 Hz reference is obvious, indicating a dependency of the tracking accuracy on the rate at which computations take place. Note that the graphs have different scale

Full size image

This effect is expected to originate from the linearization implicitly introduced by the Jacobian-based control. The measures of Jacobian manipulability can be used here to assess the Jacobian properties. In particular by observing the singular values of the task Jacobian matrix \(\varvec{A}\), where the largest singular value is (in average) 2.02 and the smallest \(3.5 \cdot 10^{-3}\), it can be concluded how the Jacobian matrix \(\varvec{A}\) is ill-conditioned. The overall manipulability index \(\mu \):

$$\begin{aligned} \mu = \sqrt{\det \left( {\varvec{A}{\varvec{A}^T}} \right) }, \end{aligned}$$
(26)

is changing through different configurations of the robot. Figure 9 shows the manipulability of the robot task Jacobian matrix \(\varvec{A}\) in a subset of the robot workspace (see also Fig. 7). This manipulability corresponds to a robot configuration where the trocar constraint is maintained and the robot keeps the orientation of the instrument tip towards the ground. Part of the workspace remained unreachable under these conditions, since the robot was unable to orientate the tip towards the ground in these areas. The rigid robot arm was extending towards its limits as it approached these areas.

Fig. 9
figure 9

Colour indicates a manipulability index in a subset of the robot workspace. The robot tip was orientated towards the ground, and the rigid part of the tool was passing through the trocar constraint. A part of the workspace remained unreachable. Object frame \( \left\{ o \right\} \) is placed in the area of the robot workspace with low manipulability

Full size image

The effect of low manipulability directly relates to the dexterity of the robot, and higher sampling rates are required [17] in case of low manipulability in order to ensure the desired tracking behaviour. This effect is also visible in larger tracking errors when following faster trajectory in simulation (Fig. 8) even though the sample time is the same in both simulations. Furthermore, due to the nonlinear nature of the robotic system, the amplitude and phase of these tracking errors are highly related to the configuration of the robot itself and the position of the robot w.r.t. the location of the trocar and the location of \( \left\{ o \right\} \).

Experiments

Following the results obtained in simulation, an actual experiment was performed. The positions of the trocar point and the object frame as well as the trajectories are set identical to the simulations.

Figure 10 shows the results of a trajectory following experiment. Each graph shows the evolution of a single constraint that is set during the experiment. Overall the tracking performance is acceptable. For example, this can be seen by observing the quality of the sinusoidal reference trajectory following (third graph of Fig. 10). However, even though the sinusoidal reference is applied only on the z coordinate, some oscillations can also be observed in the other graphs showing that the system is not entirely decoupled. The position error remains, however, below 3 mm for both x and y coordinates. The orientation error goes up to \(5^{\circ }\), whereas the error in maintaining a fixed trocar position remains below 2 mm. The tracking errors during this experiment are thus larger than those observed in simulations. In particular, a rise in tracking error of 1,mm for the x-component and 3 mm for the y-component was observed. The error in z direction remains below \(2\hbox { mm}\). Note that in the experiment, the evaluation of tracking accuracy relies on the kinematics of the rigid robot. The technical manual of the KUKA LWR robot reports repeatability of \(0.05\hbox { mm}\). In reality, these errors are expected to be even higher which would result in higher tracking errors than currently reported in the paper.

Fig. 10
figure 10

Experimental results of sinusoidal trajectory following, a tip position, b tip orientation, c position error of the trocar point in x–y plane, expressed in \( \left\{ b \right\} \)

Full size image

The errors could originate from several sources: (i) imperfect system assembly. Amongst others, the alignment of muscles was not done as perfectly symmetric as assumed by the kinematics model. (ii) Muscles have different K coefficients. As such, each muscle has a different pressure to curvature ratio. (iii) Some hysteresis appeared in the continuum robot response. (iv) Dynamic effects that follow from the non-negligible mass of the robot, as well as the presence of non-modelled internal structural damping. (v) Dynamics from the air pressure valves and the delay following from the relatively long air supply tubes. (vi) The lower sampling rate of the em tracking system (40 Hz) as opposed to the (100 Hz) sampling rate of the rigid robot. In further work, the origin of this tracking error will be investigated more deeply. However, the study of the continuum robot characterization is beyond the scope of this paper.

Discussion

This subsection summarizes some additional observations that were made during the experiments and that cannot be immediately observed from the experimental results alone.

Robot workspace

It has been found that certain tool-tip poses are not reachable with the proposed instrument configuration. In particular, the robot was unable to orient the tool accurately in the entire mock-up workspace. This follows from the fact that the continuum robot can only achieve a maximum bending angle within the range of \(\pm 45^{\circ }\). Therefore, the ability of the robot to reach a desired pose depends largely on various parameters such as the relative position of the incision point w.r.t. the rigid robot position, the configuration of the rigid robot and the proximity to its joint limits. These limitations can be overcome or mitigated by planning the procedure and calculating the optimal location of the rigid robot, or by devising new flexible instruments with larger range of angulation or with a certain amount of pre-curvature.

Sample time

It has been observed in the simulation that the trajectory following accuracy improves with faster loop rates. At present, the loop rate is set to 0.01 s which is equal to the rate at which the joint velocity controller of the rigid robot works. Following the rationale given in section “Simulation”, the employed hybrid robotic mechanism requires faster loop rates due to the ill-conditioned robot Jacobian.

Hybrid robotic mechanism

Another relevant observation resulting from this study relates to the use of the rigid robot employed in the scope of this work (KUKA LWR). The robot is a serial-chain robot with seven revolute joints. Since the proposed procedure requires insertion/retraction of the tool through the trocar and some pivoting about this point, the nature of the procedure could indicate that the employed rigid robotic mechanism is not the best possible option to achieve this task. We suggest that the rigid robot should be chosen in combination with the employed continuum robotic mechanism such that the more uniform manipulability of the overall hybrid robotic mechanism is achieved. A possible extension of this work could be to use the proposed simulation framework in order to identify and validate the best possible combination of rigid-continuum robot for a given clinical application.

Effect of tracking errors on the system safety

Whereas a 3 mm error in one of the axes could be dangerous in case if heart motion would abruptly change, the risk of tissue damage is largely mitigated through the compliant nature of the robot, which allows for a part of the impact to be absorbed. For example, assuming that the collision with the heart causes a 3 mm deflection of the tip, the associated force of the continuum robot will be below 0.2 N in such case. Note that such force occurs for the worst-case scenario where the contact force with the tissue is aligned along the (stiff) axis of the continuum robot. If deemed necessary, this force level can be reduced by lowering the axial stiffness of the continuum robot, which involves altering the machining parameters of the surrounding NiTi tube.

Conclusion

This paper proposes a hybrid robotic and control approach, combining a classical rigid robot with a continuum segment, in order to overcome the classical limitations of transapical heart surgery and, more generally, of keyhole surgery. The rigid robot allows 6-DoF instrument positioning, while the continuum robot adds two more DoF at the tip, effectively restoring the 2 DoFs that are lost when passing through a trocar. The continuum robot tip is compliant and as such introduces extra safety into the system, reducing the risk for tissue damage.

The paper shows how by using a control framework adapted from eTC, it becomes possible to control the tip degrees of freedom and make those follow a complex trajectory while respecting constraints imposed by the insertion point. The quality of the proposed approach is validated both in simulation and experiment.

Although the proposed hybrid robotic system shows great potential for use in complex keyhole procedures, further work need to be conducted before allowing this system in a real clinical operating theatre. This includes a further study of the phenomena that explain the existence of a certain tracking error. Also the control framework must be evaluated in more detail to understand how the presence of possible conflicting constraints affect the overall outcome and whether any kind of performance guarantees can be made.

Aside from the TAVI procedure, the range of possible applications of the proposed robotic system could include other cardiac procedures where entrance to the apex can be foreseen (e.g. radio frequency ablation for treatment of atrial fibrillation, mitral valve repair and mitral valve replacement). The system could also be applied in more traditional laparoscopic procedures such as laser ablation and in-situ imaging in foetal surgery.

The effect of low manipulability of the robot Jacobian caused some positioning errors even in the ideal conditions as shown in the simulation scenarios. These positioning errors were larger when the robot was commanded to follow faster trajectories. This effect is also closely related to the lower sampling rates of the controller.

Finally, the developed model of the constant curvature kinematics and differential kinematics is adapted to the continuum robot employed within this study. The continuum robot treated here is inextensible and actuated with a pair of antagonistic muscles. The developed model is free of singularities in case of a straight robot. Extending this model to a more general formulation may yield an alternative to the models which rely on robot description through configuration spaces of robot-specific and robot- independent mapping.