Simultaneous Estimation of Shape and Body Contact Force for Continuum Robots Under Mixed External Geometric Constraints

Abstract

It is challenging to simultaneously estimate the shape and contact force of miniature continuum surgical robots due to the limited sensing capabilities, deformable structure, and complexity of the external environment they operate in. In this work, the integration of a quaternion-based multicontact Cosserat model with contact force regularization enables a dual-pronged approach for simultaneous shape and body contact force estimation using minimum shape measurement input (i.e., tip position). In addition to internal actuation, cable friction, and material nonlinearity, the proposed state estimation approach also accounts for multiple environmental contacts with clinically relevant mixed geometric constraints (point, plane, and curved surface), as these complex and variable anatomical interactions critically influence the robot’s behavior. The solving procedure is then reformulated as a nonlinear large-scale optimization problem to improve the state estimation robustness. Finally, both simulations and experiments on a cable-driven variable-stiffness notched-tube continuum robot were conducted to validate the proposed state estimation approach, demonstrating a comparable level of accuracy to state-of-the-art methods.

Keywords

continuum surgical robots Cosserat rod model contact modeling shape estimation force sensing

Introduction

The work in this article is based on Chapter 2 of Jibiao Chen’s PhD thesis,¹ with Experiment 1 presenting more polished and refined results.

Accurate estimation^2,3 of robot states (e.g., robot pose, strain, shape, and contact force) is a fundamental prerequisite for continuum robots to effectively address challenges in design optimization,⁴ motion planning,⁵ and force control.⁶ In general, sensor-based techniques,⁷ including embedded sensors and external imaging, are common approaches to measure the robot states for continuum robots. However, continuum robots, especially in surgical applications, often face limited sensing capabilities due to their miniature size, soft deformable body structure, and the complexity of the surrounding environments,⁸ which hinder the integration of traditional sensors.⁹ Therefore, accurate modeling becomes indispensable, potentially providing a reliable and efficient way for estimating the continuum robot states to ensure its safe operation.³

Related work

Much modeling effort in the past decade has focused on estimating the robot shape and/or tip force for continuum robots. Lilge et al.² recently proposed a stochastic approach that uses Gaussian process (GP) regression for estimating shapes and strains of continuum robots. This approach demonstrates accurate estimation of robot states using a zero-mean prior. Besides, the anisotropic shape for a multi-contact-aided continuum robot was also estimated for endoluminal intervention.¹⁰ The early work by Xu and Simaan¹¹ presented the intrinsic tip force sensing method based on the singular value decomposition of Jacobian mapping. Back et al.¹² used a classical Cosserat rod model to estimate tip force, leveraging the pre-detected robot shape from an endoscopic camera. Additionally, the robot shape and tip force of laser-profiled continuum manipulators were simultaneously estimated in endobronchial intervention.¹³ However, most of these aforementioned works focus on estimation approaches for continuum robots operating in free space or under an external force applied only to the tip of continuum robots.

The motion behavior of continuum robots can easily lead to contact with the surrounding along the arc length of its body, especially in a constrained environment. Therefore, some recent research has focused on sensing body contact force for continuum robots, which presents a more challenging estimation problem than tip force estimation. Ryu et al.^14,15 introduced the Cosserat rod model to estimate body contact force, but it requires its body shape as prerequisite information. Based on the fixed centrode deviation method, Chen et al.¹⁶ proposed a two-dimensional model-based approach to estimate the external contact location for a pneumatic soft robot with one degree of freedom (DOF), while neglecting the actual magnitude of the contact force. Cosserat model was then investigated in multiple works for estimation and localization of body contact force for various soft and continuum robots.^17,18 Gao and Lin⁹ proposed a model-based estimation method utilizing Fiber Bragg Grating (FBG) sensors to measure tendon tension and robot curvature, enabling simultaneous estimation of body contact force, contact number, and contact location. However, most of the aforementioned approaches require prior determination of either the FBG-based robot shape,^9,19 both the image-based robot shape and point contact locations,^12,18 or image-based contact locations together with environmental constraints¹⁴ as inputs for contact force estimation. This strategy treats shape estimation and force estimation as separate processes without accounting for their inherent nonlinear coupling.³

Only a few works have been developed to date for the simultaneous estimation of the shape and body contact forces in continuum robots. The pseudo rigid body model^20,21 was first proposed to estimate robot shape and body contact force using known contact positions and locations under point constraints. Implementing Lagrange multipliers, an optimization-based framework was proposed to predict profile and body contact force of a planar cable-driven continuum robot in the reference.²² Ferguson et al.³ extended the stochastic GP-based approach² to enable the estimation of both the robot shape and external force for continuum robots. This advanced approach incorporates the fusion of noise and sensing measurements to enhance accuracy. Toshimitsu et al.²³ employed embedded soft-robot sensors (e.g., internal capacitive flex sensors) to estimate the pose and external forces of a soft continuum manipulator, demonstrating strong potential for proprioceptive sensing. In combination with the finite element method, multitapped flex sensors were further integrated into soft robots by the same group to achieve high-accuracy shape and force estimation.²⁴ Additionally, a new shooting-based Cosserat rod model²⁵ and a novel dynamic model with frictional contact constraints²⁶ were, respectively, presented for modeling soft robots in contact scenarios. By integrating FBG sensors, a model-based approach was proposed to predict shape and body contact force for flexible tubes¹⁹ without considering internal actuation. Lu et al.²⁷ proposed a data-driven approach to simultaneously estimate the robot shape and contact force through sparse FBG measurements. However, those approaches still necessitate multiple sensor measurements (e.g., several strains, curvatures, and backbone positions) along the robot arc length for the simultaneous estimation. Additionally, many recent works fail to consider internal actuation^19,26 and only consider either the point contact constraint^3,18,19,28 or the single contact scenarios,^18,22 which do not provide accurate representation of most surgical environments.

Contributions

In confined environments, the sensing limitations (e.g., miniature size) of continuum robots allow only minimal shape measurements for state estimation. Incorporating complex mixed external geometric constraints is also crucial for accurately representing realistic environmental interactions (e.g., endovascular navigation), as these constraints significantly affect the robot’s mechanical behavior. In contrast to previous works that employ the model-based approach for estimating robot states under either point loads,^3,18,19,28 single contact scenarios,^18,22 or situations without internal actuation,^19,26 we propose a quaternion-based multicontact Cosserat model for state estimation with minimum shape measurement input, offering robust parameterization and integration of coupled states for continuum robots.

The main contributions of this article can be summarized as follows: 1.

A quaternion-based Cosserat rod model is introduced to perform simultaneous estimation of robot shape and body contact force (both magnitude and location) in cable-driven continuum robots while requiring only the tip position information.

It is a deterministic model-based estimation approach that reduces essential predetermined parameters required in continuum robot state estimation, thereby enabling accurate state reconstruction in constrained environments with minimal shape sensing.

The proposed state estimation is reformulated as a nonlinear global optimization problem, employing a large-scale optimization method combined with contact force regularization to enhance the robustness of body contact force estimation.

Implemented on a cable-driven variable-stiffness notched-tube continuum robot (VSNC), the proposed approach achieves mean shape error of around 1 mm, mean contact force magnitude error of around 55 mN, and mean contact location error of around 1.5 mm, which are of comparable accuracy to the state-of-the-art (SOTA) methods^3,18,19,28 that require additional shape inputs.

The proposed state estimation approach takes into account the effects of internal actuation, cable friction, material nonlinearity, and in particular multiple external contacts with mixed geometric constraints, including point, plane, and curved surface constraints that more closely represent the environmental contacts during the surgical procedure. Our approach advances recent works by modeling both the robot’s inherent nonlinearities and the coupling of multiple external geometric constraints, yielding more accurate shape and force estimates.

The rest of this article is organized as follows. The second section formulates the robot state estimation problem for continuum robots, specifically for the VSNC. The third section proposes a quaternion-based multicontact Cosserat rod model for continuum robots. The solving procedure of the proposed state estimation approach is then clarified in the fourth section. The fifth and sixth sections, respectively, evaluate and validate the simulation and experimental results of the proposed Cosserat-based contact estimation approach. The last two sections provide discussion, concluding remarks, and an outline of future work for our article.

Motivation and Robot Structure

Motivation

Implementing robust state estimation methodologies is essential for continuum robots to ensure safe navigation and manipulation, especially in complex surgical environments with the limited shape sensing capabilities. For example, in the endovascular interventional (EI) procedure, navigating catheters and guidewires through the human vascular system is challenging due to the complex mixed external constraints, such as the intricate shape of the vessels like the internal carotid arteries in the neck,^29,30 as shown in Figure 1. The limited shape sensing capabilities and the complex external geometry of anatomical structures in continuum surgical robots during the EI procedure motivate our work of quaternion-based multicontact Cosserat modeling for simultaneous estimation of shape and body contact force, using minimal shape measurements and incorporating mixed external constraints. Our motivation is driven by the following two key considerations:

FIG. 1.

Description of robotic catheterization for the EI procedure. (A) Insertion location for the robotic catheter: femoral artery in the patient’s leg. (B) Vascular system in the patient’s head, including the intricate vessels. (C) Utilizing the VSNC with a single bending segment to achieve a substantial bending angle during the endovascular navigation procedure. EI, endovascular interventional procedure; VSNC, variable-stiffness notched-tube continuum robot.

From a theoretical standpoint, inferring external forces from shape deformation is sometimes ill-conditioned, as similar shapes can result from different force conditions.²⁸ To address this, we use a high-fidelity Cosserat rod model that jointly estimates shape and force. By representing both pose and contact locations within a modified quaternion-based Cosserat model, we avoid Euler-angle singularities^31,32 and achieve robust handling of contacts and boundary forces in multicontact scenarios.

From an application perspective, enabling contact force estimation using only tip position can significantly reduce system cost. Achieving high-resolution shape sensing along the entire body of a miniature continuum robot requires complex and expensive sensor integration, along with sophisticated data fusion. Our method offers the potential to enable contact force estimation using only tip sensing.

Structural parameters of notched components

As shown in Figure 1, a steerable catheter is typically inserted via the femoral artery in the patient’s leg³³ and potentially to the narrow and tortuous vascular system in the patient’s head and neck. The controlled steering of the catheter across various sharp bifurcations is crucial to ensure a smooth and safe catheterization process.³⁴ In our work, a cable-driven VSNC is introduced as the steerable robotic catheter in the EI procedure, utilizing a single bending segment with sections of different bending curvatures to achieve enhanced ability in navigating sharp turns. It is also the continuum robot candidate used to validate our proposed state estimation approach. Besides, it is noted that the phantom models (vessels A and B) used in our simulations and experiments were segmented and molded based on the anatomical vessels shown in Figure 1A, B.

Notched-tube structures made from superelastic Nickel Titanium (Nitinol) materials are increasingly being studied in surgical robotics due to their excellent properties of superelasticity and biocompatibility, which can be easily integrated into surgical instruments.³⁵ A notched-tube continuum robot consists of tubes with strategically cut notches, reducing its stiffness and enabling it to bend at a large angle.³⁶

