Concentric Tube Robot Redundancy Resolution via Velocity/Compliance Manipulability Optimization

Abstract

Concentric Tube Robots (CTR) have the potential to enable effective minimally invasive surgeries. While extensive modeling and control work have been proposed in the past decade, limited efforts have been made to improve the path tracking performance from the perspective of manipulability, which can be critical to generate safe motion and feasible actuator commands. In this paper, we propose a gradient-based redundancy resolution framework that optimizes velocity/compliance manipulability-based performance indices during path tracking for a kinematically redundant CTR. We efficiently calculate the gradients of manipulabilities by propagating the first- and second-order derivatives of state variables of the Cosserat rod model along the CTR arc length, reducing the gradient computation time by 68% compared to the finite difference method. Task-specific performance indices are optimized by projecting the gradient into the null-space of path tracking. Simulation results show that the proposed method is able to accomplish the required tasks while commonly used redundancy resolution approaches underperform or even fail.

I. Introduction

Concentric Tube Robots (CTR) consist of multiple super-elastic, pre-curved elastic tubes that are nested together. The dexterity and compact dimension of these devices make them ideal for minimally invasive surgeries [1]. Extensive research has been conducted on the mechanics modeling of CTR, aiming to characterize the mapping from actuator input to robot configuration. The most widely adopted approach is the Cosserat rod-based model [2], [3]. This approach describes the spatial evolution of robot states with a system of ordinary differential equations (ODEs), resulting in a boundary value problem (BVP) that can be numerically solved by nonlinear root-finding algorithms. The Cosserat-based model has been used to solve problems of stability analysis [4], stiffness modulation [5], and force sensing [6].

Despite significant advancements over the past decade, achieving reliable and efficient online path tracking with CTR still presents a significant technical challenge, primarily due to the redundancy of the robot and the often complicated working environment. This statement remains particularly true when CTRs are required to achieve multiple objectives, such as following a designated path of the tip while avoiding possibly dynamic obstacles or compensating for deflections caused by external loads. While it is hard to achieve redundancy resolution with direct inverse kinematics, the Jacobian-based resolved rate controller and many of its variations have been widely adopted to partially address this problem. Recent progress includes the efficient Jacobian calculation via forward integration approach [7], and redundancy resolution for secondary task optimization such as joint limit avoidance [8], and instability avoidance [9]. The damped least squares approach [10] can be used to prevent undesirable behavior of the robot under ill-conditioned Jacobians, but the additional regulation term may drive the robot away from the desired trajectory, leading to unwanted behavior.

The velocity/compliance manipulability is a crucial performance measure to evaluate the robot singularity and force capacity for a given configuration [11]. The concept of manipulability was originally proposed for rigid-link robots [12] to determine whether the posture was compatible with task requirements. Despite the significant structural differences between CTR and rigid-link robots, the concept of manipulability can be generalized to CTR [13]. A unified force/velocity manipulability index for CTR was proposed in [11] to estimate the optimal direction for a better force/velocity transmission ratio. However, there have been limited efforts to address the path tracking problem by considering CTR manipulability. One of the most recent studies used the gradient projection method, which tried to reshape the unified force/velocity manipulability ellipsoid into a sphere along the trajectory to avoid instability [14]. However, this method only considered the velocity manipulability at the tip for instability avoidance, and the Hessian was approximated using the Broyden-Fletcher-Goldfarb-Shanno (BFGS) method, which introduced errors that might cause unstable performance and slow down the optimization process [15].

In this paper, we present a gradient-based redundancy resolution framework for CTR that optimizes motion/force capability along any task-required direction during path tracking. We develop the derivative propagation method for the gradient of manipulability, enabling efficient calculation and online path tracking as needed. Furthermore, we propose several task-specific performance indices based on velocity/compliance manipulability, which are optimized by gradient projection. The performance of the redundancy resolution framework is demonstrated through simulations of a three-tube CTR with 6 degree of freedoms (DoFs), where the robot is controlled to follow a desired trajectory (3D position) while utilizing the redundant DoFs to accomplish secondary tasks, including singularity avoidance, obstacle avoidance, and stiffness modulation. This paper is organized as follows: Sec. II provides an overview of the CTR model and manipulability indices. The derivative propagation method is presented in Sec. III. Sec. IV details the redundancy resolution with task-specific performance indices. The simulation results are presented in Sec. V, followed by the conclusion in Sec. VI.

II. Preliminaries

A. Review of CTR Mechanics Model

The use of Cosserat rod theory for modeling the mechanics of CTR is a widely accepted approach [2]. This section provides a brief overview of the CTR mechanics model, and Table 1 summarizes the nomenclature used in this paper. We refer the reader to [7] for detailed derivation.

TABLE I.

Nomenclature

Notation	Definitions
$i=1$	Tube index, the innermost tube is $i$ = 1.
$s$	Arc-length parameter for the central axis
$l_i$	Length of the i-th tube
$l_{s,i}$	Length of the straight part of the i-th tube
${\alpha }_i$	Rotation angle of the i-th tube
${\beta }_i$	Translation length of the i-th tube
$s_{e,i}$	Arc-length parameter at the end of the i-th tube: $s_{e,i}={\beta }_i+l_i$
$s_{t,i}$	Arc-length parameter at the transition from the straight to the precurved part of the i-th tube: $s_{t,i}={\beta }_i+l_{s,i}$
${\gamma }_i$	Exposed length of the i-th tube, ${\gamma }_i=s_{e,i}-s_{e,i+1}$
$\mathbf{q}$	$\mathbf{q}={\left[{\alpha }_1{\beta }_1\dots {\alpha }_n{\beta }_n\right]}^T$ Actuation vector
$\mathbf{p}\left(s\right)$	Position vector of material frames w.r.t. the reference frame
${\mathbf{R}}_i\left(s\right)$	Rotation matrix of the i-th material frame w.r.t. the reference frame
${\mathbf{u}}_i\left(s\right)$	Curvature of the i-th tube w.r.t. the i-th material frame
${\mathbf{m}}_i\left(s\right)$	Internal moment of the i-th tube w.r.t. the reference frame
${\mathbf{m}}^b\left(s\right)$	Total internal moment w.r.t. the first material frame
${\theta }_i\left(s\right)$	Rotation angle from the first tube to the i-th tube
$\mathbf{F},\mathbf{L}$	External force and moment applied to the tip of CTR.
${\mathbf{e}}_3$	Unit vector of z-axis: ${\left[001\right]}^T$
$E,G$	Young’s Modulus and Shear Modulus
$I_i$	Second moment of area of the cross-section of the i-th tube
$J_i$	Polar moment of inertia of the cross-section of the i-th tube
${\partial }_x,{\partial }_{x,y}^2$	Concise notations of $\frac{\partial }{\partial x}$ and $\frac{{\partial }^2}{\partial x\partial y}$
$(\cdot {)}^{\wedge }$	Mapping of a vector in ${\mathbb{R}}^3$ and ${\mathbb{R}}^6$ to the corresponding element in $\mathfrak{s}\mathfrak{o}(3)$ and $\mathfrak{s}\mathfrak{e}(3)$, respectively.
$(\cdot {)}^{\mathrm{\vee }}$	Inverse operation of $(\cdot {)}^{\wedge }$
$(\cdot {)}_{xy}$	${\mathbb{R}}^3$ to ${\mathbb{R}}^2$, extraction of the first two dimension
$(\cdot {)}^{\prime }$	Derivative w.r.t. arc-length parameter $s$
$\otimes$	Tensor product that contracts the last dimension of a tensor and the first dimension of a matrix, e.g. $\mathbf{W}=\mathbf{U}\otimes \mathbf{V}$, if then ${\mathbf{W}}_{i,j,m}={\sum }_k{\mathbf{U}}_{i,j,k}\cdot {\mathbf{V}}_{k,m}$

