Inverse Kinematics Analysis of a P2CuP2Cu Concentric Tube Robot with Embedded Micro-actuation for 3T-1R Contactless Tasks

Abstract

This paper introduces a novel kinematic structure based on the concentric tube robot (CTR) paradigm, augmented with embedded soft micro-actuation. The latter allows to replace troublesome R-joints in CTR with 3 tubes by active tube curvatures (Cu-joints). First, the forward kinematic model is derived. Furthermore, the inverse kinematic problem is partially solved by restricting it to 3-translations/1-rotation movements. Finally, the inverse model is used to perform path planning schemes in medical scenarios.

1 Introduction

Flexible and miniaturized instruments are widely used for minimally invasive interventions. In this scope, continuum robots provide doctors with a controllable small device offering high precision navigation inside the human body for both therapeutic and diagnosis purposes [4]. Particularly, concentric tube robots (CTR) have proven their efficiency for a wide range of medical applications [2, 10, 11, 13, 15]. CTR are formed by several pre-shaped tubes nested in each other that can translate and rotate relatively in a telescopic way. The improved efficiency is due, among other reasons, to the use of tubes of small diameters ranging from 3 mm down to 0.8 mm [16] with a free lumen able to embed different medical tools. Despite all these advantages, CTR suffer from mechanical limits such as snapping explained in [16] and caused by the high torsional energy involved when rotating the tubes relatively. In fact, the rotation input at the tube base is all the more different from its output angle at the tip as its length and/or curvature are higher. Another issue with CTR is that once the tubes are assembled, the curvature of each component is a fixed parameter and cannot be modified afterward, which narrows the possible workspace to cover as demonstrated in [6,7,8]. Furthermore, in order to perform the relative movements, each tube is attached to the rotation stage, which can be connected to the translation stage [3, 10, 18]. For n concentric tubes, one needs 2n actuators that should overcome the frictional and torsional efforts of the tubes and thus are of a consequent size compared to the effector size. With a 3-tubes CTR, 6 actuators are used implying the possibility to control the 6 degrees of freedom (DoF) of the robot end-effector.

The aim of this work is to propose an equivalent structure of a standard 3-tubes CTR but augmented with embedded soft micro-actuation based on 2 tubes only and considered as a P2CuP2Cu. P denotes the prismatic joints, equivalent to the translational movements; and the 2 active tube curvatures in 2 orthogonal directions for each tube are denoted by Cu for each bending. This structure is intended to keep the same performances with less mechanical constraints and an embedded soft actuation scheme able to be easily integrated into an actual operating room. If the forward kinematic model is rather easy to derive, inversion of the robot model in a geometrical way is quite challenging but useful for stable path planning including obstacle avoidance, tissue and organ examination with imaging systems, and full robot shape monitoring in constrained environments. From a control point of view, this solution is proposed here in contrast with (i) Jacobian-based inverse-kinematics that require a full knowledge of the kinematics, an important computational time and that are tributary to the non-singular configurations [10], and with (ii) the kinematic-equivalent model based inverse kinematics [14] that monitor exclusively the position of the robot tip (added to the overall shape) by solution exploration without controlling its orientation.

For the sake of the completeness of this introduction, a few words are needed on technology. The comparison of the available micro-actuators such as shape memory alloys (SMA) and piezoelectric materials (PEM) presents the electro-active polymers (EAP) as the best candidates for this application. In fact, EAP-based actuators do not produce additional heating in contrast with SMA, require very low voltages (<2 volts for some ionic EAP) in contrast with PEM, and do not alter the structure mechanics because of their lightweight [6]. Biocompatibility with a medical device is also confirmed and a relatively high strain (more than 20$\%$) should be noted. Section 2 presents the combination of these promising technologies and the forward modeling as the first work led to the best of our knowledge. Furthermore, the inversion of these models is developed in a geometrical way in Sect. 3 for the P2CuP2Cu, it stems for the computation of two tangentially connected circular arcs of controllable curvature, orientation (hence, arc planes), and length. Simulations of path planning results are also presented based on the developed models.

Fig. 1

a CAD design of the proposed P2CuP2Cu robot based on embedded soft micro-actuators at an arbitrary configuration, b Schematic description of an arc of a circle in 3D

2 Concentric Tube Robot with Embedded Soft Micro-actuation

2.1 Embedded Soft Micro-actuation in Concentric Tube Robots

