Online Disturbance Estimation for Improving Kinematic Accuracy in Continuum Manipulators

Abstract

Continuum manipulators are flexible robots which undergo continuous deformation as they are actuated. To describe the elastic deformation of such robots, kinematic models have been developed and successfully applied to a large variety of designs and to various levels of constitutive stiffness. Independent of the design, kinematic models need to be calibrated to best describe the deformation of the manipulator. However, even after calibration, unmodeled effects such as friction, nonlinear elastic and/or spatially varying material properties as well as manufacturing imprecision reduce the accuracy of these models. In this paper, we present a method for improving the accuracy of kinematic models of continuum manipulators through the incorporation of orientation sensor feedback. We achieve this through the use of a “disturbance wrench”, which is used to compensate for these unmodeled effects, and is continuously estimated based on orientation sensor feedback as the robot moves through its workspace. The presented method is applied to the HydroJet, a waterjet-actuated soft continuum manipulator, and shows an average of 40% reduction in root mean square position and orientation error in the two most common types of kinematic models for continuum manipulators, a Cosserat rod model and a pseudo-rigid body model.

I. Introduction

CONTINUUM manipulators (CM) have captured attention within the research community for their unique capabilities, including whole-arm grasping and manipulation [1], navigation through complex and unpredictable environments [2], and passive compliance, which can make them safer for human interaction [3]. A variety of different designs for CMs have been proposed by researchers, including tendon-actuated arms [4], manipulators driven by multiple push-pull rods [5], concentric tube robots [6], [7], pneumatic and hydraulic soft manipulators [8], and even robots actuated by the motion of external magnets [9]. While the flexible nature of these robots provides many benefits, it also presents kinematic modeling challenges not seen in traditional rigid-link robotic arms, and as a result, tip accuracies for these robots tend to be lower than that of their rigid counterparts [10].

A number of different types of kinematic models for continuum robots have been developed, ranging in complexity from simple constant curvature models [11] to more elaborate models based on Cosserat rod theory [12]. The choice of modeling approach for a specific manipulator depends on a number of factors, including the mechanism of actuation, the significance of the effect of external loads like gravity, the required level of accuracy, and in some cases computation speed. All of these modeling approaches include parameters which typically require calibration, including material properties and geometric/structural characteristics of the robot [13]. However, even after calibration of these parameters, kinematic accuracy may still be less than desired, due to unmodeled effects such as friction, nonlinear elastic and/or spatially varying material properties, and manufacturing imprecision.

One option for further improving kinematic accuracy is to incorporate some of these effects into the model. For example, for certain designs, researchers have integrated more complex, nonlinear constitutive laws within the models [14]. However, accurately determining these constitutive laws requires tedious material testing processes. Friction and manufacturing uncertainties can also be modeled in principle, but for many designs, these are extremely difficult to accurately describe in practice [15], [16]. In addition, incorporating these effects increases overall model complexity and can significantly increase the number of parameters, thereby requiring an even more complex calibration procedure.

Another approach to improving kinematic accuracy is to incorporate feedback from sensors. One of the most commonly used method is to embed strain sensors along the principal bending directions and then use the kinematic model to predict the shape of the manipulator based on strain measurement [17], [18]. This approach, however, is not suitable for all types of continuum manipulators, as these sensors do not scale down well to small designs and require precise alignment, complicating the fabrication process. Another common method is to use fiber optic shape sensors [19], [20]. However, this method begins to lose accuracy at high curvatures due to propagation losses, and cannot be implemented for many low-stiffness designs, as the fiber optic sensor is often stiffer than the manipulator itself. As an alternative which does not require any mechanical interaction between the robot and the sensors, much work has focused on reconstructing the shape from an external camera [21], [22]. The use of an external camera, however, can become problematic when direct visualization is limited by occlusions, such as during operation in a confined environment.

