Toward On-line Parameter Estimation of Concentric Tube Robots Using a Mechanics-based Kinematic Model

Abstract

Although existing mechanics-based models of concentric tube robots have been experimentally demonstrated to approximate the actual kinematics, determining accurate estimates of model parameters remains difficult due to the complex relationship between the parameters and available measurements. Further, because the mechanics-based models neglect some phenomena like friction, nonlinear elasticity, and cross section deformation, it is also not clear if model error is due to model simplification or to parameter estimation errors. The parameters of the superelastic materials used in these robots can be slowly time-varying, necessitating periodic re-estimation. This paper proposes a method for estimating the mechanics-based model parameters using an extended Kalman filter as a step toward on-line parameter estimation. Our methodology is validated through both simulation and experiments.

I. INTRODUCTION

Concentric tube robots are a type of continuum robot composed of thin superelastic pre-curved tubes. They have been developed in the context of both steerable needles and minimally invasive surgical robots [1], [2], [3], [4]. Similar to traditional robot architectures, kinematic model calibration is a necessary condition for achieving high positioning accuracy. The kinematic models of traditional robots, comprised of relatively rigid links connected by revolute and prismatic joints, pose fewer challenges, however, in parameter calibration.

While traditional robot kinematics can be modeled by algebraic equations, concentric tube robot forward kinematics is modeled by a boundary value problem (BVP). In addition, phenomena such as friction, nonlinear elasticity, and cross section deformation are neglected in formulating this BVP. These phenomena can also be time varying. Consequently, initial calibration of a robot tube set is difficult; its accuracy can vary over the workspace and depend on the data set used for calibration; and the robot can require periodic recalibration to accommodate changes in tube properties, e.g., pre-curvature.

On-line parameter estimation provides a means to both monitor model accuracy as well as to tune the model during operation. Such real-time tuning can enhance the performance of position control by using accurate feedforward control which is usually combined with feedback control. Prior work on this topic has considered using an algebraic functional approximation [1] for the mechanics instead of the mechanics-based model [5]. The advantage of this approach is that the number of parameters can be easily varied to potentially account for neglected phenomena and on-line estimation can be implemented as recursive least squares [5].

These advantages have yet to be proven, however, and on-line estimation using a mechanics-based model may provide advantages of its own. For example, estimation of mechanics-based parameters can provide physical insights that a functional approximation would not. Furthermore, the comparatively small and fixed number of mechanics-based parameters guards against overfitting the data - tuning the parameters to fit data in a small neighborhood at the expense of global accuracy. In addition, despite its mathematical complexity, careful implementation may enable real-time mechanics-based model estimation. And as additional phenomena are added to the mechanics-based model, the approach developed here can be extended to include them.

The estimation problem considered here, as shown in Fig. 1, is defined by the relationship between sensor-measured tip configuration and that predicted by the kinematics model,

$$\left[\begin{array}{cc}{R}_m& p_m\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}{h}_R(q,\varphi )& h_p(q,\varphi )\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{R}_o& p_o\\ 0& 1\end{array}\right]$$ (1)

where p_m ∈ ℝ³, R_m ∈ SO(3) are the measurements of tip position and orientation and the nonlinear functions $h_p:{\mathbb{R}}^{2n}\times {\mathbb{R}}^{N_p}\to {\mathbb{R}}^3$ and $h_R:{\mathbb{R}}^{2n}\times {\mathbb{R}}^{N_p}\to SO(3)$ are derived from the forward kinematic model. $\varphi \in {\mathbb{R}}^{N_p}$ denotes the model parameters to estimate, e.g., stiffness tensor K_i, pre-curvature û_i and Poission’s ratio, ν_i. The kinematic input variables, q ∈ ℝ²ⁿ, are the base rotation θ_i,b and base translation l_i of each tube. The estimation problem also includes solving for a constant offset displacement {R_o, p_o} relating the robot tip and sensor frames.

Fig. 1.

