2.1. Review of Cosserat rod model

Cosserat rod model describes the equilibrium state of a small segment of thin rod subjected to internal and external distributed forces and moments. Previous works [Reference Jones, Gray and Turlapati32] have detailed the differential geometry of a thin rod with Cosserat rod theory so that the deformation can be estimated by solving Ordinary differential equations (ODEs) in (1)–(4) given external forces $\boldsymbol{F}_{e}$ and external moments $\boldsymbol{L}_e$ . To distinguish variables from different frames, we use lowercase letters for variables in local frame and uppercase letters for variables in global frame.

(1) \begin{align} \dot{\boldsymbol{P}} = \boldsymbol{R} \boldsymbol{v} \end{align}

(2) \begin{align} \dot{ R} = R \hat{\boldsymbol{u}} \end{align}

(3) \begin{align} \dot{\boldsymbol{N}} = -\boldsymbol{F}_e \end{align}

(4) \begin{align} \dot{\boldsymbol{M}} = -\dot{\boldsymbol{P}}\times \boldsymbol{N} - \boldsymbol{L}_e \end{align}

where the dot symbol in $\dot{\boldsymbol{x}}$ represents the derivative of $\boldsymbol{x}$ respect to the arc length $s$ , hat symbol in $\hat{\boldsymbol{x}}$ reconstruct vector $\boldsymbol{x}$ to a 3 by 3 skew symmetric matrix, $\boldsymbol{P}$ is the shape in global frame, $R$ is the rotation matrix of local frame relative to the global frame, $\boldsymbol{N}$ and $\boldsymbol{M}$ are internal force and moment in global frame which obeys the constitutive law

(5) \begin{align} \boldsymbol{N} = RK_{SE}\left (\boldsymbol{v}-\boldsymbol{v}^*\right ) \end{align}

(6) \begin{align} \boldsymbol{M} = RK_{BT}\left (\boldsymbol{u}-\boldsymbol{u}^*\right ) \end{align}

$\boldsymbol{v}$ and $\boldsymbol{u}$ are differential geometry parameters of a rod, $\boldsymbol{v}^*$ and $\boldsymbol{u}^*$ are differential geometry parameters of a rod with no external loads. $K_{SE}$ and $K_{BT}$ are the stiffness matrices. The boundary conditions for solving the ODEs (1)–(4) are described as

(7) \begin{align} \boldsymbol{N}\left (loc_{tip}\right ) = \boldsymbol{F}_{tip} \end{align}

(8) \begin{align} \boldsymbol{M}\left (loc_{tip}\right ) = \boldsymbol{T}_{tip} \end{align}

where $\boldsymbol{F}_{tip}$ and $\boldsymbol{T}_{tip}$ are external force and torque applied at the tip. $loc_{tip}$ is the tip position in arc length. Thus, the rod shape $\boldsymbol{P}$ can be computed by solving the boundary value problem (BVP).

2.2. ODEs for model curvature calculation

The widely accepted Cosserat rod model in (1)–(4) are expressed in global frame. But it is beneficial to use the model in local frame to calculate the curvatures. We firstly take the derivative of $\boldsymbol{M}$ in (6) and combine with (4)

(9) \begin{equation} R \hat{\boldsymbol{u}} K_{BT}\left (\boldsymbol{u}-\boldsymbol{u}^*\right ) + R K_{BT}\dot{\boldsymbol{u}} = -\dot{\boldsymbol{P}}\times \boldsymbol{N}-\boldsymbol{L}_e \end{equation}

Then, using the relation $\dot{\boldsymbol{P}}\times \boldsymbol{N} = \hat{\dot{\boldsymbol{P}}}\boldsymbol{N}$ , $\dot{\boldsymbol{P}} = R\boldsymbol{v}$ , and $\hat{(R\boldsymbol{v})} = R\hat{\boldsymbol{v}}R^T$ (this is true when $R \in SO(3)$ ), we can obtain