In this paper, we present a new method to improve the accuracy of the kinematic model for CMs by utilizing sensory information from two inclinometers, one at the base of the manipulator and one at the tip. These sensors avoid the problem of occlusions associated with camera-based sensing modalities, while remaining compatible with low-stiffness CMs undergoing large deflections, as the sensors themselves do not need to bend with the structure of the CM. In addition, this type of sensor is inexpensive, making it well-suited to cost-sensitive applications. Utilizing this sensor feedback, our approach is to include a “disturbance” parameter in the model, which can compensate for a variety of unmodeled effects, and then calibrate this parameter in real time, based on the sensor data. The disturbance is in the form of an external point wrench, which is not meant to describe any particular external loads or actuation forces, both of which are considered separately within the CM modeling framework. The update of the disturbance can be performed by iteratively sensing the end effector’s orientation through the inclinometers. Since the disturbance represents a set of additional model parameters, it is possible to numerically evaluate a sensitivity matrix that expresses how changes in the disturbance affect the end effector’s angular velocity. The sensitivity matrix can then be inverted to update the disturbance estimate. This can then be applied back to the model, with the process being repeated iteratively to find a disturbance wrench that minimizes orientation error.

The contributions of this paper can be summarized as follows. First, we propose a calibration method for CMs that considers the addition of a disturbance wrench to compensate for unmodeled effects such as friction, nonlinear elasticity, non-uniform geometric and material properties, and inaccuracies in manufacturing and assembly. Second, we propose a method to update the disturbance wrench using the absolute orientation provided by two inclinometers located at the proximal and distal ends of the manipulator through the use of the sensitivity matrix. Lastly, to demonstrate the presented method on a physical robot, we present a case study on a soft continuum robot actuated by waterjets at the tip and undergoing large deflections in three dimensional space. The improvement in kinematic model accuracy is seen for both a kinematic-based model (Pseudo Rigid Body (PRB) Model) and a mechanics-based model (Cosserat Model), which are the two most commonly used frameworks for CMs.

II. Disturbance Estimation Approach

In this section, we describe the estimation procedure used to find a disturbance wrench which minimizes the tip rotation error associated with the kinematics. We approach this problem by first calibrating geometric and material properties of the manipulator with the additional disturbance wrench parameter to find a solution for the manipulator parameters which is optimal across the initial calibration data set. Then, during robot operation, the calibrated value of the tip disturbance is iteratively changed by using sensory information to minimize the orientation error at the current position within the workspace.

To describe the disturbance estimation approach, we begin by assuming a general kinematic model for the continuum robot which can be expressed in the form:

$$g=f(q,\zeta )$$ (1)

where g represents the pose of the end effector as a function of the actuation inputs q, and a set of manipulator’s parameters ζ = [η w_d]^T composed by geometric and material properties of the manipulator η, (such as length, Young’s Modulus, etc.), and by the disturbance wrench w_d. The nominal mapping between end effector pose and actuator inputs can be used to calibrate the manipulator’s parameters by using the scalar product of end effector absolute orientations as the error metric [23]. After calibration, the resulting parameters (indicated as ζ*) are used to estimate the equilibrium pose of the end effector that corresponds to the inputs q.

A. Iterative Disturbance Estimation

The sensing of absolute orientations can help to correct the rotation error iteratively through the use of the sensitivity matrix:

$$S_{\zeta *}={\left(g^{-1}\frac{\partial g}{\partial \zeta *}\right)}^{\vee }$$ (2)

which maps body frame end-effector velocities to changes in the manipulator’s calibrated parameters ζ*. Independent of which kinematic modelling approach is used, the sensitivity matrix can be computed numerically using a finite difference approximation [24]. Each column can be computed as:

$$S_j\approx {\left(g^{-1}\frac{g_j-g}{\Delta {\zeta }_i}\right)}^{\vee }$$ (3)

where g_j is the homogeneous transformation describing the end effector pose obtained after perturbing g with a small change in the i^th calibrated parameter by Δζ_i. Using this method, each column of the sensitivity matrix can be obtained after 2 solutions of the kinematic equations.