Estimation problem. Actual robot shown in solid and kinematic model prediction is shown dashed. Frame {W} is fixed world frame. Frame {F_i,b} (i = 1, …, n) is the tube-fixed base frame of the i-th tube. Constant displacement {R_o, p_o} relates robot tip and sensor location.

The contribution of the paper is to derive a recursive filtering technique for estimating the parameters of the mechanics-based model based on an Extended Kalman Filter (EKF). Implemented in Matlab, its capabilities for parameter estimation are evaluated off line using simulated and experimentally-collected data sets.

The remainder of the paper is arranged as follows. In Section II, we revisit the forward kinematics of concentric tube robots briefly, and derive the differential kinematics with respect to general parameters which include both physical parameters and control variables. Next, an Extended Kalman Filter (EKF) is formulated to estimate both the model parameters and the offset between tip frame and measurement frame. In Section IV, parameter estimation using measurement of tip position and orientation is demonstrated using simulation and experimental data. Conclusions appear in Section V.

II. KINEMATICS OF CONCENTRIC TUBE ROBOTS

The forward kinematic equations, i.e., solving for the entire shape of the robot given the input variables and parameters, are derived in [6] with the consideration of external forces and moments. Using the frame definitions and equations (1)–(22) of [6], we define the state variable for an n-tube robot as $y={[{\theta }_i^T{u}_{iz}^T{m}_0^T{n}_0^T]}^T\in {\mathbb{R}}^{N_s}$ in which θ_i, u_iz, m₀, n₀ are twist angle, torsional strain of the i-th tube, net bending moment, and net shear force respectively. The state equation is $\dot{y}=f(s,y,\mathrm{\Omega })$ (refer to (16)–(19) in [6]), where N_s = 2n + 5 is the number of state variables [6], s ∈ [0, L] is the arc length parameter. In anticipation of deriving the differential kinematics with respect to input variables, model parameters and boundary conditions, we define Ω = [ϕ^T, q^T, w^T ]^T in which w is any wrench applied at the tip. Throughout the paper, the upper dot is defined as the derivative with respect to s.

A. Differential Kinematics

The differential kinematics are needed for parameter estimation and can also be used for inverse kinematic control. While they can be calculated by finite difference, this approach is slower and less accurate than analytical calculations. Furthermore, it has been previously demonstrated that computation of Jacobian and compliance matrices can be formulated as an initial value problem(IVP) [7]. That approach is generalized here to derive the Jacobian with respect to arbitrary parameters.

Assume the tip of concentric tube robot is at the n-th tube. Then the column of body Jacobian matrix with respect to arbitrary parameter set $\mathrm{\Omega }\in {\mathbb{R}}^{N_p}$ is given as [8]:

$$J_{\mathrm{\Omega }}={(g_n^{-1}\frac{d{g}_n}{d\mathrm{\Omega }})}^{\vee },$$ (2)

where g_n ∈ SE(3) is the frame of the n-th tube, and the ∨ operator changes an element in se(3) from its matrix form to vector form [8]. After some algebra similar to the one in [7], the differential equation for J_Ω is given from differentiating (2) with respect to s near the solution y^*(s):

$$\begin{array}{l}{\dot{J}}_{\mathrm{\Omega }}=-\left[\begin{array}{cc}[u_n]& 0\\ [v_n]& [u_n]\end{array}\right]J_{\mathrm{\Omega }}+\left[\begin{array}{c}\frac{d{u}_n}{d\mathrm{\Omega }}\\ 0\end{array}\right],\\ J_{\mathrm{\Omega }}(0)={(g_n^{-1}(0)\frac{d{g}_n}{d\mathrm{\Omega }}(0))}^{\vee },\end{array}$$ (3)

where u_n (s), v_n(s) ∈ℝ³ are the angular and linear strain (rate of change with respect to arc length) of the n-th tube, and the bracket [] makes a vector x ∈ ℝ³ to a skew symmetric matrix [8]. As the solution of (3) we obtain J_Ω(s), J_Ω as a function of arc length s:

$$J_{\mathrm{\Omega }}(s)=[A{d}_{g_n{(s)}^{-1}}]J_{\mathrm{\Omega }}(0)+J_{\mathbf{b}}(s){\mathbf{b}}_{\mathrm{\Omega }}+{\mathbf{c}}_{\mathrm{\Omega }}(s),$$ (4)

where ${\mathbf{b}}_{\mathrm{\Omega }}=\frac{dy}{d\mathrm{\Omega }}(0)$, [Ad_g] is the adjoint matrix of g ∈ SE(3). J_b(s) ∈ ℝ^6×N, ${\mathbf{c}}_{\mathrm{\Omega }}(s)\in {\mathbb{R}}^{6\times N_p}$ are the solutions of following IVPs ( $\mathrm{\Phi }(s,0)\in {\mathbb{R}}^{N_s\times N_s}$, ${\mathbf{a}}_{\mathrm{\Omega }}(s)\in {\mathbb{R}}^{N_s\times N_p}$ are also necessary):

$${\dot{J}}_{\mathbf{b}}=-\left[\begin{array}{cc}[u_n]& 0\\ [v_n]& [u_n]\end{array}\right]J_{\mathbf{b}}+\left[\begin{array}{c}\frac{\partial u_n}{\partial y}\mathrm{\Phi }(s,0)\\ 0\end{array}\right],$$ (5)

$${\dot{\mathbf{c}}}_{\mathrm{\Omega }}=-\left[\begin{array}{cc}[u_n]& 0\\ [v_n]& [u_n]\end{array}\right]{\mathbf{c}}_{\mathrm{\Omega }}+\left[\begin{array}{c}\frac{\partial u_n}{\partial y}{\mathbf{a}}_{\mathrm{\Omega }}+\frac{\partial u_n}{\partial \mathrm{\Omega }}\\ 0\end{array}\right],$$ (6)

$$\dot{\mathrm{\Phi }}(s,0)=\frac{\partial f}{\partial y}\mathrm{\Phi }(s,0),$$ (7)

$${\dot{\mathbf{a}}}_{\mathrm{\Omega }}=\frac{\partial f}{\partial y}{\mathbf{a}}_{\mathrm{\Omega }}+\frac{\partial f}{\partial \mathrm{\Omega }},$$ (8)

with initial conditions J_b(0) = 0, c_Ω(0) = 0, Φ(0, 0) = I, a_Ω(0) = 0. The vector b_Ω can be found from separated boundary conditions(BCs) at base (s = 0) and at tip (s = L). After solving IVPs in (7), (8), derivative of BC at tip is given as

$$\frac{dy}{d\mathrm{\Omega }}(L)=\mathrm{\Phi }(L,0)\frac{dy}{d\mathrm{\Omega }}(0)+{\mathbf{a}}_{\mathrm{\Omega }}(L).$$ (9)

As our BCs are known separately, this is only a linear equation, and one can find N_s unknowns with given N_s equations. If we solve this linear equations and gather $\frac{dy}{d\mathrm{\Omega }}(0)$, this will be our b_Ω. Note that if N_k elements of $\frac{dy}{d\mathrm{\Omega }}(0)$ are equal to zero, then corresponding columns of Φ(s, 0) and J_b(s) are not included in any of the calculations to get Jacobians, e.g., in (4), (9). Therefore we only need to integrate for N_u = N_s − N_k columns of Φ(s, 0) and J_b(s) (will denote them ${\mathrm{\Phi }}_u\in {\mathbb{R}}^{N_s\times N_u}$, $J_{\mathbf{b},u}\in {\mathbb{R}}^{6\times N_u}$).

In conclusion, to get the Jacobian with respect to general parameters Ω, we need to solve a large ODE on {Φ_u, a_Ω, J_b,u, c_Ω}, i.e., ODEs in (5)–(8), given the solution of the forward kinematics. Finally, the number of dimension of this IVP is (N_s + 6) × (N_p + N_u). Note that when $\frac{\partial f}{\partial \mathrm{\Omega }}(s)$ and $\frac{\partial u_n}{\partial \mathrm{\Omega }}(s)$ becomes zero or Dirac delta functions, it is not needed to solve IVP for a_Ω and c_Ω on that Ω. This occurs quite frequently, e.g., for control Jacobian or compliance matrices in constant stiffness and piecewise constant curvature case. The parameters (control variables or wrench) directly affect the state equation only at the boundary or only at some finite points along the robot.