As shown in Figure 2A, the VSNC consists of three different bending sections A, B, and C with outer diameter ( $D_{o}$ ) of 1.26 mm and inner diameter ( $D_{i}$ ) of 1 mm and is proposed for use in the endovascular procedure. The notch structural parameters $s_{m}$ , $b_{m}$ , $h_{m}$ , $t_{m}$ , $l_{m}$ , and w of the continuum robot are represented in Figure 2A. Hence, the geometric relationships of the notch unit length $l_{m}$ and the notch section (A, B, and C) length $L_{m}$ can be expressed as follows: (

\begin{array}{l} l_{m} = b_{m} + 2 h_{m} + t_{m} L_{m} = s_{m} + 3 l_{m} + (b_{m} + h_{m} + t_{m}) \end{array},

where m represents the section index of the robot (

m = 1, 2, and 3

). Therefore, the total length

ℓ

of the robot can be written as follows: (

ℓ = L_{1} + L_{2} + L_{3} .

FIG. 2.

Discretization of the VSNC along the robot arc length. (A) The robot consists of three bending sections (A, B, and C) that are represented by blue boxes. (B) Discrete points on the robot edges that are represented in red in each bending section, and $j = 1, 2, 3, 4$ indicates the index of the notch element in every bending section.

It is noted that the different structural parameters in sections A, B, and C are essential for achieving variable stiffness and adaptability and can be determined through a combination of our proposed modeling approach, design optimization techniques,³⁷ and experimental validation.^4,38 By adjusting these parameters, we can locally control bending stiffness and flexibility. The proximal section (A) requires higher stiffness for support and force transmission, while the middle and distal sections (B and C), with intermediate and lower bending stiffness, need greater flexibility to navigate complex anatomical pathways in confined spaces.

As shown in Figure 2B, each notch section includes four notch elements ( $j = 1, 2, 3, 4$ ). It is noted that the discrete points on the left edge, the right edge, and the centerline of the continuum robot can be, respectively, represented by ${j = 1, 2, 3, 4 & m = 1, 2, 3 | P_{L 0}, P_{L m, j λ}}$ , ${j = 1, 2, 3, 4 & m = 1, 2, 3 | P_{R 0}, P_{R m, j λ}}$ , and ${j = 1, 2, 3, 4 & m = 1, 2, 3 | P_{0}, P_{m, j λ}}$ , where $λ = 1, 2, 3, 4$ . The discrete points on the left edge, the right edge, and the centerline of the robot are written as $P_{L i}$ , $P_{R i}$ , and $P_{i}$ for brevity.

The shear/extension and bending/torsion matrices can be, respectively, expressed as follows: (

\begin{array}{l} K_{s e} (s) = diag (G A (s), G A (s), E A (s)) \\ K_{b t} (s) = diag (E I_{x x} (s), E I_{y y} (s), G I_{z z} (s)) \end{array},

where E and G are Young’s and shear moduli of the proposed model, respectively. E can be expressed as a piecewise function using

E_{A}

E_{B}

, and

E_{C}

to account for the material superelasticity (refer to Eq. 9 for detailed information).

I_{x x} (s)

and

I_{y y} (s)

are the second moments of area related to the x- and y-axes, while

I_{z z} (s) = I_{x x} (s) + I_{y y} (s)

is the polar moment of inertia of the cross-section about the z-axis. It is noted that the detailed derivation of

I_{x x (y y)} (s)

can be found in Section 1 of Supplementary Data S1.

Cosserat-Based Contact Modeling Under Multiple External Constraints

In this section, we derive a new quaternion-based Cosserat contact model for cable-driven continuum robots. Taking into account frictional interaction, material nonlinearity, and environmental contact constraints, the classical Cosserat rod model is modified to accommodate general external loading conditions.

Model assumptions

The basic assumptions of the proposed Cosserat-based contact model are clarified as follows: 1.

As shown in Figure 3, the tendon interactions are assumed to occur only at the edges of each notch element.³⁹ Therefore, the cable friction forces only exist at the discrete points ${j = 1, 2, 3, 4 & m = 1, 2, 3 | P_{L m, j 2}, P_{L m, j 3}}$ on the left tip edge. Additionally, we can get the relationship of the cable force at the discrete points ${j = 1, 2, 3, 4 & m = 1, 2, 3 | P_{L m, j 1}, P_{L m, j 4}}$ in the successive notch elements: $F_{c m, (j + 1) 1} = F_{c m, j 4}$ and $F_{c m, j 1} = F_{c m, j 2}$ .

The angles between $F_{c m, j 2}$ and $F_{c m, j 3}$ , and between $F_{c m, j 3}$ and $F_{c m, j 4}$ , are defined as the notch bending angles $α_{m, j 2}$ and $α_{m, j 3}$ , respectively. The angle between the friction force $f_{t m, j 2}$ and the cable force $F_{c m, j 2}$ is assumed to be equal to $π - α_{m, j 2}$ . The angle between the friction force $f_{t m, j 3}$ and the cable force $F_{c m, j 3}$ is also assumed to be equal to $π - α_{m, j 3}$ . Besides, $α_{m, j 2}$ is assumed to be equal to $α_{m, j 3}$ due to the symmetry during notch bending.

The discrete points ${j = 1, 2, 3, 4 & m = 1, 2, 3 | {\bar{P}}_{L m, j 1}, {\bar{P}}_{L m, j 4}, {\bar{P}}_{R m, j 1}, {\bar{P}}_{R m, j 4}}$ on the robot are assumed to be locations where collisions or contacts with the environment may occur. The corresponding body contact forces are, respectively, denoted by $f_{L m, j 1}$ , $f_{L m, j 4}$ , $f_{R m, j 1}$ , and $f_{R m, j 4}$ , as shown in Figure 3B. The frames $x_{{m, j λ}} y_{{m, j λ}} z_{{m, j λ}}$ and $x_{i} y_{i} z_{i}$ denote the local frame g along the robot arc length. The frame XYZ denotes the global frame G relative to the robot base $P_{0}$ .

The bending of the continuum robot is dominated by the different bending members,⁴⁰ as indicated by the orange and purple dotted rectangles in Figure 2A. The bending behavior can be tuned by adjusting the structural parameters (e.g., $b_{m}$ and $h_{m}$ ) to remove specific tube materials and create a variable-stiffness notch pattern. Section A of the robot has the highest bending stiffness, while section C has the lowest bending stiffness. The bending of section A is dominated by the lateral portion (bending member 2), whereas the bending of section C is dominated by the central portion (bending member 1). Both bending members 1 and 2 contribute to the bending of section B. It is noted that a nomenclature is provided in Supplementary Data S2 to clarify the definitions of symbols and variables in this section.

FIG. 3.

Force analysis of each calculation element. (A) Discrete points on the left edge, the right edge, and the centerline of each notch element on the robot. (B) Balance of forces and moments in each notch element.

Cable friction force

As shown in Figure 3B, the scalar forms of friction forces $f_{t m, j 2}$ and $f_{t m, j 3}$ at contact points $P_{L m, j 2}$ and $P_{L m, j 3}$ can be written as follows: (

\begin{array}{l} f_{t m, j 2} = {\bar{μ}}_{m, j 2} N_{m, j 2} f_{t m, j 3} = {\bar{μ}}_{m, j 3} N_{m, j 3} \end{array},

where

N_{m, j 2}

and

N_{m, j 3}

are the scalar forms of the normal forces at contact points

P_{L m, j 2}

and

P_{L m, j 3}

, respectively.

{\bar{μ}}_{m, j 2}

and

{\bar{μ}}_{m, j 3}

are the equivalent friction coefficients at contact points

P_{L m, j 2}

and

P_{L m, j 3}

, which are defined as follows: (

\begin{array}{l} {\bar{μ}}_{m, j 2} = η_{m, j 2} μ {\bar{μ}}_{m, j 3} = η_{m, j 3} μ \end{array},

where

μ

denotes the original friction coefficient.

η_{m, j 2}

and

η_{m, j 3} \in {- 1, 1}

are the adjusting factors that describe the current situation of the friction force direction.³⁹ Then, the cable force balance for the actuation cable at discrete point

P_{L m, j 2}

can be written as follows: (

\begin{array}{l} {\bar{μ}}_{m, j 2} N_{m, j 2} + F_{c m, j 3} \cos 2 α_{m, j 2} - F_{c m, j 2} \cos α_{m, j 2} = 0 \\ N_{m, j 2} - F_{c m, j 3} \sin 2 α_{m, j 2} + F_{c m, j 2} \sin α_{m, j 2} = 0 \end{array} .

Hence, the scalar form of normal force $N_{m, j 2}$ can be expressed as follows: (

\begin{array}{l} N_{m, j 2} = \frac{F_{c m, j 3} \sin α_{m, j 2}}{\cos α_{m, j 2} + {\bar{μ}}_{m, j 2} \sin α_{m, j 2}} = \frac{F_{c m, j 2} \sin α_{m, j 2}}{\cos 2 α_{m, j 2} + {\bar{μ}}_{m, j 2} \sin 2 α_{m, j 2}} \end{array} .

Taking into account the cable force balance equation at discrete point $P_{L m, j 3}$ , we obtain a similar formulation for the normal force $N_{m, j 3}$ as follows: (

\begin{array}{l} N_{m, j 3} = \frac{F_{c m, j 4} \sin α_{m, j 3}}{\cos α_{m, j 3} + {\bar{μ}}_{m, j 3} \sin α_{m, j 3}} = \frac{F_{c m, j 3} \sin α_{m, j 3}}{\cos 2 α_{m, j 3} + {\bar{μ}}_{m, j 3} \sin 2 α_{m, j 3}} \end{array} .

Material nonlinearity

Nitinol materials exhibit a nonlinear mechanical behavior, leading to hysteresis in the relationship between actuation forces and bending angles.⁴¹ To provide an approximate estimation of the Young’s modulus of the notched-tube robot, our work employs a simplified material model that represents the stress-strain curve using a piecewise linear function^35,41 of the Young’s modulus. $E_{A}$ , $E_{B}$ , and $E_{C}$ denote the equivalent Young’s moduli of sections A, B, and C, respectively. $E_{A}$ and $E_{B}$ are assumed to be constant, as small bending angles occur in sections A and B, resulting in the stress-strain curve remaining within the linear stage. Using a piecewise linear stress-strain curve, $E_{C}$ can be written as a piecewise linear function as follows: (

E_{C} (σ) = {\begin{array}{l} E_{s 2}, & σ \leq σ_{u_{1}} \\ E_{s 1}, & σ \in (σ_{u_{1}}, σ_{u_{2}}) \\ E_{s 2}, & σ \geq σ_{u_{2}} \end{array},

where

σ_{u 1}

and

σ_{u 2}

represent the minimum and maximum stresses on the cross-section, respectively.

E_{s 1}

and

E_{s 2}

represent the austenite and superelastic phases, respectively, in the stress-strain curve of Nitinol materials. The detailed derivation of

σ

is provided in Section 2 of Supplementary Data S1.

Quaternion-Based Cosserat contact model

In contrast to our previous inextensible Cosserat-based model (i.e., $s = s^{*}$ , Kirchhoff rod model)^42,43 designed for slender flexible bodies without internal actuation, the proposed Cosserat-based model incorporates internal actuation, transverse shear, axial strains, and multiple external contact constraints for continuum robots. s and $s^{*}$ represent the arc coordinates along the robot centerline, with s indicating the coordinate without shear and stretching, and $s^{*}$ indicating the coordinate with shear and stretching. Additionally, in contrast to the previous work,³¹ our model adopts a quaternion-based formulation of Cosserat rods to avoid singularities during numerical calculations. Assuming that the robot materials are incompressible and every cross-section always retains a circular shape⁴⁴ during motion deformation, the governing equations of the proposed Cosserat-based model in the local (Lagrangian) frame g, with respect to $s^{*}$ , can be expressed as follows: (

\frac{\tilde{d} F (s^{*})}{d s^{*}} + ω^{*} \times F (s^{*}) + f (s^{*}) = 0

(

\frac{\tilde{d} M (s^{*})}{d s^{*}} + T^{*} \times F (s^{*}) + ω^{*} \times M (s^{*}) + m (s^{*}) = 0

(

F (s^{*}) = K_{s e} (ε - ε_{0}) M (s^{*}) = K_{b t} (ω^{*} - ω_{0})

(

T^{*} = {[ε_{x} ε_{y} 1]}^{T}

(

f (s^{*}) = \frac{1}{ζ} f (s) m (s^{*}) = \frac{1}{ζ} m (s)

(

ζ = \frac{d s^{*}}{d s} = 1 + ε_{z}

where F, T, M,

ω

, f, and m are the internal force, unit tangent vector of the robot centerline, internal moment, curvature vector (also called angular rate of change of the local frame³¹), force distribution per unit length, and moment distribution per unit length in the local frame g, respectively. The symbols with * indicate the symbols relative to

s^{*}

. The strain

ε

of every cross-section is expressed as

{[ε_{x} ε_{y} ε_{z}]}^{T}

in the local frame g. The stretch ratio between s and

s^{*}

is defined as

ζ = \frac{d s^{*}}{d s}

. The wavy line

\sim

on the top of differential sign d in Eqs. (10)–(11) indicates the derivatives with respect to arc coordinate s in the local frame g. The linear constitutive laws (i.e., Eq. 12) to calculate the internal forces and moments are expressed in g. It is noted that

ω^{*}

is assumed to be equal to

ω

because second-order terms of the small strains of every cross-section can be omitted.

The initial strain $ε_{0}$ and initial curvature vector $ω_{0}$ are assumed to be $0$ , since the initial rest configuration of continuum robots is typically straight. The quaternion $q_{i} \in H$ , relative to the discretized calculation element i along the robot arc length, can be expressed as follows: (

q_{i} = {[\begin{matrix} q_{1, i} & q_{2, i} & q_{3, i} & q_{4, i} \end{matrix}]}^{T} .

Transforming Eqs. (10)–(11) and discretizing the quaternion-based Cosserat rod model with respect to $s^{*}$ yields the discretized system for arbitrary discretized calculation element i as follows: (

c_{1, i} = q_{1, i}^{2} + q_{2, i}^{2} + q_{3, i}^{2} + q_{4, i}^{2} - 1 = 0

(

c_{2, i} = \frac{F_{x, i + 1} - F_{x, i}}{(1 + ε_{z, i}) \cdot Δ s_{i}} + ω_{y, i} F_{z, i} - ω_{z, i} F_{y, i} + \frac{f_{x, i}}{(1 + ε_{z, i})} = 0

(

c_{3, i} = \frac{F_{y, i + 1} - F_{y, i}}{(1 + ε_{z, i}) \cdot Δ s_{i}} + ω_{z, i} F_{x, i} - ω_{x, i} F_{z, i} + \frac{f_{y, i}}{(1 + ε_{z, i})} = 0

(

c_{4, i} = \frac{F_{z, i + 1} - F_{z, i}}{(1 + ε_{z, i}) \cdot Δ s_{i}} + ω_{x, i} F_{y, i} - ω_{y, i} F_{x, i} + \frac{f_{z, i}}{(1 + ε_{z, i})} = 0

(

c_{5, i} = 2 (- q_{2, i} J_{1, i} + q_{1, i} J_{2, i} + q_{4, i} J_{3, i} - q_{3, i} J_{4, i}) + \frac{ε_{y, i} F_{z, i} - F_{y, i}}{e_{x}^{T} K_{b t} e_{x}} + \frac{(e_{z}^{T} K_{b t} e_{z} - e_{y}^{T} K_{b t} e_{y})}{e_{x}^{T} K_{b t} e_{x}} ω_{y, i} ω_{z, i} + \frac{m_{x, i}}{e_{x}^{T} K_{b t} e_{x} (1 + ε_{z, i})} = 0

(

c_{6, i} = 2 (- q_{3, i} J_{1, i} - q_{4, i} J_{2, i} + q_{1, i} J_{3, i} + q_{2, i} J_{4, i}) + \frac{F_{x, i} - ε_{x, i} F_{z, i}}{e_{y}^{T} K_{b t} e_{y}} + \frac{(e_{x}^{T} K_{b t} e_{x} - e_{z}^{T} K_{b t} e_{z})}{e_{y}^{T} K_{b t} e_{y}} ω_{x, i} ω_{z, i} + \frac{m_{y, i}}{e_{y}^{T} K_{b t} e_{y} (1 + ε_{z, i})} = 0

(

c_{7, i} = 2 (- q_{4, i} J_{1, i} + q_{3, i} J_{2, i} - q_{2, i} J_{3, i} + q_{1, i} J_{4, i}) + \frac{ε_{x, i} F_{y, i} - ε_{y, i} F_{x, i}}{e_{z}^{T} K_{b t} e_{z}} + \frac{(e_{y}^{T} K_{b t} e_{y} - e_{x}^{T} K_{b t} e_{x})}{e_{z}^{T} K_{b t} e_{z}} ω_{x, i} ω_{y, i} + \frac{m_{z, i}}{e_{z}^{T} K_{b t} e_{z} (1 + ε_{z, i})} = 0,

where (

\begin{array}{l} ω_{i}^{*} = {[ω_{x, i} ω_{y, i} ω_{z, i}]}^{T} = 2 Q_{1} Q_{2} \\ Q_{1} = [\begin{matrix} - q_{2, i} & q_{1, i} & q_{4, i} & - q_{3, i} \\ - q_{3, i} & - q_{4, i} & q_{1, i} & q_{2, i} \\ - q_{4, i} & q_{3, i} & - q_{2, i} & q_{1, i} \end{matrix}] \\ Q_{2} = {[\begin{matrix} Q_{1, i} & Q_{2, i} & Q_{3, i} & Q_{4, i} \end{matrix}]}^{T} \end{array}

(

ε_{z, i} = \frac{F_{z, i} + F_{z, i + 1}}{2 e_{z}^{T} K_{s e} e_{z}}

c_{1, i}

c_{2, i}

c_{3, i}

c_{4, i}

c_{5, i}

c_{6, i}

, and

c_{7, i}

represent the basic equations of each discretized calculation element in the proposed model, respectively.

Δ s_{i}

denotes the element length between two successive discretized points along the undeformed robot arc length. The set

{e_{x}, e_{y}, e_{z}}

forms an orthonormal fixed basis of unit vectors in the global frame G, where

e_{x} = {[1 0 0]}^{T}

e_{y} = {[0 1 0]}^{T}

, and

e_{z} = {[0 0 1]}^{T}

represent the unit vectors along the coordinate axes of G, respectively.

In the discretized expression of the proposed contact model, $F_{i} = (F_{x, i}, F_{y, i}, F_{z, i})$ is the internal force in the local frame g, the change rate of the quaternions is defined as $Q_{{1, 2, 3, 4}, i} = \frac{q_{{1, 2, 3, 4}, i + 1} - q_{{1, 2, 3, 4}, i}}{(1 + ε_{z, i}) \cdot Δ s_{i}}$ , and the change rate of $Q_{{1, 2, 3, 4}, i}$ is defined as $J_{{1, 2, 3, 4}, i} = \frac{Q_{{1, 2, 3, 4}, i + 1} - Q_{{1, 2, 3, 4}, i}}{(1 + ε_{z, i}) \cdot Δ s_{i}}$ , respectively. The projection of the force distribution of every calculation element onto g is defined as $f_{i} = (f_{x, i}, f_{y, i}, f_{z, i})$ , which consists of the contact force and the force distribution that the actuation cable applies to the robot. $m_{i} = (m_{x, i}, m_{y, i}, m_{z, i})$ also denotes the moment distribution of every calculation element in g.

As shown in Figure 3, each notch element on the notched-tube continuum robot is considered as a calculation element in the proposed model. Every calculation element consists of three portions (I, II, and III), with $P_{i}$ representing its center. The normal forces ( $N_{m, j 2}$ and $N_{m, j 3}$ ) and cable friction forces ( $f_{t m, j 2}$ and $f_{t m, j 3}$ ) in the local frame g can be found in Section 3 of Supplementary Data S1. The boundary conditions for these three portions in Figure 3A, including the force distribution $(f_{i} (I)$ , $f_{i} (I I)$ , and $f_{i} (III))$ and the moment distribution $(m_{i} (I)$ , $m_{i} (I I)$ , and $m_{i} (III))$ , can also be found in Section 4 of Supplementary Data S1.

As shown in Figure 3B, $M_{i}$ and $F_{i}$ are assumed to be the equivalent internal moment and internal force along the robot arc length, which are transformed from the internal moment and internal force in the bending members and can be automatically solved during the optimization procedure. Therefore, combining the force distributions in portions I, II, and III, the collective force distribution $f_{i} (s^{*})$ and the collective moment distribution $m_{i} (s^{*})$ along the robot centerline for every calculation element i are expressed as follows: ((

\begin{array}{l} f_{i} (s^{*}) = f_{i} (I) + f_{i} (I I) + f_{i} (III) \\ m_{i} (s^{*}) = m_{i} (I) + m_{i} (I I) + m_{i} (III) \end{array} .

The bending angle $α_{m, j 2 (j 3)}$ of each calculation element is written as $α_{i}$ for brevity, which is expressed as follows: ((

α_{i} = α_{m, j 2} = α_{m, j 3} = \frac{\arccos [(R_{i} \cdot e_{z}) \cdot (R_{i - 1} \cdot e_{z})]}{2},

where

R_{i}

denotes the rotation matrix of the local frame

g

with respect to the global frame G, which can be expressed as follows: ((

\begin{array}{l} R_{i} = [\begin{matrix} 2 (q_{1, i}^{2} + q_{2, i}^{2}) - 1 & 2 (q_{2, i} q_{3, i} - q_{1, i} q_{4, i}) & 2 (q_{2, i} q_{4, i} + q_{1, i} q_{3, i}) \\ 2 (q_{2, i} q_{3, i} + q_{1, i} q_{4, i}) & 2 (q_{1, i}^{2} + q_{3, i}^{2}) - 1 & 2 (q_{3, i} q_{4, i} - q_{1, i} q_{2, i}) \\ 2 (q_{2, i} q_{4, i} - q_{1, i} q_{3, i}) & 2 (q_{3, i} q_{4, i} + q_{1, i} q_{2, i}) & 2 (q_{1, i}^{2} + q_{4, i}^{2}) - 1 \end{matrix}] \end{array} .

The total bending angle $Θ$ of the VSNC is written as follows: ((

Θ = 2 \sum_{i = 1}^{i = n_{e}} α_{i},

where

n_{e}

denotes the total number of the calculation elements along the VSNC.

Additionally, the directions of body contact forces $f_{L m, j 1}$ , $f_{L m, j 4}$ , $f_{R m, j 1}$ , and $f_{R m, j 4}$ are determined by the geometric relationship of the external constraints, as shown in the following Eqs. (38)–(40). The body contact force is written as $f_{k}$ for brevity ( $1 \leq k \leq K$ ). k is the index of contact location, while K denotes the number of the total contact locations. The actuation cable force $F_{c} (s^{*})$ in the local frame g at the robot base ( $s^{*} = 0$ ) can be expressed as follows: ((

F_{c} (0) = {[0 0 - F_{c 0}]}^{T},

where

F_{c 0}

denotes the proximal cable force at the robot base. It is measured as the input in the proposed model.

Since the robot base is assumed to be aligned with the origin of the global frame G, the position $p (s^{*})$ of each discrete point $P_{i}$ on the robot centerline can be expressed as follows: ((

\begin{array}{l} p (s^{*}) = [\begin{matrix} 2 \int_{0}^{s^{*}} (q_{2} (s^{*}) q_{4} (s^{*}) + q_{1} (s^{*}) q_{3} (s^{*})) d s^{*} \\ 2 \int_{0}^{s^{*}} (q_{3} (s^{*}) q_{4} (s^{*}) - q_{1} (s^{*}) q_{2} (s^{*})) d s^{*} \\ \int_{0}^{s^{*}} [2 (q_{1}^{2} (s^{*}) + q_{4}^{2} (s^{*})) - 1] d s^{*} \end{matrix}] \end{array} .

Integrating Eq. (31), the position of the distal endpoint of every calculation element, $(p_{X, i}, p_{Y, i}, p_{Z, i})$ , can be specifically presented by the discretized form as follows: ((

\begin{array}{l} p_{X, i} = \frac{1}{3} \sum_{ς = 1}^{ς = i} {(1 + ε_{z, ς}) \cdot Δ s_{i} \cdot [(2 q_{2, ς} + q_{2, ς + 1}) q_{4, ς} \\ + (q_{2, ς} + 2 q_{2, ς + 1}) q_{4, ς + 1} + (2 q_{1, ς} + q_{1, ς + 1}) q_{3, ς} \\ + (q_{1, ς} + 2 q_{1, ς + 1}) q_{3, ς + 1}]} \end{array}

((

\begin{array}{l} p_{Y, i} = \frac{1}{3} \sum_{ς = 1}^{ς = i} {(1 + ε_{z, ς}) \cdot Δ s_{i} \cdot [(2 q_{3, ς} + q_{3, ς + 1}) q_{4, ς} \\ + (q_{3, ς} + 2 q_{3, ς + 1}) q_{4, ς + 1} - (2 q_{1, ς} + q_{1, ς + 1}) q_{2, ς} \\ - (q_{1, ς} + 2 q_{1, ς + 1}) q_{2, ς + 1}]} \end{array}

((

\begin{array}{l} p_{Z, i} = \frac{1}{3} \sum_{ς = 1}^{ς = i} {(1 + ε_{z, ς}) \cdot Δ s_{i} \cdot [2 (q_{1, ς}^{2} + q_{1, ς + 1} q_{1, ς} \\ + q_{1, ς + 1}^{2} + q_{4, ς}^{2} + q_{4, ς + 1} q_{4, ς} + q_{4, ς + 1}^{2}) - 3]} \end{array},

where

ς \in {1, …, i}

denotes the index number of the discretized calculation elements prior to element i in the proposed Cosserat-based contact model. It is noted that

p_{X, i}

is assumed to be 0 if the robot is restricted to bending in a planar motion.

Environmental contact constraints

The geometric information of the external environment can be predetermined through sensing techniques, such as magnetic resonance (MR) or computed tomography (CT) imaging. Since our proposed approach assumes the external environmental constraints are fixed and rigid, a single preoperative image reconstruction is sufficient. By applying the Point Inclusion in Polygon Test algorithm,⁴⁵ the shape of the external environment can be efficiently extracted from preoperative images. By incorporating the obtained geometric information about the external environment’s shape, constraint equations can be established to estimate the body contact forces with the environment. The majority of external contact constraints can be simplified as a combination of contact constraints involving points, planes, and curved surfaces. The external contact constraints between the robot and the environment are assumed to be static. Continuum surgical robots may encounter multiple types of external contact constraints in minimally invasive treatment. The human vascular system is chosen as a typical example of the external environment in our work. The robot should navigate through the intricate vasculature to achieve a large bending angle during the EI procedure. As shown in Figure 4, in the global frame G, the external constraints between the robot and the vessels, including the point contact constraint, plane contact constraint, and curved surface contact constraint, are included in the proposed Cosserat-based contact model.

FIG. 4.

Multiple types of external contact constraints between the continuum robot and the environment, in terms of the point contact constraint, plane contact constraint, and curved surface contact constraint.

Point contact constraint

The relatively sharp contact regions in the external environment can be regarded as point constraints. $(p_{X_{1}}, p_{Y_{1}}, p_{Z_{1}})$ is the position of the contact constraint point in the global frame G, and the constraint equation ( $h_{p 1}$ ) of the point contact constraint can be expressed as follows: ((

h_{p 1} = | f_{k} | \cdot [{(p_{X_{1}} - p_{X, k})}^{2} + {(p_{Y_{1}} - p_{Y, k})}^{2} + {(p_{Z_{1}} - p_{Z, k})}^{2}] = 0,

where

(p_{X, k}, p_{Y, k}, p_{Z, k})

represents the potential discrete point on the robot that coincides with the contact constraint point.

f_{k}

denotes the norm of the body contact force

f_{k}

Plane contact constraint

The relatively flat and smooth contact regions in the external environment can be treated as plane constraints. $(A_{1}, B_{1}, C_{1}, D_{1})$ are the constraint parameters of the contact constraint plane, and $(p_{X_{2}}, p_{Y_{2}}, p_{Z_{2}})$ is the position of the corresponding contact constraint point. The constraint equation ( $h_{p 2}$ ) of the plane contact constraint can be expressed as follows: ((

h_{p 2} = | f_{k} | \cdot (A_{1} p_{X, k} + B_{1} p_{Y, k} + C_{1} p_{Z, k} + D_{1}) = 0 .

Curved surface contact constraint

The curved or protruding regions in the external environment can be considered as curved surface constraints. $(p_{X_{3}}, p_{Y_{3}}, p_{Z_{3}})$ and $R_{c}$ are the center position and the radius of the curved surface constraint, respectively. The constraint equation ( $h_{c}$ ) of the curved surface contact constraint can then be written as follows: (

\begin{array}{l} h_{c} = | f_{k} | \cdot | \sqrt{{(p_{X, k} - p_{X_{3}})}^{2} + {(p_{Y, k} - p_{Y_{3}})}^{2} + {(p_{Z, k} - p_{Z_{3}})}^{2}} - R_{c} | \\ + | f_{k} | \cdot | t_{k} \cdot n_{k} | = 0 \end{array},

where

t_{k}

denotes the unit tangent vector of the constraint surface at the contact point.

n_{k}

denotes the unit vector in the direction of the body contact force

f_{k}

in the global frame G, which can be expressed as follows: ((

\begin{array}{l} n_{k} = {[n_{x} n_{y} n_{z}]}^{T} = {\begin{array}{l} R_{i}^{- 1} \cdot e_{y}, & point constraint \\ \frac{{[A_{1} B_{1} C_{1}]}^{T}}{\sqrt{A_{1}^{2} + B_{1}^{2} + C_{1}^{2}}}, & plane constraint \\ \frac{{[p_{X, k} - p_{X_{3}} \cdot p_{Y, k} - p_{Y_{3}} \cdot p_{Z, k} - p_{Z_{3}}]}^{T}}{R_{c}}, & curved surface constraint \end{array} \end{array} .

Additionally, $p_{k} = (p_{X, k}, p_{Y, k}, p_{Z, k})$ denotes the position of the potential contact point on the robot (e.g., discrete points inside ${{\bar{P}}_{L m, j 1}, {\bar{P}}_{L m, j 4}, {\bar{P}}_{R m, j 1}, {\bar{P}}_{R m, j 4}}$ ) where contact may occur. The position of these contact points can be written as follows: ((

p_{k} = {\begin{array}{l} p - r_{o} \cdot R_{i} \cdot e_{y}, & for left tip edge \\ p + r_{o} \cdot R_{i} \cdot e_{y}, & for right tip edge \end{array} .

The projection of $f_{k}$ onto the local frame g is also obtained as follows: ((

f_{k} = [\begin{matrix} f_{k, x} \\ f_{k, y} \\ f_{k, z} \end{matrix}] = f_{k} \cdot R_{i}^{- 1} \cdot [\begin{matrix} n_{x} \\ n_{y} \\ n_{z} \end{matrix}] .

Methods

Incorporating the boundary condition and contact force regularization, the solving process of the proposed Cosserat-based contact model is transformed into a nonlinear global optimization problem, which simultaneously estimates both the shape and body contact force for continuum robots. Accounting for the boundary conditions and environmental contact constraints, the overall solving procedure of the proposed state estimation approach for continuum robots is illustrated in Figure 5.

FIG. 5.

(A) Block diagram of known and solved variables in the solving procedure. (B) Solving procedure of the proposed state estimation approach for continuum robots.

Boundary conditions

The proposed Cosserat-based contact model has mixed boundary conditions. The position of the robot base is fixed at the origin of the global frame G, resulting in $p (0) = {[0 0 0]}^{T}$ . The continuum robot is clamped at the robot base ( $s = 0$ ), with the robot tip experiencing an applied force $F_{t}$ and an applied moment $M_{t}$ at $s^{*} = \bar{l}$ . Therefore, the boundary condition ( $h_{b}$ ) for the entire continuum robot is as follows: ((

\begin{array}{l} h_{b} = [\begin{matrix} F (\bar{l}) - F_{t} \\ M (\bar{l}) - M_{t} \\ F_{c} (0) - {\hat{F}}_{c} (0) \\ q (0) - q_{0} \\ R (q (\bar{l})) - R (Θ) \\ p (\bar{l}) - \hat{p} (\bar{l}) \end{matrix}] = 0 \end{array},

where

\bar{l} = \sum_{i = 1}^{i = n_{c}} [(1 + ε_{z, i}) \cdot Δ s_{i}]

denotes the deformed length of the robot centerline.

n_{c}

denotes the number of discretized calculation elements in the proposed Cosserat-based model (

n_{c} = n_{e}

in the case of the VSNC). The applied force and moment are calculated based on the distal cable force applied to the robot tip,³¹ yielding

F_{t} = - F_{c 3, j 4} \cdot T^{*}

and

M_{t} = (r_{o} \cdot e_{y}) \times F_{t}

q_{0}

represents the quaternions of the cross-section at

s^{*} = 0

{\hat{F}}_{c} (0)

is the measured actuation cable force at

s^{*} = 0

q_{0}

is assumed to be

{[1 0 0 0]}^{T}

since the global frame G and local frame g are coincident at the robot base.

R (q (\bar{l}))

and

R (Θ)

represent the orientation matrices of the distal end of the robot, calculated using the quaternion

q (\bar{l})

at the robot tip and the total bending angle

Θ

, respectively.

p (\bar{l})

and

\hat{p} (\bar{l})

denote the theoretical and measured tip positions from both the proposed model and FBG sensor, respectively. It is also worth noting that the x-position of the robot tip can serve as an additional boundary condition, such as

p_{X, i} = 0

when the robot is bent in a planar space.

Owing to the z-axial strain along the robot centerline, the length condition of the total arc length of the robot centerline is imposed as follows: ((

\begin{array}{l} h_{l} = \bar{l} - ‖ p ((1 + ε_{z, 1}) \cdot Δ s_{1}) ‖ - \sum_{i = 2}^{i = n_{c}} ‖ p_{i} - p_{i - 1} ‖ = 0 \end{array},

where

p_{i} = p (\sum_{j = 1}^{j = i} [(1 + ε_{z, j}) \cdot Δ s_{j}])

for

i \geq 2

Contact force regularization

Quasi-rod mechanics equilibrium

To enhance the robustness of solving the body contact force, the arc length along the robot, ranging from 0 to $s_{k}^{*}$ , is treated as a quasi-rod mechanics equilibrium when calculating the robot state. To achieve mechanical equilibrium in a quasi-rod, the sum of forces acting on the rod must be zero. The following condition ( $h_{q}$ ) for translational equilibrium must be satisfied: ((

\begin{array}{l} h_{q} = - F (0) + \sum_{j = 1}^{j = k - 1} [R (q (s_{j}^{*})) \cdot λ_{j} f_{j}] \\ + R (q (s_{k}^{*})) \cdot [F (s_{k}^{*}) + λ_{k} f_{k}] = 0 \end{array},

where

s_{k}^{*}

denotes the contact location relative to

f_{k}

. j and

f_{j}

denote the index of potential contacts before

s_{k}

and corresponding contact forces.

λ_{j}

and

λ_{k}

represent the adjusting coefficients, which scale

f_{j}

and

f_{k}

to account for the effects of internal friction and actuation force within the quasi-rod.

Penalty approach

The penalty approach is employed to reformulate the body contact force $f_{k}$ that can ensure a stable numerical calculation. $Δ_{c}$ denotes the distance between the obstacle boundary and the robot edge. The magnitude of the body contact force $f_{k}$ exhibits a rapid increase when $Δ_{c}$ is extremely small. The regularized body contact force ${\bar{f}}_{k}$ is expressed as follows: ((

{\bar{f}}_{k} = (1 + \frac{δ_{c}}{Δ_{c}^{2}}) \cdot f_{k} \cdot R_{i}^{- 1} \cdot n_{k},

where

δ_{c}

is a penalty coefficient that is a very small positive value, such as 0.1. To ensure the convergence of the numerical iteration when estimating

f_{k}

δ_{c}

should be chosen to be sufficiently small.

Optimization procedure

The solving process can be regarded as a nonlinear global optimization problem. The proposed Cosserat-based contact model comprises Eqs. (17)–(23), (35)–(37), and (41)–(42). These equations collectively formulate the objective function $F (x)$ , which is minimized to estimate the robot state $x$ .

The state vector $x$ of the robot is defined as follows: ((

x = {[q {(s^{*})}^{T} ε {(s^{*})}^{T} f_{k} {(s^{*})}^{T} s_{k}^{*} α_{i}]}^{T},

where the robot shape can be obtained by integrating

q (s^{*})

, and the internal force

F (s^{*})

can be determined by

ε (s^{*})

f_{k} (s^{*})

and

s_{k}^{*}

represent the body contact force and its corresponding contact location. All the variables in

x

are solved by our optimization procedure, except for

q (0)

, which is provided as an input from the boundary condition in (Eq. 41) and determines the robot’s base pose.

We first present this as a constrained optimization problem: ((

\begin{array}{l} x_{best} = \underset{x}{argmin} F (x) = \sum_{i = 1}^{i = n_{c}} ‖ c_{i} ‖^{2} \\ s.t. f_{k} \geq 0 \\ h_{b} = 0 \\ h_{q} = 0 \\ h_{l} = 0 \\ h_{p 1}, h_{p 2}, h_{c} = 0 \end{array},

where

c_{i} = {[c_{1, i} c_{2, i} c_{3, i} c_{4, i} c_{5, i} c_{6, i} c_{7, i}]}^{T}

denotes the basic equation vector in the proposed contact Cosserat rod model.

x_{best}

denotes the optimal solution for the state vector

x

To convert the constrained optimization problem [Eq. (46)] into an unconstrained one, a penalty function $F (x)$ is implemented to reformulate an unconstrained optimization problem: ((

\begin{array}{l} x_{b e s t} = \underset{x}{arg min} F (x) \\ F (x) = \sum_{i = 1}^{i = n_{c}} [A^{T} \cdot (c_{i} ◦ c_{i})] + {‖ h_{b} ‖}^{2} + h_{l}^{2} \\ + B^{T} \cdot {[{‖ h_{q} ‖}^{2} h_{p 1}^{2} h_{p 2}^{2} h_{c}^{2}]}^{T} \\ A = {[1 1 1 1 0.5 0.5 0.5]}^{T} \\ B = {[0.1 0.5 0.5 0.5]}^{T} \end{array}

where A and B denote the weighting matrices for

c_{i}

and contact constraints.

°

denotes the Hadamard product. The reformulated unconstrained objective function

F (x)

is then minimized by an optimization solver employing the Levenberg–Marquardt algorithm with a trust-region strategy.⁴² To ensure the convergence of the optimization procedure, the initial guess

x_{0}

for the state vector

x

is determined using the proposed model without external constraints, providing a well-informed starting point for reliable convergence.

Figure 5A illustrates the sensor inputs required for solving the proposed contact model, which include the robot base pose $q_{0}$ and tip position $\hat{p} (\bar{l})$ obtained from FBG shape sensing, the cable force $F_{c 0}$ at the base, and environmental contact constraints ( $h_{p 1}$ , $h_{p 2}$ , and $h_{c}$ ) derived from MR/CT images. Additionally, the optimization process solves for the robot state $x_{best}$ , whose main components are the estimated robot shape $q (s^{*})$ , estimated body contact force $f_{k} (s^{*})$ , and estimated contact location $s_{k}^{*}$ .

The collision-free robot shape is first estimated by the proposed model in an obstacle-free environment and under a geometrically unconstrained condition [i.e., without Eqs. (35)–(37)]. The robot contact portions of possible collisions between the collision-free robot shape and the external environment can be selected either manually by the user or automatically by the solver. Then, the corresponding discretized segments relative to the contact regions on the robot are determined to be subject to the external contact constraints [i.e., Eqs. (35)–(37)]. This can result in more efficient computations for the proposed model when considering the external contact constraints. As illustrated in Figure 5B, the detailed solving procedure for the proposed state estimation approach can be outlined in the following three steps:

Step 1: By directly solving the proposed model without the external contact constraints, the robot shape and internal forces are obtained simultaneously. This can then be used to determine the potential collision segments with the geometric shape of the environment by calculating the overlap volume between the robot and the environment.

Step 2: If a collision is detected, the discrete points on the continuum robot within the collision segments are then assumed to be potential contact locations subject to the external contact constraints. Additionally, the environment containing the potential collision regions is considered to be external obstacles, which can be simplified into the aforementioned three types of environmental contact constraints such as the point, plane, and curved surface constraints.

Step 3: Then, the corresponding discrete points on the continuum robot are subjected to the additional constraint equations defined by the external contact constraints in the environment. The robot shape, body contact force, and contact location are eventually simultaneously determined by solving the proposed model that incorporates the external contact constraints. It is noted that the pseudocode of the aforementioned three steps is provided in Supplementary Data S3.

Additionally, in the process of solving the proposed contact model, the body contact force status is employed to determine whether the continuum robot collides with the external environment at possible contact locations within the potential collision segments. Specifically, if the body contact force $f_{k}$ is equal to 0, then the robot does not collide with the environment. Conversely, if the body contact force $f_{k}$ is nonzero, then collisions between the robot and the external environment occur. In this case, the proposed contact model can be used to simultaneously estimate the robot shape, body contact forces, and contact locations.

Simulation results

The cable-driven VSNC is necessary to navigate through the intricate internal carotid arteries in the neck during the target case study of the EI procedure. The variable stiffness of the robot provides a variable-curvature motion during navigation in the vessels. It offers a low curvature of bending motion to navigate through a flat vessel. However, it must also be able to achieve a large bending angle to access another vessel branch at a confined blood vessel corner.

Based on the complex geometric shape of the arteries that the VSNC would need to navigate through in the EI procedure, the proposed state estimation approach for the VSNC is evaluated using two distinct cases (Case I and Case II). Case I is subject to both point and plane constraints, while Case II is subject to both plane and curved surface constraints. According to the structural parameters of the target internal carotid arteries in the vascular system⁴⁶ based on patient data, the constrained external environment maps of the two clinical scenarios, Case I and Case II, were obtained, as shown in Figure 6A, B. This process involved segmenting the anatomy to generate a point cloud map, which was then used to create a constraint map, and eventually, an environment map.

FIG. 6.

Generation of the constrained environment maps for the clinical scenarios of Case I (A) and Case II (B). (C–I) Simulated endovascular navigation of the VSNC in the vessel A of Case I. (J–P) Simulated endovascular navigation of the VSNC in the vessel B of Case II. Seven motion steps of the VSNC were selected to demonstrate the estimates of the robot.

As depicted in Figure 6A, B, by fitting the extracted points on the boundary in the segmented map, the specific standard equations of the constraint point, constraint plane, and constraint curved surface in the global frame G can be, respectively, expressed as follows: 1)

Constraint point: ((

(p_{X_{1}}, p_{Y_{1}}, p_{Z_{1}}) = (0, 4, 3.26) mm

Constraint plane: ((

A_{1} \cdot X + B_{1} \cdot Y + C_{1} \cdot Z + D_{1} = 0,

where

(A_{1}, B_{1}, C_{1}, D_{1}) = (0, - 0.65, - 1, 13.5)

Constraint curved surface: ((

\begin{array}{l} {(X - p_{X_{3}})}^{2} + {(Y - p_{Y_{3}})}^{2} + {(Z - p_{Z_{3}})}^{2} = R_{c}^{2} \end{array},

where

(p_{X_{3}}, p_{Y_{3}}, p_{Z_{3}}) = (0, 5.5, 1.8) mm

and

R_{c} = 3.6

mm.

In both Case I and Case II of the simulation experiments, the VSNC is required to navigate and access another vessel branch near a confined blood vessel corner with a large bending angle. It is noted that the tip positions of the VSNC for the simulation experiments were obtained from the real-world experiments.

Case I: Contact estimation under the point and plane constraints

In Case I, as shown in Figure 6C–I, the VSNC encounters only the point and plane contact constraints during the endovascular navigation in the vessel A. Seven motion steps were selected to demonstrate the estimates, including the body contact force and the robot shape, between the VSNC and the vessel during the navigation. In Case I, the robot navigates from branch 1 to branch 2 in the vessel A. As the VSNC moves forward and bends to the right (branch 2), the robot first makes contact with the constraint point, and its body contact force gradually increases to ∼301 mN, as shown in Figure 6G. The olive green arrow represents the estimated body contact force. Subsequently, the robot simultaneously makes contact with both the constraint point and the constraint plane (see Fig. 6H), causing the contact force near the constraint point to gradually decrease, while the contact force on the constraint plane increases to ∼246 mN (see Fig. 6I). Finally, the VSNC successfully bends from branch 1 to branch 2, achieving a maximum bending angle of around 150 degrees.

Case II: Contact estimation under the plane and curved surface constraints

In Case II, the VSNC navigates from branch 3 to branch 4 in the vessel B, as shown in Figure 6J–P. In this instance, the curved surface and plane contact constraints arise during the endovascular navigation of the robot in the vessel B. In comparison with Case I, in Case II, the constraint point is replaced by a constraint curved surface. As shown in Figure 6M, the maximum body contact force at the bend of the blood vessel was reduced to 133 mN ( $< 301$ mN). At this point, as the robot transitions from branch 3 to branch 4, the overall body contact force on the VSNC decreases because the contact area of the constraint curved surface is larger than that of the constraint point. Additionally, due to the presence of the curved surface, the direction of the contact force varies over a larger range according to the local curvature direction of the curved surface. In this case, it is demonstrated that even with increased complexity of external environmental constraints, the proposed estimation method remains numerically stable.

Computation time analysis

The simulation experiments were conducted using MATLAB R2023a installed on a DELL desktop computer equipped with a 13th Generation Intel Core i7-13700 CPU running at 2.10 GHz and 32.0 GB of RAM. On the aforementioned specified computational platform, each shape and force estimation takes ∼60–90 s. Additionally, in multicontact scenarios, poorly chosen initial values can significantly affect the convergence efficiency of our estimation approach. Although we employ a trust-region-based solver⁴² that promotes convergence by iteratively updating and scaling the gradient, the nonconvex nature of our optimization problem can lead to convergence time exceeding 10 min when the initial value is poorly chosen (e.g., significantly deviating from the no-contact model solution).

Experiments and Results

Experimental setup

In the experiments, a prototype of the VSNC was developed to obtain measured shape and contact force data, which can then be compared with the simulation results. The VSNC features structural parameters as follows: ${s_{1}, s_{2}, s_{3}}$ = ${0.8, 0.6, 0.8}$ mm, ${b_{1}, b_{2}, b_{3}}$ = ${0.2, 0.25, 0.4}$ mm, ${t_{1}, t_{2}, t_{3}}$ = ${0.1, 0.1, 0.1}$ mm, ${h_{1}, h_{2}, h_{3}}$ = ${0.4, 0.3, 0.2}$ mm, $w = 0.83$ mm, $D_{i}$ = 1 mm, and $D_{0}$ = 1.26 mm. The total length of the notched bending segment (13.1 mm), together with its straight transition section (13 mm), is ∼26.1 mm, which is regarded as the overall length of our robot. As shown in Figure 7A, the 1-DOF VSNC is designed to bend within a plane. During the experiment, known weights were hung on the actuation cable via a pulley to apply a variable load, thereby determining the proximal cable force $F_{c 0}$ at the robot base. The base of the VSNC was manually translated and rotated using linear and rotation stages.

FIG. 7.

(A) Experimental setup to measure both robot shape and contact force along the robot. (B) Vessel phantom representing the geometric shapes of the internal carotid arteries (i.e., vessels A and B) for contact estimation in the clinical scenario.

To demonstrate the contact motion of the VSNC in the target case study of the EI procedure, a vessel phantom was employed to represent the intricate shape of the internal carotid arteries within the vascular system, as shown in Figure 7B. Based on the geometric shapes of the two simulated vessels described in the previous section, the vessel phantom was fabricated from silicone materials using a molding technique.⁴⁷ The vessel phantom consisted of vessel A (with point and plane contact constraints) and vessel B (with curved surface and plane contact constraints), corresponding to the simulated clinical scenarios of Case I and Case II in the previous section. The vessel wall thickness in the phantom was set to 1 mm, which is similar to the mean value of the wall thickness of the internal carotid arteries in the vascular system.⁴⁶ To ensure the relative orientation and position between the vessel phantom and the VSNC are consistent with the simulations, the linear translation stages I and II, along with the rotation stage, were utilized to calibrate the pose between the vessel phantom and the VSNC.

As shown in Figure 7A, a multicore FBG fiber was placed at the central lumen of the VSNC with a robust sensing algorithm⁴⁸ to obtain the ground truth of the robot shape. The measured tip position from the FBG sensor would serve as input information for the proposed Cosserat-based contact model. While the FBG sensor enabled full shape sensing, its main purpose was to offer ground-truth data for validating the shape estimated by our approach. The FBG sensing system, including a multicore FBG fiber, a fan-out box (FBGS, Belgium), and an FBG interrogator (RTS 125+, Sensuron, USA), was integrated within the lumen of the VSNC for shape sensing. The spatial resolution (i.e., spacing distance) between two adjacent FBG sets of the multicore FBG fiber is 3.3 mm. The body contact force along the robot was measured by a force gauge (SF-3, AIPU, China). The force gauge was mounted on the linear stage III to adjust its position and orientation relative to the vessel phantom for measuring the contact force. The maximum load and resolution of the force gauge are 3 N and 1 mN, respectively. The pointed and flat contact heads were then implemented to measure the contact forces under the point/curved surface contact constraint and the plane contact constraint, respectively. The camera (MV-CS060-10GM, Hikvision, China) was employed to record the navigation motion of the VSNC. According to the recorded snapshot of the VSNC motion, the contact points (denoted by the orange dots in Fig. 7A) along the VSNC were manually determined by counting the robot notches nearest to the contact head. Additionally, the contact points were double-checked with the force gauge to ensure that the maximum contact force occurred at the potential contact points. The measured contact location was then calculated by the total arc relative to the contact notch from the robot base at G. The red arrow in Figure 7B indicates the direction of motion during the navigation of the VSNC in the experiment.

Parameter calibration

To identify the parameters in the proposed state estimation approach, the VSNC was actuated under both loading and unloading procedures in free bending. The proximal cable force $F_{c 0}$ is equivalent to the total payload weights applied to the actuation cable, with a range of 0–500 g. The equivalent Young’s moduli ( $E_{A}$ , $E_{B}$ , $E_{s 1}$ , and $E_{s 2}$ ) of the VSNC and the friction coefficient ( $μ$ ) were calibrated with a brute force approach in constrained optimization.⁴ The calibrated results are as follows: $G = 21.3$ MPa, $E_{A} = 57.6$ MPa, $E_{B} = 43.8$ MPa, $E_{s 1} = 16.4$ MPa, $E_{s 2} = 25.7$ MPa, and $μ = 0.32$ .

Figure 8 demonstrates that the proposed model accurately replicates the experimental results during the bending motion of the VSNC. As shown in Figure 8A, B, the estimated shapes from the proposed Cosserat-based model were consistent with the measured shapes obtained from FBG sensing during both the loading and unloading procedures. The mean and maximum shape deviation errors during the loading procedure were 0.228 and 0.550 mm, respectively. In the unloading procedure, the mean and maximum shape deviation errors were 0.351 and 0.993 mm, respectively. As shown in Figure 8C, the VSNC achieves a bending angle of ∼169°. The proposed model predicts a broader hysteresis range of the bending angle compared with the experimental data. This discrepancy arises from an error in solving for the quaternions, where the quadratic sum of the quaternions (i.e., $c_{1, i}$ ) slightly exceeds 1. This is because, to ensure the convergence of the solving procedure, a small penalty value is applied to constrain $c_{1, i}$ .

FIG. 8.

Results from the parameter calibration procedure. (A) Comparison between the estimated and measured robot shapes during the loading procedure. (B) Comparison between the estimated and measured robot shapes during the unloading procedure. (C) Comparison between the estimated and measured bending angles during both the loading and unloading procedures.

Experiment 1: Contact estimation validation under the single and multiple point contact constraints

In Experiment 1, the contact estimation for the VSNC initially accounted for single and multiple point contact constraints, as shown in Figure 9. The external point loads were determined by hanging known weights at specific locations using the pulleys. The magnitude of the arbitrary external point load j (denoted by ${\bar{f}}_{e j}$ ) and its corresponding contact location j (denoted by ${\bar{s}}_{e j}$ ) were known from the experiment, for $j = 1, 2, 3, 4, 5, 6, 7$ . It is noted that the symbols without the overbar (i.e., without “-”) denote the estimates, while the symbols with the overbar (i.e., with “-”) denote the measured values of contact forces and contact locations along the robot in the experiments. As shown in Figure 9A, B, the external point loads 1 and 2, that is, ${\bar{f}}_{e 1}$ and ${\bar{f}}_{e 2}$ , were applied separately to the VSNC with different contact locations. The actual contact locations ${\bar{s}}_{e 1} = 7.6$ mm and ${\bar{s}}_{e 2} = 3.4$ mm. The two external forces were maintained at the identical value, that is, ${\bar{f}}_{e 1} = {\bar{f}}_{e 2} = 196$ mN, but the actuation cable force $F_{c 0}$ was adjusted to verify the compatibility of the proposed state estimation approach for different contact locations.

FIG. 9.

Contact estimation experiment under single and multiple point contact constraints. (A) Single point contact case: point load 1 ( ${\bar{f}}_{e 1}$ ). (B) Second single point contact case: point load 2 ( ${\bar{f}}_{e 2}$ ). (C) Multiple point contact case (dual contact) with two external loads: point load 3 ( ${\bar{f}}_{e 3}$ ) and point load 4 ( ${\bar{f}}_{e 4}$ ). (D) Multiple point contact case (triple contact) with three external loads: point load 5 ( ${\bar{f}}_{e 5}$ ), point load 6 ( ${\bar{f}}_{e 6}$ ), and point load 7 ( ${\bar{f}}_{e 7}$ ). (E–H) Comparisons of estimated and measured robot shapes across four scenarios: single contact, second single contact, dual contact, and triple contact. (I–L) Comparisons of estimated and measured results for the same four scenarios.

As shown in Figure 9E, F, the mean and maximum shape deviation errors under load 1 were 0.566 and 1.17 mm, respectively, while under load 2, they were 0.402 and 1.21 mm, respectively. In the experiment under the single point contact constraints, the results of the contact estimation are shown in Figure 9I, J. In the case of point loads 1 and 2, the mean absolute magnitude error of the body contact force was 32.5 mN, while the mean absolute contact location error was 0.91 mm. The estimated contact force magnitude achieved a level of accuracy similar to that of the experiment conducted on a cantilevered slender rod with a predetermined FBG-sensed shape and without internal actuation consideration, as reported in the SOTA work¹⁹ (52.01 mN).

As shown in Figure 9C, in this case of the multiple point contact scenario involving two point loads, the two point loads ( ${\bar{f}}_{e 3} = 98$ mN, ${\bar{f}}_{e 4} = 196$ mN, ${\bar{s}}_{e 3} = 10.5$ mm, and ${\bar{s}}_{e 4} = 4.9$ mm) and the cable action create a quasi-static equilibrium during the initial bending phase, resulting in a very small bending angle of the VSNC. In contrast to the single point contact estimation, as shown in Figure 9G, the mean and maximum shape deviation errors under two contact loads were reduced to 0.330 and 0.688 mm, respectively. In this dual-contact scenario, a larger actuation cable force was required to achieve the equivalent bending angle since the point load 3 induced an opposing external force ( ${\bar{f}}_{e 3}$ ) for the bending motion of the VSNC. The external point loads 3 and 4 were both applied to the VSNC with different contact locations, that is, ${\bar{s}}_{e 3}$ and ${\bar{s}}_{e 4}$ . As shown in Figure 9K, the mean absolute magnitude error of the body contact force was 36.5 mN, while the mean absolute contact location error was 0.88 mm. At the initial bending stage (e.g., $F_{c 0} = 0.63$ N), the absolute magnitude error of $f_{e 3}$ reached a peak of ∼120 mN. In the contact case of $F_{c 0} = 0.63$ N, the external forces counteract each other due to the combined effects of internal actuation, $f_{e 3}$ , and $f_{e 4}$ , causing the deflection to remain relatively small ( $< 1$ mm). This is attributed to the ill-conditioned problem²⁸ encountered when the slender continuum robot is in a near-degenerate configuration.

As shown in Figure 9D, a triple-contact scenario involving three external loads [point load 5 ( ${\bar{f}}_{e 5} = 98$ mN, ${\bar{s}}_{e 5} = 11.4$ mm), point load 6 ( ${\bar{f}}_{e 6} = 196$ mN, ${\bar{s}}_{e 6} = 6.3$ mm), and point load 7 ( ${\bar{f}}_{e 7} = 69$ mN, ${\bar{s}}_{e 7} = 1.6$ mm)] was conducted to further validate the applicability of our approach in multicontact settings. In this triple-contact case, the three point loads and the cable action create a quasi-static equilibrium during the initial bending phase, resulting in a very small bending angle of the VSNC. The mean and maximum shape deviations of this triple-contact case were ∼0.456 and 0.839 mm, as shown in Figure 9H. As shown in Figure 9L, in this triple-contact scenario, the mean absolute magnitude error of the body contact force was 38.9 mN, while the mean absolute contact location error was 0.79 mm. The force estimation error was greater, particularly for $f_{e 6}$ , in the triple-contact scenario due to the increased interaction complexity and overlapping effects between multiple contact points, which lead to more degenerate and ill-conditioned configurations. This is because, in such configurations, our optimization problem becomes increasingly sensitive to small perturbations, leading to reduced accuracy in estimating individual contact forces, especially when the contact points approach singular configurations. To address the ill-conditioning and singularities in force regularization, data-driven approaches²⁷ provide a viable alternative for solving Eq. (47), learning from experimental data to accurately estimate near-degenerate states.

Experiment 2: Contact estimation validation in the clinical scenario

In the contact estimation for the clinical scenario, vessel phantom A (which includes point and plane contact constraints in Case I) and vessel phantom B (which features curved surface and plane contact constraints in Case II) were used to validate the contact estimation for the out-of-plane (i.e., three-dimensional) manipulation of the VSNC. As shown in Figure 10, a polyethylene (PE) tubing with an outer diameter of 0.7 mm and an inner diameter of 0.35 mm is embedded in the VSNC to house the FBG sensor. This blue tubing extends slightly beyond the robot. This design allows the FBG fiber to pass through the narrow channel in the blood vessels in advance, ensuring that the tip of the FBG fiber is not disturbed during the robot’s movement, thereby improving the accuracy of shape measurement. As shown in Figure 10A–F, the endovascular navigation procedure of the VSNC was performed in the vessel phantom A, navigating from branch 1 to branch 2. Similarly, in the second clinical scenario, the endovascular navigation procedure of the VSNC was conducted in the vessel phantom B, moving from branch 3 to branch 4, as shown in Figure 10G–L.

FIG. 10.

(A–F) Endovascular navigation procedure of the VSNC in the clinical scenario of Case I. (G–L) Endovascular navigation procedure of the VSNC in the clinical scenario of Case II. (M) Comparison between the estimated and measured robot shapes in Case I. (N) Comparison between the estimated and measured robot shapes in Case II.

In the experiment of the first clinical scenario (Case I), $f_{k 1}$ and $s_{k 1}$ denote the estimates of the contact force and its contact location under the plane contact constraint. $f_{k 2}$ and $s_{k 2}$ denote the estimates of the contact force and corresponding contact location under the point contact constraint. In the experiment of the second clinical scenario (Case II), $f_{k 3}$ and $s_{k 3}$ denote the estimates of the contact force and its contact location under the curved surface contact constraint. $Δ$ denotes the insertion length of the robot base for the VSNC in the clinical scenario. As the VSNC is inserted, the actuation cable force $F_{c 0}$ gradually increases, allowing the robot to bend from one branch to the other during endovascular navigation.

Clinical Case I

In the experiment of Case I, the results of the contact estimation under the point and plane contact constraints (Case I) are shown in Table 1. The mean absolute magnitude error of the body contact force was 34.4 mN, while the mean absolute contact location error was 1.06 mm. As shown in Figure 10M, the mean and maximum shape deviation errors in Case I were 0.514 and 1.66 mm, respectively. It can be seen that the proposed estimation approach can also achieve high accuracy in three-dimensional cases.

Table 1.

Clinical Case I with Point and Plane Contact Constraints: Estimated ( $f_{k 1}$ and $f_{k 2}$ ) and Measured ( ${\bar{f}}_{k 1}$ and ${\bar{f}}_{k 2}$ ) Body Contact Forces, as Well as Estimated ( $s_{k 1}$ and $s_{k 2}$ ) and Measured ( ${\bar{s}}_{k 1}$ and ${\bar{s}}_{k 2}$ ) Contact Locations

Δ	$F_{c 0}$	$f_{k 1}$	${\bar{f}}_{k 1}$	$s_{k 1}$	${\bar{s}}_{k 1}$	$f_{k 2}$	${\bar{f}}_{k 2}$	$s_{k 2}$	${\bar{s}}_{k 2}$
2	1.61	/	/	/	/	40	32	12.9	12.3
4	2.59	/	/	/	/	15	37	9.8	11.5
6	3.08	/	/	/	/	108	70	8.9	9.9
8	4.06	/	/	/	/	155	117	7.7	8.6
10	4.55	/	/	/	/	301	193	6.7	7.7
12	2.10	114	89	12.9	12.3	45	62	5.8	6.8
14	1.12	246	227	9.8	11.5	/	/	/	/

The units of Δ, $s_{k 1 (k 2)}$ , and ${\bar{s}}_{k 1 (k 2)}$ are all millimeter. The units of $f_{k 1 (k 2)}$ and ${\bar{f}}_{k 1 (k 2)}$ are all millinewton.

Clinical Case II

The estimates for Case II under the curved surface and plane contact constraints are shown in Table 2. The mean absolute magnitude error of the body contact force was 21.3 mN, and the mean absolute error in contact location was 1.24 mm. Compared with the robot estimates in Case I, the contact force on the constraint curved surface was lower than that on the constraint point. This is because the constraint point creates a sharper turn in the blood vessel, which poses a greater hindrance to the navigation of the VSNC. As illustrated in Figure 10N, the mean and maximum shape deviation errors in Case II were 0.359 and 0.965 mm, respectively. Due to the larger contact area of the constraint curved surface compared with the constraint point, in VSNC navigation, most of the robot shapes were enveloped by the constraint curved surface, leading to smaller shape deviation errors compared with Case I.

Table 2.

Clinical Case II with Curved Surface and Plane Contact Constraints: Estimated ( $f_{k 1}$ and $f_{k 3}$ ) and Measured ( ${\bar{f}}_{k 1}$ and ${\bar{f}}_{k 3}$ ) Body Contact Forces, as Well as Estimated ( $s_{k 1}$ and $s_{k 3}$ ) and Measured ( ${\bar{s}}_{k 1}$ and ${\bar{s}}_{k 3}$ ) Contact Locations

Δ	$F_{c 0}$	$f_{k 1}$	${\bar{f}}_{k 1}$	$s_{k 1}$	${\bar{s}}_{k 1}$	$f_{k 3}$	${\bar{f}}_{k 3}$	$s_{k 3}$	${\bar{s}}_{k 3}$
2	1.61	/	/	/	/	15	22	11.6	12.7
4	2.59	/	/	/	/	16	36	8.9	11.3
6	3.08	/	/	/	/	91	63	10.7	10.4
8	4.06	/	/	/	/	133	94	7.7	9.5
10	4.55	/	/	/	/	104	109	6.7	8.1
12	2.10	53	78	12.9	12.8	47	40	3.3	5.6
14	1.12	204	165	11.6	12.1	/	/	/	/

The units of $s_{k 1 (k 3)}$ and ${\bar{s}}_{k 1 (k 3)}$ are all millimeter, and the units of $f_{k 1 (k 3)}$ and ${\bar{f}}_{k 1 (k 3)}$ are all millinewton.

Performance comparison with SOTA approaches

Table 3 presents a performance comparison between the proposed estimation approach and SOTA estimation methods. Continuum robots often navigate confined environments (e.g., blood vessels and tracheobronchial trees), where external structures impose complex geometric constraints on their motion. Additionally, due to the cable friction interaction and superelastic properties of robot materials (e.g., Nitinol tubes), notched-tube continuum robots often exhibit nonlinear behavior, making linear material assumptions inaccurate. Besides, in contrast to Euler–Bernoulli beams and previous Cosserat-based models, which focus on tip force estimation, assume a single contact point, or require the entire shape as input, our proposed quaternion-based Cosserat model accounts for shear deformation, axial stretch, and distributed loads, making it more suitable for highly complex contact scenarios. Therefore, in this work, we have explored the simultaneous estimation of shape and body contact force of continuum robots, while considering key factors such as complex external geometric constraints (e.g., curved surface contact), internal actuation, material nonlinearity, and multicontact interaction.

Table 3.

Comparison of Proposed Estimation Approach with State-of-the-Art Approaches

Approach	Model	Model consideration			External constraint			Input^a	Output			Simultaneous estimation of shape and body contact force	$E_{1}$ (mm)	$E_{2}$ (mN)	$E_{3}$ (%)
Approach	Model	Internal actuation	Material nonlinearity	Multiple contact	Point	Plane	Curved surface	Input^a	Shape^b	Tip^c force	Body contact force	Simultaneous estimation of shape and body contact force	$E_{1}$ (mm)	$E_{2}$ (mN)	$E_{3}$ (%)
³	Cosserat rod model	Yes	No	Yes	Yes	No	No	Several backbone positions	Yes	Yes	Yes	Yes	/	70	1.5^e
⁹	Beam model	Yes	No	Yes	Yes	No	No	Entire shape	No	Yes	Yes	No	/	10.8	4.6
¹³	Beam model	Yes	No	No	Yes	No	No	Tip position	Yes	Yes	No	No	1.51	/	/
¹⁸	Cosserat rod model	Yes	No	No	Yes	No	No	Entire shape	No	Yes	Yes	No	1.22^d	13.03	5.15
¹⁹	Cosserat rod model	No	No	Yes	Yes	No	No	Entire shape	No	Yes	Yes	No	0.84^d	52.01	5.99
²⁸	Cosserat rod model	Yes	No	Yes	Yes	No	No	Entire shape	No	Yes	Yes	No	/	580	7
⁴⁹	Cosserat rod model	Yes	No	No	Yes	No	No	Entire shape	No	Yes	Yes	No	/	24	4.3^e
Ours	Cosserat rod model	Yes	Yes	Yes	Yes	Yes	Yes	Tip position	Yes	Yes	Yes	Yes	1	55	5.7^f

It represents the required input shape information for the SOTA model-based estimation approaches to estimate contact forces.

It represents the estimated shape calculated by the SOTA approaches, rather than predetermined or premeasured shape information.

If a SOTA approach can estimate body contact forces, it is assumed to be capable of estimating tip forces, even though some literature does not provide tip force estimation results. This is because tip force is merely a special case of body contact force at the robot tip.

It denotes the mean error of the predetermined or premeasured shape. $E_{1}$ : Mean error of the estimated shape. $E_{2}$ : Mean error of the estimated body contact force. $E_{3}$ : Ratio of the mean contact location error to the total length of the respective continuum robot.

The indirectly calculated or estimated value in the corresponding reference.

The mean contact location error is ∼1.5 mm. The total length of the robot is 26.1 mm, consisting of a 13.1 mm notched-tube flexible segment and a 13 mm straight transition section.

SOTA, state-of-the-art.

As shown in Table 3, it is clearly seen that the proposed method can achieve a comparable level of estimation accuracy to Cosserat-based estimation approaches.^3,19 Although providing complete and accurate robot shape information as the input can greatly improve the accuracy of contact force estimation, obtaining full shape data is often challenging in scenarios with stringent miniature robot size requirements, such as in continuum surgical robots. Our method overcomes this limitation by enabling the simultaneous estimation of the shape and body contact force using only minimum shape input information (i.e., tip position). Although a multicore FBG fiber was used in our experiments for full shape sensing, its main role was to provide ground-truth data for validating our estimated shape. The FBG-based tip position used in our approach can equally be obtained with electromagnetic (EM) sensors or commonly used medical imaging modalities such as X-ray fluoroscopy. Thus, our method enables contact force estimation using only tip sensing, reducing overall robot complexity and minimizing the need for advanced shape sensing.

Discussion

Among the most recent works on state estimation in continuum robots, it is notable that only a few SOTA approaches¹⁹ can achieve real-time estimation performance, primarily because the entire robot shape is predetermined as a prerequisite. Other Cosserat-based estimation approaches,^3,9,49 including our method, are unable to achieve real-time performance due to the necessity of solving the additional shape within the coupled state estimation procedure. The nonconvexity of our optimization problem [Eq. (47)], caused by complex nonlinear constraints and multiple coupling terms, significantly contributes to the runtime. This limitation could potentially be mitigated through approximate convex reformulation, the integration of efficient search strategies, and parallel computing. Besides, by implementing the proposed method in programming languages such as C/C++ and deploying it on more advanced hardware, its performance could be significantly enhanced, potentially enabling real-time applicability.

Additionally, among the most recent works on force sensing in continuum robots, only Gao and Lin et al.⁹ have explicitly elaborated the effect of the distance between two adjacent contact points, making it a pioneering contribution in the field. Accurate force estimation becomes challenging when the distance between two adjacent contact points is smaller than 5 mm.⁹ A similar phenomenon is observed in our approach, where the force estimation error increases when the distance between two adjacent contact points is less than $4 Δ s_{i}$ (∼4.5 mm). An adaptive weighting function could be incorporated into the objective function to dynamically adjust the weight coefficients based on contact point spacing, potentially ensuring automatic parameter adjustments that minimize estimation errors when the spacing is small.

Conclusion

In this work, a deterministic model-based estimation approach is proposed to simultaneously estimate both the robot shape and body contact force for continuum robots. Specifically, accounting for internal actuation, frictional interaction, material nonlinearity, and multiple environmental contacts with different geometric constraints, a quaternion-based Cosserat multicontact model is proposed and validated using a typical type of continuum robots (VSNC). This approach is numerically stable and can be applied to a broader range of Cosserat-based problems involving multiple and mixed contact estimation. In future work, the proposed contact model requires validation in scenarios with more than three simultaneous contact points. To address the challenge of acquiring the external environment’s shape in real time, we will integrate intraoperative imaging and a real-time deformation model of anatomical structures into our approach to enable real-time computational performance. Additionally, since our approach has so far only been tested on a planar continuum robot, it should be further validated on spatial continuum robots to fully evaluate its capability in estimating three-dimensional external forces.

Authors’ Contributions

J.C.: Conceptualization, estimation, simulations and experiments, and writing—original draft. Y.L.: Assistance in the FBG-based shape sensing. J.Y.: Fabrication of the notched-tube continuum robot. Y.-H.L. and S.S.C.: Supervision, formulation of the proposed approach, and financial support. S.S.C.: Conceptualization, review and editing.

Footnotes

Author Disclosure Statement

No competing financial interests exist.

Funding Information

Research reported in this work was supported in part by Research Grants Council (RGC) of Hong Kong (CUHK 24201219, CUHK 14217822, CUHK 14207823, AoE/E-407/24-N), in part by Innovation and Technology Commission of Hong Kong (ITS/235/22, ITS/225/23, MHP/096/22), in part by Multi-Scale Medical Robotics Center, InnoHK, Hong Kong, and in part by The Chinese University of Hong Kong (CUHK) Direct Grant. The content is solely the responsibility of the authors and does not necessarily represent the official views of the sponsors.

Supplemental Material

References

Chen

. Modeling and Planning of Continuum Surgical Robots. Ph.D. dissertation. The Chinese University of Hong Kong: Hong Kong; 2025.

Lilge

, Barfoot

, Burgner-Kahrs

. Continuum robot state estimation using Gaussian process regression on SE (3). Int J Rob Res, 2022; 41(13–14):1099–1120.

Ferguson

, Rucker

, Webster

. Unified shape and external load state estimation for continuum robots. IEEE Trans Robot, 2024; 40:1813–1827.

Yan

, Chen

, et al. A continuum robotic cannula with tip following capability and distal dexterity for intracerebral hemorrhage evacuation. IEEE Trans Biomed Eng, 2022; 69(9):2958–2969.

Chen

, Yan

, Qiu

, et al. A cross-entropy motion planning framework for hybrid continuum robots. IEEE Robot Autom Lett, 2023; 8(12):8200–8207.

Campisano

, Caló

, Remirez

, et al. Closed-loop control of soft continuum manipulators under tip follower actuation. Int J Rob Res, 2021; 40(6–7):923–938.

Wang

, Xie

, Xin

, et al. Sensing expectation enables simultaneous proprioception and contact detection in an intelligent soft continuum robot. Nat Commun, 2024; 15(1):9978.

Zhou

, Xu

, Lin

, et al. Submillimeter fiber robots capable of decoupled macro-micro motion for endoluminal manipulation. Sci Adv, 2024; 10(47):eadr6428.

Gao

, Lin

, Zhou

, et al. Body contact estimation of continuum robots with tension-profile sensing of actuation fibers. IEEE Trans Robot, 2024; 40:1492–1508.

10.

, Gao

, Lin

, et al. A multi-contact-aided continuum manipulator with anisotropic shapes. IEEE Robot Autom Lett, 2021; 6(3):4560–4567.

11.

, Simaan

. An investigation of the intrinsic force sensing capabilities of continuum robots. IEEE Trans Robot, 2008; 24(3):576–587.

12.

Back

, Manwell

, Karim

, et al. Catheter contact force estimation from shape detection using a real-time Cosserat rod model. In: 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE; 2015. pp. 2037–2042.

13.

Gao

, Liu

, Shen

, et al. Laser-profiled continuum robot with integrated tension sensing for simultaneous shape and tip force estimation. Soft Robot, 2020; 7(4):421–443.

14.

Ryu

, Woo

, So

, et al. Shape and contact force estimation of inserted flexible medical device. Int J Control Autom Syst, 2020; 18(1):163–174.

15.

Jeon

, Yi

, Shin

, et al. Kinematic modelling and configuration estimation for elastic wire using minimum sensors. Proc Instit Mech Eng C J Mech Eng Sci, 2015; 229(12):2195–2206.

16.

Chen

, Wang

, Galloway

, et al. Modal-based kinematics and contact detection of soft robots. Soft Robot, 2021; 8(3):298–309.

17.

Wang

, Wang

, Xu

, et al. Towards Collision Detection, Localization and Force Estimation for a Soft Cable-driven Robot Manipulator. In: 2021 IEEE International Conference on Robotics and Automation (ICRA). IEEE; 2021, pp. 12222–12228.

18.

Brumfiel

, Qi

, Ravigopal

, et al. Image-Based Force Localization and Estimation of a Micro-Scale Continuum Guidewire Robot. IEEE Trans Med Robot Bionics, 2024; 6(1):153–162.

19.

Qiao

, Borghesan

, De Schutter

, et al. Force from Shape—Estimating the Location and Magnitude of the External Force on Flexible Instruments. IEEE Trans Robot, 2021; 37(5):1826–1833.

20.

Venkiteswaran

, Sikorski

, Misra

. Shape and contact force estimation of continuum manipulators using pseudo rigid body models. Mech Mach Theory, 2019; 139:34–45.

21.

Venkiteswaran

, Su

. A parameter optimization framework for determining the pseudo-rigid-body model of cantilever-beams. Precis Eng, 2015; 40:46–54.

22.

Ashwin

, Mahapatra

, Ghosal

. Profile and contact force estimation of cable-driven continuum robots in presence of obstacles. Mech Mach Theory, 2021; 164:104404.

23.

Toshimitsu

, Wong

, Buchner

, et al. Sopra: Fabrication & dynamical modeling of a scalable soft continuum robotic arm with integrated proprioceptive sensing. In: 2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE; 2021, pp. 653–660.

24.

Tian

, Cangan

, Navarro

, et al. Multi-tap resistive sensing and fem modeling enables shape and force estimation in soft robots. IEEE Robot Autom Lett, 2024; 9(3):2830–2837.

25.

Wiese

, Berthold

, Wangenheim

, et al. Describing and analyzing mechanical contact for continuum robots using a shooting-based cosserat rod implementation. IEEE Robot Autom Lett, 2024; 9(2):1668–1675.

26.

Xun

, Zheng

, Kruszewski

. Cosserat-rod based dynamic modeling of soft slender robot interacting with environment. IEEE Trans Robot, 2024; 40:2811–2830.

27.

, Li

, Chen

, et al. Simultaneous Estimation of Shape and Force along Highly Deformable Surgical Manipulators Using Sparse FBG Measurement. In: 2024 IEEE International Conference on Robotics and Automation (ICRA). IEEE; 2024. pp. 9866–9872.

28.

Aloi

, Dang

, Barth

, et al. Estimating forces along continuum robots. IEEE Robot Autom Lett, 2022; 7(4):8877–8884.

29.

Russ

, O’Hara

, Nagesh

, et al. Treatment planning for image-guided neuro-vascular interventions using patient-specific 3D printed phantoms. In: Medical Imaging 2015: Biomedical Applications in Molecular, Structural, and Functional Imaging, vol. 9417. SPIE; 2015, pp. 580–590.

30.

Dreyfus

, Boehler

, Lyttle

, et al. Dexterous helical magnetic robot for improved endovascular access. Sci Robot, 2024; 9(87):eadh0298.

31.

Rucker

, Webster

III . Statics and dynamics of continuum robots with general tendon routing and external loading. IEEE Trans Robot, 2011; 27(6):1033–1044.

32.

Zeng

, Yan

, et al. Modeling a symmetrically-notched continuum neurosurgical robot with non-constant curvature and superelastic property. IEEE Robot Autom Lett, 2021; 6(4):6489–6496.

33.

Dima

, Scheau

, Stefanescu

, et al. Endovascular minimally invasive treatment of the intracranial aneurysms–first 124 cases. J Med Life, 2012; 5(3):360.

34.

Rafii-Tari

, Payne

, Yang

. Current and emerging robot-assisted endovascular catheterization technologies: A review. Ann Biomed Eng, 2014; 42(4):697–715.

35.

Chitalia

, Jeong

, Yamamoto

, et al. Modeling and control of a 2-dof meso-scale continuum robotic tool for pediatric neurosurgery. IEEE Trans Robot, 2021; 37(2):520–531.

36.

Legrand

, Ourak

, Vandebroek

, et al. A large displacement model for superelastic material side-notched tube instruments. Int J Mech Sci, 2021; 197:106329.

37.

, Gao

, Chen

. Anatomy-specific optimization of a multi-contact-aided continuum manipulator. IEEE Trans Med Robot Bionics, 2024; 6(1):84–95.

38.

Gao

, Liu

, Jia

, et al. Multi-axis robotic forceps with decoupled pneumatic actuation and force sensing for cochlear implantation. Nat Commun, 2025; 16(1):1648.

39.

Gao

, Zou

, Wang

, et al. A general friction model of discrete interactions for tendon actuated dexterous manipulators. Journal of Mechanisms and Robotics, 2017; 9(4).

40.

Chitalia

, Wang

, Nguyen

, et al. Design and analysis of a bidirectional notch joint for a robotic pediatric neuroendoscope. In: Proceedings of the 2018 International Symposium on Experimental Robotics. Springer; 2020, pp. 24–33.

41.

Beckers

, Borghesan

, Vander Poorten

. A Quasi-Static Model Accounting for Friction-Induced Hysteresis in Tendon-Actuated Superelastic Notched Tube Instruments. J Med Robot Res, 2024; 09(03n04).

42.

Liu

, Chen

, Liu

, et al. Nonlinear mechanics of flexible cables in space robotic arms subject to complex physical environment. Nonlinear Dyn, 2018; 94(1):649–667.

43.

Chen

, Chen

, Yang

, et al. Modeling and Dynamic Characteristic Analysis of Space Robotic Cables (Flexible Bodies). In: 2019 International Conference on Artificial Intelligence and Advanced Manufacturing (AIAM). IEEE; 2019. pp. 712–715.

44.

Gazzola

, Dudte

, McCormick

, et al. Forward and inverse problems in the mechanics of soft filaments. R Soc Open Sci, 2018; 5(6):171628.

45.

Franklin

. Pnpoly-point inclusion in polygon test. 2025. Available from: https://wrfranklin.org/Research/Short_Notes/pnpoly.html [Last accessed: June 10, 2025].

46.

Harteveld

, Denswil

, Van Hecke

, et al. Data on vessel wall thickness measurements of intracranial arteries derived from human circle of Willis specimens. Data Brief, 2018; 19:6–12.

47.

Morais

, Tavares

JMR

, Queirós

, et al. Development of a patient-specific atrial phantom model for planning and training of inter-atrial interventions. Med Phys, 2017; 44(11):5638–5649.

48.

, Chen

, Li

, et al. A robust graph-based framework for 3-d shape reconstruction of flexible medical instruments using multi-core fbgs. IEEE Trans Med Robot Bionics, 2023; 5(3):472–485.

49.

Wang

, Jiang

, Zhang

, et al. Estimating contact forces along the continuum robot considering actuation. IEEE Sensors J, 2024; 24(22):36798–36805.

Supplementary Material

Please find the following supplemental material available below.

For Open Access articles published under a Creative Commons License, all supplemental material carries the same license as the article it is associated with.

For non-Open Access articles published, all supplemental material carries a non-exclusive license, and permission requests for re-use of supplemental material or any part of supplemental material shall be sent directly to the copyright owner as specified in the copyright notice associated with the article.

0.09 MB

0.10 MB

0.00 MB

0.05 MB