The rotation error is iteratively minimized by using a gradient descent approach where the change in the disturbance wrench ${\stackrel{.}{w}}_d{|}_m$ is calculated by:

$${\stackrel{.}{w}}_d{|}_m=\lambda \left(S_{w_d}^T{\left(S_{w_d}{S}_{w_d}^T-\mu I_3\right)}^{-1}\right){\omega }_{\mathit{\text{des}}}$$ (4)

where S_wd represents the columns of the sensitivity matrix which map changes in the disturbance parameter to changes in the tip orientation, ω_des can be calculated by using the orientation error between the current estimated rotation Rⁱ and the measured value from the sensors R_m expressed in body coordinate, λ represents a gain correction coefficient which can be experimentally tuned, μ is a damping factor and I₃ is the identity matrix (Fig. 1). The calculated change can then be added to the previously estimated value of the disturbance and applied to the model $w_d^{(i+1)}=w_d^{(i)}+{\stackrel{.}{w}}_d$ where ${\stackrel{.}{w}}_d={\left[{\stackrel{.}{w}}_d{|}_f{\stackrel{.}{w}}_d{|}_m\right]}^T$ and ${\stackrel{.}{w}}_d{|}_f=0$.

Fig. 1.

Iterative disturbance estimation algorithm. [A] Block diagram illustrating the iterative update of the disturbance based on inclinometer data. The initial value for $w_d^{(i)}$ is obtained after initial calibration. [B] The compliance matrix S_wd is used to minimize the orientation error between current estimated orientation R⁽ⁱ⁾ and the measured value from the sensors R_m.

III. Case study: The Hydrojet System

The HydroJet Endoscopic Device (HJ) was introduced in [25]. It is a soft continuum endoscope that addresses the need for upper GI cancer screening in Low and Middle Income Countries (LMIC) and remote areas. The device uses three miniature waterjet actuators, spaced 120 degrees apart around the diameter (Fig. 2), to directly maneuver the camera located at the tip. The pressurized water is carried through a multilumen catheter that is connected directly to three solenoid valves which control the flow to each nozzle. The deflection is facilitated by a soft elastomer sleeve (Ecoflex 00–30), located at the distal end of the multilumen catheter, which wraps around three flexible single lumen tubes that are connected to the capsule. This structure is designed to be soft and flexible to allow for high curvatures and thereby large deflections of the capsule. By controlling each jet’s actuation force individually, the deflection generated by the three waterjets creates a resultant motion in two DoFs. The net applied wrench in the body frame acting on the tip of the manipulator due to the waterjet forces is defined as:

$$\begin{gathered} F_a{|}_{xy}=A\left[\begin{array}{l}{q}_1\hfill \\ q_2\hfill \\ q_3\hfill \end{array}\right] \\ A=\left[\begin{array}{ccc}\text{sin\hspace{0.17em}}\left(\frac{\pi }{3}\right)& 0& -\text{sin\hspace{0.17em}}\left(\frac{\pi }{3}\right)\\ \text{cos\hspace{0.17em}}\left(\frac{\pi }{3}\right)& -1& \text{cos\hspace{0.17em}}\left(\frac{\pi }{3}\right)\end{array}\right] \end{gathered}$$ (5)

where q₁, q₂, and q₃ are the three applied forces due to the water jets and F_a|_xy represents the x and y components of the tip force in the frame of the tip of the manipulator, as defined in Fig. 2. Since the three actuators are all coplanar with the tip of the endoscope, the z component of the force is zero.

Fig. 2.

Mechanical deflection for the HydroJet endoscopic device expressed in both PRB and Cosserat frameworks. The waterjet actuators are spaced 120 degrees around the capsule diameter and modeled as a tip follower force.

IV. Kinematic Modelling for the Hydrojet System

In this section we present two commonly used kinematic models for continuum manipulators and describe how they can be applied to decribe deflection of the HydroJet Device.

A. Pseudo-Rigid Body Kinematics