III. ON-LINE PARAMETER ESTIMATION

Given the nonlinearity of the kinematic model, we employ the differential kinematics formulated above in an extended Kalman filter (EKF). Referring back to (1), we wish to estimate both the model parameters and the fixed offset displacement between the robot tip and sensor, {R_o, p_o}. Here, special concern is needed to handle the variable R_o as it is not a standard vector but in SO(3). Our approach follows [9], [10] in implementing EKF on SO(3) and is formulated using similar notation to [11].

Suppose the parameters are almost constant or vary slowly, which is a reasonable assumption for our model parameters, e.g. tube pre-curvature and stiffness. Our problem is to estimate these parameters based on measurement of tip position and orientation given the kinematic control inputs. Our discretized system equations for k = 1, 2, 3, … are given as follows.

Process equation:

$${\varphi }_k={\varphi }_{k-1}+w_{k-1},$$ (10)

$$p_{o,k}=p_{o,k-1}+w_{p,k-1},$$ (11)

$$R_{o,k}=R_{o,k-1}{e}^{[{\gamma }_{k-1}]}$$ (12)

Measurement equation:

$$p_{m,k}=h_p(q_k,{\varphi }_k)+h_R(q_k,{\varphi }_k)p_{o,k}+v_k,$$ (13)

$$R_{m,k}=h_R(q_k,{\varphi }_k)R_{o,k}{e}^{[{\xi }_k]},$$ (14)

where subscript k denotes the k-th time step, $w_k\in {\mathbb{R}}^{N_p}~N(0,W_k)$, w_p,k ∈ ℝ³ ~ N(0, W_p,k), γ_k ∈ ℝ³ ~ N(0, Γ_k), v_k ∈ ℝ³ ~ N(0, V_k), ξ_k ∈ ℝ³ ~ N(0, Ξ_k) are white Gaussian noise for process and measurement equations, which are assumed to be uncorrelated to each other. Here, process noise covariances control the variability of the parameters and are set to be proportional to the squared values of each of the parameters.

Note that in (12), (14), the operator e is the exponential mapping which maps r ∈ so(3) to R SO(3)[8]. Noise in SO(3) is assumed to be applied as an exponential map of white Gaussian noise as this can make the calculations far more tractable at the cost of little loss on mathematical rigorousness [9]. The inverse of the exponential map, log, is called the logarithm, which is locally defined near identity [8]. It maps R ∈ SO(3) to its exponential coordinates r ∈ so(3).

In our matrix calculations, such as linearization of state and measurement equations or covariance update of EKF, an SO(3) random variable R is treated as a vector random variable $r=\log ({\widehat{R}}^{T}R)$, which can be interpreted as a local coordinate of R ∈ SO(3) with $\widehat{R}\in SO(3)$ as its origin. In this way we can maintain the algorithm in matrix form while handling SO(3) variables.

The overall EKF algorithm for the estimation of (ϕ, p_o, R_o) is given as follows:

Initialize:

$$\begin{array}{l}\text{Set}{\widehat{\varphi }}_0^{+}\in {\mathbb{R}}^{N_p},{\widehat{p}}_{o,0}^0\in {\mathbb{R}}^3,{\widehat{R}}_{o,0}^{+}\in SO(3),\\ P_0^{+}\in {\mathbb{R}}^{(N_p+6)\times (N_p+6)}\end{array}$$

where P is the covariance matrix of concatenation of ϕ, p_o, $\log ({\widehat{R}}_o^T{R}_o)$.

Process update (for each time step k = 1, 2, …):