(10) \begin{equation} R \hat{\boldsymbol{u}}K_{BT}(\boldsymbol{u}-\boldsymbol{u}^*) + RK_{BT}\dot{\boldsymbol{u}} = -R\hat{\boldsymbol{v}}R^T \boldsymbol{N}-\boldsymbol{L}_e \end{equation}

Multiplying $R^T$ on both side, and solve for $\dot{\boldsymbol{u}}$ , we can have

(11) \begin{equation} \dot{\boldsymbol{u}} = -K_{BT}^{-1}\left ( \hat{\boldsymbol{u}}K_{BT}(\boldsymbol{u}-\boldsymbol{u}^*) + \hat{\boldsymbol{v}}R^T\boldsymbol{N} + R^T\boldsymbol{L}_e \right ) \end{equation}

We define local variables $\boldsymbol{n} = R^T\boldsymbol{N}$ , and $l_e = R^T\boldsymbol{L}_e$ . The meaning of the variables $\boldsymbol{n}$ and $\boldsymbol{l}_e$ are actually the variables $\boldsymbol{N}$ and $\boldsymbol{L}_e$ expressed in local frame, respectively. Hence, (11) becomes

(12) \begin{equation} \dot{\boldsymbol{u}} = -K_{BT}^{-1}\left ( \hat{\boldsymbol{u}}K_{BT}(\boldsymbol{u}-\boldsymbol{u}^*) + \hat{\boldsymbol{v}}\boldsymbol{n} + \boldsymbol{l}e \right ) \end{equation}

In order to solve $\boldsymbol{n}$ , we use (3) and multiply $R^T$ on the two sides and add $\dot{R}^T\boldsymbol{N}$ , which gives

(13) \begin{equation} R^T\dot{\boldsymbol{N}} + \dot{R}^T\boldsymbol{N} = -R^T\boldsymbol{F}_e + \dot{R}^T\boldsymbol{N} \end{equation}

Again, we define $R^T\boldsymbol{F}_e = \boldsymbol{f}_e$ . Notice that the left-hand side in (13) is actually the derivative of $R^T\boldsymbol{N}$ , which is $\dot{\boldsymbol{n}}$ . $\dot{R}$ can be replaced by (2) on the right-hand side. Thus, we have

(14) \begin{equation} \dot{\boldsymbol{n}} = -\boldsymbol{f}_e+\hat{\boldsymbol{u}}\boldsymbol{n} \end{equation}

To sum up, the curvatures of a general rod can be calculated by

(15) \begin{align} \dot{\boldsymbol{u}} = -K_{BT}^{-1}\left ( \hat{\boldsymbol{u}}K_{BT}(\boldsymbol{u}-\boldsymbol{u}^*) + \hat{\boldsymbol{v}}\boldsymbol{n} + \boldsymbol{l}_e \right ) \end{align}

(16) \begin{align} \dot{\boldsymbol{n}} = -\boldsymbol{f}_e + \hat{\boldsymbol{u}}\boldsymbol{n} \end{align}

with boundary conditions

(17) \begin{align} \boldsymbol{u}\left(loc_{tip}\right) = \textbf{0} \end{align}

(18) \begin{align} \boldsymbol{n}\left(loc_{tip}\right) = \boldsymbol{f}_{tip} \end{align}

where $loc_{tip}$ and $f_{tip}$ are the location and applied external force at the tip, respectively. (15) and (16) can be simplified in a special case where the rod is straight ( $\boldsymbol{u}^* = \textbf{0}$ and $\boldsymbol{v}^* = [0\quad 0\quad 1 ]^T$ ), with circular cross-section ( $K_{11} = K_{22} = K_{33}/2$ ), nonshear and inextensible ( $\boldsymbol{v} = [0\quad 0\quad 1 ]^T$ ), and no external moment ( $\boldsymbol{l}_e=\textbf{0}$ ). The simplified ODEs can be written as

(19) \begin{align} \dot{u}_x &= -n_y/K_{BT,11} \end{align}

(20) \begin{align} \dot{u}_y &= n_x/K_{BT,22} \end{align}

(21) \begin{align} \dot{n}_x &= -f_x + u_y n_z \end{align}