The PRB model is based on the approximation of subdividing the elastic body of the continuum manipulator into a series of rigid links connected by conventional revolute, universal, or spherical joints. In this case, the continuum structure is modeled as n + 1 uniformly spaced rigid links connected by n spherical joints (Fig. 2 (B)) [26]. The rotation of each joint i with respect to the orientation of the previous joint i − 1 is described using a rotation vector ω_iθ_i = ω_ixθ_ix + ω_iyθ_iy + ω_izθ_iz, where ω_ix, ω_iy, ω_iz are the three orthonormal axes attached to the ith joint and θ_ix, θ_iy, θ_iz are the rotation angles around each axis. The resulting rotation axis and magnitude of rotation are indicated with ω_i and θ_i respectively. Considering that the magnitude and the direction of rotation of the ith joint can be calculated as ϕ_i = ‖θ_i‖ and ω_i = θ_i/‖θ_i‖, the corresponding twist, ${\widehat{\xi }}_i\in se(3)$ results in:

$${\widehat{\xi }}_i=\left[\begin{array}{cc}{\widehat{\omega }}_i& {\omega }_i\times q_i\\ 0& 0\end{array}\right]$$ (6)

where q_i is a vector pointing from the origin to any location on the axis ω_i, and ${\widehat{\omega }}_i\in so(3)$ is the skew-symmetric matrix:

$${\widehat{\omega }}_i=\left[\begin{array}{ccc}0& -{\omega }_{iz}& {\omega }_{iy}\\ {\omega }_{iz}& 0& -{\omega }_{ix}\\ -{\omega }_{iy}& {\omega }_{ix}& 0\end{array}\right].$$ (7)

The homogeneous transformation matrix of each joint can be determined using the twists of equation (6), the rotation angle ϕ_i and the product of exponentials formula [27]:

$$g_{si}(\theta )=e^{{\widehat{\xi }}_1{\varphi }_1}{e}^{{\widehat{\xi }}_2{\varphi }_2}\dots e^{{\widehat{\xi }}_i{\varphi }_i}{g}_{si}(0)$$ (8)

where g_si(θ) ∈ SE(3) is the configuration of joint i in the space frame for joint angles [θ₁ θ₂ … θ_i]^T and g_si(0) represents the initial configurations of joint i.

The relationship between the internal bending moment and deflection angle at each joint i can be modeled as:

$$\begin{gathered} {\tau }_i=K_i{\omega }_i{\theta }_i \\ =\left[\begin{array}{ccc}{k}_{i,x}& 0& 0\\ 0& k_{i,y}& 0\\ 0& 0& k_{i,z}\end{array}\right]\left[\begin{array}{c}{\omega }_{ix}{\theta }_{ix}\\ {\omega }_{iy}{\theta }_{iy}\\ {\omega }_{iz}{\theta }_{iz}\end{array}\right]. \end{gathered}$$ (9)

where ${\tau }_i\in {ℝ}^3$ is a vector representing the internal bending moment and K_i is the stiffness matrix of joint i with components in x, y and z directions. The effective torsional spring constant can be found in terms of the elastic material properties by comparing spring energy and strain energy due to bending, as explained in [20]:

$$k_{i,x}=l_c/L\frac{2E_{L,x}I}{l_s}+\left(1-l_c/L\right)\frac{2E_{0,x}I}{l_s}$$ (10)

where l_s is the length of the ith link, l_c ∈ [0, L] is an incremental counter with l_s increments, and E is varying along the length of the body, with E_0,x and E_L,x representing the Young’s modulus of the joints i in the direction x at l = 0 and l = L, respectively. A similar expression can be found for y and z directions.

The Jacobian matrix can be calculated using [26], [27]:

$$J_{si}(\theta )=\left[{\xi }_1^{†}\hspace{1em}\dots \hspace{1em}{\xi }_i^{†}\hspace{1em}0\hspace{1em}\dots \hspace{1em}0\right]$$ (11)

where