$${\widehat{\varphi }}_k^{-}={\widehat{\varphi }}_{k-1}^{+},{\widehat{p}}_{o,k}^{-}={\widehat{p}}_{o,k-1}^{+},{\widehat{R}}_{o,k}^{-}={\widehat{R}}_{o,k-1}^{+}$$ (15)

$$P_k^{-}=P_{k-1}^{+}+L_k$$ (16)

where ${\widehat{()}}_k^{-}$, $P_k^{-}$ denotes a priori state estimation and covariance, ${\widehat{()}}_k^{+}$, $P_k^{+}$ is a posteriori state estimation and covariance, L_k = Diag{W_k, W_p,k, Γ_k}. This is the second order approximation of nonlinear covariance update for SO(3) [9].

Measurement update ( $h_p=h_p(q_k,{\widehat{\varphi }}_k^{-})$, $h_R=h_R(q_k,{\widehat{\varphi }}_k^{-})$):

$$H=\left[\begin{array}{ccc}{h}_R{J}_{\varphi ,p}-h_R[{\widehat{p}}_{o,k}^{-}]J_{\varphi ,R}& h_R& O\\ {({\widehat{R}}_{o,k}^{-})}^T{J}_{\varphi ,R}& O& I\end{array}\right]$$ (17)

$$K=\left[\begin{array}{c}{K}_{\varphi }\\ K_{p_o}\\ K_{R_o}\end{array}\right]=P_k^{-}{H}^T{(H{P}_k^{-}{H}^T+M_k)}^{-1}$$ (18)

$$\widehat{p}=h_p+h_R{\widehat{p}}_{o,k}^{-},\widehat{R}=h_R{\widehat{R}}_{o,k}^{-}$$ (19)

$$\delta =\left[\begin{array}{c}{p}_{m,k}-\widehat{p}\\ \log ({\widehat{R}}^T{R}_{m,k})\end{array}\right]$$ (20)

$${\widehat{\varphi }}_k^{+}={\widehat{\varphi }}_k^{-}+K_{\varphi }\delta ,$$ (21)

$${\widehat{p}}_{o,k}^{+}={\widehat{p}}_{o,k}^{+}+K_{p_o}\delta ,$$ (22)

$${\widehat{R}}_{o,k}^{+}={\widehat{R}}_{o,k}^{-}{e}^{[K_{R_o}\delta ]}$$ (23)

$$P_k^{+}=\mathcal{J}(I-KH)P_k^{-}{\mathcal{J}}^T$$ (24)

where $H\in {\mathbb{R}}^{6\times (N_p+6)}$ is the linearization of the measurement equation with respect to estimation parameters ( $\varphi ,p_o,\log ({\widehat{R}}_o^T{R}_o)$), $J_{\varphi ,p},J_{\varphi ,R}\in {\mathbb{R}}^{3\times N_p}$ are position and orientation part of the body Jacobian J_ϕ (here, body frame is the forward kinematics frame) with respect to parameters, $K\in {\mathbb{R}}^{(N_p+6)\times 6}$, $K_{\varphi }\in {\mathbb{R}}^{N_p\times 6}$, $K_{p_o}$, $K_{R_o}\in {\mathbb{R}}^{3\times 6}$ are Kalman gain and its respective rows for each variables, M_k = Diag{V_k, Ξ_k} ∈ ℝ^6×6, and $\mathcal{J}\in {\mathbb{R}}^{(N_p+6)\times (N_p+6)}$ is the Jacobian of a posteriori random variable with respect to a priori random variable, i.e.,

$$\mathcal{J}=\frac{\partial ({\varphi }_k^{+},p_{o,k}^{+},\log ({({\widehat{R}}_{o,k}^{+})}^T{R}_{o,k}))}{\partial ({\varphi }_k^{-},p_{o,k}^{-},\log ({({\widehat{R}}_{o,k}^{-})}^T{R}_{o,k}))}.$$ (25)