(22) \begin{align} \dot{n}_y &= -f_y - u_x n_z \end{align}

(23) \begin{align} \dot{n}_z &= -f_z-u_yn_x + u_xn_y \end{align}

where $x$ , $y$ , and $z$ are the first, second, and third component of a vector, respectively. $K_{BT,ij}$ means the entry of $K_{BT}$ at row $i$ and column $j$ . And (21)–(23) is the same as (16). Note that both the integral variables and the boundary conditions are defined in local frame, the curvature $u_x$ and $u_y$ can be solved by simple backward integration from the distal point ( $s = L$ ) to the proximal point ( $s = 0$ ), which requires no iterative computation to solve a standard BVP. Therefore, the computational complexity is only determined by the number of nodes we divide along the arc length. Moreover, solving (19)–(23) will lead to the same result as solving the model in (1)–(4), but the former method will have faster performance for curvature calculation.

3.1. Point forces for Cosserat model

The force defined in Cosserat rod theory is distributed force whose unit is N/m. However, the objective of this paper is to estimate the magnitude and location of point force whose unit is N. Conversion has to be performed between point force and distributed force. Firstly, we define the applied point forces in a new force vector which contains the locations and magnitude components of all forces.

(24) \begin{equation} \boldsymbol{f}_{vec} = \left [s^1,f^1_{pt,x},f^1_{pt,y},s^2,f^2_{pt,x},f^2_{pt,y},\ldots,s^h,f^h_{pt,x},f^h_{pt,y}\right ]^T \end{equation}

where $s^i$ is the location of the $i^{th}$ point force, $f_{pt,x}^i$ and $f_{pt,y}^i$ are the components of the $i^{th}$ point force, and $h$ is the total number of forces acting on the continuum robot. The arc length of the manipulator can be divided into $q-1$ segments, and this gives

(25) \begin{equation} \boldsymbol{Loc} = \left [loc_1, loc_2, \ldots, loc_q\right ] \end{equation}

where $loc_i$ is the location of the $i^{th}$ node, $\boldsymbol{Loc}$ is the whole list of the nodes. In order to achieve subnode accuracy, we convert the point forces to distributed forces $\boldsymbol{f}_{vec}$ on the nodes $\boldsymbol{Loc}$ . Figure 1 shows an example to distribute the point force $f^i_{pt}$ to two adjacent nodes $j-1$ and $j$ , the point force is distributed linearly according to the arc length distance between the two nodes. Therefore, the distributed forces can be calculated by

(26) \begin{align} f^{j-1} &= \frac{f^i_{pt}}{\left(loc_j - loc_{j-1}\right)^2}\left(s^i - loc_{j-1}\right) \end{align}

(27) \begin{align} f^{j} &=\frac{f^i_{pt}}{\left(loc_j - loc_{j-1}\right)^2}\left(loc_{j} - s^i\right) \end{align}

where $f^j$ (N/m) is the distributed force, $f_{pt}^i$ (N) is the point force, $loc_{j-1}$ and $loc_j$ are two adjacent nodes for $s^i$ . Note that the $s^i$ is located between hte nodes $loc_{j-1}$ and $loc_j$ , which allows the model to solve the solution in subnode accuracy. In the case where multiple point forces are close and distributed to the same node, the forces on that node will be superposed. After knowing the distributed forces along the rod, the mechanics model is complete and curvature can then be calculated through the integration of (19)–(23).

Figure 1.

Distribution of point force to adjacent nodes to achieve subnode accuracy.

Figure 2.

Loss maps computed by (a) shape (b) curvature.

3.2. Curvature-based force estimation

A robust method for force estimation is to minimize the least square loss between the calculated curvature with measured curvatures.

(28) \begin{equation} loss_u(\boldsymbol{f}_{vec}) = \lVert \boldsymbol{u}_{cal} - \tilde{\boldsymbol{u}}\rVert ^2 \end{equation}

where $\boldsymbol{u}_{cal}$ is the curvature calculated from $\boldsymbol{f}_{vec}$ and $\tilde{\boldsymbol{u}}$ is the curvature measured by sensors.