$$\begin{gathered} {\xi }_1^{†}=\left[{\xi }_{i,x}^{†}\hspace{1em}{\xi }_{i,y}^{†}\hspace{1em}{\xi }_{i,z}^{†}\right] \\ {\xi }_{i,j}^{†}=A{d}_{\left(e^{{\xi }_i{\varphi }_i}\dots e^{{\xi }_n{\varphi }_n}{g}_{si}(0)\right)}^{-1}{\xi }_{i,j} \end{gathered}$$ (12)

The Jacobian for the end-effector can be expressed as:

$$J_{sn}(\theta )=\left[{\xi }_1^{†}\hspace{1em}\dots \hspace{1em}{\xi }_{n-1}^{†}\hspace{1em}{\xi }_n^{†}\right].$$ (13)

The mapping of forces applied on the body (i.e distributed loads and actuations) are modeled through the manipulator Jacobian. The effect of a distributed load on the body can be modeled as:

$${\tau }_f=\sum _{i=1}^n{J}_{si}{(\theta )}^T\left[\begin{array}{c}\overline{w}\\ 0_{3\times 1}\end{array}\right]$$ (14)

where $\overline{w}$ represents a distributed force expressed in body frame at the ith joint. The effect of the actuators can be modeled as

$${\tau }_a=J_{sn}{(\theta )}^T\left[\begin{array}{c}{F}_a+w_d{|}_f\\ M_a+w_d{|}_m\end{array}\right]$$ (15)

where F_a represents force and M_a moment due to the actuators, w_d|_f represents the disturbance force at the tip and w_d|_m represents the disturbance wrench.

The configuration vector θ is obtained by equalizing internal and external moments applied on the body. The solution to this equation cannot be obtained analytically, and is thus obtained through the following minimization:

$$\underset{\theta }{\text{minimize}}K\omega (\theta )-{\tau }_f(\theta )-{\tau }_a(\theta ,q)$$ (16)

The first term in the objective function is the internal moment due to the stiffness of the body, where Kω(θ) is the internal bending moment of the whole body. The second and third terms represent the external moments acting on the body, which are related to the configuration vector θ through the manipulator Jacobian, and q represents the actuation forces (e.g. forces applied by the water jets).

B. Cosserat Rod Kinematics

A Cosserat rod-based kinematic model for continuum manipulators has been derived in [12]. In this section, we review the model using the notation provided in [7]. The centerline of an unloaded rod is expressed using a homogenoeous reference frame consisting of an arc length-parameterized space curve $\tilde{p}(s)\in {ℝ}^3$ and a rotation matrix $\tilde{R}\left(s\right)$, with s denoting arc length. Under an external load, the parametric curve $\tilde{p}(s)$ deflects to the curve p(s), and $\tilde{R}\left(s\right)$ rotates to R(s) (Fig. 2 (A)). The homogeneous frames (g(s)), defined by:

$$g(s)=\left[\begin{array}{cc}R(s)& p(s)\\ 0^T& 1\end{array}\right]$$ (17)

are assigned so that the z-axis of each frame is tangent to the curve. The rotation of each point on the rod may be expressed in terms of a local curvature vector u(s), which can be found using the relationship [27]:

$$u(s)={\left(R^T(s)R^{\prime }(s)\right)}^{\vee }$$ (18)

where the operator ^∨ denotes conversion of an element of so(3) to its corresponding element in ${ℝ}^3$. The constitutive relationship between the curvature vector and internal moments, expressed in the local coordinate frame at s, is:

$$M_b(s)=K(u(s)-\tilde{u}\left(s\right))$$ (19)

where M_b(s) is the internal moment and K represents the stiffness matrix, defined as:

$$K=\left[\begin{array}{ccc}{E}_x{I}_x& 0& 0\\ 0& E_y{I}_y& 0\\ 0& 0& G_J\end{array}\right]$$ (20)

where E_x, E_y are the the Young’s modulus, I_x and I_y are the second moments of area of the tube cross-section, G is the shear modulus and J is the polar moment of inertia of the tube cross-section. The Young’s modulus is considered to be linearly varying along the length of the body going from a value E₀ at the base to E_L at the tip following the equations: E = sE_L,x + (1 − s)E_0,x. A similar expression can be found for the y and z directions. The resulting Cosserat rod equations that govern the rod shape expressed in local frame coordinates are:

$$\begin{gathered} g^{\prime }(s)=g(s)\widehat{\xi }(s) \\ {n}_b^{\prime }(s)=-\widehat{u}{n}_b(s)-\overline{w} \\ {u}^{\prime }(s)={\tilde{u}}^{\prime }(s)-K^{-1}(\left(\widehat{u}K+K^{\prime }\right)(u(s)-\tilde{u}(s))+{\widehat{e}}_z{n}_b(s)) \end{gathered}$$ (21)

where n_b is the internal force, $\overline{w}$ is the distributed force and $\widehat{u}$ and ${\widehat{e}}_z$ are the skew-symmetric versions of vectors e_z and u, respectively. In the equations listed above, we have ignored the contribution of distributed moments along the body.

In the Cosserat framework, forces and moments applied at the tip represent boundary conditions on equation (21). The boundary conditions at s = L can be expressed as:

$$\begin{gathered} n_b(L)=F_a+w_d{|}_f \\ u(L)-\tilde{u}(l)=K^{-1}\left(M_a+w_d{|}_m\right) \end{gathered}$$ (22)

while $\overline{w}$ is already included in eq. (21). The boundary conditions at s = 0 are:

$$\begin{array}{l}p(0)={\left[\begin{array}{lll}0\hfill & 0\hfill & 0\hfill \end{array}\right]}^T\hfill \\ R(0)=I\hfill \end{array}.$$ (23)

and solution of the resulting boundary value problem provides the full pose of the manipulators as a function of arc length.

V. Experimental Validation

A. HydroJet System Testbed

The testbed was assembled as depicted in Fig. 3. The capsule was manufactured from a durable plastic (Clear resin, FormLabs, Sommerville, MA, USA) through rapid prototyping. The capsule’s outer diameter and length are 9.8 mm and 28 mm, respectively. A 6 DoF electromagnetic sensor (EM) (Northern Digital Inc., Canada), with 0.48 mm and 0.70 deg accuracy, was integrated within the capsule, with its frame aligned to the capsule in a known orientation. The wire of the electromagnetic sensor runs through the soft elastomer sleeve together with the three single-lumen water lines connecting the tip to the base connector. The base was connected to three solenoid water valves (A352273, Asco Numatics, USA) using standard hydraulic tubing (1/16”ID × 1/8”OD Tygon E-3603, Cole-Parmer, USA). The base connector was held in the desired orientation using a custom 3D printed holder. The assembly was secured in place through attachment to an aluminum frame (Rexroth, Bosch, Germany). A second 6-DoF EM sensor was attached to the base holder to sense the orientation of this frame as well and compute the direction of gravity with respect to the capsule and tether.

Fig. 3.

Experimental bench test setup consisting of the Hydrojet Endoscopic Device. A 3D printed holder anchors the base of the device while the waterjets at the tip cause deflection of the soft elastomer body.

B. Initial Calibration

The purpose of calibration is to find values for the parameters of the model that best describe the deflection of the manipulator for a set of N poses distributed throughout the workspace of the manipulator. This means that the intrinsic parameters found during calibration may not be the optimal for a single actuation value. The parameters of both models are optimized such that the rotation at the tip and at the base closely approximate those of the experimental data under the same set of actuations. In these experiments, the data used for calibration consist of a set of 40 randomly distributed equilibrium configurations within the workspace. The overall collection time was around 3 min and 30 sec. The parameter optimization problem is defined as follows:

$$\underset{\zeta }{\text{minimize}}\sum _{i=1}^{40}\Vert \left(\langle h_m\left(q_i,\zeta \right),h_i\rangle \right){\Vert }^2$$ (24)