Unlike the vector space EKF, $\mathcal{J}$ is needed as the origin of local coordinates of SO(3) changes from ${\widehat{R}}_{o,k}^{-}$ (before update) to ${\widehat{R}}_{o,k}^{+}$ (after update) [10]. From change of coordinate, (24) follows by defining $\mathcal{J}\equiv \mathit{Diag}\{I_{N_p\times N_p},I_{3\times 3},{\mathcal{J}}_{(K_{R_o}\delta )}\}$, where ${\mathcal{J}}_{(K_{R_o}\delta )}$ is called the right Jacobian of SO(3) [9], [10]. For α ∈ so(3), ${\mathcal{J}}_{(\alpha )}$ is given as

$${\mathcal{J}}_{(\alpha )}=I-\frac{1-\cos \Vert \alpha \Vert }{\Vert \alpha \Vert }[\widehat{\alpha }]+\left(1-\frac{\sin \Vert \alpha \Vert }{\Vert \alpha \Vert }\right){[\widehat{\alpha }]}^2,$$

where $\widehat{\alpha }=\frac{\alpha }{\Vert \alpha \Vert }$. This is quite a general formulation and one can subtract some variables or measurements such as p_o, R_o or h_R(q, ϕ) appropriately according to the problem setting.

IV. EXPERIMENTS

Two types of offline estimation experiments are conducted using a Matlab implementation of the algorithms described above. The first set uses simulated data with the goals of verifying the correctness of our formulation and also investigating the sensitivity of the algorithm to registration errors in the transformation between the sensor coordinate frame and the robot base frame. Next, we evaluate the algorithm using data collected from actual hardware experiments. In these experiments, an electromagnetic sensor is used to track the robot tip and the sensor coordinate frame is defined with respect to the transmitter unit. Both sets of experiments employ the three-tube robot design defined by Fig. 2 and Table I.

Fig. 2.

Concentric tube robot model used in the experiments.

TABLE I.

Tube Parameters

	Tube1	Tube2	Tube3
	sec1	sec1	sec1	sec2
length(mm)	150	150	150	86.4
kxy (relative value)	1	1	0.286
û(1/mm)	1/264	1/264	0	1/55
ν	0.3	0.3	0.3

A. Estimation Using Simulated Data

In the experiments using simulated data, we assume the tip-sensor transformation is given as R_o = R_y(90°), and $p_o={\left[\begin{array}{ccc}0& 0& 0\end{array}\right]}^T$, where R_y(θ) is the rotation with respect to y-axis with angle θ. Since the model depends on relative tube stiffness, we set k₁ = 1 and also assume that tube pre-curvature is constant and planar. Poisson’s ratio is assumed to be identical for all tubes. The resulting set of parameters to be estimated is given by (k₂, k₃, û_1,y, û_2,y, û_3,y, ν, p_o, R_o).

Data are generated using as actual parameter values those in Table I for a trajectory derived by rotating tube2 and translating tube3 back and forth repeatedly to cover the workspace shown in Fig. 3. Initial parameter values are selected randomly and virtual measurement data are generated by adding noise to the model data. Measurement standard deviation are set to be 0.25mm for position measurement and 0.05rad for orientation measurement.

Fig. 3.

Workspace used to generate simulated data. Blue and green lines are robot; black and red lines are reference trajectory and biased trajectory, respectively.

The estimation result using this trajectory is given in Fig. 4 with all physical parameters normalized to their actual values. The results show that the parameters all converge to a neighborhood of the actual values. The corresponding tip position and orientation errors can be seen in Fig. 5 to converge rapidly with maximum position and orientation errors of about 2mm and 10deg. Here, the distance between estimated and measured rotation matrices is calculated from $\Vert \log ({\widehat{R}}^T{R}_m)\Vert$, where $\widehat{R}$ and R_m are the estimated and measured rotation matrices, respectively.

Fig. 4.

Normalized parameter estimation result using simulated data.

Fig. 5.

Position and orientation error between estimated model and sensor measurement (solid line) and error in p_o, R_o (dashed line) from simulated data.

The selection of process noise covariance W_k, W_p,k, Γ_k controls the speed of convergence: low values slow convergence, while high values reduce the position and orientation errors but cause large swings in parameter values as a result of overfitting.