Similar to minimizing the loss of curvature, the loss of shape $loss_{P}(\boldsymbol{f}_{vec})$ could also be used for the objective function of the optimization algorithm. Figure 2 shows a simulation example of the loss of shape $loss_{P}(\boldsymbol{f}_{vec})$ and loss of curvature $loss_{u}(\boldsymbol{f}_{vec})$ with various force magnitudes and locations. The loss map is calculated by following steps: 1) Choosing $\boldsymbol{f}_{vec} = [200\quad 0.3\quad 0 ]^T$ as the ground truth, and calculate the curvature $\tilde{\boldsymbol{u}}$ and shape $\tilde{\boldsymbol{p}}$ using Cosserat rod theory. The $\tilde{\boldsymbol{u}}$ and $\tilde{\boldsymbol{p}}$ can be assumed as the measured data from sensors. 2) Calculate $\boldsymbol{u}_{cal}$ and $\boldsymbol{P}_{cal}$ with the same method, but use different $\boldsymbol{f}_{vec}$ whose first component ranges from 100 to 290 mm and the second component ranges from 0 to 0.5 N. 3) Use (28) to compute the loss for each pair of force magnitude and location. As illustrated in Fig. 2, the loss of curvature (b) shows better convex property than that of shape coordinates (a), and thus more robust results can be calculated [Reference Bubeck33]. Therefore, we use curvature-based method to estimate the force.

Figure 3.

Compute the curvatures using template disk. $A_I$ is the area surrounded by the template disk and the shape, and $b$ is the radius of the template disk.

3.3. Curvature feedback

Curvature feedback of a elastic tube can be obtained from either FBG sensors or images. For curvature measurement using FBG sensors, multicore FBG fiber is typically used to compute the curvature through the change of the Bragg wavelength [Reference Moore and Rogge34]. In this paper, the curvature measurement using FBG sensors is obtained directly from the software Shape Sensing v1.3.1 provided by the supplier.

To obtain the curvature feedback from the intraoperative image feedback, one of the major challenges is that the shape of the tube on the image is noisy, especially when the resolution of the image is low. This is because the tube shape can only be discretely captured by pixels of the image and the resulting centerline of the tube is not continuous. Thus, it is inapplicable to compute the curvature based on the basic geometry, which is the norm of the derivative of the tangent direction. The derivative of a noisy shape will increase the noise magnitude and reduce the quality of the curvature feedback. Many methods have been proposed to reduce the noise of the curvature feedback from the image feedback, including segmentation methods [Reference Nguyen and Debled-Rennesson27, Reference Nguyen and Debled-Rennesson28] and integral invariant methods [Reference Lin, Chiu, Widder, Hu and Boston26, Reference Pottmann, Wallner, Yang, Lai and Hu29]. In this paper, we implemented the integral invariant method proposed in ref. [Reference Frette, Virnovsky and Silin25] to compute the curvature feedback of the elastic tube from an image, which can be described as

(29) \begin{equation} \Vert \boldsymbol{u} \Vert \approx \frac{3A_I}{b^3} - \frac{3\pi }{2b} \end{equation}

where $\Vert \boldsymbol{u} \Vert$ is the norm of the curvature feedback at the center of the template disk, $A_I$ is the area surrounded by the template disk and the shape, and $b$ is the radius of the template disk (see Fig. 3). However, this integral invariant method can only be used to compute the curvature feedback of a tube if the tube only has in-plane bending. Special care has to be taken to compute the curvature feedback from image of a tube with out-of-plane bending, which will be the focus of our future work.

4.1. Force estimation result using multicore FBG sensors

4.1.1. Experiment setup

As illustrated in Fig. 4, the experiments were performed by adding multiple forces to a straight Nitinol tube. ATI force sensor (ATI Industrial Automation, United States) with 3D printed probe was mounted on a fixed vertical board. The probes are designed with different height in order to contact the Nitinol tube at various location and orientation (Fig. 4(c)). For every single probe that mounted on the force sensor, a counterpart probe (Fig. 4(a) and (b)) is also designed to keep the same distance from the probe head to the wall. This allows the force sensor with probe to be interchangeable with its counterpart such that the shape of Nitinol tube shape remains unchanged. For example, the Nitinol tube in Fig. 4(a) and (b) has the same shape, but the location of the force sensor is swapped. Thus, we can use one force sensor to measure multiple external forces. Fiber Bragg gratings sensors (FBGS International NV, Belgium) was inserted inside the Nitinol tube to measure the curvature.