As shown in Fig. 1, the shape of a CTR is described as a differentiable spatial curve parameterized by its arc length $s$. A material frame is assigned to each tube such that the origin of the frame moves along the curve at $s$, and the z-axis of the frame aligns with the tangent of the curve. Assuming that all tubes conform to the same curve, the position of all material frames w.r.t. the fixed reference frame is given by $\mathbf{p}\left(s\right)$ and the orientation of the i-th tube is given by ${\mathbf{R}}_i\left(s\right)$. To simplify notation, we use ${\mathbf{R}}_1\left(s\right)=\mathbf{R}\left(s\right)$. It then follows that ${\mathbf{R}}_i\left(s\right)=\mathbf{R}\left(s\right){\mathbf{R}}_z\left({\theta }_i\left(s\right)\right)$, where ${\mathbf{R}}_z\left(\theta \right)$ denotes the rotation around z-axis for angle $\theta$. The curvature of the i-th tube at $s$ represents the rate of change of ${\mathbf{R}}_i\left(s\right)$ w.r.t. $s$, ${\mathbf{u}}_i(s)={\left({\mathbf{R}}_i(s{)}^T{\mathbf{R}}_i^{\mathrm{\text{'}}}(s)\right)}^{\vee }$.

Fig. 1.

A CTR with three precurved tubes. The base rotation, translation, and exposed length of i-th tube are denoted as ${\alpha }_i$, ${\beta }_i$, and ${\gamma }_i$, respectively.

Consider an $n$-tube CTR with wrench $\mathbf{w}={\left[{\mathbf{F}}^T{\mathbf{L}}^T\right]}^T$ applied to the tip. The tubes are assumed to have planar precurvature ${\mathbf{u}}_i^{\mathrm{*}}(s)={\left[{\kappa }_i,0,0\right]}^T$. We also neglect the shear and extension in the tubes and friction between the tubes, which is widely adopted in the literature [2], [3]. Cosserat rod model of CTR describes the evolution of the curve and internal moment by a system of ODEs consisting of geometric constraints, moment equilibrium, and linear constitutive laws as follows:

$${\mathbf{p}}^{\prime }={\mathbf{R}\mathbf{e}}_3,{\mathbf{R}}^{\prime }=\mathbf{R}{\stackrel{\mathbf{ˆ}}{\mathbf{u}}}_1$$ (1a)

$${\theta }_i^{\prime }=u_{iz}-u_{1z},i=2\dots n$$ (1b)

$$u_{iz}^{\prime }=-\frac{{\kappa }_{i}E{I}_i}{G{J}_i}{u}_{iy},i=1\dots n$$ (1c)

$${\mathbf{m}}_{xy}^{b^{\prime }}={\left(-{\stackrel{\mathbf{ˆ}}{\mathbf{u}}}_1{\mathbf{m}}^b-{\stackrel{\mathbf{ˆ}}{\mathbf{e}}}_3{\mathbf{R}}^T\mathbf{F}\right)}_{xy},$$ (1d)

The unknown variables on the right-hand side are given by

$$\begin{gathered} m_z^b=G\sum _{i=1}^n{J}_i{u}_{iz},u_{iy}=\left[\begin{array}{ll}-\text{sin}{\theta }_i\hfill & \text{cos}{\theta }_i\hfill \end{array}\right]{\mathbf{u}}_{1xy} \\ {\mathbf{u}}_{1xy}=\frac{1}{({\sum }_{i=1}^{n}E{I}_i)}({\mathbf{m}}_{xy}^b+\sum _{i=1}^n{\left[\begin{array}{ll}\text{cos}{\theta }_i\hfill & \text{sin}{\theta }_i\hfill \end{array}\right]}^{T}E{I}_i{\kappa }_i) \end{gathered}$$ (2)

We can write (1) in a compact form:

$${\mathbf{g}}^{\prime }(s)={\left[\begin{array}{cc}\mathbf{R}(s)& \mathbf{p}(s)\\ 0_{1\times 3}& 1\end{array}\right]}^{\prime }=\mathbf{g}\hat{\zeta }(\mathbf{y})$$ (3a)

$${\mathbf{y}}^{\prime }=\mathbf{f}(s,\mathbf{y},\mathbf{R},\mathbf{w})$$ (3b)

where $\zeta ={\left[{\mathbf{v}}^T{\mathbf{u}}^T\right]}^T$ is the body twist of the material frame w.r.t. $s$, and $\mathbf{y}={\left[{\theta }_2\dots {\theta }_n{u}_{1z}\dots u_{nz}{m}_x^b{m}_y^b\right]}^T$. The above ODE is constrained at the robot base and the end of each tube. The initial conditions at the robot base are given by geometric constraints determined by $\mathbf{q}$ and $u_{iz}$:

$$\mathbf{p}(0)=[0,0,0{]}^T,\mathbf{R}(0)={\mathbf{R}}_z\left({\alpha }_1-{\beta }_1{u}_{1z}(0)\right)$$ (4a)

$${\theta }_i(0)={\alpha }_i-{\alpha }_1-\left({\beta }_i{u}_{iz}(0)-{\beta }_1{u}_{1z}(0)\right)$$ (4b)

The boundary constraints come from the moment equilibrium at the end of each tube:

$$\begin{gathered} 0=\mathbf{b}(\mathbf{x})=\left[G{J}_1{u}_{1z}\left(s_{e,1}\right)-{\mathbf{e}}_3^T{\mathbf{R}}^T\mathbf{L}\right., \\ {u}_{2z}\left(s_{e,2}\right),\dots ,u_{nz}\left(s_{e,n}\right), \\ {\left.{\mathbf{m}}_{xy}^b{\left(s_{e,1}\right)}^T-{\left({\mathbf{R}}^T\mathbf{L}\right)}_{xy}^T\right]}^T. \end{gathered}$$ (5)

where the vector $\mathbf{x}={\left[{\mathbf{q}}^T{\mathbf{w}}^T{\mathbf{y}}_u(0{)}^T\right]}^T$ contains the independent system inputs $\mathbf{q}$ and $\mathbf{w}$, as well as the unknown initial variables ${\mathbf{y}}_u(0)={\left[u_{1z}(0)\dots u_{nz}(0)m_x^b(0)m_y^b(0)\right]}^T$. Equations (3)–(5) form a BVP that can be solved using shooting method, which uses nonlinear root-finding algorithms to iteratively search for the ${\mathbf{y}}_u(0)$ that satisfies $\mathbf{b}(\mathbf{x})=0$. In each iteration, $\mathbf{b}(\mathbf{x})$ is obtained by solving an initial value problem (IVP) consisting ${\mathbf{y}}_u(0)$ together with (3)–(4).

B. Manipulability Analysis

To characterize the robot versatility of moving in the task space, the notion of velocity manipulability ellipsoid (VME) is proposed in [12]. It is defined as

$$\mathrm{V}\mathrm{M}\mathrm{E}:=\{\xi \mid \xi =\mathbf{J}\dot{\mathbf{q}},\parallel \dot{\mathbf{q}}\parallel =1\}$$ (6)

where $\mathbf{J}=\left[{\left(\left({\mathrm{d}}_{q_1}\mathbf{g}\right){\mathbf{g}}^{-1}\right)}^{\vee }\dots {\left(\left({\mathrm{d}}_{q_n}\mathbf{g}\right){\mathbf{g}}^{-1}\right)}^{\vee }\right]$ is the spatial Jacobian that maps a unit sphere of joint space velocity to the ellipsoid of task space velocity. The velocity manipulability index (VMI) ${\mu }_v$ is then defined to be the volume of the VME:

$${\mu }_v=\sqrt{\left|{\mathbf{J}\mathbf{J}}^T\right|}$$ (7)

where $|\cdot |$ denotes the matrix determinant. Similarly, the compliance manipulability ellipsoid (CME) is defined as

$$\mathrm{C}\mathrm{M}\mathrm{E}:=\{\xi \mid \xi =\mathbf{C}\dot{\mathbf{w}},\parallel \dot{\mathbf{w}}\parallel =1\}$$ (8)

where $\mathbf{C}=\left[{\left(\left({\mathrm{d}}_{w_1}\mathbf{g}\right){\mathbf{g}}^{-1}\right)}^{\vee }\dots {\left(\left({\mathrm{d}}_{w_6}\mathbf{g}\right){\mathbf{g}}^{-1}\right)}^{\vee }\right]$ is the compliance matrix of the robot. And the compliance manipulability index (CMI) ${\mu }_c$ is defined as:

$${\mu }_c=\sqrt{\left|{\mathbf{C}\mathbf{C}}^T\right|}$$ (9)

Note that VMI and CMI are functions of $\mathbf{J}$ and $\mathbf{C}$, respectively. To optimize the manipulability using redundancy resolution, we need to calculate the gradients of $\mathbf{J}$ and $\mathbf{C}$ w.r.t. $\mathbf{q}$, i.e. the Hessians. However, for the Cosserat rod models, a closed-form expression of $\mathbf{J}$ and $\mathbf{C}$ are usually not available. A feasible yet computationally expensive way to compute their gradient is using finite difference (FD). To reduce the computational cost, we propose an efficient method for calculating the Hessian below.

III. Derivative Propagation for the Hessian

Our calculation of the Hessian adopts the idea of derivative propagation, which essentially combines the propagation of system state variables together with their derivatives into a new system of ODEs. In [7], an augmented IVP was defined which, in addition to (1), included the propagation of first-order derivatives of state variables along the arc length, to efficiently compute the Jacobian of the CTR. We extend this derivative propagation technique to second-order derivatives, allowing the calculation of the Hessian by solving a single IVP after solving the BVP for ${\mathbf{y}}_u(0)$, which facilitates the manipulability optimization for redundancy resolution.

We first find the formulation for the Jacobian and compliance matrices. The changes in actuation variables $\mathbf{q}$ and external wrench $\mathbf{w}$ contribute to the spatial twist $\xi ={\left(\dot{\mathbf{g}}{\mathbf{g}}^{-1}\right)}^{\vee }$, where $\dot{\mathbf{g}}$ denotes the time derivative of $\mathbf{g}$:

$$\xi =\mathbf{J}\dot{\mathbf{q}}+\mathbf{C}\dot{\mathbf{w}}$$ (10)

Now, consider $\mathbf{g}(s)$ and $\mathbf{b}$ as the solution to the IVP formed by (3)–(4). Since they are fully determined by $\mathbf{x}$, their total derivatives consist only of their partial derivatives w.r.t. each component of $\mathbf{x}$. For $\mathbf{g}(s)$, since it stays on $SE(3)$, we consider the spatial twists given by [16]:

$$\mathbf{E}=\left[{\mathbf{E}}_{\mathbf{q}}{\mathbf{E}}_{\mathbf{w}}{\mathbf{E}}_{\mathbf{u}}\right]=\left[{\left(\left({\partial }_{x_1}\mathbf{g}\right){\mathbf{g}}^{-1}\right)}^{\vee }\dots {\left(\left({\partial }_{x_N}\mathbf{g}\right){\mathbf{g}}^{-1}\right)}^{\vee }\right]$$ (11)

And we can obtain $\xi$ for the $\mathbf{g}(s)$ as an IVP solution by using the chain rule:

$$\xi ={\mathbf{E}}_{\mathbf{q}}\dot{\mathbf{q}}+{\mathbf{E}}_{\mathbf{w}}\dot{\mathbf{w}}+{\mathbf{E}}_{\mathbf{u}}{\dot{\mathbf{y}}}_u(0)$$ (12)

The partial derivatives of $\mathbf{b}$ are given by

$$\mathbf{B}=\left[{\mathbf{B}}_{\mathbf{q}}{\mathbf{B}}_{\mathbf{w}}{\mathbf{B}}_{\mathbf{u}}\right]=\left[{\partial }_{x_1}\mathbf{b}\dots {\partial }_{x_N}\mathbf{b}\right]$$ (13)

Observe that, for a real system, $\mathbf{g}(s)$ should always remain as a solution to the BVP (3)–(5) while varying with $\mathbf{q}$ and $\mathbf{w}$. This requires ${\mathbf{y}}_u(0)$ to vary in a way that it compensates the variations in $\mathbf{q}$ and $\mathbf{w}$, such that $\mathbf{b}(\mathbf{x})=0$ always holds. This constraint is obtained by taking the time derivative of (5):

$$0=\dot{\mathbf{b}}={\mathbf{B}}_{\mathbf{q}}\dot{\mathbf{q}}+{\mathbf{B}}_{\mathbf{w}}\dot{\mathbf{w}}+{\mathbf{B}}_{\mathbf{u}}{\dot{\mathbf{y}}}_u(0)$$ (14)

Using (14) to eliminate the ${\dot{\mathbf{y}}}_u(0)$ in (12) results in the expression of (10) by partial derivatives:

$$\xi =\left({\mathbf{E}}_{\mathbf{q}}-{\mathbf{E}}_{\mathbf{u}}{\mathbf{B}}_{\mathbf{u}}^{†}{\mathbf{B}}_{\mathbf{q}}\right)\dot{\mathbf{q}}+\left({\mathbf{E}}_{\mathbf{w}}-{\mathbf{E}}_{\mathbf{u}}{\mathbf{B}}_{\mathbf{u}}^{†}{\mathbf{B}}_{\mathbf{w}}\right)\dot{\mathbf{w}}$$ (15)

from which we obtain the Jacobian and compliance matrices:

$$\mathbf{J}={\mathbf{E}}_{\mathbf{q}}-{\mathbf{E}}_{\mathbf{u}}{\mathbf{B}}_{\mathbf{u}}^{†}{\mathbf{B}}_{\mathbf{q}},\mathbf{C}={\mathbf{E}}_{\mathbf{w}}-{\mathbf{E}}_{\mathbf{u}}{\mathbf{B}}_{\mathbf{u}}^{†}{\mathbf{B}}_{\mathbf{w}}$$ (16)

where ${\mathbf{B}}_{\mathbf{u}}^{†}$ is the pseudo-inverse of ${\mathbf{B}}_{\mathbf{u}}$.

For gradient-based redundancy resolution, we calculate the derivatives of Jacobian and compliance matrix w.r.t. $\mathbf{q}$, i.e. the Hessians, using the same technique. Treating $\mathbf{J}$ and $\mathbf{C}$ as functions of solutions to the IVP and taking the time derivatives yield:

$$\dot{\mathbf{J}}={\partial }_{\mathbf{q}}\mathbf{J}\otimes \dot{\mathbf{q}}+{\partial }_{\mathbf{w}}\mathbf{J}\otimes \dot{\mathbf{w}}+{\partial }_{\mathbf{u}}\mathbf{J}\otimes {\dot{\mathbf{y}}}_u(0)$$

$$\dot{\mathbf{C}}={\partial }_{\mathbf{q}}\mathbf{C}\otimes \dot{\mathbf{q}}+{\partial }_{\mathbf{w}}\mathbf{C}\otimes \dot{\mathbf{w}}+{\partial }_{\mathbf{u}}\mathbf{C}\otimes {\dot{\mathbf{y}}}_u(0)$$

Eliminating the ${\dot{\mathbf{y}}}_u(0)$ using (14), the derivatives of $\mathbf{J}$ and $\mathbf{C}$ that satisfy the BVP are given by:

$$\dot{\mathbf{J}}={\mathrm{d}}_{\mathbf{q}}\mathbf{J}\otimes \dot{\mathbf{q}}+{\mathrm{d}}_{\mathbf{w}}\mathbf{J}\otimes \dot{\mathbf{w}},\dot{\mathbf{C}}={\mathrm{d}}_{\mathbf{q}}\mathbf{C}\otimes \dot{\mathbf{q}}+{\mathrm{d}}_{\mathbf{w}}\mathbf{C}\otimes \dot{\mathbf{w}}$$ (17)

where the Hessians used for redundancy resolution are

$${\mathrm{d}}_{\mathbf{q}}\mathbf{J}={\partial }_{\mathbf{q}}\mathbf{J}-{\partial }_{\mathbf{u}}\mathbf{J}\otimes \left({\mathbf{B}}_{\mathbf{u}}^{†}{\mathbf{B}}_{\mathbf{q}}\right),{\mathrm{d}}_{\mathbf{q}}\mathbf{C}={\partial }_{\mathbf{q}}\mathbf{C}-{\partial }_{\mathbf{u}}\mathbf{C}\otimes \left({\mathbf{B}}_{\mathbf{u}}^{†}{\mathbf{B}}_{\mathbf{q}}\right)$$ (18)

To obtain the partial derivatives in the above equations, we further define the derivatives of $\mathbf{E}$ and $\mathbf{B}$ as $\mathbf{D}={\partial }_{\mathbf{X}}\mathbf{E}$ and $\mathbf{A}={\partial }_{\mathbf{x}}\mathbf{B}$, and take the derivatives of (16) w.r.t. $\mathbf{x}$ :

$$\begin{gathered} {\partial }_{x_{\mathrm{r}}}\mathbf{J}={\mathbf{D}}_{\mathbf{q},\mathrm{r}}-{\mathbf{D}}_{\mathbf{u},\mathrm{r}}{\mathbf{B}}_{\mathbf{u}}^{†}{\mathbf{B}}_{\mathbf{q}}-{\mathbf{E}}_{\mathbf{u}}{\mathbf{B}}_{\mathbf{u}}^{†}{\mathbf{A}}_{\mathbf{u},\mathrm{r}}{\mathbf{B}}_{\mathbf{u}}^{†}{\mathbf{B}}_{\mathbf{q}}-{\mathbf{E}}_{\mathbf{u}}{\mathbf{B}}_{\mathbf{u}}^{†}{\mathbf{A}}_{\mathbf{q},\mathrm{r}} \\ {\partial }_{x_{\mathrm{r}}}\mathbf{C}={\mathbf{D}}_{\mathbf{w},\mathrm{r}}-{\mathbf{D}}_{\mathbf{u},\mathrm{r}}{\mathbf{B}}_{\mathbf{u}}^{†}{\mathbf{B}}_{\mathbf{w}}-{\mathbf{E}}_{\mathbf{u}}{\mathbf{B}}_{\mathbf{u}}^{†}{\mathbf{A}}_{\mathbf{u},\mathrm{r}}{\mathbf{B}}_{\mathbf{u}}^{†}{\mathbf{B}}_{\mathbf{w}}-{\mathbf{E}}_{\mathbf{u}}{\mathbf{B}}_{\mathbf{u}}^{†}{\mathbf{A}}_{\mathbf{w},\mathrm{r}} \end{gathered}$$ (19)

where the r-th page of tensors $\mathbf{D}$ and $\mathbf{A}$ are denoted as

$$\begin{gathered} {\mathbf{D}}_{\mathrm{r}}=\left[{\mathbf{D}}_{\mathbf{q},\mathrm{r}}{\mathbf{D}}_{\mathbf{w},\mathrm{r}}{\mathbf{D}}_{\mathbf{u},\mathrm{r}}\right]=\left[{\partial }_{x_{\mathrm{r}}}{\mathbf{E}}_{\mathbf{q}}{\partial }_{x_{\mathrm{r}}}{\mathbf{E}}_{\mathbf{w}}{\partial }_{x_{\mathrm{r}}}{\mathbf{E}}_{\mathbf{u}}\right] \\ {\mathbf{A}}_{\mathrm{r}}=\left[{\mathbf{A}}_{\mathbf{q},\mathrm{r}}{\mathbf{A}}_{\mathbf{w},\mathrm{r}}{\mathbf{A}}_{\mathbf{u},\mathrm{r}}\right]=\left[{\partial }_{x_{\mathrm{r}}}{\mathbf{B}}_{\mathbf{q}}{\partial }_{x_{\mathrm{r}}}{\mathbf{B}}_{\mathbf{w}}{\partial }_{x_{\mathrm{r}}}{\mathbf{B}}_{\mathbf{u}}\right] \end{gathered}$$ (20)

We can observe from (16), (18) and (19) that, calculating the Jacobian and compliance matrices and the corresponding Hessians requires calculating $\mathbf{B}$, $\mathbf{E}$, $\mathbf{A}$, and $\mathbf{D}$. While $\mathbf{E}$ and $\mathbf{D}$ are derivatives of $\mathbf{g}(s)$ and can be obtained from initial conditions since they exist along the robot length, $\mathbf{B}$ and $\mathbf{A}$ are evaluated only at $s_{e,i}$ and cannot propagate with $s$. However, note from (5) that $\mathbf{b}$ is a function of $\mathbf{y}\left(s_{e,i}\right)$ and $\mathbf{g}\left(s_{e,i}\right)$, hence we can obtain $\mathbf{B}$ and $\mathbf{A}$ by propagating the derivatives of $\mathbf{y}(s)$ and $\mathbf{g}(s)$ w.r.t. $\mathbf{x}$. Denote the first- and second-order derivatives of the state vector $\mathbf{y}$ w.r.t. $\mathbf{x}$ as:

$$\mathbf{V}={\partial }_{\mathbf{x}}\mathbf{y},\mathbf{U}={\partial }_{\mathbf{x},\mathbf{x}}^2\mathbf{y}$$

Then $\mathbf{B}$ and $\mathbf{A}$ can be obtained by taking derivatives of $\mathbf{b}(\mathbf{x})$ and plugging in values of $\mathbf{V}$ and $\mathbf{U}$ at $s_{e,i}$. Note that the first-order partial derivatives $\mathbf{E}$, $\mathbf{V}$ and the second-order partial derivatives $\mathbf{D}$, $\mathbf{U}$ are themselves functions of $s$, they can be calculated by integrating along the arc length through a new set of ODEs. Since $\mathbf{g}$ and $\mathbf{y}$ are piecewise continously differentiable, we have the relationship ${\mathbf{V}}^{\prime }={\left({\partial }_{\mathbf{x}}\mathbf{y}\right)}^{\prime }={\partial }_{\mathbf{x}}\left({\mathbf{y}}^{\prime }\right)$. Hence the k-th column of matrices ${\mathbf{E}}^{\prime }$ and ${\mathbf{V}}^{\prime }$ are given by:

$$\begin{gathered} {\mathbf{E}}_k^{\prime }={\left({\partial }_{x_k}\left({\mathbf{g}}^{\prime }\right)\cdot {\mathbf{g}}^{-1}+{\partial }_{x_k}\mathbf{g}\cdot {\left({\mathbf{g}}^{-1}\right)}^{\prime }\right)}^{\vee }={\left(\mathbf{g}\cdot {\partial }_{x_k}\hat{\zeta }\cdot {\mathbf{g}}^{-1}\right)}^{\vee } \\ {\mathbf{V}}_k^{\prime }={\partial }_{\mathbf{y}}\mathbf{f}\cdot {\mathbf{V}}_k+{\partial }_{x_k}\mathbf{f}+{\partial }_{\mathrm{v}\mathrm{e}\mathrm{c}(\mathbf{R})}\mathbf{f}\cdot \mathrm{v}\mathrm{e}\mathrm{c}\left({\partial }_{x_k}\mathbf{R}\right) \end{gathered}$$ (21)

where the partial derivative of $\zeta$ and $\mathbf{R}$ can be obtained by ${\partial }_{x_k}\zeta ={\partial }_{\mathbf{y}}\zeta \cdot {\mathbf{V}}_k$, ${\partial }_{x_k}\mathbf{R}={\left(\left[0_{3\times 3}{\mathbf{I}}_{3\times 3}\right]{\mathbf{E}}_k\right)}^{\vee }\mathbf{R}$, and $\mathrm{v}\mathrm{e}\mathrm{c}()$ reshapes a matrix into a column vector. Denoting (21) as ${\left[{\left({\mathbf{E}}^{\prime }\right)}^T{\left({\mathbf{V}}^{\prime }\right)}^T\right]}^T={\mathbf{f}}^1(s,\mathbf{y},\mathbf{R},\mathbf{w},\mathbf{E},\mathbf{V})$, the ODEs for the second-order derivatives are derived by taking the derivative of ${\mathbf{f}}^1$ w.r.t. $\mathbf{x}$. The r-th page of tensors ${\mathbf{D}}^{\prime }$, ${\mathbf{U}}^{\prime }$ is given by:

$$\begin{gathered} {\left[\begin{array}{l}{\mathbf{D}}_{\mathrm{r}}\\ {\mathbf{U}}_{\mathrm{r}}\end{array}\right]}^{\prime }={\partial }_{\mathrm{y}}{\mathbf{f}}^1\cdot {\mathbf{V}}_r+{\partial }_{x_r}{\mathbf{f}}^1+{\partial }_{\mathrm{v}\mathrm{e}\mathrm{c}(\mathbf{R})}{\mathbf{f}}^1\cdot \mathrm{v}\mathrm{e}\mathrm{c}\left({\partial }_{x_k}\mathbf{R}\right) \\ +{\partial }_{\mathrm{v}\mathrm{e}\mathrm{c}(\mathbf{V})}{\mathbf{f}}^1\cdot \mathrm{v}\mathrm{e}\mathrm{c}\left({\mathbf{U}}_{\mathrm{r}}\right)+{\partial }_{\mathrm{v}\mathrm{e}\mathrm{c}(\mathbf{E})}{\mathbf{f}}^1\cdot \mathrm{v}\mathrm{e}\mathrm{c}\left({\mathbf{D}}_{\mathrm{r}}\right) \end{gathered}$$ (22)

Combining (21) and (22) with (1) gives an augmented system of ODEs:

$${\mathbf{g}}^{\prime }(s)=\mathbf{g}\hat{\zeta }\left(\mathbf{y}\right)$$ (23a)

$${\mathbf{y}}^{\prime }(s)=\mathbf{f}\left(s,\mathbf{y},\mathbf{R},\mathbf{w}\right)$$ (23b)

$${\left[\begin{array}{l}\mathbf{E}(s)\\ \mathbf{V}(s)\end{array}\right]}^{\prime }={\mathbf{f}}^1\left(s,\mathbf{y},\mathbf{R},\mathbf{w},\mathbf{E},\mathbf{V}\right)$$ (23c)

$${\left[\begin{array}{l}\mathbf{D}(s)\\ \mathbf{U}(s)\end{array}\right]}^{\prime }={\mathbf{f}}^2\left(s,\mathbf{y},\mathbf{R},\mathbf{w},\mathbf{E},\mathbf{V},\mathbf{D},\mathbf{U}\right)$$ (23d)

where (23d) is the concise form of (22). Using initial values calculated by taking the first- and second-order derivatives of (4) w.r.t. $\mathbf{x}$, (23) can be solved as an IVP.

Note that at the end of each tube or at positions where the precurvature or stiffness of tubes has discontinuity, partial derivatives $\mathbf{V}$, $\mathbf{E}$, $\mathbf{U}$, $\mathbf{D}$ are discontinuous since they are continuous functions of stiffness and precurvature. Their transition functions at these positions can be obtained by taking the derivatives of the transition functions of $\mathbf{y}$ and $\mathbf{g}$. The arc length when these discontinuities appear is denoted as $s_i$, representing either $s_{e,i}$ or $s_{t,i}$. Transitions of $\mathbf{y}$ and $\mathbf{g}$ are given by:

$${\left.{\mathbf{y}}^{+}(s,\mathbf{x})\right|}_{s=s_i}={\left.\mathbf{h}\left({\mathbf{y}}^{-}(s,\mathbf{x})\right)\right|}_{s=s_i}$$ (24a)

$${\left.{\mathbf{g}}^{+}(s,\mathbf{x})\right|}_{s=s_i}={\left.{\mathbf{g}}^{-}(s,\mathbf{x})\right|}_{s=s_i}$$ (24b)

where − and + denote the state vector immediately before and after the transition point, respectively, and $\mathbf{h}()$ is the transition function of $\mathbf{y}$ that enforces boundary conditions (5) and static equilibrium at transition points $s_i$. Define the elements in vector $\mathbf{x}$ excluding ${\beta }_i$ as ${\varphi }_j\in \mathbf{x}\backslash {\beta }_i$. To take the first- and second-order derivatives of (24a) over ${\beta }_i$ and ${\varphi }_j$, we note that $s_i$ is a linear function of ${\beta }_i$, hence ${\left.{\mathrm{d}}_{{\beta }_i}\mathbf{y}\right|}_{s=s_{e,i}}={\partial }_{{\beta }_i}\mathbf{y}+{\partial }_s\mathbf{y}\cdot {\mathrm{d}}_{{\beta }_i}{s}_{e,i}={\partial }_{{\beta }_i}\mathbf{y}+{\partial }_s\mathbf{y}$ and we have

$$\begin{gathered} {\partial }_{{\varphi }_j}{\mathbf{y}}^{+}={\partial }_{\mathbf{y}}\mathbf{h}\left({\partial }_{{\varphi }_j}{\mathbf{y}}^{-}\right), \\ {\partial }_{{\beta }_i}{\mathbf{y}}^{+}={\partial }_{\mathbf{y}}\mathbf{h}\left({\partial }_{{\beta }_i}{\mathbf{y}}^{-}+{\partial }_s{\mathbf{y}}^{-}\right)-{\partial }_s{\mathbf{y}}^{+}, \\ {\partial }_{{\beta }_i,{\varphi }_j}^2{\mathbf{y}}^{+}={\partial }_{\mathbf{y},\mathbf{y}}^2\mathbf{h}\left({\partial }_{{\beta }_i,{\varphi }_j}^2{\mathbf{y}}^{-}+{\partial }_{s,{\varphi }_j}^2{\mathbf{y}}^{-}\right)-{\partial }_{s,{\varphi }_j}^2{\mathbf{y}}^{+}, \\ {\partial }_{{\beta }_i,{\beta }_i}^2{\mathbf{y}}^{+}={\partial }_{\mathbf{y},\mathbf{y}}^2\mathbf{h}\left({\partial }_{{\beta }_i,{\beta }_i}^2{\mathbf{y}}^{-}+2{\partial }_{s,{\beta }_i}^2{\mathbf{y}}^{-}+{\partial }_{s,s}^2{\mathbf{y}}^{-}\right) \\ -2{\partial }_{s,{\beta }_i}^2{\mathbf{y}}^{+}-{\partial }_{s,s}^2{\mathbf{y}}^{+} \end{gathered}$$ (25)

The transition conditions for derivatives of $\mathbf{g}$ take a similar form. When the forward integration of (23) passes through transition points, the above conditions are used to properly transition the augmented state variables in (23).

By solving the IVP (23), we obtain $\mathbf{V},\mathbf{E},\mathbf{U},\mathbf{D}$, from which we can calculate $\mathbf{B}$, $\mathbf{A}$. Then $\mathbf{B},\mathbf{A},\mathbf{E},\mathbf{D}$ are plugged into (16), (19), and (18) to obtain $\mathbf{J}$, $\mathbf{C}$ and their gradients. While the FD method requires solving several BVPs or IVPs, the derivative propagation method only needs to solve an augmented IVP, therefore reduces the computational load. The overall procedure of calculating the Hessians is summarized in Fig. 2.

Fig. 2.

Flow chart of steps for calculating the Hessians via derivative propagation.

IV. Task-Specific Redundancy Resolution

In this section, we present the redundancy resolution scheme to regulate the robot configuration for effective path tracking. We incorporate performance index optimization in the redundancy resolution for tasks including singularity avoidance, obstacle avoidance, and tracking under external force. The proposed redundancy resolution can be used as low-level building blocks for a high-level task and motion planner.

A. Path Tracking and Joint Limit Avoidance

The primary task in all scenarios we consider is tracking a desired trajectory designated by either teleoperation or a motion planner, while avoiding joint limits. Consider points of interest on the robot where desired twists ${\xi }_{j,d}$, $j=1,\dots ,m$ are designated. The primary task can be formulated as:

$$\underset{\dot{\mathbf{q}}}{\mathrm{m}\mathrm{i}\mathrm{n}}{\dot{\mathbf{q}}}^T\mathbf{W}\left(\mathbf{q}\right)\dot{\mathbf{q}}$$ (26a)

$$\text{s.t.}{\mathbf{J}}_j\dot{\mathbf{q}}={\xi }_{j,d},j=1,\dots ,m$$ (26b)

where ${\mathbf{J}}_j$ is the Jacobian of the jth point of interest, $\mathbf{W}$ is an adaptive positive definite weight matrix.

The cost function (26a) is designed to penalize the velocity that drives joints to their limits. The joint limits of CTR consist of limits on the exposed lengths ${\gamma }_i$ (Fig.1), which prevent withdrawing the inner tube entirely into the outer tube $\left({\gamma }_i>0\right)$ or extending the inner tube too much $\left({\beta }_i<{\beta }_{i+1}\right)$. Hence the weight matrix is defined as $\mathbf{W}(\mathbf{q})=\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left(\left[1,w_1,1,w_2,1,w_3\right]\right)$ so that only ${\beta }_i$ is regulated. To penalize the joint velocity that drives the exposed length ${\gamma }_i$ to its limit, the adaptive weight is defined as [17]:

$$w_i=1+\left|\frac{{\left({\gamma }_{i,max}-{\gamma }_{i,min}\right)}^2\left({\gamma }_i-{\bar{\gamma }}_i\right)}{2{\left({\gamma }_{i,max}-{\gamma }_i\right)}^2{\left({\gamma }_i-{\gamma }_{i,min}\right)}^2}\right|$$ (27)

where ${\gamma }_i\in \left[{\gamma }_{i,min},{\gamma }_{i,max}\right]$, and ${\bar{\gamma }}_i=\left({\gamma }_{i,min}+{\gamma }_{i,max}\right)/2$. When ${\gamma }_i$ approaches the limits, the weight $w_i$ approaches infinity and penalizes ${\dot{\gamma }}_i$ towards 0.

To solve (26), define the augmented Jacobian [18] as $\mathbf{J}={\left[{\mathbf{J}}_1^T,\dots ,{\mathbf{J}}_m^T\right]}^T$, and the augmented desired twist ${\xi }_d={\left[{\xi }_{1,d}^T,\dots ,{\xi }_{m,d}^T\right]}^T$. The transformation ${\mathbf{J}}_w={\mathbf{J}\mathbf{W}}^{-\frac{1}{2}}$, ${\dot{\mathbf{q}}}_w={\mathbf{W}}^{\frac{1}{2}}\dot{\mathbf{q}}$ simplifies (26) into minimizing ${∥{\dot{\mathbf{q}}}_w∥}^2$ while satisfying ${\mathbf{J}}_w{\dot{\mathbf{q}}}_w={\xi }_d$, which has the closed-form solution

$$\dot{\mathbf{q}}={\mathbf{W}}^{-\frac{1}{2}}{\mathbf{J}}_w^{†}{\xi }_d={\mathbf{W}}^{-1}{\mathbf{J}}^T{\left({\mathbf{J}\mathbf{W}}^{-1}{\mathbf{J}}^T\right)}^{-1}{\xi }_d$$ (28)

where ${\mathbf{J}}^{†}={\mathbf{J}}^T{\left({\mathbf{J}\mathbf{J}}^T\right)}^{-1}$ for a redundant robot.

B. Task-Specific Performance Index

We consider three scenarios and derive the task-specific performance indices together with their gradient based on previous derivations.

Scenario 1:

When the robot is tracking a desired trajectory in free space, it is beneficial to maintain a relatively high VMI to avoid singular configuration. Particularly, if the desired trajectory is suddenly updated, a sufficient VMI will enable the robot to follow the new trajectory immediately. It is also reported that increasing VMI helps avoid the snapping of the CTR [14]. Here, we consider the VMI of the robot tip and incorporate the joint limits into the VMI by substituting the original Jacobian in (7) with the weighted Jacobian ${\mathbf{J}}_w$ defined earlier. Note that when ${\beta }_i$ approaches its limits, the corresponding column in ${\mathbf{J}}_w$ is penalized towards $0$, effectively reducing the manipulability generated by ${\beta }_i$. For gradient-based redundancy resolution, the analytical gradient of the VMI can be derived using Jacobi’s formula:

$${\partial }_{q_i}{\mu }_v(\mathbf{q})=\frac{1}{2}\mathrm{d}\mathrm{e}\mathrm{t}{\left({\mathbf{\Gamma }}_v\right)}^{-\frac{1}{2}}\mathrm{d}\mathrm{e}\mathrm{t}\left({\mathbf{\Gamma }}_v\right)\mathrm{t}\mathrm{r}\left({\mathbf{\Gamma }}_v^{-1}{\partial }_{q_i}{\mathbf{\Gamma }}_v\right)$$ (29)

where ${\mathbf{\Gamma }}_v={\mathbf{J}}_w{\mathbf{J}}_w^T$, $\mathrm{t}\mathrm{r}()$ represents the trace of square matrices, and the partial derivative ${\partial }_{q_i}{\mathbf{\Gamma }}_v$ :

$${\partial }_{q_i}{\mathbf{\Gamma }}_v=\left({\partial }_{q_i}\mathbf{J}\right){\mathbf{W}}^{-1}{\mathbf{J}}^T+\mathbf{J}{\mathbf{W}}^{-1}{\left({\partial }_{q_i}\mathbf{J}\right)}^T$$ (30)

can be obtained by using the Hessian from (18).

Scenario 2:

When navigating through a confined space, the CTR needs to avoid obstacles that can potentially collide with the robot body. Increasing the robot body VMI would improve the motion capability of the robot and facilitate obstacle avoidance. We assume that a map of obstacles is known, and the points of interest on CTR can be determined by task-specific criteria, such as selecting the closest point to each nearby obstacle. At each point of interest, a desired velocity that guides the robot away from obstacles can be obtained. Denote the unit vector of the desired direction as $\rho$, an oriented VMI is defined as the projection of VME along $\rho$ :

$${\mu }_{j,v}^d(\mathbf{q},\rho )={\left[{\rho }^T{\left({\mathbf{J}}_j{\mathbf{J}}_j^T\right)}^{-1}\rho \right]}^{-\frac{1}{2}}$$ (31)

A weighted whole-body VMI is then defined as:

$${\mu }_o(\mathbf{q})=\sum _{j=1}^p{c}_j{\mu }_{j,v}^d(\mathbf{q},{\rho }_j)$$ (32)

where ${\mu }_{j,v}^d$ is the oriented VMI of the j-th point of interest. The weight $c_j$ describes the relative importance of the j-th point of interest and can be determined as a function of e.g. the distance between the robot body. The gradient of ${\mu }_c^d$ is derived as:

$${\partial }_{q_i}{\mu }_{j,v}^d(\mathbf{q},\rho )=\frac{1}{2}{({\rho }^T{\mathbf{\Gamma }}_{j,v}^{-1}\rho )}^{-\frac{3}{2}}{\rho }^T{\mathbf{\Gamma }}_{j,v}^{-1}\left({\partial }_{q_i}{\mathbf{\Gamma }}_{j,v}\right){\mathbf{\Gamma }}_{j,v}^{-1}\rho$$ (33)

where ${\mathbf{\Gamma }}_{j,v}={\mathbf{J}}_j{\mathbf{J}}_j^T$, and the partial derivative ${\partial }_{q_i}{\mathbf{\Gamma }}_v$ can be obtained using the Hessian in (18).

Scenario 3:

When performing certain surgical procedures, such as forcep-based biopsy, there is a concentrated external load $\mathbf{w}$ applied to the robot tip that deforms the robot, potentially leading to undesired behavior. It is usually desirable to suppress the robot deformation while following the designated trajectory. This can be achieved by minimizing the compliance in the direction of the external load. Similar to the definition of the oriented VMI, we denote the unit vector along the direction of the tip load as $v$, and the compliance in this direction is obtained by the projection of CME along $v$ :

$${\mu }_c^d(\mathbf{q},v)={[v^T{\left({\mathbf{C}\mathbf{C}}^T\right)}^{-1}v]}^{-\frac{1}{2}}$$ (34)

Its gradient ${\partial }_{q_i}{\mu }_c^d(\mathbf{q},v)$ can be obtained similar to (33).

C. Redundancy Resolution with Task-Specific Gradient Projection (TSGP)

The redundancy resolution is conducted such that the path tracking and joint limit avoidance (28) is firstly satisfied, then the gradient-based optimization of the task-specific performance index is performed using the remaining DoFs. This is achieved by projecting the gradient of the performance index $\mu$ into the null-space of ${\mathbf{J}}_w$:

$${\dot{\mathbf{q}}}_w={\mathbf{J}}_w^{†}{\xi }_d+\alpha \left(\mathbf{I}-{\mathbf{J}}_w^{†}{\mathbf{J}}_w\right){\nabla }_{{\mathbf{q}}_w}\mu$$ (35)

where $\alpha$ is a scalar parameter. A positive $\alpha$ would increase $\mu$, and a negative $\alpha$ would decrease it. As mentioned in [17], choosing a suitable gain $\alpha$ across the whole workspace is critical for TSGP. In this paper, we adopt the method in our recent work [19] to find a suitable $\alpha$ that balances the desired velocity and null-space projection for manipulability optimization. The final instantaneous joint velocity of the TSGP controller is obtained by transforming ${\mathbf{q}}_w$ back to $\mathbf{q}$:

$$\begin{gathered} \dot{\mathbf{q}}={\mathbf{W}}^{-1}{\mathbf{J}}^T{\left(\mathbf{J}{\mathbf{W}}^{-1}{\mathbf{J}}^T\right)}^{-1}{\xi }_d \\ +\alpha (\mathbf{I}-{\mathbf{W}}^{-1}{\mathbf{J}}^T{\left(\mathbf{J}{\mathbf{W}}^{-1}{\mathbf{J}}^T\right)}^{-1}\mathbf{J}){\mathbf{W}}^{-1}{\nabla }_{\mathbf{q}}\mu \end{gathered}$$ (36)

V. Simulation Study

To evaluate the performance of the algorithms developed in Sec. III and IV, path tracking simulations were conducted using a 3-tube CTR to achieve tasks that reflected the scenarios presented in the previous section. Parameters of the CTR were given in Table II. The joint limit for each tube was given by ${\gamma }_{i,min}=10\mathrm{m}\mathrm{m}$, and ${\gamma }_{i,max}=200\mathrm{m}\mathrm{m}$. We compared the performance of the proposed TSGP controller with other widely used kinematic controllers and state-of-the-art approaches, namely:

TABLE II.

Simulation Parameters for Tubes

	Tube 1	Tube 2	Tube 3
Inner Diameter (mm)	0.640	0.953	1.400
Outer Diameter (mm)	0.840	1.270	1.600
Straight Section Length (mm)	450	250	100
Curved Section Length (mm)	150	150	100
Curvature (m−1)	20	10	5
Young’s Modulus, E (GPa)	60	60	60
Shear Modulus, G (GPa)	23.1	23.1	23.1

1. The standard resolved-rates (RR) controller given by (28).

2. A generalized damped least-square (DLS) controller.

3. The Model Predictive Control (MPC) strategy of the CTR proposed in [20].

4. The gradient projection controller that considered a unified manipulability ellipsoid (UME) only at the robot tip as in [11], [14].

The DLS controller minimized a quadratic cost function [8], [9] :

$$h(\dot{\mathbf{q}})={\left(\mathbf{J}\dot{\mathbf{q}}-{\dot{\mathbf{p}}}_d\right)}^T{\mathbf{W}}_t\left(\mathbf{J}\dot{\mathbf{q}}-{\dot{\mathbf{p}}}_d\right)+{\dot{\mathbf{q}}}^T\left({\mathbf{W}}_D+{\mathbf{W}}_J\right)\dot{\mathbf{q}}$$ (37)

where ${\mathbf{W}}_t$, ${\mathbf{W}}_D$, ${\mathbf{W}}_J$ were the weight matrix for path tracking, singularity robustness, and joint limit avoidance, respectively. The instantaneous joint velocity could be obtained by setting $\nabla h=0$ :

$$\dot{\mathbf{q}}={\left({\mathbf{J}}^T{\mathbf{W}}_t\mathbf{J}+{\mathbf{W}}_D+{\mathbf{W}}_J\right)}^{-1}{\mathbf{J}}^T{\mathbf{W}}_t{\dot{\mathbf{p}}}_d$$ (38)

In this simulation, we used the 3 × 6 Jacobian for the desired linear velocity ${\dot{\mathbf{p}}}_d$. The MPC controller had a horizon of 5 steps, and was solved using fmincon with interior point algorithm. The desired stiffness for the UME was 1.5N/mm, and the thickness of the desired ellipsoid was 0.05. The reader can refer to [11], [14], [20] for details of these methods. The gain $\alpha$ in (36) was 0.028 for UME controller, and 0.470 for TSGP controller, which were selected by a grid search with a resolution of 0.001 to generate the least path tracking error. The DLS parameters were set to: ${\mathbf{W}}_t={\mathbf{I}}_{3\times 3}$, ${\mathbf{W}}_D=0.001{\mathbf{I}}_{6\times 6}$, ${\mathbf{W}}_J=0.001\mathbf{W}(\mathbf{q})$, where $\mathbf{W}(\mathbf{q})$ was the joint limit weight defined in Sec. IV-A. All algorithms were implemented in Matlab and ran on an 8-core 2.30 GHz processor.

A. Computational Efficiency for Hessian Calculation

The computational efficiency of the derivative propagation method was evaluated by randomly sampling 10,000 configurations and calculating the corresponding Hessians. Different non-stiff ODE solvers provided by Matlab were explored for accuracy and efficiency. We compared the performance of the proposed method to that of the finite difference (FD) method, which calculated $\mathbf{D}$ and $\mathbf{U}$ by applying perturbations to $\mathbf{q}$ and calculating multiple $\mathbf{E}\mathrm{s}$ and $\mathbf{V}\mathrm{s}$, each from one IVP using (23a)–(23c). The Hessians calculated by the Dormand-Prince method (ode45 solver) using FD was used as the reference to evaluate the errors of the derivative propagation. We recorded the largest relative error among all elements compared to the reference Hessians. Since the step length of different solvers varied, we evaluated the average CPU time together with the number of calls for the forward integration of (23) for the derivative propagation method or equations (23a)–(23c) for the FD method. We did not test calculating $\mathbf{E}$ and $\mathbf{V}$ using FD since this was significantly slower.

As shown in Table III, the Hessians by the proposed method agreed with those of the FD method very well. The proposed method reduced the CPU times by 68% and the number of ODE calls (computation of (23a)–(23c) for FD or (23a)–(23d) for the proposed method) by 94% compared to the FD method. The total numbers of multiplications and additions were also reported to mitigate the influence of code implementation on the performance. The proposed method decreased the total number of multiplications by 52% and additions by 51%.

TABLE III.

Computational Efficiency for the Hessian

	FD of Jacobian			Derivative Propagation
ODE solver	ode45	ode23	ode113	ode45	ode23	ode113
Time (s)	0.498	0.320	0.293	0.159	0.133	0.125
ODE calls	1645	908	953	95	67	54
No. × (106)	3.06	1.69	1.77	1.43	1.01	0.812
No. + (106)	1.18	0.651	0.683	0.571	0.402	0.325
Error (%)	0	3.82	1.90	0.0526	1.23	0.649

In this comparison, we did not implement parallel computing. While it is straightforward to accelerate the FD by parallel computing, we note that the calculation of IVP can also be parallelized to further accelerate the derivative propagation method. In our simulation, FD using parallel computing typically resulted in 0.1s CPU time, while 3 times speedup of ODE computing for 8 threads was reported in literature [21]. Therefore, we argue that the proposed method is more efficient given the same amount of computational resources.

B. Free Space Path Tracking

For the scenario 1 in Sec. IV-B, we used a square trajectory that contains 90° turns and passes through neighborhoods of singularities. For each point along the trajectory, the controllers were given one iteration (0.5s time interval) to move the robot toward that point. This setup required the robot to maintain a relatively high VMI to avoid singularities and follow the trajectory closely at sharp turns. Thus, the TSGP tried to maximize the VMI for this scenario. The reciprocal of condition number ${\epsilon }_3/{\epsilon }_1$ of the Jacobian was used to determine if the robot was close to singularities, where ${\epsilon }_3$ and ${\epsilon }_1$ denote the minimal and maximal singular value of the Jacobian.

Fig. 3 gave the simulation results. As shown in Fig. 3-(B), TSGP was able to maintain a high reciprocal of condition number throughout the trajectory, leading to reduced tracking error. The RR controller resulted in large position errors at corners of the trajectory since the robot was close to singularities at these points and could not generate large enough velocity in the desired direction. The DLS controller better avoided singularities but at the cost of an overall higher position error, since the control law (38) effectively damped the singular values of the Jacobian and distorted it. On the other hand, although the UME and MPC controller avoided singularities and achieved small tracking errors, both methods had higher computational costs compared to TSGP. The total computation time to finish the path tracking was 22.3 s for the UME and 452.1s for the MPC, compared to 9.4s for TSGP. The UME controller was computationally expensive mainly because it calculated the gradient of the cost function by FD. For the MPC controller, the nonlinear programming for future predictions required $N_i\times N_h$ Hessian calculations for each step, where $N_i$, $N_h$ were the number of iterations and the horizon respectively. On the contrary, the forward step of TSGP in (36) took only 1 Hessian calculation, thus achieving a similar tracking error with much less computation time.

Fig. 3.

Simulation results of tracking a square trajectory in free space. (A) 3D view of the trajectories. (B) The changes of the reciprocal of condition number and position errors along the trajectory.

C. Obstacle Avoidance

Corresponding to scenario 2 in Sec. IV-B, the robot needed to achieve online obstacle avoidance while tracking a straight trajectory in this simulation. Apart from the robot tip, the point of interest was defined as the closest point on the robot curve to the obstacle. This point was updated during each iteration by performing a nearest neighbor search between the discretized robot curve and the point cloud representing the surface of the obstacle. We defined the unit vector that was aligned with the closest point-pair and pointed towards the robot as $\mathbf{k}$. Once the shortest distance was below a threshold, the following obstacle avoidance task was added to (26b):

$$v={\rho }^T{\mathbf{J}}_v\dot{\mathbf{q}},0={\mathbf{k}}^T{\mathbf{J}}_v\dot{\mathbf{q}}$$ (39)

where $\rho =\left({\mathbf{R}\mathbf{e}}_3\right)\times \mathbf{k}$, ${\mathbf{R}\mathbf{e}}_3$ was the tangent vector of the robot at the point of interest, $v$ was the magnitude of the obstacle avoiding velocity, and ${\mathbf{J}}_v$ was the linear velocity Jacobian at the point closest to the obstacle. For the 6-DoF CTR, the above task took 2 DoFs and path tracking took 3 DoFs, hence 1 DoF was left to optimize the body manipulability in (32). The task (39) was also added to the RR, DLS, and UME controllers for comparison. For the MPC, we planned a path for the point of interest (Fig. 4-(C), green dashed line) that agreed with (39).

Fig. 4.

Simulation results of obstacle avoidance. (A) Motion history of the robot using different controllers. (B) A front view of (A) showing the trajectories of the CTR shapes. (C) A top view of (A) showing the trajectories of the closest point on the CTR to the obstacle. (D) The changes of oriented VMIs along the trajectory. (E) The changes of the robot-obstacle distance along the trajectory.

As shown in Fig. 4, a cuboid obstacle was placed to block the robot. Fig. 4-(C) showed the desired velocity direction that generated a sideward motion of the robot. The TSGP was capable of increasing the VMI of the robot at the point of interest, thus generating enough sideward motion to avoid the obstacle while following the tip trajectory. However, the RR, DLS, UME, and MPC controllers failed to avoid the obstacle due to the lack of sideward motion capability, as shown in Fig. 4-(D).

D. Path Tracking under External Load

The last simulation corresponds to scenario 3 in Sec. IV-B. While tracking a straight trajectory, a constant vertical force $\mathbf{F}={\left[\begin{array}{lll}0& 0& -0.25\mathrm{N}\end{array}\right]}^T$ was applied to the tip of the CTR, and the TSGP controller minimized the compliance in the vertical direction, i.e. ${\mu }_c^d\left(\mathbf{q},-{\mathbf{e}}_3\right)$ defined in (34), to compensate for the effect of the external force. The MPC method in [20] did not consider external loads and was thus not used for comparison in this scenario.

As shown in Fig.5-(A) and (B), the position error of the TSGP controller was significantly lower than those of RR, DLS, and UME. It can be observed from Fig. 5-(A) that the robot shapes for RR and DLS at the end of the trajectory were visibly deflected by the external tip force. This corresponds to the CMEs shown in Fig. 5-(B) and the change of ${\mu }_c^d$ shown in Fig. 5-(C). As the robot moved forward, the TSGP slightly reduced the compliance in the vertical direction, which could compensate for the shape deformation and trajectory deviation induced by the external force. By contrast, both RR and DLS controllers generated large vertical compliance. The UME controller maintained a lower compliance compared to RR and DLS but was still unable to closely track the desired path. A probable reason was that, while it tried to regulate the entire UME to the desired shape, the robot lacked the DoF to achieve this (Fig. 5-(B)). The target function of the UME controller did not specifically focus on the task-specific direction, so it could not fully utilize the redundant DoFs in this direction as the proposed oriented CMI did.

Fig. 5.

Simulation results of path tracking under vertical external force. (A) Motion histories of the robot using different controllers. (B) Zoom-in view of the robot tip trajectories and the CMEs of different controllers at the last time step. The actual UME at the last time step is superposed to the desired UME. (C) The changes of oriented CMIs along the trajectory.

VI. Conclusion

In this paper, we present a redundancy resolution framework for CTR based on an efficient method for calculating the gradient of CTR manipulability. Task-specific performance indices based on velocity/compliance manipulability are proposed for path tracking in different operation scenarios. The proposed derivative propagation method reduces the computational time for the Hessians by 68% compared to the FD method. Simulation studies were conducted in three specific scenarios corresponding to avoiding singularity, avoiding obstacles, and overcoming external forces. The proposed redundancy resolution scheme consistently outperformed the standard resolved-rates, the damped least square method, and other state-of-the-art methods in [20], [14] for path tracking, demonstrating potentials in facilitating teleoperation as well as task planning.

Although this paper only studies quasi-static path tracking, the proposed task-specific controller can be extended to facilitate the dynamic control of the CTR. The proposed rapid Hessian calculation can also be used to accelerate kineto-static planning and design or any optimization process that requires the estimation of the CTR Hessian.

Our future work will focus on the investigation of applying the proposed redundancy resolution algorithm in catheter-based cardiac arrhythmia ablation therapies [22], which requires simultaneous control of catheter and guiding sheath as well as hybrid position/force control.