To investigate the sensitivity of model parameter estimation to registration errors in the transformation between the sensor and robot coordinate frames, we next introduce errors in the simulated data set corresponding to rotations and translations of robot base with respect to the sensor system (base unit of EM tracker in subsequent hardware experiments). We employ the reference trajectories of Fig. 3 rotated by 1° with respect to each of x, y and z-axes or translated by 3mm along each axis of robot base frame.

The resulting parameter estimation errors given as root mean square values normalized with respect to nominal values are summarized in Table II. As a specific example, the path associated with a rotation with respect to the x-axis is given in Fig. 6. Only û_y,3 converges to the actual value and the convergence rate of the other parameters decreases compared to Fig. 4.

TABLE II.

Root mean squared error of parameters normalized with respect to nominal parameter.

Estimated parameter set		{ûi,y}	{ki}	{ν}
1° rotation about	x-axis	20.84%	71.3%	204.2%
	y-axis	14.47%	56.55%	55.31%
	z-axis	6.06%	9.37%	42.15%
3mm translation along	x-axis	22.08%	60.00%	34.62%
	y-axis	21.32%	63.39%	181.8%
	z-axis	49.35%	76.87%	62.27%

Fig. 6.

Normalized parameter estimation assuming robot-sensor registration error.

This result suggests that parameter estimation may also need to consider sensor-robot registration. There will likely be a tradeoff, however, since this will increase the total number of parameters to be estimated. Since the errors in Table II are smallest for the curvature variables, we choose them as the parameters to estimate in following hardware experiments.

B. Estimation Using Experimental Data

The experimental platform of Fig. 7 is used to collect data for the depicted reference path. The control inputs for the path are obtained from the inverse kinematics using the parameters of Table I. Note that, due to model error, these serve as nominal parameter values and the actual path differs from the reference path. Given the sensitivity to registration error between the EM base unit and the robot base frame, only the three tube pre-curvatures are estimated with the remaining parameters (tube stiffnesses and Poisson’s ratio) set to their nominal values. Since sensor-robot translation can be accurately measured, only R_o is estimated. Generally measurement covariance is set to be the actual covariance of sensor uncertainty, but due to inherent inaccuracies in the model, fictitious covariances are determined based on the results in [1]. Owing to our Matlab implementation, estimation is performed offline.

Fig. 7.

Experimental set up. Robot path shown in white. EM tracker is used to measure tip position and orientation.

Estimation results for {û_i,y} and R_o are given in Figs. 8, 9, 10, 11. In Fig. 8, the estimated pre-curvatures approach the nominal values, but do not converge to a specific value. Fig. 9 shows that orientation error is substantially reduced by the algorithm and mean orientation error is 2.38°, despite some variation in the estimated R_o. It is observed that tracing a periodic path results in parameter value estimates tracing corresponding periodic paths. The mean position error reduces from 4.67mm based on nominal curvatures to 1.12mm with estimated values. This indicates that parameters locally adapt to minimize error between prediction and measurement as in Fig. 10 and 11.

Fig. 8.

Estimation of pre-curvature from experimental data.

Fig. 9.

Estimation of R_o from experimental data. Each value corresponds to the exponential coordinate of R_o.

Fig. 10.

Robot tip position error (top) and orientation error (bottom) using experimental data. In top figure, solid line is error based on estimated parameters while dashed line is error based on nominal parameters.

Fig. 11.

Workspace paths using estimated parameters (estimation) and nominal parameters (reference) versus measured path.

V. CONCLUSIONS

This paper proposes a filtering algorithm for estimating the kinematic parameters of the current mechanics-based model for concentric tube robots. Given prior successful real-time implementations of inverse kinematics based on differential kinematics [12] and noting that parameter estimation need not be performed at a high update rate, we anticipate that our method can also be implemented in real time. While not detailed here, it is also possible to adapt the algorithm to include external forces and torques, assuming that these can be sensed during operation. Future work will introduce methods to select process noise covariance to balance convergence rate versus overfitting and to address sensitivity to sensor-robot registration.