Figure 4.

Experiment setup for force estimation. The ATI force sensor with a probe is mounted on a fixed vertical wall. The FBGS is inserted inside Nitinol tube for curvature measurement. Counterpart of each force sensor with probe are designed to keep the shape of Nitinol in (a) and (b) the same, and the force sensor can swap the location. Different height design of the probe in (c) allows for 3D shape deformation.

4.1.2. System parameter calibration

The system calibration was conducted in two steps: (1) location calibration and (2) stiffness calibration. Since the FBGS fiber is transparent, it is hard to identify the locations of gratings inside the fiber. But the relative locations (spacing between adjacent gratings) are specified on the user manual (20 mm). The objective of location calibration is to ensure the curvatures we measured are aligned with the positions on the Nitinol. Three cases of single force at different locations were recorded, and the bias of the location can be minimized by adding an offset to the results.

(30) \begin{equation} s_{est} = s_{cal} + s_{bias} \end{equation}

where $s_{est}$ is the estimated location of the external force, $s_{cal}$ is the result of the model, and $s_{bias}$ is the bias to offset the error. After location calibration, we conducted stiffness calibration to match the calculated force magnitude with the measured force magnitude. Notice that the stiffness change has trivial impact on the location estimation, therefore the location calibration is still valid after completing the stiffness calibration. The result of the calibration is listed in Table I.

Table I.

Calibrated parameters.

4.1.3. Force estimation results

The error analysis of the force estimation result is grouped into three categories: single force estimation analysis, double forces estimation analysis, and triple forces estimation analysis. For each category, the experiment was performed 13 times by applying varying contact force (0.27–1.96 N) at 13 different locations on the Nitinol tube. The step size of the integration of the ODEs for curvature calculation is $ds =$ 1 mm.

As summarized in Table II, the RMSE of single force estimation, double force estimation, and triple force estimation are $0.037 \pm 0.069$ N, $0.060 \pm 0.046$ N, and $0.127 \pm 0.081$ N, respectively. Therefore, the proposed method can estimate the force magnitude accurately, despite the fact that larger number of forces can slightly reduce the estimation accuracy on force magnitude. The RMSE of the single-location estimation, double-location estimation, and triple-location estimation is 2.95 $\pm$ 2.17 mm, $2.58\pm 2.06$ mm, and $4.69\pm 4.06$ mm, respectively. This indicates that a larger location error will occur when the number of external forces increases.

Table II.

Force estimation RMSE using FBG sensor feedback.

Figure 5.

Experimental results for single force estimation. (a) Force magnitude estimation. (b) force location estimation.

Figures 5 and 6 summarize the force estimation results for single force and multiple forces, respectively. The blue dots are the predicted result and the red dots are the measured results. Most of the cases in Fig. 5 can be predicted accurately except case 1 (error magnitude of 8.72%). This error occurs because the curvature can only be measured by the first few gratings (the spacing of adjacent gratings is 20 mm), while the rest gratings will read 0 because no internal moment exists after the location where the force is applied. In Fig. 6, each case has two or three values, which refer to the two or three forces, respectively. The red dots and the blue dots are aligned well with each other, which demonstrates the accuracy of the proposed estimation method.

Figure 6.

Experimental results for double and triple forces estimation. (a) Force magnitudes estimation. (b) force locations estimation. During each experiment, the circle marker indicates the first force, diamond marker indicates the second force, and the triangle marker indicates the third force (triple force scenario).

4.2. Force Eestimation result using image feedback