where h_m = a + bi + cj + dk represents the quaternion associated with the rotation of the tip with respect to the base, as computed by solving for the equilibrium configuration (eq. (16) for the PRB model and eqs. (21), (22), (23) for the Cosserat model) with inputs q_i and manipulator parameters ζ. Similarly, h_i represents the quaternion associated with the measured experimental rotation, which is obtained by calculating the relative rotations of the two EM sensors. The operator < · > represents the scalar product of two vectors and is chosen as the distance metric between two elements of SO(3) [23].

Assuming the soft elastomer body to have a cylindrical shape with radially isotropic properties results in E_L,x = E_L,y = E_L, E_0,x = E_0,y = E₀ and G_L = E_L/2γ and G₀ = E₀/2γ, where γ is the Poisson’s ratio. The manipulator’s parameters that we seek to calibrate are then ζ = [E₀ E_l L wd]^T. The results of calibration using both models are summarized in Table I.

TABLE I.

Calibrated values of model parameters

Parameter	PRB Model (Value)	Cosserat Model (Value)
E0	2.35 · 105 N/m2	3.00 · 105 N/m2
El	1.68 · 105 N/m2	2.00 · 105 N/m2
L	0.0705 m	0.0698 m
ωd,x|f	−0.99 · 10−3 N	−1.54 · 10−2 N
ωd,y|f	0.02 · 10−3 N	−9.07 · 10−3 N
ωd,z|f	0.01 · 10−3 N	2.07 · 10−2 N
ωd,x|m	−0.04 · 10−3 Nm	2.71 · 10−2 Nm
ωd,y|m	−0.15 · 10−3 Nm	−2.33 · 10−2 Nm
ωd,z|m	4.69 · 10−3 Nm	−1.74 · 10−1 Nm

C. Constant Disturbance Results

The optimal value for the manipulator parameters ζ* was obtained for both PRB and Cosserat models using the calibration procedure described in the previous section. To validate the accuracy of both models after calibration, the soft manipulator was moved throughout its workspace by gradually increasing the actuation force of one waterjet at a time until the maximum force was reached. This resulted in three sweeping motions, shown in different colors in Fig. 4. During this motion, the orientations of the base and the tip of the manipulator were recorded through the EM sensors. The force generated by the waterjet was directly controlled by input signals to the solenoid valves. To facilitate quasi-static motion, each command was sent with a delay of 5 seconds, which is higher than the settling time of the body to its equilibrium. This delay time is primarily a requirement of the actuator dynamics for this particular robot design. The sweeping motion corresponding to each of the three jets was repeated three times, for a resulting total of nine separate motions. For each actuation force, each model was evaluated with the parameters obtained during calibration (ζ*) to compute the end-effector pose. The results are shown in Figs. 4 and 5. The position error (‖ϵ‖₂) is defined as the Euclidean norm of the difference between model-predicted tip position and measured tip position obtained from the EM sensors, while the rotation error Δψ is calculated as in section V-B. To enable similar discretization of the body of the manipulator for both frameworks, the length of the links for the PRB model was chosen to be the same as the integration step size used for the Cosserat model (20 links in total were chosen for the PRB model). This allows for a more direct correlation between analogous parameters in the two models. The root-mean-square (RMS) and standard deviation (SD) position and orientation error resulting from the nine sweeps was 0.69 cm ± 0.20 cm with 6.85 deg ± 3.41 deg for the PRB and 0.51 cm ± 0.20 cm with 5.66 deg ± 4.21deg for the Cosserat model, showing similar behavior of the two modelling frameworks.

Fig. 4.

Experimental validation of the calibrated coefficient ζ*. Each sweep is marked with a different color. The experimental deflection (dots) is projected on xy, xz, yz planes together with the corresponding position estimated from both PRB (black dashed line) and Cosserat (black continuous line) frameworks.

Fig. 5.

Box Plot Comparison of PRB and Cosserat Models. Position and orientation errors with constant disturbance wrench.

D. Iterative Disturbance Estimation Results