Adding micro-actuation to a concentric tube robot provides several improvements. First, the intrinsic curvature of each tube is accessible when activating the EAP-based soft micro-actuators deposed as 4 electrodes onto each tube as described in Fig. 1a. In fact, each pair of electrodes generates antagonistic efforts due to an electro-chemo-mechanical conversion. When one positively activated electrode expands in volume, its diametrically opposite one (negatively activated) shrinks which leads to the bending of the substrate tube along the plane containing these electrode central lines. This emerging technology is under continuous improvement and has shown promising results that required expertise in chemistry, micro-fabrication and clean room developments [1, 9]. Moreover, with this 4-electrodes configuration, the rotation motors are not mandatory which reduces substantially the actuation unit size. Furthermore, biocompatible flexible tubes are used and thus small efforts are required to deploy them telescopically. Smaller translation stages are adequate and replace for the high-torque-requiring motors used in standard CTR actuation. The thickness of such actuators varies between 10 and 30 microns and thus does not alter the concentric tube approach and saves the free-lumen configuration, in contrast with cable-driven continuum robots [5]. Finally, in order to preserve the accessible 6 DoF of the standard CTR, we introduce the P2CuP2Cu: a configuration with 2 concentric tubes augmented with 4 EAP electrodes each in order to control their bending in 2 orthogonal directions added to their telescopic deployment.

2.2 Forward Kinematic Model

The forward kinematic model first describes a single section of a CTR considering its actuators. The modeling is based on the piece-wise constant curvature assumption [12, 17]. Indeed, an arc of a circle in 3D is defined, following Fig. 1b, by its curvature $\kappa _j$ (inverse of the radius of curvature $r_j$), its length $\ell _j$, and the angle of the plane in which it is located $\phi _j$. Thus, the transformation matrix from the arc origin to its tip is:

$$\begin{aligned} ^{j-1}\mathbf {T}_j =\,^{j-1}\mathbf {T}_j(\phi _j) ^{j-1}\mathbf {T}_j(\kappa _j,\ell _j) = \left[ \begin{array}{cc} \mathbf {R}_{z}(\phi _j) &{} 0 \\ 0 &{} 1\end{array}\right] \left[ \begin{array}{cc} \mathbf {R}_{y}(\theta _j) &{} \mathbf {p}_j \\ 0 &{} 1\end{array}\right] \end{aligned}$$

(1)

where $\theta _j=\kappa _j \ell _j$ is the bending angle and $\mathbf {p}_j= [r_j(1-\cos \theta _j), 0, r_j \sin \theta _j]^{T}$. The arc variables are directly linked to the robot actuators, depending on the overlapping of the n tubes constituting m sections. For the considered P2CuP2Cu with 2 tubes, the actuator space is constituted of $\mathbf {q} = [\begin{matrix} v_{1x}&v_{1y}&v_{2x}&v_{2y}&\rho _1&\rho _2 \end{matrix}]^T$ where $v_{ix,y}$ is the supplied voltage to the $i^{th}$ tube according to its x and y axes respectively, and $\rho _i$ is its translation for $i=\{1,2\}$. The intrinsic curvatures of each tubes are denoted $\kappa _{ix,y_{in}}=C_{EAP_i}v_{ix,y}$ noting that $C_{EAP_i}$ is the EAP electro-chemical constant of the $i^{th}$ tube electrodes. The intrinsic arc variables for the 2 tubes ($i=\{1,2\}$) are given by:

$$\begin{aligned} {\left\{ \begin{array}{ll} \kappa _{i_{in}} = \sqrt{\kappa _{ix_{in}}^2+\kappa _{iy_{in}}^2} \\ \phi _{i_{in}} = atan2(\kappa _{iy_{in}},\kappa _{ix_{in}}) \end{array}\right. } \end{aligned}$$

(2)

The second section (containing only tube 2) variables are directly identified as $\kappa _2 = \kappa _{2_{in}}$ and $\phi _2=\phi _{2_{in}}$. However, for the first section subject to the mechanical interaction of 2 tubes, one must compute the first section variables by:

$$\begin{aligned} {\left\{ \begin{array}{ll} \kappa _1 = \sqrt{\kappa _{1x}^2+\kappa _{1y}^2} \\ \phi _1 = atan2(\kappa _{1y},\kappa _{1x}) \end{array}\right. } \end{aligned}$$

(3)

where $\kappa _{1x} = \dfrac{\sum _{i=1}^2 E_{i}I_{i}\kappa _{i_{in}}\cos \phi _{i_{in}}}{\sum _{i=1}^2E_{i}I_{i}}$, and $\kappa _{1y} = \dfrac{\sum _{i=1}^2 E_{i}I_{i}\kappa _{i_{in}}\sin \phi _{i_{in}}}{\sum _{i=1}^2E_{i}I_{i}}$. Note that $\kappa _{ix}$ and $\kappa _{iy}$ are the decomposition of the main curvature along the x and y axes respectively for the $j^{th}$ section, $E_{i}$ is the elastic modulus, and $I_i$ is the cross sectional moment of inertia of the $i^{th}$ tube. We should note that $\kappa _{i_{in}}$ is constant for the standard CTR when they are directly accessible in the proposed P2CuP2Cu with the pair of electrodes. Finally, considering the initial pose of the robot where all the tubes are withdrawn, the link lengths are such that $\ell _j=0, \forall j=\{1,2\}$. Whenever the tubes are deployed, the link lengths are computed as $\ell _1=\rho _1$ and $\ell _2=\rho _2-\rho _1$.

3 A Geometrical Approach to Kinematic Model Inversion for 3T-1R Contactless Planning

3.1 Closed-Form Planar Solution to the Translation Part

The model is inverted geometrically in order to find a closed-form planar solution to properly control the robot end-effector position as a first step. For standard CTR with 3 tubes, straightforward computation of the exact inverse kinematic model is very arduous due to the non-linear equations involved and require challenging inverse kinematics or heuristic methods which are often limited by singularity issues, demand significant computational resources and a consequent execution time. In summary, the proposed algorithm computes the inverse kinematic model of the P2CuP2Cu in 3D for 4 DoF including the three position components and one in-plane orientation as described below.

3.1.1 Plane Definition

Recalling the geometrical description of an arc (or a tube) in Fig. 1b, let us assume that S is its start point, $E(X_{E},Y_{E},Z_{E})$ its end point, and $\overrightarrow{z_{j_1}}$ its tangent at the origin S. We consider that the robot sections lie in the same plane $\varPi$ where $\varPi =(S,\overrightarrow{z_j},\overrightarrow{SE})$. The output of this step is the homogeneous out-of-plane transformation $^{j-1}\mathbf {T}_j(\phi _j)$ defined in Eq. 1 such that:

$$\begin{aligned} ^{j-1}\mathbf {T}_j(\phi _j) = f(S,E) \quad \text {where} \quad \phi _j = atan2(Y_{E},X_{E}) \end{aligned}$$

(4)

The new frame at S is then $\left( \overrightarrow{x_j},\overrightarrow{y_j},\overrightarrow{z_j}\right)$ where $[\overrightarrow{u_j}, 1]^T =\,^{{j-1}}\mathbf {T}_j(\phi _j) [\overrightarrow{u_{j-1}}, 1]^T$ for $\overrightarrow{u} = \{\overrightarrow{x},\overrightarrow{y},\overrightarrow{z} \}$.

3.1.2 Planar Solution for a Single Arc

Once the plane $\varPi$ is figured out, the process is performed in a planar way and thus reduces substantially the computation complexity. The desired point is projected onto the frame related to the plane $\varPi$ such that its coordinates are $^{\varPi }X_{E}=\sqrt{X_{E}^2+Y_{E}^2}$, $^{\varPi }Y_{E}=0$, and $^{\varPi }Z_{E}=Z_{E}$. In all cases, for a tangent vector $\overrightarrow{z_j}$, the end-effector of an arc of a circle is described in its base frame $(S,\overrightarrow{x_j},\overrightarrow{y_j},\overrightarrow{z_j})$ (cf. Fig. 1b) by Eqs. 5 and 6.

$$\begin{aligned} \kappa _j = {\frac{2 ^{\varPi }X_{E}}{^{\varPi }X_{E}^2+^{\varPi }Z_{E}^2}}, \end{aligned}$$

(5)

$$\begin{aligned} \ell _j = {\left\{ \begin{array}{ll} \frac{1}{\kappa _j} \ acos\left( 1-\kappa _j \ ^{\varPi }X_{E}\right) \quad \text {if} \quad ^{\varPi }Z_{E} > 0\\ \frac{1}{\kappa _j} \left( 2\pi -acos\left( 1-\kappa _j \ ^{\varPi }X_{E}\right) \right) \quad \text {else.} \end{array}\right. } \end{aligned}$$

(6)

The homogeneous in-plane transformation matrix is then computed such as $^{j-1}\mathbf {T}_j$ in Eq. 1. This step is denoted $(\kappa _j,\ell _j,^{j-1}\mathbf {T}_j(\kappa _j,\ell _j)) = g(S,E,\overrightarrow{z_j})$.

3.1.3 Closed-Form Solution to the Translation Part

For the considered 2-tubes P2CuP2Cu, this single-arc inverse kinematic model g is computed for each arc separately, once the plane $\varPi$ is defined. Both arcs meet at A which is the inflection point along the robot structure. Let us assure that for any $A \in \varPi$, closed-form solutions can be computed [14]. We compute the single arc solution following this order: (i) from the robot origin O to the first section end-effector A – assuming that O is also the world frame $(O,\overrightarrow{x_0},\overrightarrow{y_0},\overrightarrow{z_0})$ origin – then (ii) from A to the second section (and the robot) end-effector B:

$$\begin{aligned} {\left\{ \begin{array}{ll} \left( \kappa _1,\ell _1,^{0}\mathbf {T}_1(\kappa _1,\ell _1)\right) = g(O,\overrightarrow{z_1},A) \quad \text {for tube 1}\\ \left( \kappa _2,\ell _2,^{1}\mathbf {T}_2(\kappa _2,\ell _2)\right) = g(A,\overrightarrow{z_2},^2B) \quad \text {for tube 2} \end{array}\right. } \end{aligned}$$

(7)

which can be expressed by the global closed-form function h:

Fig. 2

Solving the closed-form inverse model for a desired point B after computing the plane $\varPi$ (in green) with an arbitrary inflection point A. Section 1 plot is blue and Section 2 is red

$$\begin{aligned} \left( \kappa _1,\ell _1,\kappa _2,\ell _2,^{0}\mathbf {T}_1(\kappa _1,\ell _1),^{1}\mathbf {T}_2(\kappa _2,\ell _2)\right) = h(O,\overrightarrow{z_1},A,B) \end{aligned}$$

(8)

Note that using f(O, A), we define $F_1=(O,\overrightarrow{x_1},\overrightarrow{y_1},\overrightarrow{z_1})$ the origin frame of the first arc and thus $\overrightarrow{z_1}=\overrightarrow{z_0}$. The output frames of the first and second arcs respectively are $F_2 = (A,\overrightarrow{x_2},\overrightarrow{y_2},\overrightarrow{z_2})$ and $F_3 = (B,\overrightarrow{x_3},\overrightarrow{y_3},\overrightarrow{z_3})$ computed using g function, and $^2B$ is the expression of B in the frame $F_2$ obtained by inverting $^0\mathbf {T}_1(\kappa _1,\ell _1)$. To check the validity of this solution, the results of Eq. 8 are fed through the forward model. It enables to draw the robot shape and pose. The end-effector position must match the initial desired point as described in Fig. 2 with orientation $\overrightarrow{z_3}$.

3.2 Monovariable Virtual Proportional Control

The closed-form model inversion presented above solves for the 3D position of the end-effector at B. In order to control the end-effector in-plane orientation, we use Algorithm 1. It is based on virtual proportional control of a single variable which is the in-plane orientation at B denoted $\overrightarrow{z_B}$ with a proportional gain $\lambda$. After initializing the inflection point A at an arbitrary position in the calculated plane $\varPi$ (cf. Sect. 3.1.1), we solve the inverse kinematics for the position of point B through the function h in Eq. 8. The output orientation $\overrightarrow{z_3}$ at B is then compared to the desired orientation $\overrightarrow{z_B}$. At every step, the point A is slid on the perpendicular (D) to (OB) in the plane $\varPi$ at A defined by its guiding vector $\overrightarrow{u_D}$.

Once the arc variables $\mathbf {\chi } = [\begin{matrix} \kappa _{1}&\phi _{1}&\kappa _{2}&\phi _{2}&\ell _{1}&\ell _{2} \end{matrix}]^T$ are computed, the actuator configurations $\mathbf {q}$ are calculated by inverting Eqs. 2 and 3. For the brevity of this paper, such demonstrations are not detailed.

3.3 3T-1R Task Planning

Using the aforementioned kinematic inversion, examples of the path planning simulations of the P2CuP2Cu are presented hereby. For the intended medical applications, a sweeping scheme of the distal tube end-effector housing an optical imaging system (camera with fiber bundle, OCT probe, confocal microscope) is developed. More specifically, a square sweeping of $10 \times 10$ mm$^2$ is validated by the simulations in Fig. 3a. One can note that the end-effector orientation is preserved (orthogonal to the examined tissue surface) which respects the constraints of a sweeping procedure. A circular path on a sphere of 2 mm radius is also performed (Fig. 3b) while keeping the orientation pointing to the sphere center as for a tissue examination with a sub-degree precision. These planning schemes were performed according to mechanical and fabrication constraints of a maximum arc lengths of 40 mm and maximum curvatures of $200\,$m$^{-1}$.

Fig. 3

For an arc length ratio $L_{r}=1$, the robot poses during model inversion based path planning are shown for a a square scanning and b a spherical cap scanning schemes

4 Conclusions

An alternative structure to the usual 3-tubes CTR was presented in this paper. This P2CuP2Cu is based on EAP soft micro-actuators and provides equivalent kinematic performances with only 2 tubes with a free lumen and a continuum shape approach. The major expected advantages of our robot are the compactness due to the embedded soft micro-actuation and the controllability especially for medical applications. Furthermore, the inverse kinematic model was analyzed in a geometrical approach and improved with an in-plane orientation control. This virtual control was validated by path planning schemes (namely tissue scanning) in simulations for 3T-1R tasks. Deriving the complete inverse kinematic model is a future challenge and will conduct to a full pose control.