In this subsection, we investigate the potential application of curvature-based force estimation method in image-guided interventions. We assume the continuum robot with in-plane bending so that its shape can be captured by one image. However, the 3D shape reconstruction of the continuum robot with out-of-plane bending can be achieved by images captured from multiple viewing angles or 3D imaging techniques such as MRI [Reference Kwok, Chen, Chau, Luk, Nilsson, Schmidt and Zion35] or 3D ultrasould imaging [Reference Dai, Lei, Roper, Chen, Bradley, Curran, Liu and Yang36].

4.2.1. Workflow to estimate the force from images

The process to estimate the contact force can be summarized as: (a) continuum robot centerline detection; (b) compute the curvature feedback based on the detected centerline; and (c) estimate the contact force based on the curvature feedback. Here, we assume the continuum robot shape can be accurately identified via various image segmentation techniques [Reference Manakov, Kolpashchikov, Danilov, Nikita Vitalievich Laptev and Gerget37] or mounting active tracking coils on the continuum device [Reference Chen, Howard, Godage and Sengupta12, Reference Yue Chen, Wang, Kwong, Stevenson and Schmidt38]. Thus, we will skip the image segmentation step in this preliminary study. To obtain this ideal continuum robot shape from image feedback, we first deform the Nitinol tube with random force, overlay the deformed tube (Fig. 7(a)) on a 128 × 170 grid with the pixel resolution of 0.5 mm × 0.5 mm (which is the ultrasound imaging resolution), and then threshold the grid (as shown in Fig. 7(b)) to obtain Fig. 7(c). After obtaining continuum tube image, the centerline of the continuum robot, as presented in Fig. 7(d) can be detected. This will be achieved by the skeletonization algorithm proposed in refs. [Reference Kerschnitzki, Kollmannsberger, Burghammer, Duda, Weinkamer, Wagermaier and Fratzl39, Reference Lee, Kashyap and Chu40], which is well-accepted imaging processing algorithm in the field of computer vision. A sample continuum robot image and the corresponding result of the skeletonization algorithm are presented in Fig. 7(d). After detecting the centerline of the continuum robot, the curvature feedback of the robot can then be computed through (29). Finally, the contact force can be estimated by the curvature-based force estimation method by feeding the curvature feedback we computed.

Figure 7.

(a) Shape of the deformed rod. (b) The blurred grid within the threshold. (c) The resulting image of the deformed rod. (d) Centerline (red pixels) detection using skeletonization algorithm.

Figure 8.

Curvatures computed from the imaging.

4.2.2. Force estimation results

As presented in Fig. 8, the curvature feedback can be computed accurately from image data. This is a critical step for the curvature-based force estimation method. After obtaining the curvature feedback, we use them to estimate the force magnitudes and the corresponding locations. The step size of the integration of the ODEs for curvature calculation is $ds =$ 1 mm. The results of the curvature-based force estimation are presented in Figs. 9 and 10. As summarized in Table III, the overall force magnitude estimation error is $0.049 \pm 0.048$ N and the overall location estimation error is $2.75 \pm 1.71$ mm. For single force estimation, the force magnitude estimation error is $0.045 \pm 0.040$ N and the location error is $2.20 \pm 1.73$ mm. For double force estimation, the force magnitude estimation error is $0.051 \pm 0.048$ N and the location estimation error is $3.11 \pm 1.64$ mm. For triple force estimation, the force magnitude estimation error is $0.050 \pm 0.037$ N and the location estimation error is $2.59 \pm 1.41$ mm. The result shows that the curvature-based method can estimate the force locations accurately for either single force or multiple force estimations. However, the force magnitude accuracy will be degraded if more forces are acting on the rod. Although moderate errors exist, this result validates the feasibility of applying curvature-based force estimation method with image data to estimate the contact force.

Table III.

Force estimation RMSE using image feedback.

Figure 9.

Single force estimation result based on curvature computed from image. (a) Force magnitude estimation. (b) Force location estimation.

Figure 10.

Experimental results for double and triple forces estimation based on image feedback. (a) Force magnitudes estimation. (b) Force locations estimation. During each experiment, the circle marker indicates the first force, diamond marker indicates the second force, and the triangle marker indicates the third force (triple force scenario).