The same sweeping motions described in the previous section were used with online adjustment of the disturbance wrench based on sensor orientation. The disturbance wrench was iteratively adjusted following the gradient descent minimization approach described in Section 3 using a gain correction coefficient of λ = 0.4. The local minimum was generally reached after one to two iterations per time step. Results are shown in Fig. 6. Compared to the constant disturbance approach, the iterative estimation reduces the orientation error by 43%, going from an average error of 6.85 deg to 3.96 deg in the case of PRB model, and by 42%, from 5.66 deg to 3.29 deg for the Cosserat model. While the error in orientation is significantly reduced, there is no significant variation in positional error, which remains confined within the same range of the values observed for the PRB model without online disturbance estimation, and is slightly improved for the Cosserat model. In Table II, the RMS errors calculated using the iterative estimation method (Proposed) are compared to the standard geometric calibration (Ref.). The proposed method significantly improves both positional and orientation error over the standard calibration method showing 42% positional error reduction (from 17.9% of the robot length to 10.7%) and 32% orientation error reduction for 75° deflection of the end effector.

Fig. 6.

Box plot showing error distribution and variability over the nine sweeping motions. The iterative estimation of the disturbance wrench using sensor data allows for significant reduction in the rotation error for both modelling frameworks.

TABLE II.

RMSE Position and Orientation of Iterative Disturbance Estimation compared to Standard Geometric Calibration

	Bending Angle		10°	25°	50°	75°
PRB Model	Position RMSE (cm)	Ref.	0.44	0.75	1.16	1.34
		Proposed.	0.66	0.69	0.74	0.79
	Orientation RMSE (deg)	Ref.	9.06	9.89	10.02	10.75
		Proposed.	1.74	3.18	5.18	7.07
Cosserat Model	Position RMSE (cm)	Ref.	0.52	0.78	1.01	1.25
		Proposed.	0.37	0.47	0.56	0.75
	Orientation RMSE (deg)	Ref.	9.17	9.81	9.27	10.04
		Proposed.	0.82	2.46	3.96	7.18

VI. Discussion

In this paper, we have presented a new method for using sensory feedback from orientation sensors to improve the kinematic modeling accuracy for continuum manipulators. We achieve this by incorporating a “disturbance wrench” to account for unmodeled phenomena, which is continually updated based on the sensor data. While this method could in principle be used to estimate external loads due to contact with the environment, force estimation and contact detection are substantial research problems in their own right, and typically depend on the design and actuator methodology of the specific CM. The validation of our method with external environmental forces is thus left for future work. In addition, future work will focus also on the integration of this method into a Jacobian-based motion controller; the improved modeling accuracy demonstrated in this work is expected to improve trajectory following abilities as well.

The improved modeling and control accuracy which this approach can enable is expected to be highly valuable, as improving kinematic modelling accuracy is still a very active area of research for CMs in particular. For the HydroJet System described in Section III, we anticipate that this improved accuracy will enable medical providers to more precisely control the device within the stomach during upper gastrointestinal endoscopy. It is expected that this approach could provide similar benefits for a wide range of soft robotics applications, both in and outside of the medical field. In this paper we have used EM sensors to calculate absolute orientations; however, these sensors can be replaced by cheaper inertial motion units (IMUs) without changing the proposed disturbance estimation method. Absolute orientation can be obtained from IMUs through an orientation filter, as the one proposed in [28], which provides static accuracy comparable to that of the EM sensors (0.6 degrees). While IMUs are well suited to the HydroJet device, integrating even these low cost sensors will not be practical for all applications, making the need for adding sensors one of the primary limitations of our method.

VII. Conclusion

This paper presents a method for augmenting the kinematic models for continuum manipulators with information from orientation sensors in order to improve model accuracy. The method is based on online calibration of a “disturbance wrench” used to account for various unmodeled phenomena. We apply this method in particular to continuum manipulators actuated by a tip follower wrench and experiencing external loading, and demonstrate the performance of the method on a soft continuum endoscope actuated by waterjet propulsion. Our results indicate that the method is effective for both PRB and Cosserat rod modeling frameworks.